Free cookie consent management tool by TermsFeed Policy Generator

source: branches/HeuristicLab.Analysis.AlgorithmBehavior/MIConvexHull/ConvexHull/ConvexHullInternal.cs @ 9945

Last change on this file since 9945 was 9945, checked in by ascheibe, 10 years ago

#1886

  • added new convex hull algorithm based on SMO/SVM
  • added unit test and a visualization
  • updated MIConvexHull
File size: 40.5 KB
Line 
1namespace 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        List<DeferredFace> ConeFaceBuffer;
45        Stack<DeferredFace> ConeFaceStack;
46        HashSet<VertexWrap> SingularVertices;
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>();
74            ConeFaceBuffer = new List<DeferredFace>();
75            ConeFaceStack = new Stack<DeferredFace>();
76            SingularVertices = new HashSet<VertexWrap>();
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>
138        private bool CalculateFacePlane(ConvexFaceInternal face)
139        {
140            var vertices = face.Vertices;
141            var normal = face.Normal;
142            FindNormalVector(vertices, normal);
143
144            if (double.IsNaN(normal[0]))
145            {
146                return false;
147                //ThrowSingular();
148            }
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;
169
170            return true;
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>
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>
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>
382        private bool CreateCone()
383        {
384            var currentVertexIndex = CurrentVertex.Index;
385            ConeFaceBuffer.Clear();
386
387            for (int fIndex = 0; fIndex < AffectedFaceBuffer.Count; fIndex++)
388            {
389                var oldFace = AffectedFaceBuffer[fIndex];
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
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;
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
463                    if (!CalculateFacePlane(newFace))
464                    {
465                        return false;
466                    }
467
468                    ConeFaceBuffer.Add(GetDeferredFace(newFace, orderedPivotIndex, adjacentFace, oldFaceAdjacentIndex, oldFace));
469                }
470            }
471           
472            return true;
473        }
474
475        /// <summary>
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>
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        {
751            var count = ConvexHull.Count + 1;
752            for (int i = 0; i < Dimension; i++) Center[i] *= (count - 1);
753            double f = 1.0 / count;
754            for (int i = 0; i < Dimension; i++) Center[i] = f * (Center[i] + CurrentVertex.PositionData[i]);
755        }
756
757        /// <summary>
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>
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;
780                // update center must be called before adding the vertex.
781                UpdateCenter();
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            {
833                double maximum = 0.000001;
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        {
883            var initPtsNum = initialPoints.Count;
884            //var m = nDMatrix;
885            var sum = 0.0;
886       
887            for (int i = 0; i < initPtsNum; i++)
888            {
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]);
893            }
894
895            return sum;
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>
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>
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;
984                                               
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.
991                if (!SingularVertices.Contains(CurrentVertex) && CreateCone()) CommitCone();
992                else HandleSingular();
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}
Note: See TracBrowser for help on using the repository browser.