| | | 1 | | //======================================================================= |
| | | 2 | | // BoxCylinderGeometry.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 | | |
| | | 8 | | using System; |
| | | 9 | | |
| | | 10 | | namespace FixedMathSharp.Geometry; |
| | | 11 | | |
| | | 12 | | /// <summary> |
| | | 13 | | /// Exact box-cylinder geometry in the box's authored rotation frame. Position, |
| | | 14 | | /// extent, half-axis and radius coordinates share one reduced scale per raw |
| | | 15 | | /// coordinate; the cylinder direction is independently primitive. |
| | | 16 | | /// </summary> |
| | | 17 | | internal readonly struct BoxCylinderGeometry |
| | | 18 | | { |
| | | 19 | | internal readonly WideRationalBasis3d WorldBasis; |
| | | 20 | | internal readonly WideAxis3 Axis; |
| | | 21 | | internal readonly WideAxis3 Half; |
| | | 22 | | internal readonly WideAxis3 CenterDifference; |
| | | 23 | | internal readonly WideAxis3 HalfExtents; |
| | | 24 | | internal readonly Signed320 Radius; |
| | | 25 | | internal readonly Signed192 RawScale; |
| | | 26 | | internal readonly int ValueShift; |
| | | 27 | | |
| | | 28 | | internal BoxCylinderGeometry(Vector3d boxCenter, FixedQuaternion boxRotation, |
| | | 29 | | Vector3d boxHalfExtents, Vector3d cylinderCenter, FixedQuaternion cylinderRotation, |
| | | 30 | | Vector3d localCylinderAxis, Fixed64 cylinderLength, Fixed64 cylinderRadius) |
| | | 31 | | { |
| | 1183 | 32 | | WorldBasis = new WideRationalBasis3d(boxRotation); |
| | 1183 | 33 | | var cylinderBasis = new WideRationalBasis3d(cylinderRotation); |
| | 1183 | 34 | | WideRationalBasis3d relative = WideRationalBasis3d.CreateRelative(WorldBasis, cylinderBasis); |
| | 1183 | 35 | | WideAxis3 axis = WideRigidProjection.TransformLocalAxis(relative, |
| | 1183 | 36 | | Signed192.Raw(localCylinderAxis.X), Signed192.Raw(localCylinderAxis.Y), |
| | 1183 | 37 | | Signed192.Raw(localCylinderAxis.Z)); |
| | 1183 | 38 | | Signed192 axisX = Signed192.NarrowProven(axis.X); |
| | 1183 | 39 | | Signed192 axisY = Signed192.NarrowProven(axis.Y); |
| | 1183 | 40 | | Signed192 axisZ = Signed192.NarrowProven(axis.Z); |
| | 1183 | 41 | | Signed192 axisGcd = WideArithmetic.GetGreatestCommonDivisor( |
| | 1183 | 42 | | WideArithmetic.GetGreatestCommonDivisor(axisX, axisY), axisZ); |
| | 1183 | 43 | | Axis = new WideAxis3( |
| | 1183 | 44 | | Signed320.ExtendValue(WideArithmetic.DivideExactSigned192(axisX, axisGcd)), |
| | 1183 | 45 | | Signed320.ExtendValue(WideArithmetic.DivideExactSigned192(axisY, axisGcd)), |
| | 1183 | 46 | | Signed320.ExtendValue(WideArithmetic.DivideExactSigned192(axisZ, axisGcd))); |
| | | 47 | | |
| | 1183 | 48 | | Signed192 twiceOne = Signed192.Raw(Fixed64.Two); |
| | 1183 | 49 | | Signed320 rawScale = WideArithmetic.MultiplySigned192(relative.Denominator, twiceOne); |
| | 1183 | 50 | | WideAxis3 half = Scale(axis, Signed320.ExtendValue(Signed192.Raw(cylinderLength))); |
| | 1183 | 51 | | WideOrientedBox.GetRelativeLocalPointNumerators(cylinderCenter, boxCenter, WorldBasis, |
| | 1183 | 52 | | out Signed192 dx, out Signed192 dy, out Signed192 dz); |
| | 1183 | 53 | | WideAxis3 difference = Scale(new WideAxis3(Signed320.ExtendValue(dx), |
| | 1183 | 54 | | Signed320.ExtendValue(dy), Signed320.ExtendValue(dz)), |
| | 1183 | 55 | | WideArithmetic.MultiplySigned192(cylinderBasis.Denominator, twiceOne)); |
| | 1183 | 56 | | WideAxis3 extents = Scale(new WideAxis3( |
| | 1183 | 57 | | Signed320.ExtendValue(Signed192.Raw(boxHalfExtents.X)), |
| | 1183 | 58 | | Signed320.ExtendValue(Signed192.Raw(boxHalfExtents.Y)), |
| | 1183 | 59 | | Signed320.ExtendValue(Signed192.Raw(boxHalfExtents.Z))), rawScale); |
| | 1183 | 60 | | Signed320 radius = Signed320.NarrowValue( |
| | 1183 | 61 | | WideArithmetic.MultiplySigned320(rawScale, Signed192.Raw(cylinderRadius))); |
| | | 62 | | |
| | | 63 | | // Admitted quaternion denominators have at most 65 magnitude bits. |
| | | 64 | | // Relative basis entries are <2^130, the transformed axis <2^165, |
| | | 65 | | // and rawScale <2^163. Full raw-domain positions give difference |
| | | 66 | | // components <2^229; half-axis components are <2^228 and extents |
| | | 67 | | // and radius <2^226. Every product narrowed above therefore fits. |
| | 1183 | 68 | | Span<Signed320> coordinates = stackalloc Signed320[11] |
| | 1183 | 69 | | { |
| | 1183 | 70 | | rawScale, half.X, half.Y, half.Z, difference.X, difference.Y, difference.Z, |
| | 1183 | 71 | | extents.X, extents.Y, extents.Z, radius |
| | 1183 | 72 | | }; |
| | 1183 | 73 | | WideArithmetic.ReduceCommonScale(coordinates); |
| | 1183 | 74 | | RawScale = Signed192.NarrowProven(coordinates[0]); |
| | 1183 | 75 | | Half = new WideAxis3(coordinates[1], coordinates[2], coordinates[3]); |
| | 1183 | 76 | | CenterDifference = new WideAxis3(coordinates[4], coordinates[5], coordinates[6]); |
| | 1183 | 77 | | HalfExtents = new WideAxis3(coordinates[7], coordinates[8], coordinates[9]); |
| | 1183 | 78 | | Radius = coordinates[10]; |
| | | 79 | | |
| | 1183 | 80 | | int bits = 0; |
| | 1183 | 81 | | Span<ulong> magnitude = stackalloc ulong[5]; |
| | 26026 | 82 | | for (int index = 1; index < coordinates.Length; index++) |
| | | 83 | | { |
| | 11830 | 84 | | WideArithmetic.GetMagnitude(coordinates[index], out magnitude[4], out magnitude[3], |
| | 11830 | 85 | | out magnitude[2], out magnitude[1], out magnitude[0]); |
| | 11830 | 86 | | bits = Math.Max(bits, WideArithmetic.GetMagnitudeBitLength(magnitude)); |
| | | 87 | | } |
| | | 88 | | // Orthogonal projection cannot increase length. The residual formed |
| | | 89 | | // from vertex + cap half-axis - center and a radius contribution is |
| | | 90 | | // <2^(bits+4), and the authored bounds above also keep it <2^240. |
| | 1183 | 91 | | ValueShift = Math.Min(480, 2 * (bits + 4)); |
| | 1183 | 92 | | } |
| | | 93 | | |
| | 60635 | 94 | | internal WideAxis3 GetCapVertexOffset(int cornerBits, int capSign) => new( |
| | 60635 | 95 | | Offset(HalfExtents.X, Half.X, CenterDifference.X, (cornerBits & 1) != 0, capSign), |
| | 60635 | 96 | | Offset(HalfExtents.Y, Half.Y, CenterDifference.Y, (cornerBits & 2) != 0, capSign), |
| | 60635 | 97 | | Offset(HalfExtents.Z, Half.Z, CenterDifference.Z, (cornerBits & 4) != 0, capSign)); |
| | | 98 | | |
| | | 99 | | internal static Signed320 GetComponent(in WideAxis3 value, int index) => |
| | 414634 | 100 | | index switch { 0 => value.X, 1 => value.Y, _ => value.Z }; |
| | | 101 | | |
| | 3549 | 102 | | private static WideAxis3 Scale(WideAxis3 value, Signed320 scale) => new( |
| | 3549 | 103 | | Signed320.NarrowValue(WideArithmetic.MultiplySigned320(value.X, scale)), |
| | 3549 | 104 | | Signed320.NarrowValue(WideArithmetic.MultiplySigned320(value.Y, scale)), |
| | 3549 | 105 | | Signed320.NarrowValue(WideArithmetic.MultiplySigned320(value.Z, scale))); |
| | | 106 | | |
| | | 107 | | private static Signed320 Offset(Signed320 extent, Signed320 half, Signed320 center, |
| | 181905 | 108 | | bool positiveVertex, int capSign) => WideArithmetic.SubtractSigned320( |
| | 181905 | 109 | | WideArithmetic.AddSigned320(positiveVertex ? extent : WideArithmetic.Negate(extent), |
| | 181905 | 110 | | capSign == 0 ? default : capSign > 0 ? half : WideArithmetic.Negate(half)), center); |
| | | 111 | | } |