[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;


 44 


 45  const int ConnectorTableSize = 2017;


 46  ConnectorList[] ConnectorTable;


 47 


 48  #endregion


 49 


 50  /// <summary>


 51  /// Initialize buffers and lists.


 52  /// </summary>


 53  void Initialize()


 54  {


 55  ConvexHull = new List<VertexWrap>();


 56  UnprocessedFaces = new FaceList(); // new LinkedList<ConvexFaceInternal>();


 57  ConvexFaces = new List<ConvexFaceInternal>();


 58 


 59  Center = new double[Dimension];


 60  ntX = new double[Dimension];


 61  ntY = new double[Dimension];


 62  ntZ = new double[Dimension];


 63  TraverseStack = new Stack<ConvexFaceInternal>();


 64  UpdateBuffer = new ConvexFaceInternal[Dimension];


 65  UpdateIndices = new int[Dimension];


 66  RecycledFaceStack = new Stack<ConvexFaceInternal>();


 67  ConnectorStack = new Stack<FaceConnector>();


 68  EmptyBufferStack = new Stack<VertexBuffer>();


 69  EmptyBuffer = new VertexBuffer();


 70  AffectedFaceBuffer = new List<ConvexFaceInternal>();


 71  BeyondBuffer = new VertexBuffer();


 72 


 73  ConnectorTable = Enumerable.Range(0, ConnectorTableSize).Select(_ => new ConnectorList()).ToArray();


 74 


 75  nDNormalSolveVector = new double[Dimension];


 76  jaggedNDMatrix = new double[Dimension][];


 77  for (var i = 0; i < Dimension; i++)


 78  {


 79  nDNormalSolveVector[i] = 1.0;


 80  jaggedNDMatrix[i] = new double[Dimension];


 81  }


 82  nDMatrix = new double[Dimension, Dimension];


 83  }


 84 


 85  /// <summary>


 86  /// Check the dimensionality of the input data.


 87  /// </summary>


 88  int DetermineDimension()


 89  {


 90  var r = new Random();


 91  var VCount = InputVertices.Count;


 92  var dimensions = new List<int>();


 93  for (var i = 0; i < 10; i++)


 94  dimensions.Add(InputVertices[r.Next(VCount)].Vertex.Position.Length);


 95  var dimension = dimensions.Min();


 96  if (dimension != dimensions.Max()) throw new ArgumentException("Invalid input data (nonuniform dimension).");


 97  return dimension;


 98  }


 99 


 100  /// <summary>


 101  /// Create the first faces from (dimension + 1) vertices.


 102  /// </summary>


 103  /// <returns></returns>


 104  ConvexFaceInternal[] InitiateFaceDatabase()


 105  {


 106  var faces = new ConvexFaceInternal[Dimension + 1];


 107 


 108  for (var i = 0; i < Dimension + 1; i++)


 109  {


 110  var vertices = ConvexHull.Where((_, j) => i != j).ToArray(); // Skips the ith vertex


 111  var newFace = new ConvexFaceInternal(Dimension, new VertexBuffer());


 112  newFace.Vertices = vertices;


 113  Array.Sort(vertices, VertexWrapComparer.Instance);


 114  CalculateFacePlane(newFace);


 115  faces[i] = newFace;


 116  }


 117 


 118  // update the adjacency (check all pairs of faces)


 119  for (var i = 0; i < Dimension; i++)


 120  {


 121  for (var j = i + 1; j < Dimension + 1; j++) UpdateAdjacency(faces[i], faces[j]);


 122  }


 123 


 124  return faces;


 125  }


 126 


 127  /// <summary>


 128  /// Calculates the normal and offset of the hyperplane given by the face's vertices.


 129  /// </summary>


 130  /// <param name="face"></param>


 131  /// <returns></returns>


 132  private void CalculateFacePlane(ConvexFaceInternal face)


 133  {


 134  var vertices = face.Vertices;


 135  var normal = face.Normal;


 136  FindNormalVector(vertices, normal);


 137 


 138  if (double.IsNaN(normal[0])) ThrowSingular();


 139 


 140  double offset = 0.0;


 141  double centerDistance = 0.0;


 142  var fi = vertices[0].PositionData;


 143  for (int i = 0; i < Dimension; i++)


 144  {


 145  double n = normal[i];


 146  offset += n * fi[i];


 147  centerDistance += n * Center[i];


 148  }


 149  face.Offset = offset;


 150  centerDistance = offset;


 151 


 152  if (centerDistance > 0)


 153  {


 154  for (int i = 0; i < Dimension; i++) normal[i] = normal[i];


 155  face.Offset = offset;


 156  face.IsNormalFlipped = true;


 157  }


 158  else face.IsNormalFlipped = false;


 159  }


 160 


 161  /// <summary>


 162  /// Check if the vertex is "visible" from the face.


 163  /// The vertex is "over face" if the return value is >= 0.


 164  /// </summary>


 165  /// <param name="v"></param>


 166  /// <param name="f"></param>


 167  /// <returns>The vertex is "over face" if the result is positive.</returns>


 168  double GetVertexDistance(VertexWrap v, ConvexFaceInternal f)


 169  {


 170  double[] normal = f.Normal;


 171  double[] p = v.PositionData;


 172  double distance = f.Offset;


 173  for (int i = 0; i < Dimension; i++) distance += normal[i] * p[i];


 174  return distance;


 175  }


 176 


 177  //unsafe double GetVertexDistance(VertexWrap v, ConvexFaceInternal f)


 178  //{


 179  // fixed (double* pNormal = f.Normal)


 180  // fixed (double* pP = v.PositionData)


 181  // {


 182  // double* normal = pNormal;


 183  // double* p = pP;


 184 


 185  // double distance = f.Offset;


 186  // for (int i = 0; i < Dimension; i++)


 187  // {


 188  // distance += (*normal) * (*p);


 189  // normal++;


 190  // p++;


 191  // }


 192  // return distance;


 193  // }


 194  //}


 195 


 196  /// <summary>


 197  /// Tags all faces seen from the current vertex with 1.


 198  /// </summary>


 199  /// <param name="currentFace"></param>


 200  void TagAffectedFaces(ConvexFaceInternal currentFace)


 201  {


 202  AffectedFaceBuffer.Clear();


 203  AffectedFaceBuffer.Add(currentFace);


 204  TraverseAffectedFaces(currentFace);


 205  }


 206 


 207  /// <summary>


 208  /// Recursively traverse all the relevant faces.


 209  /// </summary>


 210  void TraverseAffectedFaces(ConvexFaceInternal currentFace)


 211  {


 212  TraverseStack.Clear();


 213  TraverseStack.Push(currentFace);


 214  currentFace.Tag = 1;


 215 


 216  while (TraverseStack.Count > 0)


 217  {


 218  var top = TraverseStack.Pop();


 219  for (int i = 0; i < Dimension; i++)


 220  {


 221  var adjFace = top.AdjacentFaces[i];


 222 


 223  if (adjFace.Tag == 0 && GetVertexDistance(CurrentVertex, adjFace) >= 0)


 224  {


 225  AffectedFaceBuffer.Add(adjFace);


 226  //TraverseAffectedFaces(adjFace);


 227  adjFace.Tag = 1;


 228  TraverseStack.Push(adjFace);


 229  }


 230  }


 231  }


 232 


 233  ////for (int i = 0; i < Dimension; i++)


 234  ////{


 235  //// var adjFace = currentFace.AdjacentFaces[i];


 236 


 237  //// if (adjFace.Tag == 0 && GetVertexDistance(CurrentVertex, adjFace) >= 0)


 238  //// {


 239  //// AffectedFaceBuffer.Add(adjFace);


 240  //// TraverseAffectedFaces(adjFace);


 241  //// }


 242  ////}


 243  }


 244 


 245  /// <summary>


 246  /// Check if 2 faces are adjacent and if so, update their AdjacentFaces array.


 247  /// </summary>


 248  /// <param name="l"></param>


 249  /// <param name="r"></param>


 250  void UpdateAdjacency(ConvexFaceInternal l, ConvexFaceInternal r)


 251  {


 252  var lv = l.Vertices;


 253  var rv = r.Vertices;


 254  int i;


 255 


 256  // reset marks on the 1st face


 257  for (i = 0; i < Dimension; i++) lv[i].Marked = false;


 258 


 259  // mark all vertices on the 2nd face


 260  for (i = 0; i < Dimension; i++) rv[i].Marked = true;


 261 


 262  // find the 1st false index


 263  for (i = 0; i < Dimension; i++) if (!lv[i].Marked) break;


 264 


 265  // no vertex was marked


 266  if (i == Dimension) return;


 267 


 268  // check if only 1 vertex wasn't marked


 269  for (int j = i + 1; j < Dimension; j++) if (!lv[j].Marked) return;


 270 


 271  // if we are here, the two faces share an edge


 272  l.AdjacentFaces[i] = r;


 273 


 274  // update the adj. face on the other face  find the vertex that remains marked


 275  for (i = 0; i < Dimension; i++) lv[i].Marked = false;


 276  for (i = 0; i < Dimension; i++)


 277  {


 278  if (rv[i].Marked) break;


 279  }


 280  r.AdjacentFaces[i] = l;


 281  }


 282 


 283  #region Memory stuff.


 284 


 285  /// <summary>


 286  /// Recycle face for future use.


 287  /// </summary>


 288  void RecycleFace(ConvexFaceInternal face)


 289  {


 290  for (int i = 0; i < Dimension; i++)


 291  {


 292  face.AdjacentFaces[i] = null;


 293  }


 294  }


 295 


 296  /// <summary>


 297  /// Get a fresh face.


 298  /// </summary>


 299  /// <returns></returns>


 300  ConvexFaceInternal GetNewFace()


 301  {


 302  return RecycledFaceStack.Count != 0


 303  ? RecycledFaceStack.Pop()


 304  : new ConvexFaceInternal(Dimension, EmptyBufferStack.Count != 0 ? EmptyBufferStack.Pop() : new VertexBuffer());


 305  }


 306 


 307  /// <summary>


 308  /// Get a new connector.


 309  /// </summary>


 310  /// <returns></returns>


 311  FaceConnector GetNewConnector()


 312  {


 313  return ConnectorStack.Count != 0


 314  ? ConnectorStack.Pop()


 315  : new FaceConnector(Dimension);


 316  }


 317  #endregion


 318 


 319  /// <summary>


 320  /// Connect faces using a connector.


 321  /// </summary>


 322  /// <param name="connector"></param>


 323  void ConnectFace(FaceConnector connector)


 324  {


 325  var index = connector.HashCode % ConnectorTableSize;


 326  var list = ConnectorTable[index];


 327 


 328  int count = 0;


 329  for (var current = list.First; current != null; current = current.Next)


 330  {


 331  if (FaceConnector.AreConnectable(connector, current, Dimension))


 332  {


 333  list.Remove(current);


 334  FaceConnector.Connect(current, connector);


 335  current.Face = null;


 336  connector.Face = null;


 337  ConnectorStack.Push(current);


 338  ConnectorStack.Push(connector);


 339  return;


 340  }


 341  }


 342 


 343  list.Add(connector);


 344  }


 345 


 346  /// <summary>


 347  /// Removes the faces "covered" by the current vertex and adds the newly created ones.


 348  /// </summary>


 349  private void CreateCone()


 350  {


 351  var oldFaces = AffectedFaceBuffer;


 352 


 353  var currentVertexIndex = CurrentVertex.Index;


 354 


 355  for (int fIndex = 0; fIndex < oldFaces.Count; fIndex++)


 356  {


 357  var oldFace = oldFaces[fIndex];


 358 


 359  // Find the faces that need to be updated


 360  int updateCount = 0;


 361  for (int i = 0; i < Dimension; i++)


 362  {


 363  var af = oldFace.AdjacentFaces[i];


 364  if (af.Tag == 0) // Tag == 0 when oldFaces does not contain af


 365  {


 366  UpdateBuffer[updateCount] = af;


 367  UpdateIndices[updateCount] = i;


 368  ++updateCount;


 369  }


 370  }


 371 


 372  // Recycle the face for future use


 373  if (updateCount == 0)


 374  {


 375  // If the face is present in the unprocessed list, remove it


 376  UnprocessedFaces.Remove(oldFace);


 377 


 378  RecycleFace(oldFace);


 379  RecycledFaceStack.Push(oldFace);


 380  }


 381 


 382  for (int i = 0; i < updateCount; i++)


 383  {


 384  var adjacentFace = UpdateBuffer[i];


 385 


 386  int oldFaceAdjacentIndex = 0;


 387  var adjFaceAdjacency = adjacentFace.AdjacentFaces;


 388  for (int j = 0; j < Dimension; j++)


 389  {


 390  if (object.ReferenceEquals(oldFace, adjFaceAdjacency[j]))


 391  {


 392  oldFaceAdjacentIndex = j;


 393  break;


 394  }


 395  }


 396 


 397  var forbidden = UpdateIndices[i]; // Index of the face that corresponds to this adjacent face


 398 


 399  ConvexFaceInternal newFace;


 400 


 401  int oldVertexIndex;


 402  VertexWrap[] vertices;


 403 


 404  // Recycle the oldFace


 405  if (i == updateCount  1)


 406  {


 407  RecycleFace(oldFace);


 408  newFace = oldFace;


 409  vertices = newFace.Vertices;


 410  oldVertexIndex = vertices[forbidden].Index;


 411  }


 412  else // Pop a face from the recycled stack or create a new one


 413  {


 414  newFace = GetNewFace();


 415  vertices = newFace.Vertices;


 416  for (int j = 0; j < Dimension; j++) vertices[j] = oldFace.Vertices[j];


 417  oldVertexIndex = vertices[forbidden].Index;


 418  }


 419 


 420  int orderedPivotIndex;


 421 


 422  // correct the ordering


 423  if (currentVertexIndex < oldVertexIndex)


 424  {


 425  orderedPivotIndex = 0;


 426  for (int j = forbidden  1; j >= 0; j)


 427  {


 428  if (vertices[j].Index > currentVertexIndex) vertices[j + 1] = vertices[j];


 429  else


 430  {


 431  orderedPivotIndex = j + 1;


 432  break;


 433  }


 434  }


 435  }


 436  else


 437  {


 438  orderedPivotIndex = Dimension  1;


 439  for (int j = forbidden + 1; j < Dimension; j++)


 440  {


 441  if (vertices[j].Index < currentVertexIndex) vertices[j  1] = vertices[j];


 442  else


 443  {


 444  orderedPivotIndex = j  1;


 445  break;


 446  }


 447  }


 448  }


 449 


 450  vertices[orderedPivotIndex] = CurrentVertex;


 451 


 452  CalculateFacePlane(newFace);


 453  newFace.AdjacentFaces[orderedPivotIndex] = adjacentFace;


 454  adjacentFace.AdjacentFaces[oldFaceAdjacentIndex] = newFace;


 455 


 456  // let there be a connection.


 457  for (int j = 0; j < Dimension; j++)


 458  {


 459  if (j == orderedPivotIndex) continue;


 460  var connector = GetNewConnector();


 461  connector.Update(newFace, j, Dimension);


 462  ConnectFace(connector);


 463  }


 464 


 465  // This could slightly help...


 466  if (adjacentFace.VerticesBeyond.Count < oldFace.VerticesBeyond.Count)


 467  {


 468  FindBeyondVertices(newFace, adjacentFace.VerticesBeyond, oldFace.VerticesBeyond);


 469  }


 470  else


 471  {


 472  FindBeyondVertices(newFace, oldFace.VerticesBeyond, adjacentFace.VerticesBeyond);


 473  }


 474 


 475  // This face will definitely lie on the hull


 476  if (newFace.VerticesBeyond.Count == 0)


 477  {


 478  ConvexFaces.Add(newFace);


 479  UnprocessedFaces.Remove(newFace);


 480  EmptyBufferStack.Push(newFace.VerticesBeyond);


 481  newFace.VerticesBeyond = EmptyBuffer;


 482  }


 483  else // Add the face to the list


 484  {


 485  UnprocessedFaces.Add(newFace);


 486  }


 487  }


 488  }


 489  }


 490 


 491  /// <summary>


 492  /// Subtracts vectors x and y and stores the result to target.


 493  /// </summary>


 494  /// <param name="x"></param>


 495  /// <param name="y"></param>


 496  /// <param name="target"></param>


 497  void SubtractFast(double[] x, double[] y, double[] target)


 498  {


 499  for (int i = 0; i < Dimension; i++)


 500  {


 501  target[i] = x[i]  y[i];


 502  }


 503  }


 504 


 505  /// <summary>


 506  /// Finds 4D normal vector.


 507  /// </summary>


 508  /// <param name="vertices"></param>


 509  /// <param name="normal"></param>


 510  void FindNormalVector4D(VertexWrap[] vertices, double[] normal)


 511  {


 512  SubtractFast(vertices[1].PositionData, vertices[0].PositionData, ntX);


 513  SubtractFast(vertices[2].PositionData, vertices[1].PositionData, ntY);


 514  SubtractFast(vertices[3].PositionData, vertices[2].PositionData, ntZ);


 515 


 516  var x = ntX;


 517  var y = ntY;


 518  var z = ntZ;


 519 


 520  // This was generated using Mathematica


 521  var nx = x[3] * (y[2] * z[1]  y[1] * z[2])


 522  + x[2] * (y[1] * z[3]  y[3] * z[1])


 523  + x[1] * (y[3] * z[2]  y[2] * z[3]);


 524  var ny = x[3] * (y[0] * z[2]  y[2] * z[0])


 525  + x[2] * (y[3] * z[0]  y[0] * z[3])


 526  + x[0] * (y[2] * z[3]  y[3] * z[2]);


 527  var nz = x[3] * (y[1] * z[0]  y[0] * z[1])


 528  + x[1] * (y[0] * z[3]  y[3] * z[0])


 529  + x[0] * (y[3] * z[1]  y[1] * z[3]);


 530  var nw = x[2] * (y[0] * z[1]  y[1] * z[0])


 531  + x[1] * (y[2] * z[0]  y[0] * z[2])


 532  + x[0] * (y[1] * z[2]  y[2] * z[1]);


 533 


 534  double norm = System.Math.Sqrt(nx * nx + ny * ny + nz * nz + nw * nw);


 535 


 536  double f = 1.0 / norm;


 537  normal[0] = f * nx;


 538  normal[1] = f * ny;


 539  normal[2] = f * nz;


 540  normal[3] = f * nw;


 541  }


 542 


 543  /// <summary>


 544  /// Finds 3D normal vector.


 545  /// </summary>


 546  /// <param name="vertices"></param>


 547  /// <param name="normal"></param>


 548  void FindNormalVector3D(VertexWrap[] vertices, double[] normal)


 549  {


 550  SubtractFast(vertices[1].PositionData, vertices[0].PositionData, ntX);


 551  SubtractFast(vertices[2].PositionData, vertices[1].PositionData, ntY);


 552 


 553  var x = ntX;


 554  var y = ntY;


 555 


 556  var nx = x[1] * y[2]  x[2] * y[1];


 557  var ny = x[2] * y[0]  x[0] * y[2];


 558  var nz = x[0] * y[1]  x[1] * y[0];


 559 


 560  double norm = System.Math.Sqrt(nx * nx + ny * ny + nz * nz);


 561 


 562  double f = 1.0 / norm;


 563  normal[0] = f * nx;


 564  normal[1] = f * ny;


 565  normal[2] = f * nz;


 566  }


 567 


 568  /// <summary>


 569  /// Finds 2D normal vector.


 570  /// </summary>


 571  /// <param name="vertices"></param>


 572  /// <param name="normal"></param>


 573  void FindNormalVector2D(VertexWrap[] vertices, double[] normal)


 574  {


 575  SubtractFast(vertices[1].PositionData, vertices[0].PositionData, ntX);


 576 


 577  var x = ntX;


 578 


 579  var nx = x[1];


 580  var ny = x[0];


 581 


 582  double norm = System.Math.Sqrt(nx * nx + ny * ny);


 583 


 584  double f = 1.0 / norm;


 585  normal[0] = f * nx;


 586  normal[1] = f * ny;


 587  }


 588 


 589  /// <summary>


 590  /// Finds normal vector of a hyperplane given by vertices.


 591  /// Stores the results to normalData.


 592  /// </summary>


 593  /// <param name="vertices"></param>


 594  /// <param name="normalData"></param>


 595  private void FindNormalVector(VertexWrap[] vertices, double[] normalData)


 596  {


 597  switch (Dimension)


 598  {


 599  case 2: FindNormalVector2D(vertices, normalData); break;


 600  case 3: FindNormalVector3D(vertices, normalData); break;


 601  case 4: FindNormalVector4D(vertices, normalData); break;


 602  default:


 603  {


 604  for (var i = 0; i < Dimension; i++) nDNormalSolveVector[i] = 1.0;


 605  for (var i = 0; i < Dimension; i++)


 606  {


 607  var row = jaggedNDMatrix[i];


 608  var pos = vertices[i].Vertex.Position;


 609  for (int j = 0; j < Dimension; j++) row[j] = pos[j];


 610  }


 611  StarMath.gaussElimination(Dimension, jaggedNDMatrix, nDNormalSolveVector, normalData);


 612  StarMath.normalizeInPlace(normalData, Dimension);


 613  break;


 614  }


 615  }


 616  }


 617 


 618  /// <summary>


 619  /// Check whether the vertex v is beyond the given face. If so, add it to beyondVertices.


 620  /// </summary>


 621  /// <param name="face"></param>


 622  /// <param name="beyondVertices"></param>


 623  /// <param name="v"></param>


 624  void IsBeyond(ConvexFaceInternal face, VertexBuffer beyondVertices, VertexWrap v)


 625  {


 626  double distance = GetVertexDistance(v, face);


 627  if (distance >= 0)


 628  {


 629  if (distance > MaxDistance)


 630  {


 631  MaxDistance = distance;


 632  FurthestVertex = v;


 633  }


 634  beyondVertices.Add(v);


 635  }


 636  }


 637 


 638  /// <summary>


 639  /// Used in the "initialization" code.


 640  /// </summary>


 641  void FindBeyondVertices(ConvexFaceInternal face)


 642  {


 643  var beyondVertices = face.VerticesBeyond;


 644 


 645  MaxDistance = double.NegativeInfinity;


 646  FurthestVertex = null;


 647 


 648  int count = InputVertices.Count;


 649  for (int i = 0; i < count; i++) IsBeyond(face, beyondVertices, InputVertices[i]);


 650 


 651  face.FurthestVertex = FurthestVertex;


 652  //face.FurthestDistance = MaxDistance;


 653  }


 654 


 655  /// <summary>


 656  /// Used by update faces.


 657  /// </summary>


 658  void FindBeyondVertices(ConvexFaceInternal face, VertexBuffer beyond, VertexBuffer beyond1)


 659  {


 660  var beyondVertices = BeyondBuffer;


 661 


 662  MaxDistance = double.NegativeInfinity;


 663  FurthestVertex = null;


 664  VertexWrap v;


 665 


 666  int count = beyond1.Count;


 667  for (int i = 0; i < count; i++) beyond1[i].Marked = true;


 668  CurrentVertex.Marked = false;


 669  count = beyond.Count;


 670  for (int i = 0; i < count; i++)


 671  {


 672  v = beyond[i];


 673  if (object.ReferenceEquals(v, CurrentVertex)) continue;


 674  v.Marked = false;


 675  IsBeyond(face, beyondVertices, v);


 676  }


 677 


 678  count = beyond1.Count;


 679  for (int i = 0; i < count; i++)


 680  {


 681  v = beyond1[i];


 682  if (v.Marked) IsBeyond(face, beyondVertices, v);


 683  }


 684 


 685  face.FurthestVertex = FurthestVertex;


 686  //face.FurthestDistance = MaxDistance;


 687 


 688  // Pull the old switch a roo


 689  var temp = face.VerticesBeyond;


 690  face.VerticesBeyond = beyondVertices;


 691  if (temp.Count > 0) temp.Clear();


 692  BeyondBuffer = temp;


 693  }


 694 


 695  /// <summary>


 696  /// Recalculates the centroid of the current hull.


 697  /// </summary>


 698  void UpdateCenter()


 699  {


 700  for (int i = 0; i < Dimension; i++) Center[i] *= (ConvexHull.Count  1);


 701  double f = 1.0 / ConvexHull.Count;


 702  for (int i = 0; i < Dimension; i++) Center[i] = f * (Center[i] + CurrentVertex.PositionData[i]);


 703  }


 704 


 705  /// <summary>


 706  /// Find the (dimension+1) initial points and create the simplexes.


 707  /// </summary>


 708  void InitConvexHull()


 709  {


 710  var extremes = FindExtremes();


 711  var initialPoints = FindInitialPoints(extremes);


 712 


 713  // Add the initial points to the convex hull.


 714  foreach (var vertex in initialPoints)


 715  {


 716  CurrentVertex = vertex;


 717  ConvexHull.Add(CurrentVertex);


 718  UpdateCenter();


 719  InputVertices.Remove(vertex);


 720 


 721  // Because of the AklTou heuristic.


 722  extremes.Remove(vertex);


 723  }


 724 


 725  // Create the initial simplexes.


 726  var faces = InitiateFaceDatabase();


 727 


 728  // Init the vertex beyond buffers.


 729  foreach (var face in faces)


 730  {


 731  FindBeyondVertices(face);


 732  if (face.VerticesBeyond.Count == 0) ConvexFaces.Add(face); // The face is on the hull


 733  else UnprocessedFaces.Add(face);


 734  }


 735  }


 736 


 737  /// <summary>


 738  /// Finds (dimension + 1) initial points.


 739  /// </summary>


 740  /// <param name="extremes"></param>


 741  /// <returns></returns>


 742  private List<VertexWrap> FindInitialPoints(List<VertexWrap> extremes)


 743  {


 744  List<VertexWrap> initialPoints = new List<VertexWrap>();// { extremes[0], extremes[1] };


 745 


 746  VertexWrap first = null, second = null;


 747  double maxDist = 0;


 748  for (int i = 0; i < extremes.Count  1; i++)


 749  {


 750  var a = extremes[i];


 751  for (int j = i + 1; j < extremes.Count; j++)


 752  {


 753  var b = extremes[j];


 754  var dist = StarMath.norm2(StarMath.subtract(a.PositionData, b.PositionData, Dimension), Dimension, true);


 755  if (dist > maxDist)


 756  {


 757  first = a;


 758  second = b;


 759  maxDist = dist;


 760  }


 761  }


 762  }


 763 


 764  initialPoints.Add(first);


 765  initialPoints.Add(second);


 766 


 767  for (int i = 2; i <= Dimension; i++)


 768  {


 769  double maximum = 0.0001;


 770  VertexWrap maxPoint = null;


 771  for (int j = 0; j < extremes.Count; j++)


 772  {


 773  var extreme = extremes[j];


 774  if (initialPoints.Contains(extreme)) continue;


 775 


 776  var val = GetSimplexVolume(extreme, initialPoints);


 777 


 778  if (val > maximum)


 779  {


 780  maximum = val;


 781  maxPoint = extreme;


 782  }


 783  }


 784  if (maxPoint != null) initialPoints.Add(maxPoint);


 785  else


 786  {


 787  int vCount = InputVertices.Count;


 788  for (int j = 0; j < vCount; j++)


 789  {


 790  var point = InputVertices[j];


 791  if (initialPoints.Contains(point)) continue;


 792 


 793  var val = GetSimplexVolume(point, initialPoints);


 794 


 795  if (val > maximum)


 796  {


 797  maximum = val;


 798  maxPoint = point;


 799  }


 800  }


 801 


 802  if (maxPoint != null) initialPoints.Add(maxPoint);


 803  else ThrowSingular();


 804  }


 805  }


 806  return initialPoints;


 807  }


 808 


 809  /// <summary>


 810  /// Computes the volume of the (n=initialPoints.Count)D simplex defined by the


 811  /// pivot and initialPoints.


 812  /// This is computed as the determinant of the matrix  initialPoints[i]  pivot 


 813  /// </summary>


 814  /// <param name="pivot"></param>


 815  /// <param name="initialPoints"></param>


 816  /// <returns></returns>


 817  double GetSimplexVolume(VertexWrap pivot, List<VertexWrap> initialPoints)


 818  {


 819  var dim = initialPoints.Count;


 820  var m = nDMatrix;


 821 


 822  for (int i = 0; i < dim; i++)


 823  {


 824  var pts = initialPoints[i];


 825  for (int j = 0; j < dim; j++) m[i, j] = pts.PositionData[j]  pivot.PositionData[j];


 826  }


 827 


 828  return Math.Abs(StarMath.determinantDestructive(m, dim));


 829  }


 830 


 831  /// <summary>


 832  /// Finds the extremes in all dimensions.


 833  /// </summary>


 834  /// <returns></returns>


 835  private List<VertexWrap> FindExtremes()


 836  {


 837  var extremes = new List<VertexWrap>(2 * Dimension);


 838 


 839  int vCount = InputVertices.Count;


 840  for (int i = 0; i < Dimension; i++)


 841  {


 842  double min = double.MaxValue, max = double.MinValue;


 843  int minInd = 0, maxInd = 0;


 844  for (int j = 0; j < vCount; j++)


 845  {


 846  var v = InputVertices[j].PositionData[i];


 847  if (v < min)


 848  {


 849  min = v;


 850  minInd = j;


 851  }


 852  if (v > max)


 853  {


 854  max = v;


 855  maxInd = j;


 856  }


 857  }


 858 


 859  if (minInd != maxInd)


 860  {


 861  extremes.Add(InputVertices[minInd]);


 862  extremes.Add(InputVertices[maxInd]);


 863  }


 864  else extremes.Add(InputVertices[minInd]);


 865  }


 866  return extremes;


 867  }


 868 


 869  /// <summary>


 870  /// The exception thrown if singular input data detected.


 871  /// </summary>


 872  void ThrowSingular()


 873  {


 874  throw new InvalidOperationException(


 875  "ConvexHull: Singular input data (i.e. trying to triangulate a data that contain a regular lattice of points).\n"


 876  + "Introducing some noise to the data might resolve the issue.");


 877  }


 878 


 879  /// <summary>


 880  /// Fins the convex hull.


 881  /// </summary>


 882  void FindConvexHull()


 883  {


 884  // Find the (dimension+1) initial points and create the simplexes.


 885  InitConvexHull();


 886 


 887  // Expand the convex hull and faces.


 888  while (UnprocessedFaces.First != null)


 889  {


 890  var currentFace = UnprocessedFaces.First;


 891  CurrentVertex = currentFace.FurthestVertex;


 892  ConvexHull.Add(CurrentVertex);


 893  UpdateCenter();


 894 


 895  // The affected faces get tagged


 896  TagAffectedFaces(currentFace);


 897 


 898  // Create the cone from the currentVertex and the affected faces horizon.


 899  CreateCone();


 900 


 901  // Need to reset the tags


 902  int count = AffectedFaceBuffer.Count;


 903  for (int i = 0; i < count; i++) AffectedFaceBuffer[i].Tag = 0;


 904  }


 905  }


 906 


 907  /// <summary>


 908  /// Wraps the vertices and determines the dimension if it's unknown.


 909  /// </summary>


 910  /// <param name="vertices"></param>


 911  /// <param name="dim"></param>


 912  private ConvexHullInternal(IEnumerable<IVertex> vertices)


 913  {


 914  InputVertices = new List<VertexWrap>(vertices.Select((v, i) => new VertexWrap { Vertex = v, PositionData = v.Position, Index = i }));


 915  Dimension = DetermineDimension();


 916  Initialize();


 917  }


 918 


 919  /// <summary>


 920  /// Finds the vertices on the convex hull and optionally converts them to the TVertex array.


 921  /// </summary>


 922  /// <typeparam name="TVertex"></typeparam>


 923  /// <param name="onlyCompute"></param>


 924  /// <returns></returns>


 925  private IEnumerable<TVertex> GetConvexHullInternal<TVertex>(bool onlyCompute = false) where TVertex : IVertex


 926  {


 927  if (Computed) return onlyCompute ? null : ConvexHull.Select(v => (TVertex)v.Vertex).ToArray();


 928 


 929  if (Dimension < 2) throw new ArgumentException("Dimension of the input must be 2 or greater.");


 930 


 931  FindConvexHull();


 932  Computed = true;


 933  return onlyCompute ? null : ConvexHull.Select(v => (TVertex)v.Vertex).ToArray();


 934  }


 935 


 936  /// <summary>


 937  /// Finds the convex hull and creates the TFace objects.


 938  /// </summary>


 939  /// <typeparam name="TVertex"></typeparam>


 940  /// <typeparam name="TFace"></typeparam>


 941  /// <returns></returns>


 942  private IEnumerable<TFace> GetConvexFacesInternal<TVertex, TFace>()


 943  where TFace : ConvexFace<TVertex, TFace>, new()


 944  where TVertex : IVertex


 945  {


 946  if (!Computed) GetConvexHullInternal<TVertex>(true);


 947 


 948  var faces = ConvexFaces;


 949  int cellCount = faces.Count;


 950  var cells = new TFace[cellCount];


 951 


 952  for (int i = 0; i < cellCount; i++)


 953  {


 954  var face = faces[i];


 955  var vertices = new TVertex[Dimension];


 956  for (int j = 0; j < Dimension; j++) vertices[j] = (TVertex)face.Vertices[j].Vertex;


 957  cells[i] = new TFace


 958  {


 959  Vertices = vertices,


 960  Adjacency = new TFace[Dimension],


 961  Normal = face.Normal


 962  };


 963  face.Tag = i;


 964  }


 965 


 966  for (int i = 0; i < cellCount; i++)


 967  {


 968  var face = faces[i];


 969  var cell = cells[i];


 970  for (int j = 0; j < Dimension; j++)


 971  {


 972  if (face.AdjacentFaces[j] == null) continue;


 973  cell.Adjacency[j] = cells[face.AdjacentFaces[j].Tag];


 974  }


 975 


 976  // Fix the vertex orientation.


 977  if (face.IsNormalFlipped)


 978  {


 979  var tempVert = cell.Vertices[0];


 980  cell.Vertices[0] = cell.Vertices[Dimension  1];


 981  cell.Vertices[Dimension  1] = tempVert;


 982 


 983  var tempAdj = cell.Adjacency[0];


 984  cell.Adjacency[0] = cell.Adjacency[Dimension  1];


 985  cell.Adjacency[Dimension  1] = tempAdj;


 986  }


 987  }


 988 


 989  return cells;


 990  }


 991 


 992  /// <summary>


 993  /// This is used by the Delaunay triangulation code.


 994  /// </summary>


 995  /// <typeparam name="TVertex"></typeparam>


 996  /// <typeparam name="TFace"></typeparam>


 997  /// <param name="data"></param>


 998  /// <returns></returns>


 999  internal static List<ConvexFaceInternal> GetConvexFacesInternal<TVertex, TFace>(IEnumerable<TVertex> data)


 1000  where TFace : ConvexFace<TVertex, TFace>, new()


 1001  where TVertex : IVertex


 1002  {


 1003  ConvexHullInternal ch = new ConvexHullInternal(data.Cast<IVertex>());


 1004  ch.GetConvexHullInternal<TVertex>(true);


 1005  return ch.ConvexFaces;


 1006  }


 1007 


 1008  /// <summary>


 1009  /// This is called by the "ConvexHull" class.


 1010  /// </summary>


 1011  /// <typeparam name="TVertex"></typeparam>


 1012  /// <typeparam name="TFace"></typeparam>


 1013  /// <param name="data"></param>


 1014  /// <returns></returns>


 1015  internal static Tuple<IEnumerable<TVertex>, IEnumerable<TFace>> GetConvexHullAndFaces<TVertex, TFace>(IEnumerable<IVertex> data)


 1016  where TFace : ConvexFace<TVertex, TFace>, new()


 1017  where TVertex : IVertex


 1018  {


 1019  ConvexHullInternal ch = new ConvexHullInternal(data);


 1020  return Tuple.Create(


 1021  ch.GetConvexHullInternal<TVertex>(),


 1022  ch.GetConvexFacesInternal<TVertex, TFace>());


 1023  }


 1024  }


 1025  }

