diff options
Diffstat (limited to 'Box2D/Collision/Shapes')
| -rw-r--r-- | Box2D/Collision/Shapes/b2ChainShape.cpp | 171 | ||||
| -rw-r--r-- | Box2D/Collision/Shapes/b2ChainShape.h | 102 | ||||
| -rw-r--r-- | Box2D/Collision/Shapes/b2CircleShape.cpp | 100 | ||||
| -rw-r--r-- | Box2D/Collision/Shapes/b2CircleShape.h | 91 | ||||
| -rw-r--r-- | Box2D/Collision/Shapes/b2EdgeShape.cpp | 139 | ||||
| -rw-r--r-- | Box2D/Collision/Shapes/b2EdgeShape.h | 74 | ||||
| -rw-r--r-- | Box2D/Collision/Shapes/b2PolygonShape.cpp | 361 | ||||
| -rw-r--r-- | Box2D/Collision/Shapes/b2PolygonShape.h | 95 | ||||
| -rw-r--r-- | Box2D/Collision/Shapes/b2Shape.h | 101 |
9 files changed, 1234 insertions, 0 deletions
diff --git a/Box2D/Collision/Shapes/b2ChainShape.cpp b/Box2D/Collision/Shapes/b2ChainShape.cpp new file mode 100644 index 0000000..bd23346 --- /dev/null +++ b/Box2D/Collision/Shapes/b2ChainShape.cpp @@ -0,0 +1,171 @@ +/* +* Copyright (c) 2006-2010 Erin Catto http://www.box2d.org +* +* 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 <Box2D/Collision/Shapes/b2ChainShape.h> +#include <Box2D/Collision/Shapes/b2EdgeShape.h> +#include <new> +#include <cstring> +using namespace std; + +b2ChainShape::~b2ChainShape() +{ + b2Free(m_vertices); + m_vertices = NULL; + m_count = 0; +} + +void b2ChainShape::CreateLoop(const b2Vec2* vertices, int32 count) +{ + b2Assert(m_vertices == NULL && m_count == 0); + b2Assert(count >= 3); + m_count = count + 1; + m_vertices = (b2Vec2*)b2Alloc(m_count * sizeof(b2Vec2)); + memcpy(m_vertices, vertices, count * sizeof(b2Vec2)); + m_vertices[count] = m_vertices[0]; + m_prevVertex = m_vertices[m_count - 2]; + m_nextVertex = m_vertices[1]; + m_hasPrevVertex = true; + m_hasNextVertex = true; +} + +void b2ChainShape::CreateChain(const b2Vec2* vertices, int32 count) +{ + b2Assert(m_vertices == NULL && m_count == 0); + b2Assert(count >= 2); + m_count = count; + m_vertices = (b2Vec2*)b2Alloc(count * sizeof(b2Vec2)); + memcpy(m_vertices, vertices, m_count * sizeof(b2Vec2)); + m_hasPrevVertex = false; + m_hasNextVertex = false; +} + +void b2ChainShape::SetPrevVertex(const b2Vec2& prevVertex) +{ + m_prevVertex = prevVertex; + m_hasPrevVertex = true; +} + +void b2ChainShape::SetNextVertex(const b2Vec2& nextVertex) +{ + m_nextVertex = nextVertex; + m_hasNextVertex = true; +} + +b2Shape* b2ChainShape::Clone(b2BlockAllocator* allocator) const +{ + void* mem = allocator->Allocate(sizeof(b2ChainShape)); + b2ChainShape* clone = new (mem) b2ChainShape; + clone->CreateChain(m_vertices, m_count); + clone->m_prevVertex = m_prevVertex; + clone->m_nextVertex = m_nextVertex; + clone->m_hasPrevVertex = m_hasPrevVertex; + clone->m_hasNextVertex = m_hasNextVertex; + return clone; +} + +int32 b2ChainShape::GetChildCount() const +{ + // edge count = vertex count - 1 + return m_count - 1; +} + +void b2ChainShape::GetChildEdge(b2EdgeShape* edge, int32 index) const +{ + b2Assert(0 <= index && index < m_count - 1); + edge->m_type = b2Shape::e_edge; + edge->m_radius = m_radius; + + edge->m_vertex1 = m_vertices[index + 0]; + edge->m_vertex2 = m_vertices[index + 1]; + + if (index > 0) + { + edge->m_vertex0 = m_vertices[index - 1]; + edge->m_hasVertex0 = true; + } + else + { + edge->m_vertex0 = m_prevVertex; + edge->m_hasVertex0 = m_hasPrevVertex; + } + + if (index < m_count - 2) + { + edge->m_vertex3 = m_vertices[index + 2]; + edge->m_hasVertex3 = true; + } + else + { + edge->m_vertex3 = m_nextVertex; + edge->m_hasVertex3 = m_hasNextVertex; + } +} + +bool b2ChainShape::TestPoint(const b2Transform& xf, const b2Vec2& p) const +{ + B2_NOT_USED(xf); + B2_NOT_USED(p); + return false; +} + +bool b2ChainShape::RayCast(b2RayCastOutput* output, const b2RayCastInput& input, + const b2Transform& xf, int32 childIndex) const +{ + b2Assert(childIndex < m_count); + + b2EdgeShape edgeShape; + + int32 i1 = childIndex; + int32 i2 = childIndex + 1; + if (i2 == m_count) + { + i2 = 0; + } + + edgeShape.m_vertex1 = m_vertices[i1]; + edgeShape.m_vertex2 = m_vertices[i2]; + + return edgeShape.RayCast(output, input, xf, 0); +} + +void b2ChainShape::ComputeAABB(b2AABB* aabb, const b2Transform& xf, int32 childIndex) const +{ + b2Assert(childIndex < m_count); + + int32 i1 = childIndex; + int32 i2 = childIndex + 1; + if (i2 == m_count) + { + i2 = 0; + } + + b2Vec2 v1 = b2Mul(xf, m_vertices[i1]); + b2Vec2 v2 = b2Mul(xf, m_vertices[i2]); + + aabb->lowerBound = b2Min(v1, v2); + aabb->upperBound = b2Max(v1, v2); +} + +void b2ChainShape::ComputeMass(b2MassData* massData, float32 density) const +{ + B2_NOT_USED(density); + + massData->mass = 0.0f; + massData->center.SetZero(); + massData->I = 0.0f; +} diff --git a/Box2D/Collision/Shapes/b2ChainShape.h b/Box2D/Collision/Shapes/b2ChainShape.h new file mode 100644 index 0000000..d7c5b29 --- /dev/null +++ b/Box2D/Collision/Shapes/b2ChainShape.h @@ -0,0 +1,102 @@ +/* +* Copyright (c) 2006-2010 Erin Catto http://www.box2d.org +* +* 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_CHAIN_SHAPE_H +#define B2_CHAIN_SHAPE_H + +#include <Box2D/Collision/Shapes/b2Shape.h> + +class b2EdgeShape; + +/// A chain shape is a free form sequence of line segments. +/// The chain has two-sided collision, so you can use inside and outside collision. +/// Therefore, you may use any winding order. +/// Since there may be many vertices, they are allocated using b2Alloc. +/// Connectivity information is used to create smooth collisions. +/// WARNING: The chain will not collide properly if there are self-intersections. +class b2ChainShape : public b2Shape +{ +public: + b2ChainShape(); + + /// The destructor frees the vertices using b2Free. + ~b2ChainShape(); + + /// Create a loop. This automatically adjusts connectivity. + /// @param vertices an array of vertices, these are copied + /// @param count the vertex count + void CreateLoop(const b2Vec2* vertices, int32 count); + + /// Create a chain with isolated end vertices. + /// @param vertices an array of vertices, these are copied + /// @param count the vertex count + void CreateChain(const b2Vec2* vertices, int32 count); + + /// Establish connectivity to a vertex that precedes the first vertex. + /// Don't call this for loops. + void SetPrevVertex(const b2Vec2& prevVertex); + + /// Establish connectivity to a vertex that follows the last vertex. + /// Don't call this for loops. + void SetNextVertex(const b2Vec2& nextVertex); + + /// Implement b2Shape. Vertices are cloned using b2Alloc. + b2Shape* Clone(b2BlockAllocator* allocator) const; + + /// @see b2Shape::GetChildCount + int32 GetChildCount() const; + + /// Get a child edge. + void GetChildEdge(b2EdgeShape* edge, int32 index) const; + + /// This always return false. + /// @see b2Shape::TestPoint + bool TestPoint(const b2Transform& transform, const b2Vec2& p) const; + + /// Implement b2Shape. + bool RayCast(b2RayCastOutput* output, const b2RayCastInput& input, + const b2Transform& transform, int32 childIndex) const; + + /// @see b2Shape::ComputeAABB + void ComputeAABB(b2AABB* aabb, const b2Transform& transform, int32 childIndex) const; + + /// Chains have zero mass. + /// @see b2Shape::ComputeMass + void ComputeMass(b2MassData* massData, float32 density) const; + + /// The vertices. Owned by this class. + b2Vec2* m_vertices; + + /// The vertex count. + int32 m_count; + + b2Vec2 m_prevVertex, m_nextVertex; + bool m_hasPrevVertex, m_hasNextVertex; +}; + +inline b2ChainShape::b2ChainShape() +{ + m_type = e_chain; + m_radius = b2_polygonRadius; + m_vertices = NULL; + m_count = 0; + m_hasPrevVertex = NULL; + m_hasNextVertex = NULL; +} + +#endif diff --git a/Box2D/Collision/Shapes/b2CircleShape.cpp b/Box2D/Collision/Shapes/b2CircleShape.cpp new file mode 100644 index 0000000..d494b13 --- /dev/null +++ b/Box2D/Collision/Shapes/b2CircleShape.cpp @@ -0,0 +1,100 @@ +/* +* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org +* +* 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 <Box2D/Collision/Shapes/b2CircleShape.h> +#include <new> +using namespace std; + +b2Shape* b2CircleShape::Clone(b2BlockAllocator* allocator) const +{ + void* mem = allocator->Allocate(sizeof(b2CircleShape)); + b2CircleShape* clone = new (mem) b2CircleShape; + *clone = *this; + return clone; +} + +int32 b2CircleShape::GetChildCount() const +{ + return 1; +} + +bool b2CircleShape::TestPoint(const b2Transform& transform, const b2Vec2& p) const +{ + b2Vec2 center = transform.p + b2Mul(transform.q, m_p); + b2Vec2 d = p - center; + return b2Dot(d, d) <= m_radius * m_radius; +} + +// Collision Detection in Interactive 3D Environments by Gino van den Bergen +// From Section 3.1.2 +// x = s + a * r +// norm(x) = radius +bool b2CircleShape::RayCast(b2RayCastOutput* output, const b2RayCastInput& input, + const b2Transform& transform, int32 childIndex) const +{ + B2_NOT_USED(childIndex); + + b2Vec2 position = transform.p + b2Mul(transform.q, m_p); + b2Vec2 s = input.p1 - position; + float32 b = b2Dot(s, s) - m_radius * m_radius; + + // Solve quadratic equation. + b2Vec2 r = input.p2 - input.p1; + float32 c = b2Dot(s, r); + float32 rr = b2Dot(r, r); + float32 sigma = c * c - rr * b; + + // Check for negative discriminant and short segment. + if (sigma < 0.0f || rr < b2_epsilon) + { + return false; + } + + // 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; + } + + return false; +} + +void b2CircleShape::ComputeAABB(b2AABB* aabb, const b2Transform& transform, int32 childIndex) const +{ + B2_NOT_USED(childIndex); + + b2Vec2 p = transform.p + b2Mul(transform.q, m_p); + aabb->lowerBound.Set(p.x - m_radius, p.y - m_radius); + aabb->upperBound.Set(p.x + m_radius, p.y + m_radius); +} + +void b2CircleShape::ComputeMass(b2MassData* massData, float32 density) const +{ + massData->mass = density * b2_pi * m_radius * m_radius; + massData->center = m_p; + + // inertia about the local origin + massData->I = massData->mass * (0.5f * m_radius * m_radius + b2Dot(m_p, m_p)); +} diff --git a/Box2D/Collision/Shapes/b2CircleShape.h b/Box2D/Collision/Shapes/b2CircleShape.h new file mode 100644 index 0000000..3209f92 --- /dev/null +++ b/Box2D/Collision/Shapes/b2CircleShape.h @@ -0,0 +1,91 @@ +/* +* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org +* +* 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_CIRCLE_SHAPE_H +#define B2_CIRCLE_SHAPE_H + +#include <Box2D/Collision/Shapes/b2Shape.h> + +/// A circle shape. +class b2CircleShape : public b2Shape +{ +public: + b2CircleShape(); + + /// Implement b2Shape. + b2Shape* Clone(b2BlockAllocator* allocator) const; + + /// @see b2Shape::GetChildCount + int32 GetChildCount() const; + + /// Implement b2Shape. + bool TestPoint(const b2Transform& transform, const b2Vec2& p) const; + + /// Implement b2Shape. + bool RayCast(b2RayCastOutput* output, const b2RayCastInput& input, + const b2Transform& transform, int32 childIndex) const; + + /// @see b2Shape::ComputeAABB + void ComputeAABB(b2AABB* aabb, const b2Transform& transform, int32 childIndex) 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 1; } + + /// Get a vertex by index. Used by b2Distance. + const b2Vec2& GetVertex(int32 index) const; + + /// Position + b2Vec2 m_p; +}; + +inline b2CircleShape::b2CircleShape() +{ + m_type = e_circle; + m_radius = 0.0f; + m_p.SetZero(); +} + +inline int32 b2CircleShape::GetSupport(const b2Vec2 &d) const +{ + B2_NOT_USED(d); + return 0; +} + +inline const b2Vec2& b2CircleShape::GetSupportVertex(const b2Vec2 &d) const +{ + B2_NOT_USED(d); + return m_p; +} + +inline const b2Vec2& b2CircleShape::GetVertex(int32 index) const +{ + B2_NOT_USED(index); + b2Assert(index == 0); + return m_p; +} + +#endif diff --git a/Box2D/Collision/Shapes/b2EdgeShape.cpp b/Box2D/Collision/Shapes/b2EdgeShape.cpp new file mode 100644 index 0000000..cbac41c --- /dev/null +++ b/Box2D/Collision/Shapes/b2EdgeShape.cpp @@ -0,0 +1,139 @@ +/* +* Copyright (c) 2006-2010 Erin Catto http://www.box2d.org +* +* 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 <Box2D/Collision/Shapes/b2EdgeShape.h> +#include <new> +using namespace std; + +void b2EdgeShape::Set(const b2Vec2& v1, const b2Vec2& v2) +{ + m_vertex1 = v1; + m_vertex2 = v2; + m_hasVertex0 = false; + m_hasVertex3 = false; +} + +b2Shape* b2EdgeShape::Clone(b2BlockAllocator* allocator) const +{ + void* mem = allocator->Allocate(sizeof(b2EdgeShape)); + b2EdgeShape* clone = new (mem) b2EdgeShape; + *clone = *this; + return clone; +} + +int32 b2EdgeShape::GetChildCount() const +{ + return 1; +} + +bool b2EdgeShape::TestPoint(const b2Transform& xf, const b2Vec2& p) const +{ + B2_NOT_USED(xf); + B2_NOT_USED(p); + return false; +} + +// p = p1 + t * d +// v = v1 + s * e +// p1 + t * d = v1 + s * e +// s * e - t * d = p1 - v1 +bool b2EdgeShape::RayCast(b2RayCastOutput* output, const b2RayCastInput& input, + const b2Transform& xf, int32 childIndex) const +{ + B2_NOT_USED(childIndex); + + // Put the ray into the edge's frame of reference. + b2Vec2 p1 = b2MulT(xf.q, input.p1 - xf.p); + b2Vec2 p2 = b2MulT(xf.q, input.p2 - xf.p); + b2Vec2 d = p2 - p1; + + b2Vec2 v1 = m_vertex1; + b2Vec2 v2 = m_vertex2; + b2Vec2 e = v2 - v1; + b2Vec2 normal(e.y, -e.x); + normal.Normalize(); + + // q = p1 + t * d + // dot(normal, q - v1) = 0 + // dot(normal, p1 - v1) + t * dot(normal, d) = 0 + float32 numerator = b2Dot(normal, v1 - p1); + float32 denominator = b2Dot(normal, d); + + if (denominator == 0.0f) + { + return false; + } + + float32 t = numerator / denominator; + if (t < 0.0f || input.maxFraction < t) + { + return false; + } + + b2Vec2 q = p1 + t * d; + + // q = v1 + s * r + // s = dot(q - v1, r) / dot(r, r) + b2Vec2 r = v2 - v1; + float32 rr = b2Dot(r, r); + if (rr == 0.0f) + { + return false; + } + + float32 s = b2Dot(q - v1, r) / rr; + if (s < 0.0f || 1.0f < s) + { + return false; + } + + output->fraction = t; + if (numerator > 0.0f) + { + output->normal = -normal; + } + else + { + output->normal = normal; + } + return true; +} + +void b2EdgeShape::ComputeAABB(b2AABB* aabb, const b2Transform& xf, int32 childIndex) const +{ + B2_NOT_USED(childIndex); + + b2Vec2 v1 = b2Mul(xf, m_vertex1); + b2Vec2 v2 = b2Mul(xf, m_vertex2); + + b2Vec2 lower = b2Min(v1, v2); + b2Vec2 upper = b2Max(v1, v2); + + b2Vec2 r(m_radius, m_radius); + aabb->lowerBound = lower - r; + aabb->upperBound = upper + r; +} + +void b2EdgeShape::ComputeMass(b2MassData* massData, float32 density) const +{ + B2_NOT_USED(density); + + massData->mass = 0.0f; + massData->center = 0.5f * (m_vertex1 + m_vertex2); + massData->I = 0.0f; +} diff --git a/Box2D/Collision/Shapes/b2EdgeShape.h b/Box2D/Collision/Shapes/b2EdgeShape.h new file mode 100644 index 0000000..0acef85 --- /dev/null +++ b/Box2D/Collision/Shapes/b2EdgeShape.h @@ -0,0 +1,74 @@ +/* +* Copyright (c) 2006-2010 Erin Catto http://www.box2d.org +* +* 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_EDGE_SHAPE_H +#define B2_EDGE_SHAPE_H + +#include <Box2D/Collision/Shapes/b2Shape.h> + +/// A line segment (edge) shape. These can be connected in chains or loops +/// to other edge shapes. The connectivity information is used to ensure +/// correct contact normals. +class b2EdgeShape : public b2Shape +{ +public: + b2EdgeShape(); + + /// Set this as an isolated edge. + void Set(const b2Vec2& v1, const b2Vec2& v2); + + /// Implement b2Shape. + b2Shape* Clone(b2BlockAllocator* allocator) const; + + /// @see b2Shape::GetChildCount + int32 GetChildCount() const; + + /// @see b2Shape::TestPoint + bool TestPoint(const b2Transform& transform, const b2Vec2& p) const; + + /// Implement b2Shape. + bool RayCast(b2RayCastOutput* output, const b2RayCastInput& input, + const b2Transform& transform, int32 childIndex) const; + + /// @see b2Shape::ComputeAABB + void ComputeAABB(b2AABB* aabb, const b2Transform& transform, int32 childIndex) const; + + /// @see b2Shape::ComputeMass + void ComputeMass(b2MassData* massData, float32 density) const; + + /// These are the edge vertices + b2Vec2 m_vertex1, m_vertex2; + + /// Optional adjacent vertices. These are used for smooth collision. + b2Vec2 m_vertex0, m_vertex3; + bool m_hasVertex0, m_hasVertex3; +}; + +inline b2EdgeShape::b2EdgeShape() +{ + m_type = e_edge; + m_radius = b2_polygonRadius; + m_vertex0.x = 0.0f; + m_vertex0.y = 0.0f; + m_vertex3.x = 0.0f; + m_vertex3.y = 0.0f; + m_hasVertex0 = false; + m_hasVertex3 = false; +} + +#endif diff --git a/Box2D/Collision/Shapes/b2PolygonShape.cpp b/Box2D/Collision/Shapes/b2PolygonShape.cpp new file mode 100644 index 0000000..499e168 --- /dev/null +++ b/Box2D/Collision/Shapes/b2PolygonShape.cpp @@ -0,0 +1,361 @@ +/* +* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org +* +* 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 <Box2D/Collision/Shapes/b2PolygonShape.h> +#include <new> + +b2Shape* b2PolygonShape::Clone(b2BlockAllocator* allocator) const +{ + void* mem = allocator->Allocate(sizeof(b2PolygonShape)); + b2PolygonShape* clone = new (mem) b2PolygonShape; + *clone = *this; + return clone; +} + +void b2PolygonShape::SetAsBox(float32 hx, float32 hy) +{ + m_vertexCount = 4; + m_vertices[0].Set(-hx, -hy); + m_vertices[1].Set( hx, -hy); + m_vertices[2].Set( hx, hy); + m_vertices[3].Set(-hx, hy); + m_normals[0].Set(0.0f, -1.0f); + m_normals[1].Set(1.0f, 0.0f); + m_normals[2].Set(0.0f, 1.0f); + m_normals[3].Set(-1.0f, 0.0f); + m_centroid.SetZero(); +} + +void b2PolygonShape::SetAsBox(float32 hx, float32 hy, const b2Vec2& center, float32 angle) +{ + m_vertexCount = 4; + m_vertices[0].Set(-hx, -hy); + m_vertices[1].Set( hx, -hy); + m_vertices[2].Set( hx, hy); + m_vertices[3].Set(-hx, hy); + m_normals[0].Set(0.0f, -1.0f); + m_normals[1].Set(1.0f, 0.0f); + m_normals[2].Set(0.0f, 1.0f); + m_normals[3].Set(-1.0f, 0.0f); + m_centroid = center; + + b2Transform xf; + xf.p = center; + xf.q.Set(angle); + + // Transform vertices and normals. + for (int32 i = 0; i < m_vertexCount; ++i) + { + m_vertices[i] = b2Mul(xf, m_vertices[i]); + m_normals[i] = b2Mul(xf.q, m_normals[i]); + } +} + +int32 b2PolygonShape::GetChildCount() const +{ + return 1; +} + +static b2Vec2 ComputeCentroid(const b2Vec2* vs, int32 count) +{ + b2Assert(count >= 3); + + b2Vec2 c; c.Set(0.0f, 0.0f); + float32 area = 0.0f; + + // pRef is the reference point for forming triangles. + // It's location doesn't change the result (except for rounding error). + b2Vec2 pRef(0.0f, 0.0f); +#if 0 + // This code would put the reference point inside the polygon. + for (int32 i = 0; i < count; ++i) + { + pRef += vs[i]; + } + pRef *= 1.0f / count; +#endif + + const float32 inv3 = 1.0f / 3.0f; + + for (int32 i = 0; i < count; ++i) + { + // Triangle vertices. + b2Vec2 p1 = pRef; + b2Vec2 p2 = vs[i]; + b2Vec2 p3 = i + 1 < count ? vs[i+1] : vs[0]; + + b2Vec2 e1 = p2 - p1; + b2Vec2 e2 = p3 - p1; + + float32 D = b2Cross(e1, e2); + + float32 triangleArea = 0.5f * D; + area += triangleArea; + + // Area weighted centroid + c += triangleArea * inv3 * (p1 + p2 + p3); + } + + // Centroid + b2Assert(area > b2_epsilon); + c *= 1.0f / area; + return c; +} + +void b2PolygonShape::Set(const b2Vec2* vertices, int32 count) +{ + b2Assert(3 <= count && count <= b2_maxPolygonVertices); + m_vertexCount = count; + + // Copy vertices. + for (int32 i = 0; i < m_vertexCount; ++i) + { + m_vertices[i] = vertices[i]; + } + + // Compute normals. Ensure the edges have non-zero length. + for (int32 i = 0; i < m_vertexCount; ++i) + { + int32 i1 = i; + int32 i2 = i + 1 < m_vertexCount ? i + 1 : 0; + b2Vec2 edge = m_vertices[i2] - m_vertices[i1]; + b2Assert(edge.LengthSquared() > b2_epsilon * b2_epsilon); + m_normals[i] = b2Cross(edge, 1.0f); + m_normals[i].Normalize(); + } + +#ifdef _DEBUG + // Ensure the polygon is convex and the interior + // is to the left of each edge. + for (int32 i = 0; i < m_vertexCount; ++i) + { + int32 i1 = i; + int32 i2 = i + 1 < m_vertexCount ? i + 1 : 0; + b2Vec2 edge = m_vertices[i2] - m_vertices[i1]; + + for (int32 j = 0; j < m_vertexCount; ++j) + { + // Don't check vertices on the current edge. + if (j == i1 || j == i2) + { + continue; + } + + b2Vec2 r = m_vertices[j] - m_vertices[i1]; + + // If this crashes, your polygon is non-convex, has colinear edges, + // or the winding order is wrong. + float32 s = b2Cross(edge, r); + b2Assert(s > 0.0f && "ERROR: Please ensure your polygon is convex and has a CCW winding order"); + } + } +#endif + + // Compute the polygon centroid. + m_centroid = ComputeCentroid(m_vertices, m_vertexCount); +} + +bool b2PolygonShape::TestPoint(const b2Transform& xf, const b2Vec2& p) const +{ + b2Vec2 pLocal = b2MulT(xf.q, p - xf.p); + + for (int32 i = 0; i < m_vertexCount; ++i) + { + float32 dot = b2Dot(m_normals[i], pLocal - m_vertices[i]); + if (dot > 0.0f) + { + return false; + } + } + + return true; +} + +bool b2PolygonShape::RayCast(b2RayCastOutput* output, const b2RayCastInput& input, + const b2Transform& xf, int32 childIndex) const +{ + B2_NOT_USED(childIndex); + + // Put the ray into the polygon's frame of reference. + b2Vec2 p1 = b2MulT(xf.q, input.p1 - xf.p); + b2Vec2 p2 = b2MulT(xf.q, input.p2 - xf.p); + b2Vec2 d = p2 - p1; + + float32 lower = 0.0f, upper = input.maxFraction; + + int32 index = -1; + + for (int32 i = 0; i < m_vertexCount; ++i) + { + // p = p1 + a * d + // dot(normal, p - v) = 0 + // dot(normal, p1 - v) + a * dot(normal, d) = 0 + float32 numerator = b2Dot(m_normals[i], m_vertices[i] - p1); + float32 denominator = b2Dot(m_normals[i], d); + + if (denominator == 0.0f) + { + if (numerator < 0.0f) + { + return false; + } + } + else + { + // Note: we want this predicate without division: + // lower < numerator / denominator, where denominator < 0 + // Since denominator < 0, we have to flip the inequality: + // lower < numerator / denominator <==> denominator * lower > numerator. + if (denominator < 0.0f && numerator < lower * denominator) + { + // Increase lower. + // The segment enters this half-space. + lower = numerator / denominator; + index = i; + } + else if (denominator > 0.0f && numerator < upper * denominator) + { + // Decrease upper. + // The segment exits this half-space. + upper = numerator / denominator; + } + } + + // The use of epsilon here causes the assert on lower to trip + // in some cases. Apparently the use of epsilon was to make edge + // shapes work, but now those are handled separately. + //if (upper < lower - b2_epsilon) + if (upper < lower) + { + return false; + } + } + + b2Assert(0.0f <= lower && lower <= input.maxFraction); + + if (index >= 0) + { + output->fraction = lower; + output->normal = b2Mul(xf.q, m_normals[index]); + return true; + } + + return false; +} + +void b2PolygonShape::ComputeAABB(b2AABB* aabb, const b2Transform& xf, int32 childIndex) const +{ + B2_NOT_USED(childIndex); + + b2Vec2 lower = b2Mul(xf, m_vertices[0]); + b2Vec2 upper = lower; + + for (int32 i = 1; i < m_vertexCount; ++i) + { + b2Vec2 v = b2Mul(xf, m_vertices[i]); + lower = b2Min(lower, v); + upper = b2Max(upper, v); + } + + b2Vec2 r(m_radius, m_radius); + aabb->lowerBound = lower - r; + aabb->upperBound = upper + r; +} + +void b2PolygonShape::ComputeMass(b2MassData* massData, float32 density) const +{ + // Polygon mass, centroid, and inertia. + // Let rho be the polygon density in mass per unit area. + // Then: + // mass = rho * int(dA) + // centroid.x = (1/mass) * rho * int(x * dA) + // centroid.y = (1/mass) * rho * int(y * dA) + // I = rho * int((x*x + y*y) * dA) + // + // We can compute these integrals by summing all the integrals + // for each triangle of the polygon. To evaluate the integral + // for a single triangle, we make a change of variables to + // the (u,v) coordinates of the triangle: + // x = x0 + e1x * u + e2x * v + // y = y0 + e1y * u + e2y * v + // where 0 <= u && 0 <= v && u + v <= 1. + // + // We integrate u from [0,1-v] and then v from [0,1]. + // We also need to use the Jacobian of the transformation: + // D = cross(e1, e2) + // + // Simplification: triangle centroid = (1/3) * (p1 + p2 + p3) + // + // The rest of the derivation is handled by computer algebra. + + b2Assert(m_vertexCount >= 3); + + b2Vec2 center; center.Set(0.0f, 0.0f); + float32 area = 0.0f; + float32 I = 0.0f; + + // s is the reference point for forming triangles. + // It's location doesn't change the result (except for rounding error). + b2Vec2 s(0.0f, 0.0f); + + // This code would put the reference point inside the polygon. + for (int32 i = 0; i < m_vertexCount; ++i) + { + s += m_vertices[i]; + } + s *= 1.0f / m_vertexCount; + + const float32 k_inv3 = 1.0f / 3.0f; + + for (int32 i = 0; i < m_vertexCount; ++i) + { + // Triangle vertices. + b2Vec2 e1 = m_vertices[i] - s; + b2Vec2 e2 = i + 1 < m_vertexCount ? m_vertices[i+1] - s : m_vertices[0] - s; + + float32 D = b2Cross(e1, e2); + + float32 triangleArea = 0.5f * D; + area += triangleArea; + + // Area weighted centroid + center += triangleArea * k_inv3 * (e1 + e2); + + float32 ex1 = e1.x, ey1 = e1.y; + float32 ex2 = e2.x, ey2 = e2.y; + + float32 intx2 = ex1*ex1 + ex2*ex1 + ex2*ex2; + float32 inty2 = ey1*ey1 + ey2*ey1 + ey2*ey2; + + I += (0.25f * k_inv3 * D) * (intx2 + inty2); + } + + // Total mass + massData->mass = density * area; + + // Center of mass + b2Assert(area > b2_epsilon); + center *= 1.0f / area; + massData->center = center + s; + + // Inertia tensor relative to the local origin (point s). + massData->I = density * I; + + // Shift to center of mass then to original body origin. + massData->I += massData->mass * (b2Dot(massData->center, massData->center) - b2Dot(center, center)); +} diff --git a/Box2D/Collision/Shapes/b2PolygonShape.h b/Box2D/Collision/Shapes/b2PolygonShape.h new file mode 100644 index 0000000..5c53212 --- /dev/null +++ b/Box2D/Collision/Shapes/b2PolygonShape.h @@ -0,0 +1,95 @@ +/* +* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org +* +* 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_SHAPE_H +#define B2_POLYGON_SHAPE_H + +#include <Box2D/Collision/Shapes/b2Shape.h> + +/// A convex polygon. It is assumed that the interior of the polygon is to +/// the left of each edge. +/// Polygons have a maximum number of vertices equal to b2_maxPolygonVertices. +/// In most cases you should not need many vertices for a convex polygon. +class b2PolygonShape : public b2Shape +{ +public: + b2PolygonShape(); + + /// Implement b2Shape. + b2Shape* Clone(b2BlockAllocator* allocator) const; + + /// @see b2Shape::GetChildCount + int32 GetChildCount() const; + + /// Copy vertices. This assumes the vertices define a convex polygon. + /// It is assumed that the exterior is the the right of each edge. + /// The count must be in the range [3, b2_maxPolygonVertices]. + void Set(const b2Vec2* vertices, int32 vertexCount); + + /// Build vertices to represent an axis-aligned box. + /// @param hx the half-width. + /// @param hy the half-height. + void SetAsBox(float32 hx, float32 hy); + + /// Build vertices to represent an oriented box. + /// @param hx the half-width. + /// @param hy the half-height. + /// @param center the center of the box in local coordinates. + /// @param angle the rotation of the box in local coordinates. + void SetAsBox(float32 hx, float32 hy, const b2Vec2& center, float32 angle); + + /// @see b2Shape::TestPoint + bool TestPoint(const b2Transform& transform, const b2Vec2& p) const; + + /// Implement b2Shape. + bool RayCast(b2RayCastOutput* output, const b2RayCastInput& input, + const b2Transform& transform, int32 childIndex) const; + + /// @see b2Shape::ComputeAABB + void ComputeAABB(b2AABB* aabb, const b2Transform& transform, int32 childIndex) const; + + /// @see b2Shape::ComputeMass + void ComputeMass(b2MassData* massData, float32 density) const; + + /// Get the vertex count. + int32 GetVertexCount() const { return m_vertexCount; } + + /// Get a vertex by index. + const b2Vec2& GetVertex(int32 index) const; + + b2Vec2 m_centroid; + b2Vec2 m_vertices[b2_maxPolygonVertices]; + b2Vec2 m_normals[b2_maxPolygonVertices]; + int32 m_vertexCount; +}; + +inline b2PolygonShape::b2PolygonShape() +{ + m_type = e_polygon; + m_radius = b2_polygonRadius; + m_vertexCount = 0; + m_centroid.SetZero(); +} + +inline const b2Vec2& b2PolygonShape::GetVertex(int32 index) const +{ + b2Assert(0 <= index && index < m_vertexCount); + return m_vertices[index]; +} + +#endif diff --git a/Box2D/Collision/Shapes/b2Shape.h b/Box2D/Collision/Shapes/b2Shape.h new file mode 100644 index 0000000..170ea1a --- /dev/null +++ b/Box2D/Collision/Shapes/b2Shape.h @@ -0,0 +1,101 @@ +/* +* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org +* +* 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_SHAPE_H +#define B2_SHAPE_H + +#include <Box2D/Common/b2BlockAllocator.h> +#include <Box2D/Common/b2Math.h> +#include <Box2D/Collision/b2Collision.h> + +/// This holds the mass data computed for a shape. +struct b2MassData +{ + /// The mass of the shape, usually in kilograms. + float32 mass; + + /// The position of the shape's centroid relative to the shape's origin. + b2Vec2 center; + + /// The rotational inertia of the shape about the local origin. + float32 I; +}; + +/// A shape is used for collision detection. You can create a shape however you like. +/// Shapes used for simulation in b2World are created automatically when a b2Fixture +/// is created. Shapes may encapsulate a one or more child shapes. +class b2Shape +{ +public: + + enum Type + { + e_circle = 0, + e_edge = 1, + e_polygon = 2, + e_chain = 3, + e_typeCount = 4 + }; + + virtual ~b2Shape() {} + + /// Clone the concrete shape using the provided allocator. + virtual b2Shape* Clone(b2BlockAllocator* allocator) const = 0; + + /// Get the type of this shape. You can use this to down cast to the concrete shape. + /// @return the shape type. + Type GetType() const; + + /// Get the number of child primitives. + virtual int32 GetChildCount() const = 0; + + /// Test a point for containment in this shape. This only works for convex shapes. + /// @param xf the shape world transform. + /// @param p a point in world coordinates. + virtual bool TestPoint(const b2Transform& xf, const b2Vec2& p) const = 0; + + /// Cast a ray against a child shape. + /// @param output the ray-cast results. + /// @param input the ray-cast input parameters. + /// @param transform the transform to be applied to the shape. + /// @param childIndex the child shape index + virtual bool RayCast(b2RayCastOutput* output, const b2RayCastInput& input, + const b2Transform& transform, int32 childIndex) const = 0; + + /// Given a transform, compute the associated axis aligned bounding box for a child shape. + /// @param aabb returns the axis aligned box. + /// @param xf the world transform of the shape. + /// @param childIndex the child shape + virtual void ComputeAABB(b2AABB* aabb, const b2Transform& xf, int32 childIndex) const = 0; + + /// Compute the mass properties of this shape using its dimensions and density. + /// The inertia tensor is computed about the local origin. + /// @param massData returns the mass data for this shape. + /// @param density the density in kilograms per meter squared. + virtual void ComputeMass(b2MassData* massData, float32 density) const = 0; + + Type m_type; + float32 m_radius; +}; + +inline b2Shape::Type b2Shape::GetType() const +{ + return m_type; +} + +#endif |