MathCM.cs 9.5 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396
  1. using System;
  2. using UnityEngine;
  3. public class MathCM
  4. {
  5. public static float calcPointLineDist(ref Vector3 p, ref MathCM.Line l, out Vector3 h, out float t)
  6. {
  7. float sqrMagnitude = l.v.sqrMagnitude;
  8. t = 0f;
  9. if (sqrMagnitude > 0f)
  10. {
  11. t = Vector3.Dot(l.v, p - l.s) / sqrMagnitude;
  12. }
  13. h = l.s + t * l.v;
  14. return (h - p).magnitude;
  15. }
  16. public static bool isSharpAngle(ref Vector3 p1, ref Vector3 p2, ref Vector3 p3)
  17. {
  18. return Vector3.Dot(p1 - p2, p3 - p2) >= 0f;
  19. }
  20. public static float calcPointSegmentDist(ref Vector3 p, ref MathCM.Segment seg, out Vector3 h, out float t)
  21. {
  22. Vector3 e = seg.e;
  23. MathCM.Line line = new MathCM.Line(seg.s, e);
  24. float result = MathCM.calcPointLineDist(ref p, ref line, out h, out t);
  25. if (!MathCM.isSharpAngle(ref p, ref seg.s, ref e))
  26. {
  27. h = seg.s;
  28. return (seg.s - p).magnitude;
  29. }
  30. if (!MathCM.isSharpAngle(ref p, ref e, ref seg.s))
  31. {
  32. h = e;
  33. return (e - p).magnitude;
  34. }
  35. return result;
  36. }
  37. public static bool isParallel(ref Vector3 v1, ref Vector3 r)
  38. {
  39. float sqrMagnitude = Vector3.Cross(v1, r).sqrMagnitude;
  40. return -MathCM._OX_EPSILON_ < sqrMagnitude && sqrMagnitude < MathCM._OX_EPSILON_;
  41. }
  42. public static float calcLineLineDist(MathCM.Line l1, MathCM.Line l2, out Vector3 p1, out Vector3 p2, out float t1, out float t2)
  43. {
  44. if (MathCM.isParallel(ref l1.v, ref l2.v))
  45. {
  46. float result = MathCM.calcPointLineDist(ref l1.s, ref l2, out p2, out t2);
  47. p1 = l1.s;
  48. t1 = 0f;
  49. return result;
  50. }
  51. float num = Vector3.Dot(l1.v, l2.v);
  52. float sqrMagnitude = l1.v.sqrMagnitude;
  53. float sqrMagnitude2 = l2.v.sqrMagnitude;
  54. Vector3 rhs = l1.s - l2.s;
  55. t1 = (num * Vector3.Dot(l2.v, rhs) - sqrMagnitude2 * Vector3.Dot(l1.v, rhs)) / (sqrMagnitude * sqrMagnitude2 - num * num);
  56. p1 = l1.getPoint(t1);
  57. t2 = Vector3.Dot(l2.v, p1 - l2.s) / sqrMagnitude2;
  58. p2 = l2.getPoint(t2);
  59. return (p2 - p1).magnitude;
  60. }
  61. public static float calcLineLineDistNoParallelCheck(ref MathCM.Segment l1, ref MathCM.Segment l2, out Vector3 p1, out Vector3 p2, out float t1, out float t2)
  62. {
  63. float num = Vector3.Dot(l1.v, l2.v);
  64. float sqrMagnitude = l1.v.sqrMagnitude;
  65. float sqrMagnitude2 = l2.v.sqrMagnitude;
  66. Vector3 rhs = l1.s - l2.s;
  67. t1 = (num * Vector3.Dot(l2.v, rhs) - sqrMagnitude2 * Vector3.Dot(l1.v, rhs)) / (sqrMagnitude * sqrMagnitude2 - num * num);
  68. p1 = l1.getPoint(t1);
  69. t2 = Vector3.Dot(l2.v, p1 - l2.s) / sqrMagnitude2;
  70. p2 = l2.getPoint(t2);
  71. return (p2 - p1).magnitude;
  72. }
  73. public static float calcSegmentSegmentDist(ref MathCM.Segment s1, ref MathCM.Segment s2, out Vector3 p1, out Vector3 p2, out float t1, out float t2)
  74. {
  75. if (s1.v.sqrMagnitude < MathCM._OX_EPSILON_)
  76. {
  77. float result;
  78. if (s2.v.sqrMagnitude < MathCM._OX_EPSILON_)
  79. {
  80. result = (s2.s - s1.s).magnitude;
  81. p1 = s1.s;
  82. p2 = s2.s;
  83. t1 = (t2 = 0f);
  84. return result;
  85. }
  86. result = MathCM.calcPointSegmentDist(ref s1.s, ref s2, out p2, out t2);
  87. p1 = s1.s;
  88. t1 = 0f;
  89. t2 = Mathf.Clamp01(t2);
  90. return result;
  91. }
  92. else
  93. {
  94. float result;
  95. if (s2.v.sqrMagnitude < MathCM._OX_EPSILON_)
  96. {
  97. result = MathCM.calcPointSegmentDist(ref s2.s, ref s1, out p1, out t1);
  98. p2 = s2.s;
  99. t1 = Mathf.Clamp01(t1);
  100. t2 = 0f;
  101. return result;
  102. }
  103. if (MathCM.isParallel(ref s1.v, ref s2.v))
  104. {
  105. t1 = 0f;
  106. p1 = s1.s;
  107. result = MathCM.calcPointSegmentDist(ref s1.s, ref s2, out p2, out t2);
  108. if (0f <= t2 && t2 <= 1f)
  109. {
  110. return result;
  111. }
  112. }
  113. else
  114. {
  115. result = MathCM.calcLineLineDistNoParallelCheck(ref s1, ref s2, out p1, out p2, out t1, out t2);
  116. if (0f <= t1 && t1 <= 1f && 0f <= t2 && t2 <= 1f)
  117. {
  118. return result;
  119. }
  120. }
  121. t1 = Mathf.Clamp01(t1);
  122. p1 = s1.getPoint(t1);
  123. result = MathCM.calcPointSegmentDist(ref p1, ref s2, out p2, out t2);
  124. if (0f <= t2 && t2 <= 1f)
  125. {
  126. return result;
  127. }
  128. t2 = Mathf.Clamp01(t2);
  129. p2 = s2.getPoint(t2);
  130. result = MathCM.calcPointSegmentDist(ref p2, ref s1, out p1, out t1);
  131. if (0f <= t1 && t1 <= 1f)
  132. {
  133. return result;
  134. }
  135. t1 = Mathf.Clamp01(t1);
  136. p1 = s1.getPoint(t1);
  137. return (p2 - p1).magnitude;
  138. }
  139. }
  140. public static int hittest_point_polygon_3d(Vector3 A, Vector3 B, Vector3 C, Vector3 P)
  141. {
  142. Vector3 lhs = B - A;
  143. Vector3 rhs = P - B;
  144. Vector3 lhs2 = C - B;
  145. Vector3 rhs2 = P - C;
  146. Vector3 lhs3 = A - C;
  147. Vector3 rhs3 = P - A;
  148. Vector3 lhs4 = Vector3.Cross(lhs, rhs);
  149. Vector3 rhs4 = Vector3.Cross(lhs2, rhs2);
  150. Vector3 rhs5 = Vector3.Cross(lhs3, rhs3);
  151. double num = (double)Vector3.Dot(lhs4, rhs4);
  152. double num2 = (double)Vector3.Dot(lhs4, rhs5);
  153. if (num > 0.0 && num2 > 0.0)
  154. {
  155. return 0;
  156. }
  157. return 1;
  158. }
  159. public static bool TriangleRayIntersect(Vector3 Orig, Vector3 dir, Vector3 v0, Vector3 v1, Vector3 v2, ref float pRetT, ref float pRetU, ref float pRetV)
  160. {
  161. Vector3 vector = v1 - v0;
  162. Vector3 vector2 = v2 - v0;
  163. Vector3 rhs = Vector3.Cross(dir, vector2);
  164. float num = Vector3.Dot(vector, rhs);
  165. float num2;
  166. Vector3 rhs2;
  167. float num3;
  168. if ((double)num > 0.001)
  169. {
  170. Vector3 lhs = Orig - v0;
  171. num2 = Vector3.Dot(lhs, rhs);
  172. if (num2 < 0f || num2 > num)
  173. {
  174. return false;
  175. }
  176. rhs2 = Vector3.Cross(lhs, vector);
  177. num3 = Vector3.Dot(dir, rhs2);
  178. if ((double)num3 < 0.0 || num2 + num3 > num)
  179. {
  180. return false;
  181. }
  182. }
  183. else
  184. {
  185. if ((double)num >= -0.001)
  186. {
  187. return false;
  188. }
  189. Vector3 lhs = Orig - v0;
  190. num2 = Vector3.Dot(lhs, rhs);
  191. if ((double)num2 > 0.0 || num2 < num)
  192. {
  193. return false;
  194. }
  195. rhs2 = Vector3.Cross(lhs, vector);
  196. num3 = Vector3.Dot(dir, rhs2);
  197. if ((double)num3 > 0.0 || num2 + num3 < num)
  198. {
  199. return false;
  200. }
  201. }
  202. float num4 = 1f / num;
  203. float num5 = Vector3.Dot(vector2, rhs2);
  204. num5 *= num4;
  205. num2 *= num4;
  206. num3 *= num4;
  207. pRetT = num5;
  208. pRetU = num2;
  209. pRetV = num3;
  210. return true;
  211. }
  212. public static bool TriangleRayIntersect(Vector3 tvec, Vector3 dir, Vector3 v0, Vector3 e1, Vector3 e2, ref float pRetT)
  213. {
  214. Vector3 rhs = Vector3.Cross(dir, e2);
  215. float num = Vector3.Dot(e1, rhs);
  216. Vector3 rhs2;
  217. if ((double)num > 0.001)
  218. {
  219. float num2 = Vector3.Dot(tvec, rhs);
  220. if (num2 < 0f || num2 > num)
  221. {
  222. return false;
  223. }
  224. rhs2 = Vector3.Cross(tvec, e1);
  225. float num3 = Vector3.Dot(dir, rhs2);
  226. if ((double)num3 < 0.0 || num2 + num3 > num)
  227. {
  228. return false;
  229. }
  230. }
  231. else
  232. {
  233. if ((double)num >= -0.001)
  234. {
  235. return false;
  236. }
  237. float num2 = Vector3.Dot(tvec, rhs);
  238. if ((double)num2 > 0.0 || num2 < num)
  239. {
  240. return false;
  241. }
  242. rhs2 = Vector3.Cross(tvec, e1);
  243. float num3 = Vector3.Dot(dir, rhs2);
  244. if ((double)num3 > 0.0 || num2 + num3 < num)
  245. {
  246. return false;
  247. }
  248. }
  249. float num4 = 1f / num;
  250. float num5 = Vector3.Dot(e2, rhs2);
  251. num5 *= num4;
  252. pRetT = num5;
  253. return true;
  254. }
  255. public static bool TriangleLineIntersect(Vector3 src, Vector3 dest, Vector3 tvec, Vector3 v0, Vector3 e1, Vector3 e2, ref float pRetT)
  256. {
  257. Vector3 vector = dest - src;
  258. if (MathCM.TriangleRayIntersect(tvec, vector.normalized, v0, e1, e2, ref pRetT))
  259. {
  260. float num = Mathf.Abs(pRetT);
  261. if (0f <= pRetT && num * num <= vector.sqrMagnitude)
  262. {
  263. return true;
  264. }
  265. }
  266. return false;
  267. }
  268. public static bool SphireRayIntersect(Vector3 orig, Vector3 dir, Vector3 center, float r, ref Vector3 intersect)
  269. {
  270. Vector3 vector = orig - center;
  271. float num = Vector3.Dot(vector, dir);
  272. float num2 = Vector3.Dot(vector, vector) - r * r;
  273. if ((double)num2 > 0.0 && (double)num > 0.0)
  274. {
  275. return false;
  276. }
  277. float num3 = num * num - num2;
  278. if ((double)num3 < 0.0)
  279. {
  280. return false;
  281. }
  282. float num4 = -num - Mathf.Sqrt(num3);
  283. if ((double)num4 < 0.0)
  284. {
  285. return false;
  286. }
  287. intersect = orig + num4 * dir;
  288. return true;
  289. }
  290. public static Vector3 NearPosOnPlane(Vector3 targetPos, Vector3 pointOnPlane, Vector3 normalPlane)
  291. {
  292. Vector3 rhs = targetPos - pointOnPlane;
  293. float d = Vector3.Dot(normalPlane, rhs);
  294. return targetPos - normalPlane * d;
  295. }
  296. public static bool PlaneSegmentIntersect(Plane plane, Vector3 start, Vector3 end, out Vector3 pointOnPlane)
  297. {
  298. float num = Vector3.Distance(start, end);
  299. Vector3 normalized = (end - start).normalized;
  300. Ray ray = new Ray(start, normalized);
  301. float num2 = 0f;
  302. if (plane.Raycast(ray, out num2) && num2 <= num)
  303. {
  304. pointOnPlane = start + normalized * num2;
  305. return true;
  306. }
  307. pointOnPlane = Vector3.zero;
  308. return false;
  309. }
  310. public static float SignedAngle(Vector3 from, Vector3 to, Vector3 axis)
  311. {
  312. float num = Vector3.Angle(from, to);
  313. float num2 = from.y * to.z - from.z * to.y;
  314. float num3 = from.z * to.x - from.x * to.z;
  315. float num4 = from.x * to.y - from.y * to.x;
  316. float num5 = Mathf.Sign(axis.x * num2 + axis.y * num3 + axis.z * num4);
  317. return num * num5;
  318. }
  319. public static int SearchRayToNearVtxDistance(Vector3[] poss, MathCM.Segment seg)
  320. {
  321. float num = 1f;
  322. int result = -1;
  323. for (int i = 0; i < poss.Length; i++)
  324. {
  325. Vector3 vector;
  326. float num3;
  327. float num2 = MathCM.calcPointSegmentDist(ref poss[i], ref seg, out vector, out num3);
  328. if (num2 < num)
  329. {
  330. num = num2;
  331. result = i;
  332. }
  333. }
  334. return result;
  335. }
  336. private static float _OX_EPSILON_ = 1E-06f;
  337. public struct Line
  338. {
  339. public Line(Vector3 fs, Vector3 fe)
  340. {
  341. this.s = fs;
  342. this.e = fe;
  343. this.v = fe - fs;
  344. }
  345. public Vector3 getPoint(float t)
  346. {
  347. return this.s + t * this.v;
  348. }
  349. public Vector3 s;
  350. public Vector3 e;
  351. public Vector3 v;
  352. }
  353. public struct Segment
  354. {
  355. public Segment(Vector3 fs, Vector3 fe)
  356. {
  357. this.s = fs;
  358. this.e = fe;
  359. this.v = fe - fs;
  360. }
  361. public Vector3 getPoint(float t)
  362. {
  363. return this.s + t * this.v;
  364. }
  365. public Vector3 s;
  366. public Vector3 e;
  367. public Vector3 v;
  368. }
  369. }