b2PolygonShape.cpp
上传用户:gb3593
上传日期:2022-01-07
资源大小:3028k
文件大小:11k
源码类别:

游戏引擎

开发平台:

Visual C++

  1. /*
  2. * Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
  3. *
  4. * This software is provided 'as-is', without any express or implied
  5. * warranty.  In no event will the authors be held liable for any damages
  6. * arising from the use of this software.
  7. * Permission is granted to anyone to use this software for any purpose,
  8. * including commercial applications, and to alter it and redistribute it
  9. * freely, subject to the following restrictions:
  10. * 1. The origin of this software must not be misrepresented; you must not
  11. * claim that you wrote the original software. If you use this software
  12. * in a product, an acknowledgment in the product documentation would be
  13. * appreciated but is not required.
  14. * 2. Altered source versions must be plainly marked as such, and must not be
  15. * misrepresented as being the original software.
  16. * 3. This notice may not be removed or altered from any source distribution.
  17. */
  18. #include <Box2D/Collision/Shapes/b2PolygonShape.h>
  19. #include <new>
  20. b2Shape* b2PolygonShape::Clone(b2BlockAllocator* allocator) const
  21. {
  22. void* mem = allocator->Allocate(sizeof(b2PolygonShape));
  23. b2PolygonShape* clone = new (mem) b2PolygonShape;
  24. *clone = *this;
  25. return clone;
  26. }
  27. void b2PolygonShape::SetAsBox(float32 hx, float32 hy)
  28. {
  29. m_vertexCount = 4;
  30. m_vertices[0].Set(-hx, -hy);
  31. m_vertices[1].Set( hx, -hy);
  32. m_vertices[2].Set( hx,  hy);
  33. m_vertices[3].Set(-hx,  hy);
  34. m_normals[0].Set(0.0f, -1.0f);
  35. m_normals[1].Set(1.0f, 0.0f);
  36. m_normals[2].Set(0.0f, 1.0f);
  37. m_normals[3].Set(-1.0f, 0.0f);
  38. m_centroid.SetZero();
  39. }
  40. void b2PolygonShape::SetAsBox(float32 hx, float32 hy, const b2Vec2& center, float32 angle)
  41. {
  42. m_vertexCount = 4;
  43. m_vertices[0].Set(-hx, -hy);
  44. m_vertices[1].Set( hx, -hy);
  45. m_vertices[2].Set( hx,  hy);
  46. m_vertices[3].Set(-hx,  hy);
  47. m_normals[0].Set(0.0f, -1.0f);
  48. m_normals[1].Set(1.0f, 0.0f);
  49. m_normals[2].Set(0.0f, 1.0f);
  50. m_normals[3].Set(-1.0f, 0.0f);
  51. m_centroid = center;
  52. b2Transform xf;
  53. xf.position = center;
  54. xf.R.Set(angle);
  55. // Transform vertices and normals.
  56. for (int32 i = 0; i < m_vertexCount; ++i)
  57. {
  58. m_vertices[i] = b2Mul(xf, m_vertices[i]);
  59. m_normals[i] = b2Mul(xf.R, m_normals[i]);
  60. }
  61. }
  62. void b2PolygonShape::SetAsEdge(const b2Vec2& v1, const b2Vec2& v2)
  63. {
  64. m_vertexCount = 2;
  65. m_vertices[0] = v1;
  66. m_vertices[1] = v2;
  67. m_centroid = 0.5f * (v1 + v2);
  68. m_normals[0] = b2Cross(v2 - v1, 1.0f);
  69. m_normals[0].Normalize();
  70. m_normals[1] = -m_normals[0];
  71. }
  72. static b2Vec2 ComputeCentroid(const b2Vec2* vs, int32 count)
  73. {
  74. b2Assert(count >= 2);
  75. b2Vec2 c; c.Set(0.0f, 0.0f);
  76. float32 area = 0.0f;
  77. if (count == 2)
  78. {
  79. c = 0.5f * (vs[0] + vs[1]);
  80. return c;
  81. }
  82. // pRef is the reference point for forming triangles.
  83. // It's location doesn't change the result (except for rounding error).
  84. b2Vec2 pRef(0.0f, 0.0f);
  85. #if 0
  86. // This code would put the reference point inside the polygon.
  87. for (int32 i = 0; i < count; ++i)
  88. {
  89. pRef += vs[i];
  90. }
  91. pRef *= 1.0f / count;
  92. #endif
  93. const float32 inv3 = 1.0f / 3.0f;
  94. for (int32 i = 0; i < count; ++i)
  95. {
  96. // Triangle vertices.
  97. b2Vec2 p1 = pRef;
  98. b2Vec2 p2 = vs[i];
  99. b2Vec2 p3 = i + 1 < count ? vs[i+1] : vs[0];
  100. b2Vec2 e1 = p2 - p1;
  101. b2Vec2 e2 = p3 - p1;
  102. float32 D = b2Cross(e1, e2);
  103. float32 triangleArea = 0.5f * D;
  104. area += triangleArea;
  105. // Area weighted centroid
  106. c += triangleArea * inv3 * (p1 + p2 + p3);
  107. }
  108. // Centroid
  109. b2Assert(area > b2_epsilon);
  110. c *= 1.0f / area;
  111. return c;
  112. }
  113. void b2PolygonShape::Set(const b2Vec2* vertices, int32 count)
  114. {
  115. b2Assert(2 <= count && count <= b2_maxPolygonVertices);
  116. m_vertexCount = count;
  117. // Copy vertices.
  118. for (int32 i = 0; i < m_vertexCount; ++i)
  119. {
  120. m_vertices[i] = vertices[i];
  121. }
  122. // Compute normals. Ensure the edges have non-zero length.
  123. for (int32 i = 0; i < m_vertexCount; ++i)
  124. {
  125. int32 i1 = i;
  126. int32 i2 = i + 1 < m_vertexCount ? i + 1 : 0;
  127. b2Vec2 edge = m_vertices[i2] - m_vertices[i1];
  128. b2Assert(edge.LengthSquared() > b2_epsilon * b2_epsilon);
  129. m_normals[i] = b2Cross(edge, 1.0f);
  130. m_normals[i].Normalize();
  131. }
  132. #ifdef _DEBUG
  133. // Ensure the polygon is convex and the interior
  134. // is to the left of each edge.
  135. for (int32 i = 0; i < m_vertexCount; ++i)
  136. {
  137. int32 i1 = i;
  138. int32 i2 = i + 1 < m_vertexCount ? i + 1 : 0;
  139. b2Vec2 edge = m_vertices[i2] - m_vertices[i1];
  140. for (int32 j = 0; j < m_vertexCount; ++j)
  141. {
  142. // Don't check vertices on the current edge.
  143. if (j == i1 || j == i2)
  144. {
  145. continue;
  146. }
  147. b2Vec2 r = m_vertices[j] - m_vertices[i1];
  148. // Your polygon is non-convex (it has an indentation) or
  149. // has colinear edges.
  150. float32 s = b2Cross(edge, r);
  151. b2Assert(s > 0.0f);
  152. }
  153. }
  154. #endif
  155. // Compute the polygon centroid.
  156. m_centroid = ComputeCentroid(m_vertices, m_vertexCount);
  157. }
  158. bool b2PolygonShape::TestPoint(const b2Transform& xf, const b2Vec2& p) const
  159. {
  160. b2Vec2 pLocal = b2MulT(xf.R, p - xf.position);
  161. for (int32 i = 0; i < m_vertexCount; ++i)
  162. {
  163. float32 dot = b2Dot(m_normals[i], pLocal - m_vertices[i]);
  164. if (dot > 0.0f)
  165. {
  166. return false;
  167. }
  168. }
  169. return true;
  170. }
  171. bool b2PolygonShape::RayCast(b2RayCastOutput* output, const b2RayCastInput& input, const b2Transform& xf) const
  172. {
  173. // Put the ray into the polygon's frame of reference.
  174. b2Vec2 p1 = b2MulT(xf.R, input.p1 - xf.position);
  175. b2Vec2 p2 = b2MulT(xf.R, input.p2 - xf.position);
  176. b2Vec2 d = p2 - p1;
  177. if (m_vertexCount == 2)
  178. {
  179. b2Vec2 v1 = m_vertices[0];
  180. b2Vec2 v2 = m_vertices[1];
  181. b2Vec2 normal = m_normals[0];
  182. // q = p1 + t * d
  183. // dot(normal, q - v1) = 0
  184. // dot(normal, p1 - v1) + t * dot(normal, d) = 0
  185. float32 numerator = b2Dot(normal, v1 - p1);
  186. float32 denominator = b2Dot(normal, d);
  187. if (denominator == 0.0f)
  188. {
  189. return false;
  190. }
  191. float32 t = numerator / denominator;
  192. if (t < 0.0f || 1.0f < t)
  193. {
  194. return false;
  195. }
  196. b2Vec2 q = p1 + t * d;
  197. // q = v1 + s * r
  198. // s = dot(q - v1, r) / dot(r, r)
  199. b2Vec2 r = v2 - v1;
  200. float32 rr = b2Dot(r, r);
  201. if (rr == 0.0f)
  202. {
  203. return false;
  204. }
  205. float32 s = b2Dot(q - v1, r) / rr;
  206. if (s < 0.0f || 1.0f < s)
  207. {
  208. return false;
  209. }
  210. output->fraction = t;
  211. if (numerator > 0.0f)
  212. {
  213. output->normal = -normal;
  214. }
  215. else
  216. {
  217. output->normal = normal;
  218. }
  219. return true;
  220. }
  221. else
  222. {
  223. float32 lower = 0.0f, upper = input.maxFraction;
  224. int32 index = -1;
  225. for (int32 i = 0; i < m_vertexCount; ++i)
  226. {
  227. // p = p1 + a * d
  228. // dot(normal, p - v) = 0
  229. // dot(normal, p1 - v) + a * dot(normal, d) = 0
  230. float32 numerator = b2Dot(m_normals[i], m_vertices[i] - p1);
  231. float32 denominator = b2Dot(m_normals[i], d);
  232. if (denominator == 0.0f)
  233. {
  234. if (numerator < 0.0f)
  235. {
  236. return false;
  237. }
  238. }
  239. else
  240. {
  241. // Note: we want this predicate without division:
  242. // lower < numerator / denominator, where denominator < 0
  243. // Since denominator < 0, we have to flip the inequality:
  244. // lower < numerator / denominator <==> denominator * lower > numerator.
  245. if (denominator < 0.0f && numerator < lower * denominator)
  246. {
  247. // Increase lower.
  248. // The segment enters this half-space.
  249. lower = numerator / denominator;
  250. index = i;
  251. }
  252. else if (denominator > 0.0f && numerator < upper * denominator)
  253. {
  254. // Decrease upper.
  255. // The segment exits this half-space.
  256. upper = numerator / denominator;
  257. }
  258. }
  259. // The use of epsilon here causes the assert on lower to trip
  260. // in some cases. Apparently the use of epsilon was to make edge
  261. // shapes work, but now those are handled separately.
  262. //if (upper < lower - b2_epsilon)
  263. if (upper < lower)
  264. {
  265. return false;
  266. }
  267. }
  268. b2Assert(0.0f <= lower && lower <= input.maxFraction);
  269. if (index >= 0)
  270. {
  271. output->fraction = lower;
  272. output->normal = b2Mul(xf.R, m_normals[index]);
  273. return true;
  274. }
  275. }
  276. return false;
  277. }
  278. void b2PolygonShape::ComputeAABB(b2AABB* aabb, const b2Transform& xf) const
  279. {
  280. b2Vec2 lower = b2Mul(xf, m_vertices[0]);
  281. b2Vec2 upper = lower;
  282. for (int32 i = 1; i < m_vertexCount; ++i)
  283. {
  284. b2Vec2 v = b2Mul(xf, m_vertices[i]);
  285. lower = b2Min(lower, v);
  286. upper = b2Max(upper, v);
  287. }
  288. b2Vec2 r(m_radius, m_radius);
  289. aabb->lowerBound = lower - r;
  290. aabb->upperBound = upper + r;
  291. }
  292. void b2PolygonShape::ComputeMass(b2MassData* massData, float32 density) const
  293. {
  294. // Polygon mass, centroid, and inertia.
  295. // Let rho be the polygon density in mass per unit area.
  296. // Then:
  297. // mass = rho * int(dA)
  298. // centroid.x = (1/mass) * rho * int(x * dA)
  299. // centroid.y = (1/mass) * rho * int(y * dA)
  300. // I = rho * int((x*x + y*y) * dA)
  301. //
  302. // We can compute these integrals by summing all the integrals
  303. // for each triangle of the polygon. To evaluate the integral
  304. // for a single triangle, we make a change of variables to
  305. // the (u,v) coordinates of the triangle:
  306. // x = x0 + e1x * u + e2x * v
  307. // y = y0 + e1y * u + e2y * v
  308. // where 0 <= u && 0 <= v && u + v <= 1.
  309. //
  310. // We integrate u from [0,1-v] and then v from [0,1].
  311. // We also need to use the Jacobian of the transformation:
  312. // D = cross(e1, e2)
  313. //
  314. // Simplification: triangle centroid = (1/3) * (p1 + p2 + p3)
  315. //
  316. // The rest of the derivation is handled by computer algebra.
  317. b2Assert(m_vertexCount >= 2);
  318. // A line segment has zero mass.
  319. if (m_vertexCount == 2)
  320. {
  321. massData->center = 0.5f * (m_vertices[0] + m_vertices[1]);
  322. massData->mass = 0.0f;
  323. massData->I = 0.0f;
  324. return;
  325. }
  326. b2Vec2 center; center.Set(0.0f, 0.0f);
  327. float32 area = 0.0f;
  328. float32 I = 0.0f;
  329. // pRef is the reference point for forming triangles.
  330. // It's location doesn't change the result (except for rounding error).
  331. b2Vec2 pRef(0.0f, 0.0f);
  332. #if 0
  333. // This code would put the reference point inside the polygon.
  334. for (int32 i = 0; i < m_vertexCount; ++i)
  335. {
  336. pRef += m_vertices[i];
  337. }
  338. pRef *= 1.0f / count;
  339. #endif
  340. const float32 k_inv3 = 1.0f / 3.0f;
  341. for (int32 i = 0; i < m_vertexCount; ++i)
  342. {
  343. // Triangle vertices.
  344. b2Vec2 p1 = pRef;
  345. b2Vec2 p2 = m_vertices[i];
  346. b2Vec2 p3 = i + 1 < m_vertexCount ? m_vertices[i+1] : m_vertices[0];
  347. b2Vec2 e1 = p2 - p1;
  348. b2Vec2 e2 = p3 - p1;
  349. float32 D = b2Cross(e1, e2);
  350. float32 triangleArea = 0.5f * D;
  351. area += triangleArea;
  352. // Area weighted centroid
  353. center += triangleArea * k_inv3 * (p1 + p2 + p3);
  354. float32 px = p1.x, py = p1.y;
  355. float32 ex1 = e1.x, ey1 = e1.y;
  356. float32 ex2 = e2.x, ey2 = e2.y;
  357. float32 intx2 = k_inv3 * (0.25f * (ex1*ex1 + ex2*ex1 + ex2*ex2) + (px*ex1 + px*ex2)) + 0.5f*px*px;
  358. float32 inty2 = k_inv3 * (0.25f * (ey1*ey1 + ey2*ey1 + ey2*ey2) + (py*ey1 + py*ey2)) + 0.5f*py*py;
  359. I += D * (intx2 + inty2);
  360. }
  361. // Total mass
  362. massData->mass = density * area;
  363. // Center of mass
  364. b2Assert(area > b2_epsilon);
  365. center *= 1.0f / area;
  366. massData->center = center;
  367. // Inertia tensor relative to the local origin.
  368. massData->I = density * I;
  369. }