aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/OpenSim/Region/Physics/OdePlugin/Meshing/SimpleHull.cs
blob: 2caa818c00c54b39477c00ffa44c8720931fae71 (plain)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
using System;
using System.Collections.Generic;
using System.Text;

using OpenSim.Framework.Console;

namespace OpenSim.Region.Physics.OdePlugin.Meshing
{
    // A simple hull is a set of vertices building up to simplices that border a region
    // The word simple referes to the fact, that this class assumes, that all simplices
    // do not intersect
    // Simple hulls can be added and subtracted.
    // Vertices can be checked to lie inside a hull
    // Also note, that the sequence of the vertices is important and defines if the region that
    // is defined by the hull lies inside or outside the simplex chain
    public class SimpleHull
    {
        List<Vertex> vertices = new List<Vertex>();
        List<Vertex> holeVertices = new List<Vertex>(); // Only used, when the hull is hollow

        // Adds a vertex to the end of the list
        public void AddVertex(Vertex v) {
            vertices.Add(v);
        }

        override public String ToString()
        {
            String result="";
            foreach (Vertex v in vertices)
            {
                result += "b:" + v.ToString() + "\n";
            }

            return result;
        }


        public List<Vertex> getVertices() {
            List<Vertex> newVertices = new List<Vertex>();

            newVertices.AddRange(vertices);
            newVertices.Add(null);
            newVertices.AddRange(holeVertices);

            return newVertices;
        }

        public SimpleHull Clone()
        {
            SimpleHull result = new SimpleHull();
            foreach (Vertex v in vertices)
            {
                result.AddVertex(v.Clone());
            }

            foreach (Vertex v in this.holeVertices)
            {
                result.holeVertices.Add(v.Clone());
            }
            
            return result;
        }

        public bool IsPointIn(Vertex v1)
        {
            int iCounter=0;
            List<Simplex> simplices=buildSimplexList();
            foreach (Simplex s in simplices)
            {
                // Send a ray along the positive X-Direction
                // Note, that this direction must correlate with the "below" interpretation
                // of handling for the special cases below
                Manager.PhysicsVector intersection = s.RayIntersect(v1, new Manager.PhysicsVector(1.0f, 0.0f, 0.0f), true);

                if (intersection == null)
                    continue;   // No intersection. Done. More tests to follow otherwise

                // Did we hit the end of a simplex?
                // Then this can be one of two special cases:
                // 1. we go through a border exactly at a joint
                // 2. we have just marginally touched a corner
                // 3. we can slide along a border
                // Solution: If the other vertex is "below" the ray, we don't count it
                // Thus corners pointing down are counted twice, corners pointing up are not counted
                // borders are counted once
                if (intersection.IsIdentical(s.v1, 0.001f)) {
                    if (s.v2.Y < v1.Y)
                        continue;
                }
                // Do this for the other vertex two
                if (intersection.IsIdentical(s.v2, 0.001f)) {
                    if (s.v1.Y<v1.Y)
                        continue;
                }
                iCounter++;
            }

            return iCounter % 2 == 1; // Point is inside if the number of intersections is odd
        }

        public bool containsPointsFrom(SimpleHull otherHull)
        {
            foreach (Vertex v in otherHull.vertices)
            {
                if (IsPointIn(v))
                    return true;
            }

            return false;
        }


        List<Simplex> buildSimplexList() {

            List<Simplex> result = new List<Simplex>();

            // Not asserted but assumed: at least three vertices
            for (int i=0; i<vertices.Count-1; i++) {
                Simplex s=new Simplex(vertices[i], vertices[i+1]);
                result.Add(s);
            }
            Simplex s1=new Simplex(vertices[vertices.Count-1], vertices[0]);
            result.Add(s1);

            if (holeVertices.Count==0)
                return result;

            // Same here. At least three vertices in hole assumed
            for (int i = 0; i < holeVertices.Count - 1; i++)
            {
                Simplex s = new Simplex(holeVertices[i], holeVertices[i + 1]);
                result.Add(s);
            }
            
            s1 = new Simplex(holeVertices[holeVertices.Count - 1], holeVertices[0]);
            result.Add(s1);
            return result;
        }

