[9730] | 1 | namespace MIConvexHull
|
---|
| 2 | {
|
---|
| 3 | using System;
|
---|
| 4 | using System.Collections.Generic;
|
---|
| 5 | using System.Linq;
|
---|
| 6 |
|
---|
| 7 | internal class ConvexHullInternal
|
---|
| 8 | {
|
---|
| 9 | bool Computed;
|
---|
| 10 | readonly int Dimension;
|
---|
| 11 |
|
---|
| 12 | List<VertexWrap> InputVertices;
|
---|
| 13 | List<VertexWrap> ConvexHull;
|
---|
| 14 | FaceList UnprocessedFaces;
|
---|
| 15 | List<ConvexFaceInternal> ConvexFaces;
|
---|
| 16 |
|
---|
| 17 | #region "Buffers"
|
---|
| 18 | VertexWrap CurrentVertex;
|
---|
| 19 | double MaxDistance;
|
---|
| 20 | VertexWrap FurthestVertex;
|
---|
| 21 |
|
---|
| 22 | /// <summary>
|
---|
| 23 | /// The centroid of the currently computed hull.
|
---|
| 24 | /// </summary>
|
---|
| 25 | double[] Center;
|
---|
| 26 |
|
---|
| 27 | // Buffers for normal computation.
|
---|
| 28 | double[] ntX, ntY, ntZ;
|
---|
| 29 | double[] nDNormalSolveVector;
|
---|
| 30 | double[,] nDMatrix;
|
---|
| 31 | double[][] jaggedNDMatrix;
|
---|
| 32 |
|
---|
| 33 | ConvexFaceInternal[] UpdateBuffer;
|
---|
| 34 | int[] UpdateIndices;
|
---|
| 35 |
|
---|
| 36 |
|
---|
| 37 | Stack<ConvexFaceInternal> TraverseStack;
|
---|
| 38 | Stack<ConvexFaceInternal> RecycledFaceStack;
|
---|
| 39 | Stack<FaceConnector> ConnectorStack;
|
---|
| 40 | Stack<VertexBuffer> EmptyBufferStack;
|
---|
| 41 | VertexBuffer EmptyBuffer; // this is used for VerticesBeyond for faces that are on the convex hull
|
---|
| 42 | VertexBuffer BeyondBuffer;
|
---|
| 43 | List<ConvexFaceInternal> AffectedFaceBuffer;
|
---|
[9945] | 44 | List<DeferredFace> ConeFaceBuffer;
|
---|
| 45 | Stack<DeferredFace> ConeFaceStack;
|
---|
| 46 | HashSet<VertexWrap> SingularVertices;
|
---|
[9730] | 47 |
|
---|
| 48 | const int ConnectorTableSize = 2017;
|
---|
| 49 | ConnectorList[] ConnectorTable;
|
---|
| 50 |
|
---|
| 51 | #endregion
|
---|
| 52 |
|
---|
| 53 | /// <summary>
|
---|
| 54 | /// Initialize buffers and lists.
|
---|
| 55 | /// </summary>
|
---|
| 56 | void Initialize()
|
---|
| 57 | {
|
---|
| 58 | ConvexHull = new List<VertexWrap>();
|
---|
| 59 | UnprocessedFaces = new FaceList(); // new LinkedList<ConvexFaceInternal>();
|
---|
| 60 | ConvexFaces = new List<ConvexFaceInternal>();
|
---|
| 61 |
|
---|
| 62 | Center = new double[Dimension];
|
---|
| 63 | ntX = new double[Dimension];
|
---|
| 64 | ntY = new double[Dimension];
|
---|
| 65 | ntZ = new double[Dimension];
|
---|
| 66 | TraverseStack = new Stack<ConvexFaceInternal>();
|
---|
| 67 | UpdateBuffer = new ConvexFaceInternal[Dimension];
|
---|
| 68 | UpdateIndices = new int[Dimension];
|
---|
| 69 | RecycledFaceStack = new Stack<ConvexFaceInternal>();
|
---|
| 70 | ConnectorStack = new Stack<FaceConnector>();
|
---|
| 71 | EmptyBufferStack = new Stack<VertexBuffer>();
|
---|
| 72 | EmptyBuffer = new VertexBuffer();
|
---|
| 73 | AffectedFaceBuffer = new List<ConvexFaceInternal>();
|
---|
[9945] | 74 | ConeFaceBuffer = new List<DeferredFace>();
|
---|
| 75 | ConeFaceStack = new Stack<DeferredFace>();
|
---|
| 76 | SingularVertices = new HashSet<VertexWrap>();
|
---|
[9730] | 77 | BeyondBuffer = new VertexBuffer();
|
---|
| 78 |
|
---|
| 79 | ConnectorTable = Enumerable.Range(0, ConnectorTableSize).Select(_ => new ConnectorList()).ToArray();
|
---|
| 80 |
|
---|
| 81 | nDNormalSolveVector = new double[Dimension];
|
---|
| 82 | jaggedNDMatrix = new double[Dimension][];
|
---|
| 83 | for (var i = 0; i < Dimension; i++)
|
---|
| 84 | {
|
---|
| 85 | nDNormalSolveVector[i] = 1.0;
|
---|
| 86 | jaggedNDMatrix[i] = new double[Dimension];
|
---|
| 87 | }
|
---|
| 88 | nDMatrix = new double[Dimension, Dimension];
|
---|
| 89 | }
|
---|
| 90 |
|
---|
| 91 | /// <summary>
|
---|
| 92 | /// Check the dimensionality of the input data.
|
---|
| 93 | /// </summary>
|
---|
| 94 | int DetermineDimension()
|
---|
| 95 | {
|
---|
| 96 | var r = new Random();
|
---|
| 97 | var VCount = InputVertices.Count;
|
---|
| 98 | var dimensions = new List<int>();
|
---|
| 99 | for (var i = 0; i < 10; i++)
|
---|
| 100 | dimensions.Add(InputVertices[r.Next(VCount)].Vertex.Position.Length);
|
---|
| 101 | var dimension = dimensions.Min();
|
---|
| 102 | if (dimension != dimensions.Max()) throw new ArgumentException("Invalid input data (non-uniform dimension).");
|
---|
| 103 | return dimension;
|
---|
| 104 | }
|
---|
| 105 |
|
---|
| 106 | /// <summary>
|
---|
| 107 | /// Create the first faces from (dimension + 1) vertices.
|
---|
| 108 | /// </summary>
|
---|
| 109 | /// <returns></returns>
|
---|
| 110 | ConvexFaceInternal[] InitiateFaceDatabase()
|
---|
| 111 | {
|
---|
| 112 | var faces = new ConvexFaceInternal[Dimension + 1];
|
---|
| 113 |
|
---|
| 114 | for (var i = 0; i < Dimension + 1; i++)
|
---|
| 115 | {
|
---|
| 116 | var vertices = ConvexHull.Where((_, j) => i != j).ToArray(); // Skips the i-th vertex
|
---|
| 117 | var newFace = new ConvexFaceInternal(Dimension, new VertexBuffer());
|
---|
| 118 | newFace.Vertices = vertices;
|
---|
| 119 | Array.Sort(vertices, VertexWrapComparer.Instance);
|
---|
| 120 | CalculateFacePlane(newFace);
|
---|
| 121 | faces[i] = newFace;
|
---|
| 122 | }
|
---|
| 123 |
|
---|
| 124 | // update the adjacency (check all pairs of faces)
|
---|
| 125 | for (var i = 0; i < Dimension; i++)
|
---|
| 126 | {
|
---|
| 127 | for (var j = i + 1; j < Dimension + 1; j++) UpdateAdjacency(faces[i], faces[j]);
|
---|
| 128 | }
|
---|
| 129 |
|
---|
| 130 | return faces;
|
---|
| 131 | }
|
---|
| 132 |
|
---|
| 133 | /// <summary>
|
---|
| 134 | /// Calculates the normal and offset of the hyper-plane given by the face's vertices.
|
---|
| 135 | /// </summary>
|
---|
| 136 | /// <param name="face"></param>
|
---|
| 137 | /// <returns></returns>
|
---|
[9945] | 138 | private bool CalculateFacePlane(ConvexFaceInternal face)
|
---|
[9730] | 139 | {
|
---|
| 140 | var vertices = face.Vertices;
|
---|
| 141 | var normal = face.Normal;
|
---|
| 142 | FindNormalVector(vertices, normal);
|
---|
| 143 |
|
---|
[9945] | 144 | if (double.IsNaN(normal[0]))
|
---|
| 145 | {
|
---|
| 146 | return false;
|
---|
| 147 | //ThrowSingular();
|
---|
| 148 | }
|
---|
[9730] | 149 |
|
---|
| 150 | double offset = 0.0;
|
---|
| 151 | double centerDistance = 0.0;
|
---|
| 152 | var fi = vertices[0].PositionData;
|
---|
| 153 | for (int i = 0; i < Dimension; i++)
|
---|
| 154 | {
|
---|
| 155 | double n = normal[i];
|
---|
| 156 | offset += n * fi[i];
|
---|
| 157 | centerDistance += n * Center[i];
|
---|
| 158 | }
|
---|
| 159 | face.Offset = -offset;
|
---|
| 160 | centerDistance -= offset;
|
---|
| 161 |
|
---|
| 162 | if (centerDistance > 0)
|
---|
| 163 | {
|
---|
| 164 | for (int i = 0; i < Dimension; i++) normal[i] = -normal[i];
|
---|
| 165 | face.Offset = offset;
|
---|
| 166 | face.IsNormalFlipped = true;
|
---|
| 167 | }
|
---|
| 168 | else face.IsNormalFlipped = false;
|
---|
[9945] | 169 |
|
---|
| 170 | return true;
|
---|
[9730] | 171 | }
|
---|
| 172 |
|
---|
| 173 | /// <summary>
|
---|
| 174 | /// Check if the vertex is "visible" from the face.
|
---|
| 175 | /// The vertex is "over face" if the return value is >= 0.
|
---|
| 176 | /// </summary>
|
---|
| 177 | /// <param name="v"></param>
|
---|
| 178 | /// <param name="f"></param>
|
---|
| 179 | /// <returns>The vertex is "over face" if the result is positive.</returns>
|
---|
| 180 | double GetVertexDistance(VertexWrap v, ConvexFaceInternal f)
|
---|
| 181 | {
|
---|
| 182 | double[] normal = f.Normal;
|
---|
| 183 | double[] p = v.PositionData;
|
---|
| 184 | double distance = f.Offset;
|
---|
| 185 | for (int i = 0; i < Dimension; i++) distance += normal[i] * p[i];
|
---|
| 186 | return distance;
|
---|
| 187 | }
|
---|
| 188 |
|
---|
| 189 | //unsafe double GetVertexDistance(VertexWrap v, ConvexFaceInternal f)
|
---|
| 190 | //{
|
---|
| 191 | // fixed (double* pNormal = f.Normal)
|
---|
| 192 | // fixed (double* pP = v.PositionData)
|
---|
| 193 | // {
|
---|
| 194 | // double* normal = pNormal;
|
---|
| 195 | // double* p = pP;
|
---|
| 196 |
|
---|
| 197 | // double distance = f.Offset;
|
---|
| 198 | // for (int i = 0; i < Dimension; i++)
|
---|
| 199 | // {
|
---|
| 200 | // distance += (*normal) * (*p);
|
---|
| 201 | // normal++;
|
---|
| 202 | // p++;
|
---|
| 203 | // }
|
---|
| 204 | // return distance;
|
---|
| 205 | // }
|
---|
| 206 | //}
|
---|
| 207 |
|
---|
| 208 | /// <summary>
|
---|
| 209 | /// Tags all faces seen from the current vertex with 1.
|
---|
| 210 | /// </summary>
|
---|
| 211 | /// <param name="currentFace"></param>
|
---|
| 212 | void TagAffectedFaces(ConvexFaceInternal currentFace)
|
---|
| 213 | {
|
---|
| 214 | AffectedFaceBuffer.Clear();
|
---|
| 215 | AffectedFaceBuffer.Add(currentFace);
|
---|
| 216 | TraverseAffectedFaces(currentFace);
|
---|
| 217 | }
|
---|
| 218 |
|
---|
| 219 | /// <summary>
|
---|
| 220 | /// Recursively traverse all the relevant faces.
|
---|
| 221 | /// </summary>
|
---|
| 222 | void TraverseAffectedFaces(ConvexFaceInternal currentFace)
|
---|
| 223 | {
|
---|
| 224 | TraverseStack.Clear();
|
---|
| 225 | TraverseStack.Push(currentFace);
|
---|
| 226 | currentFace.Tag = 1;
|
---|
| 227 |
|
---|
| 228 | while (TraverseStack.Count > 0)
|
---|
| 229 | {
|
---|
| 230 | var top = TraverseStack.Pop();
|
---|
| 231 | for (int i = 0; i < Dimension; i++)
|
---|
| 232 | {
|
---|
| 233 | var adjFace = top.AdjacentFaces[i];
|
---|
| 234 |
|
---|
| 235 | if (adjFace.Tag == 0 && GetVertexDistance(CurrentVertex, adjFace) >= 0)
|
---|
| 236 | {
|
---|
| 237 | AffectedFaceBuffer.Add(adjFace);
|
---|
| 238 | //TraverseAffectedFaces(adjFace);
|
---|
| 239 | adjFace.Tag = 1;
|
---|
| 240 | TraverseStack.Push(adjFace);
|
---|
| 241 | }
|
---|
| 242 | }
|
---|
| 243 | }
|
---|
| 244 |
|
---|
| 245 | ////for (int i = 0; i < Dimension; i++)
|
---|
| 246 | ////{
|
---|
| 247 | //// var adjFace = currentFace.AdjacentFaces[i];
|
---|
| 248 |
|
---|
| 249 | //// if (adjFace.Tag == 0 && GetVertexDistance(CurrentVertex, adjFace) >= 0)
|
---|
| 250 | //// {
|
---|
| 251 | //// AffectedFaceBuffer.Add(adjFace);
|
---|
| 252 | //// TraverseAffectedFaces(adjFace);
|
---|
| 253 | //// }
|
---|
| 254 | ////}
|
---|
| 255 | }
|
---|
| 256 |
|
---|
| 257 | /// <summary>
|
---|
| 258 | /// Check if 2 faces are adjacent and if so, update their AdjacentFaces array.
|
---|
| 259 | /// </summary>
|
---|
| 260 | /// <param name="l"></param>
|
---|
| 261 | /// <param name="r"></param>
|
---|
| 262 | void UpdateAdjacency(ConvexFaceInternal l, ConvexFaceInternal r)
|
---|
| 263 | {
|
---|
| 264 | var lv = l.Vertices;
|
---|
| 265 | var rv = r.Vertices;
|
---|
| 266 | int i;
|
---|
| 267 |
|
---|
| 268 | // reset marks on the 1st face
|
---|
| 269 | for (i = 0; i < Dimension; i++) lv[i].Marked = false;
|
---|
| 270 |
|
---|
| 271 | // mark all vertices on the 2nd face
|
---|
| 272 | for (i = 0; i < Dimension; i++) rv[i].Marked = true;
|
---|
| 273 |
|
---|
| 274 | // find the 1st false index
|
---|
| 275 | for (i = 0; i < Dimension; i++) if (!lv[i].Marked) break;
|
---|
| 276 |
|
---|
| 277 | // no vertex was marked
|
---|
| 278 | if (i == Dimension) return;
|
---|
| 279 |
|
---|
| 280 | // check if only 1 vertex wasn't marked
|
---|
| 281 | for (int j = i + 1; j < Dimension; j++) if (!lv[j].Marked) return;
|
---|
| 282 |
|
---|
| 283 | // if we are here, the two faces share an edge
|
---|
| 284 | l.AdjacentFaces[i] = r;
|
---|
| 285 |
|
---|
| 286 | // update the adj. face on the other face - find the vertex that remains marked
|
---|
| 287 | for (i = 0; i < Dimension; i++) lv[i].Marked = false;
|
---|
| 288 | for (i = 0; i < Dimension; i++)
|
---|
| 289 | {
|
---|
| 290 | if (rv[i].Marked) break;
|
---|
| 291 | }
|
---|
| 292 | r.AdjacentFaces[i] = l;
|
---|
| 293 | }
|
---|
| 294 |
|
---|
| 295 | #region Memory stuff.
|
---|
| 296 |
|
---|
| 297 | /// <summary>
|
---|
| 298 | /// Recycle face for future use.
|
---|
| 299 | /// </summary>
|
---|
| 300 | void RecycleFace(ConvexFaceInternal face)
|
---|
| 301 | {
|
---|
| 302 | for (int i = 0; i < Dimension; i++)
|
---|
| 303 | {
|
---|
| 304 | face.AdjacentFaces[i] = null;
|
---|
| 305 | }
|
---|
| 306 | }
|
---|
| 307 |
|
---|
| 308 | /// <summary>
|
---|
| 309 | /// Get a fresh face.
|
---|
| 310 | /// </summary>
|
---|
| 311 | /// <returns></returns>
|
---|
| 312 | ConvexFaceInternal GetNewFace()
|
---|
| 313 | {
|
---|
| 314 | return RecycledFaceStack.Count != 0
|
---|
| 315 | ? RecycledFaceStack.Pop()
|
---|
| 316 | : new ConvexFaceInternal(Dimension, EmptyBufferStack.Count != 0 ? EmptyBufferStack.Pop() : new VertexBuffer());
|
---|
| 317 | }
|
---|
| 318 |
|
---|
| 319 | /// <summary>
|
---|
| 320 | /// Get a new connector.
|
---|
| 321 | /// </summary>
|
---|
| 322 | /// <returns></returns>
|
---|
| 323 | FaceConnector GetNewConnector()
|
---|
| 324 | {
|
---|
| 325 | return ConnectorStack.Count != 0
|
---|
| 326 | ? ConnectorStack.Pop()
|
---|
| 327 | : new FaceConnector(Dimension);
|
---|
| 328 | }
|
---|
| 329 | #endregion
|
---|
| 330 |
|
---|
| 331 | /// <summary>
|
---|
[9945] | 332 | /// Creates a new deferred face.
|
---|
| 333 | /// </summary>
|
---|
| 334 | /// <param name="face"></param>
|
---|
| 335 | /// <param name="faceIndex"></param>
|
---|
| 336 | /// <param name="pivot"></param>
|
---|
| 337 | /// <param name="pivotIndex"></param>
|
---|
| 338 | /// <param name="oldFace"></param>
|
---|
| 339 | /// <returns></returns>
|
---|
| 340 | DeferredFace GetDeferredFace(ConvexFaceInternal face, int faceIndex, ConvexFaceInternal pivot, int pivotIndex, ConvexFaceInternal oldFace)
|
---|
| 341 | {
|
---|
| 342 | var ret = ConeFaceStack.Count > 0 ? ConeFaceStack.Pop() : new DeferredFace();
|
---|
| 343 |
|
---|
| 344 | ret.Face = face;
|
---|
| 345 | ret.FaceIndex = faceIndex;
|
---|
| 346 | ret.Pivot = pivot;
|
---|
| 347 | ret.PivotIndex = pivotIndex;
|
---|
| 348 | ret.OldFace = oldFace;
|
---|
| 349 |
|
---|
| 350 | return ret;
|
---|
| 351 | }
|
---|
| 352 |
|
---|
| 353 | /// <summary>
|
---|
[9730] | 354 | /// Connect faces using a connector.
|
---|
| 355 | /// </summary>
|
---|
| 356 | /// <param name="connector"></param>
|
---|
| 357 | void ConnectFace(FaceConnector connector)
|
---|
| 358 | {
|
---|
| 359 | var index = connector.HashCode % ConnectorTableSize;
|
---|
| 360 | var list = ConnectorTable[index];
|
---|
| 361 |
|
---|
| 362 | for (var current = list.First; current != null; current = current.Next)
|
---|
| 363 | {
|
---|
| 364 | if (FaceConnector.AreConnectable(connector, current, Dimension))
|
---|
| 365 | {
|
---|
| 366 | list.Remove(current);
|
---|
| 367 | FaceConnector.Connect(current, connector);
|
---|
| 368 | current.Face = null;
|
---|
| 369 | connector.Face = null;
|
---|
| 370 | ConnectorStack.Push(current);
|
---|
| 371 | ConnectorStack.Push(connector);
|
---|
| 372 | return;
|
---|
| 373 | }
|
---|
| 374 | }
|
---|
| 375 |
|
---|
| 376 | list.Add(connector);
|
---|
| 377 | }
|
---|
| 378 |
|
---|
| 379 | /// <summary>
|
---|
| 380 | /// Removes the faces "covered" by the current vertex and adds the newly created ones.
|
---|
| 381 | /// </summary>
|
---|
[9945] | 382 | private bool CreateCone()
|
---|
[9730] | 383 | {
|
---|
| 384 | var currentVertexIndex = CurrentVertex.Index;
|
---|
[9945] | 385 | ConeFaceBuffer.Clear();
|
---|
[9730] | 386 |
|
---|
[9945] | 387 | for (int fIndex = 0; fIndex < AffectedFaceBuffer.Count; fIndex++)
|
---|
[9730] | 388 | {
|
---|
[9945] | 389 | var oldFace = AffectedFaceBuffer[fIndex];
|
---|
[9730] | 390 |
|
---|
| 391 | // Find the faces that need to be updated
|
---|
| 392 | int updateCount = 0;
|
---|
| 393 | for (int i = 0; i < Dimension; i++)
|
---|
| 394 | {
|
---|
| 395 | var af = oldFace.AdjacentFaces[i];
|
---|
| 396 | if (af.Tag == 0) // Tag == 0 when oldFaces does not contain af
|
---|
| 397 | {
|
---|
| 398 | UpdateBuffer[updateCount] = af;
|
---|
| 399 | UpdateIndices[updateCount] = i;
|
---|
| 400 | ++updateCount;
|
---|
| 401 | }
|
---|
| 402 | }
|
---|
| 403 |
|
---|
| 404 | for (int i = 0; i < updateCount; i++)
|
---|
| 405 | {
|
---|
| 406 | var adjacentFace = UpdateBuffer[i];
|
---|
| 407 |
|
---|
| 408 | int oldFaceAdjacentIndex = 0;
|
---|
| 409 | var adjFaceAdjacency = adjacentFace.AdjacentFaces;
|
---|
| 410 | for (int j = 0; j < Dimension; j++)
|
---|
| 411 | {
|
---|
| 412 | if (object.ReferenceEquals(oldFace, adjFaceAdjacency[j]))
|
---|
| 413 | {
|
---|
| 414 | oldFaceAdjacentIndex = j;
|
---|
| 415 | break;
|
---|
| 416 | }
|
---|
| 417 | }
|
---|
| 418 |
|
---|
| 419 | var forbidden = UpdateIndices[i]; // Index of the face that corresponds to this adjacent face
|
---|
| 420 |
|
---|
| 421 | ConvexFaceInternal newFace;
|
---|
| 422 |
|
---|
| 423 | int oldVertexIndex;
|
---|
| 424 | VertexWrap[] vertices;
|
---|
| 425 |
|
---|
[9945] | 426 | newFace = GetNewFace();
|
---|
| 427 | vertices = newFace.Vertices;
|
---|
| 428 | for (int j = 0; j < Dimension; j++) vertices[j] = oldFace.Vertices[j];
|
---|
| 429 | oldVertexIndex = vertices[forbidden].Index;
|
---|
[9730] | 430 |
|
---|
| 431 | int orderedPivotIndex;
|
---|
| 432 |
|
---|
| 433 | // correct the ordering
|
---|
| 434 | if (currentVertexIndex < oldVertexIndex)
|
---|
| 435 | {
|
---|
| 436 | orderedPivotIndex = 0;
|
---|
| 437 | for (int j = forbidden - 1; j >= 0; j--)
|
---|
| 438 | {
|
---|
| 439 | if (vertices[j].Index > currentVertexIndex) vertices[j + 1] = vertices[j];
|
---|
| 440 | else
|
---|
| 441 | {
|
---|
| 442 | orderedPivotIndex = j + 1;
|
---|
| 443 | break;
|
---|
| 444 | }
|
---|
| 445 | }
|
---|
| 446 | }
|
---|
| 447 | else
|
---|
| 448 | {
|
---|
| 449 | orderedPivotIndex = Dimension - 1;
|
---|
| 450 | for (int j = forbidden + 1; j < Dimension; j++)
|
---|
| 451 | {
|
---|
| 452 | if (vertices[j].Index < currentVertexIndex) vertices[j - 1] = vertices[j];
|
---|
| 453 | else
|
---|
| 454 | {
|
---|
| 455 | orderedPivotIndex = j - 1;
|
---|
| 456 | break;
|
---|
| 457 | }
|
---|
| 458 | }
|
---|
| 459 | }
|
---|
| 460 |
|
---|
| 461 | vertices[orderedPivotIndex] = CurrentVertex;
|
---|
| 462 |
|
---|
[9945] | 463 | if (!CalculateFacePlane(newFace))
|
---|
[9730] | 464 | {
|
---|
[9945] | 465 | return false;
|
---|
[9730] | 466 | }
|
---|
| 467 |
|
---|
[9945] | 468 | ConeFaceBuffer.Add(GetDeferredFace(newFace, orderedPivotIndex, adjacentFace, oldFaceAdjacentIndex, oldFace));
|
---|
[9730] | 469 | }
|
---|
| 470 | }
|
---|
[9945] | 471 |
|
---|
| 472 | return true;
|
---|
[9730] | 473 | }
|
---|
| 474 |
|
---|
| 475 | /// <summary>
|
---|
[9945] | 476 | /// Commits a cone and adds a vertex to the convex hull.
|
---|
| 477 | /// </summary>
|
---|
| 478 | void CommitCone()
|
---|
| 479 | {
|
---|
| 480 | // Add the current vertex.
|
---|
| 481 | ConvexHull.Add(CurrentVertex);
|
---|
| 482 |
|
---|
| 483 | // Fill the adjacency.
|
---|
| 484 | for (int i = 0; i < ConeFaceBuffer.Count; i++)
|
---|
| 485 | {
|
---|
| 486 | var face = ConeFaceBuffer[i];
|
---|
| 487 |
|
---|
| 488 | var newFace = face.Face;
|
---|
| 489 | var adjacentFace = face.Pivot;
|
---|
| 490 | var oldFace = face.OldFace;
|
---|
| 491 | var orderedPivotIndex = face.FaceIndex;
|
---|
| 492 |
|
---|
| 493 | newFace.AdjacentFaces[orderedPivotIndex] = adjacentFace;
|
---|
| 494 | adjacentFace.AdjacentFaces[face.PivotIndex] = newFace;
|
---|
| 495 |
|
---|
| 496 | // let there be a connection.
|
---|
| 497 | for (int j = 0; j < Dimension; j++)
|
---|
| 498 | {
|
---|
| 499 | if (j == orderedPivotIndex) continue;
|
---|
| 500 | var connector = GetNewConnector();
|
---|
| 501 | connector.Update(newFace, j, Dimension);
|
---|
| 502 | ConnectFace(connector);
|
---|
| 503 | }
|
---|
| 504 |
|
---|
| 505 | // This could slightly help...
|
---|
| 506 | if (adjacentFace.VerticesBeyond.Count < oldFace.VerticesBeyond.Count)
|
---|
| 507 | {
|
---|
| 508 | FindBeyondVertices(newFace, adjacentFace.VerticesBeyond, oldFace.VerticesBeyond);
|
---|
| 509 | }
|
---|
| 510 | else
|
---|
| 511 | {
|
---|
| 512 | FindBeyondVertices(newFace, oldFace.VerticesBeyond, adjacentFace.VerticesBeyond);
|
---|
| 513 | }
|
---|
| 514 |
|
---|
| 515 | // This face will definitely lie on the hull
|
---|
| 516 | if (newFace.VerticesBeyond.Count == 0)
|
---|
| 517 | {
|
---|
| 518 | ConvexFaces.Add(newFace);
|
---|
| 519 | UnprocessedFaces.Remove(newFace);
|
---|
| 520 | EmptyBufferStack.Push(newFace.VerticesBeyond);
|
---|
| 521 | newFace.VerticesBeyond = EmptyBuffer;
|
---|
| 522 | }
|
---|
| 523 | else // Add the face to the list
|
---|
| 524 | {
|
---|
| 525 | UnprocessedFaces.Add(newFace);
|
---|
| 526 | }
|
---|
| 527 |
|
---|
| 528 | // recycle the object.
|
---|
| 529 | ConeFaceStack.Push(face);
|
---|
| 530 | }
|
---|
| 531 |
|
---|
| 532 | // Recycle the affected faces.
|
---|
| 533 | for (int fIndex = 0; fIndex < AffectedFaceBuffer.Count; fIndex++)
|
---|
| 534 | {
|
---|
| 535 | var face = AffectedFaceBuffer[fIndex];
|
---|
| 536 | UnprocessedFaces.Remove(face);
|
---|
| 537 | RecycleFace(face);
|
---|
| 538 | RecycledFaceStack.Push(face);
|
---|
| 539 | }
|
---|
| 540 | }
|
---|
| 541 |
|
---|
| 542 | /// <summary>
|
---|
[9730] | 543 | /// Subtracts vectors x and y and stores the result to target.
|
---|
| 544 | /// </summary>
|
---|
| 545 | /// <param name="x"></param>
|
---|
| 546 | /// <param name="y"></param>
|
---|
| 547 | /// <param name="target"></param>
|
---|
| 548 | void SubtractFast(double[] x, double[] y, double[] target)
|
---|
| 549 | {
|
---|
| 550 | for (int i = 0; i < Dimension; i++)
|
---|
| 551 | {
|
---|
| 552 | target[i] = x[i] - y[i];
|
---|
| 553 | }
|
---|
| 554 | }
|
---|
| 555 |
|
---|
| 556 | /// <summary>
|
---|
| 557 | /// Finds 4D normal vector.
|
---|
| 558 | /// </summary>
|
---|
| 559 | /// <param name="vertices"></param>
|
---|
| 560 | /// <param name="normal"></param>
|
---|
| 561 | void FindNormalVector4D(VertexWrap[] vertices, double[] normal)
|
---|
| 562 | {
|
---|
| 563 | SubtractFast(vertices[1].PositionData, vertices[0].PositionData, ntX);
|
---|
| 564 | SubtractFast(vertices[2].PositionData, vertices[1].PositionData, ntY);
|
---|
| 565 | SubtractFast(vertices[3].PositionData, vertices[2].PositionData, ntZ);
|
---|
| 566 |
|
---|
| 567 | var x = ntX;
|
---|
| 568 | var y = ntY;
|
---|
| 569 | var z = ntZ;
|
---|
| 570 |
|
---|
| 571 | // This was generated using Mathematica
|
---|
| 572 | var nx = x[3] * (y[2] * z[1] - y[1] * z[2])
|
---|
| 573 | + x[2] * (y[1] * z[3] - y[3] * z[1])
|
---|
| 574 | + x[1] * (y[3] * z[2] - y[2] * z[3]);
|
---|
| 575 | var ny = x[3] * (y[0] * z[2] - y[2] * z[0])
|
---|
| 576 | + x[2] * (y[3] * z[0] - y[0] * z[3])
|
---|
| 577 | + x[0] * (y[2] * z[3] - y[3] * z[2]);
|
---|
| 578 | var nz = x[3] * (y[1] * z[0] - y[0] * z[1])
|
---|
| 579 | + x[1] * (y[0] * z[3] - y[3] * z[0])
|
---|
| 580 | + x[0] * (y[3] * z[1] - y[1] * z[3]);
|
---|
| 581 | var nw = x[2] * (y[0] * z[1] - y[1] * z[0])
|
---|
| 582 | + x[1] * (y[2] * z[0] - y[0] * z[2])
|
---|
| 583 | + x[0] * (y[1] * z[2] - y[2] * z[1]);
|
---|
| 584 |
|
---|
| 585 | double norm = System.Math.Sqrt(nx * nx + ny * ny + nz * nz + nw * nw);
|
---|
| 586 |
|
---|
| 587 | double f = 1.0 / norm;
|
---|
| 588 | normal[0] = f * nx;
|
---|
| 589 | normal[1] = f * ny;
|
---|
| 590 | normal[2] = f * nz;
|
---|
| 591 | normal[3] = f * nw;
|
---|
| 592 | }
|
---|
| 593 |
|
---|
| 594 | /// <summary>
|
---|
| 595 | /// Finds 3D normal vector.
|
---|
| 596 | /// </summary>
|
---|
| 597 | /// <param name="vertices"></param>
|
---|
| 598 | /// <param name="normal"></param>
|
---|
| 599 | void FindNormalVector3D(VertexWrap[] vertices, double[] normal)
|
---|
| 600 | {
|
---|
| 601 | SubtractFast(vertices[1].PositionData, vertices[0].PositionData, ntX);
|
---|
| 602 | SubtractFast(vertices[2].PositionData, vertices[1].PositionData, ntY);
|
---|
| 603 |
|
---|
| 604 | var x = ntX;
|
---|
| 605 | var y = ntY;
|
---|
| 606 |
|
---|
| 607 | var nx = x[1] * y[2] - x[2] * y[1];
|
---|
| 608 | var ny = x[2] * y[0] - x[0] * y[2];
|
---|
| 609 | var nz = x[0] * y[1] - x[1] * y[0];
|
---|
| 610 |
|
---|
| 611 | double norm = System.Math.Sqrt(nx * nx + ny * ny + nz * nz);
|
---|
| 612 |
|
---|
| 613 | double f = 1.0 / norm;
|
---|
| 614 | normal[0] = f * nx;
|
---|
| 615 | normal[1] = f * ny;
|
---|
| 616 | normal[2] = f * nz;
|
---|
| 617 | }
|
---|
| 618 |
|
---|
| 619 | /// <summary>
|
---|
| 620 | /// Finds 2D normal vector.
|
---|
| 621 | /// </summary>
|
---|
| 622 | /// <param name="vertices"></param>
|
---|
| 623 | /// <param name="normal"></param>
|
---|
| 624 | void FindNormalVector2D(VertexWrap[] vertices, double[] normal)
|
---|
| 625 | {
|
---|
| 626 | SubtractFast(vertices[1].PositionData, vertices[0].PositionData, ntX);
|
---|
| 627 |
|
---|
| 628 | var x = ntX;
|
---|
| 629 |
|
---|
| 630 | var nx = -x[1];
|
---|
| 631 | var ny = x[0];
|
---|
| 632 |
|
---|
| 633 | double norm = System.Math.Sqrt(nx * nx + ny * ny);
|
---|
| 634 |
|
---|
| 635 | double f = 1.0 / norm;
|
---|
| 636 | normal[0] = f * nx;
|
---|
| 637 | normal[1] = f * ny;
|
---|
| 638 | }
|
---|
| 639 |
|
---|
| 640 | /// <summary>
|
---|
| 641 | /// Finds normal vector of a hyper-plane given by vertices.
|
---|
| 642 | /// Stores the results to normalData.
|
---|
| 643 | /// </summary>
|
---|
| 644 | /// <param name="vertices"></param>
|
---|
| 645 | /// <param name="normalData"></param>
|
---|
| 646 | private void FindNormalVector(VertexWrap[] vertices, double[] normalData)
|
---|
| 647 | {
|
---|
| 648 | switch (Dimension)
|
---|
| 649 | {
|
---|
| 650 | case 2: FindNormalVector2D(vertices, normalData); break;
|
---|
| 651 | case 3: FindNormalVector3D(vertices, normalData); break;
|
---|
| 652 | case 4: FindNormalVector4D(vertices, normalData); break;
|
---|
| 653 | default:
|
---|
| 654 | {
|
---|
| 655 | for (var i = 0; i < Dimension; i++) nDNormalSolveVector[i] = 1.0;
|
---|
| 656 | for (var i = 0; i < Dimension; i++)
|
---|
| 657 | {
|
---|
| 658 | var row = jaggedNDMatrix[i];
|
---|
| 659 | var pos = vertices[i].Vertex.Position;
|
---|
| 660 | for (int j = 0; j < Dimension; j++) row[j] = pos[j];
|
---|
| 661 | }
|
---|
| 662 | StarMath.gaussElimination(Dimension, jaggedNDMatrix, nDNormalSolveVector, normalData);
|
---|
| 663 | StarMath.normalizeInPlace(normalData, Dimension);
|
---|
| 664 | break;
|
---|
| 665 | }
|
---|
| 666 | }
|
---|
| 667 | }
|
---|
| 668 |
|
---|
| 669 | /// <summary>
|
---|
| 670 | /// Check whether the vertex v is beyond the given face. If so, add it to beyondVertices.
|
---|
| 671 | /// </summary>
|
---|
| 672 | /// <param name="face"></param>
|
---|
| 673 | /// <param name="beyondVertices"></param>
|
---|
| 674 | /// <param name="v"></param>
|
---|
| 675 | void IsBeyond(ConvexFaceInternal face, VertexBuffer beyondVertices, VertexWrap v)
|
---|
| 676 | {
|
---|
| 677 | double distance = GetVertexDistance(v, face);
|
---|
| 678 | if (distance >= 0)
|
---|
| 679 | {
|
---|
| 680 | if (distance > MaxDistance)
|
---|
| 681 | {
|
---|
| 682 | MaxDistance = distance;
|
---|
| 683 | FurthestVertex = v;
|
---|
| 684 | }
|
---|
| 685 | beyondVertices.Add(v);
|
---|
| 686 | }
|
---|
| 687 | }
|
---|
| 688 |
|
---|
| 689 | /// <summary>
|
---|
| 690 | /// Used in the "initialization" code.
|
---|
| 691 | /// </summary>
|
---|
| 692 | void FindBeyondVertices(ConvexFaceInternal face)
|
---|
| 693 | {
|
---|
| 694 | var beyondVertices = face.VerticesBeyond;
|
---|
| 695 |
|
---|
| 696 | MaxDistance = double.NegativeInfinity;
|
---|
| 697 | FurthestVertex = null;
|
---|
| 698 |
|
---|
| 699 | int count = InputVertices.Count;
|
---|
| 700 | for (int i = 0; i < count; i++) IsBeyond(face, beyondVertices, InputVertices[i]);
|
---|
| 701 |
|
---|
| 702 | face.FurthestVertex = FurthestVertex;
|
---|
| 703 | //face.FurthestDistance = MaxDistance;
|
---|
| 704 | }
|
---|
| 705 |
|
---|
| 706 | /// <summary>
|
---|
| 707 | /// Used by update faces.
|
---|
| 708 | /// </summary>
|
---|
| 709 | void FindBeyondVertices(ConvexFaceInternal face, VertexBuffer beyond, VertexBuffer beyond1)
|
---|
| 710 | {
|
---|
| 711 | var beyondVertices = BeyondBuffer;
|
---|
| 712 |
|
---|
| 713 | MaxDistance = double.NegativeInfinity;
|
---|
| 714 | FurthestVertex = null;
|
---|
| 715 | VertexWrap v;
|
---|
| 716 |
|
---|
| 717 | int count = beyond1.Count;
|
---|
| 718 | for (int i = 0; i < count; i++) beyond1[i].Marked = true;
|
---|
| 719 | CurrentVertex.Marked = false;
|
---|
| 720 | count = beyond.Count;
|
---|
| 721 | for (int i = 0; i < count; i++)
|
---|
| 722 | {
|
---|
| 723 | v = beyond[i];
|
---|
| 724 | if (object.ReferenceEquals(v, CurrentVertex)) continue;
|
---|
| 725 | v.Marked = false;
|
---|
| 726 | IsBeyond(face, beyondVertices, v);
|
---|
| 727 | }
|
---|
| 728 |
|
---|
| 729 | count = beyond1.Count;
|
---|
| 730 | for (int i = 0; i < count; i++)
|
---|
| 731 | {
|
---|
| 732 | v = beyond1[i];
|
---|
| 733 | if (v.Marked) IsBeyond(face, beyondVertices, v);
|
---|
| 734 | }
|
---|
| 735 |
|
---|
| 736 | face.FurthestVertex = FurthestVertex;
|
---|
| 737 | //face.FurthestDistance = MaxDistance;
|
---|
| 738 |
|
---|
| 739 | // Pull the old switch a roo
|
---|
| 740 | var temp = face.VerticesBeyond;
|
---|
| 741 | face.VerticesBeyond = beyondVertices;
|
---|
| 742 | if (temp.Count > 0) temp.Clear();
|
---|
| 743 | BeyondBuffer = temp;
|
---|
| 744 | }
|
---|
| 745 |
|
---|
| 746 | /// <summary>
|
---|
| 747 | /// Recalculates the centroid of the current hull.
|
---|
| 748 | /// </summary>
|
---|
| 749 | void UpdateCenter()
|
---|
| 750 | {
|
---|
[9945] | 751 | var count = ConvexHull.Count + 1;
|
---|
| 752 | for (int i = 0; i < Dimension; i++) Center[i] *= (count - 1);
|
---|
| 753 | double f = 1.0 / count;
|
---|
[9730] | 754 | for (int i = 0; i < Dimension; i++) Center[i] = f * (Center[i] + CurrentVertex.PositionData[i]);
|
---|
| 755 | }
|
---|
| 756 |
|
---|
| 757 | /// <summary>
|
---|
[9945] | 758 | /// Removes the last vertex from the center.
|
---|
| 759 | /// </summary>
|
---|
| 760 | void RollbackCenter()
|
---|
| 761 | {
|
---|
| 762 | var count = ConvexHull.Count + 1;
|
---|
| 763 | for (int i = 0; i < Dimension; i++) Center[i] *= count;
|
---|
| 764 | double f = 1.0 / (count - 1);
|
---|
| 765 | for (int i = 0; i < Dimension; i++) Center[i] = f * (Center[i] - CurrentVertex.PositionData[i]);
|
---|
| 766 | }
|
---|
| 767 |
|
---|
| 768 | /// <summary>
|
---|
[9730] | 769 | /// Find the (dimension+1) initial points and create the simplexes.
|
---|
| 770 | /// </summary>
|
---|
| 771 | void InitConvexHull()
|
---|
| 772 | {
|
---|
| 773 | var extremes = FindExtremes();
|
---|
| 774 | var initialPoints = FindInitialPoints(extremes);
|
---|
| 775 |
|
---|
| 776 | // Add the initial points to the convex hull.
|
---|
| 777 | foreach (var vertex in initialPoints)
|
---|
| 778 | {
|
---|
| 779 | CurrentVertex = vertex;
|
---|
[9945] | 780 | // update center must be called before adding the vertex.
|
---|
| 781 | UpdateCenter();
|
---|
[9730] | 782 | ConvexHull.Add(CurrentVertex);
|
---|
| 783 | InputVertices.Remove(vertex);
|
---|
| 784 |
|
---|
| 785 | // Because of the AklTou heuristic.
|
---|
| 786 | extremes.Remove(vertex);
|
---|
| 787 | }
|
---|
| 788 |
|
---|
| 789 | // Create the initial simplexes.
|
---|
| 790 | var faces = InitiateFaceDatabase();
|
---|
| 791 |
|
---|
| 792 | // Init the vertex beyond buffers.
|
---|
| 793 | foreach (var face in faces)
|
---|
| 794 | {
|
---|
| 795 | FindBeyondVertices(face);
|
---|
| 796 | if (face.VerticesBeyond.Count == 0) ConvexFaces.Add(face); // The face is on the hull
|
---|
| 797 | else UnprocessedFaces.Add(face);
|
---|
| 798 | }
|
---|
| 799 | }
|
---|
| 800 |
|
---|
| 801 | /// <summary>
|
---|
| 802 | /// Finds (dimension + 1) initial points.
|
---|
| 803 | /// </summary>
|
---|
| 804 | /// <param name="extremes"></param>
|
---|
| 805 | /// <returns></returns>
|
---|
| 806 | private List<VertexWrap> FindInitialPoints(List<VertexWrap> extremes)
|
---|
| 807 | {
|
---|
| 808 | List<VertexWrap> initialPoints = new List<VertexWrap>();// { extremes[0], extremes[1] };
|
---|
| 809 |
|
---|
| 810 | VertexWrap first = null, second = null;
|
---|
| 811 | double maxDist = 0;
|
---|
| 812 | for (int i = 0; i < extremes.Count - 1; i++)
|
---|
| 813 | {
|
---|
| 814 | var a = extremes[i];
|
---|
| 815 | for (int j = i + 1; j < extremes.Count; j++)
|
---|
| 816 | {
|
---|
| 817 | var b = extremes[j];
|
---|
| 818 | var dist = StarMath.norm2(StarMath.subtract(a.PositionData, b.PositionData, Dimension), Dimension, true);
|
---|
| 819 | if (dist > maxDist)
|
---|
| 820 | {
|
---|
| 821 | first = a;
|
---|
| 822 | second = b;
|
---|
| 823 | maxDist = dist;
|
---|
| 824 | }
|
---|
| 825 | }
|
---|
| 826 | }
|
---|
| 827 |
|
---|
| 828 | initialPoints.Add(first);
|
---|
| 829 | initialPoints.Add(second);
|
---|
| 830 |
|
---|
| 831 | for (int i = 2; i <= Dimension; i++)
|
---|
| 832 | {
|
---|
[9945] | 833 | double maximum = 0.000001;
|
---|
[9730] | 834 | VertexWrap maxPoint = null;
|
---|
| 835 | for (int j = 0; j < extremes.Count; j++)
|
---|
| 836 | {
|
---|
| 837 | var extreme = extremes[j];
|
---|
| 838 | if (initialPoints.Contains(extreme)) continue;
|
---|
| 839 |
|
---|
| 840 | var val = GetSimplexVolume(extreme, initialPoints);
|
---|
| 841 |
|
---|
| 842 | if (val > maximum)
|
---|
| 843 | {
|
---|
| 844 | maximum = val;
|
---|
| 845 | maxPoint = extreme;
|
---|
| 846 | }
|
---|
| 847 | }
|
---|
| 848 | if (maxPoint != null) initialPoints.Add(maxPoint);
|
---|
| 849 | else
|
---|
| 850 | {
|
---|
| 851 | int vCount = InputVertices.Count;
|
---|
| 852 | for (int j = 0; j < vCount; j++)
|
---|
| 853 | {
|
---|
| 854 | var point = InputVertices[j];
|
---|
| 855 | if (initialPoints.Contains(point)) continue;
|
---|
| 856 |
|
---|
| 857 | var val = GetSimplexVolume(point, initialPoints);
|
---|
| 858 |
|
---|
| 859 | if (val > maximum)
|
---|
| 860 | {
|
---|
| 861 | maximum = val;
|
---|
| 862 | maxPoint = point;
|
---|
| 863 | }
|
---|
| 864 | }
|
---|
| 865 |
|
---|
| 866 | if (maxPoint != null) initialPoints.Add(maxPoint);
|
---|
| 867 | else ThrowSingular();
|
---|
| 868 | }
|
---|
| 869 | }
|
---|
| 870 | return initialPoints;
|
---|
| 871 | }
|
---|
| 872 |
|
---|
| 873 | /// <summary>
|
---|
| 874 | /// Computes the volume of the (n=initialPoints.Count)D simplex defined by the
|
---|
| 875 | /// pivot and initialPoints.
|
---|
| 876 | /// This is computed as the determinant of the matrix | initialPoints[i] - pivot |
|
---|
| 877 | /// </summary>
|
---|
| 878 | /// <param name="pivot"></param>
|
---|
| 879 | /// <param name="initialPoints"></param>
|
---|
| 880 | /// <returns></returns>
|
---|
| 881 | double GetSimplexVolume(VertexWrap pivot, List<VertexWrap> initialPoints)
|
---|
| 882 | {
|
---|
[9945] | 883 | var initPtsNum = initialPoints.Count;
|
---|
| 884 | //var m = nDMatrix;
|
---|
| 885 | var sum = 0.0;
|
---|
| 886 |
|
---|
| 887 | for (int i = 0; i < initPtsNum; i++)
|
---|
[9730] | 888 | {
|
---|
[9945] | 889 | var initPt = initialPoints[i];
|
---|
| 890 | for (int j = 0; j < Dimension; j++)
|
---|
| 891 | sum += (initPt.PositionData[j] - pivot.PositionData[j])
|
---|
| 892 | * (initPt.PositionData[j] - pivot.PositionData[j]);
|
---|
[9730] | 893 | }
|
---|
| 894 |
|
---|
[9945] | 895 | return sum;
|
---|
[9730] | 896 | }
|
---|
| 897 |
|
---|
| 898 | /// <summary>
|
---|
| 899 | /// Finds the extremes in all dimensions.
|
---|
| 900 | /// </summary>
|
---|
| 901 | /// <returns></returns>
|
---|
| 902 | private List<VertexWrap> FindExtremes()
|
---|
| 903 | {
|
---|
| 904 | var extremes = new List<VertexWrap>(2 * Dimension);
|
---|
| 905 |
|
---|
| 906 | int vCount = InputVertices.Count;
|
---|
| 907 | for (int i = 0; i < Dimension; i++)
|
---|
| 908 | {
|
---|
| 909 | double min = double.MaxValue, max = double.MinValue;
|
---|
| 910 | int minInd = 0, maxInd = 0;
|
---|
| 911 | for (int j = 0; j < vCount; j++)
|
---|
| 912 | {
|
---|
| 913 | var v = InputVertices[j].PositionData[i];
|
---|
| 914 | if (v < min)
|
---|
| 915 | {
|
---|
| 916 | min = v;
|
---|
| 917 | minInd = j;
|
---|
| 918 | }
|
---|
| 919 | if (v > max)
|
---|
| 920 | {
|
---|
| 921 | max = v;
|
---|
| 922 | maxInd = j;
|
---|
| 923 | }
|
---|
| 924 | }
|
---|
| 925 |
|
---|
| 926 | if (minInd != maxInd)
|
---|
| 927 | {
|
---|
| 928 | extremes.Add(InputVertices[minInd]);
|
---|
| 929 | extremes.Add(InputVertices[maxInd]);
|
---|
| 930 | }
|
---|
| 931 | else extremes.Add(InputVertices[minInd]);
|
---|
| 932 | }
|
---|
| 933 | return extremes;
|
---|
| 934 | }
|
---|
| 935 |
|
---|
| 936 | /// <summary>
|
---|
| 937 | /// The exception thrown if singular input data detected.
|
---|
| 938 | /// </summary>
|
---|
| 939 | void ThrowSingular()
|
---|
| 940 | {
|
---|
| 941 | throw new InvalidOperationException(
|
---|
| 942 | "ConvexHull: Singular input data (i.e. trying to triangulate a data that contain a regular lattice of points).\n"
|
---|
| 943 | + "Introducing some noise to the data might resolve the issue.");
|
---|
| 944 | }
|
---|
| 945 |
|
---|
| 946 | /// <summary>
|
---|
[9945] | 947 | /// Handles singular vertex.
|
---|
| 948 | /// </summary>
|
---|
| 949 | void HandleSingular()
|
---|
| 950 | {
|
---|
| 951 | RollbackCenter();
|
---|
| 952 | SingularVertices.Add(CurrentVertex);
|
---|
| 953 |
|
---|
| 954 | // This means that all the affected faces must be on the hull and that all their "vertices beyond" are singular.
|
---|
| 955 | for (int fIndex = 0; fIndex < AffectedFaceBuffer.Count; fIndex++)
|
---|
| 956 | {
|
---|
| 957 | var face = AffectedFaceBuffer[fIndex];
|
---|
| 958 | var vb = face.VerticesBeyond;
|
---|
| 959 | for (int i = 0; i < vb.Count; i++)
|
---|
| 960 | {
|
---|
| 961 | SingularVertices.Add(vb[i]);
|
---|
| 962 | }
|
---|
| 963 |
|
---|
| 964 | ConvexFaces.Add(face);
|
---|
| 965 | UnprocessedFaces.Remove(face);
|
---|
| 966 | EmptyBufferStack.Push(face.VerticesBeyond);
|
---|
| 967 | face.VerticesBeyond = EmptyBuffer;
|
---|
| 968 | }
|
---|
| 969 | }
|
---|
| 970 |
|
---|
| 971 | /// <summary>
|
---|
[9730] | 972 | /// Fins the convex hull.
|
---|
| 973 | /// </summary>
|
---|
| 974 | void FindConvexHull()
|
---|
| 975 | {
|
---|
| 976 | // Find the (dimension+1) initial points and create the simplexes.
|
---|
| 977 | InitConvexHull();
|
---|
| 978 |
|
---|
| 979 | // Expand the convex hull and faces.
|
---|
| 980 | while (UnprocessedFaces.First != null)
|
---|
| 981 | {
|
---|
| 982 | var currentFace = UnprocessedFaces.First;
|
---|
| 983 | CurrentVertex = currentFace.FurthestVertex;
|
---|
[9945] | 984 |
|
---|
[9730] | 985 | UpdateCenter();
|
---|
| 986 |
|
---|
| 987 | // The affected faces get tagged
|
---|
| 988 | TagAffectedFaces(currentFace);
|
---|
| 989 |
|
---|
| 990 | // Create the cone from the currentVertex and the affected faces horizon.
|
---|
[9945] | 991 | if (!SingularVertices.Contains(CurrentVertex) && CreateCone()) CommitCone();
|
---|
| 992 | else HandleSingular();
|
---|
[9730] | 993 |
|
---|
| 994 | // Need to reset the tags
|
---|
| 995 | int count = AffectedFaceBuffer.Count;
|
---|
| 996 | for (int i = 0; i < count; i++) AffectedFaceBuffer[i].Tag = 0;
|
---|
| 997 | }
|
---|
| 998 | }
|
---|
| 999 |
|
---|
| 1000 | /// <summary>
|
---|
| 1001 | /// Wraps the vertices and determines the dimension if it's unknown.
|
---|
| 1002 | /// </summary>
|
---|
| 1003 | /// <param name="vertices"></param>
|
---|
| 1004 | /// <param name="dim"></param>
|
---|
| 1005 | private ConvexHullInternal(IEnumerable<IVertex> vertices)
|
---|
| 1006 | {
|
---|
| 1007 | InputVertices = new List<VertexWrap>(vertices.Select((v, i) => new VertexWrap { Vertex = v, PositionData = v.Position, Index = i }));
|
---|
| 1008 | Dimension = DetermineDimension();
|
---|
| 1009 | Initialize();
|
---|
| 1010 | }
|
---|
| 1011 |
|
---|
| 1012 | /// <summary>
|
---|
| 1013 | /// Finds the vertices on the convex hull and optionally converts them to the TVertex array.
|
---|
| 1014 | /// </summary>
|
---|
| 1015 | /// <typeparam name="TVertex"></typeparam>
|
---|
| 1016 | /// <param name="onlyCompute"></param>
|
---|
| 1017 | /// <returns></returns>
|
---|
| 1018 | private IEnumerable<TVertex> GetConvexHullInternal<TVertex>(bool onlyCompute = false) where TVertex : IVertex
|
---|
| 1019 | {
|
---|
| 1020 | if (Computed) return onlyCompute ? null : ConvexHull.Select(v => (TVertex)v.Vertex).ToArray();
|
---|
| 1021 |
|
---|
| 1022 | if (Dimension < 2) throw new ArgumentException("Dimension of the input must be 2 or greater.");
|
---|
| 1023 |
|
---|
| 1024 | FindConvexHull();
|
---|
| 1025 | Computed = true;
|
---|
| 1026 | return onlyCompute ? null : ConvexHull.Select(v => (TVertex)v.Vertex).ToArray();
|
---|
| 1027 | }
|
---|
| 1028 |
|
---|
| 1029 | /// <summary>
|
---|
| 1030 | /// Finds the convex hull and creates the TFace objects.
|
---|
| 1031 | /// </summary>
|
---|
| 1032 | /// <typeparam name="TVertex"></typeparam>
|
---|
| 1033 | /// <typeparam name="TFace"></typeparam>
|
---|
| 1034 | /// <returns></returns>
|
---|
| 1035 | private IEnumerable<TFace> GetConvexFacesInternal<TVertex, TFace>()
|
---|
| 1036 | where TFace : ConvexFace<TVertex, TFace>, new()
|
---|
| 1037 | where TVertex : IVertex
|
---|
| 1038 | {
|
---|
| 1039 | if (!Computed) GetConvexHullInternal<TVertex>(true);
|
---|
| 1040 |
|
---|
| 1041 | var faces = ConvexFaces;
|
---|
| 1042 | int cellCount = faces.Count;
|
---|
| 1043 | var cells = new TFace[cellCount];
|
---|
| 1044 |
|
---|
| 1045 | for (int i = 0; i < cellCount; i++)
|
---|
| 1046 | {
|
---|
| 1047 | var face = faces[i];
|
---|
| 1048 | var vertices = new TVertex[Dimension];
|
---|
| 1049 | for (int j = 0; j < Dimension; j++) vertices[j] = (TVertex)face.Vertices[j].Vertex;
|
---|
| 1050 | cells[i] = new TFace
|
---|
| 1051 | {
|
---|
| 1052 | Vertices = vertices,
|
---|
| 1053 | Adjacency = new TFace[Dimension],
|
---|
| 1054 | Normal = face.Normal
|
---|
| 1055 | };
|
---|
| 1056 | face.Tag = i;
|
---|
| 1057 | }
|
---|
| 1058 |
|
---|
| 1059 | for (int i = 0; i < cellCount; i++)
|
---|
| 1060 | {
|
---|
| 1061 | var face = faces[i];
|
---|
| 1062 | var cell = cells[i];
|
---|
| 1063 | for (int j = 0; j < Dimension; j++)
|
---|
| 1064 | {
|
---|
| 1065 | if (face.AdjacentFaces[j] == null) continue;
|
---|
| 1066 | cell.Adjacency[j] = cells[face.AdjacentFaces[j].Tag];
|
---|
| 1067 | }
|
---|
| 1068 |
|
---|
| 1069 | // Fix the vertex orientation.
|
---|
| 1070 | if (face.IsNormalFlipped)
|
---|
| 1071 | {
|
---|
| 1072 | var tempVert = cell.Vertices[0];
|
---|
| 1073 | cell.Vertices[0] = cell.Vertices[Dimension - 1];
|
---|
| 1074 | cell.Vertices[Dimension - 1] = tempVert;
|
---|
| 1075 |
|
---|
| 1076 | var tempAdj = cell.Adjacency[0];
|
---|
| 1077 | cell.Adjacency[0] = cell.Adjacency[Dimension - 1];
|
---|
| 1078 | cell.Adjacency[Dimension - 1] = tempAdj;
|
---|
| 1079 | }
|
---|
| 1080 | }
|
---|
| 1081 |
|
---|
| 1082 | return cells;
|
---|
| 1083 | }
|
---|
| 1084 |
|
---|
| 1085 | /// <summary>
|
---|
| 1086 | /// This is used by the Delaunay triangulation code.
|
---|
| 1087 | /// </summary>
|
---|
| 1088 | /// <typeparam name="TVertex"></typeparam>
|
---|
| 1089 | /// <typeparam name="TFace"></typeparam>
|
---|
| 1090 | /// <param name="data"></param>
|
---|
| 1091 | /// <returns></returns>
|
---|
| 1092 | internal static List<ConvexFaceInternal> GetConvexFacesInternal<TVertex, TFace>(IEnumerable<TVertex> data)
|
---|
| 1093 | where TFace : ConvexFace<TVertex, TFace>, new()
|
---|
| 1094 | where TVertex : IVertex
|
---|
| 1095 | {
|
---|
| 1096 | ConvexHullInternal ch = new ConvexHullInternal(data.Cast<IVertex>());
|
---|
| 1097 | ch.GetConvexHullInternal<TVertex>(true);
|
---|
| 1098 | return ch.ConvexFaces;
|
---|
| 1099 | }
|
---|
| 1100 |
|
---|
| 1101 | /// <summary>
|
---|
| 1102 | /// This is called by the "ConvexHull" class.
|
---|
| 1103 | /// </summary>
|
---|
| 1104 | /// <typeparam name="TVertex"></typeparam>
|
---|
| 1105 | /// <typeparam name="TFace"></typeparam>
|
---|
| 1106 | /// <param name="data"></param>
|
---|
| 1107 | /// <returns></returns>
|
---|
| 1108 | internal static Tuple<IEnumerable<TVertex>, IEnumerable<TFace>> GetConvexHullAndFaces<TVertex, TFace>(IEnumerable<IVertex> data)
|
---|
| 1109 | where TFace : ConvexFace<TVertex, TFace>, new()
|
---|
| 1110 | where TVertex : IVertex
|
---|
| 1111 | {
|
---|
| 1112 | ConvexHullInternal ch = new ConvexHullInternal(data);
|
---|
| 1113 | return Tuple.Create(
|
---|
| 1114 | ch.GetConvexHullInternal<TVertex>(),
|
---|
| 1115 | ch.GetConvexFacesInternal<TVertex, TFace>());
|
---|
| 1116 | }
|
---|
| 1117 | }
|
---|
| 1118 | }
|
---|