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

游戏引擎

开发平台:

Visual C++

  1. Index: Box2D/Box2D/Box2D.h
  2. ===================================================================
  3. --- Box2D/Box2D/Box2D.h (revision 70)
  4. +++ Box2D/Box2D/Box2D.h (working copy)
  5. @@ -35,6 +35,7 @@
  6.  
  7.  #include <Box2D/Collision/Shapes/b2CircleShape.h>
  8.  #include <Box2D/Collision/Shapes/b2PolygonShape.h>
  9. +#include <Box2D/Collision/Shapes/b2CapsuleShape.h>
  10.  
  11.  #include <Box2D/Collision/b2BroadPhase.h>
  12.  #include <Box2D/Collision/b2Distance.h>
  13. Index: Box2D/Box2D/CMakeLists.txt
  14. ===================================================================
  15. --- Box2D/Box2D/CMakeLists.txt (revision 70)
  16. +++ Box2D/Box2D/CMakeLists.txt (working copy)
  17. @@ -15,10 +15,12 @@
  18.   Collision/b2TimeOfImpact.h  )  set(BOX2D_Shapes_SRCS + Collision/Shapes/b2CapsuleShape.cpp   Collision/Shapes/b2CircleShape.cpp   Collision/Shapes/b2PolygonShape.cpp  )  set(BOX2D_Shapes_HDRS + Collision/Shapes/b2CapsuleShape.h   Collision/Shapes/b2CircleShape.h   Collision/Shapes/b2PolygonShape.h   Collision/Shapes/b2Shape.h @@ -53,18 +55,24 @@
  19.   Dynamics/b2WorldCallbacks.h  )  set(BOX2D_Contacts_SRCS + Dynamics/Contacts/b2CapsuleAndCircleContact.cpp + Dynamics/Contacts/b2CapsuleContact.cpp   Dynamics/Contacts/b2CircleContact.cpp   Dynamics/Contacts/b2Contact.cpp   Dynamics/Contacts/b2ContactSolver.cpp + Dynamics/Contacts/b2PolygonAndCapsuleContact.cpp   Dynamics/Contacts/b2PolygonAndCircleContact.cpp   Dynamics/Contacts/b2PolygonContact.cpp   Dynamics/Contacts/b2TOISolver.cpp  )  set(BOX2D_Contacts_HDRS + Dynamics/Contacts/b2CapsuleAndCircleContact.h + Dynamics/Contacts/b2CapsuleContact.h   Dynamics/Contacts/b2CircleContact.h   Dynamics/Contacts/b2Contact.h   Dynamics/Contacts/b2ContactSolver.h   Dynamics/Contacts/b2NullContact.h + Dynamics/Contacts/b2PolygonAndCapsuleContact.h   Dynamics/Contacts/b2PolygonAndCircleContact.h   Dynamics/Contacts/b2PolygonContact.h   Dynamics/Contacts/b2TOISolver.h Index: Box2D/Box2D/Collision/b2Collision.cpp
  20. ===================================================================
  21. --- Box2D/Box2D/Collision/b2Collision.cpp (revision 70)
  22. +++ Box2D/Box2D/Collision/b2Collision.cpp (working copy)
  23. @@ -248,3 +248,220 @@
  24.  
  25.   return output.distance < 10.0f * b2_epsilon;
  26.  }
  27. +
  28. +
  29. +// Collision Detection in Interactive 3D Environments by Gino van den Bergen
  30. +// From Section 3.4.1
  31. +// x = mu1 * p1 + mu2 * p2
  32. +// mu1 + mu2 = 1 && mu1 >= 0 && mu2 >= 0
  33. +// mu1 = 1 - mu2;
  34. +// x = (1 - mu2) * p1 + mu2 * p2
  35. +//   = p1 + mu2 * (p2 - p1)
  36. +// x = s + a * r (s := start, r := end - start)
  37. +// s + a * r = p1 + mu2 * d (d := p2 - p1)
  38. +// -a * r + mu2 * d = b (b := s - p1)
  39. +// [-r d] * [a; mu2] = b
  40. +// Cramer's rule:
  41. +// denom = det[-r d]
  42. +// a = det[b d] / denom
  43. +// mu2 = det[-r b] / denom
  44. +bool b2RaycastSegment(b2RayCastOutput* output, const b2RayCastInput& input, const b2Vec2& p1, const b2Vec2& p2)
  45. +{
  46. + b2Vec2 s = input.p1;
  47. + b2Vec2 r = input.p2 - s;
  48. + b2Vec2 d = p2 - p1;
  49. + b2Vec2 n = b2Cross(d, 1.0f);
  50. +
  51. + const float32 k_slop = 100.0f * b2_epsilon;
  52. + float32 denom = -b2Dot(r, n);
  53. +
  54. + // Cull back facing collision and ignore parallel segments.
  55. + if (denom > k_slop)
  56. + {
  57. + // Does the segment intersect the infinite line associated with this segment?
  58. + b2Vec2 b = s - p1;
  59. + float32 a = b2Dot(b, n);
  60. +
  61. + if (0.0f <= a && a <= input.maxFraction * denom)
  62. + {
  63. + float32 mu2 = -r.x * b.y + r.y * b.x;
  64. +
  65. + // Does the segment intersect this segment?
  66. + if (-k_slop * denom <= mu2 && mu2 <= denom * (1.0f + k_slop))
  67. + {
  68. + a /= denom;
  69. + n.Normalize();
  70. + output->normal = n;
  71. + output->fraction = a;
  72. + return true;
  73. + }
  74. + }
  75. + }
  76. +
  77. + return false;
  78. +}
  79. +
  80. +
  81. +static void FindNearestPointOnLineSegment( b2Vec2* nearest, float32* parameter,
  82. + const b2Vec2& A1, const b2Vec2& L, const b2Vec2& B,
  83. + bool infinite_line )
  84. +{
  85. + float32 D = L.LengthSquared();
  86. + if (D < b2_epsilon * b2_epsilon)
  87. + {
  88. + *nearest = A1;
  89. + return;
  90. + }
  91. +
  92. + b2Vec2 AB = B - A1;
  93. + *parameter = b2Dot(L, AB) / D;
  94. + if (!infinite_line)
  95. + {
  96. + *parameter = b2Clamp(*parameter, 0.0f, 1.0f);
  97. + }
  98. + *nearest = A1 + *parameter * L;
  99. +}
  100. +
  101. +// Based on Game Programming Gems 2
  102. +// Fast, Robust Intersection of 3D Line Segments
  103. +// Graham Rhodes, Applied Research Associates
  104. +b2Vec2 b2NearestPointOnLine(const b2Vec2& point, const b2Vec2& p1, const b2Vec2& p2)
  105. +{
  106. + b2Vec2 result;
  107. + float32 unused;
  108. + FindNearestPointOnLineSegment(&result, &unused, p1, p2 - p1, point, false);
  109. + return result;
  110. +}
  111. +
  112. +static void FindNearestPointOfParallelLineSegments( b2Vec2* x1, b2Vec2* x2,
  113. + const b2Vec2& A1, const b2Vec2& A2, const b2Vec2& La,
  114. + const b2Vec2& B1, const b2Vec2& B2, const b2Vec2& Lb)
  115. +{
  116. + float32 s, t;
  117. + FindNearestPointOnLineSegment(x1, &s, A1, La, B1, true);
  118. + FindNearestPointOnLineSegment(x2, &t, A1, La, B2, true);
  119. +
  120. + if (s < 0.0f && t < 0.0f)
  121. + {
  122. + *x1 = A1;
  123. + if (s < t)
  124. + *x2 = B2;
  125. + else
  126. + *x2 = B1;
  127. + return;
  128. + }
  129. +
  130. + if (s > 1.0f && t > 1.0f)
  131. + {
  132. + *x1 = A2;
  133. + if (s < t)
  134. + *x2 = B1;
  135. + else
  136. + *x2 = B2;
  137. + return;
  138. + }
  139. +
  140. + float32 temp = 0.5f * (b2Clamp(s, 0.0f, 1.0f) + b2Clamp(t, 0.0f, 1.0f));
  141. + *x1 = A1 + temp * La;
  142. +
  143. + float32 unused;
  144. + FindNearestPointOnLineSegment(x2, &unused, B1, Lb, *x1, true);
  145. +}
  146. +
  147. +static void AdjustNearestPoints(b2Vec2* x1, b2Vec2* x2,
  148. + const b2Vec2& A1, const b2Vec2& A2, const b2Vec2& La,
  149. + const b2Vec2& B1, const b2Vec2& B2, const b2Vec2& Lb,
  150. + float32 s, float32 t)
  151. +{
  152. + if ((s < 0.0f || s > 1.0f) && (t < 0.0f || t > 1.0f))
  153. + {
  154. + s = b2Clamp(s, 0.0f, 1.0f);
  155. + *x1 = A1 + s * La;
  156. + FindNearestPointOnLineSegment(x2, &t, B1, Lb, *x1, true);
  157. + if (t < 0.0f || t > 1.0f)
  158. + {
  159. + t = b2Clamp(t, 0.0f, 1.0f);
  160. + *x2 = B1 + t * Lb;
  161. + FindNearestPointOnLineSegment(x1, &s, A1, La, *x2, false);
  162. + FindNearestPointOnLineSegment(x2, &t, B1, Lb, *x1, false);
  163. + }
  164. + return;
  165. + }
  166. + if (s < 0.0f || s > 1.0f)
  167. + {
  168. + s = b2Clamp(s, 0.0f, 1.0f);
  169. + *x1 = A1 + s * La;
  170. + FindNearestPointOnLineSegment(x2, &t, B1, Lb, *x1, false);
  171. + return;
  172. + }
  173. + b2Assert(t < 0.0f || t > 1.0f);
  174. + t = b2Clamp(t, 0.0f, 1.0f);
  175. + *x2 = B1 + t * Lb;
  176. + FindNearestPointOnLineSegment(x1, &s, A1, La, *x2, false);
  177. +}
  178. +
  179. +static void IntersectLineSegments( b2Vec2* x1, b2Vec2* x2,
  180. + const b2Vec2& A1, const b2Vec2& A2,
  181. + const b2Vec2& B1, const b2Vec2& B2)
  182. +{
  183. + float32 unused;
  184. +
  185. + b2Vec2 La = A2 - A1;
  186. + b2Vec2 Lb = B2 - B1;
  187. + float32 L11 = La.LengthSquared();
  188. + float32 L22 = Lb.LengthSquared();
  189. +
  190. + // Check for degenerate parameters
  191. + if (L11 < b2_epsilon * b2_epsilon)
  192. + {
  193. + *x1 = A1;
  194. + FindNearestPointOnLineSegment(x2, &unused, B1, Lb, A1, false);
  195. + return;
  196. + }
  197. + if (L22 < b2_epsilon * b2_epsilon)
  198. + {
  199. + *x2 = B1;
  200. + FindNearestPointOnLineSegment(x1, &unused, A1, La, B1, false);
  201. + return;
  202. + }
  203. +
  204. + b2Vec2 AB = B1 - A1;
  205. + float32 L12 = -b2Dot(La, Lb);
  206. + float32 DetL = L11 * L22 - L12 * L12;
  207. +
  208. + // parallel segments
  209. + if (b2Abs(DetL) < b2_epsilon)
  210. + {
  211. + FindNearestPointOfParallelLineSegments(x1, x2, A1, A2, La, B1, B2, Lb);
  212. + return;
  213. + }
  214. +
  215. + float32 ra = b2Dot(La, AB);
  216. + float32 rb = -b2Dot(Lb, AB);
  217. +
  218. + float32 t = (L11 * rb - ra * L12) / DetL;
  219. + float32 s = (ra - L12 * t) / L11;
  220. +
  221. + // These tests can't quite stay within B2_FLT_EPSILON
  222. + //b2Assert(b2Abs(s * L11 + t * L12 - ra) < .0001f);
  223. + //b2Assert(b2Abs(s * L12 + t * L22 - rb) < .0001f);
  224. +
  225. + *x1 = A1 + s * La;
  226. + *x2 = B1 + t * Lb;
  227. +
  228. + if (s < 0.0f || s > 1.0f || t < 0.0f || t > 1.0f)
  229. + {
  230. + AdjustNearestPoints(x1, x2, A1, A2, La, B1, B2, Lb, s, t);
  231. + }
  232. +}
  233. +
  234. +// Based on Game Programming Gems 2
  235. +// Fast, Robust Intersection of 3D Line Segments
  236. +// Graham Rhodes, Applied Research Associates
  237. +float32 b2DistanceBetweenLines( b2Vec2* x1, b2Vec2* x2,
  238. + const b2Vec2& A1, const b2Vec2& A2,
  239. + const b2Vec2& B1, const b2Vec2& B2)
  240. +{
  241. + IntersectLineSegments(x1, x2, A1, A2, B1, B2);
  242. + return (*x2 - *x1).Length();
  243. +}
  244.  No newline at end of file
  245. Index: Box2D/Box2D/Collision/b2Collision.h
  246. ===================================================================
  247. --- Box2D/Box2D/Collision/b2Collision.h (revision 70)
  248. +++ Box2D/Box2D/Collision/b2Collision.h (working copy)
  249. @@ -27,6 +27,7 @@
  250.  /// queries, and TOI queries.
  251.  
  252.  class b2Shape;
  253. +class b2CapsuleShape;
  254.  class b2CircleShape;
  255.  class b2PolygonShape;
  256.  
  257. @@ -212,6 +213,18 @@
  258.  bool b2TestOverlap(const b2Shape* shapeA, const b2Shape* shapeB,
  259.      const b2Transform& xfA, const b2Transform& xfB);
  260.  
  261. +/// Perform a raycast against a given line segment.
  262. +bool b2RaycastSegment(b2RayCastOutput* output, const b2RayCastInput& input, const b2Vec2& p1, const b2Vec2& p2);
  263. +
  264. +/// Find the nearest point on a line.
  265. +b2Vec2 b2NearestPointOnLine(const b2Vec2& point, const b2Vec2& p1, const b2Vec2& p2);
  266. +
  267. +/// Find the nearest points on between two different lines.
  268. +float32 b2DistanceBetweenLines( b2Vec2* x1, b2Vec2* x2,
  269. + const b2Vec2& A1, const b2Vec2& A2,
  270. + const b2Vec2& B1, const b2Vec2& B2);
  271. +
  272. +
  273.  // ---------------- Inline Functions ------------------------------------------
  274.  
  275.  inline bool b2AABB::IsValid() const
  276. Index: Box2D/Box2D/Collision/b2Distance.cpp
  277. ===================================================================
  278. --- Box2D/Box2D/Collision/b2Distance.cpp (revision 70)
  279. +++ Box2D/Box2D/Collision/b2Distance.cpp (working copy)
  280. @@ -17,6 +17,7 @@
  281.  */
  282.  
  283.  #include <Box2D/Collision/b2Distance.h>
  284. +#include <Box2D/Collision/Shapes/b2CapsuleShape.h>
  285.  #include <Box2D/Collision/Shapes/b2CircleShape.h>
  286.  #include <Box2D/Collision/Shapes/b2PolygonShape.h>
  287.  
  288. @@ -44,6 +45,15 @@
  289.   m_radius = polygon->m_radius;
  290.   }
  291.   break;
  292. +
  293. + case b2Shape::e_capsule:
  294. +     {
  295. + const b2CapsuleShape* capsule = (b2CapsuleShape*)shape;
  296. + m_vertices = capsule->m_vertices;
  297. + m_count = 2;
  298. + m_radius = capsule->m_radius;
  299. +     }
  300. +     break;
  301.  
  302.   default:
  303.   b2Assert(false);
  304. Index: Box2D/Box2D/Collision/Shapes/b2CapsuleShape.cpp
  305. ===================================================================
  306. --- Box2D/Box2D/Collision/Shapes/b2CapsuleShape.cpp (revision 0)
  307. +++ Box2D/Box2D/Collision/Shapes/b2CapsuleShape.cpp (revision 0)
  308. @@ -0,0 +1,194 @@
  309. +/*
  310. +* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
  311. +*
  312. +* This software is provided 'as-is', without any express or implied
  313. +* warranty.  In no event will the authors be held liable for any damages
  314. +* arising from the use of this software.
  315. +* Permission is granted to anyone to use this software for any purpose,
  316. +* including commercial applications, and to alter it and redistribute it
  317. +* freely, subject to the following restrictions:
  318. +* 1. The origin of this software must not be misrepresented; you must not
  319. +* claim that you wrote the original software. If you use this software
  320. +* in a product, an acknowledgment in the product documentation would be
  321. +* appreciated but is not required.
  322. +* 2. Altered source versions must be plainly marked as such, and must not be
  323. +* misrepresented as being the original software.
  324. +* 3. This notice may not be removed or altered from any source distribution.
  325. +*/
  326. +
  327. +#include <Box2D/Collision/Shapes/b2CapsuleShape.h>
  328. +#include <new>
  329. +
  330. +void b2CapsuleShape::SetByExtentsX(float32 radius, float32 length)
  331. +{
  332. + m_radius = radius;
  333. + float32 half = length * 0.5f;
  334. + m_vertices[0] = b2Vec2(-half, 0.0f);
  335. + m_vertices[1] = b2Vec2(half, 0.0f);
  336. +}
  337. +
  338. +void b2CapsuleShape::SetByExtentsX(float32 radius, float32 length,
  339. +   const b2Vec2& center, float32 angle)
  340. +{
  341. + SetByExtentsX(radius, length);
  342. + b2Transform xf;
  343. + xf.position = center;
  344. + xf.R.Set(angle);
  345. +
  346. + m_vertices[0] = b2Mul(xf, m_vertices[0]);
  347. + m_vertices[1] = b2Mul(xf, m_vertices[1]);
  348. +}
  349. +
  350. +
  351. +void b2CapsuleShape::SetByExtentsY(float32 radius, float32 height)
  352. +{
  353. + m_radius = radius;
  354. + float32 half = height * 0.5f;
  355. + m_vertices[0] = b2Vec2(0.0f, -half);
  356. + m_vertices[1] = b2Vec2(0.0f, half);
  357. +}
  358. +
  359. +void b2CapsuleShape::SetByExtentsY(float32 radius, float32 height,
  360. +   const b2Vec2& center, float32 angle)
  361. +{
  362. + SetByExtentsY(radius, height);
  363. + b2Transform xf;
  364. + xf.position = center;
  365. + xf.R.Set(angle);
  366. +
  367. + m_vertices[0] = b2Mul(xf, m_vertices[0]);
  368. + m_vertices[1] = b2Mul(xf, m_vertices[1]);
  369. +}
  370. +
  371. +b2Shape* b2CapsuleShape::Clone(b2BlockAllocator* allocator) const
  372. +{
  373. + void* mem = allocator->Allocate(sizeof(b2CapsuleShape));
  374. + b2CapsuleShape* clone = new (mem) b2CapsuleShape;
  375. + *clone = *this;
  376. + return clone;
  377. +}
  378. +
  379. +bool b2CapsuleShape::TestPoint(const b2Transform& transform, const b2Vec2& p) const
  380. +{
  381. + b2Vec2 local = b2MulT(transform, p);
  382. + b2Vec2 d = b2NearestPointOnLine(local, m_vertices[0], m_vertices[1]) - local;
  383. + return (d.LengthSquared() <= m_radius * m_radius);
  384. +}
  385. +
  386. +bool b2CapsuleShape::RayCast(b2RayCastOutput* output, const b2RayCastInput& input, const b2Transform& transform) const
  387. +{
  388. + // Test for starting inside first significantly reduces complexity
  389. + if (TestPoint(transform, input.p1)) {
  390. +     return false;
  391. + }
  392. +
  393. + // Project capsule to world
  394. + b2Vec2 capsule[2];
  395. + capsule[0] = b2Mul(transform, m_vertices[0]);
  396. + capsule[1] = b2Mul(transform, m_vertices[1]);
  397. + b2Vec2 d = capsule[1] - capsule[0];
  398. +
  399. + // Consider "cylinder"
  400. +
  401. + // Figure out signed distance from p1 to infinite capsule line
  402. + float32 ld = b2Cross(d, capsule[0] - input.p1);
  403. +
  404. + // Only bother if we don't start inside the infinite "cylinder"
  405. + if (!(ld * ld <= m_radius * m_radius * d.LengthSquared())) {
  406. +
  407. + // Find a perpendicular vector to the intersect, with length equal to radius
  408. + b2Vec2 perp = b2Cross(d, m_radius / d.Length());
  409. +
  410. + // Push the capsule segment by that vector
  411. + // Must swap if coming from the other side
  412. + b2Vec2 boundary[2];
  413. + if (ld < 0) {
  414. + boundary[0] = capsule[1] - perp;
  415. + boundary[1] = capsule[0] - perp;
  416. + }
  417. + else {
  418. + boundary[0] = capsule[0] + perp;
  419. + boundary[1] = capsule[1] + perp;
  420. + }
  421. +
  422. + // Check intersection against the adjusted segments.
  423. + if (b2RaycastSegment(output, input, boundary[0], boundary[1]))
  424. + return true;
  425. + }
  426. +
  427. + // Consider circular caps
  428. +
  429. + // Precompute some circle values
  430. + b2Vec2 r = input.p2 - input.p1;
  431. + float32 rr = r.LengthSquared();
  432. +
  433. + // Check for short segment
  434. + if (rr < b2_epsilon)
  435. + {
  436. + return false;
  437. + }
  438. +
  439. + // Check the circle caps, starting with closer
  440. + int startingIndex = 0;
  441. + if ((capsule[1] - input.p1).LengthSquared() < (capsule[0] - input.p1).LengthSquared()) {
  442. + startingIndex = 1;
  443. + }
  444. + b2Vec2 center = capsule[startingIndex];
  445. + for (int i = 0; i < 2; ++i)
  446. + {
  447. + b2Vec2 s = input.p1 - center;
  448. + float32 b = b2Dot(s, s) - m_radius * m_radius;
  449. +
  450. + // Should not start inside
  451. + b2Assert(!(b < 0.0f));
  452. +
  453. + // Solve quadratic equation.
  454. + float32 c =  b2Dot(s, r);
  455. + float32 sigma = c * c - rr * b;
  456. +
  457. + // Check for negative discriminant.
  458. + if (!(sigma < 0.0f))
  459. + {
  460. + // Find the point of intersection of the line with the circle.
  461. + float32 a = -(c + b2Sqrt(sigma));
  462. +
  463. + // Is the intersection point on the segment?
  464. + if (0.0f <= a && a <= input.maxFraction * rr)
  465. + {
  466. + a /= rr;
  467. + output->fraction = a;
  468. + output->normal = s + a * r;
  469. + output->normal.Normalize();
  470. + return true;
  471. + }
  472. + }
  473. + center = capsule[!startingIndex];
  474. + }
  475. +
  476. + return false;
  477. +}
  478. +
  479. +void b2CapsuleShape::ComputeAABB(b2AABB* aabb, const b2Transform& transform) const
  480. +{
  481. + b2Vec2 p1 = transform.position + b2Mul(transform.R, m_vertices[0]);
  482. + b2Vec2 p2 = transform.position + b2Mul(transform.R, m_vertices[1]);
  483. + b2Vec2 min = b2Min(p1, p2);
  484. + b2Vec2 max = b2Max(p1, p2);
  485. + aabb->lowerBound.Set(min.x - m_radius, min.y - m_radius);
  486. + aabb->upperBound.Set(max.x + m_radius, max.y + m_radius);
  487. +}
  488. +
  489. +void b2CapsuleShape::ComputeMass(b2MassData* massData, float32 density) const
  490. +{
  491. +  // Area of rectangle + 2 half circles
  492. + float32 rectHeight = (m_vertices[1] - m_vertices[0]).Length();
  493. + float32 rectMass = density * rectHeight * m_radius;
  494. + float32 circleMass = density * b2_pi * m_radius * m_radius;
  495. + massData->mass = rectMass + circleMass;
  496. + massData->center = 0.5f * (m_vertices[0] + m_vertices[1]);
  497. +
  498. + // inertia about the local origin
  499. + float32 rectInertia = rectMass * (rectHeight * rectHeight + m_radius * m_radius) / 12.0f;
  500. + float32 circleInertia = circleMass * (0.5f * m_radius * m_radius + rectHeight * rectHeight * .25f);
  501. + massData->I = rectInertia + circleInertia + massData->mass * massData->center.LengthSquared();
  502. +}
  503. Index: Box2D/Box2D/Collision/Shapes/b2CapsuleShape.h
  504. ===================================================================
  505. --- Box2D/Box2D/Collision/Shapes/b2CapsuleShape.h (revision 0)
  506. +++ Box2D/Box2D/Collision/Shapes/b2CapsuleShape.h (revision 0)
  507. @@ -0,0 +1,103 @@
  508. +/*
  509. +* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
  510. +*
  511. +* This software is provided 'as-is', without any express or implied
  512. +* warranty.  In no event will the authors be held liable for any damages
  513. +* arising from the use of this software.
  514. +* Permission is granted to anyone to use this software for any purpose,
  515. +* including commercial applications, and to alter it and redistribute it
  516. +* freely, subject to the following restrictions:
  517. +* 1. The origin of this software must not be misrepresented; you must not
  518. +* claim that you wrote the original software. If you use this software
  519. +* in a product, an acknowledgment in the product documentation would be
  520. +* appreciated but is not required.
  521. +* 2. Altered source versions must be plainly marked as such, and must not be
  522. +* misrepresented as being the original software.
  523. +* 3. This notice may not be removed or altered from any source distribution.
  524. +*/
  525. +
  526. +#ifndef B2_CAPSULE_SHAPE_H
  527. +#define B2_CAPSULE_SHAPE_H
  528. +
  529. +#include <Box2D/Collision/Shapes/b2Shape.h>
  530. +
  531. +/// A circle shape.
  532. +class b2CapsuleShape : public b2Shape
  533. +{
  534. +public:
  535. + b2CapsuleShape();
  536. +
  537. + /// Set to a capsule across the X axis, with given width between focii.
  538. + void SetByExtentsX(float32 radius, float32 width);
  539. + /// Set to a capsule across the X axis, rotated and transposed.
  540. + void SetByExtentsX(float32 radius, float32 width,
  541. +     const b2Vec2& center, float32 angle);
  542. +    
  543. + /// Set to a capsule across the Y axis, with given height between focii.
  544. + void SetByExtentsY(float32 radius, float32 height);
  545. + /// Set to a capsule across the Y axis, rotated and transposed.
  546. + void SetByExtentsY(float32 radius, float32 height,
  547. +     const b2Vec2& center, float32 angle);
  548. +
  549. + /// Implement b2Shape.
  550. + b2Shape* Clone(b2BlockAllocator* allocator) const;
  551. +
  552. + /// Implement b2Shape.
  553. + bool TestPoint(const b2Transform& transform, const b2Vec2& p) const;
  554. +
  555. + /// Implement b2Shape.
  556. + bool RayCast(b2RayCastOutput* output, const b2RayCastInput& input, const b2Transform& transform) const;
  557. +
  558. + /// @see b2Shape::ComputeAABB
  559. + void ComputeAABB(b2AABB* aabb, const b2Transform& transform) const;
  560. +
  561. + /// @see b2Shape::ComputeMass
  562. + void ComputeMass(b2MassData* massData, float32 density) const;
  563. +
  564. + /// Get the supporting vertex index in the given direction.
  565. + int32 GetSupport(const b2Vec2& d) const;
  566. +
  567. + /// Get the supporting vertex in the given direction.
  568. + const b2Vec2& GetSupportVertex(const b2Vec2& d) const;
  569. +
  570. + /// Get the vertex count.
  571. + int32 GetVertexCount() const { return 2; }
  572. +
  573. + /// Get a vertex by index. Used by b2Distance.
  574. + const b2Vec2& GetVertex(int32 index) const;
  575. +
  576. + /// Position of circle centers
  577. + b2Vec2 m_vertices[2];
  578. +};
  579. +
  580. +inline b2CapsuleShape::b2CapsuleShape()
  581. +{
  582. + m_type = e_capsule;
  583. + m_radius = 0.0f;
  584. + m_vertices[0].SetZero();
  585. + m_vertices[1].SetZero();
  586. +}
  587. +
  588. +
  589. +inline int32 b2CapsuleShape::GetSupport(const b2Vec2 &d) const
  590. +{
  591. +    if ((m_vertices[0] - d).LengthSquared() <= (m_vertices[1] - d).LengthSquared()) {
  592. +        return 0;
  593. +    }
  594. + return 1;
  595. +}
  596. +
  597. +inline const b2Vec2& b2CapsuleShape::GetSupportVertex(const b2Vec2 &d) const
  598. +{
  599. +    return GetVertex(GetSupport(d));
  600. +}
  601. +
  602. +inline const b2Vec2& b2CapsuleShape::GetVertex(int32 index) const
  603. +{
  604. + b2Assert(index == 0 || index == 1);
  605. + return m_vertices[index];
  606. +}
  607. +
  608. +
  609. +
  610. +#endif
  611. Index: Box2D/Box2D/Collision/Shapes/b2Shape.h
  612. ===================================================================
  613. --- Box2D/Box2D/Collision/Shapes/b2Shape.h (revision 70)
  614. +++ Box2D/Box2D/Collision/Shapes/b2Shape.h (working copy)
  615. @@ -48,7 +48,8 @@
  616.   e_unknown= -1,
  617.   e_circle = 0,
  618.   e_polygon = 1,
  619. - e_typeCount = 2,
  620. + e_capsule = 2,
  621. + e_typeCount = 3,
  622.   };
  623.  
  624.   b2Shape() { m_type = e_unknown; }
  625. Index: Box2D/Box2D/Dynamics/b2Fixture.cpp
  626. ===================================================================
  627. --- Box2D/Box2D/Dynamics/b2Fixture.cpp (revision 70)
  628. +++ Box2D/Box2D/Dynamics/b2Fixture.cpp (working copy)
  629. @@ -18,6 +18,7 @@
  630.  
  631.  #include <Box2D/Dynamics/b2Fixture.h>
  632.  #include <Box2D/Dynamics/Contacts/b2Contact.h>
  633. +#include <Box2D/Collision/Shapes/b2CapsuleShape.h>
  634.  #include <Box2D/Collision/Shapes/b2CircleShape.h>
  635.  #include <Box2D/Collision/Shapes/b2PolygonShape.h>
  636.  #include <Box2D/Collision/b2BroadPhase.h>
  637. @@ -82,6 +83,14 @@
  638.   allocator->Free(s, sizeof(b2PolygonShape));
  639.   }
  640.   break;
  641. +
  642. + case b2Shape::e_capsule:
  643. +     {
  644. + b2CapsuleShape* s = (b2CapsuleShape*)m_shape;
  645. + s->~b2CapsuleShape();
  646. + allocator->Free(s, sizeof(b2CapsuleShape));
  647. +     }
  648. +     break;
  649.  
  650.   default:
  651.   b2Assert(false);
  652. Index: Box2D/Box2D/Dynamics/b2World.cpp
  653. ===================================================================
  654. --- Box2D/Box2D/Dynamics/b2World.cpp (revision 70)
  655. +++ Box2D/Box2D/Dynamics/b2World.cpp (working copy)
  656. @@ -26,6 +26,7 @@
  657.  #include <Box2D/Dynamics/Contacts/b2TOISolver.h>
  658.  #include <Box2D/Collision/b2Collision.h>
  659.  #include <Box2D/Collision/b2BroadPhase.h>
  660. +#include <Box2D/Collision/Shapes/b2CapsuleShape.h>
  661.  #include <Box2D/Collision/Shapes/b2CircleShape.h>
  662.  #include <Box2D/Collision/Shapes/b2PolygonShape.h>
  663.  #include <Box2D/Collision/b2TimeOfImpact.h>
  664. @@ -907,6 +908,18 @@
  665.   m_debugDraw->DrawSolidPolygon(vertices, vertexCount, color);
  666.   }
  667.   break;
  668. +
  669. + case b2Shape::e_capsule:
  670. + {
  671. + b2CapsuleShape* capsule = (b2CapsuleShape*)fixture->GetShape();
  672. +
  673. + b2Vec2 p1 = b2Mul(xf, capsule->m_vertices[0]);
  674. + b2Vec2 p2 = b2Mul(xf, capsule->m_vertices[1]);
  675. + float32 radius = capsule->m_radius;
  676. +
  677. + m_debugDraw->DrawSolidCapsule(p1, p2, radius, color);
  678. + }
  679. + break;
  680.   }
  681.  }
  682.  
  683. Index: Box2D/Box2D/Dynamics/b2WorldCallbacks.h
  684. ===================================================================
  685. --- Box2D/Box2D/Dynamics/b2WorldCallbacks.h (revision 70)
  686. +++ Box2D/Box2D/Dynamics/b2WorldCallbacks.h (working copy)
  687. @@ -206,6 +206,12 @@
  688.   /// Draw a solid circle.
  689.   virtual void DrawSolidCircle(const b2Vec2& center, float32 radius, const b2Vec2& axis, const b2Color& color) = 0;
  690.  
  691. + /// Draw a capsule.
  692. + virtual void DrawCapsule(const b2Vec2& p1, const b2Vec2& p2, float32 radius, const b2Color& color) = 0;
  693. +
  694. + /// Draw a solid capsule.
  695. + virtual void DrawSolidCapsule(const b2Vec2& p1, const b2Vec2& p2, float32 radius, const b2Color& color) = 0;
  696. +
  697.   /// Draw a line segment.
  698.   virtual void DrawSegment(const b2Vec2& p1, const b2Vec2& p2, const b2Color& color) = 0;
  699.  
  700. Index: Box2D/Box2D/Dynamics/Contacts/b2CapsuleAndCircleContact.cpp
  701. ===================================================================
  702. --- Box2D/Box2D/Dynamics/Contacts/b2CapsuleAndCircleContact.cpp (revision 0)
  703. +++ Box2D/Box2D/Dynamics/Contacts/b2CapsuleAndCircleContact.cpp (revision 0)
  704. @@ -0,0 +1,63 @@
  705. +/*
  706. +* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
  707. +*
  708. +* This software is provided 'as-is', without any express or implied
  709. +* warranty.  In no event will the authors be held liable for any damages
  710. +* arising from the use of this software.
  711. +* Permission is granted to anyone to use this software for any purpose,
  712. +* including commercial applications, and to alter it and redistribute it
  713. +* freely, subject to the following restrictions:
  714. +* 1. The origin of this software must not be misrepresented; you must not
  715. +* claim that you wrote the original software. If you use this software
  716. +* in a product, an acknowledgment in the product documentation would be
  717. +* appreciated but is not required.
  718. +* 2. Altered source versions must be plainly marked as such, and must not be
  719. +* misrepresented as being the original software.
  720. +* 3. This notice may not be removed or altered from any source distribution.
  721. +*/
  722. +
  723. +#include <Box2D/Dynamics/Contacts/b2CapsuleAndCircleContact.h>
  724. +#include <Box2D/Dynamics/b2Body.h>
  725. +#include <Box2D/Dynamics/b2Fixture.h>
  726. +#include <Box2D/Dynamics/b2WorldCallbacks.h>
  727. +#include <Box2D/Common/b2BlockAllocator.h>
  728. +#include <Box2D/Collision/b2TimeOfImpact.h>
  729. +#include <Box2D/Collision/Shapes/b2CapsuleShape.h>
  730. +#include <Box2D/Collision/Shapes/b2PolygonShape.h>
  731. +
  732. +#include <new>
  733. +
  734. +b2Contact* b2CapsuleAndCircleContact::Create(b2Fixture* fixtureA, b2Fixture* fixtureB, b2BlockAllocator* allocator)
  735. +{
  736. + void* mem = allocator->Allocate(sizeof(b2CapsuleAndCircleContact));
  737. + return new (mem) b2CapsuleAndCircleContact(fixtureA, fixtureB);
  738. +}
  739. +
  740. +void b2CapsuleAndCircleContact::Destroy(b2Contact* contact, b2BlockAllocator* allocator)
  741. +{
  742. + ((b2CapsuleAndCircleContact*)contact)->~b2CapsuleAndCircleContact();
  743. + allocator->Free(contact, sizeof(b2CapsuleAndCircleContact));
  744. +}
  745. +
  746. +b2CapsuleAndCircleContact::b2CapsuleAndCircleContact(b2Fixture* fixtureA, b2Fixture* fixtureB)
  747. + : b2Contact(fixtureA, fixtureB)
  748. +{
  749. + b2Assert(m_fixtureA->GetType() == b2Shape::e_capsule);
  750. + b2Assert(m_fixtureB->GetType() == b2Shape::e_circle);
  751. +}
  752. +
  753. +void b2CapsuleAndCircleContact::Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB)
  754. +{
  755. + b2Body* bodyA = m_fixtureA->GetBody();
  756. + b2Body* bodyB = m_fixtureB->GetBody();
  757. +
  758. + b2CapsuleShape* capsule = (b2CapsuleShape*)m_fixtureA->GetShape();
  759. +
  760. + b2PolygonShape polygon;
  761. + polygon.SetAsEdge(capsule->m_vertices[0], capsule->m_vertices[1]);
  762. + polygon.m_radius = capsule->m_radius;
  763. +
  764. + b2CollidePolygonAndCircle( manifold,
  765. + &polygon, xfA,
  766. + (b2CircleShape*)m_fixtureB->GetShape(), xfB);
  767. +}
  768. Index: Box2D/Box2D/Dynamics/Contacts/b2CapsuleAndCircleContact.h
  769. ===================================================================
  770. --- Box2D/Box2D/Dynamics/Contacts/b2CapsuleAndCircleContact.h (revision 0)
  771. +++ Box2D/Box2D/Dynamics/Contacts/b2CapsuleAndCircleContact.h (revision 0)
  772. @@ -0,0 +1,38 @@
  773. +/*
  774. +* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
  775. +*
  776. +* This software is provided 'as-is', without any express or implied
  777. +* warranty.  In no event will the authors be held liable for any damages
  778. +* arising from the use of this software.
  779. +* Permission is granted to anyone to use this software for any purpose,
  780. +* including commercial applications, and to alter it and redistribute it
  781. +* freely, subject to the following restrictions:
  782. +* 1. The origin of this software must not be misrepresented; you must not
  783. +* claim that you wrote the original software. If you use this software
  784. +* in a product, an acknowledgment in the product documentation would be
  785. +* appreciated but is not required.
  786. +* 2. Altered source versions must be plainly marked as such, and must not be
  787. +* misrepresented as being the original software.
  788. +* 3. This notice may not be removed or altered from any source distribution.
  789. +*/
  790. +
  791. +#ifndef B2_CAPSULE_AND_CIRCLE_CONTACT_H
  792. +#define B2_CAPSULE_AND_CIRCLE_CONTACT_H
  793. +
  794. +#include <Box2D/Dynamics/Contacts/b2Contact.h>
  795. +
  796. +class b2BlockAllocator;
  797. +
  798. +class b2CapsuleAndCircleContact : public b2Contact
  799. +{
  800. +public:
  801. + static b2Contact* Create(b2Fixture* fixtureA, b2Fixture* fixtureB, b2BlockAllocator* allocator);
  802. + static void Destroy(b2Contact* contact, b2BlockAllocator* allocator);
  803. +
  804. + b2CapsuleAndCircleContact(b2Fixture* fixtureA, b2Fixture* fixtureB);
  805. + ~b2CapsuleAndCircleContact() {}
  806. +
  807. + void Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB);
  808. +};
  809. +
  810. +#endif
  811. Index: Box2D/Box2D/Dynamics/Contacts/b2CapsuleContact.cpp
  812. ===================================================================
  813. --- Box2D/Box2D/Dynamics/Contacts/b2CapsuleContact.cpp (revision 0)
  814. +++ Box2D/Box2D/Dynamics/Contacts/b2CapsuleContact.cpp (revision 0)
  815. @@ -0,0 +1,66 @@
  816. +/*
  817. +* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
  818. +*
  819. +* This software is provided 'as-is', without any express or implied
  820. +* warranty.  In no event will the authors be held liable for any damages
  821. +* arising from the use of this software.
  822. +* Permission is granted to anyone to use this software for any purpose,
  823. +* including commercial applications, and to alter it and redistribute it
  824. +* freely, subject to the following restrictions:
  825. +* 1. The origin of this software must not be misrepresented; you must not
  826. +* claim that you wrote the original software. If you use this software
  827. +* in a product, an acknowledgment in the product documentation would be
  828. +* appreciated but is not required.
  829. +* 2. Altered source versions must be plainly marked as such, and must not be
  830. +* misrepresented as being the original software.
  831. +* 3. This notice may not be removed or altered from any source distribution.
  832. +*/
  833. +
  834. +#include <Box2D/Dynamics/Contacts/b2CapsuleContact.h>
  835. +#include <Box2D/Dynamics/b2Body.h>
  836. +#include <Box2D/Dynamics/b2Fixture.h>
  837. +#include <Box2D/Dynamics/b2WorldCallbacks.h>
  838. +#include <Box2D/Common/b2BlockAllocator.h>
  839. +#include <Box2D/Collision/b2TimeOfImpact.h>
  840. +#include <Box2D/Collision/Shapes/b2CapsuleShape.h>
  841. +#include <Box2D/Collision/Shapes/b2PolygonShape.h>
  842. +
  843. +#include <new>
  844. +
  845. +b2Contact* b2CapsuleContact::Create(b2Fixture* fixtureA, b2Fixture* fixtureB, b2BlockAllocator* allocator)
  846. +{
  847. + void* mem = allocator->Allocate(sizeof(b2CapsuleContact));
  848. + return new (mem) b2CapsuleContact(fixtureA, fixtureB);
  849. +}
  850. +
  851. +void b2CapsuleContact::Destroy(b2Contact* contact, b2BlockAllocator* allocator)
  852. +{
  853. + ((b2CapsuleContact*)contact)->~b2CapsuleContact();
  854. + allocator->Free(contact, sizeof(b2CapsuleContact));
  855. +}
  856. +
  857. +b2CapsuleContact::b2CapsuleContact(b2Fixture* fixtureA, b2Fixture* fixtureB)
  858. + : b2Contact(fixtureA, fixtureB)
  859. +{
  860. + b2Assert(m_fixtureA->GetType() == b2Shape::e_capsule);
  861. + b2Assert(m_fixtureB->GetType() == b2Shape::e_capsule);
  862. +}
  863. +
  864. +void b2CapsuleContact::Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB)
  865. +{
  866. + b2Body* bodyA = m_fixtureA->GetBody();
  867. + b2Body* bodyB = m_fixtureB->GetBody();
  868. +
  869. + b2CapsuleShape* capsule1 = (b2CapsuleShape*)m_fixtureA->GetShape();
  870. + b2CapsuleShape* capsule2 = (b2CapsuleShape*)m_fixtureB->GetShape();
  871. +
  872. + b2PolygonShape polygon1, polygon2;
  873. + polygon1.SetAsEdge(capsule1->m_vertices[0], capsule1->m_vertices[1]);
  874. + polygon2.SetAsEdge(capsule2->m_vertices[0], capsule2->m_vertices[1]);
  875. + polygon1.m_radius = capsule1->m_radius;
  876. + polygon2.m_radius = capsule2->m_radius;
  877. +
  878. + b2CollidePolygons( manifold,
  879. + &polygon1, xfA,
  880. + &polygon2, xfB);
  881. +}
  882. Index: Box2D/Box2D/Dynamics/Contacts/b2CapsuleContact.h
  883. ===================================================================
  884. --- Box2D/Box2D/Dynamics/Contacts/b2CapsuleContact.h (revision 0)
  885. +++ Box2D/Box2D/Dynamics/Contacts/b2CapsuleContact.h (revision 0)
  886. @@ -0,0 +1,38 @@
  887. +/*
  888. +* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
  889. +*
  890. +* This software is provided 'as-is', without any express or implied
  891. +* warranty.  In no event will the authors be held liable for any damages
  892. +* arising from the use of this software.
  893. +* Permission is granted to anyone to use this software for any purpose,
  894. +* including commercial applications, and to alter it and redistribute it
  895. +* freely, subject to the following restrictions:
  896. +* 1. The origin of this software must not be misrepresented; you must not
  897. +* claim that you wrote the original software. If you use this software
  898. +* in a product, an acknowledgment in the product documentation would be
  899. +* appreciated but is not required.
  900. +* 2. Altered source versions must be plainly marked as such, and must not be
  901. +* misrepresented as being the original software.
  902. +* 3. This notice may not be removed or altered from any source distribution.
  903. +*/
  904. +
  905. +#ifndef B2_CAPSULE_CONTACT_H
  906. +#define B2_CAPSULE_CONTACT_H
  907. +
  908. +#include <Box2D/Dynamics/Contacts/b2Contact.h>
  909. +
  910. +class b2BlockAllocator;
  911. +
  912. +class b2CapsuleContact : public b2Contact
  913. +{
  914. +public:
  915. + static b2Contact* Create(b2Fixture* fixtureA, b2Fixture* fixtureB, b2BlockAllocator* allocator);
  916. + static void Destroy(b2Contact* contact, b2BlockAllocator* allocator);
  917. +
  918. + b2CapsuleContact(b2Fixture* fixtureA, b2Fixture* fixtureB);
  919. + ~b2CapsuleContact() {}
  920. +
  921. + void Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB);
  922. +};
  923. +
  924. +#endif
  925. Index: Box2D/Box2D/Dynamics/Contacts/b2Contact.cpp
  926. ===================================================================
  927. --- Box2D/Box2D/Dynamics/Contacts/b2Contact.cpp (revision 70)
  928. +++ Box2D/Box2D/Dynamics/Contacts/b2Contact.cpp (working copy)
  929. @@ -17,7 +17,10 @@
  930.  */
  931.  
  932.  #include <Box2D/Dynamics/Contacts/b2Contact.h>
  933. +#include <Box2D/Dynamics/Contacts/b2CapsuleContact.h>
  934. +#include <Box2D/Dynamics/Contacts/b2CapsuleAndCircleContact.h>
  935.  #include <Box2D/Dynamics/Contacts/b2CircleContact.h>
  936. +#include <Box2D/Dynamics/Contacts/b2PolygonAndCapsuleContact.h>
  937.  #include <Box2D/Dynamics/Contacts/b2PolygonAndCircleContact.h>
  938.  #include <Box2D/Dynamics/Contacts/b2PolygonContact.h>
  939.  #include <Box2D/Dynamics/Contacts/b2ContactSolver.h>
  940. @@ -38,6 +41,10 @@
  941.   AddType(b2CircleContact::Create, b2CircleContact::Destroy, b2Shape::e_circle, b2Shape::e_circle);
  942.   AddType(b2PolygonAndCircleContact::Create, b2PolygonAndCircleContact::Destroy, b2Shape::e_polygon, b2Shape::e_circle);
  943.   AddType(b2PolygonContact::Create, b2PolygonContact::Destroy, b2Shape::e_polygon, b2Shape::e_polygon);
  944. +
  945. + AddType(b2CapsuleContact::Create, b2CapsuleContact::Destroy, b2Shape::e_capsule, b2Shape::e_capsule);
  946. + AddType(b2CapsuleAndCircleContact::Create, b2CapsuleAndCircleContact::Destroy, b2Shape::e_capsule, b2Shape::e_circle);
  947. + AddType(b2PolygonAndCapsuleContact::Create, b2PolygonAndCapsuleContact::Destroy, b2Shape::e_polygon, b2Shape::e_capsule);
  948.  }
  949.  
  950.  void b2Contact::AddType(b2ContactCreateFcn* createFcn, b2ContactDestroyFcn* destoryFcn,
  951. Index: Box2D/Box2D/Dynamics/Contacts/b2PolygonAndCapsuleContact.cpp
  952. ===================================================================
  953. --- Box2D/Box2D/Dynamics/Contacts/b2PolygonAndCapsuleContact.cpp (revision 0)
  954. +++ Box2D/Box2D/Dynamics/Contacts/b2PolygonAndCapsuleContact.cpp (revision 0)
  955. @@ -0,0 +1,63 @@
  956. +/*
  957. +* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
  958. +*
  959. +* This software is provided 'as-is', without any express or implied
  960. +* warranty.  In no event will the authors be held liable for any damages
  961. +* arising from the use of this software.
  962. +* Permission is granted to anyone to use this software for any purpose,
  963. +* including commercial applications, and to alter it and redistribute it
  964. +* freely, subject to the following restrictions:
  965. +* 1. The origin of this software must not be misrepresented; you must not
  966. +* claim that you wrote the original software. If you use this software
  967. +* in a product, an acknowledgment in the product documentation would be
  968. +* appreciated but is not required.
  969. +* 2. Altered source versions must be plainly marked as such, and must not be
  970. +* misrepresented as being the original software.
  971. +* 3. This notice may not be removed or altered from any source distribution.
  972. +*/
  973. +
  974. +#include <Box2D/Dynamics/Contacts/b2PolygonAndCapsuleContact.h>
  975. +#include <Box2D/Dynamics/b2Body.h>
  976. +#include <Box2D/Dynamics/b2Fixture.h>
  977. +#include <Box2D/Dynamics/b2WorldCallbacks.h>
  978. +#include <Box2D/Common/b2BlockAllocator.h>
  979. +#include <Box2D/Collision/b2TimeOfImpact.h>
  980. +#include <Box2D/Collision/Shapes/b2CapsuleShape.h>
  981. +#include <Box2D/Collision/Shapes/b2PolygonShape.h>
  982. +
  983. +#include <new>
  984. +
  985. +b2Contact* b2PolygonAndCapsuleContact::Create(b2Fixture* fixtureA, b2Fixture* fixtureB, b2BlockAllocator* allocator)
  986. +{
  987. + void* mem = allocator->Allocate(sizeof(b2PolygonAndCapsuleContact));
  988. + return new (mem) b2PolygonAndCapsuleContact(fixtureA, fixtureB);
  989. +}
  990. +
  991. +void b2PolygonAndCapsuleContact::Destroy(b2Contact* contact, b2BlockAllocator* allocator)
  992. +{
  993. + ((b2PolygonAndCapsuleContact*)contact)->~b2PolygonAndCapsuleContact();
  994. + allocator->Free(contact, sizeof(b2PolygonAndCapsuleContact));
  995. +}
  996. +
  997. +b2PolygonAndCapsuleContact::b2PolygonAndCapsuleContact(b2Fixture* fixtureA, b2Fixture* fixtureB)
  998. + : b2Contact(fixtureA, fixtureB)
  999. +{
  1000. + b2Assert(m_fixtureA->GetType() == b2Shape::e_polygon);
  1001. + b2Assert(m_fixtureB->GetType() == b2Shape::e_capsule);
  1002. +}
  1003. +
  1004. +void b2PolygonAndCapsuleContact::Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB)
  1005. +{
  1006. + b2Body* bodyA = m_fixtureA->GetBody();
  1007. + b2Body* bodyB = m_fixtureB->GetBody();
  1008. +
  1009. + b2CapsuleShape* capsule = (b2CapsuleShape*)m_fixtureB->GetShape();
  1010. +
  1011. + b2PolygonShape polygon;
  1012. + polygon.SetAsEdge(capsule->m_vertices[0], capsule->m_vertices[1]);
  1013. + polygon.m_radius = capsule->m_radius;
  1014. +
  1015. + b2CollidePolygons( manifold,
  1016. + (b2PolygonShape*)m_fixtureA->GetShape(), xfA,
  1017. + &polygon, xfB);
  1018. +}
  1019. Index: Box2D/Box2D/Dynamics/Contacts/b2PolygonAndCapsuleContact.h
  1020. ===================================================================
  1021. --- Box2D/Box2D/Dynamics/Contacts/b2PolygonAndCapsuleContact.h (revision 0)
  1022. +++ Box2D/Box2D/Dynamics/Contacts/b2PolygonAndCapsuleContact.h (revision 0)
  1023. @@ -0,0 +1,38 @@
  1024. +/*
  1025. +* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
  1026. +*
  1027. +* This software is provided 'as-is', without any express or implied
  1028. +* warranty.  In no event will the authors be held liable for any damages
  1029. +* arising from the use of this software.
  1030. +* Permission is granted to anyone to use this software for any purpose,
  1031. +* including commercial applications, and to alter it and redistribute it
  1032. +* freely, subject to the following restrictions:
  1033. +* 1. The origin of this software must not be misrepresented; you must not
  1034. +* claim that you wrote the original software. If you use this software
  1035. +* in a product, an acknowledgment in the product documentation would be
  1036. +* appreciated but is not required.
  1037. +* 2. Altered source versions must be plainly marked as such, and must not be
  1038. +* misrepresented as being the original software.
  1039. +* 3. This notice may not be removed or altered from any source distribution.
  1040. +*/
  1041. +
  1042. +#ifndef B2_POLYGON_AND_CAPSULE_CONTACT_H
  1043. +#define B2_POLYGON_AND_CAPSULE_CONTACT_H
  1044. +
  1045. +#include <Box2D/Dynamics/Contacts/b2Contact.h>
  1046. +
  1047. +class b2BlockAllocator;
  1048. +
  1049. +class b2PolygonAndCapsuleContact : public b2Contact
  1050. +{
  1051. +public:
  1052. + static b2Contact* Create(b2Fixture* fixtureA, b2Fixture* fixtureB, b2BlockAllocator* allocator);
  1053. + static void Destroy(b2Contact* contact, b2BlockAllocator* allocator);
  1054. +
  1055. + b2PolygonAndCapsuleContact(b2Fixture* fixtureA, b2Fixture* fixtureB);
  1056. + ~b2PolygonAndCapsuleContact() {}
  1057. +
  1058. + void Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB);
  1059. +};
  1060. +
  1061. +#endif
  1062. Index: Box2D/Testbed/Framework/Render.cpp
  1063. ===================================================================
  1064. --- Box2D/Testbed/Framework/Render.cpp (revision 70)
  1065. +++ Box2D/Testbed/Framework/Render.cpp (working copy)
  1066. @@ -110,6 +110,83 @@
  1067.   glEnd();
  1068.  }
  1069.  
  1070. +void DebugDraw::DrawCapsule(const b2Vec2& p1, const b2Vec2& p2, float32 radius, const b2Color& color)
  1071. +{
  1072. +    b2Vec2 displacement = p2 - p1;
  1073. +    displacement.Normalize();
  1074. +    float32 startingAngle = atan2f(displacement.y, displacement.x) + b2_pi / 2;
  1075. +    
  1076. + const float32 k_segments = 8.0f;
  1077. + const float32 k_increment = b2_pi / k_segments;
  1078. + float32 theta = startingAngle;
  1079. + glColor3f(color.r, color.g, color.b);
  1080. + glBegin(GL_LINE_LOOP);
  1081. + for (int32 i = 0; i < k_segments; ++i)
  1082. + {
  1083. + b2Vec2 v = p1 + radius * b2Vec2(cosf(theta), sinf(theta));
  1084. + glVertex2f(v.x, v.y);
  1085. + theta += k_increment;
  1086. + }
  1087. + for (int32 i = 0; i < k_segments; ++i)
  1088. + {
  1089. + b2Vec2 v = p2 + radius * b2Vec2(cosf(theta), sinf(theta));
  1090. + glVertex2f(v.x, v.y);
  1091. + theta += k_increment;
  1092. + }
  1093. + glEnd();
  1094. +}
  1095. +
  1096. +void DebugDraw::DrawSolidCapsule(const b2Vec2& p1, const b2Vec2& p2, float32 radius, const b2Color& color)
  1097. +{
  1098. +    b2Vec2 displacement = p2 - p1;
  1099. +    displacement.Normalize();
  1100. +    float32 startingAngle = atan2f(displacement.y, displacement.x) + b2_pi / 2;
  1101. +    
  1102. + const float32 k_segments = 8.0f;
  1103. + const float32 k_increment = b2_pi / k_segments;
  1104. + float32 theta = startingAngle;
  1105. + glEnable(GL_BLEND);
  1106. + glBlendFunc (GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
  1107. + glColor4f(0.5f * color.r, 0.5f * color.g, 0.5f * color.b, 0.5f);
  1108. + glBegin(GL_TRIANGLE_FAN);
  1109. + for (int32 i = 0; i < k_segments; ++i)
  1110. + {
  1111. + b2Vec2 v = p1 + radius * b2Vec2(cosf(theta), sinf(theta));
  1112. + glVertex2f(v.x, v.y);
  1113. + theta += k_increment;
  1114. + }
  1115. + for (int32 i = 0; i < k_segments; ++i)
  1116. + {
  1117. + b2Vec2 v = p2 + radius * b2Vec2(cosf(theta), sinf(theta));
  1118. + glVertex2f(v.x, v.y);
  1119. + theta += k_increment;
  1120. + }
  1121. + glEnd();
  1122. + glDisable(GL_BLEND);
  1123. +
  1124. + theta = startingAngle;
  1125. + glColor4f(color.r, color.g, color.b, 1.0f);
  1126. + glBegin(GL_LINE_LOOP);
  1127. + for (int32 i = 0; i < k_segments; ++i)
  1128. + {
  1129. + b2Vec2 v = p1 + radius * b2Vec2(cosf(theta), sinf(theta));
  1130. + glVertex2f(v.x, v.y);
  1131. + theta += k_increment;
  1132. + }
  1133. + for (int32 i = 0; i < k_segments; ++i)
  1134. + {
  1135. + b2Vec2 v = p2 + radius * b2Vec2(cosf(theta), sinf(theta));
  1136. + glVertex2f(v.x, v.y);
  1137. + theta += k_increment;
  1138. + }
  1139. + glEnd();
  1140. +    
  1141. + glBegin(GL_LINES);
  1142. + glVertex2f(p1.x, p1.y);
  1143. + glVertex2f(p2.x, p2.y);
  1144. + glEnd();
  1145. +}
  1146. +
  1147.  void DebugDraw::DrawSegment(const b2Vec2& p1, const b2Vec2& p2, const b2Color& color)
  1148.  {
  1149.   glColor3f(color.r, color.g, color.b);
  1150. Index: Box2D/Testbed/Framework/Render.h
  1151. ===================================================================
  1152. --- Box2D/Testbed/Framework/Render.h (revision 70)
  1153. +++ Box2D/Testbed/Framework/Render.h (working copy)
  1154. @@ -36,6 +36,10 @@
  1155.  
  1156.   void DrawSolidCircle(const b2Vec2& center, float32 radius, const b2Vec2& axis, const b2Color& color);
  1157.  
  1158. + void DrawCapsule(const b2Vec2& p1, const b2Vec2& p2, float32 radius, const b2Color& color);
  1159. +
  1160. + void DrawSolidCapsule(const b2Vec2& p1, const b2Vec2& p2, float32 radius, const b2Color& color);
  1161. +
  1162.   void DrawSegment(const b2Vec2& p1, const b2Vec2& p2, const b2Color& color);
  1163.  
  1164.   void DrawTransform(const b2Transform& xf);
  1165. Index: Box2D/Testbed/Tests/PolyShapes.h
  1166. ===================================================================
  1167. --- Box2D/Testbed/Tests/PolyShapes.h (revision 70)
  1168. +++ Box2D/Testbed/Tests/PolyShapes.h (working copy)
  1169. @@ -74,6 +74,19 @@
  1170.   m_debugDraw->DrawPolygon(vertices, vertexCount, color);
  1171.   }
  1172.   break;
  1173. +
  1174. + case b2Shape::e_capsule:
  1175. + {
  1176. + b2CapsuleShape* capsule = (b2CapsuleShape*)fixture->GetShape();
  1177. +
  1178. + b2Vec2 p1 = b2Mul(xf, capsule->m_vertices[0]);
  1179. + b2Vec2 p2 = b2Mul(xf, capsule->m_vertices[1]);
  1180. + float32 radius = capsule->m_radius;
  1181. +
  1182. + m_debugDraw->DrawCapsule(p1, p2, radius, color);
  1183. + }
  1184. + break;
  1185. +
  1186.   }
  1187.   }
  1188.  
  1189. @@ -163,6 +176,14 @@
  1190.   m_circle.m_radius = 0.5f;
  1191.   }
  1192.  
  1193. + {
  1194. +     m_capsules[0].SetByExtentsX(0.5f, 0.5f);
  1195. + }
  1196. +
  1197. + {
  1198. +     m_capsules[1].SetByExtentsY(0.1f, 1.8f, b2Vec2(0.0f, 1.0f), 0.0f);
  1199. + }
  1200. +
  1201.   m_bodyIndex = 0;
  1202.   memset(m_bodies, 0, sizeof(m_bodies));
  1203.   }
  1204. @@ -197,7 +218,7 @@
  1205.   fd.friction = 0.3f;
  1206.   m_bodies[m_bodyIndex]->CreateFixture(&fd);
  1207.   }
  1208. - else
  1209. + else if (index == 4)
  1210.   {
  1211.   b2FixtureDef fd;
  1212.   fd.shape = &m_circle;
  1213. @@ -206,6 +227,15 @@
  1214.  
  1215.   m_bodies[m_bodyIndex]->CreateFixture(&fd);
  1216.   }
  1217. + else
  1218. + {
  1219. + b2FixtureDef fd;
  1220. + fd.shape = m_capsules + (index - 5);
  1221. + fd.density = 1.0f;
  1222. + fd.friction = 0.3f;
  1223. + m_bodies[m_bodyIndex]->CreateFixture(&fd);
  1224. + }
  1225.  
  1226.   m_bodyIndex = (m_bodyIndex + 1) % k_maxBodies;
  1227.   }
  1228. @@ -232,6 +262,8 @@
  1229.   case '3':
  1230.   case '4':
  1231.   case '5':
  1232. + case '6':
  1233. + case '7':
  1234.   Create(key - '1');
  1235.   break;
  1236.  
  1237. @@ -270,7 +302,7 @@
  1238.   b2Color color(0.4f, 0.7f, 0.8f);
  1239.   m_debugDraw.DrawCircle(callback.m_circle.m_p, callback.m_circle.m_radius, color);
  1240.  
  1241. - m_debugDraw.DrawString(5, m_textLine, "Press 1-5 to drop stuff");
  1242. + m_debugDraw.DrawString(5, m_textLine, "Press 1-7 to drop stuff");
  1243.   m_textLine += 15;
  1244.   m_debugDraw.DrawString(5, m_textLine, "Press 'a' to (de)activate some bodies");
  1245.   m_textLine += 15;
  1246. @@ -287,6 +319,7 @@
  1247.   b2Body* m_bodies[k_maxBodies];
  1248.   b2PolygonShape m_polygons[4];
  1249.   b2CircleShape m_circle;
  1250. + b2CapsuleShape m_capsules[2];
  1251.  };
  1252.  
  1253.  #endif
  1254. Index: Box2D/Testbed/Tests/RayCast.h
  1255. ===================================================================
  1256. --- Box2D/Testbed/Tests/RayCast.h (revision 70)
  1257. +++ Box2D/Testbed/Tests/RayCast.h (working copy)
  1258. @@ -211,6 +211,14 @@
  1259.   m_circle.m_radius = 0.5f;
  1260.   }
  1261.  
  1262. + {
  1263. +     m_capsules[0].SetByExtentsX(0.5f, 0.5f);
  1264. + }
  1265. + {
  1266. +     m_capsules[1].SetByExtentsY(0.1f, 1.8f, b2Vec2(0.0f, 1.0f), 0.0f);
  1267. + }
  1268. +
  1269.   m_bodyIndex = 0;
  1270.   memset(m_bodies, 0, sizeof(m_bodies));
  1271.  
  1272. @@ -251,7 +259,7 @@
  1273.   fd.friction = 0.3f;
  1274.   m_bodies[m_bodyIndex]->CreateFixture(&fd);
  1275.   }
  1276. - else
  1277. + else if (index == 4)
  1278.   {
  1279.   b2FixtureDef fd;
  1280.   fd.shape = &m_circle;
  1281. @@ -259,6 +267,14 @@
  1282.  
  1283.   m_bodies[m_bodyIndex]->CreateFixture(&fd);
  1284.   }
  1285. + else
  1286. + {
  1287. + b2FixtureDef fd;
  1288. + fd.shape = m_capsules + (index - 5);
  1289. + fd.friction = 0.3f;
  1290. + m_bodies[m_bodyIndex]->CreateFixture(&fd);
  1291. + }
  1292.  
  1293.   m_bodyIndex = (m_bodyIndex + 1) % e_maxBodies;
  1294.   }
  1295. @@ -285,6 +301,8 @@
  1296.   case '3':
  1297.   case '4':
  1298.   case '5':
  1299. + case '6':
  1300. + case '7':
  1301.   Create(key - '1');
  1302.   break;
  1303.  
  1304. @@ -311,7 +329,7 @@
  1305.   void Step(Settings* settings)
  1306.   {
  1307.   Test::Step(settings);
  1308. - m_debugDraw.DrawString(5, m_textLine, "Press 1-5 to drop stuff, m to change the mode");
  1309. + m_debugDraw.DrawString(5, m_textLine, "Press 1-7 to drop stuff, m to change the mode");
  1310.   m_textLine += 15;
  1311.   m_debugDraw.DrawString(5, m_textLine, "Mode = %d", m_mode);
  1312.   m_textLine += 15;
  1313. @@ -385,6 +403,7 @@
  1314.   int32 m_userData[e_maxBodies];
  1315.   b2PolygonShape m_polygons[4];
  1316.   b2CircleShape m_circle;
  1317. + b2CapsuleShape m_capsules[2];
  1318.  
  1319.   float32 m_angle;
  1320.