b2d_decompose.cpp 6.3 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162
  1. /*************************************************************************/
  2. /* b2d_decompose.cpp */
  3. /*************************************************************************/
  4. /* This file is part of: */
  5. /* GODOT ENGINE */
  6. /* https://godotengine.org */
  7. /*************************************************************************/
  8. /* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur. */
  9. /* Copyright (c) 2014-2017 Godot Engine contributors (cf. AUTHORS.md) */
  10. /* */
  11. /* Permission is hereby granted, free of charge, to any person obtaining */
  12. /* a copy of this software and associated documentation files (the */
  13. /* "Software"), to deal in the Software without restriction, including */
  14. /* without limitation the rights to use, copy, modify, merge, publish, */
  15. /* distribute, sublicense, and/or sell copies of the Software, and to */
  16. /* permit persons to whom the Software is furnished to do so, subject to */
  17. /* the following conditions: */
  18. /* */
  19. /* The above copyright notice and this permission notice shall be */
  20. /* included in all copies or substantial portions of the Software. */
  21. /* */
  22. /* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
  23. /* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
  24. /* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
  25. /* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
  26. /* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
  27. /* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
  28. /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
  29. /*************************************************************************/
  30. #include "b2d_decompose.h"
  31. #include "thirdparty/b2d_convexdecomp/b2Polygon.h"
  32. namespace b2ConvexDecomp {
  33. void add_to_res(Vector<Vector<Vector2> > &res, const b2Polygon &p_poly) {
  34. Vector<Vector2> arr;
  35. for (int i = 0; i < p_poly.nVertices; i++) {
  36. arr.push_back(Vector2(p_poly.x[i], p_poly.y[i]));
  37. }
  38. res.push_back(arr);
  39. }
  40. static Vector<Vector<Vector2> > _b2d_decompose(const Vector<Vector2> &p_polygon) {
  41. Vector<Vector<Vector2> > res;
  42. if (p_polygon.size() < 3)
  43. return res;
  44. b2Vec2 *polys = memnew_arr(b2Vec2, p_polygon.size());
  45. for (int i = 0; i < p_polygon.size(); i++)
  46. polys[i] = b2Vec2(p_polygon[i].x, p_polygon[i].y);
  47. b2Polygon *p = new b2Polygon(polys, p_polygon.size());
  48. b2Polygon *decomposed = new b2Polygon[p->nVertices - 2]; //maximum number of polys
  49. memdelete_arr(polys);
  50. int32 nPolys = DecomposeConvex(p, decomposed, p->nVertices - 2);
  51. //int32 extra = 0;
  52. for (int32 i = 0; i < nPolys; ++i) {
  53. // b2FixtureDef* toAdd = &pdarray[i+extra];
  54. // *toAdd = *prototype;
  55. //Hmm, shouldn't have to do all this...
  56. b2Polygon curr = decomposed[i];
  57. //TODO ewjordan: move this triangle handling to a better place so that
  58. //it happens even if this convenience function is not called.
  59. if (curr.nVertices == 3) {
  60. //Check here for near-parallel edges, since we can't
  61. //handle this in merge routine
  62. for (int j = 0; j < 3; ++j) {
  63. int32 lower = (j == 0) ? (curr.nVertices - 1) : (j - 1);
  64. int32 middle = j;
  65. int32 upper = (j == curr.nVertices - 1) ? (0) : (j + 1);
  66. float32 dx0 = curr.x[middle] - curr.x[lower];
  67. float32 dy0 = curr.y[middle] - curr.y[lower];
  68. float32 dx1 = curr.x[upper] - curr.x[middle];
  69. float32 dy1 = curr.y[upper] - curr.y[middle];
  70. float32 norm0 = sqrtf(dx0 * dx0 + dy0 * dy0);
  71. float32 norm1 = sqrtf(dx1 * dx1 + dy1 * dy1);
  72. if (!(norm0 > 0.0f && norm1 > 0.0f)) {
  73. //Identical points, don't do anything!
  74. goto Skip;
  75. }
  76. dx0 /= norm0;
  77. dy0 /= norm0;
  78. dx1 /= norm1;
  79. dy1 /= norm1;
  80. float32 cross = dx0 * dy1 - dx1 * dy0;
  81. float32 dot = dx0 * dx1 + dy0 * dy1;
  82. if (fabs(cross) < b2_angularSlop && dot > 0) {
  83. //Angle too close, split the triangle across from this point.
  84. //This is guaranteed to result in two triangles that satify
  85. //the tolerance (one of the angles is 90 degrees)
  86. float32 dx2 = curr.x[lower] - curr.x[upper];
  87. float32 dy2 = curr.y[lower] - curr.y[upper];
  88. float32 norm2 = sqrtf(dx2 * dx2 + dy2 * dy2);
  89. if (norm2 == 0.0f) {
  90. goto Skip;
  91. }
  92. dx2 /= norm2;
  93. dy2 /= norm2;
  94. float32 thisArea = curr.GetArea();
  95. float32 thisHeight = 2.0f * thisArea / norm2;
  96. float32 buffer2 = dx2;
  97. dx2 = dy2;
  98. dy2 = -buffer2;
  99. //Make two new polygons
  100. //printf("dx2: %f, dy2: %f, thisHeight: %f, middle: %d\n",dx2,dy2,thisHeight,middle);
  101. float32 newX1[3] = { curr.x[middle] + dx2 * thisHeight, curr.x[lower], curr.x[middle] };
  102. float32 newY1[3] = { curr.y[middle] + dy2 * thisHeight, curr.y[lower], curr.y[middle] };
  103. float32 newX2[3] = { newX1[0], curr.x[middle], curr.x[upper] };
  104. float32 newY2[3] = { newY1[0], curr.y[middle], curr.y[upper] };
  105. b2Polygon p1(newX1, newY1, 3);
  106. b2Polygon p2(newX2, newY2, 3);
  107. if (p1.IsUsable()) {
  108. add_to_res(res, p1);
  109. //++extra;
  110. } else if (B2_POLYGON_REPORT_ERRORS) {
  111. printf("Didn't add unusable polygon. Dumping vertices:\n");
  112. p1.print();
  113. }
  114. if (p2.IsUsable()) {
  115. add_to_res(res, p2);
  116. //p2.AddTo(pdarray[i+extra]);
  117. //bd->CreateFixture(toAdd);
  118. } else if (B2_POLYGON_REPORT_ERRORS) {
  119. printf("Didn't add unusable polygon. Dumping vertices:\n");
  120. p2.print();
  121. }
  122. goto Skip;
  123. }
  124. }
  125. }
  126. if (decomposed[i].IsUsable()) {
  127. add_to_res(res, decomposed[i]);
  128. //decomposed[i].AddTo(*toAdd);
  129. //bd->CreateFixture((const b2FixtureDef*)toAdd);
  130. } else if (B2_POLYGON_REPORT_ERRORS) {
  131. printf("Didn't add unusable polygon. Dumping vertices:\n");
  132. decomposed[i].print();
  133. }
  134. Skip:;
  135. }
  136. //delete[] pdarray;
  137. delete[] decomposed;
  138. delete p;
  139. return res; // pdarray; //needs to be deleted after body is created
  140. }
  141. }
  142. Vector<Vector<Vector2> > b2d_decompose(const Vector<Vector2> &p_polygon) {
  143. return b2ConvexDecomp::_b2d_decompose(p_polygon);
  144. }