        bool InsertVertex(Vertex v, int iAfter)
        {
            vertices.Insert(iAfter + 1, v);
            return true;
        }

        Vertex getNextVertex(Vertex currentVertex)
        {
            int iCurrentIndex;
            iCurrentIndex = vertices.IndexOf(currentVertex);

            // Error handling for iCurrentIndex==-1 should go here (and probably never will)

            iCurrentIndex++;
            if (iCurrentIndex == vertices.Count)
                iCurrentIndex = 0;

            return vertices[iCurrentIndex];
        }

        public Vertex FindVertex(Vertex vBase, float tolerance) {
            foreach (Vertex v in vertices) {
                if (v.IsIdentical(vBase, tolerance))
                    return v;
            }

            return null;
        }

        public void FindIntersection(Simplex s, ref Vertex Intersection, ref Vertex nextVertex)
        {
            Vertex bestIntersection=null;
            float distToV1=Single.PositiveInfinity;
            Simplex bestIntersectingSimplex=null;

            List<Simplex> simple = buildSimplexList();
            foreach (Simplex sTest in simple)
            {
                Manager.PhysicsVector vvTemp = Simplex.Intersect(sTest, s, -.001f, -.001f, 0.999f, .999f);
                
                Vertex vTemp=null;
                if (vvTemp != null)
                    vTemp = new Vertex(vvTemp);

                if (vTemp!=null) {

                    Manager.PhysicsVector diff=(s.v1-vTemp);
                    float distTemp=diff.length();

                    if (bestIntersection==null || distTemp<distToV1) {
                        bestIntersection=vTemp;
                        distToV1=distTemp;
                        bestIntersectingSimplex = sTest;
                    }

                } // end if vTemp

            } // end foreach

            Intersection = bestIntersection;
            if (bestIntersectingSimplex != null)
                nextVertex = bestIntersectingSimplex.v2;
            else
                nextVertex = null;
        }


