| | | 1 | | //======================================================================= |
| | | 2 | | // WideConvexPrismRelations.CylinderCapsuleFeatures.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 | | /// <content> |
| | | 13 | | /// Analytic normal strata and certified endpoint/rim contacts for a finite |
| | | 14 | | /// cylinder and capsule. All intermediate positions retain authored rational |
| | | 15 | | /// axes; only the selected normal and complete contact depth are rounded. |
| | | 16 | | /// </content> |
| | | 17 | | internal static partial class WideConvexPrismRelations |
| | | 18 | | { |
| | | 19 | | private readonly struct CylinderCapsuleDirection |
| | | 20 | | { |
| | | 21 | | internal readonly Signed576 X; |
| | | 22 | | internal readonly Signed576 Y; |
| | | 23 | | internal readonly Signed576 Z; |
| | | 24 | | |
| | | 25 | | internal CylinderCapsuleDirection(Signed576 x, Signed576 y, Signed576 z) |
| | | 26 | | { |
| | 15003 | 27 | | X = x; |
| | 15003 | 28 | | Y = y; |
| | 15003 | 29 | | Z = z; |
| | 15003 | 30 | | } |
| | | 31 | | |
| | | 32 | | internal CylinderCapsuleDirection(Axis3 axis) |
| | 24556 | 33 | | : this(Signed576.ExtendValue(axis.X), Signed576.ExtendValue(axis.Y), Signed576.ExtendValue(axis.Z)) { } |
| | | 34 | | |
| | 7634 | 35 | | internal bool IsZero => X.IsZero && Y.IsZero && Z.IsZero; |
| | | 36 | | } |
| | | 37 | | |
| | | 38 | | private readonly struct CylinderCapsuleFeatureGeometry |
| | | 39 | | { |
| | | 40 | | internal readonly RigidAxis3 Cylinder; |
| | | 41 | | internal readonly RigidAxis3 Capsule; |
| | | 42 | | internal readonly Signed192 DifferenceX; |
| | | 43 | | internal readonly Signed192 DifferenceY; |
| | | 44 | | internal readonly Signed192 DifferenceZ; |
| | | 45 | | internal readonly Signed192 Scale; |
| | | 46 | | internal readonly Signed320 CenterX; |
| | | 47 | | internal readonly Signed320 CenterY; |
| | | 48 | | internal readonly Signed320 CenterZ; |
| | | 49 | | internal readonly Axis3 CylinderHalf; |
| | | 50 | | internal readonly Axis3 CapsuleHalf; |
| | | 51 | | internal readonly Signed320 CylinderSquared; |
| | | 52 | | internal readonly Signed192 CylinderLength; |
| | | 53 | | internal readonly Signed192 CapsuleLength; |
| | | 54 | | internal readonly Fixed64 Radius; |
| | | 55 | | |
| | | 56 | | internal CylinderCapsuleFeatureGeometry( |
| | | 57 | | Vector3d cylinderCenter, RigidAxis3 cylinder, Signed192 cylinderLength, Fixed64 radius, |
| | | 58 | | Vector3d capsuleCenter, RigidAxis3 capsule, Signed192 capsuleLength) |
| | | 59 | | { |
| | 1598 | 60 | | Cylinder = cylinder; |
| | 1598 | 61 | | Capsule = capsule; |
| | 1598 | 62 | | CylinderLength = cylinderLength; |
| | 1598 | 63 | | CapsuleLength = capsuleLength; |
| | 1598 | 64 | | Radius = radius; |
| | 1598 | 65 | | DifferenceX = WideArithmetic.Difference(capsuleCenter.X, cylinderCenter.X); |
| | 1598 | 66 | | DifferenceY = WideArithmetic.Difference(capsuleCenter.Y, cylinderCenter.Y); |
| | 1598 | 67 | | DifferenceZ = WideArithmetic.Difference(capsuleCenter.Z, cylinderCenter.Z); |
| | 1598 | 68 | | Signed192 denominators = Signed192.NarrowProven( |
| | 1598 | 69 | | WideArithmetic.MultiplySigned192(cylinder.RotationDenominator, capsule.RotationDenominator)); |
| | 1598 | 70 | | Scale = Signed192.NarrowProven(WideArithmetic.MultiplySigned192( |
| | 1598 | 71 | | denominators, Signed192.Raw(Fixed64.Two))); |
| | 1598 | 72 | | CenterX = WideArithmetic.MultiplySigned192(DifferenceX, Scale); |
| | 1598 | 73 | | CenterY = WideArithmetic.MultiplySigned192(DifferenceY, Scale); |
| | 1598 | 74 | | CenterZ = WideArithmetic.MultiplySigned192(DifferenceZ, Scale); |
| | 1598 | 75 | | CylinderHalf = GetCylinderCapsuleScaledHalf(cylinder, cylinderLength, capsule.RotationDenominator); |
| | 1598 | 76 | | CapsuleHalf = GetCylinderCapsuleScaledHalf(capsule, capsuleLength, cylinder.RotationDenominator); |
| | 1598 | 77 | | CylinderSquared = GetAxisSquared(cylinder); |
| | 1598 | 78 | | } |
| | | 79 | | |
| | 6579 | 80 | | internal Axis3 EndpointOffset(int cylinderSign, int capsuleSign) => new( |
| | 6579 | 81 | | GetCylinderCapsuleEndpointCoordinate(CenterX, CylinderHalf.X, cylinderSign, CapsuleHalf.X, capsuleSign), |
| | 6579 | 82 | | GetCylinderCapsuleEndpointCoordinate(CenterY, CylinderHalf.Y, cylinderSign, CapsuleHalf.Y, capsuleSign), |
| | 6579 | 83 | | GetCylinderCapsuleEndpointCoordinate(CenterZ, CylinderHalf.Z, cylinderSign, CapsuleHalf.Z, capsuleSign)); |
| | | 84 | | } |
| | | 85 | | |
| | 3196 | 86 | | private static Axis3 GetCylinderCapsuleScaledHalf(RigidAxis3 axis, Signed192 length, Signed192 otherDenominator) => |
| | 3196 | 87 | | Signed320.NarrowValue(WideArithmetic.MultiplySigned320( |
| | 3196 | 88 | | WideArithmetic.MultiplySigned192(axis.X, length), otherDenominator)), |
| | 3196 | 89 | | Signed320.NarrowValue(WideArithmetic.MultiplySigned320( |
| | 3196 | 90 | | WideArithmetic.MultiplySigned192(axis.Y, length), otherDenominator)), |
| | 3196 | 91 | | Signed320.NarrowValue(WideArithmetic.MultiplySigned320( |
| | 3196 | 92 | | WideArithmetic.MultiplySigned192(axis.Z, length), otherDenominator))); |
| | | 93 | | |
| | | 94 | | private static Signed320 GetCylinderCapsuleEndpointCoordinate( |
| | | 95 | | Signed320 center, Signed320 cylinderHalf, int cylinderSign, Signed320 capsuleHalf, int capsuleSign) |
| | | 96 | | { |
| | 23008 | 97 | | Signed320 result = cylinderSign == 0 ? center : cylinderSign > 0 |
| | 23008 | 98 | | ? WideArithmetic.SubtractSigned320(center, cylinderHalf) |
| | 23008 | 99 | | : WideArithmetic.AddSigned320(center, cylinderHalf); |
| | 23008 | 100 | | return capsuleSign == 0 ? result : capsuleSign > 0 |
| | 23008 | 101 | | ? WideArithmetic.SubtractSigned320(result, capsuleHalf) |
| | 23008 | 102 | | : WideArithmetic.AddSigned320(result, capsuleHalf); |
| | | 103 | | } |
| | | 104 | | |
| | | 105 | | private static Signed576 DotCylinderCapsuleDirection(CylinderCapsuleDirection direction, RigidAxis3 axis) => |
| | 20203 | 106 | | DotCylinderCapsuleDirection(direction, axis.X, axis.Y, axis.Z); |
| | | 107 | | |
| | | 108 | | private static Signed576 DotCylinderCapsuleDirection( |
| | | 109 | | CylinderCapsuleDirection direction, Signed192 x, Signed192 y, Signed192 z) => |
| | 26975 | 110 | | WideArithmetic.AddSigned576(WideArithmetic.AddSigned576( |
| | 26975 | 111 | | WideArithmetic.MultiplySigned576(direction.X, x), WideArithmetic.MultiplySigned576(direction.Y, y)), |
| | 26975 | 112 | | WideArithmetic.MultiplySigned576(direction.Z, z)); |
| | | 113 | | |
| | | 114 | | private static CylinderCapsuleDirection ProjectCylinderCapsulePerpendicular(Axis3 offset, RigidAxis3 axis) |
| | | 115 | | { |
| | 2725 | 116 | | Signed320 squared = GetAxisSquared(axis); |
| | 2725 | 117 | | Signed576 dot = DotCylinderCapsuleDirection(new CylinderCapsuleDirection(offset), axis); |
| | 2725 | 118 | | return new CylinderCapsuleDirection( |
| | 2725 | 119 | | WideArithmetic.SubtractSigned576(WideArithmetic.MultiplySigned320(offset.X, squared), |
| | 2725 | 120 | | WideArithmetic.MultiplySigned576(dot, axis.X)), |
| | 2725 | 121 | | WideArithmetic.SubtractSigned576(WideArithmetic.MultiplySigned320(offset.Y, squared), |
| | 2725 | 122 | | WideArithmetic.MultiplySigned576(dot, axis.Y)), |
| | 2725 | 123 | | WideArithmetic.SubtractSigned576(WideArithmetic.MultiplySigned320(offset.Z, squared), |
| | 2725 | 124 | | WideArithmetic.MultiplySigned576(dot, axis.Z))); |
| | | 125 | | } |
| | | 126 | | |
| | | 127 | | private static CylinderCapsuleDirection GetCylinderCapsulePerpendicular( |
| | | 128 | | FixedQuaternion rotation, Vector3d localAxis) |
| | | 129 | | { |
| | | 130 | | // A continuous radial tie follows the authored frame, so applying a |
| | | 131 | | // common rigid rotation also rotates the selected representative. |
| | 200 | 132 | | Vector3d localPerpendicular = localAxis.X != Fixed64.Zero || localAxis.Y != Fixed64.Zero |
| | 200 | 133 | | ? new Vector3d(localAxis.Y, -localAxis.X, Fixed64.Zero) : Vector3d.Right; |
| | 200 | 134 | | WideOrientedBox.GetRotatedLocalAxisNumerators(rotation, localPerpendicular, |
| | 200 | 135 | | out Signed192 x, out Signed192 y, out Signed192 z, out _); |
| | 200 | 136 | | return new CylinderCapsuleDirection(new Axis3(Signed320.ExtendValue(x), |
| | 200 | 137 | | Signed320.ExtendValue(y), Signed320.ExtendValue(z))); |
| | | 138 | | } |
| | | 139 | | |
| | | 140 | | private static void ImportCylinderCapsuleFeature(Signed192 value, Span<ulong> destination) |
| | | 141 | | { |
| | 6396 | 142 | | destination.Clear(); |
| | 6396 | 143 | | WideArithmetic.GetMagnitude(value, out destination[2], out destination[1], out destination[0]); |
| | 6396 | 144 | | } |
| | | 145 | | |
| | | 146 | | private static void ImportCylinderCapsuleFeature(Signed320 value, Span<ulong> destination) |
| | | 147 | | { |
| | 14911 | 148 | | destination.Clear(); |
| | 14911 | 149 | | WideArithmetic.GetMagnitude(value, out destination[4], out destination[3], out destination[2], |
| | 14911 | 150 | | out destination[1], out destination[0]); |
| | 14911 | 151 | | } |
| | | 152 | | |
| | | 153 | | private static void ImportCylinderCapsuleFeature(Signed576 value, Span<ulong> destination) |
| | | 154 | | { |
| | 26241 | 155 | | destination.Clear(); |
| | 26241 | 156 | | WideArithmetic.GetMagnitude(value, destination[..9]); |
| | 26241 | 157 | | } |
| | | 158 | | |
| | | 159 | | private static void ImportCylinderCapsuleFeature(Signed832 value, Span<ulong> destination) |
| | | 160 | | { |
| | 10448 | 161 | | destination.Clear(); |
| | 10448 | 162 | | WideArithmetic.GetMagnitude(value, destination[..13]); |
| | 10448 | 163 | | } |
| | | 164 | | |
| | | 165 | | private static Span<ulong> CylinderCapsuleFeatureSlot(Span<ulong> values, int slot) => |
| | 144700 | 166 | | values.Slice(slot * ConvexContactCandidate.Words, ConvexContactCandidate.Words); |
| | | 167 | | |
| | | 168 | | private static bool BuildCylinderCapsuleAxisCandidate( |
| | | 169 | | in CylinderCapsuleFeatureGeometry geometry, CylinderCapsuleDirection direction, |
| | | 170 | | Span<ulong> values, Span<int> signs, out int gapSign) |
| | | 171 | | { |
| | 5951 | 172 | | gapSign = 0; |
| | 5951 | 173 | | if (direction.IsZero) |
| | 4 | 174 | | return false; |
| | 5947 | 175 | | values.Clear(); |
| | 5947 | 176 | | signs.Clear(); |
| | 5947 | 177 | | Signed576 cylinderAlignment = DotCylinderCapsuleDirection(direction, geometry.Cylinder); |
| | 5947 | 178 | | Signed576 capsuleAlignment = DotCylinderCapsuleDirection(direction, geometry.Capsule); |
| | 5947 | 179 | | Signed576 centerAlignment = DotCylinderCapsuleDirection(direction, |
| | 5947 | 180 | | geometry.DifferenceX, geometry.DifferenceY, geometry.DifferenceZ); |
| | 5947 | 181 | | int orientation = centerAlignment.Sign < 0 ? -1 : 1; |
| | 5947 | 182 | | ImportCylinderCapsuleFeature(direction.X, CylinderCapsuleFeatureSlot(values, 0)); |
| | 5947 | 183 | | ImportCylinderCapsuleFeature(direction.Y, CylinderCapsuleFeatureSlot(values, 1)); |
| | 5947 | 184 | | ImportCylinderCapsuleFeature(direction.Z, CylinderCapsuleFeatureSlot(values, 2)); |
| | 5947 | 185 | | signs[0] = direction.X.Sign * orientation; |
| | 5947 | 186 | | signs[1] = direction.Y.Sign * orientation; |
| | 5947 | 187 | | signs[2] = direction.Z.Sign * orientation; |
| | | 188 | | |
| | | 189 | | // Every generated rational direction is below 2^440. Dot products are |
| | | 190 | | // below 2^542, the common rational support below 2^674. The final |
| | | 191 | | // squared-gap coefficients are below 2^1780 (40 words give 2560 bits). |
| | 5947 | 192 | | Signed832 rational = WideArithmetic.SubtractSigned832(WideArithmetic.AddSigned832( |
| | 5947 | 193 | | WideArithmetic.MultiplySigned576ToSigned832(GetMagnitude576(cylinderAlignment), |
| | 5947 | 194 | | WideArithmetic.MultiplySigned192(geometry.CylinderLength, geometry.Capsule.RotationDenominator)), |
| | 5947 | 195 | | WideArithmetic.MultiplySigned576ToSigned832(GetMagnitude576(capsuleAlignment), |
| | 5947 | 196 | | WideArithmetic.MultiplySigned192(geometry.CapsuleLength, geometry.Cylinder.RotationDenominator))), |
| | 5947 | 197 | | WideArithmetic.MultiplySigned576ToSigned832(GetMagnitude576(centerAlignment), Signed320.ExtendValue(geometry |
| | | 198 | | |
| | | 199 | | const int words = ConvexContactCandidate.Words; |
| | 5947 | 200 | | Span<ulong> work = stackalloc ulong[10 * words]; |
| | 5947 | 201 | | Span<ulong> u = CylinderCapsuleFeatureSlot(work, 0); |
| | 5947 | 202 | | Span<ulong> axisSquared = CylinderCapsuleFeatureSlot(work, 1); |
| | 5947 | 203 | | Span<ulong> product = CylinderCapsuleFeatureSlot(work, 2); |
| | 5947 | 204 | | Span<ulong> temporary = CylinderCapsuleFeatureSlot(work, 3); |
| | 5947 | 205 | | Span<ulong> plane = CylinderCapsuleFeatureSlot(work, 4); |
| | 5947 | 206 | | Span<ulong> p = CylinderCapsuleFeatureSlot(work, 5); |
| | 5947 | 207 | | Span<ulong> q = CylinderCapsuleFeatureSlot(work, 6); |
| | 5947 | 208 | | Span<ulong> pSquared = CylinderCapsuleFeatureSlot(work, 7); |
| | 5947 | 209 | | Span<ulong> qSquaredK = CylinderCapsuleFeatureSlot(work, 8); |
| | 5947 | 210 | | Span<ulong> f = CylinderCapsuleFeatureSlot(work, 9); |
| | 5947 | 211 | | axisSquared.Clear(); |
| | 47576 | 212 | | for (int component = 0; component < 3; component++) |
| | | 213 | | { |
| | 17841 | 214 | | Span<ulong> coordinate = CylinderCapsuleFeatureSlot(values, component); |
| | 17841 | 215 | | WideArithmetic.MultiplyMagnitudes(coordinate, coordinate, product); |
| | 17841 | 216 | | WideArithmetic.AddMagnitudeInto(product, axisSquared); |
| | | 217 | | } |
| | 5947 | 218 | | ImportCylinderCapsuleFeature(geometry.CylinderSquared, u); |
| | 5947 | 219 | | WideArithmetic.MultiplyMagnitudes(axisSquared, u, product); |
| | 5947 | 220 | | ImportCylinderCapsuleFeature(cylinderAlignment, temporary); |
| | 5947 | 221 | | WideArithmetic.MultiplyMagnitudes(temporary, temporary, plane); |
| | 5947 | 222 | | WideArithmetic.SubtractEqualMagnitudes(product, plane, temporary); |
| | 5947 | 223 | | temporary.CopyTo(plane); |
| | 5947 | 224 | | ImportCylinderCapsuleFeature(rational, temporary); |
| | 5947 | 225 | | WideArithmetic.MultiplyMagnitudes(temporary, u, p); |
| | 5947 | 226 | | ImportCylinderCapsuleFeature(WideArithmetic.MultiplySigned192( |
| | 5947 | 227 | | Signed192.Raw(geometry.Radius), geometry.Scale), q); |
| | 5947 | 228 | | Span<ulong> k = CylinderCapsuleFeatureSlot(values, 9); |
| | 5947 | 229 | | WideArithmetic.MultiplyMagnitudes(u, plane, k); |
| | 5947 | 230 | | WideArithmetic.MultiplyMagnitudes(p, p, pSquared); |
| | 5947 | 231 | | WideArithmetic.MultiplyMagnitudes(q, q, temporary); |
| | 5947 | 232 | | WideArithmetic.MultiplyMagnitudes(temporary, k, qSquaredK); |
| | 5947 | 233 | | WideArithmetic.AddEqualMagnitudes(pSquared, qSquaredK, CylinderCapsuleFeatureSlot(values, 7)); |
| | 5947 | 234 | | WideArithmetic.MultiplyMagnitudes(p, q, temporary); |
| | 5947 | 235 | | WideArithmetic.AddEqualMagnitudes(temporary, temporary, CylinderCapsuleFeatureSlot(values, 8)); |
| | 5947 | 236 | | signs[7] = WideArithmetic.GetActiveMagnitudeLength(CylinderCapsuleFeatureSlot(values, 7)) == 0 ? 0 : 1; |
| | 5947 | 237 | | signs[8] = WideArithmetic.GetActiveMagnitudeLength(CylinderCapsuleFeatureSlot(values, 8)) == 0 ? 0 : rational.Si |
| | 5947 | 238 | | signs[9] = WideArithmetic.GetActiveMagnitudeLength(k) == 0 ? 0 : 1; |
| | 5947 | 239 | | ImportCylinderCapsuleFeature(geometry.Scale, f); |
| | 5947 | 240 | | WideArithmetic.MultiplyMagnitudes(f, u, temporary); |
| | 5947 | 241 | | WideArithmetic.MultiplyMagnitudes(temporary, temporary, product); |
| | 5947 | 242 | | WideArithmetic.MultiplyMagnitudes(product, axisSquared, CylinderCapsuleFeatureSlot(values, 10)); |
| | 5947 | 243 | | signs[10] = 1; |
| | 5947 | 244 | | gapSign = rational.Sign < 0 |
| | 5947 | 245 | | ? WideArithmetic.CompareMagnitudeEqualLength(qSquaredK, pSquared) |
| | 5947 | 246 | | : rational.Sign > 0 || WideArithmetic.GetActiveMagnitudeLength(qSquaredK) != 0 ? 1 : 0; |
| | 5947 | 247 | | return true; |
| | | 248 | | } |
| | | 249 | | |
| | | 250 | | private static bool BuildCylinderCapsuleEndpointRimCandidate( |
| | | 251 | | in CylinderCapsuleFeatureGeometry geometry, int cylinderSign, int capsuleSign, |
| | | 252 | | Span<ulong> values, Span<int> signs, out int gapSign) |
| | | 253 | | { |
| | 2958 | 254 | | gapSign = -1; |
| | 2958 | 255 | | Axis3 offset = geometry.EndpointOffset(cylinderSign, capsuleSign); |
| | 2958 | 256 | | var direction = new CylinderCapsuleDirection(offset); |
| | 2958 | 257 | | Signed576 axial = DotCylinderCapsuleDirection(direction, geometry.Cylinder); |
| | 2958 | 258 | | if (axial.Sign * cylinderSign < 0) |
| | 2252 | 259 | | return false; |
| | 706 | 260 | | Signed576 g = GetAxisSquared(offset); |
| | 706 | 261 | | Signed832 q = WideArithmetic.SubtractSigned832( |
| | 706 | 262 | | WideArithmetic.MultiplySigned576ToSigned832(g, geometry.CylinderSquared), |
| | 706 | 263 | | WideArithmetic.MultiplySigned576ToSigned832(axial, axial)); |
| | | 264 | | // On the radial axis the closest feature is a cap, not a rim. Its |
| | | 265 | | // normal is already present in the analytic boundary candidates. |
| | 706 | 266 | | if (q.IsZero) |
| | 2 | 267 | | return false; |
| | 704 | 268 | | Signed320 rf = WideArithmetic.MultiplySigned192(Signed192.Raw(geometry.Radius), geometry.Scale); |
| | 704 | 269 | | Signed576 rfSquared = WideArithmetic.MultiplySigned320(rf, rf); |
| | 704 | 270 | | if (WideArithmetic.SubtractSigned832(q, |
| | 704 | 271 | | WideArithmetic.MultiplySigned576ToSigned832(rfSquared, geometry.CylinderSquared)).Sign < 0) |
| | 97 | 272 | | return false; |
| | | 273 | | |
| | 607 | 274 | | values.Clear(); |
| | 607 | 275 | | signs.Clear(); |
| | | 276 | | const int words = ConvexContactCandidate.Words; |
| | 607 | 277 | | Span<ulong> work = stackalloc ulong[3 * words]; |
| | 607 | 278 | | Span<ulong> first = CylinderCapsuleFeatureSlot(work, 0); |
| | 607 | 279 | | Span<ulong> second = CylinderCapsuleFeatureSlot(work, 1); |
| | 607 | 280 | | Span<ulong> third = CylinderCapsuleFeatureSlot(work, 2); |
| | 607 | 281 | | ImportCylinderCapsuleFeature(q, first); |
| | 607 | 282 | | ImportCylinderCapsuleFeature(geometry.CylinderSquared, second); |
| | 607 | 283 | | Span<ulong> root = CylinderCapsuleFeatureSlot(values, 6); |
| | 607 | 284 | | WideArithmetic.MultiplyMagnitudes(first, second, root); |
| | 607 | 285 | | root.CopyTo(CylinderCapsuleFeatureSlot(values, 9)); |
| | 607 | 286 | | signs[6] = signs[9] = 1; |
| | | 287 | | |
| | 607 | 288 | | CylinderCapsuleDirection radial = ProjectCylinderCapsulePerpendicular(offset, geometry.Cylinder); |
| | 607 | 289 | | Signed832 nx = WideArithmetic.MultiplySigned576ToSigned832(radial.X, rf); |
| | 607 | 290 | | Signed832 ny = WideArithmetic.MultiplySigned576ToSigned832(radial.Y, rf); |
| | 607 | 291 | | Signed832 nz = WideArithmetic.MultiplySigned576ToSigned832(radial.Z, rf); |
| | 607 | 292 | | ImportCylinderCapsuleFeature(nx, CylinderCapsuleFeatureSlot(values, 0)); |
| | 607 | 293 | | ImportCylinderCapsuleFeature(ny, CylinderCapsuleFeatureSlot(values, 1)); |
| | 607 | 294 | | ImportCylinderCapsuleFeature(nz, CylinderCapsuleFeatureSlot(values, 2)); |
| | 607 | 295 | | signs[0] = -nx.Sign; |
| | 607 | 296 | | signs[1] = -ny.Sign; |
| | 607 | 297 | | signs[2] = -nz.Sign; |
| | 607 | 298 | | ImportCylinderCapsuleFeature(offset.X, CylinderCapsuleFeatureSlot(values, 3)); |
| | 607 | 299 | | ImportCylinderCapsuleFeature(offset.Y, CylinderCapsuleFeatureSlot(values, 4)); |
| | 607 | 300 | | ImportCylinderCapsuleFeature(offset.Z, CylinderCapsuleFeatureSlot(values, 5)); |
| | 607 | 301 | | signs[3] = offset.X.Sign; |
| | 607 | 302 | | signs[4] = offset.Y.Sign; |
| | 607 | 303 | | signs[5] = offset.Z.Sign; |
| | | 304 | | |
| | 607 | 305 | | if (!geometry.CapsuleLength.IsZero) |
| | | 306 | | { |
| | 599 | 307 | | Signed576 projection = DotCylinderCapsuleDirection(direction, geometry.Capsule); |
| | 599 | 308 | | Signed576 radialProjection = DotCylinderCapsuleDirection(radial, geometry.Capsule); |
| | 599 | 309 | | Signed832 rational = WideArithmetic.MultiplySigned576ToSigned832(radialProjection, rf); |
| | 599 | 310 | | ImportCylinderCapsuleFeature(rational, first); |
| | 599 | 311 | | ImportCylinderCapsuleFeature(projection, second); |
| | 599 | 312 | | if (GetConvexContactCandidateQuadraticSign(first, -rational.Sign, second, projection.Sign, root) * capsuleSi |
| | 467 | 313 | | return false; |
| | | 314 | | } |
| | | 315 | | |
| | 140 | 316 | | Signed832 gapRational = WideArithmetic.MultiplySigned576ToSigned832( |
| | 140 | 317 | | WideArithmetic.AddSigned576(g, rfSquared), geometry.CylinderSquared); |
| | 140 | 318 | | ImportCylinderCapsuleFeature(gapRational, CylinderCapsuleFeatureSlot(values, 7)); |
| | 140 | 319 | | ImportCylinderCapsuleFeature(WideArithmetic.AddSigned320(rf, rf), CylinderCapsuleFeatureSlot(values, 8)); |
| | 140 | 320 | | signs[7] = gapRational.Sign; |
| | 140 | 321 | | signs[8] = -rf.Sign; |
| | 140 | 322 | | ImportCylinderCapsuleFeature(geometry.Scale, first); |
| | 140 | 323 | | WideArithmetic.MultiplyMagnitudes(first, first, third); |
| | 140 | 324 | | ImportCylinderCapsuleFeature(geometry.CylinderSquared, second); |
| | 140 | 325 | | WideArithmetic.MultiplyMagnitudes(third, second, CylinderCapsuleFeatureSlot(values, 10)); |
| | 140 | 326 | | signs[10] = 1; |
| | | 327 | | // A zero residual has no normal. Shared cap/side boundaries represent |
| | | 328 | | // this exact-touch case, including the continuous tie at the rim. |
| | 140 | 329 | | return GetConvexContactCandidateQuadraticSign(CylinderCapsuleFeatureSlot(values, 7), signs[7], |
| | 140 | 330 | | CylinderCapsuleFeatureSlot(values, 8), signs[8], root) > 0; |
| | | 331 | | } |
| | | 332 | | |
| | | 333 | | private static bool BuildCylinderCapsuleProjectedCornerCandidate( |
| | | 334 | | in CylinderCapsuleFeatureGeometry geometry, Axis3 major, int cylinderSign, |
| | | 335 | | Span<ulong> values, Span<int> signs, out int gapSign) |
| | | 336 | | { |
| | 1428 | 337 | | gapSign = -1; |
| | 1428 | 338 | | Axis3 offset = geometry.EndpointOffset(cylinderSign, 0); |
| | 1428 | 339 | | Signed576 axial = DotCylinderCapsuleDirection(new CylinderCapsuleDirection(offset), geometry.Cylinder); |
| | 1428 | 340 | | if (axial.Sign * cylinderSign <= 0) |
| | 1040 | 341 | | return false; |
| | 388 | 342 | | Signed576 e = GetAxisSquared(major); |
| | 388 | 343 | | Signed576 majorProjection = DotCylinderCapsuleDirection(new CylinderCapsuleDirection(major), |
| | 388 | 344 | | geometry.DifferenceX, geometry.DifferenceY, geometry.DifferenceZ); |
| | 388 | 345 | | Signed320 radiusSquared = WideArithmetic.MultiplySigned192( |
| | 388 | 346 | | Signed192.Raw(geometry.Radius), Signed192.Raw(geometry.Radius)); |
| | 388 | 347 | | if (WideArithmetic.SubtractSigned832( |
| | 388 | 348 | | WideArithmetic.MultiplySigned576ToSigned832(majorProjection, majorProjection), |
| | 388 | 349 | | WideArithmetic.MultiplySigned576ToSigned832(e, radiusSquared)).Sign <= 0) |
| | 79 | 350 | | return false; |
| | | 351 | | |
| | | 352 | | // With perpendicular axes the projected cylinder is a rectangle. |
| | | 353 | | // Both coordinate excesses are positive, so its nearest corner gives |
| | | 354 | | // the sole non-boundary minimum on the capsule-interior normal plane. |
| | 309 | 355 | | values.Clear(); |
| | 309 | 356 | | signs.Clear(); |
| | 309 | 357 | | CylinderCapsuleDirection projected = ProjectCylinderCapsulePerpendicular(offset, geometry.Capsule); |
| | 309 | 358 | | Signed320 b = GetAxisSquared(geometry.Capsule); |
| | 309 | 359 | | Signed320 rf = WideArithmetic.MultiplySigned192(Signed192.Raw(geometry.Radius), geometry.Scale); |
| | 309 | 360 | | Signed576 rfb = WideArithmetic.MultiplySigned320(rf, b); |
| | 309 | 361 | | Signed832 nx = WideArithmetic.MultiplySigned576ToSigned832(rfb, major.X); |
| | 309 | 362 | | Signed832 ny = WideArithmetic.MultiplySigned576ToSigned832(rfb, major.Y); |
| | 309 | 363 | | Signed832 nz = WideArithmetic.MultiplySigned576ToSigned832(rfb, major.Z); |
| | 309 | 364 | | ImportCylinderCapsuleFeature(nx, CylinderCapsuleFeatureSlot(values, 0)); |
| | 309 | 365 | | ImportCylinderCapsuleFeature(ny, CylinderCapsuleFeatureSlot(values, 1)); |
| | 309 | 366 | | ImportCylinderCapsuleFeature(nz, CylinderCapsuleFeatureSlot(values, 2)); |
| | 309 | 367 | | signs[0] = -nx.Sign * majorProjection.Sign; |
| | 309 | 368 | | signs[1] = -ny.Sign * majorProjection.Sign; |
| | 309 | 369 | | signs[2] = -nz.Sign * majorProjection.Sign; |
| | 309 | 370 | | ImportCylinderCapsuleFeature(projected.X, CylinderCapsuleFeatureSlot(values, 3)); |
| | 309 | 371 | | ImportCylinderCapsuleFeature(projected.Y, CylinderCapsuleFeatureSlot(values, 4)); |
| | 309 | 372 | | ImportCylinderCapsuleFeature(projected.Z, CylinderCapsuleFeatureSlot(values, 5)); |
| | 309 | 373 | | signs[3] = projected.X.Sign; |
| | 309 | 374 | | signs[4] = projected.Y.Sign; |
| | 309 | 375 | | signs[5] = projected.Z.Sign; |
| | 309 | 376 | | ImportCylinderCapsuleFeature(e, CylinderCapsuleFeatureSlot(values, 6)); |
| | 309 | 377 | | ImportCylinderCapsuleFeature(e, CylinderCapsuleFeatureSlot(values, 9)); |
| | 309 | 378 | | signs[6] = signs[9] = 1; |
| | | 379 | | |
| | | 380 | | const int words = ConvexContactCandidate.Words; |
| | 309 | 381 | | Span<ulong> work = stackalloc ulong[5 * words]; |
| | 309 | 382 | | Span<ulong> squared = CylinderCapsuleFeatureSlot(work, 0); |
| | 309 | 383 | | Span<ulong> product = CylinderCapsuleFeatureSlot(work, 1); |
| | 309 | 384 | | Span<ulong> first = CylinderCapsuleFeatureSlot(work, 2); |
| | 309 | 385 | | Span<ulong> second = CylinderCapsuleFeatureSlot(work, 3); |
| | 309 | 386 | | Span<ulong> third = CylinderCapsuleFeatureSlot(work, 4); |
| | 309 | 387 | | squared.Clear(); |
| | 2472 | 388 | | for (int component = 3; component < 6; component++) |
| | | 389 | | { |
| | 927 | 390 | | Span<ulong> coordinate = CylinderCapsuleFeatureSlot(values, component); |
| | 927 | 391 | | WideArithmetic.MultiplyMagnitudes(coordinate, coordinate, product); |
| | 927 | 392 | | WideArithmetic.AddMagnitudeInto(product, squared); |
| | | 393 | | } |
| | 309 | 394 | | ImportCylinderCapsuleFeature(rfb, first); |
| | 309 | 395 | | WideArithmetic.MultiplyMagnitudes(first, first, product); |
| | 309 | 396 | | WideArithmetic.AddMagnitudeInto(product, squared); |
| | 309 | 397 | | WideArithmetic.MultiplyMagnitudes(squared, CylinderCapsuleFeatureSlot(values, 9), |
| | 309 | 398 | | CylinderCapsuleFeatureSlot(values, 7)); |
| | 309 | 399 | | Signed832 dot = WideArithmetic.MultiplySigned576ToSigned832( |
| | 309 | 400 | | WideArithmetic.MultiplySigned576(GetMagnitude576(majorProjection), geometry.Scale), b); |
| | 309 | 401 | | ImportCylinderCapsuleFeature(dot, second); |
| | 309 | 402 | | WideArithmetic.MultiplyMagnitudes(first, second, product); |
| | 309 | 403 | | WideArithmetic.AddEqualMagnitudes(product, product, CylinderCapsuleFeatureSlot(values, 8)); |
| | 309 | 404 | | signs[7] = 1; |
| | | 405 | | // Zero-radius cylinders use the segment feature family instead. |
| | 309 | 406 | | signs[8] = -1; |
| | 309 | 407 | | ImportCylinderCapsuleFeature(geometry.Scale, first); |
| | 309 | 408 | | ImportCylinderCapsuleFeature(b, second); |
| | 309 | 409 | | WideArithmetic.MultiplyMagnitudes(first, second, third); |
| | 309 | 410 | | WideArithmetic.MultiplyMagnitudes(third, third, product); |
| | 309 | 411 | | WideArithmetic.MultiplyMagnitudes(product, CylinderCapsuleFeatureSlot(values, 9), |
| | 309 | 412 | | CylinderCapsuleFeatureSlot(values, 10)); |
| | 309 | 413 | | signs[10] = 1; |
| | 309 | 414 | | return true; |
| | | 415 | | } |
| | | 416 | | |
| | | 417 | | private static void KeepCylinderCapsuleCandidate( |
| | | 418 | | ReadOnlySpan<ulong> values, ReadOnlySpan<int> signs, int gapSign, |
| | | 419 | | Span<ulong> bestValues, Span<int> bestSigns, ref int bestGapSign, ref bool hasBest) |
| | | 420 | | { |
| | 6543 | 421 | | var candidate = new ConvexContactCandidate(values, signs, gapSign); |
| | 6543 | 422 | | if (hasBest && CompareConvexContactCandidates(candidate, |
| | 6543 | 423 | | new ConvexContactCandidate(bestValues, bestSigns, bestGapSign)) >= 0) |
| | 4096 | 424 | | return; |
| | 2447 | 425 | | values.CopyTo(bestValues); |
| | 2447 | 426 | | signs.CopyTo(bestSigns); |
| | 2447 | 427 | | bestGapSign = gapSign; |
| | 2447 | 428 | | hasBest = true; |
| | 2447 | 429 | | } |
| | | 430 | | |
| | | 431 | | private static void KeepCylinderCapsuleDirection( |
| | | 432 | | in CylinderCapsuleFeatureGeometry geometry, CylinderCapsuleDirection direction, |
| | | 433 | | Span<ulong> values, Span<int> signs, Span<ulong> bestValues, Span<int> bestSigns, |
| | | 434 | | ref int bestGapSign, ref bool hasBest) |
| | | 435 | | { |
| | 2956 | 436 | | if (BuildCylinderCapsuleAxisCandidate(geometry, direction, values, signs, out int gapSign)) |
| | 2952 | 437 | | KeepCylinderCapsuleCandidate(values, signs, gapSign, bestValues, bestSigns, ref bestGapSign, ref hasBest); |
| | 2956 | 438 | | } |
| | | 439 | | |
| | | 440 | | internal static bool TryGetCenteredFiniteCylinderCapsuleContact( |
| | | 441 | | Vector3d cylinderCenter, FixedQuaternion cylinderRotation, Vector3d cylinderLocalAxis, |
| | | 442 | | Fixed64 cylinderLength, Fixed64 cylinderRadius, |
| | | 443 | | Vector3d capsuleCenter, FixedQuaternion capsuleRotation, Vector3d capsuleLocalAxis, |
| | | 444 | | Fixed64 capsuleLength, Fixed64 capsuleRadius, out FixedContactAnchors contact) |
| | | 445 | | { |
| | 908 | 446 | | if (!TryGetCenteredFiniteCylinderCapsulePenetration( |
| | 908 | 447 | | cylinderCenter, cylinderRotation, cylinderLocalAxis, Signed192.Raw(cylinderLength), cylinderRadius, |
| | 908 | 448 | | capsuleCenter, capsuleRotation, capsuleLocalAxis, capsuleLength, capsuleRadius, |
| | 908 | 449 | | out Vector3d normal, out Fixed64 depth, out bool depthIsClamped)) |
| | | 450 | | { |
| | 353 | 451 | | contact = default; |
| | 353 | 452 | | return false; |
| | | 453 | | } |
| | 555 | 454 | | contact = CreateCylinderCapsuleFeatureContact(normal, depth, depthIsClamped, |
| | 555 | 455 | | cylinderCenter, cylinderRotation, cylinderLocalAxis, cylinderLength, cylinderRadius, |
| | 555 | 456 | | capsuleCenter, capsuleRotation, capsuleLocalAxis, capsuleLength, capsuleRadius); |
| | 555 | 457 | | return true; |
| | | 458 | | } |
| | | 459 | | |
| | | 460 | | /// <summary> |
| | | 461 | | /// Exact closed relation for an admitted finite cylinder and capsule. The |
| | | 462 | | /// positive cylinder full length may occupy 64 unsigned raw bits, including |
| | | 463 | | /// twice a slab half-thickness; all other authored inputs retain their |
| | | 464 | | /// ordinary admission bounds. The cylinder-to-capsule normal and complete |
| | | 465 | | /// nearest-even depth are rounded only after exact feature selection. |
| | | 466 | | /// Separation returns default outputs; clamping reports conceptual overflow. |
| | | 467 | | /// </summary> |
| | | 468 | | internal static bool TryGetCenteredFiniteCylinderCapsulePenetration( |
| | | 469 | | Vector3d cylinderCenter, FixedQuaternion cylinderRotation, Vector3d cylinderLocalAxis, |
| | | 470 | | Signed192 cylinderLength, Fixed64 cylinderRadius, |
| | | 471 | | Vector3d capsuleCenter, FixedQuaternion capsuleRotation, Vector3d capsuleLocalAxis, |
| | | 472 | | Fixed64 capsuleLength, Fixed64 capsuleRadius, |
| | | 473 | | out Vector3d normal, out Fixed64 depth, out bool depthIsClamped) |
| | | 474 | | { |
| | 942 | 475 | | WideOrientedBox.GetRotatedLocalAxisNumerators(cylinderRotation, cylinderLocalAxis, |
| | 942 | 476 | | out Signed192 ax, out Signed192 ay, out Signed192 az, out Signed192 ad); |
| | 942 | 477 | | WideOrientedBox.GetRotatedLocalAxisNumerators(capsuleRotation, capsuleLocalAxis, |
| | 942 | 478 | | out Signed192 bx, out Signed192 by, out Signed192 bz, out Signed192 bd); |
| | 942 | 479 | | var cylinder = new RigidAxis3(ax, ay, az, ad); |
| | 942 | 480 | | var capsule = new RigidAxis3(bx, by, bz, bd); |
| | | 481 | | // Full length < 2^64 keeps scaled half-axes < 2^229 and endpoint |
| | | 482 | | // offsets < 2^231; the existing direction/candidate widths still hold. |
| | 942 | 483 | | var geometry = new CylinderCapsuleFeatureGeometry(cylinderCenter, cylinder, cylinderLength, |
| | 942 | 484 | | cylinderRadius, capsuleCenter, capsule, Signed192.Raw(capsuleLength)); |
| | | 485 | | const int valueCount = ConvexContactCandidate.Words * ConvexContactCandidate.Slots; |
| | 942 | 486 | | Span<ulong> values = stackalloc ulong[valueCount]; |
| | 942 | 487 | | Span<int> signs = stackalloc int[ConvexContactCandidate.Slots]; |
| | 942 | 488 | | Span<ulong> bestValues = stackalloc ulong[valueCount]; |
| | 942 | 489 | | Span<int> bestSigns = stackalloc int[ConvexContactCandidate.Slots]; |
| | 942 | 490 | | int bestGapSign = 0; |
| | 942 | 491 | | bool hasBest = false; |
| | | 492 | | |
| | | 493 | | // Stable ties: cap poles, capsule-interior principal boundaries, side |
| | | 494 | | // endpoint directions, degenerate segment endpoints, then ellipse. |
| | 942 | 495 | | KeepCylinderCapsuleDirection(geometry, new CylinderCapsuleDirection(cylinder.ToWide()), |
| | 942 | 496 | | values, signs, bestValues, bestSigns, ref bestGapSign, ref hasBest); |
| | 942 | 497 | | int capGapSign = bestGapSign; |
| | 942 | 498 | | Axis3 major = Cross(cylinder.ToWide(), capsule.ToWide()); |
| | 942 | 499 | | if (capsuleLength != Fixed64.Zero && !major.IsZero) |
| | | 500 | | { |
| | 752 | 501 | | Axis3 minor = Cross(capsule.ToWide(), major); |
| | 752 | 502 | | KeepCylinderCapsuleDirection(geometry, new CylinderCapsuleDirection(major), |
| | 752 | 503 | | values, signs, bestValues, bestSigns, ref bestGapSign, ref hasBest); |
| | 752 | 504 | | KeepCylinderCapsuleDirection(geometry, new CylinderCapsuleDirection(minor), |
| | 752 | 505 | | values, signs, bestValues, bestSigns, ref bestGapSign, ref hasBest); |
| | | 506 | | } |
| | | 507 | | |
| | 4646 | 508 | | for (int capsuleSign = -1; capsuleSign <= 1; capsuleSign += 2) |
| | | 509 | | { |
| | 1683 | 510 | | CylinderCapsuleDirection side = ProjectCylinderCapsulePerpendicular( |
| | 1683 | 511 | | geometry.EndpointOffset(0, capsuleSign), cylinder); |
| | 1683 | 512 | | _ = BuildCylinderCapsuleAxisCandidate(geometry, |
| | 1683 | 513 | | side.IsZero ? GetCylinderCapsulePerpendicular(cylinderRotation, cylinderLocalAxis) : side, |
| | 1683 | 514 | | values, signs, out int sideGapSign); |
| | 1683 | 515 | | KeepCylinderCapsuleCandidate(values, signs, sideGapSign, |
| | 1683 | 516 | | bestValues, bestSigns, ref bestGapSign, ref hasBest); |
| | 1683 | 517 | | if ((major.IsZero || capsuleLength == Fixed64.Zero) |
| | 1683 | 518 | | && (capGapSign >= 0 || sideGapSign >= 0)) |
| | | 519 | | { |
| | | 520 | | // Parallel cores sum to one finite cylinder. If the query is |
| | | 521 | | // inside either its axial slab or radial cylinder, the nearest |
| | | 522 | | // boundary is a cap or side; a rim cannot improve either gap. |
| | 163 | 523 | | return MaterializeCylinderCapsulePenetration( |
| | 163 | 524 | | new ConvexContactCandidate(bestValues, bestSigns, bestGapSign), capsuleRadius, |
| | 163 | 525 | | out normal, out depth, out depthIsClamped); |
| | | 526 | | } |
| | 8678 | 527 | | for (int cylinderSign = -1; cylinderSign <= 1; cylinderSign += 2) |
| | | 528 | | { |
| | 2958 | 529 | | if (BuildCylinderCapsuleEndpointRimCandidate(geometry, cylinderSign, capsuleSign, |
| | 2958 | 530 | | values, signs, out int rimGapSign)) |
| | | 531 | | { |
| | | 532 | | // The support point plus its outward residual proves a |
| | | 533 | | // closest point of the whole convex Minkowski sum. Unlike |
| | | 534 | | // an arbitrary separating direction, this is a global MTD. |
| | 139 | 535 | | return MaterializeCylinderCapsulePenetration( |
| | 139 | 536 | | new ConvexContactCandidate(values, signs, rimGapSign), capsuleRadius, |
| | 139 | 537 | | out normal, out depth, out depthIsClamped); |
| | | 538 | | } |
| | 2819 | 539 | | if (cylinderRadius == Fixed64.Zero) |
| | | 540 | | { |
| | 384 | 541 | | KeepCylinderCapsuleDirection(geometry, |
| | 384 | 542 | | new CylinderCapsuleDirection(geometry.EndpointOffset(cylinderSign, capsuleSign)), |
| | 384 | 543 | | values, signs, bestValues, bestSigns, ref bestGapSign, ref hasBest); |
| | | 544 | | } |
| | | 545 | | } |
| | | 546 | | // A zero-length core already returned through the cap/side |
| | | 547 | | // certificate or its unique outward endpoint/rim residual. |
| | | 548 | | } |
| | | 549 | | |
| | 640 | 550 | | if (capsuleLength != Fixed64.Zero && !major.IsZero) |
| | | 551 | | { |
| | 640 | 552 | | if (cylinderRadius == Fixed64.Zero) |
| | | 553 | | { |
| | 378 | 554 | | for (int cylinderSign = -1; cylinderSign <= 1; cylinderSign += 2) |
| | 126 | 555 | | KeepCylinderCapsuleDirection(geometry, |
| | 126 | 556 | | ProjectCylinderCapsulePerpendicular(geometry.EndpointOffset(cylinderSign, 0), capsule), |
| | 126 | 557 | | values, signs, bestValues, bestSigns, ref bestGapSign, ref hasBest); |
| | | 558 | | } |
| | 577 | 559 | | else if (GetAxisProjection(cylinder.ToWide(), capsule).IsZero) |
| | | 560 | | { |
| | 1452 | 561 | | for (int cylinderSign = -1; cylinderSign <= 1; cylinderSign += 2) |
| | 484 | 562 | | if (BuildCylinderCapsuleProjectedCornerCandidate(geometry, major, cylinderSign, |
| | 484 | 563 | | values, signs, out int cornerGapSign)) |
| | 12 | 564 | | KeepCylinderCapsuleCandidate(values, signs, cornerGapSign, |
| | 12 | 565 | | bestValues, bestSigns, ref bestGapSign, ref hasBest); |
| | | 566 | | } |
| | 335 | 567 | | else if (TryPrepareCylinderCapsuleEllipse(cylinderCenter, cylinder, cylinderLength, |
| | 335 | 568 | | cylinderRadius, capsuleCenter, capsule, out CylinderCapsuleEllipse ellipse) |
| | 335 | 569 | | && TryImproveCylinderCapsuleEllipse(ellipse, |
| | 335 | 570 | | new ConvexContactCandidate(bestValues, bestSigns, bestGapSign), cylinderRadius, capsuleRadius, |
| | 335 | 571 | | out bool intersects, out normal, out depth, out depthIsClamped)) |
| | | 572 | | { |
| | 149 | 573 | | return intersects; |
| | | 574 | | } |
| | | 575 | | } |
| | 491 | 576 | | return MaterializeCylinderCapsulePenetration( |
| | 491 | 577 | | new ConvexContactCandidate(bestValues, bestSigns, bestGapSign), capsuleRadius, |
| | 491 | 578 | | out normal, out depth, out depthIsClamped); |
| | | 579 | | } |
| | | 580 | | |
| | | 581 | | private static bool MaterializeCylinderCapsulePenetration( |
| | | 582 | | in ConvexContactCandidate candidate, Fixed64 capsuleRadius, |
| | | 583 | | out Vector3d normal, out Fixed64 depth, out bool depthIsClamped) |
| | | 584 | | { |
| | 840 | 585 | | normal = default; |
| | 840 | 586 | | depth = default; |
| | 840 | 587 | | depthIsClamped = false; |
| | 840 | 588 | | int depthSign = CompareConvexContactCandidateDepthToTwiceRaw(candidate, capsuleRadius, default); |
| | 840 | 589 | | if (depthSign < 0) |
| | 282 | 590 | | return false; |
| | 558 | 591 | | if (depthSign > 0) |
| | 519 | 592 | | GetRoundedConvexContactCandidateDepth(candidate, capsuleRadius, out depth, out depthIsClamped); |
| | 558 | 593 | | normal = GetConvexContactCandidateNormal(candidate); |
| | 558 | 594 | | return true; |
| | | 595 | | } |
| | | 596 | | |
| | | 597 | | private static FixedContactAnchors CreateCylinderCapsuleFeatureContact( |
| | | 598 | | Vector3d normal, Fixed64 depth, bool depthIsClamped, |
| | | 599 | | Vector3d cylinderCenter, FixedQuaternion cylinderRotation, Vector3d cylinderLocalAxis, |
| | | 600 | | Fixed64 cylinderLength, Fixed64 cylinderRadius, |
| | | 601 | | Vector3d capsuleCenter, FixedQuaternion capsuleRotation, Vector3d capsuleLocalAxis, |
| | 555 | 602 | | Fixed64 capsuleLength, Fixed64 capsuleRadius) => new( |
| | 555 | 603 | | WideGeometry.GetCenteredCylinderSupportAnchor(cylinderCenter, cylinderRotation, cylinderLocalAxis, |
| | 555 | 604 | | cylinderLength, cylinderRadius, normal), |
| | 555 | 605 | | WideGeometry.GetCenteredCapsuleSupportAnchor(capsuleCenter, capsuleRotation, capsuleLocalAxis, |
| | 555 | 606 | | capsuleLength, capsuleRadius, -normal), normal, depth, depthIsClamped); |
| | | 607 | | } |