Index: Box2D/Box2D/Box2D.h =================================================================== --- Box2D/Box2D/Box2D.h (revision 70) +++ Box2D/Box2D/Box2D.h (working copy) @@ -35,6 +35,7 @@ #include #include +#include #include #include Index: Box2D/Box2D/CMakeLists.txt =================================================================== --- Box2D/Box2D/CMakeLists.txt (revision 70) +++ Box2D/Box2D/CMakeLists.txt (working copy) @@ -15,10 +15,12 @@ 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 @@ 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 =================================================================== --- Box2D/Box2D/Collision/b2Collision.cpp (revision 70) +++ Box2D/Box2D/Collision/b2Collision.cpp (working copy) @@ -248,3 +248,220 @@ return output.distance < 10.0f * b2_epsilon; } + + +// Collision Detection in Interactive 3D Environments by Gino van den Bergen +// From Section 3.4.1 +// x = mu1 * p1 + mu2 * p2 +// mu1 + mu2 = 1 && mu1 >= 0 && mu2 >= 0 +// mu1 = 1 - mu2; +// x = (1 - mu2) * p1 + mu2 * p2 +// = p1 + mu2 * (p2 - p1) +// x = s + a * r (s := start, r := end - start) +// s + a * r = p1 + mu2 * d (d := p2 - p1) +// -a * r + mu2 * d = b (b := s - p1) +// [-r d] * [a; mu2] = b +// Cramer's rule: +// denom = det[-r d] +// a = det[b d] / denom +// mu2 = det[-r b] / denom +bool b2RaycastSegment(b2RayCastOutput* output, const b2RayCastInput& input, const b2Vec2& p1, const b2Vec2& p2) +{ + b2Vec2 s = input.p1; + b2Vec2 r = input.p2 - s; + b2Vec2 d = p2 - p1; + b2Vec2 n = b2Cross(d, 1.0f); + + const float32 k_slop = 100.0f * b2_epsilon; + float32 denom = -b2Dot(r, n); + + // Cull back facing collision and ignore parallel segments. + if (denom > k_slop) + { + // Does the segment intersect the infinite line associated with this segment? + b2Vec2 b = s - p1; + float32 a = b2Dot(b, n); + + if (0.0f <= a && a <= input.maxFraction * denom) + { + float32 mu2 = -r.x * b.y + r.y * b.x; + + // Does the segment intersect this segment? + if (-k_slop * denom <= mu2 && mu2 <= denom * (1.0f + k_slop)) + { + a /= denom; + n.Normalize(); + output->normal = n; + output->fraction = a; + return true; + } + } + } + + return false; +} + + +static void FindNearestPointOnLineSegment( b2Vec2* nearest, float32* parameter, + const b2Vec2& A1, const b2Vec2& L, const b2Vec2& B, + bool infinite_line ) +{ + float32 D = L.LengthSquared(); + if (D < b2_epsilon * b2_epsilon) + { + *nearest = A1; + return; + } + + b2Vec2 AB = B - A1; + *parameter = b2Dot(L, AB) / D; + if (!infinite_line) + { + *parameter = b2Clamp(*parameter, 0.0f, 1.0f); + } + *nearest = A1 + *parameter * L; +} + +// Based on Game Programming Gems 2 +// Fast, Robust Intersection of 3D Line Segments +// Graham Rhodes, Applied Research Associates +b2Vec2 b2NearestPointOnLine(const b2Vec2& point, const b2Vec2& p1, const b2Vec2& p2) +{ + b2Vec2 result; + float32 unused; + FindNearestPointOnLineSegment(&result, &unused, p1, p2 - p1, point, false); + return result; +} + +static void FindNearestPointOfParallelLineSegments( b2Vec2* x1, b2Vec2* x2, + const b2Vec2& A1, const b2Vec2& A2, const b2Vec2& La, + const b2Vec2& B1, const b2Vec2& B2, const b2Vec2& Lb) +{ + float32 s, t; + FindNearestPointOnLineSegment(x1, &s, A1, La, B1, true); + FindNearestPointOnLineSegment(x2, &t, A1, La, B2, true); + + if (s < 0.0f && t < 0.0f) + { + *x1 = A1; + if (s < t) + *x2 = B2; + else + *x2 = B1; + return; + } + + if (s > 1.0f && t > 1.0f) + { + *x1 = A2; + if (s < t) + *x2 = B1; + else + *x2 = B2; + return; + } + + float32 temp = 0.5f * (b2Clamp(s, 0.0f, 1.0f) + b2Clamp(t, 0.0f, 1.0f)); + *x1 = A1 + temp * La; + + float32 unused; + FindNearestPointOnLineSegment(x2, &unused, B1, Lb, *x1, true); +} + +static void AdjustNearestPoints(b2Vec2* x1, b2Vec2* x2, + const b2Vec2& A1, const b2Vec2& A2, const b2Vec2& La, + const b2Vec2& B1, const b2Vec2& B2, const b2Vec2& Lb, + float32 s, float32 t) +{ + if ((s < 0.0f || s > 1.0f) && (t < 0.0f || t > 1.0f)) + { + s = b2Clamp(s, 0.0f, 1.0f); + *x1 = A1 + s * La; + FindNearestPointOnLineSegment(x2, &t, B1, Lb, *x1, true); + if (t < 0.0f || t > 1.0f) + { + t = b2Clamp(t, 0.0f, 1.0f); + *x2 = B1 + t * Lb; + FindNearestPointOnLineSegment(x1, &s, A1, La, *x2, false); + FindNearestPointOnLineSegment(x2, &t, B1, Lb, *x1, false); + } + return; + } + if (s < 0.0f || s > 1.0f) + { + s = b2Clamp(s, 0.0f, 1.0f); + *x1 = A1 + s * La; + FindNearestPointOnLineSegment(x2, &t, B1, Lb, *x1, false); + return; + } + b2Assert(t < 0.0f || t > 1.0f); + t = b2Clamp(t, 0.0f, 1.0f); + *x2 = B1 + t * Lb; + FindNearestPointOnLineSegment(x1, &s, A1, La, *x2, false); +} + +static void IntersectLineSegments( b2Vec2* x1, b2Vec2* x2, + const b2Vec2& A1, const b2Vec2& A2, + const b2Vec2& B1, const b2Vec2& B2) +{ + float32 unused; + + b2Vec2 La = A2 - A1; + b2Vec2 Lb = B2 - B1; + float32 L11 = La.LengthSquared(); + float32 L22 = Lb.LengthSquared(); + + // Check for degenerate parameters + if (L11 < b2_epsilon * b2_epsilon) + { + *x1 = A1; + FindNearestPointOnLineSegment(x2, &unused, B1, Lb, A1, false); + return; + } + if (L22 < b2_epsilon * b2_epsilon) + { + *x2 = B1; + FindNearestPointOnLineSegment(x1, &unused, A1, La, B1, false); + return; + } + + b2Vec2 AB = B1 - A1; + float32 L12 = -b2Dot(La, Lb); + float32 DetL = L11 * L22 - L12 * L12; + + // parallel segments + if (b2Abs(DetL) < b2_epsilon) + { + FindNearestPointOfParallelLineSegments(x1, x2, A1, A2, La, B1, B2, Lb); + return; + } + + float32 ra = b2Dot(La, AB); + float32 rb = -b2Dot(Lb, AB); + + float32 t = (L11 * rb - ra * L12) / DetL; + float32 s = (ra - L12 * t) / L11; + + // These tests can't quite stay within B2_FLT_EPSILON + //b2Assert(b2Abs(s * L11 + t * L12 - ra) < .0001f); + //b2Assert(b2Abs(s * L12 + t * L22 - rb) < .0001f); + + *x1 = A1 + s * La; + *x2 = B1 + t * Lb; + + if (s < 0.0f || s > 1.0f || t < 0.0f || t > 1.0f) + { + AdjustNearestPoints(x1, x2, A1, A2, La, B1, B2, Lb, s, t); + } +} + +// Based on Game Programming Gems 2 +// Fast, Robust Intersection of 3D Line Segments +// Graham Rhodes, Applied Research Associates +float32 b2DistanceBetweenLines( b2Vec2* x1, b2Vec2* x2, + const b2Vec2& A1, const b2Vec2& A2, + const b2Vec2& B1, const b2Vec2& B2) +{ + IntersectLineSegments(x1, x2, A1, A2, B1, B2); + return (*x2 - *x1).Length(); +} \ No newline at end of file Index: Box2D/Box2D/Collision/b2Collision.h =================================================================== --- Box2D/Box2D/Collision/b2Collision.h (revision 70) +++ Box2D/Box2D/Collision/b2Collision.h (working copy) @@ -27,6 +27,7 @@ /// queries, and TOI queries. class b2Shape; +class b2CapsuleShape; class b2CircleShape; class b2PolygonShape; @@ -212,6 +213,18 @@ bool b2TestOverlap(const b2Shape* shapeA, const b2Shape* shapeB, const b2Transform& xfA, const b2Transform& xfB); +/// Perform a raycast against a given line segment. +bool b2RaycastSegment(b2RayCastOutput* output, const b2RayCastInput& input, const b2Vec2& p1, const b2Vec2& p2); + +/// Find the nearest point on a line. +b2Vec2 b2NearestPointOnLine(const b2Vec2& point, const b2Vec2& p1, const b2Vec2& p2); + +/// Find the nearest points on between two different lines. +float32 b2DistanceBetweenLines( b2Vec2* x1, b2Vec2* x2, + const b2Vec2& A1, const b2Vec2& A2, + const b2Vec2& B1, const b2Vec2& B2); + + // ---------------- Inline Functions ------------------------------------------ inline bool b2AABB::IsValid() const Index: Box2D/Box2D/Collision/b2Distance.cpp =================================================================== --- Box2D/Box2D/Collision/b2Distance.cpp (revision 70) +++ Box2D/Box2D/Collision/b2Distance.cpp (working copy) @@ -17,6 +17,7 @@ */ #include +#include #include #include @@ -44,6 +45,15 @@ m_radius = polygon->m_radius; } break; + + case b2Shape::e_capsule: + { + const b2CapsuleShape* capsule = (b2CapsuleShape*)shape; + m_vertices = capsule->m_vertices; + m_count = 2; + m_radius = capsule->m_radius; + } + break; default: b2Assert(false); Index: Box2D/Box2D/Collision/Shapes/b2CapsuleShape.cpp =================================================================== --- Box2D/Box2D/Collision/Shapes/b2CapsuleShape.cpp (revision 0) +++ Box2D/Box2D/Collision/Shapes/b2CapsuleShape.cpp (revision 0) @@ -0,0 +1,194 @@ +/* +* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +#include +#include + +void b2CapsuleShape::SetByExtentsX(float32 radius, float32 length) +{ + m_radius = radius; + float32 half = length * 0.5f; + m_vertices[0] = b2Vec2(-half, 0.0f); + m_vertices[1] = b2Vec2(half, 0.0f); +} + +void b2CapsuleShape::SetByExtentsX(float32 radius, float32 length, + const b2Vec2& center, float32 angle) +{ + SetByExtentsX(radius, length); + b2Transform xf; + xf.position = center; + xf.R.Set(angle); + + m_vertices[0] = b2Mul(xf, m_vertices[0]); + m_vertices[1] = b2Mul(xf, m_vertices[1]); +} + + +void b2CapsuleShape::SetByExtentsY(float32 radius, float32 height) +{ + m_radius = radius; + float32 half = height * 0.5f; + m_vertices[0] = b2Vec2(0.0f, -half); + m_vertices[1] = b2Vec2(0.0f, half); +} + +void b2CapsuleShape::SetByExtentsY(float32 radius, float32 height, + const b2Vec2& center, float32 angle) +{ + SetByExtentsY(radius, height); + b2Transform xf; + xf.position = center; + xf.R.Set(angle); + + m_vertices[0] = b2Mul(xf, m_vertices[0]); + m_vertices[1] = b2Mul(xf, m_vertices[1]); +} + +b2Shape* b2CapsuleShape::Clone(b2BlockAllocator* allocator) const +{ + void* mem = allocator->Allocate(sizeof(b2CapsuleShape)); + b2CapsuleShape* clone = new (mem) b2CapsuleShape; + *clone = *this; + return clone; +} + +bool b2CapsuleShape::TestPoint(const b2Transform& transform, const b2Vec2& p) const +{ + b2Vec2 local = b2MulT(transform, p); + b2Vec2 d = b2NearestPointOnLine(local, m_vertices[0], m_vertices[1]) - local; + return (d.LengthSquared() <= m_radius * m_radius); +} + +bool b2CapsuleShape::RayCast(b2RayCastOutput* output, const b2RayCastInput& input, const b2Transform& transform) const +{ + // Test for starting inside first significantly reduces complexity + if (TestPoint(transform, input.p1)) { + return false; + } + + // Project capsule to world + b2Vec2 capsule[2]; + capsule[0] = b2Mul(transform, m_vertices[0]); + capsule[1] = b2Mul(transform, m_vertices[1]); + b2Vec2 d = capsule[1] - capsule[0]; + + // Consider "cylinder" + + // Figure out signed distance from p1 to infinite capsule line + float32 ld = b2Cross(d, capsule[0] - input.p1); + + // Only bother if we don't start inside the infinite "cylinder" + if (!(ld * ld <= m_radius * m_radius * d.LengthSquared())) { + + // Find a perpendicular vector to the intersect, with length equal to radius + b2Vec2 perp = b2Cross(d, m_radius / d.Length()); + + // Push the capsule segment by that vector + // Must swap if coming from the other side + b2Vec2 boundary[2]; + if (ld < 0) { + boundary[0] = capsule[1] - perp; + boundary[1] = capsule[0] - perp; + } + else { + boundary[0] = capsule[0] + perp; + boundary[1] = capsule[1] + perp; + } + + // Check intersection against the adjusted segments. + if (b2RaycastSegment(output, input, boundary[0], boundary[1])) + return true; + } + + // Consider circular caps + + // Precompute some circle values + b2Vec2 r = input.p2 - input.p1; + float32 rr = r.LengthSquared(); + + // Check for short segment + if (rr < b2_epsilon) + { + return false; + } + + // Check the circle caps, starting with closer + int startingIndex = 0; + if ((capsule[1] - input.p1).LengthSquared() < (capsule[0] - input.p1).LengthSquared()) { + startingIndex = 1; + } + b2Vec2 center = capsule[startingIndex]; + for (int i = 0; i < 2; ++i) + { + b2Vec2 s = input.p1 - center; + float32 b = b2Dot(s, s) - m_radius * m_radius; + + // Should not start inside + b2Assert(!(b < 0.0f)); + + // Solve quadratic equation. + float32 c = b2Dot(s, r); + float32 sigma = c * c - rr * b; + + // Check for negative discriminant. + if (!(sigma < 0.0f)) + { + // Find the point of intersection of the line with the circle. + float32 a = -(c + b2Sqrt(sigma)); + + // Is the intersection point on the segment? + if (0.0f <= a && a <= input.maxFraction * rr) + { + a /= rr; + output->fraction = a; + output->normal = s + a * r; + output->normal.Normalize(); + return true; + } + } + center = capsule[!startingIndex]; + } + + return false; +} + +void b2CapsuleShape::ComputeAABB(b2AABB* aabb, const b2Transform& transform) const +{ + b2Vec2 p1 = transform.position + b2Mul(transform.R, m_vertices[0]); + b2Vec2 p2 = transform.position + b2Mul(transform.R, m_vertices[1]); + b2Vec2 min = b2Min(p1, p2); + b2Vec2 max = b2Max(p1, p2); + aabb->lowerBound.Set(min.x - m_radius, min.y - m_radius); + aabb->upperBound.Set(max.x + m_radius, max.y + m_radius); +} + +void b2CapsuleShape::ComputeMass(b2MassData* massData, float32 density) const +{ + // Area of rectangle + 2 half circles + float32 rectHeight = (m_vertices[1] - m_vertices[0]).Length(); + float32 rectMass = density * rectHeight * m_radius; + float32 circleMass = density * b2_pi * m_radius * m_radius; + massData->mass = rectMass + circleMass; + massData->center = 0.5f * (m_vertices[0] + m_vertices[1]); + + // inertia about the local origin + float32 rectInertia = rectMass * (rectHeight * rectHeight + m_radius * m_radius) / 12.0f; + float32 circleInertia = circleMass * (0.5f * m_radius * m_radius + rectHeight * rectHeight * .25f); + massData->I = rectInertia + circleInertia + massData->mass * massData->center.LengthSquared(); +} Index: Box2D/Box2D/Collision/Shapes/b2CapsuleShape.h =================================================================== --- Box2D/Box2D/Collision/Shapes/b2CapsuleShape.h (revision 0) +++ Box2D/Box2D/Collision/Shapes/b2CapsuleShape.h (revision 0) @@ -0,0 +1,103 @@ +/* +* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef B2_CAPSULE_SHAPE_H +#define B2_CAPSULE_SHAPE_H + +#include + +/// A circle shape. +class b2CapsuleShape : public b2Shape +{ +public: + b2CapsuleShape(); + + /// Set to a capsule across the X axis, with given width between focii. + void SetByExtentsX(float32 radius, float32 width); + /// Set to a capsule across the X axis, rotated and transposed. + void SetByExtentsX(float32 radius, float32 width, + const b2Vec2& center, float32 angle); + + /// Set to a capsule across the Y axis, with given height between focii. + void SetByExtentsY(float32 radius, float32 height); + /// Set to a capsule across the Y axis, rotated and transposed. + void SetByExtentsY(float32 radius, float32 height, + const b2Vec2& center, float32 angle); + + /// Implement b2Shape. + b2Shape* Clone(b2BlockAllocator* allocator) const; + + /// Implement b2Shape. + bool TestPoint(const b2Transform& transform, const b2Vec2& p) const; + + /// Implement b2Shape. + bool RayCast(b2RayCastOutput* output, const b2RayCastInput& input, const b2Transform& transform) const; + + /// @see b2Shape::ComputeAABB + void ComputeAABB(b2AABB* aabb, const b2Transform& transform) const; + + /// @see b2Shape::ComputeMass + void ComputeMass(b2MassData* massData, float32 density) const; + + /// Get the supporting vertex index in the given direction. + int32 GetSupport(const b2Vec2& d) const; + + /// Get the supporting vertex in the given direction. + const b2Vec2& GetSupportVertex(const b2Vec2& d) const; + + /// Get the vertex count. + int32 GetVertexCount() const { return 2; } + + /// Get a vertex by index. Used by b2Distance. + const b2Vec2& GetVertex(int32 index) const; + + /// Position of circle centers + b2Vec2 m_vertices[2]; +}; + +inline b2CapsuleShape::b2CapsuleShape() +{ + m_type = e_capsule; + m_radius = 0.0f; + m_vertices[0].SetZero(); + m_vertices[1].SetZero(); +} + + +inline int32 b2CapsuleShape::GetSupport(const b2Vec2 &d) const +{ + if ((m_vertices[0] - d).LengthSquared() <= (m_vertices[1] - d).LengthSquared()) { + return 0; + } + return 1; +} + +inline const b2Vec2& b2CapsuleShape::GetSupportVertex(const b2Vec2 &d) const +{ + return GetVertex(GetSupport(d)); +} + +inline const b2Vec2& b2CapsuleShape::GetVertex(int32 index) const +{ + b2Assert(index == 0 || index == 1); + return m_vertices[index]; +} + + + +#endif Index: Box2D/Box2D/Collision/Shapes/b2Shape.h =================================================================== --- Box2D/Box2D/Collision/Shapes/b2Shape.h (revision 70) +++ Box2D/Box2D/Collision/Shapes/b2Shape.h (working copy) @@ -48,7 +48,8 @@ e_unknown= -1, e_circle = 0, e_polygon = 1, - e_typeCount = 2, + e_capsule = 2, + e_typeCount = 3, }; b2Shape() { m_type = e_unknown; } Index: Box2D/Box2D/Dynamics/b2Fixture.cpp =================================================================== --- Box2D/Box2D/Dynamics/b2Fixture.cpp (revision 70) +++ Box2D/Box2D/Dynamics/b2Fixture.cpp (working copy) @@ -18,6 +18,7 @@ #include #include +#include #include #include #include @@ -82,6 +83,14 @@ allocator->Free(s, sizeof(b2PolygonShape)); } break; + + case b2Shape::e_capsule: + { + b2CapsuleShape* s = (b2CapsuleShape*)m_shape; + s->~b2CapsuleShape(); + allocator->Free(s, sizeof(b2CapsuleShape)); + } + break; default: b2Assert(false); Index: Box2D/Box2D/Dynamics/b2World.cpp =================================================================== --- Box2D/Box2D/Dynamics/b2World.cpp (revision 70) +++ Box2D/Box2D/Dynamics/b2World.cpp (working copy) @@ -26,6 +26,7 @@ #include #include #include +#include #include #include #include @@ -907,6 +908,18 @@ m_debugDraw->DrawSolidPolygon(vertices, vertexCount, color); } break; + + case b2Shape::e_capsule: + { + b2CapsuleShape* capsule = (b2CapsuleShape*)fixture->GetShape(); + + b2Vec2 p1 = b2Mul(xf, capsule->m_vertices[0]); + b2Vec2 p2 = b2Mul(xf, capsule->m_vertices[1]); + float32 radius = capsule->m_radius; + + m_debugDraw->DrawSolidCapsule(p1, p2, radius, color); + } + break; } } Index: Box2D/Box2D/Dynamics/b2WorldCallbacks.h =================================================================== --- Box2D/Box2D/Dynamics/b2WorldCallbacks.h (revision 70) +++ Box2D/Box2D/Dynamics/b2WorldCallbacks.h (working copy) @@ -206,6 +206,12 @@ /// Draw a solid circle. virtual void DrawSolidCircle(const b2Vec2& center, float32 radius, const b2Vec2& axis, const b2Color& color) = 0; + /// Draw a capsule. + virtual void DrawCapsule(const b2Vec2& p1, const b2Vec2& p2, float32 radius, const b2Color& color) = 0; + + /// Draw a solid capsule. + virtual void DrawSolidCapsule(const b2Vec2& p1, const b2Vec2& p2, float32 radius, const b2Color& color) = 0; + /// Draw a line segment. virtual void DrawSegment(const b2Vec2& p1, const b2Vec2& p2, const b2Color& color) = 0; Index: Box2D/Box2D/Dynamics/Contacts/b2CapsuleAndCircleContact.cpp =================================================================== --- Box2D/Box2D/Dynamics/Contacts/b2CapsuleAndCircleContact.cpp (revision 0) +++ Box2D/Box2D/Dynamics/Contacts/b2CapsuleAndCircleContact.cpp (revision 0) @@ -0,0 +1,63 @@ +/* +* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +b2Contact* b2CapsuleAndCircleContact::Create(b2Fixture* fixtureA, b2Fixture* fixtureB, b2BlockAllocator* allocator) +{ + void* mem = allocator->Allocate(sizeof(b2CapsuleAndCircleContact)); + return new (mem) b2CapsuleAndCircleContact(fixtureA, fixtureB); +} + +void b2CapsuleAndCircleContact::Destroy(b2Contact* contact, b2BlockAllocator* allocator) +{ + ((b2CapsuleAndCircleContact*)contact)->~b2CapsuleAndCircleContact(); + allocator->Free(contact, sizeof(b2CapsuleAndCircleContact)); +} + +b2CapsuleAndCircleContact::b2CapsuleAndCircleContact(b2Fixture* fixtureA, b2Fixture* fixtureB) + : b2Contact(fixtureA, fixtureB) +{ + b2Assert(m_fixtureA->GetType() == b2Shape::e_capsule); + b2Assert(m_fixtureB->GetType() == b2Shape::e_circle); +} + +void b2CapsuleAndCircleContact::Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB) +{ + b2Body* bodyA = m_fixtureA->GetBody(); + b2Body* bodyB = m_fixtureB->GetBody(); + + b2CapsuleShape* capsule = (b2CapsuleShape*)m_fixtureA->GetShape(); + + b2PolygonShape polygon; + polygon.SetAsEdge(capsule->m_vertices[0], capsule->m_vertices[1]); + polygon.m_radius = capsule->m_radius; + + b2CollidePolygonAndCircle( manifold, + &polygon, xfA, + (b2CircleShape*)m_fixtureB->GetShape(), xfB); +} Index: Box2D/Box2D/Dynamics/Contacts/b2CapsuleAndCircleContact.h =================================================================== --- Box2D/Box2D/Dynamics/Contacts/b2CapsuleAndCircleContact.h (revision 0) +++ Box2D/Box2D/Dynamics/Contacts/b2CapsuleAndCircleContact.h (revision 0) @@ -0,0 +1,38 @@ +/* +* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef B2_CAPSULE_AND_CIRCLE_CONTACT_H +#define B2_CAPSULE_AND_CIRCLE_CONTACT_H + +#include + +class b2BlockAllocator; + +class b2CapsuleAndCircleContact : public b2Contact +{ +public: + static b2Contact* Create(b2Fixture* fixtureA, b2Fixture* fixtureB, b2BlockAllocator* allocator); + static void Destroy(b2Contact* contact, b2BlockAllocator* allocator); + + b2CapsuleAndCircleContact(b2Fixture* fixtureA, b2Fixture* fixtureB); + ~b2CapsuleAndCircleContact() {} + + void Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB); +}; + +#endif Index: Box2D/Box2D/Dynamics/Contacts/b2CapsuleContact.cpp =================================================================== --- Box2D/Box2D/Dynamics/Contacts/b2CapsuleContact.cpp (revision 0) +++ Box2D/Box2D/Dynamics/Contacts/b2CapsuleContact.cpp (revision 0) @@ -0,0 +1,66 @@ +/* +* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +b2Contact* b2CapsuleContact::Create(b2Fixture* fixtureA, b2Fixture* fixtureB, b2BlockAllocator* allocator) +{ + void* mem = allocator->Allocate(sizeof(b2CapsuleContact)); + return new (mem) b2CapsuleContact(fixtureA, fixtureB); +} + +void b2CapsuleContact::Destroy(b2Contact* contact, b2BlockAllocator* allocator) +{ + ((b2CapsuleContact*)contact)->~b2CapsuleContact(); + allocator->Free(contact, sizeof(b2CapsuleContact)); +} + +b2CapsuleContact::b2CapsuleContact(b2Fixture* fixtureA, b2Fixture* fixtureB) + : b2Contact(fixtureA, fixtureB) +{ + b2Assert(m_fixtureA->GetType() == b2Shape::e_capsule); + b2Assert(m_fixtureB->GetType() == b2Shape::e_capsule); +} + +void b2CapsuleContact::Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB) +{ + b2Body* bodyA = m_fixtureA->GetBody(); + b2Body* bodyB = m_fixtureB->GetBody(); + + b2CapsuleShape* capsule1 = (b2CapsuleShape*)m_fixtureA->GetShape(); + b2CapsuleShape* capsule2 = (b2CapsuleShape*)m_fixtureB->GetShape(); + + b2PolygonShape polygon1, polygon2; + polygon1.SetAsEdge(capsule1->m_vertices[0], capsule1->m_vertices[1]); + polygon2.SetAsEdge(capsule2->m_vertices[0], capsule2->m_vertices[1]); + polygon1.m_radius = capsule1->m_radius; + polygon2.m_radius = capsule2->m_radius; + + b2CollidePolygons( manifold, + &polygon1, xfA, + &polygon2, xfB); +} Index: Box2D/Box2D/Dynamics/Contacts/b2CapsuleContact.h =================================================================== --- Box2D/Box2D/Dynamics/Contacts/b2CapsuleContact.h (revision 0) +++ Box2D/Box2D/Dynamics/Contacts/b2CapsuleContact.h (revision 0) @@ -0,0 +1,38 @@ +/* +* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef B2_CAPSULE_CONTACT_H +#define B2_CAPSULE_CONTACT_H + +#include + +class b2BlockAllocator; + +class b2CapsuleContact : public b2Contact +{ +public: + static b2Contact* Create(b2Fixture* fixtureA, b2Fixture* fixtureB, b2BlockAllocator* allocator); + static void Destroy(b2Contact* contact, b2BlockAllocator* allocator); + + b2CapsuleContact(b2Fixture* fixtureA, b2Fixture* fixtureB); + ~b2CapsuleContact() {} + + void Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB); +}; + +#endif Index: Box2D/Box2D/Dynamics/Contacts/b2Contact.cpp =================================================================== --- Box2D/Box2D/Dynamics/Contacts/b2Contact.cpp (revision 70) +++ Box2D/Box2D/Dynamics/Contacts/b2Contact.cpp (working copy) @@ -17,7 +17,10 @@ */ #include +#include +#include #include +#include #include #include #include @@ -38,6 +41,10 @@ AddType(b2CircleContact::Create, b2CircleContact::Destroy, b2Shape::e_circle, b2Shape::e_circle); AddType(b2PolygonAndCircleContact::Create, b2PolygonAndCircleContact::Destroy, b2Shape::e_polygon, b2Shape::e_circle); AddType(b2PolygonContact::Create, b2PolygonContact::Destroy, b2Shape::e_polygon, b2Shape::e_polygon); + + AddType(b2CapsuleContact::Create, b2CapsuleContact::Destroy, b2Shape::e_capsule, b2Shape::e_capsule); + AddType(b2CapsuleAndCircleContact::Create, b2CapsuleAndCircleContact::Destroy, b2Shape::e_capsule, b2Shape::e_circle); + AddType(b2PolygonAndCapsuleContact::Create, b2PolygonAndCapsuleContact::Destroy, b2Shape::e_polygon, b2Shape::e_capsule); } void b2Contact::AddType(b2ContactCreateFcn* createFcn, b2ContactDestroyFcn* destoryFcn, Index: Box2D/Box2D/Dynamics/Contacts/b2PolygonAndCapsuleContact.cpp =================================================================== --- Box2D/Box2D/Dynamics/Contacts/b2PolygonAndCapsuleContact.cpp (revision 0) +++ Box2D/Box2D/Dynamics/Contacts/b2PolygonAndCapsuleContact.cpp (revision 0) @@ -0,0 +1,63 @@ +/* +* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +b2Contact* b2PolygonAndCapsuleContact::Create(b2Fixture* fixtureA, b2Fixture* fixtureB, b2BlockAllocator* allocator) +{ + void* mem = allocator->Allocate(sizeof(b2PolygonAndCapsuleContact)); + return new (mem) b2PolygonAndCapsuleContact(fixtureA, fixtureB); +} + +void b2PolygonAndCapsuleContact::Destroy(b2Contact* contact, b2BlockAllocator* allocator) +{ + ((b2PolygonAndCapsuleContact*)contact)->~b2PolygonAndCapsuleContact(); + allocator->Free(contact, sizeof(b2PolygonAndCapsuleContact)); +} + +b2PolygonAndCapsuleContact::b2PolygonAndCapsuleContact(b2Fixture* fixtureA, b2Fixture* fixtureB) + : b2Contact(fixtureA, fixtureB) +{ + b2Assert(m_fixtureA->GetType() == b2Shape::e_polygon); + b2Assert(m_fixtureB->GetType() == b2Shape::e_capsule); +} + +void b2PolygonAndCapsuleContact::Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB) +{ + b2Body* bodyA = m_fixtureA->GetBody(); + b2Body* bodyB = m_fixtureB->GetBody(); + + b2CapsuleShape* capsule = (b2CapsuleShape*)m_fixtureB->GetShape(); + + b2PolygonShape polygon; + polygon.SetAsEdge(capsule->m_vertices[0], capsule->m_vertices[1]); + polygon.m_radius = capsule->m_radius; + + b2CollidePolygons( manifold, + (b2PolygonShape*)m_fixtureA->GetShape(), xfA, + &polygon, xfB); +} Index: Box2D/Box2D/Dynamics/Contacts/b2PolygonAndCapsuleContact.h =================================================================== --- Box2D/Box2D/Dynamics/Contacts/b2PolygonAndCapsuleContact.h (revision 0) +++ Box2D/Box2D/Dynamics/Contacts/b2PolygonAndCapsuleContact.h (revision 0) @@ -0,0 +1,38 @@ +/* +* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef B2_POLYGON_AND_CAPSULE_CONTACT_H +#define B2_POLYGON_AND_CAPSULE_CONTACT_H + +#include + +class b2BlockAllocator; + +class b2PolygonAndCapsuleContact : public b2Contact +{ +public: + static b2Contact* Create(b2Fixture* fixtureA, b2Fixture* fixtureB, b2BlockAllocator* allocator); + static void Destroy(b2Contact* contact, b2BlockAllocator* allocator); + + b2PolygonAndCapsuleContact(b2Fixture* fixtureA, b2Fixture* fixtureB); + ~b2PolygonAndCapsuleContact() {} + + void Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB); +}; + +#endif Index: Box2D/Testbed/Framework/Render.cpp =================================================================== --- Box2D/Testbed/Framework/Render.cpp (revision 70) +++ Box2D/Testbed/Framework/Render.cpp (working copy) @@ -110,6 +110,83 @@ glEnd(); } +void DebugDraw::DrawCapsule(const b2Vec2& p1, const b2Vec2& p2, float32 radius, const b2Color& color) +{ + b2Vec2 displacement = p2 - p1; + displacement.Normalize(); + float32 startingAngle = atan2f(displacement.y, displacement.x) + b2_pi / 2; + + const float32 k_segments = 8.0f; + const float32 k_increment = b2_pi / k_segments; + float32 theta = startingAngle; + glColor3f(color.r, color.g, color.b); + glBegin(GL_LINE_LOOP); + for (int32 i = 0; i < k_segments; ++i) + { + b2Vec2 v = p1 + radius * b2Vec2(cosf(theta), sinf(theta)); + glVertex2f(v.x, v.y); + theta += k_increment; + } + for (int32 i = 0; i < k_segments; ++i) + { + b2Vec2 v = p2 + radius * b2Vec2(cosf(theta), sinf(theta)); + glVertex2f(v.x, v.y); + theta += k_increment; + } + glEnd(); +} + +void DebugDraw::DrawSolidCapsule(const b2Vec2& p1, const b2Vec2& p2, float32 radius, const b2Color& color) +{ + b2Vec2 displacement = p2 - p1; + displacement.Normalize(); + float32 startingAngle = atan2f(displacement.y, displacement.x) + b2_pi / 2; + + const float32 k_segments = 8.0f; + const float32 k_increment = b2_pi / k_segments; + float32 theta = startingAngle; + glEnable(GL_BLEND); + glBlendFunc (GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA); + glColor4f(0.5f * color.r, 0.5f * color.g, 0.5f * color.b, 0.5f); + glBegin(GL_TRIANGLE_FAN); + for (int32 i = 0; i < k_segments; ++i) + { + b2Vec2 v = p1 + radius * b2Vec2(cosf(theta), sinf(theta)); + glVertex2f(v.x, v.y); + theta += k_increment; + } + for (int32 i = 0; i < k_segments; ++i) + { + b2Vec2 v = p2 + radius * b2Vec2(cosf(theta), sinf(theta)); + glVertex2f(v.x, v.y); + theta += k_increment; + } + glEnd(); + glDisable(GL_BLEND); + + theta = startingAngle; + glColor4f(color.r, color.g, color.b, 1.0f); + glBegin(GL_LINE_LOOP); + for (int32 i = 0; i < k_segments; ++i) + { + b2Vec2 v = p1 + radius * b2Vec2(cosf(theta), sinf(theta)); + glVertex2f(v.x, v.y); + theta += k_increment; + } + for (int32 i = 0; i < k_segments; ++i) + { + b2Vec2 v = p2 + radius * b2Vec2(cosf(theta), sinf(theta)); + glVertex2f(v.x, v.y); + theta += k_increment; + } + glEnd(); + + glBegin(GL_LINES); + glVertex2f(p1.x, p1.y); + glVertex2f(p2.x, p2.y); + glEnd(); +} + void DebugDraw::DrawSegment(const b2Vec2& p1, const b2Vec2& p2, const b2Color& color) { glColor3f(color.r, color.g, color.b); Index: Box2D/Testbed/Framework/Render.h =================================================================== --- Box2D/Testbed/Framework/Render.h (revision 70) +++ Box2D/Testbed/Framework/Render.h (working copy) @@ -36,6 +36,10 @@ void DrawSolidCircle(const b2Vec2& center, float32 radius, const b2Vec2& axis, const b2Color& color); + void DrawCapsule(const b2Vec2& p1, const b2Vec2& p2, float32 radius, const b2Color& color); + + void DrawSolidCapsule(const b2Vec2& p1, const b2Vec2& p2, float32 radius, const b2Color& color); + void DrawSegment(const b2Vec2& p1, const b2Vec2& p2, const b2Color& color); void DrawTransform(const b2Transform& xf); Index: Box2D/Testbed/Tests/PolyShapes.h =================================================================== --- Box2D/Testbed/Tests/PolyShapes.h (revision 70) +++ Box2D/Testbed/Tests/PolyShapes.h (working copy) @@ -74,6 +74,19 @@ m_debugDraw->DrawPolygon(vertices, vertexCount, color); } break; + + case b2Shape::e_capsule: + { + b2CapsuleShape* capsule = (b2CapsuleShape*)fixture->GetShape(); + + b2Vec2 p1 = b2Mul(xf, capsule->m_vertices[0]); + b2Vec2 p2 = b2Mul(xf, capsule->m_vertices[1]); + float32 radius = capsule->m_radius; + + m_debugDraw->DrawCapsule(p1, p2, radius, color); + } + break; + } } @@ -163,6 +176,14 @@ m_circle.m_radius = 0.5f; } + { + m_capsules[0].SetByExtentsX(0.5f, 0.5f); + } + + { + m_capsules[1].SetByExtentsY(0.1f, 1.8f, b2Vec2(0.0f, 1.0f), 0.0f); + } + m_bodyIndex = 0; memset(m_bodies, 0, sizeof(m_bodies)); } @@ -197,7 +218,7 @@ fd.friction = 0.3f; m_bodies[m_bodyIndex]->CreateFixture(&fd); } - else + else if (index == 4) { b2FixtureDef fd; fd.shape = &m_circle; @@ -206,6 +227,15 @@ m_bodies[m_bodyIndex]->CreateFixture(&fd); } + else + { + b2FixtureDef fd; + fd.shape = m_capsules + (index - 5); + fd.density = 1.0f; + fd.friction = 0.3f; + + m_bodies[m_bodyIndex]->CreateFixture(&fd); + } m_bodyIndex = (m_bodyIndex + 1) % k_maxBodies; } @@ -232,6 +262,8 @@ case '3': case '4': case '5': + case '6': + case '7': Create(key - '1'); break; @@ -270,7 +302,7 @@ b2Color color(0.4f, 0.7f, 0.8f); m_debugDraw.DrawCircle(callback.m_circle.m_p, callback.m_circle.m_radius, color); - m_debugDraw.DrawString(5, m_textLine, "Press 1-5 to drop stuff"); + m_debugDraw.DrawString(5, m_textLine, "Press 1-7 to drop stuff"); m_textLine += 15; m_debugDraw.DrawString(5, m_textLine, "Press 'a' to (de)activate some bodies"); m_textLine += 15; @@ -287,6 +319,7 @@ b2Body* m_bodies[k_maxBodies]; b2PolygonShape m_polygons[4]; b2CircleShape m_circle; + b2CapsuleShape m_capsules[2]; }; #endif Index: Box2D/Testbed/Tests/RayCast.h =================================================================== --- Box2D/Testbed/Tests/RayCast.h (revision 70) +++ Box2D/Testbed/Tests/RayCast.h (working copy) @@ -211,6 +211,14 @@ m_circle.m_radius = 0.5f; } + { + m_capsules[0].SetByExtentsX(0.5f, 0.5f); + } + + { + m_capsules[1].SetByExtentsY(0.1f, 1.8f, b2Vec2(0.0f, 1.0f), 0.0f); + } + m_bodyIndex = 0; memset(m_bodies, 0, sizeof(m_bodies)); @@ -251,7 +259,7 @@ fd.friction = 0.3f; m_bodies[m_bodyIndex]->CreateFixture(&fd); } - else + else if (index == 4) { b2FixtureDef fd; fd.shape = &m_circle; @@ -259,6 +267,14 @@ m_bodies[m_bodyIndex]->CreateFixture(&fd); } + else + { + b2FixtureDef fd; + fd.shape = m_capsules + (index - 5); + fd.friction = 0.3f; + + m_bodies[m_bodyIndex]->CreateFixture(&fd); + } m_bodyIndex = (m_bodyIndex + 1) % e_maxBodies; } @@ -285,6 +301,8 @@ case '3': case '4': case '5': + case '6': + case '7': Create(key - '1'); break; @@ -311,7 +329,7 @@ void Step(Settings* settings) { Test::Step(settings); - m_debugDraw.DrawString(5, m_textLine, "Press 1-5 to drop stuff, m to change the mode"); + m_debugDraw.DrawString(5, m_textLine, "Press 1-7 to drop stuff, m to change the mode"); m_textLine += 15; m_debugDraw.DrawString(5, m_textLine, "Mode = %d", m_mode); m_textLine += 15; @@ -385,6 +403,7 @@ int32 m_userData[e_maxBodies]; b2PolygonShape m_polygons[4]; b2CircleShape m_circle; + b2CapsuleShape m_capsules[2]; float32 m_angle;