| | | 1 | | //======================================================================= |
| | | 2 | | // FixedPointAnchor.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 | | /// Represents a 3D point as a local coordinate in one rigid world-space frame. |
| | | 14 | | /// </summary> |
| | | 15 | | /// <remarks> |
| | | 16 | | /// Keeping the frame components separate allows full-domain geometry to retain |
| | | 17 | | /// points whose rotated offset or absolute world coordinate is not representable |
| | | 18 | | /// as an intermediate <see cref="Fixed64"/> value. |
| | | 19 | | /// Relation-produced anchors can also retain an exact sub-lattice feature term |
| | | 20 | | /// that is intentionally not exposed as a public wide-arithmetic primitive. |
| | | 21 | | /// Consequently, anchors with identical rounded public local components can |
| | | 22 | | /// still compare unequal when they identify distinct exact features. |
| | | 23 | | /// </remarks> |
| | | 24 | | public readonly struct FixedPointAnchor : IEquatable<FixedPointAnchor> |
| | | 25 | | { |
| | | 26 | | /// <summary> |
| | | 27 | | /// Initializes a point anchor from a world origin, normalized local-to-world |
| | | 28 | | /// rotation, and local point. |
| | | 29 | | /// </summary> |
| | | 30 | | /// <exception cref="ArgumentException"> |
| | | 31 | | /// <paramref name="rotation"/> is not normalized. |
| | | 32 | | /// </exception> |
| | | 33 | | public FixedPointAnchor( |
| | | 34 | | Vector3d origin, |
| | | 35 | | FixedQuaternion rotation, |
| | | 36 | | Vector3d localPoint) |
| | 14563 | 37 | | : this(origin, rotation, localPoint, Vector3d.Zero) |
| | 14562 | 38 | | { } |
| | | 39 | | |
| | | 40 | | /// <summary> |
| | | 41 | | /// Initializes a point anchor from a world origin, normalized local-to-world |
| | | 42 | | /// rotation, and two additive local-space feature components. |
| | | 43 | | /// </summary> |
| | | 44 | | /// <exception cref="ArgumentException"> |
| | | 45 | | /// <paramref name="rotation"/> is not normalized. |
| | | 46 | | /// </exception> |
| | | 47 | | public FixedPointAnchor( |
| | | 48 | | Vector3d origin, |
| | | 49 | | FixedQuaternion rotation, |
| | | 50 | | Vector3d localPoint, |
| | | 51 | | Vector3d localDisplacement) |
| | 16400 | 52 | | : this( |
| | 16400 | 53 | | origin, |
| | 16400 | 54 | | rotation, |
| | 16400 | 55 | | localPoint, |
| | 16400 | 56 | | localDisplacement, |
| | 16400 | 57 | | Vector3d.Zero, |
| | 16400 | 58 | | default, |
| | 16400 | 59 | | validateRotation: true) |
| | 16399 | 60 | | { } |
| | | 61 | | |
| | | 62 | | internal FixedPointAnchor( |
| | | 63 | | Vector3d origin, |
| | | 64 | | FixedQuaternion rotation, |
| | | 65 | | Vector3d localPoint, |
| | | 66 | | Vector3d localDisplacement, |
| | | 67 | | FixedPointAnchorTerm3d exactLocalTerm) |
| | 9874 | 68 | | : this( |
| | 9874 | 69 | | origin, |
| | 9874 | 70 | | rotation, |
| | 9874 | 71 | | localPoint, |
| | 9874 | 72 | | localDisplacement, |
| | 9874 | 73 | | Vector3d.Zero, |
| | 9874 | 74 | | exactLocalTerm, |
| | 9874 | 75 | | validateRotation: true) |
| | 9874 | 76 | | { } |
| | | 77 | | |
| | | 78 | | internal static FixedPointAnchor FromValidatedFrame( |
| | | 79 | | Vector3d origin, |
| | | 80 | | FixedQuaternion rotation, |
| | | 81 | | Vector3d localPoint) => |
| | 742 | 82 | | new( |
| | 742 | 83 | | origin, |
| | 742 | 84 | | rotation, |
| | 742 | 85 | | localPoint, |
| | 742 | 86 | | Vector3d.Zero, |
| | 742 | 87 | | Vector3d.Zero, |
| | 742 | 88 | | default, |
| | 742 | 89 | | validateRotation: false); |
| | | 90 | | |
| | | 91 | | /// <summary>Retains an already rounded common-frame point under another world origin without narrowing its offset.< |
| | | 92 | | internal static FixedPointAnchor FromTranslatedPoint(Vector3d anchorOrigin, Vector3d samplingOrigin, Vector3d rounde |
| | | 93 | | { |
| | 344 | 94 | | if (Vector3d.TryAddSubtract(samplingOrigin, roundedOffset, anchorOrigin, out Vector3d localPoint)) |
| | 223 | 95 | | return FromValidatedFrame(anchorOrigin, FixedQuaternion.Identity, localPoint); |
| | | 96 | | |
| | 121 | 97 | | Span<Vector3d> components = stackalloc Vector3d[3]; |
| | 121 | 98 | | Vector3d residual = Vector3d.Zero; |
| | 968 | 99 | | for (int axis = 0; axis < 3; axis++) |
| | | 100 | | { |
| | 363 | 101 | | Signed192 remaining = WideArithmetic.SubtractSigned192(WideArithmetic.AddSigned192( |
| | 363 | 102 | | Signed192.Raw(samplingOrigin[axis]), Signed192.Raw(roundedOffset[axis])), Signed192.Raw(anchorOrigin[axi |
| | 2904 | 103 | | for (int term = 0; term < 3; term++) |
| | | 104 | | { |
| | 1089 | 105 | | long raw = WideArithmetic.SubtractSigned192(remaining, Signed192.Signed(long.MaxValue)).Sign > 0 ? long. |
| | 1089 | 106 | | : WideArithmetic.SubtractSigned192(remaining, Signed192.Signed(long.MinValue)).Sign < 0 ? long.MinVa |
| | 1089 | 107 | | : unchecked((long)remaining.Low); |
| | 1089 | 108 | | components[term][axis] = Fixed64.FromRaw(raw); |
| | 1089 | 109 | | remaining = WideArithmetic.SubtractSigned192(remaining, Signed192.Signed(raw)); |
| | | 110 | | } |
| | | 111 | | // This is a decomposition, not a clamp: all remainders are retained. |
| | | 112 | | // The exact three-term sum is in [3*Min+1,3*Max+1]. Signed scalar |
| | | 113 | | // asymmetry can leave one positive raw unit after three Max terms; |
| | | 114 | | // the existing feature residual stores that unit without a new field. |
| | 363 | 115 | | residual[axis] = Fixed64.FromRaw(unchecked((long)remaining.Low)); |
| | | 116 | | } |
| | 121 | 117 | | return new FixedPointAnchor(anchorOrigin, FixedQuaternion.Identity, components[0], components[1], components[2], |
| | 121 | 118 | | FixedPointAnchorTerm3d.CreateRadialSupport(residual, Fixed64.One, Vector3d.Zero), validateRotation: false); |
| | | 119 | | } |
| | | 120 | | |
| | | 121 | | private FixedPointAnchor( |
| | | 122 | | Vector3d origin, |
| | | 123 | | FixedQuaternion rotation, |
| | | 124 | | Vector3d localPoint, |
| | | 125 | | Vector3d localDisplacement, |
| | | 126 | | Vector3d localTranslation, |
| | | 127 | | FixedPointAnchorTerm3d exactLocalTerm, |
| | | 128 | | bool validateRotation) |
| | | 129 | | { |
| | 27144 | 130 | | if (validateRotation && !rotation.IsNormalized()) |
| | 1 | 131 | | throw new ArgumentException("The point-anchor rotation must be normalized.", nameof(rotation)); |
| | | 132 | | |
| | 27143 | 133 | | Origin = origin; |
| | 27143 | 134 | | Rotation = rotation; |
| | 27143 | 135 | | LocalPoint = localPoint; |
| | 27143 | 136 | | LocalDisplacement = localDisplacement; |
| | 27143 | 137 | | LocalTranslation = localTranslation; |
| | 27143 | 138 | | ExactLocalTerm = exactLocalTerm; |
| | 27143 | 139 | | } |
| | | 140 | | |
| | | 141 | | /// <summary> |
| | | 142 | | /// The world-space origin of the rigid frame. |
| | | 143 | | /// </summary> |
| | | 144 | | public Vector3d Origin { get; } |
| | | 145 | | |
| | | 146 | | /// <summary> |
| | | 147 | | /// The normalized local-to-world rotation of the rigid frame. |
| | | 148 | | /// </summary> |
| | | 149 | | public FixedQuaternion Rotation { get; } |
| | | 150 | | |
| | | 151 | | /// <summary> |
| | | 152 | | /// The point in the rigid frame's local coordinates. |
| | | 153 | | /// </summary> |
| | | 154 | | public Vector3d LocalPoint { get; } |
| | | 155 | | |
| | | 156 | | /// <summary> |
| | | 157 | | /// An additional local-space feature displacement that exact operations |
| | | 158 | | /// add to <see cref="LocalPoint"/> without a scalar intermediate. |
| | | 159 | | /// </summary> |
| | | 160 | | public Vector3d LocalDisplacement { get; } |
| | | 161 | | |
| | | 162 | | /// <summary> |
| | | 163 | | /// An independent local-space translation applied to the complete anchored |
| | | 164 | | /// point without merging it into either feature component. |
| | | 165 | | /// </summary> |
| | | 166 | | public Vector3d LocalTranslation { get; } |
| | | 167 | | |
| | | 168 | | internal FixedPointAnchorTerm3d ExactLocalTerm { get; } |
| | | 169 | | |
| | | 170 | | /// <summary> |
| | | 171 | | /// Returns an anchor for the same exact local feature with the specified |
| | | 172 | | /// independent local-space translation. |
| | | 173 | | /// </summary> |
| | | 174 | | public FixedPointAnchor WithLocalTranslation( |
| | | 175 | | Vector3d localTranslation) => |
| | 7 | 176 | | new( |
| | 7 | 177 | | Origin, |
| | 7 | 178 | | Rotation, |
| | 7 | 179 | | LocalPoint, |
| | 7 | 180 | | LocalDisplacement, |
| | 7 | 181 | | localTranslation, |
| | 7 | 182 | | ExactLocalTerm, |
| | 7 | 183 | | validateRotation: true); |
| | | 184 | | |
| | | 185 | | /// <summary> |
| | | 186 | | /// Attempts to materialize the absolute world point. |
| | | 187 | | /// </summary> |
| | | 188 | | public bool TryGetPoint(out Vector3d point) |
| | | 189 | | { |
| | 5144 | 190 | | if (!Rotation.IsNormalized()) |
| | | 191 | | { |
| | 1 | 192 | | point = default; |
| | 1 | 193 | | return false; |
| | | 194 | | } |
| | | 195 | | |
| | 5143 | 196 | | return WidePointAnchor3d.TryGetPoint( |
| | 5143 | 197 | | Origin, |
| | 5143 | 198 | | Rotation, |
| | 5143 | 199 | | LocalPoint, |
| | 5143 | 200 | | LocalDisplacement, |
| | 5143 | 201 | | LocalTranslation, |
| | 5143 | 202 | | ExactLocalTerm, |
| | 5143 | 203 | | out point); |
| | | 204 | | } |
| | | 205 | | |
| | | 206 | | /// <summary>Rounds on the world lattice before expressing the point relative to a sampling origin.</summary> |
| | | 207 | | internal bool TryGetPointInFrame(Vector3d samplingOrigin, out Vector3d point) |
| | | 208 | | { |
| | 354 | 209 | | if (!Rotation.IsNormalized()) |
| | | 210 | | { |
| | 1 | 211 | | point = default; |
| | 1 | 212 | | return false; |
| | | 213 | | } |
| | 353 | 214 | | return WidePointAnchor3d.TryGetPointInFrame(this, samplingOrigin, out point); |
| | | 215 | | } |
| | | 216 | | |
| | | 217 | | /// <summary> |
| | | 218 | | /// Attempts to materialize this point's world-space offset from |
| | | 219 | | /// <paramref name="other"/>. |
| | | 220 | | /// </summary> |
| | | 221 | | public bool TryGetOffsetFrom( |
| | | 222 | | in FixedPointAnchor other, |
| | | 223 | | out Vector3d offset) => |
| | 1563 | 224 | | TryGetScaledOffsetFrom(other, Fixed64.One, out offset); |
| | | 225 | | |
| | | 226 | | /// <summary> |
| | | 227 | | /// Attempts to materialize this point's world-space offset from |
| | | 228 | | /// <paramref name="other"/> after applying <paramref name="scale"/>. |
| | | 229 | | /// </summary> |
| | | 230 | | /// <remarks> |
| | | 231 | | /// The scale is applied to the complete conceptual difference before its |
| | | 232 | | /// final round-half-to-even conversion. |
| | | 233 | | /// </remarks> |
| | | 234 | | public bool TryGetScaledOffsetFrom( |
| | | 235 | | in FixedPointAnchor other, |
| | | 236 | | Fixed64 scale, |
| | | 237 | | out Vector3d offset) |
| | | 238 | | { |
| | 1838 | 239 | | if (!Rotation.IsNormalized() || !other.Rotation.IsNormalized()) |
| | | 240 | | { |
| | 2 | 241 | | offset = default; |
| | 2 | 242 | | return false; |
| | | 243 | | } |
| | 1836 | 244 | | if (scale == Fixed64.Zero) |
| | | 245 | | { |
| | 2 | 246 | | offset = Vector3d.Zero; |
| | 2 | 247 | | return true; |
| | | 248 | | } |
| | | 249 | | |
| | 1834 | 250 | | return WidePointAnchor3d.TryGetRelativeOffset( |
| | 1834 | 251 | | Origin, |
| | 1834 | 252 | | Rotation, |
| | 1834 | 253 | | LocalPoint, |
| | 1834 | 254 | | LocalDisplacement, |
| | 1834 | 255 | | LocalTranslation, |
| | 1834 | 256 | | ExactLocalTerm, |
| | 1834 | 257 | | other.Origin, |
| | 1834 | 258 | | other.Rotation, |
| | 1834 | 259 | | other.LocalPoint, |
| | 1834 | 260 | | other.LocalDisplacement, |
| | 1834 | 261 | | other.LocalTranslation, |
| | 1834 | 262 | | other.ExactLocalTerm, |
| | 1834 | 263 | | scale, |
| | 1834 | 264 | | out offset); |
| | | 265 | | } |
| | | 266 | | |
| | | 267 | | /// <summary> |
| | | 268 | | /// Attempts to project this point minus <paramref name="other"/> onto |
| | | 269 | | /// <paramref name="direction"/> with one final round-half-to-even |
| | | 270 | | /// conversion. |
| | | 271 | | /// </summary> |
| | | 272 | | public bool TryGetProjectedOffsetFrom( |
| | | 273 | | in FixedPointAnchor other, |
| | | 274 | | Vector3d direction, |
| | | 275 | | out Fixed64 projection) |
| | | 276 | | { |
| | 280 | 277 | | if (!Rotation.IsNormalized() || !other.Rotation.IsNormalized()) |
| | | 278 | | { |
| | 2 | 279 | | projection = default; |
| | 2 | 280 | | return false; |
| | | 281 | | } |
| | | 282 | | |
| | 278 | 283 | | return WidePointAnchor3d.TryGetProjectedOffset( |
| | 278 | 284 | | Origin, |
| | 278 | 285 | | Rotation, |
| | 278 | 286 | | LocalPoint, |
| | 278 | 287 | | LocalDisplacement, |
| | 278 | 288 | | LocalTranslation, |
| | 278 | 289 | | ExactLocalTerm, |
| | 278 | 290 | | other.Origin, |
| | 278 | 291 | | other.Rotation, |
| | 278 | 292 | | other.LocalPoint, |
| | 278 | 293 | | other.LocalDisplacement, |
| | 278 | 294 | | other.LocalTranslation, |
| | 278 | 295 | | other.ExactLocalTerm, |
| | 278 | 296 | | direction, |
| | 278 | 297 | | out projection); |
| | | 298 | | } |
| | | 299 | | |
| | | 300 | | /// <summary> |
| | | 301 | | /// Projects this point minus <paramref name="other"/> onto |
| | | 302 | | /// <paramref name="direction"/> and returns zero for nonpositive results, |
| | | 303 | | /// the positive projection floored to Q32.32, or |
| | | 304 | | /// <see cref="Fixed64.MaxValue"/> when only the final result is |
| | | 305 | | /// unrepresentable. |
| | | 306 | | /// </summary> |
| | | 307 | | /// <exception cref="InvalidOperationException"> |
| | | 308 | | /// This anchor does not contain a normalized rotation. |
| | | 309 | | /// </exception> |
| | | 310 | | /// <exception cref="ArgumentException"> |
| | | 311 | | /// <paramref name="other"/> does not contain a normalized rotation. |
| | | 312 | | /// </exception> |
| | | 313 | | public Fixed64 ProjectNonNegativeOffsetFrom( |
| | | 314 | | in FixedPointAnchor other, |
| | | 315 | | Vector3d direction) |
| | | 316 | | { |
| | 291 | 317 | | if (!Rotation.IsNormalized()) |
| | | 318 | | { |
| | 1 | 319 | | throw new InvalidOperationException( |
| | 1 | 320 | | "The point anchor must have a normalized rotation."); |
| | | 321 | | } |
| | 290 | 322 | | if (!other.Rotation.IsNormalized()) |
| | | 323 | | { |
| | 1 | 324 | | throw new ArgumentException( |
| | 1 | 325 | | "The other point anchor must have a normalized rotation.", |
| | 1 | 326 | | nameof(other)); |
| | | 327 | | } |
| | | 328 | | |
| | 289 | 329 | | return WidePointAnchor3d.ProjectNonNegativeOffset( |
| | 289 | 330 | | Origin, |
| | 289 | 331 | | Rotation, |
| | 289 | 332 | | LocalPoint, |
| | 289 | 333 | | LocalDisplacement, |
| | 289 | 334 | | LocalTranslation, |
| | 289 | 335 | | ExactLocalTerm, |
| | 289 | 336 | | other.Origin, |
| | 289 | 337 | | other.Rotation, |
| | 289 | 338 | | other.LocalPoint, |
| | 289 | 339 | | other.LocalDisplacement, |
| | 289 | 340 | | other.LocalTranslation, |
| | 289 | 341 | | other.ExactLocalTerm, |
| | 289 | 342 | | direction); |
| | | 343 | | } |
| | | 344 | | |
| | | 345 | | /// <summary> |
| | | 346 | | /// Compares the exact squared distance from this point to two other |
| | | 347 | | /// anchored points. |
| | | 348 | | /// </summary> |
| | | 349 | | /// <returns> |
| | | 350 | | /// A negative value when <paramref name="first"/> is closer, zero when the |
| | | 351 | | /// distances are equal, or a positive value when |
| | | 352 | | /// <paramref name="second"/> is closer. |
| | | 353 | | /// </returns> |
| | | 354 | | public int CompareSquaredDistance( |
| | | 355 | | in FixedPointAnchor first, |
| | | 356 | | in FixedPointAnchor second) |
| | | 357 | | { |
| | 45 | 358 | | if (!Rotation.IsNormalized()) |
| | | 359 | | { |
| | 1 | 360 | | throw new InvalidOperationException( |
| | 1 | 361 | | "The reference point anchor must have a normalized rotation."); |
| | | 362 | | } |
| | 44 | 363 | | if (!first.Rotation.IsNormalized()) |
| | | 364 | | { |
| | 1 | 365 | | throw new ArgumentException( |
| | 1 | 366 | | "The first point anchor must have a normalized rotation.", |
| | 1 | 367 | | nameof(first)); |
| | | 368 | | } |
| | 43 | 369 | | if (!second.Rotation.IsNormalized()) |
| | | 370 | | { |
| | 1 | 371 | | throw new ArgumentException( |
| | 1 | 372 | | "The second point anchor must have a normalized rotation.", |
| | 1 | 373 | | nameof(second)); |
| | | 374 | | } |
| | | 375 | | |
| | 42 | 376 | | return WidePointAnchor3d.CompareSquaredDistances( |
| | 42 | 377 | | this, |
| | 42 | 378 | | first, |
| | 42 | 379 | | second); |
| | | 380 | | } |
| | | 381 | | |
| | | 382 | | /// <summary> |
| | | 383 | | /// Attempts to express this point in another rigid frame's local |
| | | 384 | | /// coordinates without first materializing its absolute world position. |
| | | 385 | | /// </summary> |
| | | 386 | | public bool TryGetLocalPointIn( |
| | | 387 | | Vector3d frameOrigin, |
| | | 388 | | FixedQuaternion frameRotation, |
| | | 389 | | out Vector3d localPoint) |
| | | 390 | | { |
| | 4812 | 391 | | if (!frameRotation.IsNormalized()) |
| | | 392 | | { |
| | 1 | 393 | | localPoint = default; |
| | 1 | 394 | | return false; |
| | | 395 | | } |
| | 4811 | 396 | | if (!Rotation.IsNormalized()) |
| | | 397 | | { |
| | 1 | 398 | | localPoint = default; |
| | 1 | 399 | | return false; |
| | | 400 | | } |
| | | 401 | | |
| | 4810 | 402 | | return WidePointAnchor3d.TryGetLocalPointIn( |
| | 4810 | 403 | | Origin, |
| | 4810 | 404 | | Rotation, |
| | 4810 | 405 | | LocalPoint, |
| | 4810 | 406 | | LocalDisplacement, |
| | 4810 | 407 | | LocalTranslation, |
| | 4810 | 408 | | ExactLocalTerm, |
| | 4810 | 409 | | frameOrigin, |
| | 4810 | 410 | | frameRotation, |
| | 4810 | 411 | | out localPoint); |
| | | 412 | | } |
| | | 413 | | |
| | | 414 | | /// <summary> |
| | | 415 | | /// Attempts to express this exact conceptual point in another rigid frame |
| | | 416 | | /// without discarding any sub-lattice feature information. |
| | | 417 | | /// </summary> |
| | | 418 | | /// <remarks> |
| | | 419 | | /// The operation fails when the target frame would require a general |
| | | 420 | | /// rational local coordinate that a compact point anchor cannot retain. |
| | | 421 | | /// </remarks> |
| | | 422 | | public bool TryReframe( |
| | | 423 | | Vector3d frameOrigin, |
| | | 424 | | FixedQuaternion frameRotation, |
| | | 425 | | out FixedPointAnchor anchor) |
| | | 426 | | { |
| | 7 | 427 | | if (!Rotation.IsNormalized() |
| | 7 | 428 | | || !frameRotation.IsNormalized()) |
| | | 429 | | { |
| | 2 | 430 | | anchor = default; |
| | 2 | 431 | | return false; |
| | | 432 | | } |
| | 5 | 433 | | if (Origin == frameOrigin && Rotation == frameRotation) |
| | | 434 | | { |
| | 1 | 435 | | anchor = this; |
| | 1 | 436 | | return true; |
| | | 437 | | } |
| | 4 | 438 | | if (!TryGetLocalPointIn( |
| | 4 | 439 | | frameOrigin, |
| | 4 | 440 | | frameRotation, |
| | 4 | 441 | | out Vector3d localPoint)) |
| | | 442 | | { |
| | 1 | 443 | | anchor = default; |
| | 1 | 444 | | return false; |
| | | 445 | | } |
| | | 446 | | |
| | 3 | 447 | | var candidate = new FixedPointAnchor( |
| | 3 | 448 | | frameOrigin, |
| | 3 | 449 | | frameRotation, |
| | 3 | 450 | | localPoint); |
| | 3 | 451 | | if (!WidePointAnchor3d.RepresentsSamePoint(this, candidate)) |
| | | 452 | | { |
| | 1 | 453 | | anchor = default; |
| | 1 | 454 | | return false; |
| | | 455 | | } |
| | | 456 | | |
| | 2 | 457 | | anchor = candidate; |
| | 2 | 458 | | return true; |
| | | 459 | | } |
| | | 460 | | |
| | | 461 | | /// <summary> |
| | | 462 | | /// Compares the complete local feature identity of this anchor with |
| | | 463 | | /// <paramref name="other"/> in deterministic component order. |
| | | 464 | | /// </summary> |
| | | 465 | | /// <remarks> |
| | | 466 | | /// Frame origin, rotation, and <see cref="LocalTranslation"/> are |
| | | 467 | | /// intentionally excluded because they move a feature without changing |
| | | 468 | | /// its local identity. Exact sub-lattice centered-axis residuals are |
| | | 469 | | /// included after the public feature components, so distinct features |
| | | 470 | | /// cannot collapse when their rounded local coordinates are equal. |
| | | 471 | | /// </remarks> |
| | | 472 | | public int CompareLocalFeature(in FixedPointAnchor other) |
| | | 473 | | { |
| | 266 | 474 | | int comparison = CompareRaw( |
| | 266 | 475 | | LocalPoint.X.m_rawValue, |
| | 266 | 476 | | other.LocalPoint.X.m_rawValue); |
| | 266 | 477 | | if (comparison != 0) |
| | 2 | 478 | | return comparison; |
| | 264 | 479 | | comparison = CompareRaw( |
| | 264 | 480 | | LocalPoint.Y.m_rawValue, |
| | 264 | 481 | | other.LocalPoint.Y.m_rawValue); |
| | 264 | 482 | | if (comparison != 0) |
| | 2 | 483 | | return comparison; |
| | 262 | 484 | | comparison = CompareRaw( |
| | 262 | 485 | | LocalPoint.Z.m_rawValue, |
| | 262 | 486 | | other.LocalPoint.Z.m_rawValue); |
| | 262 | 487 | | if (comparison != 0) |
| | 2 | 488 | | return comparison; |
| | 260 | 489 | | comparison = CompareRaw( |
| | 260 | 490 | | LocalDisplacement.X.m_rawValue, |
| | 260 | 491 | | other.LocalDisplacement.X.m_rawValue); |
| | 260 | 492 | | if (comparison != 0) |
| | 2 | 493 | | return comparison; |
| | 258 | 494 | | comparison = CompareRaw( |
| | 258 | 495 | | LocalDisplacement.Y.m_rawValue, |
| | 258 | 496 | | other.LocalDisplacement.Y.m_rawValue); |
| | 258 | 497 | | if (comparison != 0) |
| | 2 | 498 | | return comparison; |
| | 256 | 499 | | comparison = CompareRaw( |
| | 256 | 500 | | LocalDisplacement.Z.m_rawValue, |
| | 256 | 501 | | other.LocalDisplacement.Z.m_rawValue); |
| | 256 | 502 | | if (comparison != 0) |
| | 2 | 503 | | return comparison; |
| | 254 | 504 | | comparison = CompareRaw( |
| | 254 | 505 | | ExactLocalTerm.X, |
| | 254 | 506 | | other.ExactLocalTerm.X); |
| | 254 | 507 | | if (comparison != 0) |
| | 7 | 508 | | return comparison; |
| | 247 | 509 | | comparison = CompareRaw( |
| | 247 | 510 | | ExactLocalTerm.Y, |
| | 247 | 511 | | other.ExactLocalTerm.Y); |
| | 247 | 512 | | return comparison != 0 |
| | 247 | 513 | | ? comparison |
| | 247 | 514 | | : CompareRaw( |
| | 247 | 515 | | ExactLocalTerm.Z, |
| | 247 | 516 | | other.ExactLocalTerm.Z); |
| | | 517 | | } |
| | | 518 | | |
| | | 519 | | /// <summary> |
| | | 520 | | /// Returns a stable 64-bit hash of the complete local feature identity. |
| | | 521 | | /// </summary> |
| | | 522 | | public ulong GetLocalFeatureHash64() |
| | | 523 | | { |
| | 24 | 524 | | ulong hash = 14695981039346656037UL; |
| | 24 | 525 | | KeepHash(ref hash, LocalPoint.X.m_rawValue); |
| | 24 | 526 | | KeepHash(ref hash, LocalPoint.Y.m_rawValue); |
| | 24 | 527 | | KeepHash(ref hash, LocalPoint.Z.m_rawValue); |
| | 24 | 528 | | KeepHash(ref hash, LocalDisplacement.X.m_rawValue); |
| | 24 | 529 | | KeepHash(ref hash, LocalDisplacement.Y.m_rawValue); |
| | 24 | 530 | | KeepHash(ref hash, LocalDisplacement.Z.m_rawValue); |
| | 24 | 531 | | KeepHash(ref hash, ExactLocalTerm.X); |
| | 24 | 532 | | KeepHash(ref hash, ExactLocalTerm.Y); |
| | 24 | 533 | | KeepHash(ref hash, ExactLocalTerm.Z); |
| | 24 | 534 | | return hash; |
| | | 535 | | } |
| | | 536 | | |
| | | 537 | | /// <summary> |
| | | 538 | | /// Returns whether both anchors have identical frames, rounded local |
| | | 539 | | /// components, and any retained exact feature identity. |
| | | 540 | | /// </summary> |
| | | 541 | | public bool Equals(FixedPointAnchor other) => |
| | 830 | 542 | | Origin.Equals(other.Origin) |
| | 830 | 543 | | && Rotation.Equals(other.Rotation) |
| | 830 | 544 | | && LocalPoint.Equals(other.LocalPoint) |
| | 830 | 545 | | && LocalDisplacement.Equals(other.LocalDisplacement) |
| | 830 | 546 | | && LocalTranslation.Equals(other.LocalTranslation) |
| | 830 | 547 | | && ExactLocalTerm.Equals(other.ExactLocalTerm); |
| | | 548 | | |
| | | 549 | | /// <summary> |
| | | 550 | | /// Returns a hash code for the complete frame and exact feature identity. |
| | | 551 | | /// </summary> |
| | | 552 | | public override bool Equals(object? obj) => |
| | 438 | 553 | | obj is FixedPointAnchor other && Equals(other); |
| | | 554 | | |
| | | 555 | | /// <inheritdoc /> |
| | | 556 | | public override int GetHashCode() |
| | | 557 | | { |
| | | 558 | | unchecked |
| | | 559 | | { |
| | 5 | 560 | | int hash = 17; |
| | 5 | 561 | | hash = (hash * 31) + Origin.GetHashCode(); |
| | 5 | 562 | | hash = (hash * 31) + Rotation.GetHashCode(); |
| | 5 | 563 | | hash = (hash * 31) + LocalPoint.GetHashCode(); |
| | 5 | 564 | | hash = (hash * 31) + LocalDisplacement.GetHashCode(); |
| | 5 | 565 | | hash = (hash * 31) + LocalTranslation.GetHashCode(); |
| | 5 | 566 | | if (!ExactLocalTerm.IsZero) |
| | 2 | 567 | | hash = (hash * 31) + ExactLocalTerm.GetHashCode(); |
| | 5 | 568 | | return hash; |
| | | 569 | | } |
| | | 570 | | } |
| | | 571 | | |
| | | 572 | | /// <summary> |
| | | 573 | | /// Returns whether two point anchors have identical frame components. |
| | | 574 | | /// </summary> |
| | | 575 | | public static bool operator ==( |
| | | 576 | | FixedPointAnchor left, |
| | | 577 | | FixedPointAnchor right) => |
| | 289 | 578 | | left.Equals(right); |
| | | 579 | | |
| | | 580 | | /// <summary> |
| | | 581 | | /// Returns whether two point anchors have different frame components. |
| | | 582 | | /// </summary> |
| | | 583 | | public static bool operator !=( |
| | | 584 | | FixedPointAnchor left, |
| | | 585 | | FixedPointAnchor right) => |
| | 1 | 586 | | !left.Equals(right); |
| | | 587 | | |
| | | 588 | | private static int CompareRaw(long left, long right) => |
| | 2307 | 589 | | left < right ? -1 : left > right ? 1 : 0; |
| | | 590 | | |
| | | 591 | | private static void KeepHash(ref ulong hash, long value) |
| | | 592 | | { |
| | 216 | 593 | | hash ^= unchecked((ulong)value); |
| | 216 | 594 | | hash *= 1099511628211UL; |
| | 216 | 595 | | } |
| | | 596 | | } |