clockwerk-opensim – Blame information for rev 1
?pathlinks?
Rev | Author | Line No. | Line |
---|---|---|---|
1 | vero | 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 | } |