        public static SimpleHull SubtractHull(SimpleHull baseHull, SimpleHull otherHull)
        {

            SimpleHull baseHullClone = baseHull.Clone();
            SimpleHull otherHullClone = otherHull.Clone();
            bool intersects = false;

            MainLog.Instance.Debug("State before intersection detection");
            MainLog.Instance.Debug("The baseHull is:\n{1}", 0, baseHullClone.ToString());
            MainLog.Instance.Debug("The otherHull is:\n{1}", 0, otherHullClone.ToString());

            {
                int iBase, iOther;

                // Insert into baseHull
                for (iBase = 0; iBase < baseHullClone.vertices.Count; iBase++)
                {
                    int iBaseNext = (iBase + 1) % baseHullClone.vertices.Count;
                    Simplex sBase = new Simplex(baseHullClone.vertices[iBase], baseHullClone.vertices[iBaseNext]);

                    for (iOther = 0; iOther < otherHullClone.vertices.Count; iOther++)
                    {
                        int iOtherNext = (iOther + 1) % otherHullClone.vertices.Count;
                        Simplex sOther = new Simplex(otherHullClone.vertices[iOther], otherHullClone.vertices[iOtherNext]);

                        Manager.PhysicsVector intersect = Simplex.Intersect(sBase, sOther, 0.001f, -.001f, 0.999f, 1.001f);
                        if (intersect != null)
                        {
                            Vertex vIntersect = new Vertex(intersect);
                            baseHullClone.vertices.Insert(iBase + 1, vIntersect);
                            sBase.v2 = vIntersect;
                            intersects = true;
                        }
                    }
                }
            }

            MainLog.Instance.Debug("State after intersection detection for the base hull");
            MainLog.Instance.Debug("The baseHull is:\n{1}", 0, baseHullClone.ToString());

            {
                int iOther, iBase;

                // Insert into otherHull
                for (iOther = 0; iOther < otherHullClone.vertices.Count; iOther++)
                {
                    int iOtherNext = (iOther + 1) % otherHullClone.vertices.Count;
                    Simplex sOther = new Simplex(otherHullClone.vertices[iOther], otherHullClone.vertices[iOtherNext]);

                    for (iBase = 0; iBase < baseHullClone.vertices.Count; iBase++)
                    {
                        int iBaseNext = (iBase + 1) % baseHullClone.vertices.Count;
                        Simplex sBase = new Simplex(baseHullClone.vertices[iBase], baseHullClone.vertices[iBaseNext]);

                        Manager.PhysicsVector intersect = Simplex.Intersect(sBase, sOther, -.001f, 0.001f, 1.001f, 0.999f);
                        if (intersect != null)
                        {
                            Vertex vIntersect = new Vertex(intersect);
                            otherHullClone.vertices.Insert(iOther + 1, vIntersect);
                            sOther.v2 = vIntersect;
                            intersects = true;
                        }
                    }
                }
            }

            MainLog.Instance.Debug("State after intersection detection for the base hull");
            MainLog.Instance.Debug("The otherHull is:\n{1}", 0, otherHullClone.ToString());


            bool otherIsInBase = baseHullClone.containsPointsFrom(otherHullClone);
            if (!intersects && otherIsInBase)
            {
                // We have a hole here
                baseHullClone.holeVertices = otherHullClone.vertices;
                return baseHullClone;
            }


            SimpleHull result = new SimpleHull();

            // Find a good starting Simplex from baseHull
            // A good starting simplex is one that is outside otherHull
            // Such a simplex must exist, otherwise the result will be empty
            Vertex baseStartVertex = null;
            {
                int iBase;
                for (iBase = 0; iBase < baseHullClone.vertices.Count; iBase++)
                {
                    int iBaseNext = (iBase + 1) % baseHullClone.vertices.Count;
                    Vertex center = new Vertex((baseHullClone.vertices[iBase] + baseHullClone.vertices[iBaseNext]) / 2.0f);
                    bool isOutside = !otherHullClone.IsPointIn(center);
                    if (isOutside)
                    {
                        baseStartVertex = baseHullClone.vertices[iBaseNext];
                        break;
                    }
                }
            }


            if (baseStartVertex == null) // i.e. no simplex fulfilled the "outside" condition. 
            // In otherwords, subtractHull completely embraces baseHull
            {
                return result;
            }

            // The simplex that *starts* with baseStartVertex is outside the cutting hull,
            // so we can start our walk with the next vertex without loosing a branch
            Vertex V1 = baseStartVertex;
            bool onBase = true;

            // And here is how we do the magic :-)
            // Start on the base hull.
            // Walk the vertices in the positive direction
            // For each vertex check, whether it is a vertex shared with the other hull
            // if this is the case, switch over to walking the other vertex list.
            // Note: The other hull *must* go backwards to our starting point (via several orther vertices)
            // Thus it is important that the cutting hull has the inverse directional sense than the
            // base hull!!!!!!!!! (means if base goes CW around it's center cutting hull must go CCW)

            bool done = false;
            while (!done)
            {
                result.AddVertex(V1);
                Vertex nextVertex = null;
                if (onBase)
                {
                    nextVertex = otherHullClone.FindVertex(V1, 0.001f);
                }
                else
                {
                    nextVertex = baseHullClone.FindVertex(V1, 0.001f);
                }

                if (nextVertex != null) // A node that represents an intersection
                {
                    V1 = nextVertex;    // Needed to find the next vertex on the other hull
                    onBase = !onBase;
                }

                if (onBase)
                    V1 = baseHullClone.getNextVertex(V1);
                else
                    V1 = otherHullClone.getNextVertex(V1);

                if (V1 == baseStartVertex)
                    done = true;
            }

            MainLog.Instance.Debug("The resulting Hull is:\n{1}", 0, result.ToString());

            return result;

        }
    }
}