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