clockwerk-opensim – Blame information for rev 1

Subversion Repositories:
Rev:
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  
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 }