diff options
Diffstat (limited to 'OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet')
23 files changed, 5679 insertions, 0 deletions
diff --git a/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/CTri.cs b/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/CTri.cs new file mode 100644 index 0000000..4d84c44 --- /dev/null +++ b/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/CTri.cs | |||
@@ -0,0 +1,341 @@ | |||
1 | /* The MIT License | ||
2 | * | ||
3 | * Copyright (c) 2010 Intel Corporation. | ||
4 | * All rights reserved. | ||
5 | * | ||
6 | * Based on the convexdecomposition library from | ||
7 | * <http://codesuppository.googlecode.com> by John W. Ratcliff and Stan Melax. | ||
8 | * | ||
9 | * Permission is hereby granted, free of charge, to any person obtaining a copy | ||
10 | * of this software and associated documentation files (the "Software"), to deal | ||
11 | * in the Software without restriction, including without limitation the rights | ||
12 | * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell | ||
13 | * copies of the Software, and to permit persons to whom the Software is | ||
14 | * furnished to do so, subject to the following conditions: | ||
15 | * | ||
16 | * The above copyright notice and this permission notice shall be included in | ||
17 | * all copies or substantial portions of the Software. | ||
18 | * | ||
19 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR | ||
20 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, | ||
21 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE | ||
22 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER | ||
23 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, | ||
24 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN | ||
25 | * THE SOFTWARE. | ||
26 | */ | ||
27 | |||
28 | using System; | ||
29 | using System.Collections.Generic; | ||
30 | |||
31 | namespace OpenSim.Region.Physics.ConvexDecompositionDotNet | ||
32 | { | ||
33 | public class Wpoint | ||
34 | { | ||
35 | public float3 mPoint; | ||
36 | public float mWeight; | ||
37 | |||
38 | public Wpoint(float3 p, float w) | ||
39 | { | ||
40 | mPoint = p; | ||
41 | mWeight = w; | ||
42 | } | ||
43 | } | ||
44 | |||
45 | public class CTri | ||
46 | { | ||
47 | private const int WSCALE = 4; | ||
48 | |||
49 | public float3 mP1; | ||
50 | public float3 mP2; | ||
51 | public float3 mP3; | ||
52 | public float3 mNear1; | ||
53 | public float3 mNear2; | ||
54 | public float3 mNear3; | ||
55 | public float3 mNormal; | ||
56 | public float mPlaneD; | ||
57 | public float mConcavity; | ||
58 | public float mC1; | ||
59 | public float mC2; | ||
60 | public float mC3; | ||
61 | public int mI1; | ||
62 | public int mI2; | ||
63 | public int mI3; | ||
64 | public int mProcessed; // already been added... | ||
65 | |||
66 | public CTri(float3 p1, float3 p2, float3 p3, int i1, int i2, int i3) | ||
67 | { | ||
68 | mProcessed = 0; | ||
69 | mI1 = i1; | ||
70 | mI2 = i2; | ||
71 | mI3 = i3; | ||
72 | |||
73 | mP1 = new float3(p1); | ||
74 | mP2 = new float3(p2); | ||
75 | mP3 = new float3(p3); | ||
76 | |||
77 | mNear1 = new float3(); | ||
78 | mNear2 = new float3(); | ||
79 | mNear3 = new float3(); | ||
80 | |||
81 | mNormal = new float3(); | ||
82 | mPlaneD = mNormal.ComputePlane(mP1, mP2, mP3); | ||
83 | } | ||
84 | |||
85 | public float Facing(CTri t) | ||
86 | { | ||
87 | return float3.dot(mNormal, t.mNormal); | ||
88 | } | ||
89 | |||
90 | public bool clip(float3 start, ref float3 end) | ||
91 | { | ||
92 | float3 sect = new float3(); | ||
93 | bool hit = lineIntersectsTriangle(start, end, mP1, mP2, mP3, ref sect); | ||
94 | |||
95 | if (hit) | ||
96 | end = sect; | ||
97 | return hit; | ||
98 | } | ||
99 | |||
100 | public bool Concave(float3 p, ref float distance, ref float3 n) | ||
101 | { | ||
102 | n.NearestPointInTriangle(p, mP1, mP2, mP3); | ||
103 | distance = p.Distance(n); | ||
104 | return true; | ||
105 | } | ||
106 | |||
107 | public void addTri(int[] indices, int i1, int i2, int i3, ref int tcount) | ||
108 | { | ||
109 | indices[tcount * 3 + 0] = i1; | ||
110 | indices[tcount * 3 + 1] = i2; | ||
111 | indices[tcount * 3 + 2] = i3; | ||
112 | tcount++; | ||
113 | } | ||
114 | |||
115 | public float getVolume() | ||
116 | { | ||
117 | int[] indices = new int[8 * 3]; | ||
118 | |||
119 | int tcount = 0; | ||
120 | |||
121 | addTri(indices, 0, 1, 2, ref tcount); | ||
122 | addTri(indices, 3, 4, 5, ref tcount); | ||
123 | |||
124 | addTri(indices, 0, 3, 4, ref tcount); | ||
125 | addTri(indices, 0, 4, 1, ref tcount); | ||
126 | |||
127 | addTri(indices, 1, 4, 5, ref tcount); | ||
128 | addTri(indices, 1, 5, 2, ref tcount); | ||
129 | |||
130 | addTri(indices, 0, 3, 5, ref tcount); | ||
131 | addTri(indices, 0, 5, 2, ref tcount); | ||
132 | |||
133 | List<float3> vertices = new List<float3> { mP1, mP2, mP3, mNear1, mNear2, mNear3 }; | ||
134 | List<int> indexList = new List<int>(indices); | ||
135 | |||
136 | float v = Concavity.computeMeshVolume(vertices, indexList); | ||
137 | return v; | ||
138 | } | ||
139 | |||
140 | public float raySect(float3 p, float3 dir, ref float3 sect) | ||
141 | { | ||
142 | float4 plane = new float4(); | ||
143 | |||
144 | plane.x = mNormal.x; | ||
145 | plane.y = mNormal.y; | ||
146 | plane.z = mNormal.z; | ||
147 | plane.w = mPlaneD; | ||
148 | |||
149 | float3 dest = p + dir * 100000f; | ||
150 | |||
151 | intersect(p, dest, ref sect, plane); | ||
152 | |||
153 | return sect.Distance(p); // return the intersection distance | ||
154 | } | ||
155 | |||
156 | public float planeDistance(float3 p) | ||
157 | { | ||
158 | float4 plane = new float4(); | ||
159 | |||
160 | plane.x = mNormal.x; | ||
161 | plane.y = mNormal.y; | ||
162 | plane.z = mNormal.z; | ||
163 | plane.w = mPlaneD; | ||
164 | |||
165 | return DistToPt(p, plane); | ||
166 | } | ||
167 | |||
168 | public bool samePlane(CTri t) | ||
169 | { | ||
170 | const float THRESH = 0.001f; | ||
171 | float dd = Math.Abs(t.mPlaneD - mPlaneD); | ||
172 | if (dd > THRESH) | ||
173 | return false; | ||
174 | dd = Math.Abs(t.mNormal.x - mNormal.x); | ||
175 | if (dd > THRESH) | ||
176 | return false; | ||
177 | dd = Math.Abs(t.mNormal.y - mNormal.y); | ||
178 | if (dd > THRESH) | ||
179 | return false; | ||
180 | dd = Math.Abs(t.mNormal.z - mNormal.z); | ||
181 | if (dd > THRESH) | ||
182 | return false; | ||
183 | return true; | ||
184 | } | ||
185 | |||
186 | public bool hasIndex(int i) | ||
187 | { | ||
188 | if (i == mI1 || i == mI2 || i == mI3) | ||
189 | return true; | ||
190 | return false; | ||
191 | } | ||
192 | |||
193 | public bool sharesEdge(CTri t) | ||
194 | { | ||
195 | bool ret = false; | ||
196 | uint count = 0; | ||
197 | |||
198 | if (t.hasIndex(mI1)) | ||
199 | count++; | ||
200 | if (t.hasIndex(mI2)) | ||
201 | count++; | ||
202 | if (t.hasIndex(mI3)) | ||
203 | count++; | ||
204 | |||
205 | if (count >= 2) | ||
206 | ret = true; | ||
207 | |||
208 | return ret; | ||
209 | } | ||
210 | |||
211 | public float area() | ||
212 | { | ||
213 | float a = mConcavity * mP1.Area(mP2, mP3); | ||
214 | return a; | ||
215 | } | ||
216 | |||
217 | public void addWeighted(List<Wpoint> list) | ||
218 | { | ||
219 | Wpoint p1 = new Wpoint(mP1, mC1); | ||
220 | Wpoint p2 = new Wpoint(mP2, mC2); | ||
221 | Wpoint p3 = new Wpoint(mP3, mC3); | ||
222 | |||
223 | float3 d1 = mNear1 - mP1; | ||
224 | float3 d2 = mNear2 - mP2; | ||
225 | float3 d3 = mNear3 - mP3; | ||
226 | |||
227 | d1 *= WSCALE; | ||
228 | d2 *= WSCALE; | ||
229 | d3 *= WSCALE; | ||
230 | |||
231 | d1 = d1 + mP1; | ||
232 | d2 = d2 + mP2; | ||
233 | d3 = d3 + mP3; | ||
234 | |||
235 | Wpoint p4 = new Wpoint(d1, mC1); | ||
236 | Wpoint p5 = new Wpoint(d2, mC2); | ||
237 | Wpoint p6 = new Wpoint(d3, mC3); | ||
238 | |||
239 | list.Add(p1); | ||
240 | list.Add(p2); | ||
241 | list.Add(p3); | ||
242 | |||
243 | list.Add(p4); | ||
244 | list.Add(p5); | ||
245 | list.Add(p6); | ||
246 | } | ||
247 | |||
248 | private static float DistToPt(float3 p, float4 plane) | ||
249 | { | ||
250 | float x = p.x; | ||
251 | float y = p.y; | ||
252 | float z = p.z; | ||
253 | float d = x*plane.x + y*plane.y + z*plane.z + plane.w; | ||
254 | return d; | ||
255 | } | ||
256 | |||
257 | private static void intersect(float3 p1, float3 p2, ref float3 split, float4 plane) | ||
258 | { | ||
259 | float dp1 = DistToPt(p1, plane); | ||
260 | |||
261 | float3 dir = new float3(); | ||
262 | dir.x = p2[0] - p1[0]; | ||
263 | dir.y = p2[1] - p1[1]; | ||
264 | dir.z = p2[2] - p1[2]; | ||
265 | |||
266 | float dot1 = dir[0] * plane[0] + dir[1] * plane[1] + dir[2] * plane[2]; | ||
267 | float dot2 = dp1 - plane[3]; | ||
268 | |||
269 | float t = -(plane[3] + dot2) / dot1; | ||
270 | |||
271 | split.x = (dir[0] * t) + p1[0]; | ||
272 | split.y = (dir[1] * t) + p1[1]; | ||
273 | split.z = (dir[2] * t) + p1[2]; | ||
274 | } | ||
275 | |||
276 | private static bool rayIntersectsTriangle(float3 p, float3 d, float3 v0, float3 v1, float3 v2, out float t) | ||
277 | { | ||
278 | t = 0f; | ||
279 | |||
280 | float3 e1, e2, h, s, q; | ||
281 | float a, f, u, v; | ||
282 | |||
283 | e1 = v1 - v0; | ||
284 | e2 = v2 - v0; | ||
285 | h = float3.cross(d, e2); | ||
286 | a = float3.dot(e1, h); | ||
287 | |||
288 | if (a > -0.00001f && a < 0.00001f) | ||
289 | return false; | ||
290 | |||
291 | f = 1f / a; | ||
292 | s = p - v0; | ||
293 | u = f * float3.dot(s, h); | ||
294 | |||
295 | if (u < 0.0f || u > 1.0f) | ||
296 | return false; | ||
297 | |||
298 | q = float3.cross(s, e1); | ||
299 | v = f * float3.dot(d, q); | ||
300 | if (v < 0.0f || u + v > 1.0f) | ||
301 | return false; | ||
302 | |||
303 | // at this stage we can compute t to find out where | ||
304 | // the intersection point is on the line | ||
305 | t = f * float3.dot(e2, q); | ||
306 | if (t > 0f) // ray intersection | ||
307 | return true; | ||
308 | else // this means that there is a line intersection but not a ray intersection | ||
309 | return false; | ||
310 | } | ||
311 | |||
312 | private static bool lineIntersectsTriangle(float3 rayStart, float3 rayEnd, float3 p1, float3 p2, float3 p3, ref float3 sect) | ||
313 | { | ||
314 | float3 dir = rayEnd - rayStart; | ||
315 | |||
316 | float d = (float)Math.Sqrt(dir[0] * dir[0] + dir[1] * dir[1] + dir[2] * dir[2]); | ||
317 | float r = 1.0f / d; | ||
318 | |||
319 | dir *= r; | ||
320 | |||
321 | float t; | ||
322 | bool ret = rayIntersectsTriangle(rayStart, dir, p1, p2, p3, out t); | ||
323 | |||
324 | if (ret) | ||
325 | { | ||
326 | if (t > d) | ||
327 | { | ||
328 | sect.x = rayStart.x + dir.x * t; | ||
329 | sect.y = rayStart.y + dir.y * t; | ||
330 | sect.z = rayStart.z + dir.z * t; | ||
331 | } | ||
332 | else | ||
333 | { | ||
334 | ret = false; | ||
335 | } | ||
336 | } | ||
337 | |||
338 | return ret; | ||
339 | } | ||
340 | } | ||
341 | } | ||
diff --git a/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/Concavity.cs b/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/Concavity.cs new file mode 100644 index 0000000..cc6383a --- /dev/null +++ b/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/Concavity.cs | |||
@@ -0,0 +1,233 @@ | |||
1 | /* The MIT License | ||
2 | * | ||
3 | * Copyright (c) 2010 Intel Corporation. | ||
4 | * All rights reserved. | ||
5 | * | ||
6 | * Based on the convexdecomposition library from | ||
7 | * <http://codesuppository.googlecode.com> by John W. Ratcliff and Stan Melax. | ||
8 | * | ||
9 | * Permission is hereby granted, free of charge, to any person obtaining a copy | ||
10 | * of this software and associated documentation files (the "Software"), to deal | ||
11 | * in the Software without restriction, including without limitation the rights | ||
12 | * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell | ||
13 | * copies of the Software, and to permit persons to whom the Software is | ||
14 | * furnished to do so, subject to the following conditions: | ||
15 | * | ||
16 | * The above copyright notice and this permission notice shall be included in | ||
17 | * all copies or substantial portions of the Software. | ||
18 | * | ||
19 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR | ||
20 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, | ||
21 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE | ||
22 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER | ||
23 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, | ||
24 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN | ||
25 | * THE SOFTWARE. | ||
26 | */ | ||
27 | |||
28 | using System; | ||
29 | using System.Collections.Generic; | ||
30 | using System.Text; | ||
31 | |||
32 | namespace OpenSim.Region.Physics.ConvexDecompositionDotNet | ||
33 | { | ||
34 | public static class Concavity | ||
35 | { | ||
36 | // compute's how 'concave' this object is and returns the total volume of the | ||
37 | // convex hull as well as the volume of the 'concavity' which was found. | ||
38 | public static float computeConcavity(List<float3> vertices, List<int> indices, ref float4 plane, ref float volume) | ||
39 | { | ||
40 | float cret = 0f; | ||
41 | volume = 1f; | ||
42 | |||
43 | HullResult result = new HullResult(); | ||
44 | HullDesc desc = new HullDesc(); | ||
45 | |||
46 | desc.MaxFaces = 256; | ||
47 | desc.MaxVertices = 256; | ||
48 | desc.SetHullFlag(HullFlag.QF_TRIANGLES); | ||
49 | desc.Vertices = vertices; | ||
50 | |||
51 | HullError ret = HullUtils.CreateConvexHull(desc, ref result); | ||
52 | |||
53 | if (ret == HullError.QE_OK) | ||
54 | { | ||
55 | volume = computeMeshVolume2(result.OutputVertices, result.Indices); | ||
56 | |||
57 | // ok..now..for each triangle on the original mesh.. | ||
58 | // we extrude the points to the nearest point on the hull. | ||
59 | List<CTri> tris = new List<CTri>(); | ||
60 | |||
61 | for (int i = 0; i < result.Indices.Count / 3; i++) | ||
62 | { | ||
63 | int i1 = result.Indices[i * 3 + 0]; | ||
64 | int i2 = result.Indices[i * 3 + 1]; | ||
65 | int i3 = result.Indices[i * 3 + 2]; | ||
66 | |||
67 | float3 p1 = result.OutputVertices[i1]; | ||
68 | float3 p2 = result.OutputVertices[i2]; | ||
69 | float3 p3 = result.OutputVertices[i3]; | ||
70 | |||
71 | CTri t = new CTri(p1, p2, p3, i1, i2, i3); | ||
72 | tris.Add(t); | ||
73 | } | ||
74 | |||
75 | // we have not pre-computed the plane equation for each triangle in the convex hull.. | ||
76 | float totalVolume = 0; | ||
77 | |||
78 | List<CTri> ftris = new List<CTri>(); // 'feature' triangles. | ||
79 | List<CTri> input_mesh = new List<CTri>(); | ||
80 | |||
81 | for (int i = 0; i < indices.Count / 3; i++) | ||
82 | { | ||
83 | int i1 = indices[i * 3 + 0]; | ||
84 | int i2 = indices[i * 3 + 1]; | ||
85 | int i3 = indices[i * 3 + 2]; | ||
86 | |||
87 | float3 p1 = vertices[i1]; | ||
88 | float3 p2 = vertices[i2]; | ||
89 | float3 p3 = vertices[i3]; | ||
90 | |||
91 | CTri t = new CTri(p1, p2, p3, i1, i2, i3); | ||
92 | input_mesh.Add(t); | ||
93 | } | ||
94 | |||
95 | for (int i = 0; i < indices.Count / 3; i++) | ||
96 | { | ||
97 | int i1 = indices[i * 3 + 0]; | ||
98 | int i2 = indices[i * 3 + 1]; | ||
99 | int i3 = indices[i * 3 + 2]; | ||
100 | |||
101 | float3 p1 = vertices[i1]; | ||
102 | float3 p2 = vertices[i2]; | ||
103 | float3 p3 = vertices[i3]; | ||
104 | |||
105 | CTri t = new CTri(p1, p2, p3, i1, i2, i3); | ||
106 | |||
107 | featureMatch(t, tris, input_mesh); | ||
108 | |||
109 | if (t.mConcavity > 0.05f) | ||
110 | { | ||
111 | float v = t.getVolume(); | ||
112 | totalVolume += v; | ||
113 | ftris.Add(t); | ||
114 | } | ||
115 | } | ||
116 | |||
117 | SplitPlane.computeSplitPlane(vertices, indices, ref plane); | ||
118 | cret = totalVolume; | ||
119 | } | ||
120 | |||
121 | return cret; | ||
122 | } | ||
123 | |||
124 | public static bool featureMatch(CTri m, List<CTri> tris, List<CTri> input_mesh) | ||
125 | { | ||
126 | bool ret = false; | ||
127 | float neardot = 0.707f; | ||
128 | m.mConcavity = 0; | ||
129 | |||
130 | for (int i = 0; i < tris.Count; i++) | ||
131 | { | ||
132 | CTri t = tris[i]; | ||
133 | |||
134 | if (t.samePlane(m)) | ||
135 | { | ||
136 | ret = false; | ||
137 | break; | ||
138 | } | ||
139 | |||
140 | float dot = float3.dot(t.mNormal, m.mNormal); | ||
141 | |||
142 | if (dot > neardot) | ||
143 | { | ||
144 | float d1 = t.planeDistance(m.mP1); | ||
145 | float d2 = t.planeDistance(m.mP2); | ||
146 | float d3 = t.planeDistance(m.mP3); | ||
147 | |||
148 | if (d1 > 0.001f || d2 > 0.001f || d3 > 0.001f) // can't be near coplaner! | ||
149 | { | ||
150 | neardot = dot; | ||
151 | |||
152 | t.raySect(m.mP1, m.mNormal, ref m.mNear1); | ||
153 | t.raySect(m.mP2, m.mNormal, ref m.mNear2); | ||
154 | t.raySect(m.mP3, m.mNormal, ref m.mNear3); | ||
155 | |||
156 | ret = true; | ||
157 | } | ||
158 | } | ||
159 | } | ||
160 | |||
161 | if (ret) | ||
162 | { | ||
163 | m.mC1 = m.mP1.Distance(m.mNear1); | ||
164 | m.mC2 = m.mP2.Distance(m.mNear2); | ||
165 | m.mC3 = m.mP3.Distance(m.mNear3); | ||
166 | |||
167 | m.mConcavity = m.mC1; | ||
168 | |||
169 | if (m.mC2 > m.mConcavity) | ||
170 | m.mConcavity = m.mC2; | ||
171 | if (m.mC3 > m.mConcavity) | ||
172 | m.mConcavity = m.mC3; | ||
173 | } | ||
174 | |||
175 | return ret; | ||
176 | } | ||
177 | |||
178 | private static float det(float3 p1, float3 p2, float3 p3) | ||
179 | { | ||
180 | return p1.x * p2.y * p3.z + p2.x * p3.y * p1.z + p3.x * p1.y * p2.z - p1.x * p3.y * p2.z - p2.x * p1.y * p3.z - p3.x * p2.y * p1.z; | ||
181 | } | ||
182 | |||
183 | public static float computeMeshVolume(List<float3> vertices, List<int> indices) | ||
184 | { | ||
185 | float volume = 0f; | ||
186 | |||
187 | for (int i = 0; i < indices.Count / 3; i++) | ||
188 | { | ||
189 | float3 p1 = vertices[indices[i * 3 + 0]]; | ||
190 | float3 p2 = vertices[indices[i * 3 + 1]]; | ||
191 | float3 p3 = vertices[indices[i * 3 + 2]]; | ||
192 | |||
193 | volume += det(p1, p2, p3); // compute the volume of the tetrahedran relative to the origin. | ||
194 | } | ||
195 | |||
196 | volume *= (1.0f / 6.0f); | ||
197 | if (volume < 0f) | ||
198 | return -volume; | ||
199 | return volume; | ||
200 | } | ||
201 | |||
202 | public static float computeMeshVolume2(List<float3> vertices, List<int> indices) | ||
203 | { | ||
204 | float volume = 0f; | ||
205 | |||
206 | float3 p0 = vertices[0]; | ||
207 | for (int i = 0; i < indices.Count / 3; i++) | ||
208 | { | ||
209 | float3 p1 = vertices[indices[i * 3 + 0]]; | ||
210 | float3 p2 = vertices[indices[i * 3 + 1]]; | ||
211 | float3 p3 = vertices[indices[i * 3 + 2]]; | ||
212 | |||
213 | volume += tetVolume(p0, p1, p2, p3); // compute the volume of the tetrahedron relative to the root vertice | ||
214 | } | ||
215 | |||
216 | return volume * (1.0f / 6.0f); | ||
217 | } | ||
218 | |||
219 | private static float tetVolume(float3 p0, float3 p1, float3 p2, float3 p3) | ||
220 | { | ||
221 | float3 a = p1 - p0; | ||
222 | float3 b = p2 - p0; | ||
223 | float3 c = p3 - p0; | ||
224 | |||
225 | float3 cross = float3.cross(b, c); | ||
226 | float volume = float3.dot(a, cross); | ||
227 | |||
228 | if (volume < 0f) | ||
229 | return -volume; | ||
230 | return volume; | ||
231 | } | ||
232 | } | ||
233 | } | ||
diff --git a/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/ConvexBuilder.cs b/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/ConvexBuilder.cs new file mode 100644 index 0000000..dfaede1 --- /dev/null +++ b/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/ConvexBuilder.cs | |||
@@ -0,0 +1,411 @@ | |||
1 | /* The MIT License | ||
2 | * | ||
3 | * Copyright (c) 2010 Intel Corporation. | ||
4 | * All rights reserved. | ||
5 | * | ||
6 | * Based on the convexdecomposition library from | ||
7 | * <http://codesuppository.googlecode.com> by John W. Ratcliff and Stan Melax. | ||
8 | * | ||
9 | * Permission is hereby granted, free of charge, to any person obtaining a copy | ||
10 | * of this software and associated documentation files (the "Software"), to deal | ||
11 | * in the Software without restriction, including without limitation the rights | ||
12 | * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell | ||
13 | * copies of the Software, and to permit persons to whom the Software is | ||
14 | * furnished to do so, subject to the following conditions: | ||
15 | * | ||
16 | * The above copyright notice and this permission notice shall be included in | ||
17 | * all copies or substantial portions of the Software. | ||
18 | * | ||
19 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR | ||
20 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, | ||
21 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE | ||
22 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER | ||
23 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, | ||
24 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN | ||
25 | * THE SOFTWARE. | ||
26 | */ | ||
27 | |||
28 | using System; | ||
29 | using System.Collections.Generic; | ||
30 | using System.Diagnostics; | ||
31 | |||
32 | namespace OpenSim.Region.Physics.ConvexDecompositionDotNet | ||
33 | { | ||
34 | public class DecompDesc | ||
35 | { | ||
36 | public List<float3> mVertices; | ||
37 | public List<int> mIndices; | ||
38 | |||
39 | // options | ||
40 | public uint mDepth; // depth to split, a maximum of 10, generally not over 7. | ||
41 | public float mCpercent; // the concavity threshold percentage. 0=20 is reasonable. | ||
42 | public float mPpercent; // the percentage volume conservation threshold to collapse hulls. 0-30 is reasonable. | ||
43 | |||
44 | // hull output limits. | ||
45 | public uint mMaxVertices; // maximum number of vertices in the output hull. Recommended 32 or less. | ||
46 | public float mSkinWidth; // a skin width to apply to the output hulls. | ||
47 | |||
48 | public ConvexDecompositionCallback mCallback; // the interface to receive back the results. | ||
49 | |||
50 | public DecompDesc() | ||
51 | { | ||
52 | mDepth = 5; | ||
53 | mCpercent = 5; | ||
54 | mPpercent = 5; | ||
55 | mMaxVertices = 32; | ||
56 | } | ||
57 | } | ||
58 | |||
59 | public class CHull | ||
60 | { | ||
61 | public float[] mMin = new float[3]; | ||
62 | public float[] mMax = new float[3]; | ||
63 | public float mVolume; | ||
64 | public float mDiagonal; | ||
65 | public ConvexResult mResult; | ||
66 | |||
67 | public CHull(ConvexResult result) | ||
68 | { | ||
69 | mResult = new ConvexResult(result); | ||
70 | mVolume = Concavity.computeMeshVolume(result.HullVertices, result.HullIndices); | ||
71 | |||
72 | mDiagonal = getBoundingRegion(result.HullVertices, mMin, mMax); | ||
73 | |||
74 | float dx = mMax[0] - mMin[0]; | ||
75 | float dy = mMax[1] - mMin[1]; | ||
76 | float dz = mMax[2] - mMin[2]; | ||
77 | |||
78 | dx *= 0.1f; // inflate 1/10th on each edge | ||
79 | dy *= 0.1f; // inflate 1/10th on each edge | ||
80 | dz *= 0.1f; // inflate 1/10th on each edge | ||
81 | |||
82 | mMin[0] -= dx; | ||
83 | mMin[1] -= dy; | ||
84 | mMin[2] -= dz; | ||
85 | |||
86 | mMax[0] += dx; | ||
87 | mMax[1] += dy; | ||
88 | mMax[2] += dz; | ||
89 | } | ||
90 | |||
91 | public void Dispose() | ||
92 | { | ||
93 | mResult = null; | ||
94 | } | ||
95 | |||
96 | public bool overlap(CHull h) | ||
97 | { | ||
98 | return overlapAABB(mMin, mMax, h.mMin, h.mMax); | ||
99 | } | ||
100 | |||
101 | // returns the d1Giagonal distance | ||
102 | private static float getBoundingRegion(List<float3> points, float[] bmin, float[] bmax) | ||
103 | { | ||
104 | float3 first = points[0]; | ||
105 | |||
106 | bmin[0] = first.x; | ||
107 | bmin[1] = first.y; | ||
108 | bmin[2] = first.z; | ||
109 | |||
110 | bmax[0] = first.x; | ||
111 | bmax[1] = first.y; | ||
112 | bmax[2] = first.z; | ||
113 | |||
114 | for (int i = 1; i < points.Count; i++) | ||
115 | { | ||
116 | float3 p = points[i]; | ||
117 | |||
118 | if (p[0] < bmin[0]) bmin[0] = p[0]; | ||
119 | if (p[1] < bmin[1]) bmin[1] = p[1]; | ||
120 | if (p[2] < bmin[2]) bmin[2] = p[2]; | ||
121 | |||
122 | if (p[0] > bmax[0]) bmax[0] = p[0]; | ||
123 | if (p[1] > bmax[1]) bmax[1] = p[1]; | ||
124 | if (p[2] > bmax[2]) bmax[2] = p[2]; | ||
125 | } | ||
126 | |||
127 | float dx = bmax[0] - bmin[0]; | ||
128 | float dy = bmax[1] - bmin[1]; | ||
129 | float dz = bmax[2] - bmin[2]; | ||
130 | |||
131 | return (float)Math.Sqrt(dx * dx + dy * dy + dz * dz); | ||
132 | } | ||
133 | |||
134 | // return true if the two AABB's overlap. | ||
135 | private static bool overlapAABB(float[] bmin1, float[] bmax1, float[] bmin2, float[] bmax2) | ||
136 | { | ||
137 | if (bmax2[0] < bmin1[0]) return false; // if the maximum is less than our minimum on any axis | ||
138 | if (bmax2[1] < bmin1[1]) return false; | ||
139 | if (bmax2[2] < bmin1[2]) return false; | ||
140 | |||
141 | if (bmin2[0] > bmax1[0]) return false; // if the minimum is greater than our maximum on any axis | ||
142 | if (bmin2[1] > bmax1[1]) return false; // if the minimum is greater than our maximum on any axis | ||
143 | if (bmin2[2] > bmax1[2]) return false; // if the minimum is greater than our maximum on any axis | ||
144 | |||
145 | return true; // the extents overlap | ||
146 | } | ||
147 | } | ||
148 | |||
149 | public class ConvexBuilder | ||
150 | { | ||
151 | public List<CHull> mChulls = new List<CHull>(); | ||
152 | private ConvexDecompositionCallback mCallback; | ||
153 | |||
154 | private int MAXDEPTH = 8; | ||
155 | private float CONCAVE_PERCENT = 1f; | ||
156 | private float MERGE_PERCENT = 2f; | ||
157 | |||
158 | public ConvexBuilder(ConvexDecompositionCallback callback) | ||
159 | { | ||
160 | mCallback = callback; | ||
161 | } | ||
162 | |||
163 | public void Dispose() | ||
164 | { | ||
165 | int i; | ||
166 | for (i = 0; i < mChulls.Count; i++) | ||
167 | { | ||
168 | CHull cr = mChulls[i]; | ||
169 | cr.Dispose(); | ||
170 | } | ||
171 | } | ||
172 | |||
173 | public bool isDuplicate(uint i1, uint i2, uint i3, uint ci1, uint ci2, uint ci3) | ||
174 | { | ||
175 | uint dcount = 0; | ||
176 | |||
177 | Debug.Assert(i1 != i2 && i1 != i3 && i2 != i3); | ||
178 | Debug.Assert(ci1 != ci2 && ci1 != ci3 && ci2 != ci3); | ||
179 | |||
180 | if (i1 == ci1 || i1 == ci2 || i1 == ci3) | ||
181 | dcount++; | ||
182 | if (i2 == ci1 || i2 == ci2 || i2 == ci3) | ||
183 | dcount++; | ||
184 | if (i3 == ci1 || i3 == ci2 || i3 == ci3) | ||
185 | dcount++; | ||
186 | |||
187 | return dcount == 3; | ||
188 | } | ||
189 | |||
190 | public void getMesh(ConvexResult cr, VertexPool vc, List<int> indices) | ||
191 | { | ||
192 | List<int> src = cr.HullIndices; | ||
193 | |||
194 | for (int i = 0; i < src.Count / 3; i++) | ||
195 | { | ||
196 | int i1 = src[i * 3 + 0]; | ||
197 | int i2 = src[i * 3 + 1]; | ||
198 | int i3 = src[i * 3 + 2]; | ||
199 | |||
200 | float3 p1 = cr.HullVertices[i1]; | ||
201 | float3 p2 = cr.HullVertices[i2]; | ||
202 | float3 p3 = cr.HullVertices[i3]; | ||
203 | |||
204 | i1 = vc.getIndex(p1); | ||
205 | i2 = vc.getIndex(p2); | ||
206 | i3 = vc.getIndex(p3); | ||
207 | } | ||
208 | } | ||
209 | |||
210 | public CHull canMerge(CHull a, CHull b) | ||
211 | { | ||
212 | if (!a.overlap(b)) // if their AABB's (with a little slop) don't overlap, then return. | ||
213 | return null; | ||
214 | |||
215 | CHull ret = null; | ||
216 | |||
217 | // ok..we are going to combine both meshes into a single mesh | ||
218 | // and then we are going to compute the concavity... | ||
219 | |||
220 | VertexPool vc = new VertexPool(); | ||
221 | |||
222 | List<int> indices = new List<int>(); | ||
223 | |||
224 | getMesh(a.mResult, vc, indices); | ||
225 | getMesh(b.mResult, vc, indices); | ||
226 | |||
227 | int vcount = vc.GetSize(); | ||
228 | List<float3> vertices = vc.GetVertices(); | ||
229 | int tcount = indices.Count / 3; | ||
230 | |||
231 | //don't do anything if hull is empty | ||
232 | if (tcount == 0) | ||
233 | { | ||
234 | vc.Clear(); | ||
235 | return null; | ||
236 | } | ||
237 | |||
238 | HullResult hresult = new HullResult(); | ||
239 | HullDesc desc = new HullDesc(); | ||
240 | |||
241 | desc.SetHullFlag(HullFlag.QF_TRIANGLES); | ||
242 | desc.Vertices = vertices; | ||
243 | |||
244 | HullError hret = HullUtils.CreateConvexHull(desc, ref hresult); | ||
245 | |||
246 | if (hret == HullError.QE_OK) | ||
247 | { | ||
248 | float combineVolume = Concavity.computeMeshVolume(hresult.OutputVertices, hresult.Indices); | ||
249 | float sumVolume = a.mVolume + b.mVolume; | ||
250 | |||
251 | float percent = (sumVolume * 100) / combineVolume; | ||
252 | if (percent >= (100.0f - MERGE_PERCENT)) | ||
253 | { | ||
254 | ConvexResult cr = new ConvexResult(hresult.OutputVertices, hresult.Indices); | ||
255 | ret = new CHull(cr); | ||
256 | } | ||
257 | } | ||
258 | |||
259 | vc.Clear(); | ||
260 | return ret; | ||
261 | } | ||
262 | |||
263 | public bool combineHulls() | ||
264 | { | ||
265 | bool combine = false; | ||
266 | |||
267 | sortChulls(mChulls); // sort the convex hulls, largest volume to least... | ||
268 | |||
269 | List<CHull> output = new List<CHull>(); // the output hulls... | ||
270 | |||
271 | int i; | ||
272 | for (i = 0; i < mChulls.Count && !combine; ++i) | ||
273 | { | ||
274 | CHull cr = mChulls[i]; | ||
275 | |||
276 | int j; | ||
277 | for (j = 0; j < mChulls.Count; j++) | ||
278 | { | ||
279 | CHull match = mChulls[j]; | ||
280 | |||
281 | if (cr != match) // don't try to merge a hull with itself, that be stoopid | ||
282 | { | ||
283 | |||
284 | CHull merge = canMerge(cr, match); // if we can merge these two.... | ||
285 | |||
286 | if (merge != null) | ||
287 | { | ||
288 | output.Add(merge); | ||
289 | |||
290 | ++i; | ||
291 | while (i != mChulls.Count) | ||
292 | { | ||
293 | CHull cr2 = mChulls[i]; | ||
294 | if (cr2 != match) | ||
295 | { | ||
296 | output.Add(cr2); | ||
297 | } | ||
298 | i++; | ||
299 | } | ||
300 | |||
301 | cr.Dispose(); | ||
302 | match.Dispose(); | ||
303 | combine = true; | ||
304 | break; | ||
305 | } | ||
306 | } | ||
307 | } | ||
308 | |||
309 | if (combine) | ||
310 | { | ||
311 | break; | ||
312 | } | ||
313 | else | ||
314 | { | ||
315 | output.Add(cr); | ||
316 | } | ||
317 | } | ||
318 | |||
319 | if (combine) | ||
320 | { | ||
321 | mChulls.Clear(); | ||
322 | mChulls = output; | ||
323 | output.Clear(); | ||
324 | } | ||
325 | |||
326 | return combine; | ||
327 | } | ||
328 | |||
329 | public int process(DecompDesc desc) | ||
330 | { | ||
331 | int ret = 0; | ||
332 | |||
333 | MAXDEPTH = (int)desc.mDepth; | ||
334 | CONCAVE_PERCENT = desc.mCpercent; | ||
335 | MERGE_PERCENT = desc.mPpercent; | ||
336 | |||
337 | ConvexDecomposition.calcConvexDecomposition(desc.mVertices, desc.mIndices, ConvexDecompResult, 0f, 0, MAXDEPTH, CONCAVE_PERCENT, MERGE_PERCENT); | ||
338 | |||
339 | while (combineHulls()) // keep combinging hulls until I can't combine any more... | ||
340 | ; | ||
341 | |||
342 | int i; | ||
343 | for (i = 0; i < mChulls.Count; i++) | ||
344 | { | ||
345 | CHull cr = mChulls[i]; | ||
346 | |||
347 | // before we hand it back to the application, we need to regenerate the hull based on the | ||
348 | // limits given by the user. | ||
349 | |||
350 | ConvexResult c = cr.mResult; // the high resolution hull... | ||
351 | |||
352 | HullResult result = new HullResult(); | ||
353 | HullDesc hdesc = new HullDesc(); | ||
354 | |||
355 | hdesc.SetHullFlag(HullFlag.QF_TRIANGLES); | ||
356 | |||
357 | hdesc.Vertices = c.HullVertices; | ||
358 | hdesc.MaxVertices = desc.mMaxVertices; // maximum number of vertices allowed in the output | ||
359 | |||
360 | if (desc.mSkinWidth != 0f) | ||
361 | { | ||
362 | hdesc.SkinWidth = desc.mSkinWidth; | ||
363 | hdesc.SetHullFlag(HullFlag.QF_SKIN_WIDTH); // do skin width computation. | ||
364 | } | ||
365 | |||
366 | HullError ret2 = HullUtils.CreateConvexHull(hdesc, ref result); | ||
367 | |||
368 | if (ret2 == HullError.QE_OK) | ||
369 | { | ||
370 | ConvexResult r = new ConvexResult(result.OutputVertices, result.Indices); | ||
371 | |||
372 | r.mHullVolume = Concavity.computeMeshVolume(result.OutputVertices, result.Indices); // the volume of the hull. | ||
373 | |||
374 | // compute the best fit OBB | ||
375 | //computeBestFitOBB(result.mNumOutputVertices, result.mOutputVertices, sizeof(float) * 3, r.mOBBSides, r.mOBBTransform); | ||
376 | |||
377 | //r.mOBBVolume = r.mOBBSides[0] * r.mOBBSides[1] * r.mOBBSides[2]; // compute the OBB volume. | ||
378 | |||
379 | //fm_getTranslation(r.mOBBTransform, r.mOBBCenter); // get the translation component of the 4x4 matrix. | ||
380 | |||
381 | //fm_matrixToQuat(r.mOBBTransform, r.mOBBOrientation); // extract the orientation as a quaternion. | ||
382 | |||
383 | //r.mSphereRadius = computeBoundingSphere(result.mNumOutputVertices, result.mOutputVertices, r.mSphereCenter); | ||
384 | //r.mSphereVolume = fm_sphereVolume(r.mSphereRadius); | ||
385 | |||
386 | mCallback(r); | ||
387 | } | ||
388 | |||
389 | result = null; | ||
390 | cr.Dispose(); | ||
391 | } | ||
392 | |||
393 | ret = mChulls.Count; | ||
394 | |||
395 | mChulls.Clear(); | ||
396 | |||
397 | return ret; | ||
398 | } | ||
399 | |||
400 | public void ConvexDecompResult(ConvexResult result) | ||
401 | { | ||
402 | CHull ch = new CHull(result); | ||
403 | mChulls.Add(ch); | ||
404 | } | ||
405 | |||
406 | public void sortChulls(List<CHull> hulls) | ||
407 | { | ||
408 | hulls.Sort(delegate(CHull a, CHull b) { return a.mVolume.CompareTo(b.mVolume); }); | ||
409 | } | ||
410 | } | ||
411 | } | ||
diff --git a/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/ConvexDecomposition.cs b/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/ConvexDecomposition.cs new file mode 100644 index 0000000..2e2bb70 --- /dev/null +++ b/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/ConvexDecomposition.cs | |||
@@ -0,0 +1,200 @@ | |||
1 | /* The MIT License | ||
2 | * | ||
3 | * Copyright (c) 2010 Intel Corporation. | ||
4 | * All rights reserved. | ||
5 | * | ||
6 | * Based on the convexdecomposition library from | ||
7 | * <http://codesuppository.googlecode.com> by John W. Ratcliff and Stan Melax. | ||
8 | * | ||
9 | * Permission is hereby granted, free of charge, to any person obtaining a copy | ||
10 | * of this software and associated documentation files (the "Software"), to deal | ||
11 | * in the Software without restriction, including without limitation the rights | ||
12 | * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell | ||
13 | * copies of the Software, and to permit persons to whom the Software is | ||
14 | * furnished to do so, subject to the following conditions: | ||
15 | * | ||
16 | * The above copyright notice and this permission notice shall be included in | ||
17 | * all copies or substantial portions of the Software. | ||
18 | * | ||
19 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR | ||
20 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, | ||
21 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE | ||
22 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER | ||
23 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, | ||
24 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN | ||
25 | * THE SOFTWARE. | ||
26 | */ | ||
27 | |||
28 | using System; | ||
29 | using System.Collections.Generic; | ||
30 | using System.Diagnostics; | ||
31 | |||
32 | namespace OpenSim.Region.Physics.ConvexDecompositionDotNet | ||
33 | { | ||
34 | public delegate void ConvexDecompositionCallback(ConvexResult result); | ||
35 | |||
36 | public class FaceTri | ||
37 | { | ||
38 | public float3 P1; | ||
39 | public float3 P2; | ||
40 | public float3 P3; | ||
41 | |||
42 | public FaceTri() { } | ||
43 | |||
44 | public FaceTri(List<float3> vertices, int i1, int i2, int i3) | ||
45 | { | ||
46 | P1 = new float3(vertices[i1]); | ||
47 | P2 = new float3(vertices[i2]); | ||
48 | P3 = new float3(vertices[i3]); | ||
49 | } | ||
50 | } | ||
51 | |||
52 | public static class ConvexDecomposition | ||
53 | { | ||
54 | private static void addTri(VertexPool vl, List<int> list, float3 p1, float3 p2, float3 p3) | ||
55 | { | ||
56 | int i1 = vl.getIndex(p1); | ||
57 | int i2 = vl.getIndex(p2); | ||
58 | int i3 = vl.getIndex(p3); | ||
59 | |||
60 | // do *not* process degenerate triangles! | ||
61 | if ( i1 != i2 && i1 != i3 && i2 != i3 ) | ||
62 | { | ||
63 | list.Add(i1); | ||
64 | list.Add(i2); | ||
65 | list.Add(i3); | ||
66 | } | ||
67 | } | ||
68 | |||
69 | public static void calcConvexDecomposition(List<float3> vertices, List<int> indices, ConvexDecompositionCallback callback, float masterVolume, int depth, | ||
70 | int maxDepth, float concavePercent, float mergePercent) | ||
71 | { | ||
72 | float4 plane = new float4(); | ||
73 | bool split = false; | ||
74 | |||
75 | if (depth < maxDepth) | ||
76 | { | ||
77 | float volume = 0f; | ||
78 | float c = Concavity.computeConcavity(vertices, indices, ref plane, ref volume); | ||
79 | |||
80 | if (depth == 0) | ||
81 | { | ||
82 | masterVolume = volume; | ||
83 | } | ||
84 | |||
85 | float percent = (c * 100.0f) / masterVolume; | ||
86 | |||
87 | if (percent > concavePercent) // if great than 5% of the total volume is concave, go ahead and keep splitting. | ||
88 | { | ||
89 | split = true; | ||
90 | } | ||
91 | } | ||
92 | |||
93 | if (depth >= maxDepth || !split) | ||
94 | { | ||
95 | HullResult result = new HullResult(); | ||
96 | HullDesc desc = new HullDesc(); | ||
97 | |||
98 | desc.SetHullFlag(HullFlag.QF_TRIANGLES); | ||
99 | |||
100 | desc.Vertices = vertices; | ||
101 | |||
102 | HullError ret = HullUtils.CreateConvexHull(desc, ref result); | ||
103 | |||
104 | if (ret == HullError.QE_OK) | ||
105 | { | ||
106 | ConvexResult r = new ConvexResult(result.OutputVertices, result.Indices); | ||
107 | callback(r); | ||
108 | } | ||
109 | |||
110 | return; | ||
111 | } | ||
112 | |||
113 | List<int> ifront = new List<int>(); | ||
114 | List<int> iback = new List<int>(); | ||
115 | |||
116 | VertexPool vfront = new VertexPool(); | ||
117 | VertexPool vback = new VertexPool(); | ||
118 | |||
119 | // ok..now we are going to 'split' all of the input triangles against this plane! | ||
120 | for (int i = 0; i < indices.Count / 3; i++) | ||
121 | { | ||
122 | int i1 = indices[i * 3 + 0]; | ||
123 | int i2 = indices[i * 3 + 1]; | ||
124 | int i3 = indices[i * 3 + 2]; | ||
125 | |||
126 | FaceTri t = new FaceTri(vertices, i1, i2, i3); | ||
127 | |||
128 | float3[] front = new float3[4]; | ||
129 | float3[] back = new float3[4]; | ||
130 | |||
131 | int fcount = 0; | ||
132 | int bcount = 0; | ||
133 | |||
134 | PlaneTriResult result = PlaneTri.planeTriIntersection(plane, t, 0.00001f, ref front, out fcount, ref back, out bcount); | ||
135 | |||
136 | if (fcount > 4 || bcount > 4) | ||
137 | { | ||
138 | result = PlaneTri.planeTriIntersection(plane, t, 0.00001f, ref front, out fcount, ref back, out bcount); | ||
139 | } | ||
140 | |||
141 | switch (result) | ||
142 | { | ||
143 | case PlaneTriResult.PTR_FRONT: | ||
144 | Debug.Assert(fcount == 3); | ||
145 | addTri(vfront, ifront, front[0], front[1], front[2]); | ||
146 | break; | ||
147 | case PlaneTriResult.PTR_BACK: | ||
148 | Debug.Assert(bcount == 3); | ||
149 | addTri(vback, iback, back[0], back[1], back[2]); | ||
150 | break; | ||
151 | case PlaneTriResult.PTR_SPLIT: | ||
152 | Debug.Assert(fcount >= 3 && fcount <= 4); | ||
153 | Debug.Assert(bcount >= 3 && bcount <= 4); | ||
154 | |||
155 | addTri(vfront, ifront, front[0], front[1], front[2]); | ||
156 | addTri(vback, iback, back[0], back[1], back[2]); | ||
157 | |||
158 | if (fcount == 4) | ||
159 | { | ||
160 | addTri(vfront, ifront, front[0], front[2], front[3]); | ||
161 | } | ||
162 | |||
163 | if (bcount == 4) | ||
164 | { | ||
165 | addTri(vback, iback, back[0], back[2], back[3]); | ||
166 | } | ||
167 | |||
168 | break; | ||
169 | } | ||
170 | } | ||
171 | |||
172 | // ok... here we recursively call | ||
173 | if (ifront.Count > 0) | ||
174 | { | ||
175 | int vcount = vfront.GetSize(); | ||
176 | List<float3> vertices2 = vfront.GetVertices(); | ||
177 | for (int i = 0; i < vertices2.Count; i++) | ||
178 | vertices2[i] = new float3(vertices2[i]); | ||
179 | int tcount = ifront.Count / 3; | ||
180 | |||
181 | calcConvexDecomposition(vertices2, ifront, callback, masterVolume, depth + 1, maxDepth, concavePercent, mergePercent); | ||
182 | } | ||
183 | |||
184 | ifront.Clear(); | ||
185 | vfront.Clear(); | ||
186 | |||
187 | if (iback.Count > 0) | ||
188 | { | ||
189 | int vcount = vback.GetSize(); | ||
190 | List<float3> vertices2 = vback.GetVertices(); | ||
191 | int tcount = iback.Count / 3; | ||
192 | |||
193 | calcConvexDecomposition(vertices2, iback, callback, masterVolume, depth + 1, maxDepth, concavePercent, mergePercent); | ||
194 | } | ||
195 | |||
196 | iback.Clear(); | ||
197 | vback.Clear(); | ||
198 | } | ||
199 | } | ||
200 | } | ||
diff --git a/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/ConvexResult.cs b/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/ConvexResult.cs new file mode 100644 index 0000000..87758b5 --- /dev/null +++ b/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/ConvexResult.cs | |||
@@ -0,0 +1,74 @@ | |||
1 | /* The MIT License | ||
2 | * | ||
3 | * Copyright (c) 2010 Intel Corporation. | ||
4 | * All rights reserved. | ||
5 | * | ||
6 | * Based on the convexdecomposition library from | ||
7 | * <http://codesuppository.googlecode.com> by John W. Ratcliff and Stan Melax. | ||
8 | * | ||
9 | * Permission is hereby granted, free of charge, to any person obtaining a copy | ||
10 | * of this software and associated documentation files (the "Software"), to deal | ||
11 | * in the Software without restriction, including without limitation the rights | ||
12 | * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell | ||
13 | * copies of the Software, and to permit persons to whom the Software is | ||
14 | * furnished to do so, subject to the following conditions: | ||
15 | * | ||
16 | * The above copyright notice and this permission notice shall be included in | ||
17 | * all copies or substantial portions of the Software. | ||
18 | * | ||
19 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR | ||
20 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, | ||
21 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE | ||
22 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER | ||
23 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, | ||
24 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN | ||
25 | * THE SOFTWARE. | ||
26 | */ | ||
27 | |||
28 | using System; | ||
29 | using System.Collections.Generic; | ||
30 | |||
31 | namespace OpenSim.Region.Physics.ConvexDecompositionDotNet | ||
32 | { | ||
33 | public class ConvexResult | ||
34 | { | ||
35 | public List<float3> HullVertices; | ||
36 | public List<int> HullIndices; | ||
37 | |||
38 | public float mHullVolume; // the volume of the convex hull. | ||
39 | |||
40 | //public float[] OBBSides = new float[3]; // the width, height and breadth of the best fit OBB | ||
41 | //public float[] OBBCenter = new float[3]; // the center of the OBB | ||
42 | //public float[] OBBOrientation = new float[4]; // the quaternion rotation of the OBB. | ||
43 | //public float[] OBBTransform = new float[16]; // the 4x4 transform of the OBB. | ||
44 | //public float OBBVolume; // the volume of the OBB | ||
45 | |||
46 | //public float SphereRadius; // radius and center of best fit sphere | ||
47 | //public float[] SphereCenter = new float[3]; | ||
48 | //public float SphereVolume; // volume of the best fit sphere | ||
49 | |||
50 | public ConvexResult() | ||
51 | { | ||
52 | HullVertices = new List<float3>(); | ||
53 | HullIndices = new List<int>(); | ||
54 | } | ||
55 | |||
56 | public ConvexResult(List<float3> hvertices, List<int> hindices) | ||
57 | { | ||
58 | HullVertices = hvertices; | ||
59 | HullIndices = hindices; | ||
60 | } | ||
61 | |||
62 | public ConvexResult(ConvexResult r) | ||
63 | { | ||
64 | HullVertices = new List<float3>(r.HullVertices); | ||
65 | HullIndices = new List<int>(r.HullIndices); | ||
66 | } | ||
67 | |||
68 | public void Dispose() | ||
69 | { | ||
70 | HullVertices = null; | ||
71 | HullIndices = null; | ||
72 | } | ||
73 | } | ||
74 | } | ||
diff --git a/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/HullClasses.cs b/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/HullClasses.cs new file mode 100644 index 0000000..d81df26 --- /dev/null +++ b/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/HullClasses.cs | |||
@@ -0,0 +1,171 @@ | |||
1 | /* The MIT License | ||
2 | * | ||
3 | * Copyright (c) 2010 Intel Corporation. | ||
4 | * All rights reserved. | ||
5 | * | ||
6 | * Based on the convexdecomposition library from | ||
7 | * <http://codesuppository.googlecode.com> by John W. Ratcliff and Stan Melax. | ||
8 | * | ||
9 | * Permission is hereby granted, free of charge, to any person obtaining a copy | ||
10 | * of this software and associated documentation files (the "Software"), to deal | ||
11 | * in the Software without restriction, including without limitation the rights | ||
12 | * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell | ||
13 | * copies of the Software, and to permit persons to whom the Software is | ||
14 | * furnished to do so, subject to the following conditions: | ||
15 | * | ||
16 | * The above copyright notice and this permission notice shall be included in | ||
17 | * all copies or substantial portions of the Software. | ||
18 | * | ||
19 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR | ||
20 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, | ||
21 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE | ||
22 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER | ||
23 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, | ||
24 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN | ||
25 | * THE SOFTWARE. | ||
26 | */ | ||
27 | |||
28 | using System; | ||
29 | using System.Collections.Generic; | ||
30 | |||
31 | namespace OpenSim.Region.Physics.ConvexDecompositionDotNet | ||
32 | { | ||
33 | public class HullResult | ||
34 | { | ||
35 | public bool Polygons = true; // true if indices represents polygons, false indices are triangles | ||
36 | public List<float3> OutputVertices = new List<float3>(); | ||
37 | public List<int> Indices; | ||
38 | |||
39 | // If triangles, then indices are array indexes into the vertex list. | ||
40 | // If polygons, indices are in the form (number of points in face) (p1, p2, p3, ..) etc.. | ||
41 | } | ||
42 | |||
43 | public class PHullResult | ||
44 | { | ||
45 | public List<float3> Vertices = new List<float3>(); | ||
46 | public List<int> Indices = new List<int>(); | ||
47 | } | ||
48 | |||
49 | [Flags] | ||
50 | public enum HullFlag : int | ||
51 | { | ||
52 | QF_DEFAULT = 0, | ||
53 | QF_TRIANGLES = (1 << 0), // report results as triangles, not polygons. | ||
54 | QF_SKIN_WIDTH = (1 << 2) // extrude hull based on this skin width | ||
55 | } | ||
56 | |||
57 | public enum HullError : int | ||
58 | { | ||
59 | QE_OK, // success! | ||
60 | QE_FAIL // failed. | ||
61 | } | ||
62 | |||
63 | public class HullDesc | ||
64 | { | ||
65 | public HullFlag Flags; // flags to use when generating the convex hull. | ||
66 | public List<float3> Vertices; | ||
67 | public float NormalEpsilon; // the epsilon for removing duplicates. This is a normalized value, if normalized bit is on. | ||
68 | public float SkinWidth; | ||
69 | public uint MaxVertices; // maximum number of vertices to be considered for the hull! | ||
70 | public uint MaxFaces; | ||
71 | |||
72 | public HullDesc() | ||
73 | { | ||
74 | Flags = HullFlag.QF_DEFAULT; | ||
75 | Vertices = new List<float3>(); | ||
76 | NormalEpsilon = 0.001f; | ||
77 | MaxVertices = 4096; | ||
78 | MaxFaces = 4096; | ||
79 | SkinWidth = 0.01f; | ||
80 | } | ||
81 | |||
82 | public HullDesc(HullFlag flags, List<float3> vertices) | ||
83 | { | ||
84 | Flags = flags; | ||
85 | Vertices = new List<float3>(vertices); | ||
86 | NormalEpsilon = 0.001f; | ||
87 | MaxVertices = 4096; | ||
88 | MaxFaces = 4096; | ||
89 | SkinWidth = 0.01f; | ||
90 | } | ||
91 | |||
92 | public bool HasHullFlag(HullFlag flag) | ||
93 | { | ||
94 | return (Flags & flag) != 0; | ||
95 | } | ||
96 | |||
97 | public void SetHullFlag(HullFlag flag) | ||
98 | { | ||
99 | Flags |= flag; | ||
100 | } | ||
101 | |||
102 | public void ClearHullFlag(HullFlag flag) | ||
103 | { | ||
104 | Flags &= ~flag; | ||
105 | } | ||
106 | } | ||
107 | |||
108 | public class ConvexH | ||
109 | { | ||
110 | public struct HalfEdge | ||
111 | { | ||
112 | public short ea; // the other half of the edge (index into edges list) | ||
113 | public byte v; // the vertex at the start of this edge (index into vertices list) | ||
114 | public byte p; // the facet on which this edge lies (index into facets list) | ||
115 | |||
116 | public HalfEdge(short _ea, byte _v, byte _p) | ||
117 | { | ||
118 | ea = _ea; | ||
119 | v = _v; | ||
120 | p = _p; | ||
121 | } | ||
122 | |||
123 | public HalfEdge(HalfEdge e) | ||
124 | { | ||
125 | ea = e.ea; | ||
126 | v = e.v; | ||
127 | p = e.p; | ||
128 | } | ||
129 | } | ||
130 | |||
131 | public List<float3> vertices = new List<float3>(); | ||
132 | public List<HalfEdge> edges = new List<HalfEdge>(); | ||
133 | public List<Plane> facets = new List<Plane>(); | ||
134 | |||
135 | public ConvexH(int vertices_size, int edges_size, int facets_size) | ||
136 | { | ||
137 | vertices = new List<float3>(vertices_size); | ||
138 | edges = new List<HalfEdge>(edges_size); | ||
139 | facets = new List<Plane>(facets_size); | ||
140 | } | ||
141 | } | ||
142 | |||
143 | public class VertFlag | ||
144 | { | ||
145 | public byte planetest; | ||
146 | public byte junk; | ||
147 | public byte undermap; | ||
148 | public byte overmap; | ||
149 | } | ||
150 | |||
151 | public class EdgeFlag | ||
152 | { | ||
153 | public byte planetest; | ||
154 | public byte fixes; | ||
155 | public short undermap; | ||
156 | public short overmap; | ||
157 | } | ||
158 | |||
159 | public class PlaneFlag | ||
160 | { | ||
161 | public byte undermap; | ||
162 | public byte overmap; | ||
163 | } | ||
164 | |||
165 | public class Coplanar | ||
166 | { | ||
167 | public ushort ea; | ||
168 | public byte v0; | ||
169 | public byte v1; | ||
170 | } | ||
171 | } | ||
diff --git a/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/HullTriangle.cs b/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/HullTriangle.cs new file mode 100644 index 0000000..1119a75 --- /dev/null +++ b/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/HullTriangle.cs | |||
@@ -0,0 +1,99 @@ | |||
1 | /* The MIT License | ||
2 | * | ||
3 | * Copyright (c) 2010 Intel Corporation. | ||
4 | * All rights reserved. | ||
5 | * | ||
6 | * Based on the convexdecomposition library from | ||
7 | * <http://codesuppository.googlecode.com> by John W. Ratcliff and Stan Melax. | ||
8 | * | ||
9 | * Permission is hereby granted, free of charge, to any person obtaining a copy | ||
10 | * of this software and associated documentation files (the "Software"), to deal | ||
11 | * in the Software without restriction, including without limitation the rights | ||
12 | * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell | ||
13 | * copies of the Software, and to permit persons to whom the Software is | ||
14 | * furnished to do so, subject to the following conditions: | ||
15 | * | ||
16 | * The above copyright notice and this permission notice shall be included in | ||
17 | * all copies or substantial portions of the Software. | ||
18 | * | ||
19 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR | ||
20 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, | ||
21 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE | ||
22 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER | ||
23 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, | ||
24 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN | ||
25 | * THE SOFTWARE. | ||
26 | */ | ||
27 | |||
28 | using System; | ||
29 | using System.Collections.Generic; | ||
30 | using System.Diagnostics; | ||
31 | |||
32 | namespace OpenSim.Region.Physics.ConvexDecompositionDotNet | ||
33 | { | ||
34 | public class HullTriangle : int3 | ||
35 | { | ||
36 | public int3 n = new int3(); | ||
37 | public int id; | ||
38 | public int vmax; | ||
39 | public float rise; | ||
40 | private List<HullTriangle> tris; | ||
41 | |||
42 | public HullTriangle(int a, int b, int c, List<HullTriangle> tris) | ||
43 | : base(a, b, c) | ||
44 | { | ||
45 | this.tris = tris; | ||
46 | |||
47 | n = new int3(-1, -1, -1); | ||
48 | id = tris.Count; | ||
49 | tris.Add(this); | ||
50 | vmax = -1; | ||
51 | rise = 0.0f; | ||
52 | } | ||
53 | |||
54 | public void Dispose() | ||
55 | { | ||
56 | Debug.Assert(tris[id] == this); | ||
57 | tris[id] = null; | ||
58 | } | ||
59 | |||
60 | public int neib(int a, int b) | ||
61 | { | ||
62 | int i; | ||
63 | |||
64 | for (i = 0; i < 3; i++) | ||
65 | { | ||
66 | int i1 = (i + 1) % 3; | ||
67 | int i2 = (i + 2) % 3; | ||
68 | if ((this)[i] == a && (this)[i1] == b) | ||
69 | return n[i2]; | ||
70 | if ((this)[i] == b && (this)[i1] == a) | ||
71 | return n[i2]; | ||
72 | } | ||
73 | |||
74 | Debug.Assert(false); | ||
75 | return -1; | ||
76 | } | ||
77 | |||
78 | public void setneib(int a, int b, int value) | ||
79 | { | ||
80 | int i; | ||
81 | |||
82 | for (i = 0; i < 3; i++) | ||
83 | { | ||
84 | int i1 = (i + 1) % 3; | ||
85 | int i2 = (i + 2) % 3; | ||
86 | if ((this)[i] == a && (this)[i1] == b) | ||
87 | { | ||
88 | n[i2] = value; | ||
89 | return; | ||
90 | } | ||
91 | if ((this)[i] == b && (this)[i1] == a) | ||
92 | { | ||
93 | n[i2] = value; | ||
94 | return; | ||
95 | } | ||
96 | } | ||
97 | } | ||
98 | } | ||
99 | } | ||
diff --git a/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/HullUtils.cs b/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/HullUtils.cs new file mode 100644 index 0000000..c9ccfe2 --- /dev/null +++ b/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/HullUtils.cs | |||
@@ -0,0 +1,1868 @@ | |||
1 | /* The MIT License | ||
2 | * | ||
3 | * Copyright (c) 2010 Intel Corporation. | ||
4 | * All rights reserved. | ||
5 | * | ||
6 | * Based on the convexdecomposition library from | ||
7 | * <http://codesuppository.googlecode.com> by John W. Ratcliff and Stan Melax. | ||
8 | * | ||
9 | * Permission is hereby granted, free of charge, to any person obtaining a copy | ||
10 | * of this software and associated documentation files (the "Software"), to deal | ||
11 | * in the Software without restriction, including without limitation the rights | ||
12 | * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell | ||
13 | * copies of the Software, and to permit persons to whom the Software is | ||
14 | * furnished to do so, subject to the following conditions: | ||
15 | * | ||
16 | * The above copyright notice and this permission notice shall be included in | ||
17 | * all copies or substantial portions of the Software. | ||
18 | * | ||
19 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR | ||
20 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, | ||
21 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE | ||
22 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER | ||
23 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, | ||
24 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN | ||
25 | * THE SOFTWARE. | ||
26 | */ | ||
27 | |||
28 | using System; | ||
29 | using System.Collections.Generic; | ||
30 | using System.Diagnostics; | ||
31 | |||
32 | namespace OpenSim.Region.Physics.ConvexDecompositionDotNet | ||
33 | { | ||
34 | public static class HullUtils | ||
35 | { | ||
36 | public static int argmin(float[] a, int n) | ||
37 | { | ||
38 | int r = 0; | ||
39 | for (int i = 1; i < n; i++) | ||
40 | { | ||
41 | if (a[i] < a[r]) | ||
42 | { | ||
43 | r = i; | ||
44 | } | ||
45 | } | ||
46 | return r; | ||
47 | } | ||
48 | |||
49 | public static float clampf(float a) | ||
50 | { | ||
51 | return Math.Min(1.0f, Math.Max(0.0f, a)); | ||
52 | } | ||
53 | |||
54 | public static float Round(float a, float precision) | ||
55 | { | ||
56 | return (float)Math.Floor(0.5f + a / precision) * precision; | ||
57 | } | ||
58 | |||
59 | public static float Interpolate(float f0, float f1, float alpha) | ||
60 | { | ||
61 | return f0 * (1 - alpha) + f1 * alpha; | ||
62 | } | ||
63 | |||
64 | public static void Swap<T>(ref T a, ref T b) | ||
65 | { | ||
66 | T tmp = a; | ||
67 | a = b; | ||
68 | b = tmp; | ||
69 | } | ||
70 | |||
71 | public static bool above(List<float3> vertices, int3 t, float3 p, float epsilon) | ||
72 | { | ||
73 | float3 vtx = vertices[t.x]; | ||
74 | float3 n = TriNormal(vtx, vertices[t.y], vertices[t.z]); | ||
75 | return (float3.dot(n, p - vtx) > epsilon); // EPSILON??? | ||
76 | } | ||
77 | |||
78 | public static int hasedge(int3 t, int a, int b) | ||
79 | { | ||
80 | for (int i = 0; i < 3; i++) | ||
81 | { | ||
82 | int i1 = (i + 1) % 3; | ||
83 | if (t[i] == a && t[i1] == b) | ||
84 | return 1; | ||
85 | } | ||
86 | return 0; | ||
87 | } | ||
88 | |||
89 | public static bool hasvert(int3 t, int v) | ||
90 | { | ||
91 | return (t[0] == v || t[1] == v || t[2] == v); | ||
92 | } | ||
93 | |||
94 | public static int shareedge(int3 a, int3 b) | ||
95 | { | ||
96 | int i; | ||
97 | for (i = 0; i < 3; i++) | ||
98 | { | ||
99 | int i1 = (i + 1) % 3; | ||
100 | if (hasedge(a, b[i1], b[i]) != 0) | ||
101 | return 1; | ||
102 | } | ||
103 | return 0; | ||
104 | } | ||
105 | |||
106 | public static void b2bfix(HullTriangle s, HullTriangle t, List<HullTriangle> tris) | ||
107 | { | ||
108 | int i; | ||
109 | for (i = 0; i < 3; i++) | ||
110 | { | ||
111 | int i1 = (i + 1) % 3; | ||
112 | int i2 = (i + 2) % 3; | ||
113 | int a = (s)[i1]; | ||
114 | int b = (s)[i2]; | ||
115 | Debug.Assert(tris[s.neib(a, b)].neib(b, a) == s.id); | ||
116 | Debug.Assert(tris[t.neib(a, b)].neib(b, a) == t.id); | ||
117 | tris[s.neib(a, b)].setneib(b, a, t.neib(b, a)); | ||
118 | tris[t.neib(b, a)].setneib(a, b, s.neib(a, b)); | ||
119 | } | ||
120 | } | ||
121 | |||
122 | public static void removeb2b(HullTriangle s, HullTriangle t, List<HullTriangle> tris) | ||
123 | { | ||
124 | b2bfix(s, t, tris); | ||
125 | s.Dispose(); | ||
126 | t.Dispose(); | ||
127 | } | ||
128 | |||
129 | public static void checkit(HullTriangle t, List<HullTriangle> tris) | ||
130 | { | ||
131 | int i; | ||
132 | Debug.Assert(tris[t.id] == t); | ||
133 | for (i = 0; i < 3; i++) | ||
134 | { | ||
135 | int i1 = (i + 1) % 3; | ||
136 | int i2 = (i + 2) % 3; | ||
137 | int a = (t)[i1]; | ||
138 | int b = (t)[i2]; | ||
139 | Debug.Assert(a != b); | ||
140 | Debug.Assert(tris[t.n[i]].neib(b, a) == t.id); | ||
141 | } | ||
142 | } | ||
143 | |||
144 | public static void extrude(HullTriangle t0, int v, List<HullTriangle> tris) | ||
145 | { | ||
146 | int3 t = t0; | ||
147 | int n = tris.Count; | ||
148 | HullTriangle ta = new HullTriangle(v, t[1], t[2], tris); | ||
149 | ta.n = new int3(t0.n[0], n + 1, n + 2); | ||
150 | tris[t0.n[0]].setneib(t[1], t[2], n + 0); | ||
151 | HullTriangle tb = new HullTriangle(v, t[2], t[0], tris); | ||
152 | tb.n = new int3(t0.n[1], n + 2, n + 0); | ||
153 | tris[t0.n[1]].setneib(t[2], t[0], n + 1); | ||
154 | HullTriangle tc = new HullTriangle(v, t[0], t[1], tris); | ||
155 | tc.n = new int3(t0.n[2], n + 0, n + 1); | ||
156 | tris[t0.n[2]].setneib(t[0], t[1], n + 2); | ||
157 | checkit(ta, tris); | ||
158 | checkit(tb, tris); | ||
159 | checkit(tc, tris); | ||
160 | if (hasvert(tris[ta.n[0]], v)) | ||
161 | removeb2b(ta, tris[ta.n[0]], tris); | ||
162 | if (hasvert(tris[tb.n[0]], v)) | ||
163 | removeb2b(tb, tris[tb.n[0]], tris); | ||
164 | if (hasvert(tris[tc.n[0]], v)) | ||
165 | removeb2b(tc, tris[tc.n[0]], tris); | ||
166 | t0.Dispose(); | ||
167 | } | ||
168 | |||
169 | public static HullTriangle extrudable(float epsilon, List<HullTriangle> tris) | ||
170 | { | ||
171 | int i; | ||
172 | HullTriangle t = null; | ||
173 | for (i = 0; i < tris.Count; i++) | ||
174 | { | ||
175 | if (t == null || (tris.Count > i && (object)tris[i] != null && t.rise < tris[i].rise)) | ||
176 | { | ||
177 | t = tris[i]; | ||
178 | } | ||
179 | } | ||
180 | return (t.rise > epsilon) ? t : null; | ||
181 | } | ||
182 | |||
183 | public static Quaternion RotationArc(float3 v0, float3 v1) | ||
184 | { | ||
185 | Quaternion q = new Quaternion(); | ||
186 | v0 = float3.normalize(v0); // Comment these two lines out if you know its not needed. | ||
187 | v1 = float3.normalize(v1); // If vector is already unit length then why do it again? | ||
188 | float3 c = float3.cross(v0, v1); | ||
189 | float d = float3.dot(v0, v1); | ||
190 | if (d <= -1.0f) // 180 about x axis | ||
191 | { | ||
192 | return new Quaternion(1f, 0f, 0f, 0f); | ||
193 | } | ||
194 | float s = (float)Math.Sqrt((1 + d) * 2f); | ||
195 | q.x = c.x / s; | ||
196 | q.y = c.y / s; | ||
197 | q.z = c.z / s; | ||
198 | q.w = s / 2.0f; | ||
199 | return q; | ||
200 | } | ||
201 | |||
202 | public static float3 PlaneLineIntersection(Plane plane, float3 p0, float3 p1) | ||
203 | { | ||
204 | // returns the point where the line p0-p1 intersects the plane n&d | ||
205 | float3 dif = p1 - p0; | ||
206 | float dn = float3.dot(plane.normal, dif); | ||
207 | float t = -(plane.dist + float3.dot(plane.normal, p0)) / dn; | ||
208 | return p0 + (dif * t); | ||
209 | } | ||
210 | |||
211 | public static float3 LineProject(float3 p0, float3 p1, float3 a) | ||
212 | { | ||
213 | float3 w = new float3(); | ||
214 | w = p1 - p0; | ||
215 | float t = float3.dot(w, (a - p0)) / (w.x * w.x + w.y * w.y + w.z * w.z); | ||
216 | return p0 + w * t; | ||
217 | } | ||
218 | |||
219 | public static float3 PlaneProject(Plane plane, float3 point) | ||
220 | { | ||
221 | return point - plane.normal * (float3.dot(point, plane.normal) + plane.dist); | ||
222 | } | ||
223 | |||
224 | public static float LineProjectTime(float3 p0, float3 p1, float3 a) | ||
225 | { | ||
226 | float3 w = new float3(); | ||
227 | w = p1 - p0; | ||
228 | float t = float3.dot(w, (a - p0)) / (w.x * w.x + w.y * w.y + w.z * w.z); | ||
229 | return t; | ||
230 | } | ||
231 | |||
232 | public static float3 ThreePlaneIntersection(Plane p0, Plane p1, Plane p2) | ||
233 | { | ||
234 | float3x3 mp = float3x3.Transpose(new float3x3(p0.normal, p1.normal, p2.normal)); | ||
235 | float3x3 mi = float3x3.Inverse(mp); | ||
236 | float3 b = new float3(p0.dist, p1.dist, p2.dist); | ||
237 | return -b * mi; | ||
238 | } | ||
239 | |||
240 | public static bool PolyHit(List<float3> vert, float3 v0, float3 v1) | ||
241 | { | ||
242 | float3 impact = new float3(); | ||
243 | float3 normal = new float3(); | ||
244 | return PolyHit(vert, v0, v1, out impact, out normal); | ||
245 | } | ||
246 | |||
247 | public static bool PolyHit(List<float3> vert, float3 v0, float3 v1, out float3 impact) | ||
248 | { | ||
249 | float3 normal = new float3(); | ||
250 | return PolyHit(vert, v0, v1, out impact, out normal); | ||
251 | } | ||
252 | |||
253 | public static bool PolyHit(List<float3> vert, float3 v0, float3 v1, out float3 impact, out float3 normal) | ||
254 | { | ||
255 | float3 the_point = new float3(); | ||
256 | |||
257 | impact = null; | ||
258 | normal = null; | ||
259 | |||
260 | int i; | ||
261 | float3 nrml = new float3(0, 0, 0); | ||
262 | for (i = 0; i < vert.Count; i++) | ||
263 | { | ||
264 | int i1 = (i + 1) % vert.Count; | ||
265 | int i2 = (i + 2) % vert.Count; | ||
266 | nrml = nrml + float3.cross(vert[i1] - vert[i], vert[i2] - vert[i1]); | ||
267 | } | ||
268 | |||
269 | float m = float3.magnitude(nrml); | ||
270 | if (m == 0.0) | ||
271 | { | ||
272 | return false; | ||
273 | } | ||
274 | nrml = nrml * (1.0f / m); | ||
275 | float dist = -float3.dot(nrml, vert[0]); | ||
276 | float d0; | ||
277 | float d1; | ||
278 | if ((d0 = float3.dot(v0, nrml) + dist) < 0 || (d1 = float3.dot(v1, nrml) + dist) > 0) | ||
279 | { | ||
280 | return false; | ||
281 | } | ||
282 | |||
283 | // By using the cached plane distances d0 and d1 | ||
284 | // we can optimize the following: | ||
285 | // the_point = planelineintersection(nrml,dist,v0,v1); | ||
286 | float a = d0 / (d0 - d1); | ||
287 | the_point = v0 * (1 - a) + v1 * a; | ||
288 | |||
289 | |||
290 | bool inside = true; | ||
291 | for (int j = 0; inside && j < vert.Count; j++) | ||
292 | { | ||
293 | // let inside = 0 if outside | ||
294 | float3 pp1 = new float3(); | ||
295 | float3 pp2 = new float3(); | ||
296 | float3 side = new float3(); | ||
297 | pp1 = vert[j]; | ||
298 | pp2 = vert[(j + 1) % vert.Count]; | ||
299 | side = float3.cross((pp2 - pp1), (the_point - pp1)); | ||
300 | inside = (float3.dot(nrml, side) >= 0.0); | ||
301 | } | ||
302 | if (inside) | ||
303 | { | ||
304 | if (normal != null) | ||
305 | { | ||
306 | normal = nrml; | ||
307 | } | ||
308 | if (impact != null) | ||
309 | { | ||
310 | impact = the_point; | ||
311 | } | ||
312 | } | ||
313 | return inside; | ||
314 | } | ||
315 | |||
316 | public static bool BoxInside(float3 p, float3 bmin, float3 bmax) | ||
317 | { | ||
318 | return (p.x >= bmin.x && p.x <= bmax.x && p.y >= bmin.y && p.y <= bmax.y && p.z >= bmin.z && p.z <= bmax.z); | ||
319 | } | ||
320 | |||
321 | public static bool BoxIntersect(float3 v0, float3 v1, float3 bmin, float3 bmax, float3 impact) | ||
322 | { | ||
323 | if (BoxInside(v0, bmin, bmax)) | ||
324 | { | ||
325 | impact = v0; | ||
326 | return true; | ||
327 | } | ||
328 | if (v0.x <= bmin.x && v1.x >= bmin.x) | ||
329 | { | ||
330 | float a = (bmin.x - v0.x) / (v1.x - v0.x); | ||
331 | //v.x = bmin.x; | ||
332 | float vy = (1 - a) * v0.y + a * v1.y; | ||
333 | float vz = (1 - a) * v0.z + a * v1.z; | ||
334 | if (vy >= bmin.y && vy <= bmax.y && vz >= bmin.z && vz <= bmax.z) | ||
335 | { | ||
336 | impact.x = bmin.x; | ||
337 | impact.y = vy; | ||
338 | impact.z = vz; | ||
339 | return true; | ||
340 | } | ||
341 | } | ||
342 | else if (v0.x >= bmax.x && v1.x <= bmax.x) | ||
343 | { | ||
344 | float a = (bmax.x - v0.x) / (v1.x - v0.x); | ||
345 | //v.x = bmax.x; | ||
346 | float vy = (1 - a) * v0.y + a * v1.y; | ||
347 | float vz = (1 - a) * v0.z + a * v1.z; | ||
348 | if (vy >= bmin.y && vy <= bmax.y && vz >= bmin.z && vz <= bmax.z) | ||
349 | { | ||
350 | impact.x = bmax.x; | ||
351 | impact.y = vy; | ||
352 | impact.z = vz; | ||
353 | return true; | ||
354 | } | ||
355 | } | ||
356 | if (v0.y <= bmin.y && v1.y >= bmin.y) | ||
357 | { | ||
358 | float a = (bmin.y - v0.y) / (v1.y - v0.y); | ||
359 | float vx = (1 - a) * v0.x + a * v1.x; | ||
360 | //v.y = bmin.y; | ||
361 | float vz = (1 - a) * v0.z + a * v1.z; | ||
362 | if (vx >= bmin.x && vx <= bmax.x && vz >= bmin.z && vz <= bmax.z) | ||
363 | { | ||
364 | impact.x = vx; | ||
365 | impact.y = bmin.y; | ||
366 | impact.z = vz; | ||
367 | return true; | ||
368 | } | ||
369 | } | ||
370 | else if (v0.y >= bmax.y && v1.y <= bmax.y) | ||
371 | { | ||
372 | float a = (bmax.y - v0.y) / (v1.y - v0.y); | ||
373 | float vx = (1 - a) * v0.x + a * v1.x; | ||
374 | // vy = bmax.y; | ||
375 | float vz = (1 - a) * v0.z + a * v1.z; | ||
376 | if (vx >= bmin.x && vx <= bmax.x && vz >= bmin.z && vz <= bmax.z) | ||
377 | { | ||
378 | impact.x = vx; | ||
379 | impact.y = bmax.y; | ||
380 | impact.z = vz; | ||
381 | return true; | ||
382 | } | ||
383 | } | ||
384 | if (v0.z <= bmin.z && v1.z >= bmin.z) | ||
385 | { | ||
386 | float a = (bmin.z - v0.z) / (v1.z - v0.z); | ||
387 | float vx = (1 - a) * v0.x + a * v1.x; | ||
388 | float vy = (1 - a) * v0.y + a * v1.y; | ||
389 | // v.z = bmin.z; | ||
390 | if (vy >= bmin.y && vy <= bmax.y && vx >= bmin.x && vx <= bmax.x) | ||
391 | { | ||
392 | impact.x = vx; | ||
393 | impact.y = vy; | ||
394 | impact.z = bmin.z; | ||
395 | return true; | ||
396 | } | ||
397 | } | ||
398 | else if (v0.z >= bmax.z && v1.z <= bmax.z) | ||
399 | { | ||
400 | float a = (bmax.z - v0.z) / (v1.z - v0.z); | ||
401 | float vx = (1 - a) * v0.x + a * v1.x; | ||
402 | float vy = (1 - a) * v0.y + a * v1.y; | ||
403 | // v.z = bmax.z; | ||
404 | if (vy >= bmin.y && vy <= bmax.y && vx >= bmin.x && vx <= bmax.x) | ||
405 | { | ||
406 | impact.x = vx; | ||
407 | impact.y = vy; | ||
408 | impact.z = bmax.z; | ||
409 | return true; | ||
410 | } | ||
411 | } | ||
412 | return false; | ||
413 | } | ||
414 | |||
415 | public static float DistanceBetweenLines(float3 ustart, float3 udir, float3 vstart, float3 vdir, float3 upoint) | ||
416 | { | ||
417 | return DistanceBetweenLines(ustart, udir, vstart, vdir, upoint, null); | ||
418 | } | ||
419 | |||
420 | public static float DistanceBetweenLines(float3 ustart, float3 udir, float3 vstart, float3 vdir) | ||
421 | { | ||
422 | return DistanceBetweenLines(ustart, udir, vstart, vdir, null, null); | ||
423 | } | ||
424 | |||
425 | public static float DistanceBetweenLines(float3 ustart, float3 udir, float3 vstart, float3 vdir, float3 upoint, float3 vpoint) | ||
426 | { | ||
427 | float3 cp = float3.normalize(float3.cross(udir, vdir)); | ||
428 | |||
429 | float distu = -float3.dot(cp, ustart); | ||
430 | float distv = -float3.dot(cp, vstart); | ||
431 | float dist = (float)Math.Abs(distu - distv); | ||
432 | if (upoint != null) | ||
433 | { | ||
434 | Plane plane = new Plane(); | ||
435 | plane.normal = float3.normalize(float3.cross(vdir, cp)); | ||
436 | plane.dist = -float3.dot(plane.normal, vstart); | ||
437 | upoint = PlaneLineIntersection(plane, ustart, ustart + udir); | ||
438 | } | ||
439 | if (vpoint != null) | ||
440 | { | ||
441 | Plane plane = new Plane(); | ||
442 | plane.normal = float3.normalize(float3.cross(udir, cp)); | ||
443 | plane.dist = -float3.dot(plane.normal, ustart); | ||
444 | vpoint = PlaneLineIntersection(plane, vstart, vstart + vdir); | ||
445 | } | ||
446 | return dist; | ||
447 | } | ||
448 | |||
449 | public static float3 TriNormal(float3 v0, float3 v1, float3 v2) | ||
450 | { | ||
451 | // return the normal of the triangle | ||
452 | // inscribed by v0, v1, and v2 | ||
453 | float3 cp = float3.cross(v1 - v0, v2 - v1); | ||
454 | float m = float3.magnitude(cp); | ||
455 | if (m == 0) | ||
456 | return new float3(1, 0, 0); | ||
457 | return cp * (1.0f / m); | ||
458 | } | ||
459 | |||
460 | public static int PlaneTest(Plane p, float3 v, float planetestepsilon) | ||
461 | { | ||
462 | float a = float3.dot(v, p.normal) + p.dist; | ||
463 | int flag = (a > planetestepsilon) ? (2) : ((a < -planetestepsilon) ? (1) : (0)); | ||
464 | return flag; | ||
465 | } | ||
466 | |||
467 | public static int SplitTest(ref ConvexH convex, Plane plane, float planetestepsilon) | ||
468 | { | ||
469 | int flag = 0; | ||
470 | for (int i = 0; i < convex.vertices.Count; i++) | ||
471 | { | ||
472 | flag |= PlaneTest(plane, convex.vertices[i], planetestepsilon); | ||
473 | } | ||
474 | return flag; | ||
475 | } | ||
476 | |||
477 | public static Quaternion VirtualTrackBall(float3 cop, float3 cor, float3 dir1, float3 dir2) | ||
478 | { | ||
479 | // routine taken from game programming gems. | ||
480 | // Implement track ball functionality to spin stuf on the screen | ||
481 | // cop center of projection | ||
482 | // cor center of rotation | ||
483 | // dir1 old mouse direction | ||
484 | // dir2 new mouse direction | ||
485 | // pretend there is a sphere around cor. Then find the points | ||
486 | // where dir1 and dir2 intersect that sphere. Find the | ||
487 | // rotation that takes the first point to the second. | ||
488 | float m; | ||
489 | // compute plane | ||
490 | float3 nrml = cor - cop; | ||
491 | float fudgefactor = 1.0f / (float3.magnitude(nrml) * 0.25f); // since trackball proportional to distance from cop | ||
492 | nrml = float3.normalize(nrml); | ||
493 | float dist = -float3.dot(nrml, cor); | ||
494 | float3 u = PlaneLineIntersection(new Plane(nrml, dist), cop, cop + dir1); | ||
495 | u = u - cor; | ||
496 | u = u * fudgefactor; | ||
497 | m = float3.magnitude(u); | ||
498 | if (m > 1) | ||
499 | { | ||
500 | u /= m; | ||
501 | } | ||
502 | else | ||
503 | { | ||
504 | u = u - (nrml * (float)Math.Sqrt(1 - m * m)); | ||
505 | } | ||
506 | float3 v = PlaneLineIntersection(new Plane(nrml, dist), cop, cop + dir2); | ||
507 | v = v - cor; | ||
508 | v = v * fudgefactor; | ||
509 | m = float3.magnitude(v); | ||
510 | if (m > 1) | ||
511 | { | ||
512 | v /= m; | ||
513 | } | ||
514 | else | ||
515 | { | ||
516 | v = v - (nrml * (float)Math.Sqrt(1 - m * m)); | ||
517 | } | ||
518 | return RotationArc(u, v); | ||
519 | } | ||
520 | |||
521 | public static bool AssertIntact(ConvexH convex, float planetestepsilon) | ||
522 | { | ||
523 | int i; | ||
524 | int estart = 0; | ||
525 | for (i = 0; i < convex.edges.Count; i++) | ||
526 | { | ||
527 | if (convex.edges[estart].p != convex.edges[i].p) | ||
528 | { | ||
529 | estart = i; | ||
530 | } | ||
531 | int inext = i + 1; | ||
532 | if (inext >= convex.edges.Count || convex.edges[inext].p != convex.edges[i].p) | ||
533 | { | ||
534 | inext = estart; | ||
535 | } | ||
536 | Debug.Assert(convex.edges[inext].p == convex.edges[i].p); | ||
537 | int nb = convex.edges[i].ea; | ||
538 | Debug.Assert(nb != 255); | ||
539 | if (nb == 255 || nb == -1) | ||
540 | return false; | ||
541 | Debug.Assert(nb != -1); | ||
542 | Debug.Assert(i == convex.edges[nb].ea); | ||
543 | } | ||
544 | for (i = 0; i < convex.edges.Count; i++) | ||
545 | { | ||
546 | Debug.Assert((0) == PlaneTest(convex.facets[convex.edges[i].p], convex.vertices[convex.edges[i].v], planetestepsilon)); | ||
547 | if ((0) != PlaneTest(convex.facets[convex.edges[i].p], convex.vertices[convex.edges[i].v], planetestepsilon)) | ||
548 | return false; | ||
549 | if (convex.edges[estart].p != convex.edges[i].p) | ||
550 | { | ||
551 | estart = i; | ||
552 | } | ||
553 | int i1 = i + 1; | ||
554 | if (i1 >= convex.edges.Count || convex.edges[i1].p != convex.edges[i].p) | ||
555 | { | ||
556 | i1 = estart; | ||
557 | } | ||
558 | int i2 = i1 + 1; | ||
559 | if (i2 >= convex.edges.Count || convex.edges[i2].p != convex.edges[i].p) | ||
560 | { | ||
561 | i2 = estart; | ||
562 | } | ||
563 | if (i == i2) // i sliced tangent to an edge and created 2 meaningless edges | ||
564 | continue; | ||
565 | float3 localnormal = TriNormal(convex.vertices[convex.edges[i].v], convex.vertices[convex.edges[i1].v], convex.vertices[convex.edges[i2].v]); | ||
566 | Debug.Assert(float3.dot(localnormal, convex.facets[convex.edges[i].p].normal) > 0); | ||
567 | if (float3.dot(localnormal, convex.facets[convex.edges[i].p].normal) <= 0) | ||
568 | return false; | ||
569 | } | ||
570 | return true; | ||
571 | } | ||
572 | |||
573 | public static ConvexH test_btbq(float planetestepsilon) | ||
574 | { | ||
575 | // back to back quads | ||
576 | ConvexH convex = new ConvexH(4, 8, 2); | ||
577 | convex.vertices[0] = new float3(0, 0, 0); | ||
578 | convex.vertices[1] = new float3(1, 0, 0); | ||
579 | convex.vertices[2] = new float3(1, 1, 0); | ||
580 | convex.vertices[3] = new float3(0, 1, 0); | ||
581 | convex.facets[0] = new Plane(new float3(0, 0, 1), 0); | ||
582 | convex.facets[1] = new Plane(new float3(0, 0, -1), 0); | ||
583 | convex.edges[0] = new ConvexH.HalfEdge(7, 0, 0); | ||
584 | convex.edges[1] = new ConvexH.HalfEdge(6, 1, 0); | ||
585 | convex.edges[2] = new ConvexH.HalfEdge(5, 2, 0); | ||
586 | convex.edges[3] = new ConvexH.HalfEdge(4, 3, 0); | ||
587 | |||
588 | convex.edges[4] = new ConvexH.HalfEdge(3, 0, 1); | ||
589 | convex.edges[5] = new ConvexH.HalfEdge(2, 3, 1); | ||
590 | convex.edges[6] = new ConvexH.HalfEdge(1, 2, 1); | ||
591 | convex.edges[7] = new ConvexH.HalfEdge(0, 1, 1); | ||
592 | AssertIntact(convex, planetestepsilon); | ||
593 | return convex; | ||
594 | } | ||
595 | |||
596 | public static ConvexH test_cube() | ||
597 | { | ||
598 | ConvexH convex = new ConvexH(8, 24, 6); | ||
599 | convex.vertices[0] = new float3(0, 0, 0); | ||
600 | convex.vertices[1] = new float3(0, 0, 1); | ||
601 | convex.vertices[2] = new float3(0, 1, 0); | ||
602 | convex.vertices[3] = new float3(0, 1, 1); | ||
603 | convex.vertices[4] = new float3(1, 0, 0); | ||
604 | convex.vertices[5] = new float3(1, 0, 1); | ||
605 | convex.vertices[6] = new float3(1, 1, 0); | ||
606 | convex.vertices[7] = new float3(1, 1, 1); | ||
607 | |||
608 | convex.facets[0] = new Plane(new float3(-1, 0, 0), 0); | ||
609 | convex.facets[1] = new Plane(new float3(1, 0, 0), -1); | ||
610 | convex.facets[2] = new Plane(new float3(0, -1, 0), 0); | ||
611 | convex.facets[3] = new Plane(new float3(0, 1, 0), -1); | ||
612 | convex.facets[4] = new Plane(new float3(0, 0, -1), 0); | ||
613 | convex.facets[5] = new Plane(new float3(0, 0, 1), -1); | ||
614 | |||
615 | convex.edges[0] = new ConvexH.HalfEdge(11, 0, 0); | ||
616 | convex.edges[1] = new ConvexH.HalfEdge(23, 1, 0); | ||
617 | convex.edges[2] = new ConvexH.HalfEdge(15, 3, 0); | ||
618 | convex.edges[3] = new ConvexH.HalfEdge(16, 2, 0); | ||
619 | |||
620 | convex.edges[4] = new ConvexH.HalfEdge(13, 6, 1); | ||
621 | convex.edges[5] = new ConvexH.HalfEdge(21, 7, 1); | ||
622 | convex.edges[6] = new ConvexH.HalfEdge(9, 5, 1); | ||
623 | convex.edges[7] = new ConvexH.HalfEdge(18, 4, 1); | ||
624 | |||
625 | convex.edges[8] = new ConvexH.HalfEdge(19, 0, 2); | ||
626 | convex.edges[9] = new ConvexH.HalfEdge(6, 4, 2); | ||
627 | convex.edges[10] = new ConvexH.HalfEdge(20, 5, 2); | ||
628 | convex.edges[11] = new ConvexH.HalfEdge(0, 1, 2); | ||
629 | |||
630 | convex.edges[12] = new ConvexH.HalfEdge(22, 3, 3); | ||
631 | convex.edges[13] = new ConvexH.HalfEdge(4, 7, 3); | ||
632 | convex.edges[14] = new ConvexH.HalfEdge(17, 6, 3); | ||
633 | convex.edges[15] = new ConvexH.HalfEdge(2, 2, 3); | ||
634 | |||
635 | convex.edges[16] = new ConvexH.HalfEdge(3, 0, 4); | ||
636 | convex.edges[17] = new ConvexH.HalfEdge(14, 2, 4); | ||
637 | convex.edges[18] = new ConvexH.HalfEdge(7, 6, 4); | ||
638 | convex.edges[19] = new ConvexH.HalfEdge(8, 4, 4); | ||
639 | |||
640 | convex.edges[20] = new ConvexH.HalfEdge(10, 1, 5); | ||
641 | convex.edges[21] = new ConvexH.HalfEdge(5, 5, 5); | ||
642 | convex.edges[22] = new ConvexH.HalfEdge(12, 7, 5); | ||
643 | convex.edges[23] = new ConvexH.HalfEdge(1, 3, 5); | ||
644 | |||
645 | return convex; | ||
646 | } | ||
647 | |||
648 | public static ConvexH ConvexHMakeCube(float3 bmin, float3 bmax) | ||
649 | { | ||
650 | ConvexH convex = test_cube(); | ||
651 | convex.vertices[0] = new float3(bmin.x, bmin.y, bmin.z); | ||
652 | convex.vertices[1] = new float3(bmin.x, bmin.y, bmax.z); | ||
653 | convex.vertices[2] = new float3(bmin.x, bmax.y, bmin.z); | ||
654 | convex.vertices[3] = new float3(bmin.x, bmax.y, bmax.z); | ||
655 | convex.vertices[4] = new float3(bmax.x, bmin.y, bmin.z); | ||
656 | convex.vertices[5] = new float3(bmax.x, bmin.y, bmax.z); | ||
657 | convex.vertices[6] = new float3(bmax.x, bmax.y, bmin.z); | ||
658 | convex.vertices[7] = new float3(bmax.x, bmax.y, bmax.z); | ||
659 | |||
660 | convex.facets[0] = new Plane(new float3(-1, 0, 0), bmin.x); | ||
661 | convex.facets[1] = new Plane(new float3(1, 0, 0), -bmax.x); | ||
662 | convex.facets[2] = new Plane(new float3(0, -1, 0), bmin.y); | ||
663 | convex.facets[3] = new Plane(new float3(0, 1, 0), -bmax.y); | ||
664 | convex.facets[4] = new Plane(new float3(0, 0, -1), bmin.z); | ||
665 | convex.facets[5] = new Plane(new float3(0, 0, 1), -bmax.z); | ||
666 | return convex; | ||
667 | } | ||
668 | |||
669 | public static ConvexH ConvexHCrop(ref ConvexH convex, Plane slice, float planetestepsilon) | ||
670 | { | ||
671 | int i; | ||
672 | int vertcountunder = 0; | ||
673 | int vertcountover = 0; | ||
674 | List<int> vertscoplanar = new List<int>(); // existing vertex members of convex that are coplanar | ||
675 | List<int> edgesplit = new List<int>(); // existing edges that members of convex that cross the splitplane | ||
676 | |||
677 | Debug.Assert(convex.edges.Count < 480); | ||
678 | |||
679 | EdgeFlag[] edgeflag = new EdgeFlag[512]; | ||
680 | VertFlag[] vertflag = new VertFlag[256]; | ||
681 | PlaneFlag[] planeflag = new PlaneFlag[128]; | ||
682 | ConvexH.HalfEdge[] tmpunderedges = new ConvexH.HalfEdge[512]; | ||
683 | Plane[] tmpunderplanes = new Plane[128]; | ||
684 | Coplanar[] coplanaredges = new Coplanar[512]; | ||
685 | int coplanaredges_num = 0; | ||
686 | |||
687 | List<float3> createdverts = new List<float3>(); | ||
688 | |||
689 | // do the side-of-plane tests | ||
690 | for (i = 0; i < convex.vertices.Count; i++) | ||
691 | { | ||
692 | vertflag[i].planetest = (byte)PlaneTest(slice, convex.vertices[i], planetestepsilon); | ||
693 | if (vertflag[i].planetest == (0)) | ||
694 | { | ||
695 | // ? vertscoplanar.Add(i); | ||
696 | vertflag[i].undermap = (byte)vertcountunder++; | ||
697 | vertflag[i].overmap = (byte)vertcountover++; | ||
698 | } | ||
699 | else if (vertflag[i].planetest == (1)) | ||
700 | { | ||
701 | vertflag[i].undermap = (byte)vertcountunder++; | ||
702 | } | ||
703 | else | ||
704 | { | ||
705 | Debug.Assert(vertflag[i].planetest == (2)); | ||
706 | vertflag[i].overmap = (byte)vertcountover++; | ||
707 | vertflag[i].undermap = 255; // for debugging purposes | ||
708 | } | ||
709 | } | ||
710 | int vertcountunderold = vertcountunder; // for debugging only | ||
711 | |||
712 | int under_edge_count = 0; | ||
713 | int underplanescount = 0; | ||
714 | int e0 = 0; | ||
715 | |||
716 | for (int currentplane = 0; currentplane < convex.facets.Count; currentplane++) | ||
717 | { | ||
718 | int estart = e0; | ||
719 | int enextface = 0; | ||
720 | int planeside = 0; | ||
721 | int e1 = e0 + 1; | ||
722 | int vout = -1; | ||
723 | int vin = -1; | ||
724 | int coplanaredge = -1; | ||
725 | do | ||
726 | { | ||
727 | |||
728 | if (e1 >= convex.edges.Count || convex.edges[e1].p != currentplane) | ||
729 | { | ||
730 | enextface = e1; | ||
731 | e1 = estart; | ||
732 | } | ||
733 | ConvexH.HalfEdge edge0 = convex.edges[e0]; | ||
734 | ConvexH.HalfEdge edge1 = convex.edges[e1]; | ||
735 | ConvexH.HalfEdge edgea = convex.edges[edge0.ea]; | ||
736 | |||
737 | planeside |= vertflag[edge0.v].planetest; | ||
738 | //if((vertflag[edge0.v].planetest & vertflag[edge1.v].planetest) == COPLANAR) { | ||
739 | // assert(ecop==-1); | ||
740 | // ecop=e; | ||
741 | //} | ||
742 | |||
743 | if (vertflag[edge0.v].planetest == (2) && vertflag[edge1.v].planetest == (2)) | ||
744 | { | ||
745 | // both endpoints over plane | ||
746 | edgeflag[e0].undermap = -1; | ||
747 | } | ||
748 | else if ((vertflag[edge0.v].planetest | vertflag[edge1.v].planetest) == (1)) | ||
749 | { | ||
750 | // at least one endpoint under, the other coplanar or under | ||
751 | |||
752 | edgeflag[e0].undermap = (short)under_edge_count; | ||
753 | tmpunderedges[under_edge_count].v = vertflag[edge0.v].undermap; | ||
754 | tmpunderedges[under_edge_count].p = (byte)underplanescount; | ||
755 | if (edge0.ea < e0) | ||
756 | { | ||
757 | // connect the neighbors | ||
758 | Debug.Assert(edgeflag[edge0.ea].undermap != -1); | ||
759 | tmpunderedges[under_edge_count].ea = edgeflag[edge0.ea].undermap; | ||
760 | tmpunderedges[edgeflag[edge0.ea].undermap].ea = (short)under_edge_count; | ||
761 | } | ||
762 | under_edge_count++; | ||
763 | } | ||
764 | else if ((vertflag[edge0.v].planetest | vertflag[edge1.v].planetest) == (0)) | ||
765 | { | ||
766 | // both endpoints coplanar | ||
767 | // must check a 3rd point to see if UNDER | ||
768 | int e2 = e1 + 1; | ||
769 | if (e2 >= convex.edges.Count || convex.edges[e2].p != currentplane) | ||
770 | { | ||
771 | e2 = estart; | ||
772 | } | ||
773 | Debug.Assert(convex.edges[e2].p == currentplane); | ||
774 | ConvexH.HalfEdge edge2 = convex.edges[e2]; | ||
775 | if (vertflag[edge2.v].planetest == (1)) | ||
776 | { | ||
777 | |||
778 | edgeflag[e0].undermap = (short)under_edge_count; | ||
779 | tmpunderedges[under_edge_count].v = vertflag[edge0.v].undermap; | ||
780 | tmpunderedges[under_edge_count].p = (byte)underplanescount; | ||
781 | tmpunderedges[under_edge_count].ea = -1; | ||
782 | // make sure this edge is added to the "coplanar" list | ||
783 | coplanaredge = under_edge_count; | ||
784 | vout = vertflag[edge0.v].undermap; | ||
785 | vin = vertflag[edge1.v].undermap; | ||
786 | under_edge_count++; | ||
787 | } | ||
788 | else | ||
789 | { | ||
790 | edgeflag[e0].undermap = -1; | ||
791 | } | ||
792 | } | ||
793 | else if (vertflag[edge0.v].planetest == (1) && vertflag[edge1.v].planetest == (2)) | ||
794 | { | ||
795 | // first is under 2nd is over | ||
796 | |||
797 | edgeflag[e0].undermap = (short)under_edge_count; | ||
798 | tmpunderedges[under_edge_count].v = vertflag[edge0.v].undermap; | ||
799 | tmpunderedges[under_edge_count].p = (byte)underplanescount; | ||
800 | if (edge0.ea < e0) | ||
801 | { | ||
802 | Debug.Assert(edgeflag[edge0.ea].undermap != -1); | ||
803 | // connect the neighbors | ||
804 | tmpunderedges[under_edge_count].ea = edgeflag[edge0.ea].undermap; | ||
805 | tmpunderedges[edgeflag[edge0.ea].undermap].ea = (short)under_edge_count; | ||
806 | vout = tmpunderedges[edgeflag[edge0.ea].undermap].v; | ||
807 | } | ||
808 | else | ||
809 | { | ||
810 | Plane p0 = convex.facets[edge0.p]; | ||
811 | Plane pa = convex.facets[edgea.p]; | ||
812 | createdverts.Add(ThreePlaneIntersection(p0, pa, slice)); | ||
813 | //createdverts.Add(PlaneProject(slice,PlaneLineIntersection(slice,convex.vertices[edge0.v],convex.vertices[edgea.v]))); | ||
814 | //createdverts.Add(PlaneLineIntersection(slice,convex.vertices[edge0.v],convex.vertices[edgea.v])); | ||
815 | vout = vertcountunder++; | ||
816 | } | ||
817 | under_edge_count++; | ||
818 | /// hmmm something to think about: i might be able to output this edge regarless of | ||
819 | // wheter or not we know v-in yet. ok i;ll try this now: | ||
820 | tmpunderedges[under_edge_count].v = (byte)vout; | ||
821 | tmpunderedges[under_edge_count].p = (byte)underplanescount; | ||
822 | tmpunderedges[under_edge_count].ea = -1; | ||
823 | coplanaredge = under_edge_count; | ||
824 | under_edge_count++; | ||
825 | |||
826 | if (vin != -1) | ||
827 | { | ||
828 | // we previously processed an edge where we came under | ||
829 | // now we know about vout as well | ||
830 | |||
831 | // ADD THIS EDGE TO THE LIST OF EDGES THAT NEED NEIGHBOR ON PARTITION PLANE!! | ||
832 | } | ||
833 | |||
834 | } | ||
835 | else if (vertflag[edge0.v].planetest == (0) && vertflag[edge1.v].planetest == (2)) | ||
836 | { | ||
837 | // first is coplanar 2nd is over | ||
838 | |||
839 | edgeflag[e0].undermap = -1; | ||
840 | vout = vertflag[edge0.v].undermap; | ||
841 | // I hate this but i have to make sure part of this face is UNDER before ouputting this vert | ||
842 | int k = estart; | ||
843 | Debug.Assert(edge0.p == currentplane); | ||
844 | while (!((planeside & 1) != 0) && k < convex.edges.Count && convex.edges[k].p == edge0.p) | ||
845 | { | ||
846 | planeside |= vertflag[convex.edges[k].v].planetest; | ||
847 | k++; | ||
848 | } | ||
849 | if ((planeside & 1) != 0) | ||
850 | { | ||
851 | tmpunderedges[under_edge_count].v = (byte)vout; | ||
852 | tmpunderedges[under_edge_count].p = (byte)underplanescount; | ||
853 | tmpunderedges[under_edge_count].ea = -1; | ||
854 | coplanaredge = under_edge_count; // hmmm should make a note of the edge # for later on | ||
855 | under_edge_count++; | ||
856 | |||
857 | } | ||
858 | } | ||
859 | else if (vertflag[edge0.v].planetest == (2) && vertflag[edge1.v].planetest == (1)) | ||
860 | { | ||
861 | // first is over next is under | ||
862 | // new vertex!!! | ||
863 | Debug.Assert(vin == -1); | ||
864 | if (e0 < edge0.ea) | ||
865 | { | ||
866 | Plane p0 = convex.facets[edge0.p]; | ||
867 | Plane pa = convex.facets[edgea.p]; | ||
868 | createdverts.Add(ThreePlaneIntersection(p0, pa, slice)); | ||
869 | //createdverts.Add(PlaneLineIntersection(slice,convex.vertices[edge0.v],convex.vertices[edgea.v])); | ||
870 | //createdverts.Add(PlaneProject(slice,PlaneLineIntersection(slice,convex.vertices[edge0.v],convex.vertices[edgea.v]))); | ||
871 | vin = vertcountunder++; | ||
872 | } | ||
873 | else | ||
874 | { | ||
875 | // find the new vertex that was created by edge[edge0.ea] | ||
876 | int nea = edgeflag[edge0.ea].undermap; | ||
877 | Debug.Assert(tmpunderedges[nea].p == tmpunderedges[nea + 1].p); | ||
878 | vin = tmpunderedges[nea + 1].v; | ||
879 | Debug.Assert(vin < vertcountunder); | ||
880 | Debug.Assert(vin >= vertcountunderold); // for debugging only | ||
881 | } | ||
882 | if (vout != -1) | ||
883 | { | ||
884 | // we previously processed an edge where we went over | ||
885 | // now we know vin too | ||
886 | // ADD THIS EDGE TO THE LIST OF EDGES THAT NEED NEIGHBOR ON PARTITION PLANE!! | ||
887 | } | ||
888 | // output edge | ||
889 | tmpunderedges[under_edge_count].v = (byte)vin; | ||
890 | tmpunderedges[under_edge_count].p = (byte)underplanescount; | ||
891 | edgeflag[e0].undermap = (short)under_edge_count; | ||
892 | if (e0 > edge0.ea) | ||
893 | { | ||
894 | Debug.Assert(edgeflag[edge0.ea].undermap != -1); | ||
895 | // connect the neighbors | ||
896 | tmpunderedges[under_edge_count].ea = edgeflag[edge0.ea].undermap; | ||
897 | tmpunderedges[edgeflag[edge0.ea].undermap].ea = (short)under_edge_count; | ||
898 | } | ||
899 | Debug.Assert(edgeflag[e0].undermap == under_edge_count); | ||
900 | under_edge_count++; | ||
901 | } | ||
902 | else if (vertflag[edge0.v].planetest == (2) && vertflag[edge1.v].planetest == (0)) | ||
903 | { | ||
904 | // first is over next is coplanar | ||
905 | |||
906 | edgeflag[e0].undermap = -1; | ||
907 | vin = vertflag[edge1.v].undermap; | ||
908 | Debug.Assert(vin != -1); | ||
909 | if (vout != -1) | ||
910 | { | ||
911 | // we previously processed an edge where we came under | ||
912 | // now we know both endpoints | ||
913 | // ADD THIS EDGE TO THE LIST OF EDGES THAT NEED NEIGHBOR ON PARTITION PLANE!! | ||
914 | } | ||
915 | |||
916 | } | ||
917 | else | ||
918 | { | ||
919 | Debug.Assert(false); | ||
920 | } | ||
921 | |||
922 | |||
923 | e0 = e1; | ||
924 | e1++; // do the modulo at the beginning of the loop | ||
925 | |||
926 | } while (e0 != estart); | ||
927 | e0 = enextface; | ||
928 | if ((planeside & 1) != 0) | ||
929 | { | ||
930 | planeflag[currentplane].undermap = (byte)underplanescount; | ||
931 | tmpunderplanes[underplanescount] = convex.facets[currentplane]; | ||
932 | underplanescount++; | ||
933 | } | ||
934 | else | ||
935 | { | ||
936 | planeflag[currentplane].undermap = 0; | ||
937 | } | ||
938 | if (vout >= 0 && (planeside & 1) != 0) | ||
939 | { | ||
940 | Debug.Assert(vin >= 0); | ||
941 | Debug.Assert(coplanaredge >= 0); | ||
942 | Debug.Assert(coplanaredge != 511); | ||
943 | coplanaredges[coplanaredges_num].ea = (ushort)coplanaredge; | ||
944 | coplanaredges[coplanaredges_num].v0 = (byte)vin; | ||
945 | coplanaredges[coplanaredges_num].v1 = (byte)vout; | ||
946 | coplanaredges_num++; | ||
947 | } | ||
948 | } | ||
949 | |||
950 | // add the new plane to the mix: | ||
951 | if (coplanaredges_num > 0) | ||
952 | { | ||
953 | tmpunderplanes[underplanescount++] = slice; | ||
954 | } | ||
955 | for (i = 0; i < coplanaredges_num - 1; i++) | ||
956 | { | ||
957 | if (coplanaredges[i].v1 != coplanaredges[i + 1].v0) | ||
958 | { | ||
959 | int j = 0; | ||
960 | for (j = i + 2; j < coplanaredges_num; j++) | ||
961 | { | ||
962 | if (coplanaredges[i].v1 == coplanaredges[j].v0) | ||
963 | { | ||
964 | Coplanar tmp = coplanaredges[i + 1]; | ||
965 | coplanaredges[i + 1] = coplanaredges[j]; | ||
966 | coplanaredges[j] = tmp; | ||
967 | break; | ||
968 | } | ||
969 | } | ||
970 | if (j >= coplanaredges_num) | ||
971 | { | ||
972 | Debug.Assert(j < coplanaredges_num); | ||
973 | return null; | ||
974 | } | ||
975 | } | ||
976 | } | ||
977 | |||
978 | ConvexH punder = new ConvexH(vertcountunder, under_edge_count + coplanaredges_num, underplanescount); | ||
979 | ConvexH under = punder; | ||
980 | |||
981 | { | ||
982 | int k = 0; | ||
983 | for (i = 0; i < convex.vertices.Count; i++) | ||
984 | { | ||
985 | if (vertflag[i].planetest != (2)) | ||
986 | { | ||
987 | under.vertices[k++] = convex.vertices[i]; | ||
988 | } | ||
989 | } | ||
990 | i = 0; | ||
991 | while (k < vertcountunder) | ||
992 | { | ||
993 | under.vertices[k++] = createdverts[i++]; | ||
994 | } | ||
995 | Debug.Assert(i == createdverts.Count); | ||
996 | } | ||
997 | |||
998 | for (i = 0; i < coplanaredges_num; i++) | ||
999 | { | ||
1000 | ConvexH.HalfEdge edge = under.edges[under_edge_count + i]; | ||
1001 | edge.p = (byte)(underplanescount - 1); | ||
1002 | edge.ea = (short)coplanaredges[i].ea; | ||
1003 | edge.v = (byte)coplanaredges[i].v0; | ||
1004 | under.edges[under_edge_count + i] = edge; | ||
1005 | |||
1006 | tmpunderedges[coplanaredges[i].ea].ea = (short)(under_edge_count + i); | ||
1007 | } | ||
1008 | |||
1009 | under.edges = new List<ConvexH.HalfEdge>(tmpunderedges); | ||
1010 | under.facets = new List<Plane>(tmpunderplanes); | ||
1011 | return punder; | ||
1012 | } | ||
1013 | |||
1014 | public static ConvexH ConvexHDup(ConvexH src) | ||
1015 | { | ||
1016 | ConvexH dst = new ConvexH(src.vertices.Count, src.edges.Count, src.facets.Count); | ||
1017 | dst.vertices = new List<float3>(src.vertices.Count); | ||
1018 | foreach (float3 f in src.vertices) | ||
1019 | dst.vertices.Add(new float3(f)); | ||
1020 | dst.edges = new List<ConvexH.HalfEdge>(src.edges.Count); | ||
1021 | foreach (ConvexH.HalfEdge e in src.edges) | ||
1022 | dst.edges.Add(new ConvexH.HalfEdge(e)); | ||
1023 | dst.facets = new List<Plane>(src.facets.Count); | ||
1024 | foreach (Plane p in src.facets) | ||
1025 | dst.facets.Add(new Plane(p)); | ||
1026 | return dst; | ||
1027 | } | ||
1028 | |||
1029 | public static int candidateplane(List<Plane> planes, int planes_count, ConvexH convex, float epsilon) | ||
1030 | { | ||
1031 | int p = 0; | ||
1032 | float md = 0; | ||
1033 | int i; | ||
1034 | for (i = 0; i < planes_count; i++) | ||
1035 | { | ||
1036 | float d = 0; | ||
1037 | for (int j = 0; j < convex.vertices.Count; j++) | ||
1038 | { | ||
1039 | d = Math.Max(d, float3.dot(convex.vertices[j], planes[i].normal) + planes[i].dist); | ||
1040 | } | ||
1041 | if (i == 0 || d > md) | ||
1042 | { | ||
1043 | p = i; | ||
1044 | md = d; | ||
1045 | } | ||
1046 | } | ||
1047 | return (md > epsilon) ? p : -1; | ||
1048 | } | ||
1049 | |||
1050 | public static float3 orth(float3 v) | ||
1051 | { | ||
1052 | float3 a = float3.cross(v, new float3(0f, 0f, 1f)); | ||
1053 | float3 b = float3.cross(v, new float3(0f, 1f, 0f)); | ||
1054 | return float3.normalize((float3.magnitude(a) > float3.magnitude(b)) ? a : b); | ||
1055 | } | ||
1056 | |||
1057 | public static int maxdir(List<float3> p, int count, float3 dir) | ||
1058 | { | ||
1059 | Debug.Assert(count != 0); | ||
1060 | int m = 0; | ||
1061 | float currDotm = float3.dot(p[0], dir); | ||
1062 | for (int i = 1; i < count; i++) | ||
1063 | { | ||
1064 | float currDoti = float3.dot(p[i], dir); | ||
1065 | if (currDoti > currDotm) | ||
1066 | { | ||
1067 | currDotm = currDoti; | ||
1068 | m = i; | ||
1069 | } | ||
1070 | } | ||
1071 | return m; | ||
1072 | } | ||
1073 | |||
1074 | public static int maxdirfiltered(List<float3> p, int count, float3 dir, byte[] allow) | ||
1075 | { | ||
1076 | //Debug.Assert(count != 0); | ||
1077 | int m = 0; | ||
1078 | float currDotm = float3.dot(p[0], dir); | ||
1079 | float currDoti; | ||
1080 | |||
1081 | while (allow[m] == 0) | ||
1082 | m++; | ||
1083 | |||
1084 | for (int i = 1; i < count; i++) | ||
1085 | { | ||
1086 | if (allow[i] != 0) | ||
1087 | { | ||
1088 | currDoti = float3.dot(p[i], dir); | ||
1089 | if (currDoti > currDotm) | ||
1090 | { | ||
1091 | currDotm = currDoti; | ||
1092 | m = i; | ||
1093 | } | ||
1094 | } | ||
1095 | } | ||
1096 | //Debug.Assert(m != -1); | ||
1097 | return m; | ||
1098 | } | ||
1099 | |||
1100 | public static int maxdirsterid(List<float3> p, int count, float3 dir, byte[] allow) | ||
1101 | { | ||
1102 | int m = -1; | ||
1103 | while (m == -1) | ||
1104 | { | ||
1105 | m = maxdirfiltered(p, count, dir, allow); | ||
1106 | if (allow[m] == 3) | ||
1107 | return m; | ||
1108 | float3 u = orth(dir); | ||
1109 | float3 v = float3.cross(u, dir); | ||
1110 | int ma = -1; | ||
1111 | for (float x = 0.0f; x <= 360.0f; x += 45.0f) | ||
1112 | { | ||
1113 | int mb; | ||
1114 | { | ||
1115 | float s = (float)Math.Sin((3.14159264f / 180.0f) * (x)); | ||
1116 | float c = (float)Math.Cos((3.14159264f / 180.0f) * (x)); | ||
1117 | mb = maxdirfiltered(p, count, dir + (u * s + v * c) * 0.025f, allow); | ||
1118 | } | ||
1119 | if (ma == m && mb == m) | ||
1120 | { | ||
1121 | allow[m] = 3; | ||
1122 | return m; | ||
1123 | } | ||
1124 | if (ma != -1 && ma != mb) // Yuck - this is really ugly | ||
1125 | { | ||
1126 | int mc = ma; | ||
1127 | for (float xx = x - 40.0f; xx <= x; xx += 5.0f) | ||
1128 | { | ||
1129 | float s = (float)Math.Sin((3.14159264f / 180.0f) * (xx)); | ||
1130 | float c = (float)Math.Cos((3.14159264f / 180.0f) * (xx)); | ||
1131 | int md = maxdirfiltered(p, count, dir + (u * s + v * c) * 0.025f, allow); | ||
1132 | if (mc == m && md == m) | ||
1133 | { | ||
1134 | allow[m] = 3; | ||
1135 | return m; | ||
1136 | } | ||
1137 | mc = md; | ||
1138 | } | ||
1139 | } | ||
1140 | ma = mb; | ||
1141 | } | ||
1142 | allow[m] = 0; | ||
1143 | m = -1; | ||
1144 | } | ||
1145 | |||
1146 | Debug.Assert(false); | ||
1147 | return m; | ||
1148 | } | ||
1149 | |||
1150 | public static int4 FindSimplex(List<float3> verts, byte[] allow) | ||
1151 | { | ||
1152 | float3[] basis = new float3[3]; | ||
1153 | basis[0] = new float3(0.01f, 0.02f, 1.0f); | ||
1154 | int p0 = maxdirsterid(verts, verts.Count, basis[0], allow); | ||
1155 | int p1 = maxdirsterid(verts, verts.Count, -basis[0], allow); | ||
1156 | basis[0] = verts[p0] - verts[p1]; | ||
1157 | if (p0 == p1 || basis[0] == new float3(0, 0, 0)) | ||
1158 | return new int4(-1, -1, -1, -1); | ||
1159 | basis[1] = float3.cross(new float3(1, 0.02f, 0), basis[0]); | ||
1160 | basis[2] = float3.cross(new float3(-0.02f, 1, 0), basis[0]); | ||
1161 | basis[1] = float3.normalize((float3.magnitude(basis[1]) > float3.magnitude(basis[2])) ? basis[1] : basis[2]); | ||
1162 | int p2 = maxdirsterid(verts, verts.Count, basis[1], allow); | ||
1163 | if (p2 == p0 || p2 == p1) | ||
1164 | { | ||
1165 | p2 = maxdirsterid(verts, verts.Count, -basis[1], allow); | ||
1166 | } | ||
1167 | if (p2 == p0 || p2 == p1) | ||
1168 | return new int4(-1, -1, -1, -1); | ||
1169 | basis[1] = verts[p2] - verts[p0]; | ||
1170 | basis[2] = float3.normalize(float3.cross(basis[1], basis[0])); | ||
1171 | int p3 = maxdirsterid(verts, verts.Count, basis[2], allow); | ||
1172 | if (p3 == p0 || p3 == p1 || p3 == p2) | ||
1173 | p3 = maxdirsterid(verts, verts.Count, -basis[2], allow); | ||
1174 | if (p3 == p0 || p3 == p1 || p3 == p2) | ||
1175 | return new int4(-1, -1, -1, -1); | ||
1176 | Debug.Assert(!(p0 == p1 || p0 == p2 || p0 == p3 || p1 == p2 || p1 == p3 || p2 == p3)); | ||
1177 | if (float3.dot(verts[p3] - verts[p0], float3.cross(verts[p1] - verts[p0], verts[p2] - verts[p0])) < 0) | ||
1178 | { | ||
1179 | Swap(ref p2, ref p3); | ||
1180 | } | ||
1181 | return new int4(p0, p1, p2, p3); | ||
1182 | } | ||
1183 | |||
1184 | public static float GetDist(float px, float py, float pz, float3 p2) | ||
1185 | { | ||
1186 | float dx = px - p2.x; | ||
1187 | float dy = py - p2.y; | ||
1188 | float dz = pz - p2.z; | ||
1189 | |||
1190 | return dx * dx + dy * dy + dz * dz; | ||
1191 | } | ||
1192 | |||
1193 | public static void ReleaseHull(PHullResult result) | ||
1194 | { | ||
1195 | if (result.Indices != null) | ||
1196 | result.Indices = null; | ||
1197 | if (result.Vertices != null) | ||
1198 | result.Vertices = null; | ||
1199 | } | ||
1200 | |||
1201 | public static int calchullgen(List<float3> verts, int vlimit, List<HullTriangle> tris) | ||
1202 | { | ||
1203 | if (verts.Count < 4) | ||
1204 | return 0; | ||
1205 | if (vlimit == 0) | ||
1206 | vlimit = 1000000000; | ||
1207 | int j; | ||
1208 | float3 bmin = new float3(verts[0]); | ||
1209 | float3 bmax = new float3(verts[0]); | ||
1210 | List<int> isextreme = new List<int>(verts.Count); | ||
1211 | byte[] allow = new byte[verts.Count]; | ||
1212 | for (j = 0; j < verts.Count; j++) | ||
1213 | { | ||
1214 | allow[j] = 1; | ||
1215 | isextreme.Add(0); | ||
1216 | bmin = float3.VectorMin(bmin, verts[j]); | ||
1217 | bmax = float3.VectorMax(bmax, verts[j]); | ||
1218 | } | ||
1219 | float epsilon = float3.magnitude(bmax - bmin) * 0.001f; | ||
1220 | |||
1221 | int4 p = FindSimplex(verts, allow); | ||
1222 | if (p.x == -1) // simplex failed | ||
1223 | return 0; | ||
1224 | |||
1225 | float3 center = (verts[p[0]] + verts[p[1]] + verts[p[2]] + verts[p[3]]) / 4.0f; // a valid interior point | ||
1226 | HullTriangle t0 = new HullTriangle(p[2], p[3], p[1], tris); | ||
1227 | t0.n = new int3(2, 3, 1); | ||
1228 | HullTriangle t1 = new HullTriangle(p[3], p[2], p[0], tris); | ||
1229 | t1.n = new int3(3, 2, 0); | ||
1230 | HullTriangle t2 = new HullTriangle(p[0], p[1], p[3], tris); | ||
1231 | t2.n = new int3(0, 1, 3); | ||
1232 | HullTriangle t3 = new HullTriangle(p[1], p[0], p[2], tris); | ||
1233 | t3.n = new int3(1, 0, 2); | ||
1234 | isextreme[p[0]] = isextreme[p[1]] = isextreme[p[2]] = isextreme[p[3]] = 1; | ||
1235 | checkit(t0, tris); | ||
1236 | checkit(t1, tris); | ||
1237 | checkit(t2, tris); | ||
1238 | checkit(t3, tris); | ||
1239 | |||
1240 | for (j = 0; j < tris.Count; j++) | ||
1241 | { | ||
1242 | HullTriangle t = tris[j]; | ||
1243 | Debug.Assert((object)t != null); | ||
1244 | Debug.Assert(t.vmax < 0); | ||
1245 | float3 n = TriNormal(verts[(t)[0]], verts[(t)[1]], verts[(t)[2]]); | ||
1246 | t.vmax = maxdirsterid(verts, verts.Count, n, allow); | ||
1247 | t.rise = float3.dot(n, verts[t.vmax] - verts[(t)[0]]); | ||
1248 | } | ||
1249 | HullTriangle te; | ||
1250 | vlimit -= 4; | ||
1251 | while (vlimit > 0 && (te = extrudable(epsilon, tris)) != null) | ||
1252 | { | ||
1253 | int3 ti = te; | ||
1254 | int v = te.vmax; | ||
1255 | Debug.Assert(isextreme[v] == 0); // wtf we've already done this vertex | ||
1256 | isextreme[v] = 1; | ||
1257 | //if(v==p0 || v==p1 || v==p2 || v==p3) continue; // done these already | ||
1258 | j = tris.Count; | ||
1259 | while (j-- != 0) | ||
1260 | { | ||
1261 | if (tris.Count <= j || (object)tris[j] == null) | ||
1262 | continue; | ||
1263 | int3 t = tris[j]; | ||
1264 | if (above(verts, t, verts[v], 0.01f * epsilon)) | ||
1265 | { | ||
1266 | extrude(tris[j], v, tris); | ||
1267 | } | ||
1268 | } | ||
1269 | // now check for those degenerate cases where we have a flipped triangle or a really skinny triangle | ||
1270 | j = tris.Count; | ||
1271 | while (j-- != 0) | ||
1272 | { | ||
1273 | if (tris.Count <= j || (object)tris[j] == null) | ||
1274 | continue; | ||
1275 | if (!hasvert(tris[j], v)) | ||
1276 | break; | ||
1277 | int3 nt = tris[j]; | ||
1278 | if (above(verts, nt, center, 0.01f * epsilon) || float3.magnitude(float3.cross(verts[nt[1]] - verts[nt[0]], verts[nt[2]] - verts[nt[1]])) < epsilon * epsilon * 0.1f) | ||
1279 | { | ||
1280 | HullTriangle nb = tris[tris[j].n[0]]; | ||
1281 | Debug.Assert(nb != null); | ||
1282 | Debug.Assert(!hasvert(nb, v)); | ||
1283 | Debug.Assert(nb.id < j); | ||
1284 | extrude(nb, v, tris); | ||
1285 | j = tris.Count; | ||
1286 | } | ||
1287 | } | ||
1288 | j = tris.Count; | ||
1289 | while (j-- != 0) | ||
1290 | { | ||
1291 | HullTriangle t = tris[j]; | ||
1292 | if (t == null) | ||
1293 | continue; | ||
1294 | if (t.vmax >= 0) | ||
1295 | break; | ||
1296 | float3 n = TriNormal(verts[(t)[0]], verts[(t)[1]], verts[(t)[2]]); | ||
1297 | t.vmax = maxdirsterid(verts, verts.Count, n, allow); | ||
1298 | if (isextreme[t.vmax] != 0) | ||
1299 | { | ||
1300 | t.vmax = -1; // already done that vertex - algorithm needs to be able to terminate. | ||
1301 | } | ||
1302 | else | ||
1303 | { | ||
1304 | t.rise = float3.dot(n, verts[t.vmax] - verts[(t)[0]]); | ||
1305 | } | ||
1306 | } | ||
1307 | vlimit--; | ||
1308 | } | ||
1309 | return 1; | ||
1310 | } | ||
1311 | |||
1312 | public static bool calchull(List<float3> verts, out List<int> tris_out, int vlimit, List<HullTriangle> tris) | ||
1313 | { | ||
1314 | tris_out = null; | ||
1315 | |||
1316 | int rc = calchullgen(verts, vlimit, tris); | ||
1317 | if (rc == 0) | ||
1318 | return false; | ||
1319 | List<int> ts = new List<int>(); | ||
1320 | for (int i = 0; i < tris.Count; i++) | ||
1321 | { | ||
1322 | if ((object)tris[i] != null) | ||
1323 | { | ||
1324 | for (int j = 0; j < 3; j++) | ||
1325 | ts.Add((tris[i])[j]); | ||
1326 | tris[i] = null; | ||
1327 | } | ||
1328 | } | ||
1329 | |||
1330 | tris_out = ts; | ||
1331 | tris.Clear(); | ||
1332 | return true; | ||
1333 | } | ||
1334 | |||
1335 | public static int calchullpbev(List<float3> verts, int vlimit, out List<Plane> planes, float bevangle, List<HullTriangle> tris) | ||
1336 | { | ||
1337 | int i; | ||
1338 | int j; | ||
1339 | planes = new List<Plane>(); | ||
1340 | int rc = calchullgen(verts, vlimit, tris); | ||
1341 | if (rc == 0) | ||
1342 | return 0; | ||
1343 | for (i = 0; i < tris.Count; i++) | ||
1344 | { | ||
1345 | if (tris[i] != null) | ||
1346 | { | ||
1347 | Plane p = new Plane(); | ||
1348 | HullTriangle t = tris[i]; | ||
1349 | p.normal = TriNormal(verts[(t)[0]], verts[(t)[1]], verts[(t)[2]]); | ||
1350 | p.dist = -float3.dot(p.normal, verts[(t)[0]]); | ||
1351 | planes.Add(p); | ||
1352 | for (j = 0; j < 3; j++) | ||
1353 | { | ||
1354 | if (t.n[j] < t.id) | ||
1355 | continue; | ||
1356 | HullTriangle s = tris[t.n[j]]; | ||
1357 | float3 snormal = TriNormal(verts[(s)[0]], verts[(s)[1]], verts[(s)[2]]); | ||
1358 | if (float3.dot(snormal, p.normal) >= Math.Cos(bevangle * (3.14159264f / 180.0f))) | ||
1359 | continue; | ||
1360 | float3 n = float3.normalize(snormal + p.normal); | ||
1361 | planes.Add(new Plane(n, -float3.dot(n, verts[maxdir(verts, verts.Count, n)]))); | ||
1362 | } | ||
1363 | } | ||
1364 | } | ||
1365 | |||
1366 | tris.Clear(); | ||
1367 | return 1; | ||
1368 | } | ||
1369 | |||
1370 | public static int overhull(List<Plane> planes, List<float3> verts, int maxplanes, out List<float3> verts_out, out List<int> faces_out, float inflate) | ||
1371 | { | ||
1372 | verts_out = null; | ||
1373 | faces_out = null; | ||
1374 | |||
1375 | int i; | ||
1376 | int j; | ||
1377 | if (verts.Count < 4) | ||
1378 | return 0; | ||
1379 | maxplanes = Math.Min(maxplanes, planes.Count); | ||
1380 | float3 bmin = new float3(verts[0]); | ||
1381 | float3 bmax = new float3(verts[0]); | ||
1382 | for (i = 0; i < verts.Count; i++) | ||
1383 | { | ||
1384 | bmin = float3.VectorMin(bmin, verts[i]); | ||
1385 | bmax = float3.VectorMax(bmax, verts[i]); | ||
1386 | } | ||
1387 | // float diameter = magnitude(bmax-bmin); | ||
1388 | // inflate *=diameter; // RELATIVE INFLATION | ||
1389 | bmin -= new float3(inflate, inflate, inflate); | ||
1390 | bmax += new float3(inflate, inflate, inflate); | ||
1391 | for (i = 0; i < planes.Count; i++) | ||
1392 | { | ||
1393 | planes[i].dist -= inflate; | ||
1394 | } | ||
1395 | float3 emin = new float3(bmin); | ||
1396 | float3 emax = new float3(bmax); | ||
1397 | float epsilon = float3.magnitude(emax - emin) * 0.025f; | ||
1398 | float planetestepsilon = float3.magnitude(emax - emin) * (0.001f); | ||
1399 | // todo: add bounding cube planes to force bevel. or try instead not adding the diameter expansion ??? must think. | ||
1400 | // ConvexH *convex = ConvexHMakeCube(bmin - float3(diameter,diameter,diameter),bmax+float3(diameter,diameter,diameter)); | ||
1401 | ConvexH c = ConvexHMakeCube(new float3(bmin), new float3(bmax)); | ||
1402 | int k; | ||
1403 | while (maxplanes-- != 0 && (k = candidateplane(planes, planes.Count, c, epsilon)) >= 0) | ||
1404 | { | ||
1405 | ConvexH tmp = c; | ||
1406 | c = ConvexHCrop(ref tmp, planes[k], planetestepsilon); | ||
1407 | if (c == null) // might want to debug this case better!!! | ||
1408 | { | ||
1409 | c = tmp; | ||
1410 | break; | ||
1411 | } | ||
1412 | if (AssertIntact(c, planetestepsilon) == false) // might want to debug this case better too!!! | ||
1413 | { | ||
1414 | c = tmp; | ||
1415 | break; | ||
1416 | } | ||
1417 | tmp.edges = null; | ||
1418 | tmp.facets = null; | ||
1419 | tmp.vertices = null; | ||
1420 | } | ||
1421 | |||
1422 | Debug.Assert(AssertIntact(c, planetestepsilon)); | ||
1423 | //return c; | ||
1424 | //C++ TO C# CONVERTER TODO TASK: The memory management function 'malloc' has no equivalent in C#: | ||
1425 | faces_out = new List<int>(); //(int)malloc(sizeof(int) * (1 + c.facets.Count + c.edges.Count)); // new int[1+c->facets.count+c->edges.count]; | ||
1426 | int faces_count_out = 0; | ||
1427 | i = 0; | ||
1428 | faces_out[faces_count_out++] = -1; | ||
1429 | k = 0; | ||
1430 | while (i < c.edges.Count) | ||
1431 | { | ||
1432 | j = 1; | ||
1433 | while (j + i < c.edges.Count && c.edges[i].p == c.edges[i + j].p) | ||
1434 | { | ||
1435 | j++; | ||
1436 | } | ||
1437 | faces_out[faces_count_out++] = j; | ||
1438 | while (j-- != 0) | ||
1439 | { | ||
1440 | faces_out[faces_count_out++] = c.edges[i].v; | ||
1441 | i++; | ||
1442 | } | ||
1443 | k++; | ||
1444 | } | ||
1445 | faces_out[0] = k; // number of faces. | ||
1446 | Debug.Assert(k == c.facets.Count); | ||
1447 | Debug.Assert(faces_count_out == 1 + c.facets.Count + c.edges.Count); | ||
1448 | verts_out = c.vertices; // new float3[c->vertices.count]; | ||
1449 | int verts_count_out = c.vertices.Count; | ||
1450 | for (i = 0; i < c.vertices.Count; i++) | ||
1451 | { | ||
1452 | verts_out[i] = new float3(c.vertices[i]); | ||
1453 | } | ||
1454 | |||
1455 | c.edges = null; | ||
1456 | c.facets = null; | ||
1457 | c.vertices = null; | ||
1458 | return 1; | ||
1459 | } | ||
1460 | |||
1461 | public static int overhullv(List<float3> verts, int maxplanes, out List<float3> verts_out, out List<int> faces_out, float inflate, float bevangle, int vlimit, List<HullTriangle> tris) | ||
1462 | { | ||
1463 | verts_out = null; | ||
1464 | faces_out = null; | ||
1465 | |||
1466 | if (verts.Count == 0) | ||
1467 | return 0; | ||
1468 | List<Plane> planes = new List<Plane>(); | ||
1469 | int rc = calchullpbev(verts, vlimit, out planes, bevangle, tris); | ||
1470 | if (rc == 0) | ||
1471 | return 0; | ||
1472 | return overhull(planes, verts, maxplanes, out verts_out, out faces_out, inflate); | ||
1473 | } | ||
1474 | |||
1475 | public static void addPoint(ref uint vcount, List<float3> p, float x, float y, float z) | ||
1476 | { | ||
1477 | p.Add(new float3(x, y, z)); | ||
1478 | vcount++; | ||
1479 | } | ||
1480 | |||
1481 | public static bool ComputeHull(List<float3> vertices, ref PHullResult result, int vlimit, float inflate) | ||
1482 | { | ||
1483 | List<HullTriangle> tris = new List<HullTriangle>(); | ||
1484 | List<int> faces; | ||
1485 | List<float3> verts_out; | ||
1486 | |||
1487 | if (inflate == 0.0f) | ||
1488 | { | ||
1489 | List<int> tris_out; | ||
1490 | bool ret = calchull(vertices, out tris_out, vlimit, tris); | ||
1491 | if (ret == false) | ||
1492 | return false; | ||
1493 | |||
1494 | result.Indices = tris_out; | ||
1495 | result.Vertices = vertices; | ||
1496 | return true; | ||
1497 | } | ||
1498 | else | ||
1499 | { | ||
1500 | int ret = overhullv(vertices, 35, out verts_out, out faces, inflate, 120.0f, vlimit, tris); | ||
1501 | if (ret == 0) | ||
1502 | return false; | ||
1503 | |||
1504 | List<int3> tris2 = new List<int3>(); | ||
1505 | int n = faces[0]; | ||
1506 | int k = 1; | ||
1507 | for (int i = 0; i < n; i++) | ||
1508 | { | ||
1509 | int pn = faces[k++]; | ||
1510 | for (int j = 2; j < pn; j++) | ||
1511 | tris2.Add(new int3(faces[k], faces[k + j - 1], faces[k + j])); | ||
1512 | k += pn; | ||
1513 | } | ||
1514 | Debug.Assert(tris2.Count == faces.Count - 1 - (n * 3)); | ||
1515 | |||
1516 | result.Indices = new List<int>(tris2.Count * 3); | ||
1517 | for (int i = 0; i < tris2.Count; i++) | ||
1518 | { | ||
1519 | result.Indices.Add(tris2[i].x); | ||
1520 | result.Indices.Add(tris2[i].y); | ||
1521 | result.Indices.Add(tris2[i].z); | ||
1522 | } | ||
1523 | result.Vertices = verts_out; | ||
1524 | |||
1525 | return true; | ||
1526 | } | ||
1527 | } | ||
1528 | |||
1529 | private static bool CleanupVertices(List<float3> svertices, out List<float3> vertices, float normalepsilon, out float3 scale) | ||
1530 | { | ||
1531 | const float EPSILON = 0.000001f; | ||
1532 | |||
1533 | vertices = new List<float3>(); | ||
1534 | scale = new float3(1f, 1f, 1f); | ||
1535 | |||
1536 | if (svertices.Count == 0) | ||
1537 | return false; | ||
1538 | |||
1539 | uint vcount = 0; | ||
1540 | |||
1541 | float[] recip = new float[3]; | ||
1542 | |||
1543 | float[] bmin = { Single.MaxValue, Single.MaxValue, Single.MaxValue }; | ||
1544 | float[] bmax = { Single.MinValue, Single.MinValue, Single.MinValue }; | ||
1545 | |||
1546 | for (int i = 0; i < svertices.Count; i++) | ||
1547 | { | ||
1548 | float3 p = svertices[i]; | ||
1549 | |||
1550 | for (int j = 0; j < 3; j++) | ||
1551 | { | ||
1552 | if (p[j] < bmin[j]) | ||
1553 | bmin[j] = p[j]; | ||
1554 | if (p[j] > bmax[j]) | ||
1555 | bmax[j] = p[j]; | ||
1556 | } | ||
1557 | } | ||
1558 | |||
1559 | float dx = bmax[0] - bmin[0]; | ||
1560 | float dy = bmax[1] - bmin[1]; | ||
1561 | float dz = bmax[2] - bmin[2]; | ||
1562 | |||
1563 | float3 center = new float3(); | ||
1564 | |||
1565 | center.x = dx * 0.5f + bmin[0]; | ||
1566 | center.y = dy * 0.5f + bmin[1]; | ||
1567 | center.z = dz * 0.5f + bmin[2]; | ||
1568 | |||
1569 | if (dx < EPSILON || dy < EPSILON || dz < EPSILON || svertices.Count < 3) | ||
1570 | { | ||
1571 | float len = Single.MaxValue; | ||
1572 | |||
1573 | if (dx > EPSILON && dx < len) | ||
1574 | len = dx; | ||
1575 | if (dy > EPSILON && dy < len) | ||
1576 | len = dy; | ||
1577 | if (dz > EPSILON && dz < len) | ||
1578 | len = dz; | ||
1579 | |||
1580 | if (len == Single.MaxValue) | ||
1581 | { | ||
1582 | dx = dy = dz = 0.01f; // one centimeter | ||
1583 | } | ||
1584 | else | ||
1585 | { | ||
1586 | if (dx < EPSILON) // 1/5th the shortest non-zero edge. | ||
1587 | dx = len * 0.05f; | ||
1588 | if (dy < EPSILON) | ||
1589 | dy = len * 0.05f; | ||
1590 | if (dz < EPSILON) | ||
1591 | dz = len * 0.05f; | ||
1592 | } | ||
1593 | |||
1594 | float x1 = center[0] - dx; | ||
1595 | float x2 = center[0] + dx; | ||
1596 | |||
1597 | float y1 = center[1] - dy; | ||
1598 | float y2 = center[1] + dy; | ||
1599 | |||
1600 | float z1 = center[2] - dz; | ||
1601 | float z2 = center[2] + dz; | ||
1602 | |||
1603 | addPoint(ref vcount, vertices, x1, y1, z1); | ||
1604 | addPoint(ref vcount, vertices, x2, y1, z1); | ||
1605 | addPoint(ref vcount, vertices, x2, y2, z1); | ||
1606 | addPoint(ref vcount, vertices, x1, y2, z1); | ||
1607 | addPoint(ref vcount, vertices, x1, y1, z2); | ||
1608 | addPoint(ref vcount, vertices, x2, y1, z2); | ||
1609 | addPoint(ref vcount, vertices, x2, y2, z2); | ||
1610 | addPoint(ref vcount, vertices, x1, y2, z2); | ||
1611 | |||
1612 | return true; // return cube | ||
1613 | } | ||
1614 | else | ||
1615 | { | ||
1616 | scale.x = dx; | ||
1617 | scale.y = dy; | ||
1618 | scale.z = dz; | ||
1619 | |||
1620 | recip[0] = 1f / dx; | ||
1621 | recip[1] = 1f / dy; | ||
1622 | recip[2] = 1f / dz; | ||
1623 | |||
1624 | center.x *= recip[0]; | ||
1625 | center.y *= recip[1]; | ||
1626 | center.z *= recip[2]; | ||
1627 | } | ||
1628 | |||
1629 | for (int i = 0; i < svertices.Count; i++) | ||
1630 | { | ||
1631 | float3 p = svertices[i]; | ||
1632 | |||
1633 | float px = p[0]; | ||
1634 | float py = p[1]; | ||
1635 | float pz = p[2]; | ||
1636 | |||
1637 | px = px * recip[0]; // normalize | ||
1638 | py = py * recip[1]; // normalize | ||
1639 | pz = pz * recip[2]; // normalize | ||
1640 | |||
1641 | if (true) | ||
1642 | { | ||
1643 | int j; | ||
1644 | |||
1645 | for (j = 0; j < vcount; j++) | ||
1646 | { | ||
1647 | float3 v = vertices[j]; | ||
1648 | |||
1649 | float x = v[0]; | ||
1650 | float y = v[1]; | ||
1651 | float z = v[2]; | ||
1652 | |||
1653 | float dx1 = Math.Abs(x - px); | ||
1654 | float dy1 = Math.Abs(y - py); | ||
1655 | float dz1 = Math.Abs(z - pz); | ||
1656 | |||
1657 | if (dx1 < normalepsilon && dy1 < normalepsilon && dz1 < normalepsilon) | ||
1658 | { | ||
1659 | // ok, it is close enough to the old one | ||
1660 | // now let us see if it is further from the center of the point cloud than the one we already recorded. | ||
1661 | // in which case we keep this one instead. | ||
1662 | float dist1 = GetDist(px, py, pz, center); | ||
1663 | float dist2 = GetDist(v[0], v[1], v[2], center); | ||
1664 | |||
1665 | if (dist1 > dist2) | ||
1666 | { | ||
1667 | v.x = px; | ||
1668 | v.y = py; | ||
1669 | v.z = pz; | ||
1670 | } | ||
1671 | |||
1672 | break; | ||
1673 | } | ||
1674 | } | ||
1675 | |||
1676 | if (j == vcount) | ||
1677 | { | ||
1678 | float3 dest = new float3(px, py, pz); | ||
1679 | vertices.Add(dest); | ||
1680 | vcount++; | ||
1681 | } | ||
1682 | } | ||
1683 | } | ||
1684 | |||
1685 | // ok..now make sure we didn't prune so many vertices it is now invalid. | ||
1686 | if (true) | ||
1687 | { | ||
1688 | float[] bmin2 = { Single.MaxValue, Single.MaxValue, Single.MaxValue }; | ||
1689 | float[] bmax2 = { Single.MinValue, Single.MinValue, Single.MinValue }; | ||
1690 | |||
1691 | for (int i = 0; i < vcount; i++) | ||
1692 | { | ||
1693 | float3 p = vertices[i]; | ||
1694 | for (int j = 0; j < 3; j++) | ||
1695 | { | ||
1696 | if (p[j] < bmin2[j]) | ||
1697 | bmin2[j] = p[j]; | ||
1698 | if (p[j] > bmax2[j]) | ||
1699 | bmax2[j] = p[j]; | ||
1700 | } | ||
1701 | } | ||
1702 | |||
1703 | float dx2 = bmax2[0] - bmin2[0]; | ||
1704 | float dy2 = bmax2[1] - bmin2[1]; | ||
1705 | float dz2 = bmax2[2] - bmin2[2]; | ||
1706 | |||
1707 | if (dx2 < EPSILON || dy2 < EPSILON || dz2 < EPSILON || vcount < 3) | ||
1708 | { | ||
1709 | float cx = dx2 * 0.5f + bmin2[0]; | ||
1710 | float cy = dy2 * 0.5f + bmin2[1]; | ||
1711 | float cz = dz2 * 0.5f + bmin2[2]; | ||
1712 | |||
1713 | float len = Single.MaxValue; | ||
1714 | |||
1715 | if (dx2 >= EPSILON && dx2 < len) | ||
1716 | len = dx2; | ||
1717 | if (dy2 >= EPSILON && dy2 < len) | ||
1718 | len = dy2; | ||
1719 | if (dz2 >= EPSILON && dz2 < len) | ||
1720 | len = dz2; | ||
1721 | |||
1722 | if (len == Single.MaxValue) | ||
1723 | { | ||
1724 | dx2 = dy2 = dz2 = 0.01f; // one centimeter | ||
1725 | } | ||
1726 | else | ||
1727 | { | ||
1728 | if (dx2 < EPSILON) // 1/5th the shortest non-zero edge. | ||
1729 | dx2 = len * 0.05f; | ||
1730 | if (dy2 < EPSILON) | ||
1731 | dy2 = len * 0.05f; | ||
1732 | if (dz2 < EPSILON) | ||
1733 | dz2 = len * 0.05f; | ||
1734 | } | ||
1735 | |||
1736 | float x1 = cx - dx2; | ||
1737 | float x2 = cx + dx2; | ||
1738 | |||
1739 | float y1 = cy - dy2; | ||
1740 | float y2 = cy + dy2; | ||
1741 | |||
1742 | float z1 = cz - dz2; | ||
1743 | float z2 = cz + dz2; | ||
1744 | |||
1745 | vcount = 0; // add box | ||
1746 | |||
1747 | addPoint(ref vcount, vertices, x1, y1, z1); | ||
1748 | addPoint(ref vcount, vertices, x2, y1, z1); | ||
1749 | addPoint(ref vcount, vertices, x2, y2, z1); | ||
1750 | addPoint(ref vcount, vertices, x1, y2, z1); | ||
1751 | addPoint(ref vcount, vertices, x1, y1, z2); | ||
1752 | addPoint(ref vcount, vertices, x2, y1, z2); | ||
1753 | addPoint(ref vcount, vertices, x2, y2, z2); | ||
1754 | addPoint(ref vcount, vertices, x1, y2, z2); | ||
1755 | |||
1756 | return true; | ||
1757 | } | ||
1758 | } | ||
1759 | |||
1760 | return true; | ||
1761 | } | ||
1762 | |||
1763 | private static void BringOutYourDead(List<float3> verts, out List<float3> overts, List<int> indices) | ||
1764 | { | ||
1765 | int[] used = new int[verts.Count]; | ||
1766 | int ocount = 0; | ||
1767 | |||
1768 | overts = new List<float3>(); | ||
1769 | |||
1770 | for (int i = 0; i < indices.Count; i++) | ||
1771 | { | ||
1772 | int v = indices[i]; // original array index | ||
1773 | |||
1774 | Debug.Assert(v >= 0 && v < verts.Count); | ||
1775 | |||
1776 | if (used[v] != 0) // if already remapped | ||
1777 | { | ||
1778 | indices[i] = used[v] - 1; // index to new array | ||
1779 | } | ||
1780 | else | ||
1781 | { | ||
1782 | indices[i] = ocount; // new index mapping | ||
1783 | |||
1784 | overts.Add(verts[v]); // copy old vert to new vert array | ||
1785 | |||
1786 | ocount++; // increment output vert count | ||
1787 | |||
1788 | Debug.Assert(ocount >= 0 && ocount <= verts.Count); | ||
1789 | |||
1790 | used[v] = ocount; // assign new index remapping | ||
1791 | } | ||
1792 | } | ||
1793 | } | ||
1794 | |||
1795 | public static HullError CreateConvexHull(HullDesc desc, ref HullResult result) | ||
1796 | { | ||
1797 | HullError ret = HullError.QE_FAIL; | ||
1798 | |||
1799 | PHullResult hr = new PHullResult(); | ||
1800 | |||
1801 | uint vcount = (uint)desc.Vertices.Count; | ||
1802 | if (vcount < 8) | ||
1803 | vcount = 8; | ||
1804 | |||
1805 | List<float3> vsource; | ||
1806 | float3 scale = new float3(); | ||
1807 | |||
1808 | bool ok = CleanupVertices(desc.Vertices, out vsource, desc.NormalEpsilon, out scale); // normalize point cloud, remove duplicates! | ||
1809 | |||
1810 | if (ok) | ||
1811 | { | ||
1812 | if (true) // scale vertices back to their original size. | ||
1813 | { | ||
1814 | for (int i = 0; i < vsource.Count; i++) | ||
1815 | { | ||
1816 | float3 v = vsource[i]; | ||
1817 | v.x *= scale[0]; | ||
1818 | v.y *= scale[1]; | ||
1819 | v.z *= scale[2]; | ||
1820 | } | ||
1821 | } | ||
1822 | |||
1823 | float skinwidth = 0; | ||
1824 | if (desc.HasHullFlag(HullFlag.QF_SKIN_WIDTH)) | ||
1825 | skinwidth = desc.SkinWidth; | ||
1826 | |||
1827 | ok = ComputeHull(vsource, ref hr, (int)desc.MaxVertices, skinwidth); | ||
1828 | |||
1829 | if (ok) | ||
1830 | { | ||
1831 | List<float3> vscratch; | ||
1832 | BringOutYourDead(hr.Vertices, out vscratch, hr.Indices); | ||
1833 | |||
1834 | ret = HullError.QE_OK; | ||
1835 | |||
1836 | if (desc.HasHullFlag(HullFlag.QF_TRIANGLES)) // if he wants the results as triangle! | ||
1837 | { | ||
1838 | result.Polygons = false; | ||
1839 | result.Indices = hr.Indices; | ||
1840 | result.OutputVertices = vscratch; | ||
1841 | } | ||
1842 | else | ||
1843 | { | ||
1844 | result.Polygons = true; | ||
1845 | result.OutputVertices = vscratch; | ||
1846 | |||
1847 | if (true) | ||
1848 | { | ||
1849 | List<int> source = hr.Indices; | ||
1850 | List<int> dest = new List<int>(); | ||
1851 | for (int i = 0; i < hr.Indices.Count / 3; i++) | ||
1852 | { | ||
1853 | dest.Add(3); | ||
1854 | dest.Add(source[i * 3 + 0]); | ||
1855 | dest.Add(source[i * 3 + 1]); | ||
1856 | dest.Add(source[i * 3 + 2]); | ||
1857 | } | ||
1858 | |||
1859 | result.Indices = dest; | ||
1860 | } | ||
1861 | } | ||
1862 | } | ||
1863 | } | ||
1864 | |||
1865 | return ret; | ||
1866 | } | ||
1867 | } | ||
1868 | } | ||
diff --git a/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/LICENSE.txt b/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/LICENSE.txt new file mode 100644 index 0000000..714ae89 --- /dev/null +++ b/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/LICENSE.txt | |||
@@ -0,0 +1,28 @@ | |||
1 | ConvexDecompositionDotNet | ||
2 | ------------------------- | ||
3 | |||
4 | The MIT License | ||
5 | |||
6 | Copyright (c) 2010 Intel Corporation. | ||
7 | All rights reserved. | ||
8 | |||
9 | Based on the convexdecomposition library from | ||
10 | <http://codesuppository.googlecode.com> by John W. Ratcliff and Stan Melax. | ||
11 | |||
12 | Permission is hereby granted, free of charge, to any person obtaining a copy | ||
13 | of this software and associated documentation files (the "Software"), to deal | ||
14 | in the Software without restriction, including without limitation the rights | ||
15 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell | ||
16 | copies of the Software, and to permit persons to whom the Software is | ||
17 | furnished to do so, subject to the following conditions: | ||
18 | |||
19 | The above copyright notice and this permission notice shall be included in | ||
20 | all copies or substantial portions of the Software. | ||
21 | |||
22 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR | ||
23 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, | ||
24 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE | ||
25 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER | ||
26 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, | ||
27 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN | ||
28 | THE SOFTWARE. | ||
diff --git a/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/Plane.cs b/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/Plane.cs new file mode 100644 index 0000000..d099676 --- /dev/null +++ b/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/Plane.cs | |||
@@ -0,0 +1,99 @@ | |||
1 | /* The MIT License | ||
2 | * | ||
3 | * Copyright (c) 2010 Intel Corporation. | ||
4 | * All rights reserved. | ||
5 | * | ||
6 | * Based on the convexdecomposition library from | ||
7 | * <http://codesuppository.googlecode.com> by John W. Ratcliff and Stan Melax. | ||
8 | * | ||
9 | * Permission is hereby granted, free of charge, to any person obtaining a copy | ||
10 | * of this software and associated documentation files (the "Software"), to deal | ||
11 | * in the Software without restriction, including without limitation the rights | ||
12 | * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell | ||
13 | * copies of the Software, and to permit persons to whom the Software is | ||
14 | * furnished to do so, subject to the following conditions: | ||
15 | * | ||
16 | * The above copyright notice and this permission notice shall be included in | ||
17 | * all copies or substantial portions of the Software. | ||
18 | * | ||
19 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR | ||
20 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, | ||
21 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE | ||
22 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER | ||
23 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, | ||
24 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN | ||
25 | * THE SOFTWARE. | ||
26 | */ | ||
27 | |||
28 | using System; | ||
29 | |||
30 | namespace OpenSim.Region.Physics.ConvexDecompositionDotNet | ||
31 | { | ||
32 | public class Plane | ||
33 | { | ||
34 | public float3 normal = new float3(); | ||
35 | public float dist; // distance below origin - the D from plane equasion Ax+By+Cz+D=0 | ||
36 | |||
37 | public Plane(float3 n, float d) | ||
38 | { | ||
39 | normal = new float3(n); | ||
40 | dist = d; | ||
41 | } | ||
42 | |||
43 | public Plane(Plane p) | ||
44 | { | ||
45 | normal = new float3(p.normal); | ||
46 | dist = p.dist; | ||
47 | } | ||
48 | |||
49 | public Plane() | ||
50 | { | ||
51 | dist = 0; | ||
52 | } | ||
53 | |||
54 | public void Transform(float3 position, Quaternion orientation) | ||
55 | { | ||
56 | // Transforms the plane to the space defined by the | ||
57 | // given position/orientation | ||
58 | float3 newNormal = Quaternion.Inverse(orientation) * normal; | ||
59 | float3 origin = Quaternion.Inverse(orientation) * (-normal * dist - position); | ||
60 | |||
61 | normal = newNormal; | ||
62 | dist = -float3.dot(newNormal, origin); | ||
63 | } | ||
64 | |||
65 | public override int GetHashCode() | ||
66 | { | ||
67 | return normal.GetHashCode() ^ dist.GetHashCode(); | ||
68 | } | ||
69 | |||
70 | public override bool Equals(object obj) | ||
71 | { | ||
72 | Plane p = obj as Plane; | ||
73 | if (p == null) | ||
74 | return false; | ||
75 | |||
76 | return this == p; | ||
77 | } | ||
78 | |||
79 | public static bool operator ==(Plane a, Plane b) | ||
80 | { | ||
81 | return (a.normal == b.normal && a.dist == b.dist); | ||
82 | } | ||
83 | |||
84 | public static bool operator !=(Plane a, Plane b) | ||
85 | { | ||
86 | return !(a == b); | ||
87 | } | ||
88 | |||
89 | public static Plane PlaneFlip(Plane plane) | ||
90 | { | ||
91 | return new Plane(-plane.normal, -plane.dist); | ||
92 | } | ||
93 | |||
94 | public static bool coplanar(Plane a, Plane b) | ||
95 | { | ||
96 | return (a == b || a == PlaneFlip(b)); | ||
97 | } | ||
98 | } | ||
99 | } | ||
diff --git a/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/PlaneTri.cs b/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/PlaneTri.cs new file mode 100644 index 0000000..31f0182 --- /dev/null +++ b/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/PlaneTri.cs | |||
@@ -0,0 +1,211 @@ | |||
1 | /* The MIT License | ||
2 | * | ||
3 | * Copyright (c) 2010 Intel Corporation. | ||
4 | * All rights reserved. | ||
5 | * | ||
6 | * Based on the convexdecomposition library from | ||
7 | * <http://codesuppository.googlecode.com> by John W. Ratcliff and Stan Melax. | ||
8 | * | ||
9 | * Permission is hereby granted, free of charge, to any person obtaining a copy | ||
10 | * of this software and associated documentation files (the "Software"), to deal | ||
11 | * in the Software without restriction, including without limitation the rights | ||
12 | * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell | ||
13 | * copies of the Software, and to permit persons to whom the Software is | ||
14 | * furnished to do so, subject to the following conditions: | ||
15 | * | ||
16 | * The above copyright notice and this permission notice shall be included in | ||
17 | * all copies or substantial portions of the Software. | ||
18 | * | ||
19 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR | ||
20 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, | ||
21 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE | ||
22 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER | ||
23 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, | ||
24 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN | ||
25 | * THE SOFTWARE. | ||
26 | */ | ||
27 | |||
28 | using System; | ||
29 | using System.Collections.Generic; | ||
30 | using System.Diagnostics; | ||
31 | |||
32 | namespace OpenSim.Region.Physics.ConvexDecompositionDotNet | ||
33 | { | ||
34 | public enum PlaneTriResult : int | ||
35 | { | ||
36 | PTR_FRONT, | ||
37 | PTR_BACK, | ||
38 | PTR_SPLIT | ||
39 | } | ||
40 | |||
41 | public static class PlaneTri | ||
42 | { | ||
43 | private static float DistToPt(float3 p, float4 plane) | ||
44 | { | ||
45 | return p.x * plane.x + p.y * plane.y + p.z * plane.z + plane.w; | ||
46 | } | ||
47 | |||
48 | private static PlaneTriResult getSidePlane(float3 p, float4 plane, float epsilon) | ||
49 | { | ||
50 | float d = DistToPt(p, plane); | ||
51 | |||
52 | if ((d + epsilon) > 0f) | ||
53 | return PlaneTriResult.PTR_FRONT; // it is 'in front' within the provided epsilon value. | ||
54 | |||
55 | return PlaneTriResult.PTR_BACK; | ||
56 | } | ||
57 | |||
58 | private static void add(float3 p, float3[] dest, ref int pcount) | ||
59 | { | ||
60 | dest[pcount++] = new float3(p); | ||
61 | Debug.Assert(pcount <= 4); | ||
62 | } | ||
63 | |||
64 | // assumes that the points are on opposite sides of the plane! | ||
65 | private static void intersect(float3 p1, float3 p2, float3 split, float4 plane) | ||
66 | { | ||
67 | float dp1 = DistToPt(p1, plane); | ||
68 | float[] dir = new float[3]; | ||
69 | |||
70 | dir[0] = p2[0] - p1[0]; | ||
71 | dir[1] = p2[1] - p1[1]; | ||
72 | dir[2] = p2[2] - p1[2]; | ||
73 | |||
74 | float dot1 = dir[0] * plane[0] + dir[1] * plane[1] + dir[2] * plane[2]; | ||
75 | float dot2 = dp1 - plane[3]; | ||
76 | |||
77 | float t = -(plane[3] + dot2) / dot1; | ||
78 | |||
79 | split.x = (dir[0] * t) + p1[0]; | ||
80 | split.y = (dir[1] * t) + p1[1]; | ||
81 | split.z = (dir[2] * t) + p1[2]; | ||
82 | } | ||
83 | |||
84 | public static PlaneTriResult planeTriIntersection(float4 plane, FaceTri triangle, float epsilon, ref float3[] front, out int fcount, ref float3[] back, out int bcount) | ||
85 | { | ||
86 | fcount = 0; | ||
87 | bcount = 0; | ||
88 | |||
89 | // get the three vertices of the triangle. | ||
90 | float3 p1 = triangle.P1; | ||
91 | float3 p2 = triangle.P2; | ||
92 | float3 p3 = triangle.P3; | ||
93 | |||
94 | PlaneTriResult r1 = getSidePlane(p1, plane, epsilon); // compute the side of the plane each vertex is on | ||
95 | PlaneTriResult r2 = getSidePlane(p2, plane, epsilon); | ||
96 | PlaneTriResult r3 = getSidePlane(p3, plane, epsilon); | ||
97 | |||
98 | if (r1 == r2 && r1 == r3) // if all three vertices are on the same side of the plane. | ||
99 | { | ||
100 | if (r1 == PlaneTriResult.PTR_FRONT) // if all three are in front of the plane, then copy to the 'front' output triangle. | ||
101 | { | ||
102 | add(p1, front, ref fcount); | ||
103 | add(p2, front, ref fcount); | ||
104 | add(p3, front, ref fcount); | ||
105 | } | ||
106 | else | ||
107 | { | ||
108 | add(p1, back, ref bcount); // if all three are in 'back' then copy to the 'back' output triangle. | ||
109 | add(p2, back, ref bcount); | ||
110 | add(p3, back, ref bcount); | ||
111 | } | ||
112 | return r1; // if all three points are on the same side of the plane return result | ||
113 | } | ||
114 | |||
115 | // ok.. we need to split the triangle at the plane. | ||
116 | |||
117 | // First test ray segment P1 to P2 | ||
118 | if (r1 == r2) // if these are both on the same side... | ||
119 | { | ||
120 | if (r1 == PlaneTriResult.PTR_FRONT) | ||
121 | { | ||
122 | add(p1, front, ref fcount); | ||
123 | add(p2, front, ref fcount); | ||
124 | } | ||
125 | else | ||
126 | { | ||
127 | add(p1, back, ref bcount); | ||
128 | add(p2, back, ref bcount); | ||
129 | } | ||
130 | } | ||
131 | else | ||
132 | { | ||
133 | float3 split = new float3(); | ||
134 | intersect(p1, p2, split, plane); | ||
135 | |||
136 | if (r1 == PlaneTriResult.PTR_FRONT) | ||
137 | { | ||
138 | |||
139 | add(p1, front, ref fcount); | ||
140 | add(split, front, ref fcount); | ||
141 | |||
142 | add(split, back, ref bcount); | ||
143 | add(p2, back, ref bcount); | ||
144 | |||
145 | } | ||
146 | else | ||
147 | { | ||
148 | add(p1, back, ref bcount); | ||
149 | add(split, back, ref bcount); | ||
150 | |||
151 | add(split, front, ref fcount); | ||
152 | add(p2, front, ref fcount); | ||
153 | } | ||
154 | |||
155 | } | ||
156 | |||
157 | // Next test ray segment P2 to P3 | ||
158 | if (r2 == r3) // if these are both on the same side... | ||
159 | { | ||
160 | if (r3 == PlaneTriResult.PTR_FRONT) | ||
161 | { | ||
162 | add(p3, front, ref fcount); | ||
163 | } | ||
164 | else | ||
165 | { | ||
166 | add(p3, back, ref bcount); | ||
167 | } | ||
168 | } | ||
169 | else | ||
170 | { | ||
171 | float3 split = new float3(); // split the point | ||
172 | intersect(p2, p3, split, plane); | ||
173 | |||
174 | if (r3 == PlaneTriResult.PTR_FRONT) | ||
175 | { | ||
176 | add(split, front, ref fcount); | ||
177 | add(split, back, ref bcount); | ||
178 | |||
179 | add(p3, front, ref fcount); | ||
180 | } | ||
181 | else | ||
182 | { | ||
183 | add(split, front, ref fcount); | ||
184 | add(split, back, ref bcount); | ||
185 | |||
186 | add(p3, back, ref bcount); | ||
187 | } | ||
188 | } | ||
189 | |||
190 | // Next test ray segment P3 to P1 | ||
191 | if (r3 != r1) // if these are both on the same side... | ||
192 | { | ||
193 | float3 split = new float3(); // split the point | ||
194 | intersect(p3, p1, split, plane); | ||
195 | |||
196 | if (r1 == PlaneTriResult.PTR_FRONT) | ||
197 | { | ||
198 | add(split, front, ref fcount); | ||
199 | add(split, back, ref bcount); | ||
200 | } | ||
201 | else | ||
202 | { | ||
203 | add(split, front, ref fcount); | ||
204 | add(split, back, ref bcount); | ||
205 | } | ||
206 | } | ||
207 | |||
208 | return PlaneTriResult.PTR_SPLIT; | ||
209 | } | ||
210 | } | ||
211 | } | ||
diff --git a/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/Properties/AssemblyInfo.cs b/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/Properties/AssemblyInfo.cs new file mode 100644 index 0000000..c5867b2 --- /dev/null +++ b/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/Properties/AssemblyInfo.cs | |||
@@ -0,0 +1,36 @@ | |||
1 | using System.Reflection; | ||
2 | using System.Runtime.CompilerServices; | ||
3 | using System.Runtime.InteropServices; | ||
4 | |||
5 | // General Information about an assembly is controlled through the following | ||
6 | // set of attributes. Change these attribute values to modify the information | ||
7 | // associated with an assembly. | ||
8 | [assembly: AssemblyTitle("ConvexDecompositionDotNet")] | ||
9 | [assembly: AssemblyDescription("")] | ||
10 | [assembly: AssemblyConfiguration("")] | ||
11 | [assembly: AssemblyCompany("Intel Corporation")] | ||
12 | [assembly: AssemblyProduct("ConvexDecompositionDotNet")] | ||
13 | [assembly: AssemblyCopyright("Copyright © Intel Corporation 2010")] | ||
14 | [assembly: AssemblyTrademark("")] | ||
15 | [assembly: AssemblyCulture("")] | ||
16 | |||
17 | // Setting ComVisible to false makes the types in this assembly not visible | ||
18 | // to COM components. If you need to access a type in this assembly from | ||
19 | // COM, set the ComVisible attribute to true on that type. | ||
20 | [assembly: ComVisible(false)] | ||
21 | |||
22 | // The following GUID is for the ID of the typelib if this project is exposed to COM | ||
23 | [assembly: Guid("2a1c9467-1a17-4c8d-bf9f-4b4d86dd0cbb")] | ||
24 | |||
25 | // Version information for an assembly consists of the following four values: | ||
26 | // | ||
27 | // Major Version | ||
28 | // Minor Version | ||
29 | // Build Number | ||
30 | // Revision | ||
31 | // | ||
32 | // You can specify all the values or you can default the Build and Revision Numbers | ||
33 | // by using the '*' as shown below: | ||
34 | // [assembly: AssemblyVersion("1.0.*")] | ||
35 | [assembly: AssemblyVersion("0.8.2.*")] | ||
36 | |||
diff --git a/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/Quaternion.cs b/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/Quaternion.cs new file mode 100644 index 0000000..0ba8f17 --- /dev/null +++ b/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/Quaternion.cs | |||
@@ -0,0 +1,209 @@ | |||
1 | /* The MIT License | ||
2 | * | ||
3 | * Copyright (c) 2010 Intel Corporation. | ||
4 | * All rights reserved. | ||
5 | * | ||
6 | * Based on the convexdecomposition library from | ||
7 | * <http://codesuppository.googlecode.com> by John W. Ratcliff and Stan Melax. | ||
8 | * | ||
9 | * Permission is hereby granted, free of charge, to any person obtaining a copy | ||
10 | * of this software and associated documentation files (the "Software"), to deal | ||
11 | * in the Software without restriction, including without limitation the rights | ||
12 | * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell | ||
13 | * copies of the Software, and to permit persons to whom the Software is | ||
14 | * furnished to do so, subject to the following conditions: | ||
15 | * | ||
16 | * The above copyright notice and this permission notice shall be included in | ||
17 | * all copies or substantial portions of the Software. | ||
18 | * | ||
19 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR | ||
20 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, | ||
21 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE | ||
22 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER | ||
23 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, | ||
24 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN | ||
25 | * THE SOFTWARE. | ||
26 | */ | ||
27 | |||
28 | using System; | ||
29 | |||
30 | namespace OpenSim.Region.Physics.ConvexDecompositionDotNet | ||
31 | { | ||
32 | public class Quaternion : float4 | ||
33 | { | ||
34 | public Quaternion() | ||
35 | { | ||
36 | x = y = z = 0.0f; | ||
37 | w = 1.0f; | ||
38 | } | ||
39 | |||
40 | public Quaternion(float3 v, float t) | ||
41 | { | ||
42 | v = float3.normalize(v); | ||
43 | w = (float)Math.Cos(t / 2.0f); | ||
44 | v = v * (float)Math.Sin(t / 2.0f); | ||
45 | x = v.x; | ||
46 | y = v.y; | ||
47 | z = v.z; | ||
48 | } | ||
49 | |||
50 | public Quaternion(float _x, float _y, float _z, float _w) | ||
51 | { | ||
52 | x = _x; | ||
53 | y = _y; | ||
54 | z = _z; | ||
55 | w = _w; | ||
56 | } | ||
57 | |||
58 | public float angle() | ||
59 | { | ||
60 | return (float)Math.Acos(w) * 2.0f; | ||
61 | } | ||
62 | |||
63 | public float3 axis() | ||
64 | { | ||
65 | float3 a = new float3(x, y, z); | ||
66 | if (Math.Abs(angle()) < 0.0000001f) | ||
67 | return new float3(1f, 0f, 0f); | ||
68 | return a * (1 / (float)Math.Sin(angle() / 2.0f)); | ||
69 | } | ||
70 | |||
71 | public float3 xdir() | ||
72 | { | ||
73 | return new float3(1 - 2 * (y * y + z * z), 2 * (x * y + w * z), 2 * (x * z - w * y)); | ||
74 | } | ||
75 | |||
76 | public float3 ydir() | ||
77 | { | ||
78 | return new float3(2 * (x * y - w * z), 1 - 2 * (x * x + z * z), 2 * (y * z + w * x)); | ||
79 | } | ||
80 | |||
81 | public float3 zdir() | ||
82 | { | ||
83 | return new float3(2 * (x * z + w * y), 2 * (y * z - w * x), 1 - 2 * (x * x + y * y)); | ||
84 | } | ||
85 | |||
86 | public float3x3 getmatrix() | ||
87 | { | ||
88 | return new float3x3(xdir(), ydir(), zdir()); | ||
89 | } | ||
90 | |||
91 | public static implicit operator float3x3(Quaternion q) | ||
92 | { | ||
93 | return q.getmatrix(); | ||
94 | } | ||
95 | |||
96 | public static Quaternion operator *(Quaternion a, Quaternion b) | ||
97 | { | ||
98 | Quaternion c = new Quaternion(); | ||
99 | c.w = a.w * b.w - a.x * b.x - a.y * b.y - a.z * b.z; | ||
100 | c.x = a.w * b.x + a.x * b.w + a.y * b.z - a.z * b.y; | ||
101 | c.y = a.w * b.y - a.x * b.z + a.y * b.w + a.z * b.x; | ||
102 | c.z = a.w * b.z + a.x * b.y - a.y * b.x + a.z * b.w; | ||
103 | return c; | ||
104 | } | ||
105 | |||
106 | public static float3 operator *(Quaternion q, float3 v) | ||
107 | { | ||
108 | // The following is equivalent to: | ||
109 | //return (q.getmatrix() * v); | ||
110 | float qx2 = q.x * q.x; | ||
111 | float qy2 = q.y * q.y; | ||
112 | float qz2 = q.z * q.z; | ||
113 | |||
114 | float qxqy = q.x * q.y; | ||
115 | float qxqz = q.x * q.z; | ||
116 | float qxqw = q.x * q.w; | ||
117 | float qyqz = q.y * q.z; | ||
118 | float qyqw = q.y * q.w; | ||
119 | float qzqw = q.z * q.w; | ||
120 | return new float3((1 - 2 * (qy2 + qz2)) * v.x + (2 * (qxqy - qzqw)) * v.y + (2 * (qxqz + qyqw)) * v.z, (2 * (qxqy + qzqw)) * v.x + (1 - 2 * (qx2 + qz2)) * v.y + (2 * (qyqz - qxqw)) * v.z, (2 * (qxqz - qyqw)) * v.x + (2 * (qyqz + qxqw)) * v.y + (1 - 2 * (qx2 + qy2)) * v.z); | ||
121 | } | ||
122 | |||
123 | public static Quaternion operator +(Quaternion a, Quaternion b) | ||
124 | { | ||
125 | return new Quaternion(a.x + b.x, a.y + b.y, a.z + b.z, a.w + b.w); | ||
126 | } | ||
127 | |||
128 | public static Quaternion operator *(Quaternion a, float b) | ||
129 | { | ||
130 | return new Quaternion(a.x *b, a.y *b, a.z *b, a.w *b); | ||
131 | } | ||
132 | |||
133 | public static Quaternion normalize(Quaternion a) | ||
134 | { | ||
135 | float m = (float)Math.Sqrt(a.w * a.w + a.x * a.x + a.y * a.y + a.z * a.z); | ||
136 | if (m < 0.000000001f) | ||
137 | { | ||
138 | a.w = 1; | ||
139 | a.x = a.y = a.z = 0; | ||
140 | return a; | ||
141 | } | ||
142 | return a * (1f / m); | ||
143 | } | ||
144 | |||
145 | public static float dot(Quaternion a, Quaternion b) | ||
146 | { | ||
147 | return (a.w * b.w + a.x * b.x + a.y * b.y + a.z * b.z); | ||
148 | } | ||
149 | |||
150 | public static Quaternion slerp(Quaternion a, Quaternion b, float interp) | ||
151 | { | ||
152 | if (dot(a, b) < 0.0) | ||
153 | { | ||
154 | a.w = -a.w; | ||
155 | a.x = -a.x; | ||
156 | a.y = -a.y; | ||
157 | a.z = -a.z; | ||
158 | } | ||
159 | float d = dot(a, b); | ||
160 | if (d >= 1.0) | ||
161 | { | ||
162 | return a; | ||
163 | } | ||
164 | float theta = (float)Math.Acos(d); | ||
165 | if (theta == 0.0f) | ||
166 | { | ||
167 | return (a); | ||
168 | } | ||
169 | return a * ((float)Math.Sin(theta - interp * theta) / (float)Math.Sin(theta)) + b * ((float)Math.Sin(interp * theta) / (float)Math.Sin(theta)); | ||
170 | } | ||
171 | |||
172 | public static Quaternion Interpolate(Quaternion q0, Quaternion q1, float alpha) | ||
173 | { | ||
174 | return slerp(q0, q1, alpha); | ||
175 | } | ||
176 | |||
177 | public static Quaternion Inverse(Quaternion q) | ||
178 | { | ||
179 | return new Quaternion(-q.x, -q.y, -q.z, q.w); | ||
180 | } | ||
181 | |||
182 | public static Quaternion YawPitchRoll(float yaw, float pitch, float roll) | ||
183 | { | ||
184 | roll *= (3.14159264f / 180.0f); | ||
185 | yaw *= (3.14159264f / 180.0f); | ||
186 | pitch *= (3.14159264f / 180.0f); | ||
187 | return new Quaternion(new float3(0.0f, 0.0f, 1.0f), yaw) * new Quaternion(new float3(1.0f, 0.0f, 0.0f), pitch) * new Quaternion(new float3(0.0f, 1.0f, 0.0f), roll); | ||
188 | } | ||
189 | |||
190 | public static float Yaw(Quaternion q) | ||
191 | { | ||
192 | float3 v = q.ydir(); | ||
193 | return (v.y == 0.0 && v.x == 0.0) ? 0.0f : (float)Math.Atan2(-v.x, v.y) * (180.0f / 3.14159264f); | ||
194 | } | ||
195 | |||
196 | public static float Pitch(Quaternion q) | ||
197 | { | ||
198 | float3 v = q.ydir(); | ||
199 | return (float)Math.Atan2(v.z, Math.Sqrt(v.x * v.x + v.y * v.y)) * (180.0f / 3.14159264f); | ||
200 | } | ||
201 | |||
202 | public static float Roll(Quaternion q) | ||
203 | { | ||
204 | q = new Quaternion(new float3(0.0f, 0.0f, 1.0f), -Yaw(q) * (3.14159264f / 180.0f)) * q; | ||
205 | q = new Quaternion(new float3(1.0f, 0.0f, 0.0f), -Pitch(q) * (3.14159264f / 180.0f)) * q; | ||
206 | return (float)Math.Atan2(-q.xdir().z, q.xdir().x) * (180.0f / 3.14159264f); | ||
207 | } | ||
208 | } | ||
209 | } | ||
diff --git a/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/README.txt b/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/README.txt new file mode 100644 index 0000000..fc53ae7 --- /dev/null +++ b/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/README.txt | |||
@@ -0,0 +1,7 @@ | |||
1 | ConvexDecompositionDotNet | ||
2 | ========================= | ||
3 | |||
4 | A C# port of the ConvexDecomposition library by John W. Ratcliff and Stan Melax. | ||
5 | The original C++ version is available at <http://codesuppository.googlecode.com/>. | ||
6 | See the blog post at <http://codesuppository.blogspot.com/2006/08/approximate-convexdecomposition.html> | ||
7 | for a thorough explanation of generating convex hulls from concave meshes. | ||
diff --git a/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/SplitPlane.cs b/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/SplitPlane.cs new file mode 100644 index 0000000..9f06a9a --- /dev/null +++ b/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/SplitPlane.cs | |||
@@ -0,0 +1,265 @@ | |||
1 | /* The MIT License | ||
2 | * | ||
3 | * Copyright (c) 2010 Intel Corporation. | ||
4 | * All rights reserved. | ||
5 | * | ||
6 | * Based on the convexdecomposition library from | ||
7 | * <http://codesuppository.googlecode.com> by John W. Ratcliff and Stan Melax. | ||
8 | * | ||
9 | * Permission is hereby granted, free of charge, to any person obtaining a copy | ||
10 | * of this software and associated documentation files (the "Software"), to deal | ||
11 | * in the Software without restriction, including without limitation the rights | ||
12 | * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell | ||
13 | * copies of the Software, and to permit persons to whom the Software is | ||
14 | * furnished to do so, subject to the following conditions: | ||
15 | * | ||
16 | * The above copyright notice and this permission notice shall be included in | ||
17 | * all copies or substantial portions of the Software. | ||
18 | * | ||
19 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR | ||
20 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, | ||
21 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE | ||
22 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER | ||
23 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, | ||
24 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN | ||
25 | * THE SOFTWARE. | ||
26 | */ | ||
27 | |||
28 | using System; | ||
29 | using System.Collections.Generic; | ||
30 | |||
31 | namespace OpenSim.Region.Physics.ConvexDecompositionDotNet | ||
32 | { | ||
33 | public class Rect3d | ||
34 | { | ||
35 | public float[] mMin = new float[3]; | ||
36 | public float[] mMax = new float[3]; | ||
37 | |||
38 | public Rect3d() | ||
39 | { | ||
40 | } | ||
41 | |||
42 | public Rect3d(float[] bmin, float[] bmax) | ||
43 | { | ||
44 | mMin[0] = bmin[0]; | ||
45 | mMin[1] = bmin[1]; | ||
46 | mMin[2] = bmin[2]; | ||
47 | |||
48 | mMax[0] = bmax[0]; | ||
49 | mMax[1] = bmax[1]; | ||
50 | mMax[2] = bmax[2]; | ||
51 | } | ||
52 | |||
53 | public void SetMin(float[] bmin) | ||
54 | { | ||
55 | mMin[0] = bmin[0]; | ||
56 | mMin[1] = bmin[1]; | ||
57 | mMin[2] = bmin[2]; | ||
58 | } | ||
59 | |||
60 | public void SetMax(float[] bmax) | ||
61 | { | ||
62 | mMax[0] = bmax[0]; | ||
63 | mMax[1] = bmax[1]; | ||
64 | mMax[2] = bmax[2]; | ||
65 | } | ||
66 | |||
67 | public void SetMin(float x, float y, float z) | ||
68 | { | ||
69 | mMin[0] = x; | ||
70 | mMin[1] = y; | ||
71 | mMin[2] = z; | ||
72 | } | ||
73 | |||
74 | public void SetMax(float x, float y, float z) | ||
75 | { | ||
76 | mMax[0] = x; | ||
77 | mMax[1] = y; | ||
78 | mMax[2] = z; | ||
79 | } | ||
80 | } | ||
81 | |||
82 | public static class SplitPlane | ||
83 | { | ||
84 | public static bool computeSplitPlane(List<float3> vertices, List<int> indices, ref float4 plane) | ||
85 | { | ||
86 | float[] bmin = { Single.MaxValue, Single.MaxValue, Single.MaxValue }; | ||
87 | float[] bmax = { Single.MinValue, Single.MinValue, Single.MinValue }; | ||
88 | |||
89 | for (int i = 0; i < vertices.Count; i++) | ||
90 | { | ||
91 | float3 p = vertices[i]; | ||
92 | |||
93 | if (p[0] < bmin[0]) | ||
94 | bmin[0] = p[0]; | ||
95 | if (p[1] < bmin[1]) | ||
96 | bmin[1] = p[1]; | ||
97 | if (p[2] < bmin[2]) | ||
98 | bmin[2] = p[2]; | ||
99 | |||
100 | if (p[0] > bmax[0]) | ||
101 | bmax[0] = p[0]; | ||
102 | if (p[1] > bmax[1]) | ||
103 | bmax[1] = p[1]; | ||
104 | if (p[2] > bmax[2]) | ||
105 | bmax[2] = p[2]; | ||
106 | } | ||
107 | |||
108 | float dx = bmax[0] - bmin[0]; | ||
109 | float dy = bmax[1] - bmin[1]; | ||
110 | float dz = bmax[2] - bmin[2]; | ||
111 | |||
112 | float laxis = dx; | ||
113 | |||
114 | int axis = 0; | ||
115 | |||
116 | if (dy > dx) | ||
117 | { | ||
118 | axis = 1; | ||
119 | laxis = dy; | ||
120 | } | ||
121 | |||
122 | if (dz > dx && dz > dy) | ||
123 | { | ||
124 | axis = 2; | ||
125 | laxis = dz; | ||
126 | } | ||
127 | |||
128 | float[] p1 = new float[3]; | ||
129 | float[] p2 = new float[3]; | ||
130 | float[] p3 = new float[3]; | ||
131 | |||
132 | p3[0] = p2[0] = p1[0] = bmin[0] + dx * 0.5f; | ||
133 | p3[1] = p2[1] = p1[1] = bmin[1] + dy * 0.5f; | ||
134 | p3[2] = p2[2] = p1[2] = bmin[2] + dz * 0.5f; | ||
135 | |||
136 | Rect3d b = new Rect3d(bmin, bmax); | ||
137 | |||
138 | Rect3d b1 = new Rect3d(); | ||
139 | Rect3d b2 = new Rect3d(); | ||
140 | |||
141 | splitRect(axis, b, b1, b2, p1); | ||
142 | |||
143 | switch (axis) | ||
144 | { | ||
145 | case 0: | ||
146 | p2[1] = bmin[1]; | ||
147 | p2[2] = bmin[2]; | ||
148 | |||
149 | if (dz > dy) | ||
150 | { | ||
151 | p3[1] = bmax[1]; | ||
152 | p3[2] = bmin[2]; | ||
153 | } | ||
154 | else | ||
155 | { | ||
156 | p3[1] = bmin[1]; | ||
157 | p3[2] = bmax[2]; | ||
158 | } | ||
159 | |||
160 | break; | ||
161 | case 1: | ||
162 | p2[0] = bmin[0]; | ||
163 | p2[2] = bmin[2]; | ||
164 | |||
165 | if (dx > dz) | ||
166 | { | ||
167 | p3[0] = bmax[0]; | ||
168 | p3[2] = bmin[2]; | ||
169 | } | ||
170 | else | ||
171 | { | ||
172 | p3[0] = bmin[0]; | ||
173 | p3[2] = bmax[2]; | ||
174 | } | ||
175 | |||
176 | break; | ||
177 | case 2: | ||
178 | p2[0] = bmin[0]; | ||
179 | p2[1] = bmin[1]; | ||
180 | |||
181 | if (dx > dy) | ||
182 | { | ||
183 | p3[0] = bmax[0]; | ||
184 | p3[1] = bmin[1]; | ||
185 | } | ||
186 | else | ||
187 | { | ||
188 | p3[0] = bmin[0]; | ||
189 | p3[1] = bmax[1]; | ||
190 | } | ||
191 | |||
192 | break; | ||
193 | } | ||
194 | |||
195 | computePlane(p1, p2, p3, plane); | ||
196 | |||
197 | return true; | ||
198 | } | ||
199 | |||
200 | internal static void computePlane(float[] A, float[] B, float[] C, float4 plane) | ||
201 | { | ||
202 | float vx = (B[0] - C[0]); | ||
203 | float vy = (B[1] - C[1]); | ||
204 | float vz = (B[2] - C[2]); | ||
205 | |||
206 | float wx = (A[0] - B[0]); | ||
207 | float wy = (A[1] - B[1]); | ||
208 | float wz = (A[2] - B[2]); | ||
209 | |||
210 | float vw_x = vy * wz - vz * wy; | ||
211 | float vw_y = vz * wx - vx * wz; | ||
212 | float vw_z = vx * wy - vy * wx; | ||
213 | |||
214 | float mag = (float)Math.Sqrt((vw_x * vw_x) + (vw_y * vw_y) + (vw_z * vw_z)); | ||
215 | |||
216 | if (mag < 0.000001f) | ||
217 | { | ||
218 | mag = 0; | ||
219 | } | ||
220 | else | ||
221 | { | ||
222 | mag = 1.0f / mag; | ||
223 | } | ||
224 | |||
225 | float x = vw_x * mag; | ||
226 | float y = vw_y * mag; | ||
227 | float z = vw_z * mag; | ||
228 | |||
229 | float D = 0.0f - ((x * A[0]) + (y * A[1]) + (z * A[2])); | ||
230 | |||
231 | plane.x = x; | ||
232 | plane.y = y; | ||
233 | plane.z = z; | ||
234 | plane.w = D; | ||
235 | } | ||
236 | |||
237 | public static void splitRect(int axis, Rect3d source, Rect3d b1, Rect3d b2, float[] midpoint) | ||
238 | { | ||
239 | switch (axis) | ||
240 | { | ||
241 | case 0: | ||
242 | b1.SetMin(source.mMin); | ||
243 | b1.SetMax(midpoint[0], source.mMax[1], source.mMax[2]); | ||
244 | |||
245 | b2.SetMin(midpoint[0], source.mMin[1], source.mMin[2]); | ||
246 | b2.SetMax(source.mMax); | ||
247 | break; | ||
248 | case 1: | ||
249 | b1.SetMin(source.mMin); | ||
250 | b1.SetMax(source.mMax[0], midpoint[1], source.mMax[2]); | ||
251 | |||
252 | b2.SetMin(source.mMin[0], midpoint[1], source.mMin[2]); | ||
253 | b2.SetMax(source.mMax); | ||
254 | break; | ||
255 | case 2: | ||
256 | b1.SetMin(source.mMin); | ||
257 | b1.SetMax(source.mMax[0], source.mMax[1], midpoint[2]); | ||
258 | |||
259 | b2.SetMin(source.mMin[0], source.mMin[1], midpoint[2]); | ||
260 | b2.SetMax(source.mMax); | ||
261 | break; | ||
262 | } | ||
263 | } | ||
264 | } | ||
265 | } | ||
diff --git a/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/VertexLookup.cs b/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/VertexLookup.cs new file mode 100644 index 0000000..6f17c9f --- /dev/null +++ b/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/VertexLookup.cs | |||
@@ -0,0 +1,70 @@ | |||
1 | /* The MIT License | ||
2 | * | ||
3 | * Copyright (c) 2010 Intel Corporation. | ||
4 | * All rights reserved. | ||
5 | * | ||
6 | * Based on the convexdecomposition library from | ||
7 | * <http://codesuppository.googlecode.com> by John W. Ratcliff and Stan Melax. | ||
8 | * | ||
9 | * Permission is hereby granted, free of charge, to any person obtaining a copy | ||
10 | * of this software and associated documentation files (the "Software"), to deal | ||
11 | * in the Software without restriction, including without limitation the rights | ||
12 | * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell | ||
13 | * copies of the Software, and to permit persons to whom the Software is | ||
14 | * furnished to do so, subject to the following conditions: | ||
15 | * | ||
16 | * The above copyright notice and this permission notice shall be included in | ||
17 | * all copies or substantial portions of the Software. | ||
18 | * | ||
19 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR | ||
20 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, | ||
21 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE | ||
22 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER | ||
23 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, | ||
24 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN | ||
25 | * THE SOFTWARE. | ||
26 | */ | ||
27 | |||
28 | using System; | ||
29 | using System.Collections.Generic; | ||
30 | |||
31 | namespace OpenSim.Region.Physics.ConvexDecompositionDotNet | ||
32 | { | ||
33 | public class VertexPool | ||
34 | { | ||
35 | private List<float3> mVertices = new List<float3>(); | ||
36 | private Dictionary<float3, int> mIndices = new Dictionary<float3, int>(); | ||
37 | |||
38 | public int getIndex(float3 vtx) | ||
39 | { | ||
40 | int idx; | ||
41 | if (mIndices.TryGetValue(vtx, out idx)) | ||
42 | return idx; | ||
43 | |||
44 | idx = mVertices.Count; | ||
45 | mVertices.Add(vtx); | ||
46 | mIndices.Add(vtx, idx); | ||
47 | return idx; | ||
48 | } | ||
49 | |||
50 | public float3 Get(int idx) | ||
51 | { | ||
52 | return mVertices[idx]; | ||
53 | } | ||
54 | |||
55 | public int GetSize() | ||
56 | { | ||
57 | return mVertices.Count; | ||
58 | } | ||
59 | |||
60 | public List<float3> GetVertices() | ||
61 | { | ||
62 | return mVertices; | ||
63 | } | ||
64 | |||
65 | public void Clear() | ||
66 | { | ||
67 | mVertices.Clear(); | ||
68 | } | ||
69 | } | ||
70 | } | ||
diff --git a/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/float2.cs b/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/float2.cs new file mode 100644 index 0000000..ce88fc8 --- /dev/null +++ b/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/float2.cs | |||
@@ -0,0 +1,70 @@ | |||
1 | /* The MIT License | ||
2 | * | ||
3 | * Copyright (c) 2010 Intel Corporation. | ||
4 | * All rights reserved. | ||
5 | * | ||
6 | * Based on the convexdecomposition library from | ||
7 | * <http://codesuppository.googlecode.com> by John W. Ratcliff and Stan Melax. | ||
8 | * | ||
9 | * Permission is hereby granted, free of charge, to any person obtaining a copy | ||
10 | * of this software and associated documentation files (the "Software"), to deal | ||
11 | * in the Software without restriction, including without limitation the rights | ||
12 | * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell | ||
13 | * copies of the Software, and to permit persons to whom the Software is | ||
14 | * furnished to do so, subject to the following conditions: | ||
15 | * | ||
16 | * The above copyright notice and this permission notice shall be included in | ||
17 | * all copies or substantial portions of the Software. | ||
18 | * | ||
19 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR | ||
20 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, | ||
21 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE | ||
22 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER | ||
23 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, | ||
24 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN | ||
25 | * THE SOFTWARE. | ||
26 | */ | ||
27 | |||
28 | using System; | ||
29 | |||
30 | namespace OpenSim.Region.Physics.ConvexDecompositionDotNet | ||
31 | { | ||
32 | public class float2 | ||
33 | { | ||
34 | public float x; | ||
35 | public float y; | ||
36 | |||
37 | public float2() | ||
38 | { | ||
39 | } | ||
40 | |||
41 | public float2(float _x, float _y) | ||
42 | { | ||
43 | x = _x; | ||
44 | y = _y; | ||
45 | } | ||
46 | |||
47 | public float this[int i] | ||
48 | { | ||
49 | get | ||
50 | { | ||
51 | switch (i) | ||
52 | { | ||
53 | case 0: return x; | ||
54 | case 1: return y; | ||
55 | } | ||
56 | throw new ArgumentOutOfRangeException(); | ||
57 | } | ||
58 | } | ||
59 | |||
60 | public static float2 operator -(float2 a, float2 b) | ||
61 | { | ||
62 | return new float2(a.x - b.x, a.y - b.y); | ||
63 | } | ||
64 | |||
65 | public static float2 operator +(float2 a, float2 b) | ||
66 | { | ||
67 | return new float2(a.x + b.x, a.y + b.y); | ||
68 | } | ||
69 | } | ||
70 | } | ||
diff --git a/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/float3.cs b/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/float3.cs new file mode 100644 index 0000000..4389114 --- /dev/null +++ b/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/float3.cs | |||
@@ -0,0 +1,444 @@ | |||
1 | /* The MIT License | ||
2 | * | ||
3 | * Copyright (c) 2010 Intel Corporation. | ||
4 | * All rights reserved. | ||
5 | * | ||
6 | * Based on the convexdecomposition library from | ||
7 | * <http://codesuppository.googlecode.com> by John W. Ratcliff and Stan Melax. | ||
8 | * | ||
9 | * Permission is hereby granted, free of charge, to any person obtaining a copy | ||
10 | * of this software and associated documentation files (the "Software"), to deal | ||
11 | * in the Software without restriction, including without limitation the rights | ||
12 | * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell | ||
13 | * copies of the Software, and to permit persons to whom the Software is | ||
14 | * furnished to do so, subject to the following conditions: | ||
15 | * | ||
16 | * The above copyright notice and this permission notice shall be included in | ||
17 | * all copies or substantial portions of the Software. | ||
18 | * | ||
19 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR | ||
20 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, | ||
21 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE | ||
22 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER | ||
23 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, | ||
24 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN | ||
25 | * THE SOFTWARE. | ||
26 | */ | ||
27 | |||
28 | using System; | ||
29 | |||
30 | namespace OpenSim.Region.Physics.ConvexDecompositionDotNet | ||
31 | { | ||
32 | public class float3 : IEquatable<float3> | ||
33 | { | ||
34 | public float x; | ||
35 | public float y; | ||
36 | public float z; | ||
37 | |||
38 | public float3() | ||
39 | { | ||
40 | x = 0; | ||
41 | y = 0; | ||
42 | z = 0; | ||
43 | } | ||
44 | |||
45 | public float3(float _x, float _y, float _z) | ||
46 | { | ||
47 | x = _x; | ||
48 | y = _y; | ||
49 | z = _z; | ||
50 | } | ||
51 | |||
52 | public float3(float3 f) | ||
53 | { | ||
54 | x = f.x; | ||
55 | y = f.y; | ||
56 | z = f.z; | ||
57 | } | ||
58 | |||
59 | public float this[int i] | ||
60 | { | ||
61 | get | ||
62 | { | ||
63 | switch (i) | ||
64 | { | ||
65 | case 0: return x; | ||
66 | case 1: return y; | ||
67 | case 2: return z; | ||
68 | } | ||
69 | throw new ArgumentOutOfRangeException(); | ||
70 | } | ||
71 | } | ||
72 | |||
73 | public float Distance(float3 a) | ||
74 | { | ||
75 | float3 d = new float3(a.x - x, a.y - y, a.z - z); | ||
76 | return d.Length(); | ||
77 | } | ||
78 | |||
79 | public float Distance2(float3 a) | ||
80 | { | ||
81 | float dx = a.x - x; | ||
82 | float dy = a.y - y; | ||
83 | float dz = a.z - z; | ||
84 | return dx * dx + dy * dy + dz * dz; | ||
85 | } | ||
86 | |||
87 | public float Length() | ||
88 | { | ||
89 | return (float)Math.Sqrt(x * x + y * y + z * z); | ||
90 | } | ||
91 | |||
92 | public float Area(float3 p1, float3 p2) | ||
93 | { | ||
94 | float A = Partial(p1); | ||
95 | A += p1.Partial(p2); | ||
96 | A += p2.Partial(this); | ||
97 | return A * 0.5f; | ||
98 | } | ||
99 | |||
100 | public float Partial(float3 p) | ||
101 | { | ||
102 | return (x * p.y) - (p.x * y); | ||
103 | } | ||
104 | |||
105 | // Given a point and a line (defined by two points), compute the closest point | ||
106 | // in the line. (The line is treated as infinitely long.) | ||
107 | public void NearestPointInLine(float3 point, float3 line0, float3 line1) | ||
108 | { | ||
109 | float3 nearestPoint = new float3(); | ||
110 | float3 lineDelta = line1 - line0; | ||
111 | |||
112 | // Handle degenerate lines | ||
113 | if (lineDelta == float3.Zero) | ||
114 | { | ||
115 | nearestPoint = line0; | ||
116 | } | ||
117 | else | ||
118 | { | ||
119 | float delta = float3.dot(point - line0, lineDelta) / float3.dot(lineDelta, lineDelta); | ||
120 | nearestPoint = line0 + lineDelta * delta; | ||
121 | } | ||
122 | |||
123 | this.x = nearestPoint.x; | ||
124 | this.y = nearestPoint.y; | ||
125 | this.z = nearestPoint.z; | ||
126 | } | ||
127 | |||
128 | // Given a point and a line segment (defined by two points), compute the closest point | ||
129 | // in the line. Cap the point at the endpoints of the line segment. | ||
130 | public void NearestPointInLineSegment(float3 point, float3 line0, float3 line1) | ||
131 | { | ||
132 | float3 nearestPoint = new float3(); | ||
133 | float3 lineDelta = line1 - line0; | ||
134 | |||
135 | // Handle degenerate lines | ||
136 | if (lineDelta == Zero) | ||
137 | { | ||
138 | nearestPoint = line0; | ||
139 | } | ||
140 | else | ||
141 | { | ||
142 | float delta = float3.dot(point - line0, lineDelta) / float3.dot(lineDelta, lineDelta); | ||
143 | |||
144 | // Clamp the point to conform to the segment's endpoints | ||
145 | if (delta < 0) | ||
146 | delta = 0; | ||
147 | else if (delta > 1) | ||
148 | delta = 1; | ||
149 | |||
150 | nearestPoint = line0 + lineDelta * delta; | ||
151 | } | ||
152 | |||
153 | this.x = nearestPoint.x; | ||
154 | this.y = nearestPoint.y; | ||
155 | this.z = nearestPoint.z; | ||
156 | } | ||
157 | |||
158 | // Given a point and a triangle (defined by three points), compute the closest point | ||
159 | // in the triangle. Clamp the point so it's confined to the area of the triangle. | ||
160 | public void NearestPointInTriangle(float3 point, float3 triangle0, float3 triangle1, float3 triangle2) | ||
161 | { | ||
162 | float3 nearestPoint = new float3(); | ||
163 | |||
164 | float3 lineDelta0 = triangle1 - triangle0; | ||
165 | float3 lineDelta1 = triangle2 - triangle0; | ||
166 | |||
167 | // Handle degenerate triangles | ||
168 | if ((lineDelta0 == Zero) || (lineDelta1 == Zero)) | ||
169 | { | ||
170 | nearestPoint.NearestPointInLineSegment(point, triangle1, triangle2); | ||
171 | } | ||
172 | else if (lineDelta0 == lineDelta1) | ||
173 | { | ||
174 | nearestPoint.NearestPointInLineSegment(point, triangle0, triangle1); | ||
175 | } | ||
176 | else | ||
177 | { | ||
178 | float3[] axis = new float3[3] { new float3(), new float3(), new float3() }; | ||
179 | axis[0].NearestPointInLine(triangle0, triangle1, triangle2); | ||
180 | axis[1].NearestPointInLine(triangle1, triangle0, triangle2); | ||
181 | axis[2].NearestPointInLine(triangle2, triangle0, triangle1); | ||
182 | |||
183 | float3 axisDot = new float3(); | ||
184 | axisDot.x = dot(triangle0 - axis[0], point - axis[0]); | ||
185 | axisDot.y = dot(triangle1 - axis[1], point - axis[1]); | ||
186 | axisDot.z = dot(triangle2 - axis[2], point - axis[2]); | ||
187 | |||
188 | bool bForce = true; | ||
189 | float bestMagnitude2 = 0; | ||
190 | float closeMagnitude2; | ||
191 | float3 closePoint = new float3(); | ||
192 | |||
193 | if (axisDot.x < 0f) | ||
194 | { | ||
195 | closePoint.NearestPointInLineSegment(point, triangle1, triangle2); | ||
196 | closeMagnitude2 = point.Distance2(closePoint); | ||
197 | if (bForce || (bestMagnitude2 > closeMagnitude2)) | ||
198 | { | ||
199 | bForce = false; | ||
200 | bestMagnitude2 = closeMagnitude2; | ||
201 | nearestPoint = closePoint; | ||
202 | } | ||
203 | } | ||
204 | if (axisDot.y < 0f) | ||
205 | { | ||
206 | closePoint.NearestPointInLineSegment(point, triangle0, triangle2); | ||
207 | closeMagnitude2 = point.Distance2(closePoint); | ||
208 | if (bForce || (bestMagnitude2 > closeMagnitude2)) | ||
209 | { | ||
210 | bForce = false; | ||
211 | bestMagnitude2 = closeMagnitude2; | ||
212 | nearestPoint = closePoint; | ||
213 | } | ||
214 | } | ||
215 | if (axisDot.z < 0f) | ||
216 | { | ||
217 | closePoint.NearestPointInLineSegment(point, triangle0, triangle1); | ||
218 | closeMagnitude2 = point.Distance2(closePoint); | ||
219 | if (bForce || (bestMagnitude2 > closeMagnitude2)) | ||
220 | { | ||
221 | bForce = false; | ||
222 | bestMagnitude2 = closeMagnitude2; | ||
223 | nearestPoint = closePoint; | ||
224 | } | ||
225 | } | ||
226 | |||
227 | // If bForce is true at this point, it means the nearest point lies | ||
228 | // inside the triangle; use the nearest-point-on-a-plane equation | ||
229 | if (bForce) | ||
230 | { | ||
231 | float3 normal; | ||
232 | |||
233 | // Get the normal of the polygon (doesn't have to be a unit vector) | ||
234 | normal = float3.cross(lineDelta0, lineDelta1); | ||
235 | |||
236 | float3 pointDelta = point - triangle0; | ||
237 | float delta = float3.dot(normal, pointDelta) / float3.dot(normal, normal); | ||
238 | |||
239 | nearestPoint = point - normal * delta; | ||
240 | } | ||
241 | } | ||
242 | |||
243 | this.x = nearestPoint.x; | ||
244 | this.y = nearestPoint.y; | ||
245 | this.z = nearestPoint.z; | ||
246 | } | ||
247 | |||
248 | public static float3 operator +(float3 a, float3 b) | ||
249 | { | ||
250 | return new float3(a.x + b.x, a.y + b.y, a.z + b.z); | ||
251 | } | ||
252 | |||
253 | public static float3 operator -(float3 a, float3 b) | ||
254 | { | ||
255 | return new float3(a.x - b.x, a.y - b.y, a.z - b.z); | ||
256 | } | ||
257 | |||
258 | public static float3 operator -(float3 a, float s) | ||
259 | { | ||
260 | return new float3(a.x - s, a.y - s, a.z - s); | ||
261 | } | ||
262 | |||
263 | public static float3 operator -(float3 v) | ||
264 | { | ||
265 | return new float3(-v.x, -v.y, -v.z); | ||
266 | } | ||
267 | |||
268 | public static float3 operator *(float3 v, float s) | ||
269 | { | ||
270 | return new float3(v.x * s, v.y * s, v.z * s); | ||
271 | } | ||
272 | |||
273 | public static float3 operator *(float s, float3 v) | ||
274 | { | ||
275 | return new float3(v.x * s, v.y * s, v.z * s); | ||
276 | } | ||
277 | |||
278 | public static float3 operator *(float3 v, float3x3 m) | ||
279 | { | ||
280 | return new float3((m.x.x * v.x + m.y.x * v.y + m.z.x * v.z), (m.x.y * v.x + m.y.y * v.y + m.z.y * v.z), (m.x.z * v.x + m.y.z * v.y + m.z.z * v.z)); | ||
281 | } | ||
282 | |||
283 | public static float3 operator *(float3x3 m, float3 v) | ||
284 | { | ||
285 | return new float3(dot(m.x, v), dot(m.y, v), dot(m.z, v)); | ||
286 | } | ||
287 | |||
288 | public static float3 operator /(float3 v, float s) | ||
289 | { | ||
290 | float sinv = 1.0f / s; | ||
291 | return new float3(v.x * sinv, v.y * sinv, v.z * sinv); | ||
292 | } | ||
293 | |||
294 | public bool Equals(float3 other) | ||
295 | { | ||
296 | return this == other; | ||
297 | } | ||
298 | |||
299 | public override bool Equals(object obj) | ||
300 | { | ||
301 | float3 f = obj as float3; | ||
302 | if (f == null) | ||
303 | return false; | ||
304 | |||
305 | return this == f; | ||
306 | } | ||
307 | |||
308 | public override int GetHashCode() | ||
309 | { | ||
310 | return x.GetHashCode() ^ y.GetHashCode() ^ z.GetHashCode(); | ||
311 | } | ||
312 | |||
313 | public static bool operator ==(float3 a, float3 b) | ||
314 | { | ||
315 | // If both are null, or both are same instance, return true. | ||
316 | if (System.Object.ReferenceEquals(a, b)) | ||
317 | return true; | ||
318 | // If one is null, but not both, return false. | ||
319 | if (((object)a == null) || ((object)b == null)) | ||
320 | return false; | ||
321 | |||
322 | return (a.x == b.x && a.y == b.y && a.z == b.z); | ||
323 | } | ||
324 | |||
325 | public static bool operator !=(float3 a, float3 b) | ||
326 | { | ||
327 | return (a.x != b.x || a.y != b.y || a.z != b.z); | ||
328 | } | ||
329 | |||
330 | public static float dot(float3 a, float3 b) | ||
331 | { | ||
332 | return a.x * b.x + a.y * b.y + a.z * b.z; | ||
333 | } | ||
334 | |||
335 | public static float3 cmul(float3 v1, float3 v2) | ||
336 | { | ||
337 | return new float3(v1.x * v2.x, v1.y * v2.y, v1.z * v2.z); | ||
338 | } | ||
339 | |||
340 | public static float3 cross(float3 a, float3 b) | ||
341 | { | ||
342 | return new float3(a.y * b.z - a.z * b.y, a.z * b.x - a.x * b.z, a.x * b.y - a.y * b.x); | ||
343 | } | ||
344 | |||
345 | public static float3 Interpolate(float3 v0, float3 v1, float alpha) | ||
346 | { | ||
347 | return v0 * (1 - alpha) + v1 * alpha; | ||
348 | } | ||
349 | |||
350 | public static float3 Round(float3 a, int digits) | ||
351 | { | ||
352 | return new float3((float)Math.Round(a.x, digits), (float)Math.Round(a.y, digits), (float)Math.Round(a.z, digits)); | ||
353 | } | ||
354 | |||
355 | public static float3 VectorMax(float3 a, float3 b) | ||
356 | { | ||
357 | return new float3(Math.Max(a.x, b.x), Math.Max(a.y, b.y), Math.Max(a.z, b.z)); | ||
358 | } | ||
359 | |||
360 | public static float3 VectorMin(float3 a, float3 b) | ||
361 | { | ||
362 | return new float3(Math.Min(a.x, b.x), Math.Min(a.y, b.y), Math.Min(a.z, b.z)); | ||
363 | } | ||
364 | |||
365 | public static float3 vabs(float3 v) | ||
366 | { | ||
367 | return new float3(Math.Abs(v.x), Math.Abs(v.y), Math.Abs(v.z)); | ||
368 | } | ||
369 | |||
370 | public static float magnitude(float3 v) | ||
371 | { | ||
372 | return (float)Math.Sqrt(v.x * v.x + v.y * v.y + v.z * v.z); | ||
373 | } | ||
374 | |||
375 | public static float3 normalize(float3 v) | ||
376 | { | ||
377 | float d = magnitude(v); | ||
378 | if (d == 0) | ||
379 | d = 0.1f; | ||
380 | d = 1 / d; | ||
381 | return new float3(v.x * d, v.y * d, v.z * d); | ||
382 | } | ||
383 | |||
384 | public static float3 safenormalize(float3 v) | ||
385 | { | ||
386 | if (magnitude(v) <= 0.0f) | ||
387 | return new float3(1, 0, 0); | ||
388 | else | ||
389 | return normalize(v); | ||
390 | } | ||
391 | |||
392 | public static float Yaw(float3 v) | ||
393 | { | ||
394 | return (v.y == 0.0 && v.x == 0.0) ? 0.0f : (float)Math.Atan2(-v.x, v.y) * (180.0f / 3.14159264f); | ||
395 | } | ||
396 | |||
397 | public static float Pitch(float3 v) | ||
398 | { | ||
399 | return (float)Math.Atan2(v.z, Math.Sqrt(v.x * v.x + v.y * v.y)) * (180.0f / 3.14159264f); | ||
400 | } | ||
401 | |||
402 | public float ComputePlane(float3 A, float3 B, float3 C) | ||
403 | { | ||
404 | float vx, vy, vz, wx, wy, wz, vw_x, vw_y, vw_z, mag; | ||
405 | |||
406 | vx = (B.x - C.x); | ||
407 | vy = (B.y - C.y); | ||
408 | vz = (B.z - C.z); | ||
409 | |||
410 | wx = (A.x - B.x); | ||
411 | wy = (A.y - B.y); | ||
412 | wz = (A.z - B.z); | ||
413 | |||
414 | vw_x = vy * wz - vz * wy; | ||
415 | vw_y = vz * wx - vx * wz; | ||
416 | vw_z = vx * wy - vy * wx; | ||
417 | |||
418 | mag = (float)Math.Sqrt((vw_x * vw_x) + (vw_y * vw_y) + (vw_z * vw_z)); | ||
419 | |||
420 | if (mag < 0.000001f) | ||
421 | { | ||
422 | mag = 0; | ||
423 | } | ||
424 | else | ||
425 | { | ||
426 | mag = 1.0f / mag; | ||
427 | } | ||
428 | |||
429 | x = vw_x * mag; | ||
430 | y = vw_y * mag; | ||
431 | z = vw_z * mag; | ||
432 | |||
433 | float D = 0.0f - ((x * A.x) + (y * A.y) + (z * A.z)); | ||
434 | return D; | ||
435 | } | ||
436 | |||
437 | public override string ToString() | ||
438 | { | ||
439 | return String.Format("<{0}, {1}, {2}>", x, y, z); | ||
440 | } | ||
441 | |||
442 | public static readonly float3 Zero = new float3(); | ||
443 | } | ||
444 | } | ||
diff --git a/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/float3x3.cs b/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/float3x3.cs new file mode 100644 index 0000000..76cf063 --- /dev/null +++ b/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/float3x3.cs | |||
@@ -0,0 +1,195 @@ | |||
1 | /* The MIT License | ||
2 | * | ||
3 | * Copyright (c) 2010 Intel Corporation. | ||
4 | * All rights reserved. | ||
5 | * | ||
6 | * Based on the convexdecomposition library from | ||
7 | * <http://codesuppository.googlecode.com> by John W. Ratcliff and Stan Melax. | ||
8 | * | ||
9 | * Permission is hereby granted, free of charge, to any person obtaining a copy | ||
10 | * of this software and associated documentation files (the "Software"), to deal | ||
11 | * in the Software without restriction, including without limitation the rights | ||
12 | * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell | ||
13 | * copies of the Software, and to permit persons to whom the Software is | ||
14 | * furnished to do so, subject to the following conditions: | ||
15 | * | ||
16 | * The above copyright notice and this permission notice shall be included in | ||
17 | * all copies or substantial portions of the Software. | ||
18 | * | ||
19 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR | ||
20 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, | ||
21 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE | ||
22 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER | ||
23 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, | ||
24 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN | ||
25 | * THE SOFTWARE. | ||
26 | */ | ||
27 | |||
28 | using System; | ||
29 | using System.Collections.Generic; | ||
30 | using System.Diagnostics; | ||
31 | |||
32 | namespace OpenSim.Region.Physics.ConvexDecompositionDotNet | ||
33 | { | ||
34 | public class float3x3 | ||
35 | { | ||
36 | public float3 x = new float3(); | ||
37 | public float3 y = new float3(); | ||
38 | public float3 z = new float3(); | ||
39 | |||
40 | public float3x3() | ||
41 | { | ||
42 | } | ||
43 | |||
44 | public float3x3(float xx, float xy, float xz, float yx, float yy, float yz, float zx, float zy, float zz) | ||
45 | { | ||
46 | x = new float3(xx, xy, xz); | ||
47 | y = new float3(yx, yy, yz); | ||
48 | z = new float3(zx, zy, zz); | ||
49 | } | ||
50 | |||
51 | public float3x3(float3 _x, float3 _y, float3 _z) | ||
52 | { | ||
53 | x = new float3(_x); | ||
54 | y = new float3(_y); | ||
55 | z = new float3(_z); | ||
56 | } | ||
57 | |||
58 | public float3 this[int i] | ||
59 | { | ||
60 | get | ||
61 | { | ||
62 | switch (i) | ||
63 | { | ||
64 | case 0: return x; | ||
65 | case 1: return y; | ||
66 | case 2: return z; | ||
67 | } | ||
68 | throw new ArgumentOutOfRangeException(); | ||
69 | } | ||
70 | } | ||
71 | |||
72 | public float this[int i, int j] | ||
73 | { | ||
74 | get | ||
75 | { | ||
76 | switch (i) | ||
77 | { | ||
78 | case 0: | ||
79 | switch (j) | ||
80 | { | ||
81 | case 0: return x.x; | ||
82 | case 1: return x.y; | ||
83 | case 2: return x.z; | ||
84 | } | ||
85 | break; | ||
86 | case 1: | ||
87 | switch (j) | ||
88 | { | ||
89 | case 0: return y.x; | ||
90 | case 1: return y.y; | ||
91 | case 2: return y.z; | ||
92 | } | ||
93 | break; | ||
94 | case 2: | ||
95 | switch (j) | ||
96 | { | ||
97 | case 0: return z.x; | ||
98 | case 1: return z.y; | ||
99 | case 2: return z.z; | ||
100 | } | ||
101 | break; | ||
102 | } | ||
103 | throw new ArgumentOutOfRangeException(); | ||
104 | } | ||
105 | set | ||
106 | { | ||
107 | switch (i) | ||
108 | { | ||
109 | case 0: | ||
110 | switch (j) | ||
111 | { | ||
112 | case 0: x.x = value; return; | ||
113 | case 1: x.y = value; return; | ||
114 | case 2: x.z = value; return; | ||
115 | } | ||
116 | break; | ||
117 | case 1: | ||
118 | switch (j) | ||
119 | { | ||
120 | case 0: y.x = value; return; | ||
121 | case 1: y.y = value; return; | ||
122 | case 2: y.z = value; return; | ||
123 | } | ||
124 | break; | ||
125 | case 2: | ||
126 | switch (j) | ||
127 | { | ||
128 | case 0: z.x = value; return; | ||
129 | case 1: z.y = value; return; | ||
130 | case 2: z.z = value; return; | ||
131 | } | ||
132 | break; | ||
133 | } | ||
134 | throw new ArgumentOutOfRangeException(); | ||
135 | } | ||
136 | } | ||
137 | |||
138 | public static float3x3 Transpose(float3x3 m) | ||
139 | { | ||
140 | return new float3x3(new float3(m.x.x, m.y.x, m.z.x), new float3(m.x.y, m.y.y, m.z.y), new float3(m.x.z, m.y.z, m.z.z)); | ||
141 | } | ||
142 | |||
143 | public static float3x3 operator *(float3x3 a, float3x3 b) | ||
144 | { | ||
145 | return new float3x3(a.x * b, a.y * b, a.z * b); | ||
146 | } | ||
147 | |||
148 | public static float3x3 operator *(float3x3 a, float s) | ||
149 | { | ||
150 | return new float3x3(a.x * s, a.y * s, a.z * s); | ||
151 | } | ||
152 | |||
153 | public static float3x3 operator /(float3x3 a, float s) | ||
154 | { | ||
155 | float t = 1f / s; | ||
156 | return new float3x3(a.x * t, a.y * t, a.z * t); | ||
157 | } | ||
158 | |||
159 | public static float3x3 operator +(float3x3 a, float3x3 b) | ||
160 | { | ||
161 | return new float3x3(a.x + b.x, a.y + b.y, a.z + b.z); | ||
162 | } | ||
163 | |||
164 | public static float3x3 operator -(float3x3 a, float3x3 b) | ||
165 | { | ||
166 | return new float3x3(a.x - b.x, a.y - b.y, a.z - b.z); | ||
167 | } | ||
168 | |||
169 | public static float Determinant(float3x3 m) | ||
170 | { | ||
171 | return m.x.x * m.y.y * m.z.z + m.y.x * m.z.y * m.x.z + m.z.x * m.x.y * m.y.z - m.x.x * m.z.y * m.y.z - m.y.x * m.x.y * m.z.z - m.z.x * m.y.y * m.x.z; | ||
172 | } | ||
173 | |||
174 | public static float3x3 Inverse(float3x3 a) | ||
175 | { | ||
176 | float3x3 b = new float3x3(); | ||
177 | float d = Determinant(a); | ||
178 | Debug.Assert(d != 0); | ||
179 | for (int i = 0; i < 3; i++) | ||
180 | { | ||
181 | for (int j = 0; j < 3; j++) | ||
182 | { | ||
183 | int i1 = (i + 1) % 3; | ||
184 | int i2 = (i + 2) % 3; | ||
185 | int j1 = (j + 1) % 3; | ||
186 | int j2 = (j + 2) % 3; | ||
187 | |||
188 | // reverse indexs i&j to take transpose | ||
189 | b[i, j] = (a[i1][j1] * a[i2][j2] - a[i1][j2] * a[i2][j1]) / d; | ||
190 | } | ||
191 | } | ||
192 | return b; | ||
193 | } | ||
194 | } | ||
195 | } | ||
diff --git a/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/float4.cs b/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/float4.cs new file mode 100644 index 0000000..fa60876 --- /dev/null +++ b/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/float4.cs | |||
@@ -0,0 +1,170 @@ | |||
1 | /* The MIT License | ||
2 | * | ||
3 | * Copyright (c) 2010 Intel Corporation. | ||
4 | * All rights reserved. | ||
5 | * | ||
6 | * Based on the convexdecomposition library from | ||
7 | * <http://codesuppository.googlecode.com> by John W. Ratcliff and Stan Melax. | ||
8 | * | ||
9 | * Permission is hereby granted, free of charge, to any person obtaining a copy | ||
10 | * of this software and associated documentation files (the "Software"), to deal | ||
11 | * in the Software without restriction, including without limitation the rights | ||
12 | * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell | ||
13 | * copies of the Software, and to permit persons to whom the Software is | ||
14 | * furnished to do so, subject to the following conditions: | ||
15 | * | ||
16 | * The above copyright notice and this permission notice shall be included in | ||
17 | * all copies or substantial portions of the Software. | ||
18 | * | ||
19 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR | ||
20 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, | ||
21 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE | ||
22 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER | ||
23 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, | ||
24 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN | ||
25 | * THE SOFTWARE. | ||
26 | */ | ||
27 | |||
28 | using System; | ||
29 | |||
30 | namespace OpenSim.Region.Physics.ConvexDecompositionDotNet | ||
31 | { | ||
32 | public class float4 | ||
33 | { | ||
34 | public float x; | ||
35 | public float y; | ||
36 | public float z; | ||
37 | public float w; | ||
38 | |||
39 | public float4() | ||
40 | { | ||
41 | x = 0; | ||
42 | y = 0; | ||
43 | z = 0; | ||
44 | w = 0; | ||
45 | } | ||
46 | |||
47 | public float4(float _x, float _y, float _z, float _w) | ||
48 | { | ||
49 | x = _x; | ||
50 | y = _y; | ||
51 | z = _z; | ||
52 | w = _w; | ||
53 | } | ||
54 | |||
55 | public float4(float3 v, float _w) | ||
56 | { | ||
57 | x = v.x; | ||
58 | y = v.y; | ||
59 | z = v.z; | ||
60 | w = _w; | ||
61 | } | ||
62 | |||
63 | public float4(float4 f) | ||
64 | { | ||
65 | x = f.x; | ||
66 | y = f.y; | ||
67 | z = f.z; | ||
68 | w = f.w; | ||
69 | } | ||
70 | |||
71 | public float this[int i] | ||
72 | { | ||
73 | get | ||
74 | { | ||
75 | switch (i) | ||
76 | { | ||
77 | case 0: return x; | ||
78 | case 1: return y; | ||
79 | case 2: return z; | ||
80 | case 3: return w; | ||
81 | } | ||
82 | throw new ArgumentOutOfRangeException(); | ||
83 | } | ||
84 | } | ||
85 | |||
86 | public float3 xyz() | ||
87 | { | ||
88 | return new float3(x, y, z); | ||
89 | } | ||
90 | |||
91 | public void setxyz(float3 xyz) | ||
92 | { | ||
93 | x = xyz.x; | ||
94 | y = xyz.y; | ||
95 | z = xyz.z; | ||
96 | } | ||
97 | |||
98 | public override int GetHashCode() | ||
99 | { | ||
100 | return x.GetHashCode() ^ y.GetHashCode() ^ z.GetHashCode() ^ w.GetHashCode(); | ||
101 | } | ||
102 | |||
103 | public override bool Equals(object obj) | ||
104 | { | ||
105 | float4 f = obj as float4; | ||
106 | if (f == null) | ||
107 | return false; | ||
108 | |||
109 | return this == f; | ||
110 | } | ||
111 | |||
112 | public static float4 Homogenize(float3 v3) | ||
113 | { | ||
114 | return Homogenize(v3, 1.0f); | ||
115 | } | ||
116 | |||
117 | //C++ TO C# CONVERTER NOTE: C# does not allow default values for parameters. Overloaded methods are inserted above. | ||
118 | //ORIGINAL LINE: float4 Homogenize(const float3 &v3, const float &w =1.0f) | ||
119 | public static float4 Homogenize(float3 v3, float w) | ||
120 | { | ||
121 | return new float4(v3.x, v3.y, v3.z, w); | ||
122 | } | ||
123 | |||
124 | public static float4 cmul(float4 a, float4 b) | ||
125 | { | ||
126 | return new float4(a.x * b.x, a.y * b.y, a.z * b.z, a.w * b.w); | ||
127 | } | ||
128 | |||
129 | public static float4 operator +(float4 a, float4 b) | ||
130 | { | ||
131 | return new float4(a.x + b.x, a.y + b.y, a.z + b.z, a.w + b.w); | ||
132 | } | ||
133 | public static float4 operator -(float4 a, float4 b) | ||
134 | { | ||
135 | return new float4(a.x - b.x, a.y - b.y, a.z - b.z, a.w - b.w); | ||
136 | } | ||
137 | |||
138 | public static float4 operator *(float4 v, float4x4 m) | ||
139 | { | ||
140 | return v.x * m.x + v.y * m.y + v.z * m.z + v.w * m.w; // yes this actually works | ||
141 | } | ||
142 | |||
143 | public static bool operator ==(float4 a, float4 b) | ||
144 | { | ||
145 | // If both are null, or both are same instance, return true. | ||
146 | if (System.Object.ReferenceEquals(a, b)) | ||
147 | return true; | ||
148 | // If one is null, but not both, return false. | ||
149 | if (((object)a == null) || ((object)b == null)) | ||
150 | return false; | ||
151 | |||
152 | return (a.x == b.x && a.y == b.y && a.z == b.z && a.w == b.w); | ||
153 | } | ||
154 | |||
155 | public static bool operator !=(float4 a, float4 b) | ||
156 | { | ||
157 | return !(a == b); | ||
158 | } | ||
159 | |||
160 | public static float4 operator *(float4 v, float s) | ||
161 | { | ||
162 | return new float4(v.x * s, v.y * s, v.z * s, v.w * s); | ||
163 | } | ||
164 | |||
165 | public static float4 operator *(float s, float4 v) | ||
166 | { | ||
167 | return new float4(v.x * s, v.y * s, v.z * s, v.w * s); | ||
168 | } | ||
169 | } | ||
170 | } | ||
diff --git a/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/float4x4.cs b/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/float4x4.cs new file mode 100644 index 0000000..7d1592f --- /dev/null +++ b/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/float4x4.cs | |||
@@ -0,0 +1,284 @@ | |||
1 | /* The MIT License | ||
2 | * | ||
3 | * Copyright (c) 2010 Intel Corporation. | ||
4 | * All rights reserved. | ||
5 | * | ||
6 | * Based on the convexdecomposition library from | ||
7 | * <http://codesuppository.googlecode.com> by John W. Ratcliff and Stan Melax. | ||
8 | * | ||
9 | * Permission is hereby granted, free of charge, to any person obtaining a copy | ||
10 | * of this software and associated documentation files (the "Software"), to deal | ||
11 | * in the Software without restriction, including without limitation the rights | ||
12 | * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell | ||
13 | * copies of the Software, and to permit persons to whom the Software is | ||
14 | * furnished to do so, subject to the following conditions: | ||
15 | * | ||
16 | * The above copyright notice and this permission notice shall be included in | ||
17 | * all copies or substantial portions of the Software. | ||
18 | * | ||
19 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR | ||
20 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, | ||
21 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE | ||
22 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER | ||
23 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, | ||
24 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN | ||
25 | * THE SOFTWARE. | ||
26 | */ | ||
27 | |||
28 | using System; | ||
29 | using System.Collections.Generic; | ||
30 | using System.Linq; | ||
31 | using System.Text; | ||
32 | |||
33 | namespace OpenSim.Region.Physics.ConvexDecompositionDotNet | ||
34 | { | ||
35 | public class float4x4 | ||
36 | { | ||
37 | public float4 x = new float4(); | ||
38 | public float4 y = new float4(); | ||
39 | public float4 z = new float4(); | ||
40 | public float4 w = new float4(); | ||
41 | |||
42 | public float4x4() | ||
43 | { | ||
44 | } | ||
45 | |||
46 | public float4x4(float4 _x, float4 _y, float4 _z, float4 _w) | ||
47 | { | ||
48 | x = new float4(_x); | ||
49 | y = new float4(_y); | ||
50 | z = new float4(_z); | ||
51 | w = new float4(_w); | ||
52 | } | ||
53 | |||
54 | public float4x4( | ||
55 | float m00, float m01, float m02, float m03, | ||
56 | float m10, float m11, float m12, float m13, | ||
57 | float m20, float m21, float m22, float m23, | ||
58 | float m30, float m31, float m32, float m33) | ||
59 | { | ||
60 | x = new float4(m00, m01, m02, m03); | ||
61 | y = new float4(m10, m11, m12, m13); | ||
62 | z = new float4(m20, m21, m22, m23); | ||
63 | w = new float4(m30, m31, m32, m33); | ||
64 | } | ||
65 | |||
66 | public float4x4(float4x4 m) | ||
67 | { | ||
68 | x = new float4(m.x); | ||
69 | y = new float4(m.y); | ||
70 | z = new float4(m.z); | ||
71 | w = new float4(m.w); | ||
72 | } | ||
73 | |||
74 | public float4 this[int i] | ||
75 | { | ||
76 | get | ||
77 | { | ||
78 | switch (i) | ||
79 | { | ||
80 | case 0: return x; | ||
81 | case 1: return y; | ||
82 | case 2: return z; | ||
83 | case 3: return w; | ||
84 | } | ||
85 | throw new ArgumentOutOfRangeException(); | ||
86 | } | ||
87 | set | ||
88 | { | ||
89 | switch (i) | ||
90 | { | ||
91 | case 0: x = value; return; | ||
92 | case 1: y = value; return; | ||
93 | case 2: z = value; return; | ||
94 | case 3: w = value; return; | ||
95 | } | ||
96 | throw new ArgumentOutOfRangeException(); | ||
97 | } | ||
98 | } | ||
99 | |||
100 | public override int GetHashCode() | ||
101 | { | ||
102 | return x.GetHashCode() ^ y.GetHashCode() ^ z.GetHashCode() ^ w.GetHashCode(); | ||
103 | } | ||
104 | |||
105 | public override bool Equals(object obj) | ||
106 | { | ||
107 | float4x4 m = obj as float4x4; | ||
108 | if (m == null) | ||
109 | return false; | ||
110 | |||
111 | return this == m; | ||
112 | } | ||
113 | |||
114 | public static float4x4 operator *(float4x4 a, float4x4 b) | ||
115 | { | ||
116 | return new float4x4(a.x * b, a.y * b, a.z * b, a.w * b); | ||
117 | } | ||
118 | |||
119 | public static bool operator ==(float4x4 a, float4x4 b) | ||
120 | { | ||
121 | return (a.x == b.x && a.y == b.y && a.z == b.z && a.w == b.w); | ||
122 | } | ||
123 | |||
124 | public static bool operator !=(float4x4 a, float4x4 b) | ||
125 | { | ||
126 | return !(a == b); | ||
127 | } | ||
128 | |||
129 | public static float4x4 Inverse(float4x4 m) | ||
130 | { | ||
131 | float4x4 d = new float4x4(); | ||
132 | //float dst = d.x.x; | ||
133 | float[] tmp = new float[12]; // temp array for pairs | ||
134 | float[] src = new float[16]; // array of transpose source matrix | ||
135 | float det; // determinant | ||
136 | // transpose matrix | ||
137 | for (int i = 0; i < 4; i++) | ||
138 | { | ||
139 | src[i] = m[i].x; | ||
140 | src[i + 4] = m[i].y; | ||
141 | src[i + 8] = m[i].z; | ||
142 | src[i + 12] = m[i].w; | ||
143 | } | ||
144 | // calculate pairs for first 8 elements (cofactors) | ||
145 | tmp[0] = src[10] * src[15]; | ||
146 | tmp[1] = src[11] * src[14]; | ||
147 | tmp[2] = src[9] * src[15]; | ||
148 | tmp[3] = src[11] * src[13]; | ||
149 | tmp[4] = src[9] * src[14]; | ||
150 | tmp[5] = src[10] * src[13]; | ||
151 | tmp[6] = src[8] * src[15]; | ||
152 | tmp[7] = src[11] * src[12]; | ||
153 | tmp[8] = src[8] * src[14]; | ||
154 | tmp[9] = src[10] * src[12]; | ||
155 | tmp[10] = src[8] * src[13]; | ||
156 | tmp[11] = src[9] * src[12]; | ||
157 | // calculate first 8 elements (cofactors) | ||
158 | d.x.x = tmp[0]*src[5] + tmp[3]*src[6] + tmp[4]*src[7]; | ||
159 | d.x.x -= tmp[1]*src[5] + tmp[2]*src[6] + tmp[5]*src[7]; | ||
160 | d.x.y = tmp[1]*src[4] + tmp[6]*src[6] + tmp[9]*src[7]; | ||
161 | d.x.y -= tmp[0]*src[4] + tmp[7]*src[6] + tmp[8]*src[7]; | ||
162 | d.x.z = tmp[2]*src[4] + tmp[7]*src[5] + tmp[10]*src[7]; | ||
163 | d.x.z -= tmp[3]*src[4] + tmp[6]*src[5] + tmp[11]*src[7]; | ||
164 | d.x.w = tmp[5]*src[4] + tmp[8]*src[5] + tmp[11]*src[6]; | ||
165 | d.x.w -= tmp[4]*src[4] + tmp[9]*src[5] + tmp[10]*src[6]; | ||
166 | d.y.x = tmp[1]*src[1] + tmp[2]*src[2] + tmp[5]*src[3]; | ||
167 | d.y.x -= tmp[0]*src[1] + tmp[3]*src[2] + tmp[4]*src[3]; | ||
168 | d.y.y = tmp[0]*src[0] + tmp[7]*src[2] + tmp[8]*src[3]; | ||
169 | d.y.y -= tmp[1]*src[0] + tmp[6]*src[2] + tmp[9]*src[3]; | ||
170 | d.y.z = tmp[3]*src[0] + tmp[6]*src[1] + tmp[11]*src[3]; | ||
171 | d.y.z -= tmp[2]*src[0] + tmp[7]*src[1] + tmp[10]*src[3]; | ||
172 | d.y.w = tmp[4]*src[0] + tmp[9]*src[1] + tmp[10]*src[2]; | ||
173 | d.y.w -= tmp[5]*src[0] + tmp[8]*src[1] + tmp[11]*src[2]; | ||
174 | // calculate pairs for second 8 elements (cofactors) | ||
175 | tmp[0] = src[2]*src[7]; | ||
176 | tmp[1] = src[3]*src[6]; | ||
177 | tmp[2] = src[1]*src[7]; | ||
178 | tmp[3] = src[3]*src[5]; | ||
179 | tmp[4] = src[1]*src[6]; | ||
180 | tmp[5] = src[2]*src[5]; | ||
181 | tmp[6] = src[0]*src[7]; | ||
182 | tmp[7] = src[3]*src[4]; | ||
183 | tmp[8] = src[0]*src[6]; | ||
184 | tmp[9] = src[2]*src[4]; | ||
185 | tmp[10] = src[0]*src[5]; | ||
186 | tmp[11] = src[1]*src[4]; | ||
187 | // calculate second 8 elements (cofactors) | ||
188 | d.z.x = tmp[0]*src[13] + tmp[3]*src[14] + tmp[4]*src[15]; | ||
189 | d.z.x -= tmp[1]*src[13] + tmp[2]*src[14] + tmp[5]*src[15]; | ||
190 | d.z.y = tmp[1]*src[12] + tmp[6]*src[14] + tmp[9]*src[15]; | ||
191 | d.z.y -= tmp[0]*src[12] + tmp[7]*src[14] + tmp[8]*src[15]; | ||
192 | d.z.z = tmp[2]*src[12] + tmp[7]*src[13] + tmp[10]*src[15]; | ||
193 | d.z.z -= tmp[3]*src[12] + tmp[6]*src[13] + tmp[11]*src[15]; | ||
194 | d.z.w = tmp[5]*src[12] + tmp[8]*src[13] + tmp[11]*src[14]; | ||
195 | d.z.w-= tmp[4]*src[12] + tmp[9]*src[13] + tmp[10]*src[14]; | ||
196 | d.w.x = tmp[2]*src[10] + tmp[5]*src[11] + tmp[1]*src[9]; | ||
197 | d.w.x-= tmp[4]*src[11] + tmp[0]*src[9] + tmp[3]*src[10]; | ||
198 | d.w.y = tmp[8]*src[11] + tmp[0]*src[8] + tmp[7]*src[10]; | ||
199 | d.w.y-= tmp[6]*src[10] + tmp[9]*src[11] + tmp[1]*src[8]; | ||
200 | d.w.z = tmp[6]*src[9] + tmp[11]*src[11] + tmp[3]*src[8]; | ||
201 | d.w.z-= tmp[10]*src[11] + tmp[2]*src[8] + tmp[7]*src[9]; | ||
202 | d.w.w = tmp[10]*src[10] + tmp[4]*src[8] + tmp[9]*src[9]; | ||
203 | d.w.w-= tmp[8]*src[9] + tmp[11]*src[10] + tmp[5]*src[8]; | ||
204 | // calculate determinant | ||
205 | det = src[0] * d.x.x + src[1] * d.x.y + src[2] * d.x.z + src[3] * d.x.w; | ||
206 | // calculate matrix inverse | ||
207 | det = 1/det; | ||
208 | for (int j = 0; j < 4; j++) | ||
209 | d[j] *= det; | ||
210 | return d; | ||
211 | } | ||
212 | |||
213 | public static float4x4 MatrixRigidInverse(float4x4 m) | ||
214 | { | ||
215 | float4x4 trans_inverse = MatrixTranslation(-m.w.xyz()); | ||
216 | float4x4 rot = new float4x4(m); | ||
217 | rot.w = new float4(0f, 0f, 0f, 1f); | ||
218 | return trans_inverse * MatrixTranspose(rot); | ||
219 | } | ||
220 | public static float4x4 MatrixTranspose(float4x4 m) | ||
221 | { | ||
222 | return new float4x4(m.x.x, m.y.x, m.z.x, m.w.x, m.x.y, m.y.y, m.z.y, m.w.y, m.x.z, m.y.z, m.z.z, m.w.z, m.x.w, m.y.w, m.z.w, m.w.w); | ||
223 | } | ||
224 | public static float4x4 MatrixPerspectiveFov(float fovy, float aspect, float zn, float zf) | ||
225 | { | ||
226 | float h = 1.0f / (float)Math.Tan(fovy / 2.0f); // view space height | ||
227 | float w = h / aspect; // view space width | ||
228 | return new float4x4(w, 0, 0, 0, 0, h, 0, 0, 0, 0, zf / (zn - zf), -1, 0, 0, zn * zf / (zn - zf), 0); | ||
229 | } | ||
230 | public static float4x4 MatrixTranslation(float3 t) | ||
231 | { | ||
232 | return new float4x4(1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, t.x, t.y, t.z, 1); | ||
233 | } | ||
234 | public static float4x4 MatrixRotationZ(float angle_radians) | ||
235 | { | ||
236 | float s = (float)Math.Sin(angle_radians); | ||
237 | float c = (float)Math.Cos(angle_radians); | ||
238 | return new float4x4(c, s, 0, 0, -s, c, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1); | ||
239 | } | ||
240 | public static float4x4 MatrixLookAt(float3 eye, float3 at, float3 up) | ||
241 | { | ||
242 | float4x4 m = new float4x4(); | ||
243 | m.w.w = 1.0f; | ||
244 | m.w.setxyz(eye); | ||
245 | m.z.setxyz(float3.normalize(eye - at)); | ||
246 | m.x.setxyz(float3.normalize(float3.cross(up, m.z.xyz()))); | ||
247 | m.y.setxyz(float3.cross(m.z.xyz(), m.x.xyz())); | ||
248 | return MatrixRigidInverse(m); | ||
249 | } | ||
250 | |||
251 | public static float4x4 MatrixFromQuatVec(Quaternion q, float3 v) | ||
252 | { | ||
253 | // builds a 4x4 transformation matrix based on orientation q and translation v | ||
254 | float qx2 = q.x * q.x; | ||
255 | float qy2 = q.y * q.y; | ||
256 | float qz2 = q.z * q.z; | ||
257 | |||
258 | float qxqy = q.x * q.y; | ||
259 | float qxqz = q.x * q.z; | ||
260 | float qxqw = q.x * q.w; | ||
261 | float qyqz = q.y * q.z; | ||
262 | float qyqw = q.y * q.w; | ||
263 | float qzqw = q.z * q.w; | ||
264 | |||
265 | return new float4x4( | ||
266 | 1 - 2 * (qy2 + qz2), | ||
267 | 2 * (qxqy + qzqw), | ||
268 | 2 * (qxqz - qyqw), | ||
269 | 0, | ||
270 | 2 * (qxqy - qzqw), | ||
271 | 1 - 2 * (qx2 + qz2), | ||
272 | 2 * (qyqz + qxqw), | ||
273 | 0, | ||
274 | 2 * (qxqz + qyqw), | ||
275 | 2 * (qyqz - qxqw), | ||
276 | 1 - 2 * (qx2 + qy2), | ||
277 | 0, | ||
278 | v.x, | ||
279 | v.y, | ||
280 | v.z, | ||
281 | 1.0f); | ||
282 | } | ||
283 | } | ||
284 | } | ||
diff --git a/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/int3.cs b/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/int3.cs new file mode 100644 index 0000000..9c5760d --- /dev/null +++ b/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/int3.cs | |||
@@ -0,0 +1,128 @@ | |||
1 | /* The MIT License | ||
2 | * | ||
3 | * Copyright (c) 2010 Intel Corporation. | ||
4 | * All rights reserved. | ||
5 | * | ||
6 | * Based on the convexdecomposition library from | ||
7 | * <http://codesuppository.googlecode.com> by John W. Ratcliff and Stan Melax. | ||
8 | * | ||
9 | * Permission is hereby granted, free of charge, to any person obtaining a copy | ||
10 | * of this software and associated documentation files (the "Software"), to deal | ||
11 | * in the Software without restriction, including without limitation the rights | ||
12 | * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell | ||
13 | * copies of the Software, and to permit persons to whom the Software is | ||
14 | * furnished to do so, subject to the following conditions: | ||
15 | * | ||
16 | * The above copyright notice and this permission notice shall be included in | ||
17 | * all copies or substantial portions of the Software. | ||
18 | * | ||
19 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR | ||
20 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, | ||
21 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE | ||
22 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER | ||
23 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, | ||
24 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN | ||
25 | * THE SOFTWARE. | ||
26 | */ | ||
27 | |||
28 | using System; | ||
29 | |||
30 | namespace OpenSim.Region.Physics.ConvexDecompositionDotNet | ||
31 | { | ||
32 | public class int3 | ||
33 | { | ||
34 | public int x; | ||
35 | public int y; | ||
36 | public int z; | ||
37 | |||
38 | public int3() | ||
39 | { | ||
40 | } | ||
41 | |||
42 | public int3(int _x, int _y, int _z) | ||
43 | { | ||
44 | x = _x; | ||
45 | y = _y; | ||
46 | z = _z; | ||
47 | } | ||
48 | |||
49 | public int this[int i] | ||
50 | { | ||
51 | get | ||
52 | { | ||
53 | switch (i) | ||
54 | { | ||
55 | case 0: return x; | ||
56 | case 1: return y; | ||
57 | case 2: return z; | ||
58 | } | ||
59 | throw new ArgumentOutOfRangeException(); | ||
60 | } | ||
61 | set | ||
62 | { | ||
63 | switch (i) | ||
64 | { | ||
65 | case 0: x = value; return; | ||
66 | case 1: y = value; return; | ||
67 | case 2: z = value; return; | ||
68 | } | ||
69 | throw new ArgumentOutOfRangeException(); | ||
70 | } | ||
71 | } | ||
72 | |||
73 | public override int GetHashCode() | ||
74 | { | ||
75 | return x.GetHashCode() ^ y.GetHashCode() ^ z.GetHashCode(); | ||
76 | } | ||
77 | |||
78 | public override bool Equals(object obj) | ||
79 | { | ||
80 | int3 i = obj as int3; | ||
81 | if (i == null) | ||
82 | return false; | ||
83 | |||
84 | return this == i; | ||
85 | } | ||
86 | |||
87 | public static bool operator ==(int3 a, int3 b) | ||
88 | { | ||
89 | // If both are null, or both are same instance, return true. | ||
90 | if (System.Object.ReferenceEquals(a, b)) | ||
91 | return true; | ||
92 | // If one is null, but not both, return false. | ||
93 | if (((object)a == null) || ((object)b == null)) | ||
94 | return false; | ||
95 | |||
96 | for (int i = 0; i < 3; i++) | ||
97 | { | ||
98 | if (a[i] != b[i]) | ||
99 | return false; | ||
100 | } | ||
101 | return true; | ||
102 | } | ||
103 | |||
104 | public static bool operator !=(int3 a, int3 b) | ||
105 | { | ||
106 | return !(a == b); | ||
107 | } | ||
108 | |||
109 | public static int3 roll3(int3 a) | ||
110 | { | ||
111 | int tmp = a[0]; | ||
112 | a[0] = a[1]; | ||
113 | a[1] = a[2]; | ||
114 | a[2] = tmp; | ||
115 | return a; | ||
116 | } | ||
117 | |||
118 | public static bool isa(int3 a, int3 b) | ||
119 | { | ||
120 | return (a == b || roll3(a) == b || a == roll3(b)); | ||
121 | } | ||
122 | |||
123 | public static bool b2b(int3 a, int3 b) | ||
124 | { | ||
125 | return isa(a, new int3(b[2], b[1], b[0])); | ||
126 | } | ||
127 | } | ||
128 | } | ||
diff --git a/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/int4.cs b/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/int4.cs new file mode 100644 index 0000000..c2b32e5 --- /dev/null +++ b/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/int4.cs | |||
@@ -0,0 +1,66 @@ | |||
1 | /* The MIT License | ||
2 | * | ||
3 | * Copyright (c) 2010 Intel Corporation. | ||
4 | * All rights reserved. | ||
5 | * | ||
6 | * Based on the convexdecomposition library from | ||
7 | * <http://codesuppository.googlecode.com> by John W. Ratcliff and Stan Melax. | ||
8 | * | ||
9 | * Permission is hereby granted, free of charge, to any person obtaining a copy | ||
10 | * of this software and associated documentation files (the "Software"), to deal | ||
11 | * in the Software without restriction, including without limitation the rights | ||
12 | * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell | ||
13 | * copies of the Software, and to permit persons to whom the Software is | ||
14 | * furnished to do so, subject to the following conditions: | ||
15 | * | ||
16 | * The above copyright notice and this permission notice shall be included in | ||
17 | * all copies or substantial portions of the Software. | ||
18 | * | ||
19 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR | ||
20 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, | ||
21 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE | ||
22 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER | ||
23 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, | ||
24 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN | ||
25 | * THE SOFTWARE. | ||
26 | */ | ||
27 | |||
28 | using System; | ||
29 | |||
30 | namespace OpenSim.Region.Physics.ConvexDecompositionDotNet | ||
31 | { | ||
32 | public class int4 | ||
33 | { | ||
34 | public int x; | ||
35 | public int y; | ||
36 | public int z; | ||
37 | public int w; | ||
38 | |||
39 | public int4() | ||
40 | { | ||
41 | } | ||
42 | |||
43 | public int4(int _x, int _y, int _z, int _w) | ||
44 | { | ||
45 | x = _x; | ||
46 | y = _y; | ||
47 | z = _z; | ||
48 | w = _w; | ||
49 | } | ||
50 | |||
51 | public int this[int i] | ||
52 | { | ||
53 | get | ||
54 | { | ||
55 | switch (i) | ||
56 | { | ||
57 | case 0: return x; | ||
58 | case 1: return y; | ||
59 | case 2: return z; | ||
60 | case 3: return w; | ||
61 | } | ||
62 | throw new ArgumentOutOfRangeException(); | ||
63 | } | ||
64 | } | ||
65 | } | ||
66 | } | ||