float3.cs 14 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444
  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. using System;
  28. namespace OpenSim.Region.PhysicsModules.ConvexDecompositionDotNet
  29. {
  30. public class float3 : IEquatable<float3>
  31. {
  32. public float x;
  33. public float y;
  34. public float z;
  35. public float3()
  36. {
  37. x = 0;
  38. y = 0;
  39. z = 0;
  40. }
  41. public float3(float _x, float _y, float _z)
  42. {
  43. x = _x;
  44. y = _y;
  45. z = _z;
  46. }
  47. public float3(float3 f)
  48. {
  49. x = f.x;
  50. y = f.y;
  51. z = f.z;
  52. }
  53. public float this[int i]
  54. {
  55. get
  56. {
  57. switch (i)
  58. {
  59. case 0: return x;
  60. case 1: return y;
  61. case 2: return z;
  62. }
  63. throw new ArgumentOutOfRangeException();
  64. }
  65. }
  66. public float Distance(float3 a)
  67. {
  68. float3 d = new float3(a.x - x, a.y - y, a.z - z);
  69. return d.Length();
  70. }
  71. public float Distance2(float3 a)
  72. {
  73. float dx = a.x - x;
  74. float dy = a.y - y;
  75. float dz = a.z - z;
  76. return dx * dx + dy * dy + dz * dz;
  77. }
  78. public float Length()
  79. {
  80. return (float)Math.Sqrt(x * x + y * y + z * z);
  81. }
  82. public float Area(float3 p1, float3 p2)
  83. {
  84. float A = Partial(p1);
  85. A += p1.Partial(p2);
  86. A += p2.Partial(this);
  87. return A * 0.5f;
  88. }
  89. public float Partial(float3 p)
  90. {
  91. return (x * p.y) - (p.x * y);
  92. }
  93. // Given a point and a line (defined by two points), compute the closest point
  94. // in the line. (The line is treated as infinitely long.)
  95. public void NearestPointInLine(float3 point, float3 line0, float3 line1)
  96. {
  97. float3 nearestPoint = new float3();
  98. float3 lineDelta = line1 - line0;
  99. // Handle degenerate lines
  100. if (lineDelta == float3.Zero)
  101. {
  102. nearestPoint = line0;
  103. }
  104. else
  105. {
  106. float delta = float3.dot(point - line0, lineDelta) / float3.dot(lineDelta, lineDelta);
  107. nearestPoint = line0 + lineDelta * delta;
  108. }
  109. this.x = nearestPoint.x;
  110. this.y = nearestPoint.y;
  111. this.z = nearestPoint.z;
  112. }
  113. // Given a point and a line segment (defined by two points), compute the closest point
  114. // in the line. Cap the point at the endpoints of the line segment.
  115. public void NearestPointInLineSegment(float3 point, float3 line0, float3 line1)
  116. {
  117. float3 nearestPoint = new float3();
  118. float3 lineDelta = line1 - line0;
  119. // Handle degenerate lines
  120. if (lineDelta == Zero)
  121. {
  122. nearestPoint = line0;
  123. }
  124. else
  125. {
  126. float delta = float3.dot(point - line0, lineDelta) / float3.dot(lineDelta, lineDelta);
  127. // Clamp the point to conform to the segment's endpoints
  128. if (delta < 0)
  129. delta = 0;
  130. else if (delta > 1)
  131. delta = 1;
  132. nearestPoint = line0 + lineDelta * delta;
  133. }
  134. this.x = nearestPoint.x;
  135. this.y = nearestPoint.y;
  136. this.z = nearestPoint.z;
  137. }
  138. // Given a point and a triangle (defined by three points), compute the closest point
  139. // in the triangle. Clamp the point so it's confined to the area of the triangle.
  140. public void NearestPointInTriangle(float3 point, float3 triangle0, float3 triangle1, float3 triangle2)
  141. {
  142. float3 nearestPoint = new float3();
  143. float3 lineDelta0 = triangle1 - triangle0;
  144. float3 lineDelta1 = triangle2 - triangle0;
  145. // Handle degenerate triangles
  146. if ((lineDelta0 == Zero) || (lineDelta1 == Zero))
  147. {
  148. nearestPoint.NearestPointInLineSegment(point, triangle1, triangle2);
  149. }
  150. else if (lineDelta0 == lineDelta1)
  151. {
  152. nearestPoint.NearestPointInLineSegment(point, triangle0, triangle1);
  153. }
  154. else
  155. {
  156. float3[] axis = new float3[3] { new float3(), new float3(), new float3() };
  157. axis[0].NearestPointInLine(triangle0, triangle1, triangle2);
  158. axis[1].NearestPointInLine(triangle1, triangle0, triangle2);
  159. axis[2].NearestPointInLine(triangle2, triangle0, triangle1);
  160. float3 axisDot = new float3();
  161. axisDot.x = dot(triangle0 - axis[0], point - axis[0]);
  162. axisDot.y = dot(triangle1 - axis[1], point - axis[1]);
  163. axisDot.z = dot(triangle2 - axis[2], point - axis[2]);
  164. bool bForce = true;
  165. float bestMagnitude2 = 0;
  166. float closeMagnitude2;
  167. float3 closePoint = new float3();
  168. if (axisDot.x < 0f)
  169. {
  170. closePoint.NearestPointInLineSegment(point, triangle1, triangle2);
  171. closeMagnitude2 = point.Distance2(closePoint);
  172. if (bForce || (bestMagnitude2 > closeMagnitude2))
  173. {
  174. bForce = false;
  175. bestMagnitude2 = closeMagnitude2;
  176. nearestPoint = closePoint;
  177. }
  178. }
  179. if (axisDot.y < 0f)
  180. {
  181. closePoint.NearestPointInLineSegment(point, triangle0, triangle2);
  182. closeMagnitude2 = point.Distance2(closePoint);
  183. if (bForce || (bestMagnitude2 > closeMagnitude2))
  184. {
  185. bForce = false;
  186. bestMagnitude2 = closeMagnitude2;
  187. nearestPoint = closePoint;
  188. }
  189. }
  190. if (axisDot.z < 0f)
  191. {
  192. closePoint.NearestPointInLineSegment(point, triangle0, triangle1);
  193. closeMagnitude2 = point.Distance2(closePoint);
  194. if (bForce || (bestMagnitude2 > closeMagnitude2))
  195. {
  196. bForce = false;
  197. bestMagnitude2 = closeMagnitude2;
  198. nearestPoint = closePoint;
  199. }
  200. }
  201. // If bForce is true at this point, it means the nearest point lies
  202. // inside the triangle; use the nearest-point-on-a-plane equation
  203. if (bForce)
  204. {
  205. float3 normal;
  206. // Get the normal of the polygon (doesn't have to be a unit vector)
  207. normal = float3.cross(lineDelta0, lineDelta1);
  208. float3 pointDelta = point - triangle0;
  209. float delta = float3.dot(normal, pointDelta) / float3.dot(normal, normal);
  210. nearestPoint = point - normal * delta;
  211. }
  212. }
  213. this.x = nearestPoint.x;
  214. this.y = nearestPoint.y;
  215. this.z = nearestPoint.z;
  216. }
  217. public static float3 operator +(float3 a, float3 b)
  218. {
  219. return new float3(a.x + b.x, a.y + b.y, a.z + b.z);
  220. }
  221. public static float3 operator -(float3 a, float3 b)
  222. {
  223. return new float3(a.x - b.x, a.y - b.y, a.z - b.z);
  224. }
  225. public static float3 operator -(float3 a, float s)
  226. {
  227. return new float3(a.x - s, a.y - s, a.z - s);
  228. }
  229. public static float3 operator -(float3 v)
  230. {
  231. return new float3(-v.x, -v.y, -v.z);
  232. }
  233. public static float3 operator *(float3 v, float s)
  234. {
  235. return new float3(v.x * s, v.y * s, v.z * s);
  236. }
  237. public static float3 operator *(float s, float3 v)
  238. {
  239. return new float3(v.x * s, v.y * s, v.z * s);
  240. }
  241. public static float3 operator *(float3 v, float3x3 m)
  242. {
  243. 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));
  244. }
  245. public static float3 operator *(float3x3 m, float3 v)
  246. {
  247. return new float3(dot(m.x, v), dot(m.y, v), dot(m.z, v));
  248. }
  249. public static float3 operator /(float3 v, float s)
  250. {
  251. float sinv = 1.0f / s;
  252. return new float3(v.x * sinv, v.y * sinv, v.z * sinv);
  253. }
  254. public bool Equals(float3 other)
  255. {
  256. return this == other;
  257. }
  258. public override bool Equals(object obj)
  259. {
  260. float3 f = obj as float3;
  261. if (f == null)
  262. return false;
  263. return this == f;
  264. }
  265. public override int GetHashCode()
  266. {
  267. return x.GetHashCode() ^ y.GetHashCode() ^ z.GetHashCode();
  268. }
  269. public static bool operator ==(float3 a, float3 b)
  270. {
  271. // If both are null, or both are same instance, return true.
  272. if (System.Object.ReferenceEquals(a, b))
  273. return true;
  274. // If one is null, but not both, return false.
  275. if (((object)a == null) || ((object)b == null))
  276. return false;
  277. return (a.x == b.x && a.y == b.y && a.z == b.z);
  278. }
  279. public static bool operator !=(float3 a, float3 b)
  280. {
  281. return (a.x != b.x || a.y != b.y || a.z != b.z);
  282. }
  283. public static float dot(float3 a, float3 b)
  284. {
  285. return a.x * b.x + a.y * b.y + a.z * b.z;
  286. }
  287. public static float3 cmul(float3 v1, float3 v2)
  288. {
  289. return new float3(v1.x * v2.x, v1.y * v2.y, v1.z * v2.z);
  290. }
  291. public static float3 cross(float3 a, float3 b)
  292. {
  293. 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);
  294. }
  295. public static float3 Interpolate(float3 v0, float3 v1, float alpha)
  296. {
  297. return v0 * (1 - alpha) + v1 * alpha;
  298. }
  299. public static float3 Round(float3 a, int digits)
  300. {
  301. return new float3((float)Math.Round(a.x, digits), (float)Math.Round(a.y, digits), (float)Math.Round(a.z, digits));
  302. }
  303. public static float3 VectorMax(float3 a, float3 b)
  304. {
  305. return new float3(Math.Max(a.x, b.x), Math.Max(a.y, b.y), Math.Max(a.z, b.z));
  306. }
  307. public static float3 VectorMin(float3 a, float3 b)
  308. {
  309. return new float3(Math.Min(a.x, b.x), Math.Min(a.y, b.y), Math.Min(a.z, b.z));
  310. }
  311. public static float3 vabs(float3 v)
  312. {
  313. return new float3(Math.Abs(v.x), Math.Abs(v.y), Math.Abs(v.z));
  314. }
  315. public static float magnitude(float3 v)
  316. {
  317. return (float)Math.Sqrt(v.x * v.x + v.y * v.y + v.z * v.z);
  318. }
  319. public static float3 normalize(float3 v)
  320. {
  321. float d = magnitude(v);
  322. if (d == 0)
  323. d = 0.1f;
  324. d = 1 / d;
  325. return new float3(v.x * d, v.y * d, v.z * d);
  326. }
  327. public static float3 safenormalize(float3 v)
  328. {
  329. if (magnitude(v) <= 0.0f)
  330. return new float3(1, 0, 0);
  331. else
  332. return normalize(v);
  333. }
  334. public static float Yaw(float3 v)
  335. {
  336. return (v.y == 0.0 && v.x == 0.0) ? 0.0f : (float)Math.Atan2(-v.x, v.y) * (180.0f / 3.14159264f);
  337. }
  338. public static float Pitch(float3 v)
  339. {
  340. return (float)Math.Atan2(v.z, Math.Sqrt(v.x * v.x + v.y * v.y)) * (180.0f / 3.14159264f);
  341. }
  342. public float ComputePlane(float3 A, float3 B, float3 C)
  343. {
  344. float vx, vy, vz, wx, wy, wz, vw_x, vw_y, vw_z, mag;
  345. vx = (B.x - C.x);
  346. vy = (B.y - C.y);
  347. vz = (B.z - C.z);
  348. wx = (A.x - B.x);
  349. wy = (A.y - B.y);
  350. wz = (A.z - B.z);
  351. vw_x = vy * wz - vz * wy;
  352. vw_y = vz * wx - vx * wz;
  353. vw_z = vx * wy - vy * wx;
  354. mag = (float)Math.Sqrt((vw_x * vw_x) + (vw_y * vw_y) + (vw_z * vw_z));
  355. if (mag < 0.000001f)
  356. {
  357. mag = 0;
  358. }
  359. else
  360. {
  361. mag = 1.0f / mag;
  362. }
  363. x = vw_x * mag;
  364. y = vw_y * mag;
  365. z = vw_z * mag;
  366. float D = 0.0f - ((x * A.x) + (y * A.y) + (z * A.z));
  367. return D;
  368. }
  369. public override string ToString()
  370. {
  371. return String.Format("<{0}, {1}, {2}>", x, y, z);
  372. }
  373. public static readonly float3 Zero = new float3();
  374. }
  375. }