diff --git a/Box2D/Android.mk b/Box2D/Android.mk deleted file mode 100644 index ca6cfca39e..0000000000 --- a/Box2D/Android.mk +++ /dev/null @@ -1,61 +0,0 @@ -LOCAL_PATH := $(call my-dir) - -include $(CLEAR_VARS) - -LOCAL_MODULE := box2d_static - -LOCAL_MODULE_FILENAME := libbox2d - -LOCAL_SRC_FILES := \ -Collision/b2BroadPhase.cpp \ -Collision/b2CollideCircle.cpp \ -Collision/b2CollideEdge.cpp \ -Collision/b2CollidePolygon.cpp \ -Collision/b2Collision.cpp \ -Collision/b2Distance.cpp \ -Collision/b2DynamicTree.cpp \ -Collision/b2TimeOfImpact.cpp \ -Collision/Shapes/b2ChainShape.cpp \ -Collision/Shapes/b2CircleShape.cpp \ -Collision/Shapes/b2EdgeShape.cpp \ -Collision/Shapes/b2PolygonShape.cpp \ -Common/b2BlockAllocator.cpp \ -Common/b2Draw.cpp \ -Common/b2Math.cpp \ -Common/b2Settings.cpp \ -Common/b2StackAllocator.cpp \ -Common/b2Timer.cpp \ -Dynamics/b2Body.cpp \ -Dynamics/b2ContactManager.cpp \ -Dynamics/b2Fixture.cpp \ -Dynamics/b2Island.cpp \ -Dynamics/b2World.cpp \ -Dynamics/b2WorldCallbacks.cpp \ -Dynamics/Contacts/b2ChainAndCircleContact.cpp \ -Dynamics/Contacts/b2ChainAndPolygonContact.cpp \ -Dynamics/Contacts/b2CircleContact.cpp \ -Dynamics/Contacts/b2Contact.cpp \ -Dynamics/Contacts/b2ContactSolver.cpp \ -Dynamics/Contacts/b2EdgeAndCircleContact.cpp \ -Dynamics/Contacts/b2EdgeAndPolygonContact.cpp \ -Dynamics/Contacts/b2PolygonAndCircleContact.cpp \ -Dynamics/Contacts/b2PolygonContact.cpp \ -Dynamics/Joints/b2DistanceJoint.cpp \ -Dynamics/Joints/b2FrictionJoint.cpp \ -Dynamics/Joints/b2GearJoint.cpp \ -Dynamics/Joints/b2Joint.cpp \ -Dynamics/Joints/b2MotorJoint.cpp \ -Dynamics/Joints/b2MouseJoint.cpp \ -Dynamics/Joints/b2PrismaticJoint.cpp \ -Dynamics/Joints/b2PulleyJoint.cpp \ -Dynamics/Joints/b2RevoluteJoint.cpp \ -Dynamics/Joints/b2RopeJoint.cpp \ -Dynamics/Joints/b2WeldJoint.cpp \ -Dynamics/Joints/b2WheelJoint.cpp \ -Rope/b2Rope.cpp - -LOCAL_EXPORT_C_INCLUDES := $(LOCAL_PATH)/.. - -LOCAL_C_INCLUDES := $(LOCAL_PATH)/.. - -include $(BUILD_STATIC_LIBRARY) diff --git a/Box2D/Box2D.h b/Box2D/Box2D.h deleted file mode 100644 index 187046131d..0000000000 --- a/Box2D/Box2D.h +++ /dev/null @@ -1,68 +0,0 @@ -/* -* 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 BOX2D_H -#define BOX2D_H - -/** -\mainpage Box2D API Documentation - -\section intro_sec Getting Started - -For documentation please see http://box2d.org/documentation.html - -For discussion please visit http://box2d.org/forum -*/ - -// These include files constitute the main Box2D API - -#include -#include -#include - -#include -#include -#include -#include - -#include -#include -#include -#include - -#include -#include -#include -#include -#include - -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#endif diff --git a/Box2D/CMakeLists.txt b/Box2D/CMakeLists.txt deleted file mode 100644 index eff3db22eb..0000000000 --- a/Box2D/CMakeLists.txt +++ /dev/null @@ -1,62 +0,0 @@ -set(BOX2D_SRC - Collision/Shapes/b2ChainShape.cpp - Collision/Shapes/b2CircleShape.cpp - Collision/Shapes/b2EdgeShape.cpp - Collision/Shapes/b2PolygonShape.cpp - Collision/b2BroadPhase.cpp - Collision/b2CollideCircle.cpp - Collision/b2CollideEdge.cpp - Collision/b2CollidePolygon.cpp - Collision/b2Collision.cpp - Collision/b2Distance.cpp - Collision/b2DynamicTree.cpp - Collision/b2TimeOfImpact.cpp - Common/b2BlockAllocator.cpp - Common/b2Draw.cpp - Common/b2Math.cpp - Common/b2Settings.cpp - Common/b2StackAllocator.cpp - Common/b2Timer.cpp - Dynamics/Contacts/b2ChainAndCircleContact.cpp - Dynamics/Contacts/b2ChainAndPolygonContact.cpp - Dynamics/Contacts/b2CircleContact.cpp - Dynamics/Contacts/b2Contact.cpp - Dynamics/Contacts/b2ContactSolver.cpp - Dynamics/Contacts/b2EdgeAndCircleContact.cpp - Dynamics/Contacts/b2EdgeAndPolygonContact.cpp - Dynamics/Contacts/b2PolygonAndCircleContact.cpp - Dynamics/Contacts/b2PolygonContact.cpp - Dynamics/Joints/b2DistanceJoint.cpp - Dynamics/Joints/b2FrictionJoint.cpp - Dynamics/Joints/b2GearJoint.cpp - Dynamics/Joints/b2Joint.cpp - Dynamics/Joints/b2MouseJoint.cpp - Dynamics/Joints/b2MotorJoint.cpp - Dynamics/Joints/b2PrismaticJoint.cpp - Dynamics/Joints/b2PulleyJoint.cpp - Dynamics/Joints/b2RevoluteJoint.cpp - Dynamics/Joints/b2RopeJoint.cpp - Dynamics/Joints/b2WeldJoint.cpp - Dynamics/Joints/b2WheelJoint.cpp - Dynamics/b2Body.cpp - Dynamics/b2ContactManager.cpp - Dynamics/b2Fixture.cpp - Dynamics/b2Island.cpp - Dynamics/b2World.cpp - Dynamics/b2WorldCallbacks.cpp - Rope/b2Rope.cpp -) - -include_directories( - .. -) - -add_library(box2d STATIC - ${BOX2D_SRC} -) - -set_target_properties(box2d - PROPERTIES - ARCHIVE_OUTPUT_DIRECTORY "${CMAKE_BINARY_DIR}/lib" - LIBRARY_OUTPUT_DIRECTORY "${CMAKE_BINARY_DIR}/lib" -) diff --git a/Box2D/Collision/Shapes/b2ChainShape.cpp b/Box2D/Collision/Shapes/b2ChainShape.cpp deleted file mode 100644 index 010b9edcb9..0000000000 --- a/Box2D/Collision/Shapes/b2ChainShape.cpp +++ /dev/null @@ -1,191 +0,0 @@ -/* -* 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 -#include -#include -#include -#include - -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); - for (int32 i = 1; i < count; ++i) - { - b2Vec2 v1 = vertices[i-1]; - b2Vec2 v2 = vertices[i]; - // If the code crashes here, it means your vertices are too close together. - b2Assert(b2DistanceSquared(v1, v2) > b2_linearSlop * b2_linearSlop); - } - - 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); - for (int32 i = 1; i < count; ++i) - { - b2Vec2 v1 = vertices[i-1]; - b2Vec2 v2 = vertices[i]; - // If the code crashes here, it means your vertices are too close together. - b2Assert(b2DistanceSquared(v1, v2) > b2_linearSlop * b2_linearSlop); - } - - m_count = count; - m_vertices = (b2Vec2*)b2Alloc(count * sizeof(b2Vec2)); - memcpy(m_vertices, vertices, m_count * sizeof(b2Vec2)); - - m_hasPrevVertex = false; - m_hasNextVertex = false; - - m_prevVertex.SetZero(); - m_nextVertex.SetZero(); -} - -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 deleted file mode 100644 index e836dfef9a..0000000000 --- a/Box2D/Collision/Shapes/b2ChainShape.h +++ /dev/null @@ -1,102 +0,0 @@ -/* -* 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 - -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 = false; - m_hasNextVertex = false; -} - -#endif diff --git a/Box2D/Collision/Shapes/b2CircleShape.cpp b/Box2D/Collision/Shapes/b2CircleShape.cpp deleted file mode 100644 index 39d4ae9998..0000000000 --- a/Box2D/Collision/Shapes/b2CircleShape.cpp +++ /dev/null @@ -1,99 +0,0 @@ -/* -* 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 -#include - -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 deleted file mode 100644 index 3209f924ad..0000000000 --- a/Box2D/Collision/Shapes/b2CircleShape.h +++ /dev/null @@ -1,91 +0,0 @@ -/* -* 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 - -/// 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 deleted file mode 100644 index 8bcd12f7a7..0000000000 --- a/Box2D/Collision/Shapes/b2EdgeShape.cpp +++ /dev/null @@ -1,138 +0,0 @@ -/* -* 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 -#include - -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 = -b2Mul(xf.q, normal); - } - else - { - output->normal = b2Mul(xf.q, 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 deleted file mode 100644 index 0acef8518e..0000000000 --- a/Box2D/Collision/Shapes/b2EdgeShape.h +++ /dev/null @@ -1,74 +0,0 @@ -/* -* 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 - -/// 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 deleted file mode 100644 index 2f32e9fe28..0000000000 --- a/Box2D/Collision/Shapes/b2PolygonShape.cpp +++ /dev/null @@ -1,459 +0,0 @@ -/* -* 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 -#include - -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_count = 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_count = 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_count; ++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); - if (count < 3) - { - SetAsBox(1.0f, 1.0f); - return; - } - - int32 n = b2Min(count, b2_maxPolygonVertices); - - // Perform welding and copy vertices into local buffer. - b2Vec2 ps[b2_maxPolygonVertices]; - int32 tempCount = 0; - for (int32 i = 0; i < n; ++i) - { - b2Vec2 v = vertices[i]; - - bool unique = true; - for (int32 j = 0; j < tempCount; ++j) - { - if (b2DistanceSquared(v, ps[j]) < 0.5f * b2_linearSlop) - { - unique = false; - break; - } - } - - if (unique) - { - ps[tempCount++] = v; - } - } - - n = tempCount; - if (n < 3) - { - // Polygon is degenerate. - b2Assert(false); - SetAsBox(1.0f, 1.0f); - return; - } - - // Create the convex hull using the Gift wrapping algorithm - // http://en.wikipedia.org/wiki/Gift_wrapping_algorithm - - // Find the right most point on the hull - int32 i0 = 0; - float32 x0 = ps[0].x; - for (int32 i = 1; i < n; ++i) - { - float32 x = ps[i].x; - if (x > x0 || (x == x0 && ps[i].y < ps[i0].y)) - { - i0 = i; - x0 = x; - } - } - - int32 hull[b2_maxPolygonVertices]; - int32 m = 0; - int32 ih = i0; - - for (;;) - { - hull[m] = ih; - - int32 ie = 0; - for (int32 j = 1; j < n; ++j) - { - if (ie == ih) - { - ie = j; - continue; - } - - b2Vec2 r = ps[ie] - ps[hull[m]]; - b2Vec2 v = ps[j] - ps[hull[m]]; - float32 c = b2Cross(r, v); - if (c < 0.0f) - { - ie = j; - } - - // Collinearity check - if (c == 0.0f && v.LengthSquared() > r.LengthSquared()) - { - ie = j; - } - } - - ++m; - ih = ie; - - if (ie == i0) - { - break; - } - } - - m_count = m; - - // Copy vertices. - for (int32 i = 0; i < m; ++i) - { - m_vertices[i] = ps[hull[i]]; - } - - // Compute normals. Ensure the edges have non-zero length. - for (int32 i = 0; i < m; ++i) - { - int32 i1 = i; - int32 i2 = i + 1 < m ? 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(); - } - - // Compute the polygon centroid. - m_centroid = ComputeCentroid(m_vertices, m); -} - -bool b2PolygonShape::TestPoint(const b2Transform& xf, const b2Vec2& p) const -{ - b2Vec2 pLocal = b2MulT(xf.q, p - xf.p); - - for (int32 i = 0; i < m_count; ++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_count; ++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_count; ++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_count >= 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_count; ++i) - { - s += m_vertices[i]; - } - s *= 1.0f / m_count; - - const float32 k_inv3 = 1.0f / 3.0f; - - for (int32 i = 0; i < m_count; ++i) - { - // Triangle vertices. - b2Vec2 e1 = m_vertices[i] - s; - b2Vec2 e2 = i + 1 < m_count ? 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)); -} - -bool b2PolygonShape::Validate() const -{ - for (int32 i = 0; i < m_count; ++i) - { - int32 i1 = i; - int32 i2 = i < m_count - 1 ? i1 + 1 : 0; - b2Vec2 p = m_vertices[i1]; - b2Vec2 e = m_vertices[i2] - p; - - for (int32 j = 0; j < m_count; ++j) - { - if (j == i1 || j == i2) - { - continue; - } - - b2Vec2 v = m_vertices[j] - p; - float32 c = b2Cross(e, v); - if (c < 0.0f) - { - return false; - } - } - } - - return true; -} diff --git a/Box2D/Collision/Shapes/b2PolygonShape.h b/Box2D/Collision/Shapes/b2PolygonShape.h deleted file mode 100644 index b19298038e..0000000000 --- a/Box2D/Collision/Shapes/b2PolygonShape.h +++ /dev/null @@ -1,101 +0,0 @@ -/* -* 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 - -/// 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; - - /// Create a convex hull from the given array of local points. - /// The count must be in the range [3, b2_maxPolygonVertices]. - /// @warning the points may be re-ordered, even if they form a convex polygon - /// @warning collinear points are handled but not removed. Collinear points - /// may lead to poor stacking behavior. - void Set(const b2Vec2* points, int32 count); - - /// Build vertices to represent an axis-aligned box centered on the local origin. - /// @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_count; } - - /// Get a vertex by index. - const b2Vec2& GetVertex(int32 index) const; - - /// Validate convexity. This is a very time consuming operation. - /// @returns true if valid - bool Validate() const; - - b2Vec2 m_centroid; - b2Vec2 m_vertices[b2_maxPolygonVertices]; - b2Vec2 m_normals[b2_maxPolygonVertices]; - int32 m_count; -}; - -inline b2PolygonShape::b2PolygonShape() -{ - m_type = e_polygon; - m_radius = b2_polygonRadius; - m_count = 0; - m_centroid.SetZero(); -} - -inline const b2Vec2& b2PolygonShape::GetVertex(int32 index) const -{ - b2Assert(0 <= index && index < m_count); - return m_vertices[index]; -} - -#endif diff --git a/Box2D/Collision/Shapes/b2Shape.h b/Box2D/Collision/Shapes/b2Shape.h deleted file mode 100644 index 170ea1a13e..0000000000 --- a/Box2D/Collision/Shapes/b2Shape.h +++ /dev/null @@ -1,101 +0,0 @@ -/* -* 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 -#include -#include - -/// 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 diff --git a/Box2D/Collision/b2BroadPhase.cpp b/Box2D/Collision/b2BroadPhase.cpp deleted file mode 100644 index 04e955d235..0000000000 --- a/Box2D/Collision/b2BroadPhase.cpp +++ /dev/null @@ -1,119 +0,0 @@ -/* -* 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 - -b2BroadPhase::b2BroadPhase() -{ - m_proxyCount = 0; - - m_pairCapacity = 16; - m_pairCount = 0; - m_pairBuffer = (b2Pair*)b2Alloc(m_pairCapacity * sizeof(b2Pair)); - - m_moveCapacity = 16; - m_moveCount = 0; - m_moveBuffer = (int32*)b2Alloc(m_moveCapacity * sizeof(int32)); -} - -b2BroadPhase::~b2BroadPhase() -{ - b2Free(m_moveBuffer); - b2Free(m_pairBuffer); -} - -int32 b2BroadPhase::CreateProxy(const b2AABB& aabb, void* userData) -{ - int32 proxyId = m_tree.CreateProxy(aabb, userData); - ++m_proxyCount; - BufferMove(proxyId); - return proxyId; -} - -void b2BroadPhase::DestroyProxy(int32 proxyId) -{ - UnBufferMove(proxyId); - --m_proxyCount; - m_tree.DestroyProxy(proxyId); -} - -void b2BroadPhase::MoveProxy(int32 proxyId, const b2AABB& aabb, const b2Vec2& displacement) -{ - bool buffer = m_tree.MoveProxy(proxyId, aabb, displacement); - if (buffer) - { - BufferMove(proxyId); - } -} - -void b2BroadPhase::TouchProxy(int32 proxyId) -{ - BufferMove(proxyId); -} - -void b2BroadPhase::BufferMove(int32 proxyId) -{ - if (m_moveCount == m_moveCapacity) - { - int32* oldBuffer = m_moveBuffer; - m_moveCapacity *= 2; - m_moveBuffer = (int32*)b2Alloc(m_moveCapacity * sizeof(int32)); - memcpy(m_moveBuffer, oldBuffer, m_moveCount * sizeof(int32)); - b2Free(oldBuffer); - } - - m_moveBuffer[m_moveCount] = proxyId; - ++m_moveCount; -} - -void b2BroadPhase::UnBufferMove(int32 proxyId) -{ - for (int32 i = 0; i < m_moveCount; ++i) - { - if (m_moveBuffer[i] == proxyId) - { - m_moveBuffer[i] = e_nullProxy; - } - } -} - -// This is called from b2DynamicTree::Query when we are gathering pairs. -bool b2BroadPhase::QueryCallback(int32 proxyId) -{ - // A proxy cannot form a pair with itself. - if (proxyId == m_queryProxyId) - { - return true; - } - - // Grow the pair buffer as needed. - if (m_pairCount == m_pairCapacity) - { - b2Pair* oldBuffer = m_pairBuffer; - m_pairCapacity *= 2; - m_pairBuffer = (b2Pair*)b2Alloc(m_pairCapacity * sizeof(b2Pair)); - memcpy(m_pairBuffer, oldBuffer, m_pairCount * sizeof(b2Pair)); - b2Free(oldBuffer); - } - - m_pairBuffer[m_pairCount].proxyIdA = b2Min(proxyId, m_queryProxyId); - m_pairBuffer[m_pairCount].proxyIdB = b2Max(proxyId, m_queryProxyId); - ++m_pairCount; - - return true; -} diff --git a/Box2D/Collision/b2BroadPhase.h b/Box2D/Collision/b2BroadPhase.h deleted file mode 100644 index 0c460cabbb..0000000000 --- a/Box2D/Collision/b2BroadPhase.h +++ /dev/null @@ -1,257 +0,0 @@ -/* -* 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_BROAD_PHASE_H -#define B2_BROAD_PHASE_H - -#include -#include -#include -#include - -struct b2Pair -{ - int32 proxyIdA; - int32 proxyIdB; -}; - -/// The broad-phase is used for computing pairs and performing volume queries and ray casts. -/// This broad-phase does not persist pairs. Instead, this reports potentially new pairs. -/// It is up to the client to consume the new pairs and to track subsequent overlap. -class b2BroadPhase -{ -public: - - enum - { - e_nullProxy = -1 - }; - - b2BroadPhase(); - ~b2BroadPhase(); - - /// Create a proxy with an initial AABB. Pairs are not reported until - /// UpdatePairs is called. - int32 CreateProxy(const b2AABB& aabb, void* userData); - - /// Destroy a proxy. It is up to the client to remove any pairs. - void DestroyProxy(int32 proxyId); - - /// Call MoveProxy as many times as you like, then when you are done - /// call UpdatePairs to finalized the proxy pairs (for your time step). - void MoveProxy(int32 proxyId, const b2AABB& aabb, const b2Vec2& displacement); - - /// Call to trigger a re-processing of it's pairs on the next call to UpdatePairs. - void TouchProxy(int32 proxyId); - - /// Get the fat AABB for a proxy. - const b2AABB& GetFatAABB(int32 proxyId) const; - - /// Get user data from a proxy. Returns NULL if the id is invalid. - void* GetUserData(int32 proxyId) const; - - /// Test overlap of fat AABBs. - bool TestOverlap(int32 proxyIdA, int32 proxyIdB) const; - - /// Get the number of proxies. - int32 GetProxyCount() const; - - /// Update the pairs. This results in pair callbacks. This can only add pairs. - template - void UpdatePairs(T* callback); - - /// Query an AABB for overlapping proxies. The callback class - /// is called for each proxy that overlaps the supplied AABB. - template - void Query(T* callback, const b2AABB& aabb) const; - - /// Ray-cast against the proxies in the tree. This relies on the callback - /// to perform a exact ray-cast in the case were the proxy contains a shape. - /// The callback also performs the any collision filtering. This has performance - /// roughly equal to k * log(n), where k is the number of collisions and n is the - /// number of proxies in the tree. - /// @param input the ray-cast input data. The ray extends from p1 to p1 + maxFraction * (p2 - p1). - /// @param callback a callback class that is called for each proxy that is hit by the ray. - template - void RayCast(T* callback, const b2RayCastInput& input) const; - - /// Get the height of the embedded tree. - int32 GetTreeHeight() const; - - /// Get the balance of the embedded tree. - int32 GetTreeBalance() const; - - /// Get the quality metric of the embedded tree. - float32 GetTreeQuality() const; - - /// Shift the world origin. Useful for large worlds. - /// The shift formula is: position -= newOrigin - /// @param newOrigin the new origin with respect to the old origin - void ShiftOrigin(const b2Vec2& newOrigin); - -private: - - friend class b2DynamicTree; - - void BufferMove(int32 proxyId); - void UnBufferMove(int32 proxyId); - - bool QueryCallback(int32 proxyId); - - b2DynamicTree m_tree; - - int32 m_proxyCount; - - int32* m_moveBuffer; - int32 m_moveCapacity; - int32 m_moveCount; - - b2Pair* m_pairBuffer; - int32 m_pairCapacity; - int32 m_pairCount; - - int32 m_queryProxyId; -}; - -/// This is used to sort pairs. -inline bool b2PairLessThan(const b2Pair& pair1, const b2Pair& pair2) -{ - if (pair1.proxyIdA < pair2.proxyIdA) - { - return true; - } - - if (pair1.proxyIdA == pair2.proxyIdA) - { - return pair1.proxyIdB < pair2.proxyIdB; - } - - return false; -} - -inline void* b2BroadPhase::GetUserData(int32 proxyId) const -{ - return m_tree.GetUserData(proxyId); -} - -inline bool b2BroadPhase::TestOverlap(int32 proxyIdA, int32 proxyIdB) const -{ - const b2AABB& aabbA = m_tree.GetFatAABB(proxyIdA); - const b2AABB& aabbB = m_tree.GetFatAABB(proxyIdB); - return b2TestOverlap(aabbA, aabbB); -} - -inline const b2AABB& b2BroadPhase::GetFatAABB(int32 proxyId) const -{ - return m_tree.GetFatAABB(proxyId); -} - -inline int32 b2BroadPhase::GetProxyCount() const -{ - return m_proxyCount; -} - -inline int32 b2BroadPhase::GetTreeHeight() const -{ - return m_tree.GetHeight(); -} - -inline int32 b2BroadPhase::GetTreeBalance() const -{ - return m_tree.GetMaxBalance(); -} - -inline float32 b2BroadPhase::GetTreeQuality() const -{ - return m_tree.GetAreaRatio(); -} - -template -void b2BroadPhase::UpdatePairs(T* callback) -{ - // Reset pair buffer - m_pairCount = 0; - - // Perform tree queries for all moving proxies. - for (int32 i = 0; i < m_moveCount; ++i) - { - m_queryProxyId = m_moveBuffer[i]; - if (m_queryProxyId == e_nullProxy) - { - continue; - } - - // We have to query the tree with the fat AABB so that - // we don't fail to create a pair that may touch later. - const b2AABB& fatAABB = m_tree.GetFatAABB(m_queryProxyId); - - // Query tree, create pairs and add them pair buffer. - m_tree.Query(this, fatAABB); - } - - // Reset move buffer - m_moveCount = 0; - - // Sort the pair buffer to expose duplicates. - std::sort(m_pairBuffer, m_pairBuffer + m_pairCount, b2PairLessThan); - - // Send the pairs back to the client. - int32 i = 0; - while (i < m_pairCount) - { - b2Pair* primaryPair = m_pairBuffer + i; - void* userDataA = m_tree.GetUserData(primaryPair->proxyIdA); - void* userDataB = m_tree.GetUserData(primaryPair->proxyIdB); - - callback->AddPair(userDataA, userDataB); - ++i; - - // Skip any duplicate pairs. - while (i < m_pairCount) - { - b2Pair* pair = m_pairBuffer + i; - if (pair->proxyIdA != primaryPair->proxyIdA || pair->proxyIdB != primaryPair->proxyIdB) - { - break; - } - ++i; - } - } - - // Try to keep the tree balanced. - //m_tree.Rebalance(4); -} - -template -inline void b2BroadPhase::Query(T* callback, const b2AABB& aabb) const -{ - m_tree.Query(callback, aabb); -} - -template -inline void b2BroadPhase::RayCast(T* callback, const b2RayCastInput& input) const -{ - m_tree.RayCast(callback, input); -} - -inline void b2BroadPhase::ShiftOrigin(const b2Vec2& newOrigin) -{ - m_tree.ShiftOrigin(newOrigin); -} - -#endif diff --git a/Box2D/Collision/b2CollideCircle.cpp b/Box2D/Collision/b2CollideCircle.cpp deleted file mode 100644 index 4181fd47cb..0000000000 --- a/Box2D/Collision/b2CollideCircle.cpp +++ /dev/null @@ -1,154 +0,0 @@ -/* -* Copyright (c) 2007-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 -#include -#include - -void b2CollideCircles( - b2Manifold* manifold, - const b2CircleShape* circleA, const b2Transform& xfA, - const b2CircleShape* circleB, const b2Transform& xfB) -{ - manifold->pointCount = 0; - - b2Vec2 pA = b2Mul(xfA, circleA->m_p); - b2Vec2 pB = b2Mul(xfB, circleB->m_p); - - b2Vec2 d = pB - pA; - float32 distSqr = b2Dot(d, d); - float32 rA = circleA->m_radius, rB = circleB->m_radius; - float32 radius = rA + rB; - if (distSqr > radius * radius) - { - return; - } - - manifold->type = b2Manifold::e_circles; - manifold->localPoint = circleA->m_p; - manifold->localNormal.SetZero(); - manifold->pointCount = 1; - - manifold->points[0].localPoint = circleB->m_p; - manifold->points[0].id.key = 0; -} - -void b2CollidePolygonAndCircle( - b2Manifold* manifold, - const b2PolygonShape* polygonA, const b2Transform& xfA, - const b2CircleShape* circleB, const b2Transform& xfB) -{ - manifold->pointCount = 0; - - // Compute circle position in the frame of the polygon. - b2Vec2 c = b2Mul(xfB, circleB->m_p); - b2Vec2 cLocal = b2MulT(xfA, c); - - // Find the min separating edge. - int32 normalIndex = 0; - float32 separation = -b2_maxFloat; - float32 radius = polygonA->m_radius + circleB->m_radius; - int32 vertexCount = polygonA->m_count; - const b2Vec2* vertices = polygonA->m_vertices; - const b2Vec2* normals = polygonA->m_normals; - - for (int32 i = 0; i < vertexCount; ++i) - { - float32 s = b2Dot(normals[i], cLocal - vertices[i]); - - if (s > radius) - { - // Early out. - return; - } - - if (s > separation) - { - separation = s; - normalIndex = i; - } - } - - // Vertices that subtend the incident face. - int32 vertIndex1 = normalIndex; - int32 vertIndex2 = vertIndex1 + 1 < vertexCount ? vertIndex1 + 1 : 0; - b2Vec2 v1 = vertices[vertIndex1]; - b2Vec2 v2 = vertices[vertIndex2]; - - // If the center is inside the polygon ... - if (separation < b2_epsilon) - { - manifold->pointCount = 1; - manifold->type = b2Manifold::e_faceA; - manifold->localNormal = normals[normalIndex]; - manifold->localPoint = 0.5f * (v1 + v2); - manifold->points[0].localPoint = circleB->m_p; - manifold->points[0].id.key = 0; - return; - } - - // Compute barycentric coordinates - float32 u1 = b2Dot(cLocal - v1, v2 - v1); - float32 u2 = b2Dot(cLocal - v2, v1 - v2); - if (u1 <= 0.0f) - { - if (b2DistanceSquared(cLocal, v1) > radius * radius) - { - return; - } - - manifold->pointCount = 1; - manifold->type = b2Manifold::e_faceA; - manifold->localNormal = cLocal - v1; - manifold->localNormal.Normalize(); - manifold->localPoint = v1; - manifold->points[0].localPoint = circleB->m_p; - manifold->points[0].id.key = 0; - } - else if (u2 <= 0.0f) - { - if (b2DistanceSquared(cLocal, v2) > radius * radius) - { - return; - } - - manifold->pointCount = 1; - manifold->type = b2Manifold::e_faceA; - manifold->localNormal = cLocal - v2; - manifold->localNormal.Normalize(); - manifold->localPoint = v2; - manifold->points[0].localPoint = circleB->m_p; - manifold->points[0].id.key = 0; - } - else - { - b2Vec2 faceCenter = 0.5f * (v1 + v2); - float32 separation = b2Dot(cLocal - faceCenter, normals[vertIndex1]); - if (separation > radius) - { - return; - } - - manifold->pointCount = 1; - manifold->type = b2Manifold::e_faceA; - manifold->localNormal = normals[vertIndex1]; - manifold->localPoint = faceCenter; - manifold->points[0].localPoint = circleB->m_p; - manifold->points[0].id.key = 0; - } -} diff --git a/Box2D/Collision/b2CollideEdge.cpp b/Box2D/Collision/b2CollideEdge.cpp deleted file mode 100644 index 88e311c668..0000000000 --- a/Box2D/Collision/b2CollideEdge.cpp +++ /dev/null @@ -1,698 +0,0 @@ -/* - * Copyright (c) 2007-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 -#include -#include -#include - - -// Compute contact points for edge versus circle. -// This accounts for edge connectivity. -void b2CollideEdgeAndCircle(b2Manifold* manifold, - const b2EdgeShape* edgeA, const b2Transform& xfA, - const b2CircleShape* circleB, const b2Transform& xfB) -{ - manifold->pointCount = 0; - - // Compute circle in frame of edge - b2Vec2 Q = b2MulT(xfA, b2Mul(xfB, circleB->m_p)); - - b2Vec2 A = edgeA->m_vertex1, B = edgeA->m_vertex2; - b2Vec2 e = B - A; - - // Barycentric coordinates - float32 u = b2Dot(e, B - Q); - float32 v = b2Dot(e, Q - A); - - float32 radius = edgeA->m_radius + circleB->m_radius; - - b2ContactFeature cf; - cf.indexB = 0; - cf.typeB = b2ContactFeature::e_vertex; - - // Region A - if (v <= 0.0f) - { - b2Vec2 P = A; - b2Vec2 d = Q - P; - float32 dd = b2Dot(d, d); - if (dd > radius * radius) - { - return; - } - - // Is there an edge connected to A? - if (edgeA->m_hasVertex0) - { - b2Vec2 A1 = edgeA->m_vertex0; - b2Vec2 B1 = A; - b2Vec2 e1 = B1 - A1; - float32 u1 = b2Dot(e1, B1 - Q); - - // Is the circle in Region AB of the previous edge? - if (u1 > 0.0f) - { - return; - } - } - - cf.indexA = 0; - cf.typeA = b2ContactFeature::e_vertex; - manifold->pointCount = 1; - manifold->type = b2Manifold::e_circles; - manifold->localNormal.SetZero(); - manifold->localPoint = P; - manifold->points[0].id.key = 0; - manifold->points[0].id.cf = cf; - manifold->points[0].localPoint = circleB->m_p; - return; - } - - // Region B - if (u <= 0.0f) - { - b2Vec2 P = B; - b2Vec2 d = Q - P; - float32 dd = b2Dot(d, d); - if (dd > radius * radius) - { - return; - } - - // Is there an edge connected to B? - if (edgeA->m_hasVertex3) - { - b2Vec2 B2 = edgeA->m_vertex3; - b2Vec2 A2 = B; - b2Vec2 e2 = B2 - A2; - float32 v2 = b2Dot(e2, Q - A2); - - // Is the circle in Region AB of the next edge? - if (v2 > 0.0f) - { - return; - } - } - - cf.indexA = 1; - cf.typeA = b2ContactFeature::e_vertex; - manifold->pointCount = 1; - manifold->type = b2Manifold::e_circles; - manifold->localNormal.SetZero(); - manifold->localPoint = P; - manifold->points[0].id.key = 0; - manifold->points[0].id.cf = cf; - manifold->points[0].localPoint = circleB->m_p; - return; - } - - // Region AB - float32 den = b2Dot(e, e); - b2Assert(den > 0.0f); - b2Vec2 P = (1.0f / den) * (u * A + v * B); - b2Vec2 d = Q - P; - float32 dd = b2Dot(d, d); - if (dd > radius * radius) - { - return; - } - - b2Vec2 n(-e.y, e.x); - if (b2Dot(n, Q - A) < 0.0f) - { - n.Set(-n.x, -n.y); - } - n.Normalize(); - - cf.indexA = 0; - cf.typeA = b2ContactFeature::e_face; - manifold->pointCount = 1; - manifold->type = b2Manifold::e_faceA; - manifold->localNormal = n; - manifold->localPoint = A; - manifold->points[0].id.key = 0; - manifold->points[0].id.cf = cf; - manifold->points[0].localPoint = circleB->m_p; -} - -// This structure is used to keep track of the best separating axis. -struct b2EPAxis -{ - enum Type - { - e_unknown, - e_edgeA, - e_edgeB - }; - - Type type; - int32 index; - float32 separation; -}; - -// This holds polygon B expressed in frame A. -struct b2TempPolygon -{ - b2Vec2 vertices[b2_maxPolygonVertices]; - b2Vec2 normals[b2_maxPolygonVertices]; - int32 count; -}; - -// Reference face used for clipping -struct b2ReferenceFace -{ - int32 i1, i2; - - b2Vec2 v1, v2; - - b2Vec2 normal; - - b2Vec2 sideNormal1; - float32 sideOffset1; - - b2Vec2 sideNormal2; - float32 sideOffset2; -}; - -// This class collides and edge and a polygon, taking into account edge adjacency. -struct b2EPCollider -{ - void Collide(b2Manifold* manifold, const b2EdgeShape* edgeA, const b2Transform& xfA, - const b2PolygonShape* polygonB, const b2Transform& xfB); - b2EPAxis ComputeEdgeSeparation(); - b2EPAxis ComputePolygonSeparation(); - - enum VertexType - { - e_isolated, - e_concave, - e_convex - }; - - b2TempPolygon m_polygonB; - - b2Transform m_xf; - b2Vec2 m_centroidB; - b2Vec2 m_v0, m_v1, m_v2, m_v3; - b2Vec2 m_normal0, m_normal1, m_normal2; - b2Vec2 m_normal; - VertexType m_type1, m_type2; - b2Vec2 m_lowerLimit, m_upperLimit; - float32 m_radius; - bool m_front; -}; - -// Algorithm: -// 1. Classify v1 and v2 -// 2. Classify polygon centroid as front or back -// 3. Flip normal if necessary -// 4. Initialize normal range to [-pi, pi] about face normal -// 5. Adjust normal range according to adjacent edges -// 6. Visit each separating axes, only accept axes within the range -// 7. Return if _any_ axis indicates separation -// 8. Clip -void b2EPCollider::Collide(b2Manifold* manifold, const b2EdgeShape* edgeA, const b2Transform& xfA, - const b2PolygonShape* polygonB, const b2Transform& xfB) -{ - m_xf = b2MulT(xfA, xfB); - - m_centroidB = b2Mul(m_xf, polygonB->m_centroid); - - m_v0 = edgeA->m_vertex0; - m_v1 = edgeA->m_vertex1; - m_v2 = edgeA->m_vertex2; - m_v3 = edgeA->m_vertex3; - - bool hasVertex0 = edgeA->m_hasVertex0; - bool hasVertex3 = edgeA->m_hasVertex3; - - b2Vec2 edge1 = m_v2 - m_v1; - edge1.Normalize(); - m_normal1.Set(edge1.y, -edge1.x); - float32 offset1 = b2Dot(m_normal1, m_centroidB - m_v1); - float32 offset0 = 0.0f, offset2 = 0.0f; - bool convex1 = false, convex2 = false; - - // Is there a preceding edge? - if (hasVertex0) - { - b2Vec2 edge0 = m_v1 - m_v0; - edge0.Normalize(); - m_normal0.Set(edge0.y, -edge0.x); - convex1 = b2Cross(edge0, edge1) >= 0.0f; - offset0 = b2Dot(m_normal0, m_centroidB - m_v0); - } - - // Is there a following edge? - if (hasVertex3) - { - b2Vec2 edge2 = m_v3 - m_v2; - edge2.Normalize(); - m_normal2.Set(edge2.y, -edge2.x); - convex2 = b2Cross(edge1, edge2) > 0.0f; - offset2 = b2Dot(m_normal2, m_centroidB - m_v2); - } - - // Determine front or back collision. Determine collision normal limits. - if (hasVertex0 && hasVertex3) - { - if (convex1 && convex2) - { - m_front = offset0 >= 0.0f || offset1 >= 0.0f || offset2 >= 0.0f; - if (m_front) - { - m_normal = m_normal1; - m_lowerLimit = m_normal0; - m_upperLimit = m_normal2; - } - else - { - m_normal = -m_normal1; - m_lowerLimit = -m_normal1; - m_upperLimit = -m_normal1; - } - } - else if (convex1) - { - m_front = offset0 >= 0.0f || (offset1 >= 0.0f && offset2 >= 0.0f); - if (m_front) - { - m_normal = m_normal1; - m_lowerLimit = m_normal0; - m_upperLimit = m_normal1; - } - else - { - m_normal = -m_normal1; - m_lowerLimit = -m_normal2; - m_upperLimit = -m_normal1; - } - } - else if (convex2) - { - m_front = offset2 >= 0.0f || (offset0 >= 0.0f && offset1 >= 0.0f); - if (m_front) - { - m_normal = m_normal1; - m_lowerLimit = m_normal1; - m_upperLimit = m_normal2; - } - else - { - m_normal = -m_normal1; - m_lowerLimit = -m_normal1; - m_upperLimit = -m_normal0; - } - } - else - { - m_front = offset0 >= 0.0f && offset1 >= 0.0f && offset2 >= 0.0f; - if (m_front) - { - m_normal = m_normal1; - m_lowerLimit = m_normal1; - m_upperLimit = m_normal1; - } - else - { - m_normal = -m_normal1; - m_lowerLimit = -m_normal2; - m_upperLimit = -m_normal0; - } - } - } - else if (hasVertex0) - { - if (convex1) - { - m_front = offset0 >= 0.0f || offset1 >= 0.0f; - if (m_front) - { - m_normal = m_normal1; - m_lowerLimit = m_normal0; - m_upperLimit = -m_normal1; - } - else - { - m_normal = -m_normal1; - m_lowerLimit = m_normal1; - m_upperLimit = -m_normal1; - } - } - else - { - m_front = offset0 >= 0.0f && offset1 >= 0.0f; - if (m_front) - { - m_normal = m_normal1; - m_lowerLimit = m_normal1; - m_upperLimit = -m_normal1; - } - else - { - m_normal = -m_normal1; - m_lowerLimit = m_normal1; - m_upperLimit = -m_normal0; - } - } - } - else if (hasVertex3) - { - if (convex2) - { - m_front = offset1 >= 0.0f || offset2 >= 0.0f; - if (m_front) - { - m_normal = m_normal1; - m_lowerLimit = -m_normal1; - m_upperLimit = m_normal2; - } - else - { - m_normal = -m_normal1; - m_lowerLimit = -m_normal1; - m_upperLimit = m_normal1; - } - } - else - { - m_front = offset1 >= 0.0f && offset2 >= 0.0f; - if (m_front) - { - m_normal = m_normal1; - m_lowerLimit = -m_normal1; - m_upperLimit = m_normal1; - } - else - { - m_normal = -m_normal1; - m_lowerLimit = -m_normal2; - m_upperLimit = m_normal1; - } - } - } - else - { - m_front = offset1 >= 0.0f; - if (m_front) - { - m_normal = m_normal1; - m_lowerLimit = -m_normal1; - m_upperLimit = -m_normal1; - } - else - { - m_normal = -m_normal1; - m_lowerLimit = m_normal1; - m_upperLimit = m_normal1; - } - } - - // Get polygonB in frameA - m_polygonB.count = polygonB->m_count; - for (int32 i = 0; i < polygonB->m_count; ++i) - { - m_polygonB.vertices[i] = b2Mul(m_xf, polygonB->m_vertices[i]); - m_polygonB.normals[i] = b2Mul(m_xf.q, polygonB->m_normals[i]); - } - - m_radius = 2.0f * b2_polygonRadius; - - manifold->pointCount = 0; - - b2EPAxis edgeAxis = ComputeEdgeSeparation(); - - // If no valid normal can be found than this edge should not collide. - if (edgeAxis.type == b2EPAxis::e_unknown) - { - return; - } - - if (edgeAxis.separation > m_radius) - { - return; - } - - b2EPAxis polygonAxis = ComputePolygonSeparation(); - if (polygonAxis.type != b2EPAxis::e_unknown && polygonAxis.separation > m_radius) - { - return; - } - - // Use hysteresis for jitter reduction. - const float32 k_relativeTol = 0.98f; - const float32 k_absoluteTol = 0.001f; - - b2EPAxis primaryAxis; - if (polygonAxis.type == b2EPAxis::e_unknown) - { - primaryAxis = edgeAxis; - } - else if (polygonAxis.separation > k_relativeTol * edgeAxis.separation + k_absoluteTol) - { - primaryAxis = polygonAxis; - } - else - { - primaryAxis = edgeAxis; - } - - b2ClipVertex ie[2]; - b2ReferenceFace rf; - if (primaryAxis.type == b2EPAxis::e_edgeA) - { - manifold->type = b2Manifold::e_faceA; - - // Search for the polygon normal that is most anti-parallel to the edge normal. - int32 bestIndex = 0; - float32 bestValue = b2Dot(m_normal, m_polygonB.normals[0]); - for (int32 i = 1; i < m_polygonB.count; ++i) - { - float32 value = b2Dot(m_normal, m_polygonB.normals[i]); - if (value < bestValue) - { - bestValue = value; - bestIndex = i; - } - } - - int32 i1 = bestIndex; - int32 i2 = i1 + 1 < m_polygonB.count ? i1 + 1 : 0; - - ie[0].v = m_polygonB.vertices[i1]; - ie[0].id.cf.indexA = 0; - ie[0].id.cf.indexB = static_cast(i1); - ie[0].id.cf.typeA = b2ContactFeature::e_face; - ie[0].id.cf.typeB = b2ContactFeature::e_vertex; - - ie[1].v = m_polygonB.vertices[i2]; - ie[1].id.cf.indexA = 0; - ie[1].id.cf.indexB = static_cast(i2); - ie[1].id.cf.typeA = b2ContactFeature::e_face; - ie[1].id.cf.typeB = b2ContactFeature::e_vertex; - - if (m_front) - { - rf.i1 = 0; - rf.i2 = 1; - rf.v1 = m_v1; - rf.v2 = m_v2; - rf.normal = m_normal1; - } - else - { - rf.i1 = 1; - rf.i2 = 0; - rf.v1 = m_v2; - rf.v2 = m_v1; - rf.normal = -m_normal1; - } - } - else - { - manifold->type = b2Manifold::e_faceB; - - ie[0].v = m_v1; - ie[0].id.cf.indexA = 0; - ie[0].id.cf.indexB = static_cast(primaryAxis.index); - ie[0].id.cf.typeA = b2ContactFeature::e_vertex; - ie[0].id.cf.typeB = b2ContactFeature::e_face; - - ie[1].v = m_v2; - ie[1].id.cf.indexA = 0; - ie[1].id.cf.indexB = static_cast(primaryAxis.index); - ie[1].id.cf.typeA = b2ContactFeature::e_vertex; - ie[1].id.cf.typeB = b2ContactFeature::e_face; - - rf.i1 = primaryAxis.index; - rf.i2 = rf.i1 + 1 < m_polygonB.count ? rf.i1 + 1 : 0; - rf.v1 = m_polygonB.vertices[rf.i1]; - rf.v2 = m_polygonB.vertices[rf.i2]; - rf.normal = m_polygonB.normals[rf.i1]; - } - - rf.sideNormal1.Set(rf.normal.y, -rf.normal.x); - rf.sideNormal2 = -rf.sideNormal1; - rf.sideOffset1 = b2Dot(rf.sideNormal1, rf.v1); - rf.sideOffset2 = b2Dot(rf.sideNormal2, rf.v2); - - // Clip incident edge against extruded edge1 side edges. - b2ClipVertex clipPoints1[2]; - b2ClipVertex clipPoints2[2]; - int32 np; - - // Clip to box side 1 - np = b2ClipSegmentToLine(clipPoints1, ie, rf.sideNormal1, rf.sideOffset1, rf.i1); - - if (np < b2_maxManifoldPoints) - { - return; - } - - // Clip to negative box side 1 - np = b2ClipSegmentToLine(clipPoints2, clipPoints1, rf.sideNormal2, rf.sideOffset2, rf.i2); - - if (np < b2_maxManifoldPoints) - { - return; - } - - // Now clipPoints2 contains the clipped points. - if (primaryAxis.type == b2EPAxis::e_edgeA) - { - manifold->localNormal = rf.normal; - manifold->localPoint = rf.v1; - } - else - { - manifold->localNormal = polygonB->m_normals[rf.i1]; - manifold->localPoint = polygonB->m_vertices[rf.i1]; - } - - int32 pointCount = 0; - for (int32 i = 0; i < b2_maxManifoldPoints; ++i) - { - float32 separation; - - separation = b2Dot(rf.normal, clipPoints2[i].v - rf.v1); - - if (separation <= m_radius) - { - b2ManifoldPoint* cp = manifold->points + pointCount; - - if (primaryAxis.type == b2EPAxis::e_edgeA) - { - cp->localPoint = b2MulT(m_xf, clipPoints2[i].v); - cp->id = clipPoints2[i].id; - } - else - { - cp->localPoint = clipPoints2[i].v; - cp->id.cf.typeA = clipPoints2[i].id.cf.typeB; - cp->id.cf.typeB = clipPoints2[i].id.cf.typeA; - cp->id.cf.indexA = clipPoints2[i].id.cf.indexB; - cp->id.cf.indexB = clipPoints2[i].id.cf.indexA; - } - - ++pointCount; - } - } - - manifold->pointCount = pointCount; -} - -b2EPAxis b2EPCollider::ComputeEdgeSeparation() -{ - b2EPAxis axis; - axis.type = b2EPAxis::e_edgeA; - axis.index = m_front ? 0 : 1; - axis.separation = FLT_MAX; - - for (int32 i = 0; i < m_polygonB.count; ++i) - { - float32 s = b2Dot(m_normal, m_polygonB.vertices[i] - m_v1); - if (s < axis.separation) - { - axis.separation = s; - } - } - - return axis; -} - -b2EPAxis b2EPCollider::ComputePolygonSeparation() -{ - b2EPAxis axis; - axis.type = b2EPAxis::e_unknown; - axis.index = -1; - axis.separation = -FLT_MAX; - - b2Vec2 perp(-m_normal.y, m_normal.x); - - for (int32 i = 0; i < m_polygonB.count; ++i) - { - b2Vec2 n = -m_polygonB.normals[i]; - - float32 s1 = b2Dot(n, m_polygonB.vertices[i] - m_v1); - float32 s2 = b2Dot(n, m_polygonB.vertices[i] - m_v2); - float32 s = b2Min(s1, s2); - - if (s > m_radius) - { - // No collision - axis.type = b2EPAxis::e_edgeB; - axis.index = i; - axis.separation = s; - return axis; - } - - // Adjacency - if (b2Dot(n, perp) >= 0.0f) - { - if (b2Dot(n - m_upperLimit, m_normal) < -b2_angularSlop) - { - continue; - } - } - else - { - if (b2Dot(n - m_lowerLimit, m_normal) < -b2_angularSlop) - { - continue; - } - } - - if (s > axis.separation) - { - axis.type = b2EPAxis::e_edgeB; - axis.index = i; - axis.separation = s; - } - } - - return axis; -} - -void b2CollideEdgeAndPolygon( b2Manifold* manifold, - const b2EdgeShape* edgeA, const b2Transform& xfA, - const b2PolygonShape* polygonB, const b2Transform& xfB) -{ - b2EPCollider collider; - collider.Collide(manifold, edgeA, xfA, polygonB, xfB); -} diff --git a/Box2D/Collision/b2CollidePolygon.cpp b/Box2D/Collision/b2CollidePolygon.cpp deleted file mode 100644 index 0aac38b2ac..0000000000 --- a/Box2D/Collision/b2CollidePolygon.cpp +++ /dev/null @@ -1,239 +0,0 @@ -/* -* 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 -#include - -// Find the max separation between poly1 and poly2 using edge normals from poly1. -static float32 b2FindMaxSeparation(int32* edgeIndex, - const b2PolygonShape* poly1, const b2Transform& xf1, - const b2PolygonShape* poly2, const b2Transform& xf2) -{ - int32 count1 = poly1->m_count; - int32 count2 = poly2->m_count; - const b2Vec2* n1s = poly1->m_normals; - const b2Vec2* v1s = poly1->m_vertices; - const b2Vec2* v2s = poly2->m_vertices; - b2Transform xf = b2MulT(xf2, xf1); - - int32 bestIndex = 0; - float32 maxSeparation = -b2_maxFloat; - for (int32 i = 0; i < count1; ++i) - { - // Get poly1 normal in frame2. - b2Vec2 n = b2Mul(xf.q, n1s[i]); - b2Vec2 v1 = b2Mul(xf, v1s[i]); - - // Find deepest point for normal i. - float32 si = b2_maxFloat; - for (int32 j = 0; j < count2; ++j) - { - float32 sij = b2Dot(n, v2s[j] - v1); - if (sij < si) - { - si = sij; - } - } - - if (si > maxSeparation) - { - maxSeparation = si; - bestIndex = i; - } - } - - *edgeIndex = bestIndex; - return maxSeparation; -} - -static void b2FindIncidentEdge(b2ClipVertex c[2], - const b2PolygonShape* poly1, const b2Transform& xf1, int32 edge1, - const b2PolygonShape* poly2, const b2Transform& xf2) -{ - const b2Vec2* normals1 = poly1->m_normals; - - int32 count2 = poly2->m_count; - const b2Vec2* vertices2 = poly2->m_vertices; - const b2Vec2* normals2 = poly2->m_normals; - - b2Assert(0 <= edge1 && edge1 < poly1->m_count); - - // Get the normal of the reference edge in poly2's frame. - b2Vec2 normal1 = b2MulT(xf2.q, b2Mul(xf1.q, normals1[edge1])); - - // Find the incident edge on poly2. - int32 index = 0; - float32 minDot = b2_maxFloat; - for (int32 i = 0; i < count2; ++i) - { - float32 dot = b2Dot(normal1, normals2[i]); - if (dot < minDot) - { - minDot = dot; - index = i; - } - } - - // Build the clip vertices for the incident edge. - int32 i1 = index; - int32 i2 = i1 + 1 < count2 ? i1 + 1 : 0; - - c[0].v = b2Mul(xf2, vertices2[i1]); - c[0].id.cf.indexA = (uint8)edge1; - c[0].id.cf.indexB = (uint8)i1; - c[0].id.cf.typeA = b2ContactFeature::e_face; - c[0].id.cf.typeB = b2ContactFeature::e_vertex; - - c[1].v = b2Mul(xf2, vertices2[i2]); - c[1].id.cf.indexA = (uint8)edge1; - c[1].id.cf.indexB = (uint8)i2; - c[1].id.cf.typeA = b2ContactFeature::e_face; - c[1].id.cf.typeB = b2ContactFeature::e_vertex; -} - -// Find edge normal of max separation on A - return if separating axis is found -// Find edge normal of max separation on B - return if separation axis is found -// Choose reference edge as min(minA, minB) -// Find incident edge -// Clip - -// The normal points from 1 to 2 -void b2CollidePolygons(b2Manifold* manifold, - const b2PolygonShape* polyA, const b2Transform& xfA, - const b2PolygonShape* polyB, const b2Transform& xfB) -{ - manifold->pointCount = 0; - float32 totalRadius = polyA->m_radius + polyB->m_radius; - - int32 edgeA = 0; - float32 separationA = b2FindMaxSeparation(&edgeA, polyA, xfA, polyB, xfB); - if (separationA > totalRadius) - return; - - int32 edgeB = 0; - float32 separationB = b2FindMaxSeparation(&edgeB, polyB, xfB, polyA, xfA); - if (separationB > totalRadius) - return; - - const b2PolygonShape* poly1; // reference polygon - const b2PolygonShape* poly2; // incident polygon - b2Transform xf1, xf2; - int32 edge1; // reference edge - uint8 flip; - const float32 k_tol = 0.1f * b2_linearSlop; - - if (separationB > separationA + k_tol) - { - poly1 = polyB; - poly2 = polyA; - xf1 = xfB; - xf2 = xfA; - edge1 = edgeB; - manifold->type = b2Manifold::e_faceB; - flip = 1; - } - else - { - poly1 = polyA; - poly2 = polyB; - xf1 = xfA; - xf2 = xfB; - edge1 = edgeA; - manifold->type = b2Manifold::e_faceA; - flip = 0; - } - - b2ClipVertex incidentEdge[2]; - b2FindIncidentEdge(incidentEdge, poly1, xf1, edge1, poly2, xf2); - - int32 count1 = poly1->m_count; - const b2Vec2* vertices1 = poly1->m_vertices; - - int32 iv1 = edge1; - int32 iv2 = edge1 + 1 < count1 ? edge1 + 1 : 0; - - b2Vec2 v11 = vertices1[iv1]; - b2Vec2 v12 = vertices1[iv2]; - - b2Vec2 localTangent = v12 - v11; - localTangent.Normalize(); - - b2Vec2 localNormal = b2Cross(localTangent, 1.0f); - b2Vec2 planePoint = 0.5f * (v11 + v12); - - b2Vec2 tangent = b2Mul(xf1.q, localTangent); - b2Vec2 normal = b2Cross(tangent, 1.0f); - - v11 = b2Mul(xf1, v11); - v12 = b2Mul(xf1, v12); - - // Face offset. - float32 frontOffset = b2Dot(normal, v11); - - // Side offsets, extended by polytope skin thickness. - float32 sideOffset1 = -b2Dot(tangent, v11) + totalRadius; - float32 sideOffset2 = b2Dot(tangent, v12) + totalRadius; - - // Clip incident edge against extruded edge1 side edges. - b2ClipVertex clipPoints1[2]; - b2ClipVertex clipPoints2[2]; - int np; - - // Clip to box side 1 - np = b2ClipSegmentToLine(clipPoints1, incidentEdge, -tangent, sideOffset1, iv1); - - if (np < 2) - return; - - // Clip to negative box side 1 - np = b2ClipSegmentToLine(clipPoints2, clipPoints1, tangent, sideOffset2, iv2); - - if (np < 2) - { - return; - } - - // Now clipPoints2 contains the clipped points. - manifold->localNormal = localNormal; - manifold->localPoint = planePoint; - - int32 pointCount = 0; - for (int32 i = 0; i < b2_maxManifoldPoints; ++i) - { - float32 separation = b2Dot(normal, clipPoints2[i].v) - frontOffset; - - if (separation <= totalRadius) - { - b2ManifoldPoint* cp = manifold->points + pointCount; - cp->localPoint = b2MulT(xf2, clipPoints2[i].v); - cp->id = clipPoints2[i].id; - if (flip) - { - // Swap features - b2ContactFeature cf = cp->id.cf; - cp->id.cf.indexA = cf.indexB; - cp->id.cf.indexB = cf.indexA; - cp->id.cf.typeA = cf.typeB; - cp->id.cf.typeB = cf.typeA; - } - ++pointCount; - } - } - - manifold->pointCount = pointCount; -} diff --git a/Box2D/Collision/b2Collision.cpp b/Box2D/Collision/b2Collision.cpp deleted file mode 100644 index 4ebf35565e..0000000000 --- a/Box2D/Collision/b2Collision.cpp +++ /dev/null @@ -1,252 +0,0 @@ -/* -* Copyright (c) 2007-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 -#include - -void b2WorldManifold::Initialize(const b2Manifold* manifold, - const b2Transform& xfA, float32 radiusA, - const b2Transform& xfB, float32 radiusB) -{ - if (manifold->pointCount == 0) - { - return; - } - - switch (manifold->type) - { - case b2Manifold::e_circles: - { - normal.Set(1.0f, 0.0f); - b2Vec2 pointA = b2Mul(xfA, manifold->localPoint); - b2Vec2 pointB = b2Mul(xfB, manifold->points[0].localPoint); - if (b2DistanceSquared(pointA, pointB) > b2_epsilon * b2_epsilon) - { - normal = pointB - pointA; - normal.Normalize(); - } - - b2Vec2 cA = pointA + radiusA * normal; - b2Vec2 cB = pointB - radiusB * normal; - points[0] = 0.5f * (cA + cB); - separations[0] = b2Dot(cB - cA, normal); - } - break; - - case b2Manifold::e_faceA: - { - normal = b2Mul(xfA.q, manifold->localNormal); - b2Vec2 planePoint = b2Mul(xfA, manifold->localPoint); - - for (int32 i = 0; i < manifold->pointCount; ++i) - { - b2Vec2 clipPoint = b2Mul(xfB, manifold->points[i].localPoint); - b2Vec2 cA = clipPoint + (radiusA - b2Dot(clipPoint - planePoint, normal)) * normal; - b2Vec2 cB = clipPoint - radiusB * normal; - points[i] = 0.5f * (cA + cB); - separations[i] = b2Dot(cB - cA, normal); - } - } - break; - - case b2Manifold::e_faceB: - { - normal = b2Mul(xfB.q, manifold->localNormal); - b2Vec2 planePoint = b2Mul(xfB, manifold->localPoint); - - for (int32 i = 0; i < manifold->pointCount; ++i) - { - b2Vec2 clipPoint = b2Mul(xfA, manifold->points[i].localPoint); - b2Vec2 cB = clipPoint + (radiusB - b2Dot(clipPoint - planePoint, normal)) * normal; - b2Vec2 cA = clipPoint - radiusA * normal; - points[i] = 0.5f * (cA + cB); - separations[i] = b2Dot(cA - cB, normal); - } - - // Ensure normal points from A to B. - normal = -normal; - } - break; - } -} - -void b2GetPointStates(b2PointState state1[b2_maxManifoldPoints], b2PointState state2[b2_maxManifoldPoints], - const b2Manifold* manifold1, const b2Manifold* manifold2) -{ - for (int32 i = 0; i < b2_maxManifoldPoints; ++i) - { - state1[i] = b2_nullState; - state2[i] = b2_nullState; - } - - // Detect persists and removes. - for (int32 i = 0; i < manifold1->pointCount; ++i) - { - b2ContactID id = manifold1->points[i].id; - - state1[i] = b2_removeState; - - for (int32 j = 0; j < manifold2->pointCount; ++j) - { - if (manifold2->points[j].id.key == id.key) - { - state1[i] = b2_persistState; - break; - } - } - } - - // Detect persists and adds. - for (int32 i = 0; i < manifold2->pointCount; ++i) - { - b2ContactID id = manifold2->points[i].id; - - state2[i] = b2_addState; - - for (int32 j = 0; j < manifold1->pointCount; ++j) - { - if (manifold1->points[j].id.key == id.key) - { - state2[i] = b2_persistState; - break; - } - } - } -} - -// From Real-time Collision Detection, p179. -bool b2AABB::RayCast(b2RayCastOutput* output, const b2RayCastInput& input) const -{ - float32 tmin = -b2_maxFloat; - float32 tmax = b2_maxFloat; - - b2Vec2 p = input.p1; - b2Vec2 d = input.p2 - input.p1; - b2Vec2 absD = b2Abs(d); - - b2Vec2 normal; - - for (int32 i = 0; i < 2; ++i) - { - if (absD(i) < b2_epsilon) - { - // Parallel. - if (p(i) < lowerBound(i) || upperBound(i) < p(i)) - { - return false; - } - } - else - { - float32 inv_d = 1.0f / d(i); - float32 t1 = (lowerBound(i) - p(i)) * inv_d; - float32 t2 = (upperBound(i) - p(i)) * inv_d; - - // Sign of the normal vector. - float32 s = -1.0f; - - if (t1 > t2) - { - b2Swap(t1, t2); - s = 1.0f; - } - - // Push the min up - if (t1 > tmin) - { - normal.SetZero(); - normal(i) = s; - tmin = t1; - } - - // Pull the max down - tmax = b2Min(tmax, t2); - - if (tmin > tmax) - { - return false; - } - } - } - - // Does the ray start inside the box? - // Does the ray intersect beyond the max fraction? - if (tmin < 0.0f || input.maxFraction < tmin) - { - return false; - } - - // Intersection. - output->fraction = tmin; - output->normal = normal; - return true; -} - -// Sutherland-Hodgman clipping. -int32 b2ClipSegmentToLine(b2ClipVertex vOut[2], const b2ClipVertex vIn[2], - const b2Vec2& normal, float32 offset, int32 vertexIndexA) -{ - // Start with no output points - int32 numOut = 0; - - // Calculate the distance of end points to the line - float32 distance0 = b2Dot(normal, vIn[0].v) - offset; - float32 distance1 = b2Dot(normal, vIn[1].v) - offset; - - // If the points are behind the plane - if (distance0 <= 0.0f) vOut[numOut++] = vIn[0]; - if (distance1 <= 0.0f) vOut[numOut++] = vIn[1]; - - // If the points are on different sides of the plane - if (distance0 * distance1 < 0.0f) - { - // Find intersection point of edge and plane - float32 interp = distance0 / (distance0 - distance1); - vOut[numOut].v = vIn[0].v + interp * (vIn[1].v - vIn[0].v); - - // VertexA is hitting edgeB. - vOut[numOut].id.cf.indexA = static_cast(vertexIndexA); - vOut[numOut].id.cf.indexB = vIn[0].id.cf.indexB; - vOut[numOut].id.cf.typeA = b2ContactFeature::e_vertex; - vOut[numOut].id.cf.typeB = b2ContactFeature::e_face; - ++numOut; - } - - return numOut; -} - -bool b2TestOverlap( const b2Shape* shapeA, int32 indexA, - const b2Shape* shapeB, int32 indexB, - const b2Transform& xfA, const b2Transform& xfB) -{ - b2DistanceInput input; - input.proxyA.Set(shapeA, indexA); - input.proxyB.Set(shapeB, indexB); - input.transformA = xfA; - input.transformB = xfB; - input.useRadii = true; - - b2SimplexCache cache; - cache.count = 0; - - b2DistanceOutput output; - - b2Distance(&output, &cache, &input); - - return output.distance < 10.0f * b2_epsilon; -} diff --git a/Box2D/Collision/b2Collision.h b/Box2D/Collision/b2Collision.h deleted file mode 100644 index ad023b3caa..0000000000 --- a/Box2D/Collision/b2Collision.h +++ /dev/null @@ -1,277 +0,0 @@ -/* -* 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_COLLISION_H -#define B2_COLLISION_H - -#include -#include - -/// @file -/// Structures and functions used for computing contact points, distance -/// queries, and TOI queries. - -class b2Shape; -class b2CircleShape; -class b2EdgeShape; -class b2PolygonShape; - -const uint8 b2_nullFeature = UCHAR_MAX; - -/// The features that intersect to form the contact point -/// This must be 4 bytes or less. -struct b2ContactFeature -{ - enum Type - { - e_vertex = 0, - e_face = 1 - }; - - uint8 indexA; ///< Feature index on shapeA - uint8 indexB; ///< Feature index on shapeB - uint8 typeA; ///< The feature type on shapeA - uint8 typeB; ///< The feature type on shapeB -}; - -/// Contact ids to facilitate warm starting. -union b2ContactID -{ - b2ContactFeature cf; - uint32 key; ///< Used to quickly compare contact ids. -}; - -/// A manifold point is a contact point belonging to a contact -/// manifold. It holds details related to the geometry and dynamics -/// of the contact points. -/// The local point usage depends on the manifold type: -/// -e_circles: the local center of circleB -/// -e_faceA: the local center of cirlceB or the clip point of polygonB -/// -e_faceB: the clip point of polygonA -/// This structure is stored across time steps, so we keep it small. -/// Note: the impulses are used for internal caching and may not -/// provide reliable contact forces, especially for high speed collisions. -struct b2ManifoldPoint -{ - b2Vec2 localPoint; ///< usage depends on manifold type - float32 normalImpulse; ///< the non-penetration impulse - float32 tangentImpulse; ///< the friction impulse - b2ContactID id; ///< uniquely identifies a contact point between two shapes -}; - -/// A manifold for two touching convex shapes. -/// Box2D supports multiple types of contact: -/// - clip point versus plane with radius -/// - point versus point with radius (circles) -/// The local point usage depends on the manifold type: -/// -e_circles: the local center of circleA -/// -e_faceA: the center of faceA -/// -e_faceB: the center of faceB -/// Similarly the local normal usage: -/// -e_circles: not used -/// -e_faceA: the normal on polygonA -/// -e_faceB: the normal on polygonB -/// We store contacts in this way so that position correction can -/// account for movement, which is critical for continuous physics. -/// All contact scenarios must be expressed in one of these types. -/// This structure is stored across time steps, so we keep it small. -struct b2Manifold -{ - enum Type - { - e_circles, - e_faceA, - e_faceB - }; - - b2ManifoldPoint points[b2_maxManifoldPoints]; ///< the points of contact - b2Vec2 localNormal; ///< not use for Type::e_points - b2Vec2 localPoint; ///< usage depends on manifold type - Type type; - int32 pointCount; ///< the number of manifold points -}; - -/// This is used to compute the current state of a contact manifold. -struct b2WorldManifold -{ - /// Evaluate the manifold with supplied transforms. This assumes - /// modest motion from the original state. This does not change the - /// point count, impulses, etc. The radii must come from the shapes - /// that generated the manifold. - void Initialize(const b2Manifold* manifold, - const b2Transform& xfA, float32 radiusA, - const b2Transform& xfB, float32 radiusB); - - b2Vec2 normal; ///< world vector pointing from A to B - b2Vec2 points[b2_maxManifoldPoints]; ///< world contact point (point of intersection) - float32 separations[b2_maxManifoldPoints]; ///< a negative value indicates overlap, in meters -}; - -/// This is used for determining the state of contact points. -enum b2PointState -{ - b2_nullState, ///< point does not exist - b2_addState, ///< point was added in the update - b2_persistState, ///< point persisted across the update - b2_removeState ///< point was removed in the update -}; - -/// Compute the point states given two manifolds. The states pertain to the transition from manifold1 -/// to manifold2. So state1 is either persist or remove while state2 is either add or persist. -void b2GetPointStates(b2PointState state1[b2_maxManifoldPoints], b2PointState state2[b2_maxManifoldPoints], - const b2Manifold* manifold1, const b2Manifold* manifold2); - -/// Used for computing contact manifolds. -struct b2ClipVertex -{ - b2Vec2 v; - b2ContactID id; -}; - -/// Ray-cast input data. The ray extends from p1 to p1 + maxFraction * (p2 - p1). -struct b2RayCastInput -{ - b2Vec2 p1, p2; - float32 maxFraction; -}; - -/// Ray-cast output data. The ray hits at p1 + fraction * (p2 - p1), where p1 and p2 -/// come from b2RayCastInput. -struct b2RayCastOutput -{ - b2Vec2 normal; - float32 fraction; -}; - -/// An axis aligned bounding box. -struct b2AABB -{ - /// Verify that the bounds are sorted. - bool IsValid() const; - - /// Get the center of the AABB. - b2Vec2 GetCenter() const - { - return 0.5f * (lowerBound + upperBound); - } - - /// Get the extents of the AABB (half-widths). - b2Vec2 GetExtents() const - { - return 0.5f * (upperBound - lowerBound); - } - - /// Get the perimeter length - float32 GetPerimeter() const - { - float32 wx = upperBound.x - lowerBound.x; - float32 wy = upperBound.y - lowerBound.y; - return 2.0f * (wx + wy); - } - - /// Combine an AABB into this one. - void Combine(const b2AABB& aabb) - { - lowerBound = b2Min(lowerBound, aabb.lowerBound); - upperBound = b2Max(upperBound, aabb.upperBound); - } - - /// Combine two AABBs into this one. - void Combine(const b2AABB& aabb1, const b2AABB& aabb2) - { - lowerBound = b2Min(aabb1.lowerBound, aabb2.lowerBound); - upperBound = b2Max(aabb1.upperBound, aabb2.upperBound); - } - - /// Does this aabb contain the provided AABB. - bool Contains(const b2AABB& aabb) const - { - bool result = true; - result = result && lowerBound.x <= aabb.lowerBound.x; - result = result && lowerBound.y <= aabb.lowerBound.y; - result = result && aabb.upperBound.x <= upperBound.x; - result = result && aabb.upperBound.y <= upperBound.y; - return result; - } - - bool RayCast(b2RayCastOutput* output, const b2RayCastInput& input) const; - - b2Vec2 lowerBound; ///< the lower vertex - b2Vec2 upperBound; ///< the upper vertex -}; - -/// Compute the collision manifold between two circles. -void b2CollideCircles(b2Manifold* manifold, - const b2CircleShape* circleA, const b2Transform& xfA, - const b2CircleShape* circleB, const b2Transform& xfB); - -/// Compute the collision manifold between a polygon and a circle. -void b2CollidePolygonAndCircle(b2Manifold* manifold, - const b2PolygonShape* polygonA, const b2Transform& xfA, - const b2CircleShape* circleB, const b2Transform& xfB); - -/// Compute the collision manifold between two polygons. -void b2CollidePolygons(b2Manifold* manifold, - const b2PolygonShape* polygonA, const b2Transform& xfA, - const b2PolygonShape* polygonB, const b2Transform& xfB); - -/// Compute the collision manifold between an edge and a circle. -void b2CollideEdgeAndCircle(b2Manifold* manifold, - const b2EdgeShape* polygonA, const b2Transform& xfA, - const b2CircleShape* circleB, const b2Transform& xfB); - -/// Compute the collision manifold between an edge and a circle. -void b2CollideEdgeAndPolygon(b2Manifold* manifold, - const b2EdgeShape* edgeA, const b2Transform& xfA, - const b2PolygonShape* circleB, const b2Transform& xfB); - -/// Clipping for contact manifolds. -int32 b2ClipSegmentToLine(b2ClipVertex vOut[2], const b2ClipVertex vIn[2], - const b2Vec2& normal, float32 offset, int32 vertexIndexA); - -/// Determine if two generic shapes overlap. -bool b2TestOverlap( const b2Shape* shapeA, int32 indexA, - const b2Shape* shapeB, int32 indexB, - const b2Transform& xfA, const b2Transform& xfB); - -// ---------------- Inline Functions ------------------------------------------ - -inline bool b2AABB::IsValid() const -{ - b2Vec2 d = upperBound - lowerBound; - bool valid = d.x >= 0.0f && d.y >= 0.0f; - valid = valid && lowerBound.IsValid() && upperBound.IsValid(); - return valid; -} - -inline bool b2TestOverlap(const b2AABB& a, const b2AABB& b) -{ - b2Vec2 d1, d2; - d1 = b.lowerBound - a.upperBound; - d2 = a.lowerBound - b.upperBound; - - if (d1.x > 0.0f || d1.y > 0.0f) - return false; - - if (d2.x > 0.0f || d2.y > 0.0f) - return false; - - return true; -} - -#endif diff --git a/Box2D/Collision/b2Distance.cpp b/Box2D/Collision/b2Distance.cpp deleted file mode 100644 index 78daab3e2e..0000000000 --- a/Box2D/Collision/b2Distance.cpp +++ /dev/null @@ -1,603 +0,0 @@ -/* -* Copyright (c) 2007-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 -#include -#include -#include -#include - -// GJK using Voronoi regions (Christer Ericson) and Barycentric coordinates. -int32 b2_gjkCalls, b2_gjkIters, b2_gjkMaxIters; - -void b2DistanceProxy::Set(const b2Shape* shape, int32 index) -{ - switch (shape->GetType()) - { - case b2Shape::e_circle: - { - const b2CircleShape* circle = static_cast(shape); - m_vertices = &circle->m_p; - m_count = 1; - m_radius = circle->m_radius; - } - break; - - case b2Shape::e_polygon: - { - const b2PolygonShape* polygon = static_cast(shape); - m_vertices = polygon->m_vertices; - m_count = polygon->m_count; - m_radius = polygon->m_radius; - } - break; - - case b2Shape::e_chain: - { - const b2ChainShape* chain = static_cast(shape); - b2Assert(0 <= index && index < chain->m_count); - - m_buffer[0] = chain->m_vertices[index]; - if (index + 1 < chain->m_count) - { - m_buffer[1] = chain->m_vertices[index + 1]; - } - else - { - m_buffer[1] = chain->m_vertices[0]; - } - - m_vertices = m_buffer; - m_count = 2; - m_radius = chain->m_radius; - } - break; - - case b2Shape::e_edge: - { - const b2EdgeShape* edge = static_cast(shape); - m_vertices = &edge->m_vertex1; - m_count = 2; - m_radius = edge->m_radius; - } - break; - - default: - b2Assert(false); - } -} - - -struct b2SimplexVertex -{ - b2Vec2 wA; // support point in proxyA - b2Vec2 wB; // support point in proxyB - b2Vec2 w; // wB - wA - float32 a; // barycentric coordinate for closest point - int32 indexA; // wA index - int32 indexB; // wB index -}; - -struct b2Simplex -{ - void ReadCache( const b2SimplexCache* cache, - const b2DistanceProxy* proxyA, const b2Transform& transformA, - const b2DistanceProxy* proxyB, const b2Transform& transformB) - { - b2Assert(cache->count <= 3); - - // Copy data from cache. - m_count = cache->count; - b2SimplexVertex* vertices = &m_v1; - for (int32 i = 0; i < m_count; ++i) - { - b2SimplexVertex* v = vertices + i; - v->indexA = cache->indexA[i]; - v->indexB = cache->indexB[i]; - b2Vec2 wALocal = proxyA->GetVertex(v->indexA); - b2Vec2 wBLocal = proxyB->GetVertex(v->indexB); - v->wA = b2Mul(transformA, wALocal); - v->wB = b2Mul(transformB, wBLocal); - v->w = v->wB - v->wA; - v->a = 0.0f; - } - - // Compute the new simplex metric, if it is substantially different than - // old metric then flush the simplex. - if (m_count > 1) - { - float32 metric1 = cache->metric; - float32 metric2 = GetMetric(); - if (metric2 < 0.5f * metric1 || 2.0f * metric1 < metric2 || metric2 < b2_epsilon) - { - // Reset the simplex. - m_count = 0; - } - } - - // If the cache is empty or invalid ... - if (m_count == 0) - { - b2SimplexVertex* v = vertices + 0; - v->indexA = 0; - v->indexB = 0; - b2Vec2 wALocal = proxyA->GetVertex(0); - b2Vec2 wBLocal = proxyB->GetVertex(0); - v->wA = b2Mul(transformA, wALocal); - v->wB = b2Mul(transformB, wBLocal); - v->w = v->wB - v->wA; - v->a = 1.0f; - m_count = 1; - } - } - - void WriteCache(b2SimplexCache* cache) const - { - cache->metric = GetMetric(); - cache->count = uint16(m_count); - const b2SimplexVertex* vertices = &m_v1; - for (int32 i = 0; i < m_count; ++i) - { - cache->indexA[i] = uint8(vertices[i].indexA); - cache->indexB[i] = uint8(vertices[i].indexB); - } - } - - b2Vec2 GetSearchDirection() const - { - switch (m_count) - { - case 1: - return -m_v1.w; - - case 2: - { - b2Vec2 e12 = m_v2.w - m_v1.w; - float32 sgn = b2Cross(e12, -m_v1.w); - if (sgn > 0.0f) - { - // Origin is left of e12. - return b2Cross(1.0f, e12); - } - else - { - // Origin is right of e12. - return b2Cross(e12, 1.0f); - } - } - - default: - b2Assert(false); - return b2Vec2_zero; - } - } - - b2Vec2 GetClosestPoint() const - { - switch (m_count) - { - case 0: - b2Assert(false); - return b2Vec2_zero; - - case 1: - return m_v1.w; - - case 2: - return m_v1.a * m_v1.w + m_v2.a * m_v2.w; - - case 3: - return b2Vec2_zero; - - default: - b2Assert(false); - return b2Vec2_zero; - } - } - - void GetWitnessPoints(b2Vec2* pA, b2Vec2* pB) const - { - switch (m_count) - { - case 0: - b2Assert(false); - break; - - case 1: - *pA = m_v1.wA; - *pB = m_v1.wB; - break; - - case 2: - *pA = m_v1.a * m_v1.wA + m_v2.a * m_v2.wA; - *pB = m_v1.a * m_v1.wB + m_v2.a * m_v2.wB; - break; - - case 3: - *pA = m_v1.a * m_v1.wA + m_v2.a * m_v2.wA + m_v3.a * m_v3.wA; - *pB = *pA; - break; - - default: - b2Assert(false); - break; - } - } - - float32 GetMetric() const - { - switch (m_count) - { - case 0: - b2Assert(false); - return 0.0f; - - case 1: - return 0.0f; - - case 2: - return b2Distance(m_v1.w, m_v2.w); - - case 3: - return b2Cross(m_v2.w - m_v1.w, m_v3.w - m_v1.w); - - default: - b2Assert(false); - return 0.0f; - } - } - - void Solve2(); - void Solve3(); - - b2SimplexVertex m_v1, m_v2, m_v3; - int32 m_count; -}; - - -// Solve a line segment using barycentric coordinates. -// -// p = a1 * w1 + a2 * w2 -// a1 + a2 = 1 -// -// The vector from the origin to the closest point on the line is -// perpendicular to the line. -// e12 = w2 - w1 -// dot(p, e) = 0 -// a1 * dot(w1, e) + a2 * dot(w2, e) = 0 -// -// 2-by-2 linear system -// [1 1 ][a1] = [1] -// [w1.e12 w2.e12][a2] = [0] -// -// Define -// d12_1 = dot(w2, e12) -// d12_2 = -dot(w1, e12) -// d12 = d12_1 + d12_2 -// -// Solution -// a1 = d12_1 / d12 -// a2 = d12_2 / d12 -void b2Simplex::Solve2() -{ - b2Vec2 w1 = m_v1.w; - b2Vec2 w2 = m_v2.w; - b2Vec2 e12 = w2 - w1; - - // w1 region - float32 d12_2 = -b2Dot(w1, e12); - if (d12_2 <= 0.0f) - { - // a2 <= 0, so we clamp it to 0 - m_v1.a = 1.0f; - m_count = 1; - return; - } - - // w2 region - float32 d12_1 = b2Dot(w2, e12); - if (d12_1 <= 0.0f) - { - // a1 <= 0, so we clamp it to 0 - m_v2.a = 1.0f; - m_count = 1; - m_v1 = m_v2; - return; - } - - // Must be in e12 region. - float32 inv_d12 = 1.0f / (d12_1 + d12_2); - m_v1.a = d12_1 * inv_d12; - m_v2.a = d12_2 * inv_d12; - m_count = 2; -} - -// Possible regions: -// - points[2] -// - edge points[0]-points[2] -// - edge points[1]-points[2] -// - inside the triangle -void b2Simplex::Solve3() -{ - b2Vec2 w1 = m_v1.w; - b2Vec2 w2 = m_v2.w; - b2Vec2 w3 = m_v3.w; - - // Edge12 - // [1 1 ][a1] = [1] - // [w1.e12 w2.e12][a2] = [0] - // a3 = 0 - b2Vec2 e12 = w2 - w1; - float32 w1e12 = b2Dot(w1, e12); - float32 w2e12 = b2Dot(w2, e12); - float32 d12_1 = w2e12; - float32 d12_2 = -w1e12; - - // Edge13 - // [1 1 ][a1] = [1] - // [w1.e13 w3.e13][a3] = [0] - // a2 = 0 - b2Vec2 e13 = w3 - w1; - float32 w1e13 = b2Dot(w1, e13); - float32 w3e13 = b2Dot(w3, e13); - float32 d13_1 = w3e13; - float32 d13_2 = -w1e13; - - // Edge23 - // [1 1 ][a2] = [1] - // [w2.e23 w3.e23][a3] = [0] - // a1 = 0 - b2Vec2 e23 = w3 - w2; - float32 w2e23 = b2Dot(w2, e23); - float32 w3e23 = b2Dot(w3, e23); - float32 d23_1 = w3e23; - float32 d23_2 = -w2e23; - - // Triangle123 - float32 n123 = b2Cross(e12, e13); - - float32 d123_1 = n123 * b2Cross(w2, w3); - float32 d123_2 = n123 * b2Cross(w3, w1); - float32 d123_3 = n123 * b2Cross(w1, w2); - - // w1 region - if (d12_2 <= 0.0f && d13_2 <= 0.0f) - { - m_v1.a = 1.0f; - m_count = 1; - return; - } - - // e12 - if (d12_1 > 0.0f && d12_2 > 0.0f && d123_3 <= 0.0f) - { - float32 inv_d12 = 1.0f / (d12_1 + d12_2); - m_v1.a = d12_1 * inv_d12; - m_v2.a = d12_2 * inv_d12; - m_count = 2; - return; - } - - // e13 - if (d13_1 > 0.0f && d13_2 > 0.0f && d123_2 <= 0.0f) - { - float32 inv_d13 = 1.0f / (d13_1 + d13_2); - m_v1.a = d13_1 * inv_d13; - m_v3.a = d13_2 * inv_d13; - m_count = 2; - m_v2 = m_v3; - return; - } - - // w2 region - if (d12_1 <= 0.0f && d23_2 <= 0.0f) - { - m_v2.a = 1.0f; - m_count = 1; - m_v1 = m_v2; - return; - } - - // w3 region - if (d13_1 <= 0.0f && d23_1 <= 0.0f) - { - m_v3.a = 1.0f; - m_count = 1; - m_v1 = m_v3; - return; - } - - // e23 - if (d23_1 > 0.0f && d23_2 > 0.0f && d123_1 <= 0.0f) - { - float32 inv_d23 = 1.0f / (d23_1 + d23_2); - m_v2.a = d23_1 * inv_d23; - m_v3.a = d23_2 * inv_d23; - m_count = 2; - m_v1 = m_v3; - return; - } - - // Must be in triangle123 - float32 inv_d123 = 1.0f / (d123_1 + d123_2 + d123_3); - m_v1.a = d123_1 * inv_d123; - m_v2.a = d123_2 * inv_d123; - m_v3.a = d123_3 * inv_d123; - m_count = 3; -} - -void b2Distance(b2DistanceOutput* output, - b2SimplexCache* cache, - const b2DistanceInput* input) -{ - ++b2_gjkCalls; - - const b2DistanceProxy* proxyA = &input->proxyA; - const b2DistanceProxy* proxyB = &input->proxyB; - - b2Transform transformA = input->transformA; - b2Transform transformB = input->transformB; - - // Initialize the simplex. - b2Simplex simplex; - simplex.ReadCache(cache, proxyA, transformA, proxyB, transformB); - - // Get simplex vertices as an array. - b2SimplexVertex* vertices = &simplex.m_v1; - const int32 k_maxIters = 20; - - // These store the vertices of the last simplex so that we - // can check for duplicates and prevent cycling. - int32 saveA[3], saveB[3]; - int32 saveCount = 0; - - float32 distanceSqr1 = b2_maxFloat; - float32 distanceSqr2 = distanceSqr1; - - // Main iteration loop. - int32 iter = 0; - while (iter < k_maxIters) - { - // Copy simplex so we can identify duplicates. - saveCount = simplex.m_count; - for (int32 i = 0; i < saveCount; ++i) - { - saveA[i] = vertices[i].indexA; - saveB[i] = vertices[i].indexB; - } - - switch (simplex.m_count) - { - case 1: - break; - - case 2: - simplex.Solve2(); - break; - - case 3: - simplex.Solve3(); - break; - - default: - b2Assert(false); - } - - // If we have 3 points, then the origin is in the corresponding triangle. - if (simplex.m_count == 3) - { - break; - } - - // Compute closest point. - b2Vec2 p = simplex.GetClosestPoint(); - distanceSqr2 = p.LengthSquared(); - - // Ensure progress - if (distanceSqr2 >= distanceSqr1) - { - //break; - } - distanceSqr1 = distanceSqr2; - - // Get search direction. - b2Vec2 d = simplex.GetSearchDirection(); - - // Ensure the search direction is numerically fit. - if (d.LengthSquared() < b2_epsilon * b2_epsilon) - { - // The origin is probably contained by a line segment - // or triangle. Thus the shapes are overlapped. - - // We can't return zero here even though there may be overlap. - // In case the simplex is a point, segment, or triangle it is difficult - // to determine if the origin is contained in the CSO or very close to it. - break; - } - - // Compute a tentative new simplex vertex using support points. - b2SimplexVertex* vertex = vertices + simplex.m_count; - vertex->indexA = proxyA->GetSupport(b2MulT(transformA.q, -d)); - vertex->wA = b2Mul(transformA, proxyA->GetVertex(vertex->indexA)); - b2Vec2 wBLocal; - vertex->indexB = proxyB->GetSupport(b2MulT(transformB.q, d)); - vertex->wB = b2Mul(transformB, proxyB->GetVertex(vertex->indexB)); - vertex->w = vertex->wB - vertex->wA; - - // Iteration count is equated to the number of support point calls. - ++iter; - ++b2_gjkIters; - - // Check for duplicate support points. This is the main termination criteria. - bool duplicate = false; - for (int32 i = 0; i < saveCount; ++i) - { - if (vertex->indexA == saveA[i] && vertex->indexB == saveB[i]) - { - duplicate = true; - break; - } - } - - // If we found a duplicate support point we must exit to avoid cycling. - if (duplicate) - { - break; - } - - // New vertex is ok and needed. - ++simplex.m_count; - } - - b2_gjkMaxIters = b2Max(b2_gjkMaxIters, iter); - - // Prepare output. - simplex.GetWitnessPoints(&output->pointA, &output->pointB); - output->distance = b2Distance(output->pointA, output->pointB); - output->iterations = iter; - - // Cache the simplex. - simplex.WriteCache(cache); - - // Apply radii if requested. - if (input->useRadii) - { - float32 rA = proxyA->m_radius; - float32 rB = proxyB->m_radius; - - if (output->distance > rA + rB && output->distance > b2_epsilon) - { - // Shapes are still no overlapped. - // Move the witness points to the outer surface. - output->distance -= rA + rB; - b2Vec2 normal = output->pointB - output->pointA; - normal.Normalize(); - output->pointA += rA * normal; - output->pointB -= rB * normal; - } - else - { - // Shapes are overlapped when radii are considered. - // Move the witness points to the middle. - b2Vec2 p = 0.5f * (output->pointA + output->pointB); - output->pointA = p; - output->pointB = p; - output->distance = 0.0f; - } - } -} diff --git a/Box2D/Collision/b2Distance.h b/Box2D/Collision/b2Distance.h deleted file mode 100644 index 7bae689d15..0000000000 --- a/Box2D/Collision/b2Distance.h +++ /dev/null @@ -1,141 +0,0 @@ - -/* -* 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_DISTANCE_H -#define B2_DISTANCE_H - -#include - -class b2Shape; - -/// A distance proxy is used by the GJK algorithm. -/// It encapsulates any shape. -struct b2DistanceProxy -{ - b2DistanceProxy() : m_vertices(NULL), m_count(0), m_radius(0.0f) {} - - /// Initialize the proxy using the given shape. The shape - /// must remain in scope while the proxy is in use. - void Set(const b2Shape* shape, int32 index); - - /// 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; - - /// Get a vertex by index. Used by b2Distance. - const b2Vec2& GetVertex(int32 index) const; - - b2Vec2 m_buffer[2]; - const b2Vec2* m_vertices; - int32 m_count; - float32 m_radius; -}; - -/// Used to warm start b2Distance. -/// Set count to zero on first call. -struct b2SimplexCache -{ - float32 metric; ///< length or area - uint16 count; - uint8 indexA[3]; ///< vertices on shape A - uint8 indexB[3]; ///< vertices on shape B -}; - -/// Input for b2Distance. -/// You have to option to use the shape radii -/// in the computation. Even -struct b2DistanceInput -{ - b2DistanceProxy proxyA; - b2DistanceProxy proxyB; - b2Transform transformA; - b2Transform transformB; - bool useRadii; -}; - -/// Output for b2Distance. -struct b2DistanceOutput -{ - b2Vec2 pointA; ///< closest point on shapeA - b2Vec2 pointB; ///< closest point on shapeB - float32 distance; - int32 iterations; ///< number of GJK iterations used -}; - -/// Compute the closest points between two shapes. Supports any combination of: -/// b2CircleShape, b2PolygonShape, b2EdgeShape. The simplex cache is input/output. -/// On the first call set b2SimplexCache.count to zero. -void b2Distance(b2DistanceOutput* output, - b2SimplexCache* cache, - const b2DistanceInput* input); - - -////////////////////////////////////////////////////////////////////////// - -inline int32 b2DistanceProxy::GetVertexCount() const -{ - return m_count; -} - -inline const b2Vec2& b2DistanceProxy::GetVertex(int32 index) const -{ - b2Assert(0 <= index && index < m_count); - return m_vertices[index]; -} - -inline int32 b2DistanceProxy::GetSupport(const b2Vec2& d) const -{ - int32 bestIndex = 0; - float32 bestValue = b2Dot(m_vertices[0], d); - for (int32 i = 1; i < m_count; ++i) - { - float32 value = b2Dot(m_vertices[i], d); - if (value > bestValue) - { - bestIndex = i; - bestValue = value; - } - } - - return bestIndex; -} - -inline const b2Vec2& b2DistanceProxy::GetSupportVertex(const b2Vec2& d) const -{ - int32 bestIndex = 0; - float32 bestValue = b2Dot(m_vertices[0], d); - for (int32 i = 1; i < m_count; ++i) - { - float32 value = b2Dot(m_vertices[i], d); - if (value > bestValue) - { - bestIndex = i; - bestValue = value; - } - } - - return m_vertices[bestIndex]; -} - -#endif diff --git a/Box2D/Collision/b2DynamicTree.cpp b/Box2D/Collision/b2DynamicTree.cpp deleted file mode 100644 index 2607c6beee..0000000000 --- a/Box2D/Collision/b2DynamicTree.cpp +++ /dev/null @@ -1,779 +0,0 @@ -/* -* Copyright (c) 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 -#include -#include - -b2DynamicTree::b2DynamicTree() -{ - m_root = b2_nullNode; - - m_nodeCapacity = 16; - m_nodeCount = 0; - m_nodes = (b2TreeNode*)b2Alloc(m_nodeCapacity * sizeof(b2TreeNode)); - memset(m_nodes, 0, m_nodeCapacity * sizeof(b2TreeNode)); - - // Build a linked list for the free list. - for (int32 i = 0; i < m_nodeCapacity - 1; ++i) - { - m_nodes[i].next = i + 1; - m_nodes[i].height = -1; - } - m_nodes[m_nodeCapacity-1].next = b2_nullNode; - m_nodes[m_nodeCapacity-1].height = -1; - m_freeList = 0; - - m_path = 0; - - m_insertionCount = 0; -} - -b2DynamicTree::~b2DynamicTree() -{ - // This frees the entire tree in one shot. - b2Free(m_nodes); -} - -// Allocate a node from the pool. Grow the pool if necessary. -int32 b2DynamicTree::AllocateNode() -{ - // Expand the node pool as needed. - if (m_freeList == b2_nullNode) - { - b2Assert(m_nodeCount == m_nodeCapacity); - - // The free list is empty. Rebuild a bigger pool. - b2TreeNode* oldNodes = m_nodes; - m_nodeCapacity *= 2; - m_nodes = (b2TreeNode*)b2Alloc(m_nodeCapacity * sizeof(b2TreeNode)); - memcpy(m_nodes, oldNodes, m_nodeCount * sizeof(b2TreeNode)); - b2Free(oldNodes); - - // Build a linked list for the free list. The parent - // pointer becomes the "next" pointer. - for (int32 i = m_nodeCount; i < m_nodeCapacity - 1; ++i) - { - m_nodes[i].next = i + 1; - m_nodes[i].height = -1; - } - m_nodes[m_nodeCapacity-1].next = b2_nullNode; - m_nodes[m_nodeCapacity-1].height = -1; - m_freeList = m_nodeCount; - } - - // Peel a node off the free list. - int32 nodeId = m_freeList; - m_freeList = m_nodes[nodeId].next; - m_nodes[nodeId].parent = b2_nullNode; - m_nodes[nodeId].child1 = b2_nullNode; - m_nodes[nodeId].child2 = b2_nullNode; - m_nodes[nodeId].height = 0; - m_nodes[nodeId].userData = NULL; - ++m_nodeCount; - return nodeId; -} - -// Return a node to the pool. -void b2DynamicTree::FreeNode(int32 nodeId) -{ - b2Assert(0 <= nodeId && nodeId < m_nodeCapacity); - b2Assert(0 < m_nodeCount); - m_nodes[nodeId].next = m_freeList; - m_nodes[nodeId].height = -1; - m_freeList = nodeId; - --m_nodeCount; -} - -// Create a proxy in the tree as a leaf node. We return the index -// of the node instead of a pointer so that we can grow -// the node pool. -int32 b2DynamicTree::CreateProxy(const b2AABB& aabb, void* userData) -{ - int32 proxyId = AllocateNode(); - - // Fatten the aabb. - b2Vec2 r(b2_aabbExtension, b2_aabbExtension); - m_nodes[proxyId].aabb.lowerBound = aabb.lowerBound - r; - m_nodes[proxyId].aabb.upperBound = aabb.upperBound + r; - m_nodes[proxyId].userData = userData; - m_nodes[proxyId].height = 0; - - InsertLeaf(proxyId); - - return proxyId; -} - -void b2DynamicTree::DestroyProxy(int32 proxyId) -{ - b2Assert(0 <= proxyId && proxyId < m_nodeCapacity); - b2Assert(m_nodes[proxyId].IsLeaf()); - - RemoveLeaf(proxyId); - FreeNode(proxyId); -} - -bool b2DynamicTree::MoveProxy(int32 proxyId, const b2AABB& aabb, const b2Vec2& displacement) -{ - b2Assert(0 <= proxyId && proxyId < m_nodeCapacity); - - b2Assert(m_nodes[proxyId].IsLeaf()); - - if (m_nodes[proxyId].aabb.Contains(aabb)) - { - return false; - } - - RemoveLeaf(proxyId); - - // Extend AABB. - b2AABB b = aabb; - b2Vec2 r(b2_aabbExtension, b2_aabbExtension); - b.lowerBound = b.lowerBound - r; - b.upperBound = b.upperBound + r; - - // Predict AABB displacement. - b2Vec2 d = b2_aabbMultiplier * displacement; - - if (d.x < 0.0f) - { - b.lowerBound.x += d.x; - } - else - { - b.upperBound.x += d.x; - } - - if (d.y < 0.0f) - { - b.lowerBound.y += d.y; - } - else - { - b.upperBound.y += d.y; - } - - m_nodes[proxyId].aabb = b; - - InsertLeaf(proxyId); - return true; -} - -void b2DynamicTree::InsertLeaf(int32 leaf) -{ - ++m_insertionCount; - - if (m_root == b2_nullNode) - { - m_root = leaf; - m_nodes[m_root].parent = b2_nullNode; - return; - } - - // Find the best sibling for this node - b2AABB leafAABB = m_nodes[leaf].aabb; - int32 index = m_root; - while (m_nodes[index].IsLeaf() == false) - { - int32 child1 = m_nodes[index].child1; - int32 child2 = m_nodes[index].child2; - - float32 area = m_nodes[index].aabb.GetPerimeter(); - - b2AABB combinedAABB; - combinedAABB.Combine(m_nodes[index].aabb, leafAABB); - float32 combinedArea = combinedAABB.GetPerimeter(); - - // Cost of creating a new parent for this node and the new leaf - float32 cost = 2.0f * combinedArea; - - // Minimum cost of pushing the leaf further down the tree - float32 inheritanceCost = 2.0f * (combinedArea - area); - - // Cost of descending into child1 - float32 cost1; - if (m_nodes[child1].IsLeaf()) - { - b2AABB aabb; - aabb.Combine(leafAABB, m_nodes[child1].aabb); - cost1 = aabb.GetPerimeter() + inheritanceCost; - } - else - { - b2AABB aabb; - aabb.Combine(leafAABB, m_nodes[child1].aabb); - float32 oldArea = m_nodes[child1].aabb.GetPerimeter(); - float32 newArea = aabb.GetPerimeter(); - cost1 = (newArea - oldArea) + inheritanceCost; - } - - // Cost of descending into child2 - float32 cost2; - if (m_nodes[child2].IsLeaf()) - { - b2AABB aabb; - aabb.Combine(leafAABB, m_nodes[child2].aabb); - cost2 = aabb.GetPerimeter() + inheritanceCost; - } - else - { - b2AABB aabb; - aabb.Combine(leafAABB, m_nodes[child2].aabb); - float32 oldArea = m_nodes[child2].aabb.GetPerimeter(); - float32 newArea = aabb.GetPerimeter(); - cost2 = newArea - oldArea + inheritanceCost; - } - - // Descend according to the minimum cost. - if (cost < cost1 && cost < cost2) - { - break; - } - - // Descend - if (cost1 < cost2) - { - index = child1; - } - else - { - index = child2; - } - } - - int32 sibling = index; - - // Create a new parent. - int32 oldParent = m_nodes[sibling].parent; - int32 newParent = AllocateNode(); - m_nodes[newParent].parent = oldParent; - m_nodes[newParent].userData = NULL; - m_nodes[newParent].aabb.Combine(leafAABB, m_nodes[sibling].aabb); - m_nodes[newParent].height = m_nodes[sibling].height + 1; - - if (oldParent != b2_nullNode) - { - // The sibling was not the root. - if (m_nodes[oldParent].child1 == sibling) - { - m_nodes[oldParent].child1 = newParent; - } - else - { - m_nodes[oldParent].child2 = newParent; - } - - m_nodes[newParent].child1 = sibling; - m_nodes[newParent].child2 = leaf; - m_nodes[sibling].parent = newParent; - m_nodes[leaf].parent = newParent; - } - else - { - // The sibling was the root. - m_nodes[newParent].child1 = sibling; - m_nodes[newParent].child2 = leaf; - m_nodes[sibling].parent = newParent; - m_nodes[leaf].parent = newParent; - m_root = newParent; - } - - // Walk back up the tree fixing heights and AABBs - index = m_nodes[leaf].parent; - while (index != b2_nullNode) - { - index = Balance(index); - - int32 child1 = m_nodes[index].child1; - int32 child2 = m_nodes[index].child2; - - b2Assert(child1 != b2_nullNode); - b2Assert(child2 != b2_nullNode); - - m_nodes[index].height = 1 + b2Max(m_nodes[child1].height, m_nodes[child2].height); - m_nodes[index].aabb.Combine(m_nodes[child1].aabb, m_nodes[child2].aabb); - - index = m_nodes[index].parent; - } - - //Validate(); -} - -void b2DynamicTree::RemoveLeaf(int32 leaf) -{ - if (leaf == m_root) - { - m_root = b2_nullNode; - return; - } - - int32 parent = m_nodes[leaf].parent; - int32 grandParent = m_nodes[parent].parent; - int32 sibling; - if (m_nodes[parent].child1 == leaf) - { - sibling = m_nodes[parent].child2; - } - else - { - sibling = m_nodes[parent].child1; - } - - if (grandParent != b2_nullNode) - { - // Destroy parent and connect sibling to grandParent. - if (m_nodes[grandParent].child1 == parent) - { - m_nodes[grandParent].child1 = sibling; - } - else - { - m_nodes[grandParent].child2 = sibling; - } - m_nodes[sibling].parent = grandParent; - FreeNode(parent); - - // Adjust ancestor bounds. - int32 index = grandParent; - while (index != b2_nullNode) - { - index = Balance(index); - - int32 child1 = m_nodes[index].child1; - int32 child2 = m_nodes[index].child2; - - m_nodes[index].aabb.Combine(m_nodes[child1].aabb, m_nodes[child2].aabb); - m_nodes[index].height = 1 + b2Max(m_nodes[child1].height, m_nodes[child2].height); - - index = m_nodes[index].parent; - } - } - else - { - m_root = sibling; - m_nodes[sibling].parent = b2_nullNode; - FreeNode(parent); - } - - //Validate(); -} - -// Perform a left or right rotation if node A is imbalanced. -// Returns the new root index. -int32 b2DynamicTree::Balance(int32 iA) -{ - b2Assert(iA != b2_nullNode); - - b2TreeNode* A = m_nodes + iA; - if (A->IsLeaf() || A->height < 2) - { - return iA; - } - - int32 iB = A->child1; - int32 iC = A->child2; - b2Assert(0 <= iB && iB < m_nodeCapacity); - b2Assert(0 <= iC && iC < m_nodeCapacity); - - b2TreeNode* B = m_nodes + iB; - b2TreeNode* C = m_nodes + iC; - - int32 balance = C->height - B->height; - - // Rotate C up - if (balance > 1) - { - int32 iF = C->child1; - int32 iG = C->child2; - b2TreeNode* F = m_nodes + iF; - b2TreeNode* G = m_nodes + iG; - b2Assert(0 <= iF && iF < m_nodeCapacity); - b2Assert(0 <= iG && iG < m_nodeCapacity); - - // Swap A and C - C->child1 = iA; - C->parent = A->parent; - A->parent = iC; - - // A's old parent should point to C - if (C->parent != b2_nullNode) - { - if (m_nodes[C->parent].child1 == iA) - { - m_nodes[C->parent].child1 = iC; - } - else - { - b2Assert(m_nodes[C->parent].child2 == iA); - m_nodes[C->parent].child2 = iC; - } - } - else - { - m_root = iC; - } - - // Rotate - if (F->height > G->height) - { - C->child2 = iF; - A->child2 = iG; - G->parent = iA; - A->aabb.Combine(B->aabb, G->aabb); - C->aabb.Combine(A->aabb, F->aabb); - - A->height = 1 + b2Max(B->height, G->height); - C->height = 1 + b2Max(A->height, F->height); - } - else - { - C->child2 = iG; - A->child2 = iF; - F->parent = iA; - A->aabb.Combine(B->aabb, F->aabb); - C->aabb.Combine(A->aabb, G->aabb); - - A->height = 1 + b2Max(B->height, F->height); - C->height = 1 + b2Max(A->height, G->height); - } - - return iC; - } - - // Rotate B up - if (balance < -1) - { - int32 iD = B->child1; - int32 iE = B->child2; - b2TreeNode* D = m_nodes + iD; - b2TreeNode* E = m_nodes + iE; - b2Assert(0 <= iD && iD < m_nodeCapacity); - b2Assert(0 <= iE && iE < m_nodeCapacity); - - // Swap A and B - B->child1 = iA; - B->parent = A->parent; - A->parent = iB; - - // A's old parent should point to B - if (B->parent != b2_nullNode) - { - if (m_nodes[B->parent].child1 == iA) - { - m_nodes[B->parent].child1 = iB; - } - else - { - b2Assert(m_nodes[B->parent].child2 == iA); - m_nodes[B->parent].child2 = iB; - } - } - else - { - m_root = iB; - } - - // Rotate - if (D->height > E->height) - { - B->child2 = iD; - A->child1 = iE; - E->parent = iA; - A->aabb.Combine(C->aabb, E->aabb); - B->aabb.Combine(A->aabb, D->aabb); - - A->height = 1 + b2Max(C->height, E->height); - B->height = 1 + b2Max(A->height, D->height); - } - else - { - B->child2 = iE; - A->child1 = iD; - D->parent = iA; - A->aabb.Combine(C->aabb, D->aabb); - B->aabb.Combine(A->aabb, E->aabb); - - A->height = 1 + b2Max(C->height, D->height); - B->height = 1 + b2Max(A->height, E->height); - } - - return iB; - } - - return iA; -} - -int32 b2DynamicTree::GetHeight() const -{ - if (m_root == b2_nullNode) - { - return 0; - } - - return m_nodes[m_root].height; -} - -// -float32 b2DynamicTree::GetAreaRatio() const -{ - if (m_root == b2_nullNode) - { - return 0.0f; - } - - const b2TreeNode* root = m_nodes + m_root; - float32 rootArea = root->aabb.GetPerimeter(); - - float32 totalArea = 0.0f; - for (int32 i = 0; i < m_nodeCapacity; ++i) - { - const b2TreeNode* node = m_nodes + i; - if (node->height < 0) - { - // Free node in pool - continue; - } - - totalArea += node->aabb.GetPerimeter(); - } - - return totalArea / rootArea; -} - -// Compute the height of a sub-tree. -int32 b2DynamicTree::ComputeHeight(int32 nodeId) const -{ - b2Assert(0 <= nodeId && nodeId < m_nodeCapacity); - b2TreeNode* node = m_nodes + nodeId; - - if (node->IsLeaf()) - { - return 0; - } - - int32 height1 = ComputeHeight(node->child1); - int32 height2 = ComputeHeight(node->child2); - return 1 + b2Max(height1, height2); -} - -int32 b2DynamicTree::ComputeHeight() const -{ - int32 height = ComputeHeight(m_root); - return height; -} - -void b2DynamicTree::ValidateStructure(int32 index) const -{ - if (index == b2_nullNode) - { - return; - } - - if (index == m_root) - { - b2Assert(m_nodes[index].parent == b2_nullNode); - } - - const b2TreeNode* node = m_nodes + index; - - int32 child1 = node->child1; - int32 child2 = node->child2; - - if (node->IsLeaf()) - { - b2Assert(child1 == b2_nullNode); - b2Assert(child2 == b2_nullNode); - b2Assert(node->height == 0); - return; - } - - b2Assert(0 <= child1 && child1 < m_nodeCapacity); - b2Assert(0 <= child2 && child2 < m_nodeCapacity); - - b2Assert(m_nodes[child1].parent == index); - b2Assert(m_nodes[child2].parent == index); - - ValidateStructure(child1); - ValidateStructure(child2); -} - -void b2DynamicTree::ValidateMetrics(int32 index) const -{ - if (index == b2_nullNode) - { - return; - } - - const b2TreeNode* node = m_nodes + index; - - int32 child1 = node->child1; - int32 child2 = node->child2; - - if (node->IsLeaf()) - { - b2Assert(child1 == b2_nullNode); - b2Assert(child2 == b2_nullNode); - b2Assert(node->height == 0); - return; - } - - b2Assert(0 <= child1 && child1 < m_nodeCapacity); - b2Assert(0 <= child2 && child2 < m_nodeCapacity); - - int32 height1 = m_nodes[child1].height; - int32 height2 = m_nodes[child2].height; - int32 height; - height = 1 + b2Max(height1, height2); - b2Assert(node->height == height); - - b2AABB aabb; - aabb.Combine(m_nodes[child1].aabb, m_nodes[child2].aabb); - - b2Assert(aabb.lowerBound == node->aabb.lowerBound); - b2Assert(aabb.upperBound == node->aabb.upperBound); - - ValidateMetrics(child1); - ValidateMetrics(child2); -} - -void b2DynamicTree::Validate() const -{ - ValidateStructure(m_root); - ValidateMetrics(m_root); - - int32 freeCount = 0; - int32 freeIndex = m_freeList; - while (freeIndex != b2_nullNode) - { - b2Assert(0 <= freeIndex && freeIndex < m_nodeCapacity); - freeIndex = m_nodes[freeIndex].next; - ++freeCount; - } - - b2Assert(GetHeight() == ComputeHeight()); - - b2Assert(m_nodeCount + freeCount == m_nodeCapacity); -} - -int32 b2DynamicTree::GetMaxBalance() const -{ - int32 maxBalance = 0; - for (int32 i = 0; i < m_nodeCapacity; ++i) - { - const b2TreeNode* node = m_nodes + i; - if (node->height <= 1) - { - continue; - } - - b2Assert(node->IsLeaf() == false); - - int32 child1 = node->child1; - int32 child2 = node->child2; - int32 balance = b2Abs(m_nodes[child2].height - m_nodes[child1].height); - maxBalance = b2Max(maxBalance, balance); - } - - return maxBalance; -} - -void b2DynamicTree::RebuildBottomUp() -{ - int32* nodes = (int32*)b2Alloc(m_nodeCount * sizeof(int32)); - int32 count = 0; - - // Build array of leaves. Free the rest. - for (int32 i = 0; i < m_nodeCapacity; ++i) - { - if (m_nodes[i].height < 0) - { - // free node in pool - continue; - } - - if (m_nodes[i].IsLeaf()) - { - m_nodes[i].parent = b2_nullNode; - nodes[count] = i; - ++count; - } - else - { - FreeNode(i); - } - } - - while (count > 1) - { - float32 minCost = b2_maxFloat; - int32 iMin = -1, jMin = -1; - for (int32 i = 0; i < count; ++i) - { - b2AABB aabbi = m_nodes[nodes[i]].aabb; - - for (int32 j = i + 1; j < count; ++j) - { - b2AABB aabbj = m_nodes[nodes[j]].aabb; - b2AABB b; - b.Combine(aabbi, aabbj); - float32 cost = b.GetPerimeter(); - if (cost < minCost) - { - iMin = i; - jMin = j; - minCost = cost; - } - } - } - - int32 index1 = nodes[iMin]; - int32 index2 = nodes[jMin]; - b2TreeNode* child1 = m_nodes + index1; - b2TreeNode* child2 = m_nodes + index2; - - int32 parentIndex = AllocateNode(); - b2TreeNode* parent = m_nodes + parentIndex; - parent->child1 = index1; - parent->child2 = index2; - parent->height = 1 + b2Max(child1->height, child2->height); - parent->aabb.Combine(child1->aabb, child2->aabb); - parent->parent = b2_nullNode; - - child1->parent = parentIndex; - child2->parent = parentIndex; - - nodes[jMin] = nodes[count-1]; - nodes[iMin] = parentIndex; - --count; - } - - m_root = nodes[0]; - b2Free(nodes); - - Validate(); -} - -void b2DynamicTree::ShiftOrigin(const b2Vec2& newOrigin) -{ - // Build array of leaves. Free the rest. - for (int32 i = 0; i < m_nodeCapacity; ++i) - { - m_nodes[i].aabb.lowerBound -= newOrigin; - m_nodes[i].aabb.upperBound -= newOrigin; - } -} diff --git a/Box2D/Collision/b2DynamicTree.h b/Box2D/Collision/b2DynamicTree.h deleted file mode 100644 index bf80f73a10..0000000000 --- a/Box2D/Collision/b2DynamicTree.h +++ /dev/null @@ -1,289 +0,0 @@ -/* -* Copyright (c) 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_DYNAMIC_TREE_H -#define B2_DYNAMIC_TREE_H - -#include -#include - -#define b2_nullNode (-1) - -/// A node in the dynamic tree. The client does not interact with this directly. -struct b2TreeNode -{ - bool IsLeaf() const - { - return child1 == b2_nullNode; - } - - /// Enlarged AABB - b2AABB aabb; - - void* userData; - - union - { - int32 parent; - int32 next; - }; - - int32 child1; - int32 child2; - - // leaf = 0, free node = -1 - int32 height; -}; - -/// A dynamic AABB tree broad-phase, inspired by Nathanael Presson's btDbvt. -/// A dynamic tree arranges data in a binary tree to accelerate -/// queries such as volume queries and ray casts. Leafs are proxies -/// with an AABB. In the tree we expand the proxy AABB by b2_fatAABBFactor -/// so that the proxy AABB is bigger than the client object. This allows the client -/// object to move by small amounts without triggering a tree update. -/// -/// Nodes are pooled and relocatable, so we use node indices rather than pointers. -class b2DynamicTree -{ -public: - /// Constructing the tree initializes the node pool. - b2DynamicTree(); - - /// Destroy the tree, freeing the node pool. - ~b2DynamicTree(); - - /// Create a proxy. Provide a tight fitting AABB and a userData pointer. - int32 CreateProxy(const b2AABB& aabb, void* userData); - - /// Destroy a proxy. This asserts if the id is invalid. - void DestroyProxy(int32 proxyId); - - /// Move a proxy with a swepted AABB. If the proxy has moved outside of its fattened AABB, - /// then the proxy is removed from the tree and re-inserted. Otherwise - /// the function returns immediately. - /// @return true if the proxy was re-inserted. - bool MoveProxy(int32 proxyId, const b2AABB& aabb1, const b2Vec2& displacement); - - /// Get proxy user data. - /// @return the proxy user data or 0 if the id is invalid. - void* GetUserData(int32 proxyId) const; - - /// Get the fat AABB for a proxy. - const b2AABB& GetFatAABB(int32 proxyId) const; - - /// Query an AABB for overlapping proxies. The callback class - /// is called for each proxy that overlaps the supplied AABB. - template - void Query(T* callback, const b2AABB& aabb) const; - - /// Ray-cast against the proxies in the tree. This relies on the callback - /// to perform a exact ray-cast in the case were the proxy contains a shape. - /// The callback also performs the any collision filtering. This has performance - /// roughly equal to k * log(n), where k is the number of collisions and n is the - /// number of proxies in the tree. - /// @param input the ray-cast input data. The ray extends from p1 to p1 + maxFraction * (p2 - p1). - /// @param callback a callback class that is called for each proxy that is hit by the ray. - template - void RayCast(T* callback, const b2RayCastInput& input) const; - - /// Validate this tree. For testing. - void Validate() const; - - /// Compute the height of the binary tree in O(N) time. Should not be - /// called often. - int32 GetHeight() const; - - /// Get the maximum balance of an node in the tree. The balance is the difference - /// in height of the two children of a node. - int32 GetMaxBalance() const; - - /// Get the ratio of the sum of the node areas to the root area. - float32 GetAreaRatio() const; - - /// Build an optimal tree. Very expensive. For testing. - void RebuildBottomUp(); - - /// Shift the world origin. Useful for large worlds. - /// The shift formula is: position -= newOrigin - /// @param newOrigin the new origin with respect to the old origin - void ShiftOrigin(const b2Vec2& newOrigin); - -private: - - int32 AllocateNode(); - void FreeNode(int32 node); - - void InsertLeaf(int32 node); - void RemoveLeaf(int32 node); - - int32 Balance(int32 index); - - int32 ComputeHeight() const; - int32 ComputeHeight(int32 nodeId) const; - - void ValidateStructure(int32 index) const; - void ValidateMetrics(int32 index) const; - - int32 m_root; - - b2TreeNode* m_nodes; - int32 m_nodeCount; - int32 m_nodeCapacity; - - int32 m_freeList; - - /// This is used to incrementally traverse the tree for re-balancing. - uint32 m_path; - - int32 m_insertionCount; -}; - -inline void* b2DynamicTree::GetUserData(int32 proxyId) const -{ - b2Assert(0 <= proxyId && proxyId < m_nodeCapacity); - return m_nodes[proxyId].userData; -} - -inline const b2AABB& b2DynamicTree::GetFatAABB(int32 proxyId) const -{ - b2Assert(0 <= proxyId && proxyId < m_nodeCapacity); - return m_nodes[proxyId].aabb; -} - -template -inline void b2DynamicTree::Query(T* callback, const b2AABB& aabb) const -{ - b2GrowableStack stack; - stack.Push(m_root); - - while (stack.GetCount() > 0) - { - int32 nodeId = stack.Pop(); - if (nodeId == b2_nullNode) - { - continue; - } - - const b2TreeNode* node = m_nodes + nodeId; - - if (b2TestOverlap(node->aabb, aabb)) - { - if (node->IsLeaf()) - { - bool proceed = callback->QueryCallback(nodeId); - if (proceed == false) - { - return; - } - } - else - { - stack.Push(node->child1); - stack.Push(node->child2); - } - } - } -} - -template -inline void b2DynamicTree::RayCast(T* callback, const b2RayCastInput& input) const -{ - b2Vec2 p1 = input.p1; - b2Vec2 p2 = input.p2; - b2Vec2 r = p2 - p1; - b2Assert(r.LengthSquared() > 0.0f); - r.Normalize(); - - // v is perpendicular to the segment. - b2Vec2 v = b2Cross(1.0f, r); - b2Vec2 abs_v = b2Abs(v); - - // Separating axis for segment (Gino, p80). - // |dot(v, p1 - c)| > dot(|v|, h) - - float32 maxFraction = input.maxFraction; - - // Build a bounding box for the segment. - b2AABB segmentAABB; - { - b2Vec2 t = p1 + maxFraction * (p2 - p1); - segmentAABB.lowerBound = b2Min(p1, t); - segmentAABB.upperBound = b2Max(p1, t); - } - - b2GrowableStack stack; - stack.Push(m_root); - - while (stack.GetCount() > 0) - { - int32 nodeId = stack.Pop(); - if (nodeId == b2_nullNode) - { - continue; - } - - const b2TreeNode* node = m_nodes + nodeId; - - if (b2TestOverlap(node->aabb, segmentAABB) == false) - { - continue; - } - - // Separating axis for segment (Gino, p80). - // |dot(v, p1 - c)| > dot(|v|, h) - b2Vec2 c = node->aabb.GetCenter(); - b2Vec2 h = node->aabb.GetExtents(); - float32 separation = b2Abs(b2Dot(v, p1 - c)) - b2Dot(abs_v, h); - if (separation > 0.0f) - { - continue; - } - - if (node->IsLeaf()) - { - b2RayCastInput subInput; - subInput.p1 = input.p1; - subInput.p2 = input.p2; - subInput.maxFraction = maxFraction; - - float32 value = callback->RayCastCallback(subInput, nodeId); - - if (value == 0.0f) - { - // The client has terminated the ray cast. - return; - } - - if (value > 0.0f) - { - // Update segment bounding box. - maxFraction = value; - b2Vec2 t = p1 + maxFraction * (p2 - p1); - segmentAABB.lowerBound = b2Min(p1, t); - segmentAABB.upperBound = b2Max(p1, t); - } - } - else - { - stack.Push(node->child1); - stack.Push(node->child2); - } - } -} - -#endif diff --git a/Box2D/Collision/b2TimeOfImpact.cpp b/Box2D/Collision/b2TimeOfImpact.cpp deleted file mode 100644 index 60da95c389..0000000000 --- a/Box2D/Collision/b2TimeOfImpact.cpp +++ /dev/null @@ -1,486 +0,0 @@ -/* -* Copyright (c) 2007-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 -#include -#include -#include -#include -#include - -#include - -float32 b2_toiTime, b2_toiMaxTime; -int32 b2_toiCalls, b2_toiIters, b2_toiMaxIters; -int32 b2_toiRootIters, b2_toiMaxRootIters; - -// -struct b2SeparationFunction -{ - enum Type - { - e_points, - e_faceA, - e_faceB - }; - - // TODO_ERIN might not need to return the separation - - float32 Initialize(const b2SimplexCache* cache, - const b2DistanceProxy* proxyA, const b2Sweep& sweepA, - const b2DistanceProxy* proxyB, const b2Sweep& sweepB, - float32 t1) - { - m_proxyA = proxyA; - m_proxyB = proxyB; - int32 count = cache->count; - b2Assert(0 < count && count < 3); - - m_sweepA = sweepA; - m_sweepB = sweepB; - - b2Transform xfA, xfB; - m_sweepA.GetTransform(&xfA, t1); - m_sweepB.GetTransform(&xfB, t1); - - if (count == 1) - { - m_type = e_points; - b2Vec2 localPointA = m_proxyA->GetVertex(cache->indexA[0]); - b2Vec2 localPointB = m_proxyB->GetVertex(cache->indexB[0]); - b2Vec2 pointA = b2Mul(xfA, localPointA); - b2Vec2 pointB = b2Mul(xfB, localPointB); - m_axis = pointB - pointA; - float32 s = m_axis.Normalize(); - return s; - } - else if (cache->indexA[0] == cache->indexA[1]) - { - // Two points on B and one on A. - m_type = e_faceB; - b2Vec2 localPointB1 = proxyB->GetVertex(cache->indexB[0]); - b2Vec2 localPointB2 = proxyB->GetVertex(cache->indexB[1]); - - m_axis = b2Cross(localPointB2 - localPointB1, 1.0f); - m_axis.Normalize(); - b2Vec2 normal = b2Mul(xfB.q, m_axis); - - m_localPoint = 0.5f * (localPointB1 + localPointB2); - b2Vec2 pointB = b2Mul(xfB, m_localPoint); - - b2Vec2 localPointA = proxyA->GetVertex(cache->indexA[0]); - b2Vec2 pointA = b2Mul(xfA, localPointA); - - float32 s = b2Dot(pointA - pointB, normal); - if (s < 0.0f) - { - m_axis = -m_axis; - s = -s; - } - return s; - } - else - { - // Two points on A and one or two points on B. - m_type = e_faceA; - b2Vec2 localPointA1 = m_proxyA->GetVertex(cache->indexA[0]); - b2Vec2 localPointA2 = m_proxyA->GetVertex(cache->indexA[1]); - - m_axis = b2Cross(localPointA2 - localPointA1, 1.0f); - m_axis.Normalize(); - b2Vec2 normal = b2Mul(xfA.q, m_axis); - - m_localPoint = 0.5f * (localPointA1 + localPointA2); - b2Vec2 pointA = b2Mul(xfA, m_localPoint); - - b2Vec2 localPointB = m_proxyB->GetVertex(cache->indexB[0]); - b2Vec2 pointB = b2Mul(xfB, localPointB); - - float32 s = b2Dot(pointB - pointA, normal); - if (s < 0.0f) - { - m_axis = -m_axis; - s = -s; - } - return s; - } - } - - // - float32 FindMinSeparation(int32* indexA, int32* indexB, float32 t) const - { - b2Transform xfA, xfB; - m_sweepA.GetTransform(&xfA, t); - m_sweepB.GetTransform(&xfB, t); - - switch (m_type) - { - case e_points: - { - b2Vec2 axisA = b2MulT(xfA.q, m_axis); - b2Vec2 axisB = b2MulT(xfB.q, -m_axis); - - *indexA = m_proxyA->GetSupport(axisA); - *indexB = m_proxyB->GetSupport(axisB); - - b2Vec2 localPointA = m_proxyA->GetVertex(*indexA); - b2Vec2 localPointB = m_proxyB->GetVertex(*indexB); - - b2Vec2 pointA = b2Mul(xfA, localPointA); - b2Vec2 pointB = b2Mul(xfB, localPointB); - - float32 separation = b2Dot(pointB - pointA, m_axis); - return separation; - } - - case e_faceA: - { - b2Vec2 normal = b2Mul(xfA.q, m_axis); - b2Vec2 pointA = b2Mul(xfA, m_localPoint); - - b2Vec2 axisB = b2MulT(xfB.q, -normal); - - *indexA = -1; - *indexB = m_proxyB->GetSupport(axisB); - - b2Vec2 localPointB = m_proxyB->GetVertex(*indexB); - b2Vec2 pointB = b2Mul(xfB, localPointB); - - float32 separation = b2Dot(pointB - pointA, normal); - return separation; - } - - case e_faceB: - { - b2Vec2 normal = b2Mul(xfB.q, m_axis); - b2Vec2 pointB = b2Mul(xfB, m_localPoint); - - b2Vec2 axisA = b2MulT(xfA.q, -normal); - - *indexB = -1; - *indexA = m_proxyA->GetSupport(axisA); - - b2Vec2 localPointA = m_proxyA->GetVertex(*indexA); - b2Vec2 pointA = b2Mul(xfA, localPointA); - - float32 separation = b2Dot(pointA - pointB, normal); - return separation; - } - - default: - b2Assert(false); - *indexA = -1; - *indexB = -1; - return 0.0f; - } - } - - // - float32 Evaluate(int32 indexA, int32 indexB, float32 t) const - { - b2Transform xfA, xfB; - m_sweepA.GetTransform(&xfA, t); - m_sweepB.GetTransform(&xfB, t); - - switch (m_type) - { - case e_points: - { - b2Vec2 localPointA = m_proxyA->GetVertex(indexA); - b2Vec2 localPointB = m_proxyB->GetVertex(indexB); - - b2Vec2 pointA = b2Mul(xfA, localPointA); - b2Vec2 pointB = b2Mul(xfB, localPointB); - float32 separation = b2Dot(pointB - pointA, m_axis); - - return separation; - } - - case e_faceA: - { - b2Vec2 normal = b2Mul(xfA.q, m_axis); - b2Vec2 pointA = b2Mul(xfA, m_localPoint); - - b2Vec2 localPointB = m_proxyB->GetVertex(indexB); - b2Vec2 pointB = b2Mul(xfB, localPointB); - - float32 separation = b2Dot(pointB - pointA, normal); - return separation; - } - - case e_faceB: - { - b2Vec2 normal = b2Mul(xfB.q, m_axis); - b2Vec2 pointB = b2Mul(xfB, m_localPoint); - - b2Vec2 localPointA = m_proxyA->GetVertex(indexA); - b2Vec2 pointA = b2Mul(xfA, localPointA); - - float32 separation = b2Dot(pointA - pointB, normal); - return separation; - } - - default: - b2Assert(false); - return 0.0f; - } - } - - const b2DistanceProxy* m_proxyA; - const b2DistanceProxy* m_proxyB; - b2Sweep m_sweepA, m_sweepB; - Type m_type; - b2Vec2 m_localPoint; - b2Vec2 m_axis; -}; - -// CCD via the local separating axis method. This seeks progression -// by computing the largest time at which separation is maintained. -void b2TimeOfImpact(b2TOIOutput* output, const b2TOIInput* input) -{ - b2Timer timer; - - ++b2_toiCalls; - - output->state = b2TOIOutput::e_unknown; - output->t = input->tMax; - - const b2DistanceProxy* proxyA = &input->proxyA; - const b2DistanceProxy* proxyB = &input->proxyB; - - b2Sweep sweepA = input->sweepA; - b2Sweep sweepB = input->sweepB; - - // Large rotations can make the root finder fail, so we normalize the - // sweep angles. - sweepA.Normalize(); - sweepB.Normalize(); - - float32 tMax = input->tMax; - - float32 totalRadius = proxyA->m_radius + proxyB->m_radius; - float32 target = b2Max(b2_linearSlop, totalRadius - 3.0f * b2_linearSlop); - float32 tolerance = 0.25f * b2_linearSlop; - b2Assert(target > tolerance); - - float32 t1 = 0.0f; - const int32 k_maxIterations = 20; // TODO_ERIN b2Settings - int32 iter = 0; - - // Prepare input for distance query. - b2SimplexCache cache; - cache.count = 0; - b2DistanceInput distanceInput; - distanceInput.proxyA = input->proxyA; - distanceInput.proxyB = input->proxyB; - distanceInput.useRadii = false; - - // The outer loop progressively attempts to compute new separating axes. - // This loop terminates when an axis is repeated (no progress is made). - for(;;) - { - b2Transform xfA, xfB; - sweepA.GetTransform(&xfA, t1); - sweepB.GetTransform(&xfB, t1); - - // Get the distance between shapes. We can also use the results - // to get a separating axis. - distanceInput.transformA = xfA; - distanceInput.transformB = xfB; - b2DistanceOutput distanceOutput; - b2Distance(&distanceOutput, &cache, &distanceInput); - - // If the shapes are overlapped, we give up on continuous collision. - if (distanceOutput.distance <= 0.0f) - { - // Failure! - output->state = b2TOIOutput::e_overlapped; - output->t = 0.0f; - break; - } - - if (distanceOutput.distance < target + tolerance) - { - // Victory! - output->state = b2TOIOutput::e_touching; - output->t = t1; - break; - } - - // Initialize the separating axis. - b2SeparationFunction fcn; - fcn.Initialize(&cache, proxyA, sweepA, proxyB, sweepB, t1); -#if 0 - // Dump the curve seen by the root finder - { - const int32 N = 100; - float32 dx = 1.0f / N; - float32 xs[N+1]; - float32 fs[N+1]; - - float32 x = 0.0f; - - for (int32 i = 0; i <= N; ++i) - { - sweepA.GetTransform(&xfA, x); - sweepB.GetTransform(&xfB, x); - float32 f = fcn.Evaluate(xfA, xfB) - target; - - printf("%g %g\n", x, f); - - xs[i] = x; - fs[i] = f; - - x += dx; - } - } -#endif - - // Compute the TOI on the separating axis. We do this by successively - // resolving the deepest point. This loop is bounded by the number of vertices. - bool done = false; - float32 t2 = tMax; - int32 pushBackIter = 0; - for (;;) - { - // Find the deepest point at t2. Store the witness point indices. - int32 indexA, indexB; - float32 s2 = fcn.FindMinSeparation(&indexA, &indexB, t2); - - // Is the final configuration separated? - if (s2 > target + tolerance) - { - // Victory! - output->state = b2TOIOutput::e_separated; - output->t = tMax; - done = true; - break; - } - - // Has the separation reached tolerance? - if (s2 > target - tolerance) - { - // Advance the sweeps - t1 = t2; - break; - } - - // Compute the initial separation of the witness points. - float32 s1 = fcn.Evaluate(indexA, indexB, t1); - - // Check for initial overlap. This might happen if the root finder - // runs out of iterations. - if (s1 < target - tolerance) - { - output->state = b2TOIOutput::e_failed; - output->t = t1; - done = true; - break; - } - - // Check for touching - if (s1 <= target + tolerance) - { - // Victory! t1 should hold the TOI (could be 0.0). - output->state = b2TOIOutput::e_touching; - output->t = t1; - done = true; - break; - } - - // Compute 1D root of: f(x) - target = 0 - int32 rootIterCount = 0; - float32 a1 = t1, a2 = t2; - for (;;) - { - // Use a mix of the secant rule and bisection. - float32 t; - if (rootIterCount & 1) - { - // Secant rule to improve convergence. - t = a1 + (target - s1) * (a2 - a1) / (s2 - s1); - } - else - { - // Bisection to guarantee progress. - t = 0.5f * (a1 + a2); - } - - ++rootIterCount; - ++b2_toiRootIters; - - float32 s = fcn.Evaluate(indexA, indexB, t); - - if (b2Abs(s - target) < tolerance) - { - // t2 holds a tentative value for t1 - t2 = t; - break; - } - - // Ensure we continue to bracket the root. - if (s > target) - { - a1 = t; - s1 = s; - } - else - { - a2 = t; - s2 = s; - } - - if (rootIterCount == 50) - { - break; - } - } - - b2_toiMaxRootIters = b2Max(b2_toiMaxRootIters, rootIterCount); - - ++pushBackIter; - - if (pushBackIter == b2_maxPolygonVertices) - { - break; - } - } - - ++iter; - ++b2_toiIters; - - if (done) - { - break; - } - - if (iter == k_maxIterations) - { - // Root finder got stuck. Semi-victory. - output->state = b2TOIOutput::e_failed; - output->t = t1; - break; - } - } - - b2_toiMaxIters = b2Max(b2_toiMaxIters, iter); - - float32 time = timer.GetMilliseconds(); - b2_toiMaxTime = b2Max(b2_toiMaxTime, time); - b2_toiTime += time; -} diff --git a/Box2D/Collision/b2TimeOfImpact.h b/Box2D/Collision/b2TimeOfImpact.h deleted file mode 100644 index 20620c1ee7..0000000000 --- a/Box2D/Collision/b2TimeOfImpact.h +++ /dev/null @@ -1,58 +0,0 @@ -/* -* 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_TIME_OF_IMPACT_H -#define B2_TIME_OF_IMPACT_H - -#include -#include - -/// Input parameters for b2TimeOfImpact -struct b2TOIInput -{ - b2DistanceProxy proxyA; - b2DistanceProxy proxyB; - b2Sweep sweepA; - b2Sweep sweepB; - float32 tMax; // defines sweep interval [0, tMax] -}; - -// Output parameters for b2TimeOfImpact. -struct b2TOIOutput -{ - enum State - { - e_unknown, - e_failed, - e_overlapped, - e_touching, - e_separated - }; - - State state; - float32 t; -}; - -/// Compute the upper bound on time before two shapes penetrate. Time is represented as -/// a fraction between [0,tMax]. This uses a swept separating axis and may miss some intermediate, -/// non-tunneling collision. If you change the time interval, you should call this function -/// again. -/// Note: use b2Distance to compute the contact point and normal at the time of impact. -void b2TimeOfImpact(b2TOIOutput* output, const b2TOIInput* input); - -#endif diff --git a/Box2D/Common/b2BlockAllocator.cpp b/Box2D/Common/b2BlockAllocator.cpp deleted file mode 100644 index b09a968435..0000000000 --- a/Box2D/Common/b2BlockAllocator.cpp +++ /dev/null @@ -1,216 +0,0 @@ -/* -* 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 -#include -#include -#include -#include - -int32 b2BlockAllocator::s_blockSizes[b2_blockSizes] = -{ - 16, // 0 - 32, // 1 - 64, // 2 - 96, // 3 - 128, // 4 - 160, // 5 - 192, // 6 - 224, // 7 - 256, // 8 - 320, // 9 - 384, // 10 - 448, // 11 - 512, // 12 - 640, // 13 -}; -uint8 b2BlockAllocator::s_blockSizeLookup[b2_maxBlockSize + 1]; -bool b2BlockAllocator::s_blockSizeLookupInitialized; - -struct b2Chunk -{ - int32 blockSize; - b2Block* blocks; -}; - -struct b2Block -{ - b2Block* next; -}; - -b2BlockAllocator::b2BlockAllocator() -{ - b2Assert(b2_blockSizes < UCHAR_MAX); - - m_chunkSpace = b2_chunkArrayIncrement; - m_chunkCount = 0; - m_chunks = (b2Chunk*)b2Alloc(m_chunkSpace * sizeof(b2Chunk)); - - memset(m_chunks, 0, m_chunkSpace * sizeof(b2Chunk)); - memset(m_freeLists, 0, sizeof(m_freeLists)); - - if (s_blockSizeLookupInitialized == false) - { - int32 j = 0; - for (int32 i = 1; i <= b2_maxBlockSize; ++i) - { - b2Assert(j < b2_blockSizes); - if (i <= s_blockSizes[j]) - { - s_blockSizeLookup[i] = (uint8)j; - } - else - { - ++j; - s_blockSizeLookup[i] = (uint8)j; - } - } - - s_blockSizeLookupInitialized = true; - } -} - -b2BlockAllocator::~b2BlockAllocator() -{ - for (int32 i = 0; i < m_chunkCount; ++i) - { - b2Free(m_chunks[i].blocks); - } - - b2Free(m_chunks); -} - -void* b2BlockAllocator::Allocate(int32 size) -{ - if (size == 0) - return NULL; - - b2Assert(0 < size); - - if (size > b2_maxBlockSize) - { - return b2Alloc(size); - } - - int32 index = s_blockSizeLookup[size]; - b2Assert(0 <= index && index < b2_blockSizes); - - if (m_freeLists[index]) - { - b2Block* block = m_freeLists[index]; - m_freeLists[index] = block->next; - return block; - } - else - { - if (m_chunkCount == m_chunkSpace) - { - b2Chunk* oldChunks = m_chunks; - m_chunkSpace += b2_chunkArrayIncrement; - m_chunks = (b2Chunk*)b2Alloc(m_chunkSpace * sizeof(b2Chunk)); - memcpy(m_chunks, oldChunks, m_chunkCount * sizeof(b2Chunk)); - memset(m_chunks + m_chunkCount, 0, b2_chunkArrayIncrement * sizeof(b2Chunk)); - b2Free(oldChunks); - } - - b2Chunk* chunk = m_chunks + m_chunkCount; - chunk->blocks = (b2Block*)b2Alloc(b2_chunkSize); -#if defined(_DEBUG) - memset(chunk->blocks, 0xcd, b2_chunkSize); -#endif - int32 blockSize = s_blockSizes[index]; - chunk->blockSize = blockSize; - int32 blockCount = b2_chunkSize / blockSize; - b2Assert(blockCount * blockSize <= b2_chunkSize); - for (int32 i = 0; i < blockCount - 1; ++i) - { - b2Block* block = (b2Block*)((int8*)chunk->blocks + blockSize * i); - b2Block* next = (b2Block*)((int8*)chunk->blocks + blockSize * (i + 1)); - block->next = next; - } - b2Block* last = (b2Block*)((int8*)chunk->blocks + blockSize * (blockCount - 1)); - last->next = NULL; - - m_freeLists[index] = chunk->blocks->next; - ++m_chunkCount; - - return chunk->blocks; - } -} - -void b2BlockAllocator::Free(void* p, int32 size) -{ - if (size == 0) - { - return; - } - - b2Assert(0 < size); - - if (size > b2_maxBlockSize) - { - b2Free(p); - return; - } - - int32 index = s_blockSizeLookup[size]; - b2Assert(0 <= index && index < b2_blockSizes); - -#ifdef _DEBUG - // Verify the memory address and size is valid. - int32 blockSize = s_blockSizes[index]; - bool found = false; - for (int32 i = 0; i < m_chunkCount; ++i) - { - b2Chunk* chunk = m_chunks + i; - if (chunk->blockSize != blockSize) - { - b2Assert( (int8*)p + blockSize <= (int8*)chunk->blocks || - (int8*)chunk->blocks + b2_chunkSize <= (int8*)p); - } - else - { - if ((int8*)chunk->blocks <= (int8*)p && (int8*)p + blockSize <= (int8*)chunk->blocks + b2_chunkSize) - { - found = true; - } - } - } - - b2Assert(found); - - memset(p, 0xfd, blockSize); -#endif - - b2Block* block = (b2Block*)p; - block->next = m_freeLists[index]; - m_freeLists[index] = block; -} - -void b2BlockAllocator::Clear() -{ - for (int32 i = 0; i < m_chunkCount; ++i) - { - b2Free(m_chunks[i].blocks); - } - - m_chunkCount = 0; - memset(m_chunks, 0, m_chunkSpace * sizeof(b2Chunk)); - - memset(m_freeLists, 0, sizeof(m_freeLists)); -} diff --git a/Box2D/Common/b2BlockAllocator.h b/Box2D/Common/b2BlockAllocator.h deleted file mode 100644 index cea3cef968..0000000000 --- a/Box2D/Common/b2BlockAllocator.h +++ /dev/null @@ -1,62 +0,0 @@ -/* -* 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_BLOCK_ALLOCATOR_H -#define B2_BLOCK_ALLOCATOR_H - -#include - -const int32 b2_chunkSize = 16 * 1024; -const int32 b2_maxBlockSize = 640; -const int32 b2_blockSizes = 14; -const int32 b2_chunkArrayIncrement = 128; - -struct b2Block; -struct b2Chunk; - -/// This is a small object allocator used for allocating small -/// objects that persist for more than one time step. -/// See: http://www.codeproject.com/useritems/Small_Block_Allocator.asp -class b2BlockAllocator -{ -public: - b2BlockAllocator(); - ~b2BlockAllocator(); - - /// Allocate memory. This will use b2Alloc if the size is larger than b2_maxBlockSize. - void* Allocate(int32 size); - - /// Free memory. This will use b2Free if the size is larger than b2_maxBlockSize. - void Free(void* p, int32 size); - - void Clear(); - -private: - - b2Chunk* m_chunks; - int32 m_chunkCount; - int32 m_chunkSpace; - - b2Block* m_freeLists[b2_blockSizes]; - - static int32 s_blockSizes[b2_blockSizes]; - static uint8 s_blockSizeLookup[b2_maxBlockSize + 1]; - static bool s_blockSizeLookupInitialized; -}; - -#endif diff --git a/Box2D/Common/b2Draw.cpp b/Box2D/Common/b2Draw.cpp deleted file mode 100644 index 2cd9b1a568..0000000000 --- a/Box2D/Common/b2Draw.cpp +++ /dev/null @@ -1,44 +0,0 @@ -/* -* Copyright (c) 2011 Erin Catto http://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 - -b2Draw::b2Draw() -{ - m_drawFlags = 0; -} - -void b2Draw::SetFlags(uint32 flags) -{ - m_drawFlags = flags; -} - -uint32 b2Draw::GetFlags() const -{ - return m_drawFlags; -} - -void b2Draw::AppendFlags(uint32 flags) -{ - m_drawFlags |= flags; -} - -void b2Draw::ClearFlags(uint32 flags) -{ - m_drawFlags &= ~flags; -} diff --git a/Box2D/Common/b2Draw.h b/Box2D/Common/b2Draw.h deleted file mode 100644 index 20359c913d..0000000000 --- a/Box2D/Common/b2Draw.h +++ /dev/null @@ -1,86 +0,0 @@ -/* -* Copyright (c) 2011 Erin Catto http://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_DRAW_H -#define B2_DRAW_H - -#include - -/// Color for debug drawing. Each value has the range [0,1]. -struct b2Color -{ - b2Color() {} - b2Color(float32 r, float32 g, float32 b) : r(r), g(g), b(b) {} - void Set(float32 ri, float32 gi, float32 bi) { r = ri; g = gi; b = bi; } - float32 r, g, b; -}; - -/// Implement and register this class with a b2World to provide debug drawing of physics -/// entities in your game. -class b2Draw -{ -public: - b2Draw(); - - virtual ~b2Draw() {} - - enum - { - e_shapeBit = 0x0001, ///< draw shapes - e_jointBit = 0x0002, ///< draw joint connections - e_aabbBit = 0x0004, ///< draw axis aligned bounding boxes - e_pairBit = 0x0008, ///< draw broad-phase pairs - e_centerOfMassBit = 0x0010 ///< draw center of mass frame - }; - - /// Set the drawing flags. - void SetFlags(uint32 flags); - - /// Get the drawing flags. - uint32 GetFlags() const; - - /// Append flags to the current flags. - void AppendFlags(uint32 flags); - - /// Clear flags from the current flags. - void ClearFlags(uint32 flags); - - /// Draw a closed polygon provided in CCW order. - virtual void DrawPolygon(const b2Vec2* vertices, int32 vertexCount, const b2Color& color) = 0; - - /// Draw a solid closed polygon provided in CCW order. - virtual void DrawSolidPolygon(const b2Vec2* vertices, int32 vertexCount, const b2Color& color) = 0; - - /// Draw a circle. - virtual void DrawCircle(const b2Vec2& center, float32 radius, const b2Color& color) = 0; - - /// Draw a solid circle. - virtual void DrawSolidCircle(const b2Vec2& center, float32 radius, const b2Vec2& axis, const b2Color& color) = 0; - - /// Draw a line segment. - virtual void DrawSegment(const b2Vec2& p1, const b2Vec2& p2, const b2Color& color) = 0; - - /// Draw a transform. Choose your own length scale. - /// @param xf a transform. - virtual void DrawTransform(const b2Transform& xf) = 0; - -protected: - uint32 m_drawFlags; -}; - -#endif diff --git a/Box2D/Common/b2GrowableStack.h b/Box2D/Common/b2GrowableStack.h deleted file mode 100644 index 44813e3eab..0000000000 --- a/Box2D/Common/b2GrowableStack.h +++ /dev/null @@ -1,86 +0,0 @@ -/* -* Copyright (c) 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_GROWABLE_STACK_H -#define B2_GROWABLE_STACK_H -#include -#include -#include - -/// This is a growable LIFO stack with an initial capacity of N. -/// If the stack size exceeds the initial capacity, the heap is used -/// to increase the size of the stack. -template -class b2GrowableStack -{ -public: - b2GrowableStack() - { - m_stack = m_array; - m_count = 0; - m_capacity = N; - } - - ~b2GrowableStack() - { - if (m_stack != m_array) - { - b2Free(m_stack); - m_stack = NULL; - } - } - - void Push(const T& element) - { - if (m_count == m_capacity) - { - T* old = m_stack; - m_capacity *= 2; - m_stack = (T*)b2Alloc(m_capacity * sizeof(T)); - memcpy(m_stack, old, m_count * sizeof(T)); - if (old != m_array) - { - b2Free(old); - } - } - - m_stack[m_count] = element; - ++m_count; - } - - T Pop() - { - b2Assert(m_count > 0); - --m_count; - return m_stack[m_count]; - } - - int32 GetCount() - { - return m_count; - } - -private: - T* m_stack; - T m_array[N]; - int32 m_count; - int32 m_capacity; -}; - - -#endif diff --git a/Box2D/Common/b2Math.cpp b/Box2D/Common/b2Math.cpp deleted file mode 100644 index d6027be90f..0000000000 --- a/Box2D/Common/b2Math.cpp +++ /dev/null @@ -1,94 +0,0 @@ -/* -* Copyright (c) 2007-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 - -const b2Vec2 b2Vec2_zero(0.0f, 0.0f); - -/// Solve A * x = b, where b is a column vector. This is more efficient -/// than computing the inverse in one-shot cases. -b2Vec3 b2Mat33::Solve33(const b2Vec3& b) const -{ - float32 det = b2Dot(ex, b2Cross(ey, ez)); - if (det != 0.0f) - { - det = 1.0f / det; - } - b2Vec3 x; - x.x = det * b2Dot(b, b2Cross(ey, ez)); - x.y = det * b2Dot(ex, b2Cross(b, ez)); - x.z = det * b2Dot(ex, b2Cross(ey, b)); - return x; -} - -/// Solve A * x = b, where b is a column vector. This is more efficient -/// than computing the inverse in one-shot cases. -b2Vec2 b2Mat33::Solve22(const b2Vec2& b) const -{ - float32 a11 = ex.x, a12 = ey.x, a21 = ex.y, a22 = ey.y; - float32 det = a11 * a22 - a12 * a21; - if (det != 0.0f) - { - det = 1.0f / det; - } - b2Vec2 x; - x.x = det * (a22 * b.x - a12 * b.y); - x.y = det * (a11 * b.y - a21 * b.x); - return x; -} - -/// -void b2Mat33::GetInverse22(b2Mat33* M) const -{ - float32 a = ex.x, b = ey.x, c = ex.y, d = ey.y; - float32 det = a * d - b * c; - if (det != 0.0f) - { - det = 1.0f / det; - } - - M->ex.x = det * d; M->ey.x = -det * b; M->ex.z = 0.0f; - M->ex.y = -det * c; M->ey.y = det * a; M->ey.z = 0.0f; - M->ez.x = 0.0f; M->ez.y = 0.0f; M->ez.z = 0.0f; -} - -/// Returns the zero matrix if singular. -void b2Mat33::GetSymInverse33(b2Mat33* M) const -{ - float32 det = b2Dot(ex, b2Cross(ey, ez)); - if (det != 0.0f) - { - det = 1.0f / det; - } - - float32 a11 = ex.x, a12 = ey.x, a13 = ez.x; - float32 a22 = ey.y, a23 = ez.y; - float32 a33 = ez.z; - - M->ex.x = det * (a22 * a33 - a23 * a23); - M->ex.y = det * (a13 * a23 - a12 * a33); - M->ex.z = det * (a12 * a23 - a13 * a22); - - M->ey.x = M->ex.y; - M->ey.y = det * (a11 * a33 - a13 * a13); - M->ey.z = det * (a13 * a12 - a11 * a23); - - M->ez.x = M->ex.z; - M->ez.y = M->ey.z; - M->ez.z = det * (a11 * a22 - a12 * a12); -} diff --git a/Box2D/Common/b2Math.h b/Box2D/Common/b2Math.h deleted file mode 100644 index 755c519b0a..0000000000 --- a/Box2D/Common/b2Math.h +++ /dev/null @@ -1,720 +0,0 @@ -/* -* 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_MATH_H -#define B2_MATH_H - -#include -#include - -/// This function is used to ensure that a floating point number is not a NaN or infinity. -inline bool b2IsValid(float32 x) -{ - int32 ix = *reinterpret_cast(&x); - return (ix & 0x7f800000) != 0x7f800000; -} - -/// This is a approximate yet fast inverse square-root. -inline float32 b2InvSqrt(float32 x) -{ - union - { - float32 x; - int32 i; - } convert; - - convert.x = x; - float32 xhalf = 0.5f * x; - convert.i = 0x5f3759df - (convert.i >> 1); - x = convert.x; - x = x * (1.5f - xhalf * x * x); - return x; -} - -#define b2Sqrt(x) sqrtf(x) -#define b2Atan2(y, x) atan2f(y, x) - -/// A 2D column vector. -struct b2Vec2 -{ - /// Default constructor does nothing (for performance). - b2Vec2() {} - - /// Construct using coordinates. - b2Vec2(float32 x, float32 y) : x(x), y(y) {} - - /// Set this vector to all zeros. - void SetZero() { x = 0.0f; y = 0.0f; } - - /// Set this vector to some specified coordinates. - void Set(float32 x_, float32 y_) { x = x_; y = y_; } - - /// Negate this vector. - b2Vec2 operator -() const { b2Vec2 v; v.Set(-x, -y); return v; } - - /// Read from and indexed element. - float32 operator () (int32 i) const - { - return (&x)[i]; - } - - /// Write to an indexed element. - float32& operator () (int32 i) - { - return (&x)[i]; - } - - /// Add a vector to this vector. - void operator += (const b2Vec2& v) - { - x += v.x; y += v.y; - } - - /// Subtract a vector from this vector. - void operator -= (const b2Vec2& v) - { - x -= v.x; y -= v.y; - } - - /// Multiply this vector by a scalar. - void operator *= (float32 a) - { - x *= a; y *= a; - } - - /// Get the length of this vector (the norm). - float32 Length() const - { - return b2Sqrt(x * x + y * y); - } - - /// Get the length squared. For performance, use this instead of - /// b2Vec2::Length (if possible). - float32 LengthSquared() const - { - return x * x + y * y; - } - - /// Convert this vector into a unit vector. Returns the length. - float32 Normalize() - { - float32 length = Length(); - if (length < b2_epsilon) - { - return 0.0f; - } - float32 invLength = 1.0f / length; - x *= invLength; - y *= invLength; - - return length; - } - - /// Does this vector contain finite coordinates? - bool IsValid() const - { - return b2IsValid(x) && b2IsValid(y); - } - - /// Get the skew vector such that dot(skew_vec, other) == cross(vec, other) - b2Vec2 Skew() const - { - return b2Vec2(-y, x); - } - - float32 x, y; -}; - -/// A 2D column vector with 3 elements. -struct b2Vec3 -{ - /// Default constructor does nothing (for performance). - b2Vec3() {} - - /// Construct using coordinates. - b2Vec3(float32 x, float32 y, float32 z) : x(x), y(y), z(z) {} - - /// Set this vector to all zeros. - void SetZero() { x = 0.0f; y = 0.0f; z = 0.0f; } - - /// Set this vector to some specified coordinates. - void Set(float32 x_, float32 y_, float32 z_) { x = x_; y = y_; z = z_; } - - /// Negate this vector. - b2Vec3 operator -() const { b2Vec3 v; v.Set(-x, -y, -z); return v; } - - /// Add a vector to this vector. - void operator += (const b2Vec3& v) - { - x += v.x; y += v.y; z += v.z; - } - - /// Subtract a vector from this vector. - void operator -= (const b2Vec3& v) - { - x -= v.x; y -= v.y; z -= v.z; - } - - /// Multiply this vector by a scalar. - void operator *= (float32 s) - { - x *= s; y *= s; z *= s; - } - - float32 x, y, z; -}; - -/// A 2-by-2 matrix. Stored in column-major order. -struct b2Mat22 -{ - /// The default constructor does nothing (for performance). - b2Mat22() {} - - /// Construct this matrix using columns. - b2Mat22(const b2Vec2& c1, const b2Vec2& c2) - { - ex = c1; - ey = c2; - } - - /// Construct this matrix using scalars. - b2Mat22(float32 a11, float32 a12, float32 a21, float32 a22) - { - ex.x = a11; ex.y = a21; - ey.x = a12; ey.y = a22; - } - - /// Initialize this matrix using columns. - void Set(const b2Vec2& c1, const b2Vec2& c2) - { - ex = c1; - ey = c2; - } - - /// Set this to the identity matrix. - void SetIdentity() - { - ex.x = 1.0f; ey.x = 0.0f; - ex.y = 0.0f; ey.y = 1.0f; - } - - /// Set this matrix to all zeros. - void SetZero() - { - ex.x = 0.0f; ey.x = 0.0f; - ex.y = 0.0f; ey.y = 0.0f; - } - - b2Mat22 GetInverse() const - { - float32 a = ex.x, b = ey.x, c = ex.y, d = ey.y; - b2Mat22 B; - float32 det = a * d - b * c; - if (det != 0.0f) - { - det = 1.0f / det; - } - B.ex.x = det * d; B.ey.x = -det * b; - B.ex.y = -det * c; B.ey.y = det * a; - return B; - } - - /// Solve A * x = b, where b is a column vector. This is more efficient - /// than computing the inverse in one-shot cases. - b2Vec2 Solve(const b2Vec2& b) const - { - float32 a11 = ex.x, a12 = ey.x, a21 = ex.y, a22 = ey.y; - float32 det = a11 * a22 - a12 * a21; - if (det != 0.0f) - { - det = 1.0f / det; - } - b2Vec2 x; - x.x = det * (a22 * b.x - a12 * b.y); - x.y = det * (a11 * b.y - a21 * b.x); - return x; - } - - b2Vec2 ex, ey; -}; - -/// A 3-by-3 matrix. Stored in column-major order. -struct b2Mat33 -{ - /// The default constructor does nothing (for performance). - b2Mat33() {} - - /// Construct this matrix using columns. - b2Mat33(const b2Vec3& c1, const b2Vec3& c2, const b2Vec3& c3) - { - ex = c1; - ey = c2; - ez = c3; - } - - /// Set this matrix to all zeros. - void SetZero() - { - ex.SetZero(); - ey.SetZero(); - ez.SetZero(); - } - - /// Solve A * x = b, where b is a column vector. This is more efficient - /// than computing the inverse in one-shot cases. - b2Vec3 Solve33(const b2Vec3& b) const; - - /// Solve A * x = b, where b is a column vector. This is more efficient - /// than computing the inverse in one-shot cases. Solve only the upper - /// 2-by-2 matrix equation. - b2Vec2 Solve22(const b2Vec2& b) const; - - /// Get the inverse of this matrix as a 2-by-2. - /// Returns the zero matrix if singular. - void GetInverse22(b2Mat33* M) const; - - /// Get the symmetric inverse of this matrix as a 3-by-3. - /// Returns the zero matrix if singular. - void GetSymInverse33(b2Mat33* M) const; - - b2Vec3 ex, ey, ez; -}; - -/// Rotation -struct b2Rot -{ - b2Rot() {} - - /// Initialize from an angle in radians - explicit b2Rot(float32 angle) - { - /// TODO_ERIN optimize - s = sinf(angle); - c = cosf(angle); - } - - /// Set using an angle in radians. - void Set(float32 angle) - { - /// TODO_ERIN optimize - s = sinf(angle); - c = cosf(angle); - } - - /// Set to the identity rotation - void SetIdentity() - { - s = 0.0f; - c = 1.0f; - } - - /// Get the angle in radians - float32 GetAngle() const - { - return b2Atan2(s, c); - } - - /// Get the x-axis - b2Vec2 GetXAxis() const - { - return b2Vec2(c, s); - } - - /// Get the u-axis - b2Vec2 GetYAxis() const - { - return b2Vec2(-s, c); - } - - /// Sine and cosine - float32 s, c; -}; - -/// A transform contains translation and rotation. It is used to represent -/// the position and orientation of rigid frames. -struct b2Transform -{ - /// The default constructor does nothing. - b2Transform() {} - - /// Initialize using a position vector and a rotation. - b2Transform(const b2Vec2& position, const b2Rot& rotation) : p(position), q(rotation) {} - - /// Set this to the identity transform. - void SetIdentity() - { - p.SetZero(); - q.SetIdentity(); - } - - /// Set this based on the position and angle. - void Set(const b2Vec2& position, float32 angle) - { - p = position; - q.Set(angle); - } - - b2Vec2 p; - b2Rot q; -}; - -/// This describes the motion of a body/shape for TOI computation. -/// Shapes are defined with respect to the body origin, which may -/// no coincide with the center of mass. However, to support dynamics -/// we must interpolate the center of mass position. -struct b2Sweep -{ - /// Get the interpolated transform at a specific time. - /// @param beta is a factor in [0,1], where 0 indicates alpha0. - void GetTransform(b2Transform* xfb, float32 beta) const; - - /// Advance the sweep forward, yielding a new initial state. - /// @param alpha the new initial time. - void Advance(float32 alpha); - - /// Normalize the angles. - void Normalize(); - - b2Vec2 localCenter; ///< local center of mass position - b2Vec2 c0, c; ///< center world positions - float32 a0, a; ///< world angles - - /// Fraction of the current time step in the range [0,1] - /// c0 and a0 are the positions at alpha0. - float32 alpha0; -}; - -/// Useful constant -extern const b2Vec2 b2Vec2_zero; - -/// Perform the dot product on two vectors. -inline float32 b2Dot(const b2Vec2& a, const b2Vec2& b) -{ - return a.x * b.x + a.y * b.y; -} - -/// Perform the cross product on two vectors. In 2D this produces a scalar. -inline float32 b2Cross(const b2Vec2& a, const b2Vec2& b) -{ - return a.x * b.y - a.y * b.x; -} - -/// Perform the cross product on a vector and a scalar. In 2D this produces -/// a vector. -inline b2Vec2 b2Cross(const b2Vec2& a, float32 s) -{ - return b2Vec2(s * a.y, -s * a.x); -} - -/// Perform the cross product on a scalar and a vector. In 2D this produces -/// a vector. -inline b2Vec2 b2Cross(float32 s, const b2Vec2& a) -{ - return b2Vec2(-s * a.y, s * a.x); -} - -/// Multiply a matrix times a vector. If a rotation matrix is provided, -/// then this transforms the vector from one frame to another. -inline b2Vec2 b2Mul(const b2Mat22& A, const b2Vec2& v) -{ - return b2Vec2(A.ex.x * v.x + A.ey.x * v.y, A.ex.y * v.x + A.ey.y * v.y); -} - -/// Multiply a matrix transpose times a vector. If a rotation matrix is provided, -/// then this transforms the vector from one frame to another (inverse transform). -inline b2Vec2 b2MulT(const b2Mat22& A, const b2Vec2& v) -{ - return b2Vec2(b2Dot(v, A.ex), b2Dot(v, A.ey)); -} - -/// Add two vectors component-wise. -inline b2Vec2 operator + (const b2Vec2& a, const b2Vec2& b) -{ - return b2Vec2(a.x + b.x, a.y + b.y); -} - -/// Subtract two vectors component-wise. -inline b2Vec2 operator - (const b2Vec2& a, const b2Vec2& b) -{ - return b2Vec2(a.x - b.x, a.y - b.y); -} - -inline b2Vec2 operator * (float32 s, const b2Vec2& a) -{ - return b2Vec2(s * a.x, s * a.y); -} - -inline bool operator == (const b2Vec2& a, const b2Vec2& b) -{ - return a.x == b.x && a.y == b.y; -} - -inline float32 b2Distance(const b2Vec2& a, const b2Vec2& b) -{ - b2Vec2 c = a - b; - return c.Length(); -} - -inline float32 b2DistanceSquared(const b2Vec2& a, const b2Vec2& b) -{ - b2Vec2 c = a - b; - return b2Dot(c, c); -} - -inline b2Vec3 operator * (float32 s, const b2Vec3& a) -{ - return b2Vec3(s * a.x, s * a.y, s * a.z); -} - -/// Add two vectors component-wise. -inline b2Vec3 operator + (const b2Vec3& a, const b2Vec3& b) -{ - return b2Vec3(a.x + b.x, a.y + b.y, a.z + b.z); -} - -/// Subtract two vectors component-wise. -inline b2Vec3 operator - (const b2Vec3& a, const b2Vec3& b) -{ - return b2Vec3(a.x - b.x, a.y - b.y, a.z - b.z); -} - -/// Perform the dot product on two vectors. -inline float32 b2Dot(const b2Vec3& a, const b2Vec3& b) -{ - return a.x * b.x + a.y * b.y + a.z * b.z; -} - -/// Perform the cross product on two vectors. -inline b2Vec3 b2Cross(const b2Vec3& a, const b2Vec3& b) -{ - return b2Vec3(a.y * b.z - a.z * b.y, a.z * b.x - a.x * b.z, a.x * b.y - a.y * b.x); -} - -inline b2Mat22 operator + (const b2Mat22& A, const b2Mat22& B) -{ - return b2Mat22(A.ex + B.ex, A.ey + B.ey); -} - -// A * B -inline b2Mat22 b2Mul(const b2Mat22& A, const b2Mat22& B) -{ - return b2Mat22(b2Mul(A, B.ex), b2Mul(A, B.ey)); -} - -// A^T * B -inline b2Mat22 b2MulT(const b2Mat22& A, const b2Mat22& B) -{ - b2Vec2 c1(b2Dot(A.ex, B.ex), b2Dot(A.ey, B.ex)); - b2Vec2 c2(b2Dot(A.ex, B.ey), b2Dot(A.ey, B.ey)); - return b2Mat22(c1, c2); -} - -/// Multiply a matrix times a vector. -inline b2Vec3 b2Mul(const b2Mat33& A, const b2Vec3& v) -{ - return v.x * A.ex + v.y * A.ey + v.z * A.ez; -} - -/// Multiply a matrix times a vector. -inline b2Vec2 b2Mul22(const b2Mat33& A, const b2Vec2& v) -{ - return b2Vec2(A.ex.x * v.x + A.ey.x * v.y, A.ex.y * v.x + A.ey.y * v.y); -} - -/// Multiply two rotations: q * r -inline b2Rot b2Mul(const b2Rot& q, const b2Rot& r) -{ - // [qc -qs] * [rc -rs] = [qc*rc-qs*rs -qc*rs-qs*rc] - // [qs qc] [rs rc] [qs*rc+qc*rs -qs*rs+qc*rc] - // s = qs * rc + qc * rs - // c = qc * rc - qs * rs - b2Rot qr; - qr.s = q.s * r.c + q.c * r.s; - qr.c = q.c * r.c - q.s * r.s; - return qr; -} - -/// Transpose multiply two rotations: qT * r -inline b2Rot b2MulT(const b2Rot& q, const b2Rot& r) -{ - // [ qc qs] * [rc -rs] = [qc*rc+qs*rs -qc*rs+qs*rc] - // [-qs qc] [rs rc] [-qs*rc+qc*rs qs*rs+qc*rc] - // s = qc * rs - qs * rc - // c = qc * rc + qs * rs - b2Rot qr; - qr.s = q.c * r.s - q.s * r.c; - qr.c = q.c * r.c + q.s * r.s; - return qr; -} - -/// Rotate a vector -inline b2Vec2 b2Mul(const b2Rot& q, const b2Vec2& v) -{ - return b2Vec2(q.c * v.x - q.s * v.y, q.s * v.x + q.c * v.y); -} - -/// Inverse rotate a vector -inline b2Vec2 b2MulT(const b2Rot& q, const b2Vec2& v) -{ - return b2Vec2(q.c * v.x + q.s * v.y, -q.s * v.x + q.c * v.y); -} - -inline b2Vec2 b2Mul(const b2Transform& T, const b2Vec2& v) -{ - float32 x = (T.q.c * v.x - T.q.s * v.y) + T.p.x; - float32 y = (T.q.s * v.x + T.q.c * v.y) + T.p.y; - - return b2Vec2(x, y); -} - -inline b2Vec2 b2MulT(const b2Transform& T, const b2Vec2& v) -{ - float32 px = v.x - T.p.x; - float32 py = v.y - T.p.y; - float32 x = (T.q.c * px + T.q.s * py); - float32 y = (-T.q.s * px + T.q.c * py); - - return b2Vec2(x, y); -} - -// v2 = A.q.Rot(B.q.Rot(v1) + B.p) + A.p -// = (A.q * B.q).Rot(v1) + A.q.Rot(B.p) + A.p -inline b2Transform b2Mul(const b2Transform& A, const b2Transform& B) -{ - b2Transform C; - C.q = b2Mul(A.q, B.q); - C.p = b2Mul(A.q, B.p) + A.p; - return C; -} - -// v2 = A.q' * (B.q * v1 + B.p - A.p) -// = A.q' * B.q * v1 + A.q' * (B.p - A.p) -inline b2Transform b2MulT(const b2Transform& A, const b2Transform& B) -{ - b2Transform C; - C.q = b2MulT(A.q, B.q); - C.p = b2MulT(A.q, B.p - A.p); - return C; -} - -template -inline T b2Abs(T a) -{ - return a > T(0) ? a : -a; -} - -inline b2Vec2 b2Abs(const b2Vec2& a) -{ - return b2Vec2(b2Abs(a.x), b2Abs(a.y)); -} - -inline b2Mat22 b2Abs(const b2Mat22& A) -{ - return b2Mat22(b2Abs(A.ex), b2Abs(A.ey)); -} - -template -inline T b2Min(T a, T b) -{ - return a < b ? a : b; -} - -inline b2Vec2 b2Min(const b2Vec2& a, const b2Vec2& b) -{ - return b2Vec2(b2Min(a.x, b.x), b2Min(a.y, b.y)); -} - -template -inline T b2Max(T a, T b) -{ - return a > b ? a : b; -} - -inline b2Vec2 b2Max(const b2Vec2& a, const b2Vec2& b) -{ - return b2Vec2(b2Max(a.x, b.x), b2Max(a.y, b.y)); -} - -template -inline T b2Clamp(T a, T low, T high) -{ - return b2Max(low, b2Min(a, high)); -} - -inline b2Vec2 b2Clamp(const b2Vec2& a, const b2Vec2& low, const b2Vec2& high) -{ - return b2Max(low, b2Min(a, high)); -} - -template inline void b2Swap(T& a, T& b) -{ - T tmp = a; - a = b; - b = tmp; -} - -/// "Next Largest Power of 2 -/// Given a binary integer value x, the next largest power of 2 can be computed by a SWAR algorithm -/// that recursively "folds" the upper bits into the lower bits. This process yields a bit vector with -/// the same most significant 1 as x, but all 1's below it. Adding 1 to that value yields the next -/// largest power of 2. For a 32-bit value:" -inline uint32 b2NextPowerOfTwo(uint32 x) -{ - x |= (x >> 1); - x |= (x >> 2); - x |= (x >> 4); - x |= (x >> 8); - x |= (x >> 16); - return x + 1; -} - -inline bool b2IsPowerOfTwo(uint32 x) -{ - bool result = x > 0 && (x & (x - 1)) == 0; - return result; -} - -inline void b2Sweep::GetTransform(b2Transform* xf, float32 beta) const -{ - xf->p = (1.0f - beta) * c0 + beta * c; - float32 angle = (1.0f - beta) * a0 + beta * a; - xf->q.Set(angle); - - // Shift to origin - xf->p -= b2Mul(xf->q, localCenter); -} - -inline void b2Sweep::Advance(float32 alpha) -{ - b2Assert(alpha0 < 1.0f); - float32 beta = (alpha - alpha0) / (1.0f - alpha0); - c0 += beta * (c - c0); - a0 += beta * (a - a0); - alpha0 = alpha; -} - -/// Normalize an angle in radians to be between -pi and pi -inline void b2Sweep::Normalize() -{ - float32 twoPi = 2.0f * b2_pi; - float32 d = twoPi * floorf(a0 / twoPi); - a0 -= d; - a -= d; -} - -#endif diff --git a/Box2D/Common/b2Settings.cpp b/Box2D/Common/b2Settings.cpp deleted file mode 100644 index c780260800..0000000000 --- a/Box2D/Common/b2Settings.cpp +++ /dev/null @@ -1,44 +0,0 @@ -/* -* 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 -#include -#include -#include - -b2Version b2_version = {2, 3, 0}; - -// Memory allocators. Modify these to use your own allocator. -void* b2Alloc(int32 size) -{ - return malloc(size); -} - -void b2Free(void* mem) -{ - free(mem); -} - -// You can modify this to use your logging facility. -void b2Log(const char* string, ...) -{ - va_list args; - va_start(args, string); - vprintf(string, args); - va_end(args); -} diff --git a/Box2D/Common/b2Settings.h b/Box2D/Common/b2Settings.h deleted file mode 100644 index 86f76bd267..0000000000 --- a/Box2D/Common/b2Settings.h +++ /dev/null @@ -1,151 +0,0 @@ -/* -* 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_SETTINGS_H -#define B2_SETTINGS_H - -#include -#include -#include - -#define B2_NOT_USED(x) ((void)(x)) -#define b2Assert(A) assert(A) - -typedef signed char int8; -typedef signed short int16; -typedef signed int int32; -typedef unsigned char uint8; -typedef unsigned short uint16; -typedef unsigned int uint32; -typedef float float32; -typedef double float64; - -#define b2_maxFloat FLT_MAX -#define b2_epsilon FLT_EPSILON -#define b2_pi 3.14159265359f - -/// @file -/// Global tuning constants based on meters-kilograms-seconds (MKS) units. -/// - -// Collision - -/// The maximum number of contact points between two convex shapes. Do -/// not change this value. -#define b2_maxManifoldPoints 2 - -/// The maximum number of vertices on a convex polygon. You cannot increase -/// this too much because b2BlockAllocator has a maximum object size. -#define b2_maxPolygonVertices 8 - -/// This is used to fatten AABBs in the dynamic tree. This allows proxies -/// to move by a small amount without triggering a tree adjustment. -/// This is in meters. -#define b2_aabbExtension 0.1f - -/// This is used to fatten AABBs in the dynamic tree. This is used to predict -/// the future position based on the current displacement. -/// This is a dimensionless multiplier. -#define b2_aabbMultiplier 2.0f - -/// A small length used as a collision and constraint tolerance. Usually it is -/// chosen to be numerically significant, but visually insignificant. -#define b2_linearSlop 0.005f - -/// A small angle used as a collision and constraint tolerance. Usually it is -/// chosen to be numerically significant, but visually insignificant. -#define b2_angularSlop (2.0f / 180.0f * b2_pi) - -/// The radius of the polygon/edge shape skin. This should not be modified. Making -/// this smaller means polygons will have an insufficient buffer for continuous collision. -/// Making it larger may create artifacts for vertex collision. -#define b2_polygonRadius (2.0f * b2_linearSlop) - -/// Maximum number of sub-steps per contact in continuous physics simulation. -#define b2_maxSubSteps 8 - - -// Dynamics - -/// Maximum number of contacts to be handled to solve a TOI impact. -#define b2_maxTOIContacts 32 - -/// A velocity threshold for elastic collisions. Any collision with a relative linear -/// velocity below this threshold will be treated as inelastic. -#define b2_velocityThreshold 1.0f - -/// The maximum linear position correction used when solving constraints. This helps to -/// prevent overshoot. -#define b2_maxLinearCorrection 0.2f - -/// The maximum angular position correction used when solving constraints. This helps to -/// prevent overshoot. -#define b2_maxAngularCorrection (8.0f / 180.0f * b2_pi) - -/// The maximum linear velocity of a body. This limit is very large and is used -/// to prevent numerical problems. You shouldn't need to adjust this. -#define b2_maxTranslation 2.0f -#define b2_maxTranslationSquared (b2_maxTranslation * b2_maxTranslation) - -/// The maximum angular velocity of a body. This limit is very large and is used -/// to prevent numerical problems. You shouldn't need to adjust this. -#define b2_maxRotation (0.5f * b2_pi) -#define b2_maxRotationSquared (b2_maxRotation * b2_maxRotation) - -/// This scale factor controls how fast overlap is resolved. Ideally this would be 1 so -/// that overlap is removed in one time step. However using values close to 1 often lead -/// to overshoot. -#define b2_baumgarte 0.2f -#define b2_toiBaugarte 0.75f - - -// Sleep - -/// The time that a body must be still before it will go to sleep. -#define b2_timeToSleep 0.5f - -/// A body cannot sleep if its linear velocity is above this tolerance. -#define b2_linearSleepTolerance 0.01f - -/// A body cannot sleep if its angular velocity is above this tolerance. -#define b2_angularSleepTolerance (2.0f / 180.0f * b2_pi) - -// Memory Allocation - -/// Implement this function to use your own memory allocator. -void* b2Alloc(int32 size); - -/// If you implement b2Alloc, you should also implement this function. -void b2Free(void* mem); - -/// Logging function. -void b2Log(const char* string, ...); - -/// Version numbering scheme. -/// See http://en.wikipedia.org/wiki/Software_versioning -struct b2Version -{ - int32 major; ///< significant changes - int32 minor; ///< incremental changes - int32 revision; ///< bug fixes -}; - -/// Current version. -extern b2Version b2_version; - -#endif diff --git a/Box2D/Common/b2StackAllocator.cpp b/Box2D/Common/b2StackAllocator.cpp deleted file mode 100644 index 321cabb232..0000000000 --- a/Box2D/Common/b2StackAllocator.cpp +++ /dev/null @@ -1,83 +0,0 @@ -/* -* 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 -#include - -b2StackAllocator::b2StackAllocator() -{ - m_index = 0; - m_allocation = 0; - m_maxAllocation = 0; - m_entryCount = 0; -} - -b2StackAllocator::~b2StackAllocator() -{ - b2Assert(m_index == 0); - b2Assert(m_entryCount == 0); -} - -void* b2StackAllocator::Allocate(int32 size) -{ - b2Assert(m_entryCount < b2_maxStackEntries); - - b2StackEntry* entry = m_entries + m_entryCount; - entry->size = size; - if (m_index + size > b2_stackSize) - { - entry->data = (char*)b2Alloc(size); - entry->usedMalloc = true; - } - else - { - entry->data = m_data + m_index; - entry->usedMalloc = false; - m_index += size; - } - - m_allocation += size; - m_maxAllocation = b2Max(m_maxAllocation, m_allocation); - ++m_entryCount; - - return entry->data; -} - -void b2StackAllocator::Free(void* p) -{ - b2Assert(m_entryCount > 0); - b2StackEntry* entry = m_entries + m_entryCount - 1; - b2Assert(p == entry->data); - if (entry->usedMalloc) - { - b2Free(p); - } - else - { - m_index -= entry->size; - } - m_allocation -= entry->size; - --m_entryCount; - - p = NULL; -} - -int32 b2StackAllocator::GetMaxAllocation() const -{ - return m_maxAllocation; -} diff --git a/Box2D/Common/b2StackAllocator.h b/Box2D/Common/b2StackAllocator.h deleted file mode 100644 index bb87a7edc9..0000000000 --- a/Box2D/Common/b2StackAllocator.h +++ /dev/null @@ -1,60 +0,0 @@ -/* -* 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_STACK_ALLOCATOR_H -#define B2_STACK_ALLOCATOR_H - -#include - -const int32 b2_stackSize = 100 * 1024; // 100k -const int32 b2_maxStackEntries = 32; - -struct b2StackEntry -{ - char* data; - int32 size; - bool usedMalloc; -}; - -// This is a stack allocator used for fast per step allocations. -// You must nest allocate/free pairs. The code will assert -// if you try to interleave multiple allocate/free pairs. -class b2StackAllocator -{ -public: - b2StackAllocator(); - ~b2StackAllocator(); - - void* Allocate(int32 size); - void Free(void* p); - - int32 GetMaxAllocation() const; - -private: - - char m_data[b2_stackSize]; - int32 m_index; - - int32 m_allocation; - int32 m_maxAllocation; - - b2StackEntry m_entries[b2_maxStackEntries]; - int32 m_entryCount; -}; - -#endif diff --git a/Box2D/Common/b2Timer.cpp b/Box2D/Common/b2Timer.cpp deleted file mode 100644 index cbea5309e4..0000000000 --- a/Box2D/Common/b2Timer.cpp +++ /dev/null @@ -1,101 +0,0 @@ -/* -* Copyright (c) 2011 Erin Catto http://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 - -#if defined(_WIN32) - -float64 b2Timer::s_invFrequency = 0.0f; - -#define WIN32_LEAN_AND_MEAN -#include - -b2Timer::b2Timer() -{ - LARGE_INTEGER largeInteger; - - if (s_invFrequency == 0.0f) - { - QueryPerformanceFrequency(&largeInteger); - s_invFrequency = float64(largeInteger.QuadPart); - if (s_invFrequency > 0.0f) - { - s_invFrequency = 1000.0f / s_invFrequency; - } - } - - QueryPerformanceCounter(&largeInteger); - m_start = float64(largeInteger.QuadPart); -} - -void b2Timer::Reset() -{ - LARGE_INTEGER largeInteger; - QueryPerformanceCounter(&largeInteger); - m_start = float64(largeInteger.QuadPart); -} - -float32 b2Timer::GetMilliseconds() const -{ - LARGE_INTEGER largeInteger; - QueryPerformanceCounter(&largeInteger); - float64 count = float64(largeInteger.QuadPart); - float32 ms = float32(s_invFrequency * (count - m_start)); - return ms; -} - -#elif defined(__linux__) || defined (__APPLE__) - -#include - -b2Timer::b2Timer() -{ - Reset(); -} - -void b2Timer::Reset() -{ - timeval t; - gettimeofday(&t, 0); - m_start_sec = t.tv_sec; - m_start_usec = t.tv_usec; -} - -float32 b2Timer::GetMilliseconds() const -{ - timeval t; - gettimeofday(&t, 0); - return 1000.0f * (t.tv_sec - m_start_sec) + 0.001f * (t.tv_usec - m_start_usec); -} - -#else - -b2Timer::b2Timer() -{ -} - -void b2Timer::Reset() -{ -} - -float32 b2Timer::GetMilliseconds() const -{ - return 0.0f; -} - -#endif diff --git a/Box2D/Common/b2Timer.h b/Box2D/Common/b2Timer.h deleted file mode 100644 index edf1ca4595..0000000000 --- a/Box2D/Common/b2Timer.h +++ /dev/null @@ -1,50 +0,0 @@ -/* -* Copyright (c) 2011 Erin Catto http://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_TIMER_H -#define B2_TIMER_H - -#include - -/// Timer for profiling. This has platform specific code and may -/// not work on every platform. -class b2Timer -{ -public: - - /// Constructor - b2Timer(); - - /// Reset the timer. - void Reset(); - - /// Get the time since construction or the last reset. - float32 GetMilliseconds() const; - -private: - -#if defined(_WIN32) - float64 m_start; - static float64 s_invFrequency; -#elif defined(__linux__) || defined (__APPLE__) - unsigned long m_start_sec; - unsigned long m_start_usec; -#endif -}; - -#endif diff --git a/Box2D/Dynamics/Contacts/b2ChainAndCircleContact.cpp b/Box2D/Dynamics/Contacts/b2ChainAndCircleContact.cpp deleted file mode 100644 index 01c921f3db..0000000000 --- a/Box2D/Dynamics/Contacts/b2ChainAndCircleContact.cpp +++ /dev/null @@ -1,53 +0,0 @@ -/* -* 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 -#include -#include -#include -#include - -#include - -b2Contact* b2ChainAndCircleContact::Create(b2Fixture* fixtureA, int32 indexA, b2Fixture* fixtureB, int32 indexB, b2BlockAllocator* allocator) -{ - void* mem = allocator->Allocate(sizeof(b2ChainAndCircleContact)); - return new (mem) b2ChainAndCircleContact(fixtureA, indexA, fixtureB, indexB); -} - -void b2ChainAndCircleContact::Destroy(b2Contact* contact, b2BlockAllocator* allocator) -{ - ((b2ChainAndCircleContact*)contact)->~b2ChainAndCircleContact(); - allocator->Free(contact, sizeof(b2ChainAndCircleContact)); -} - -b2ChainAndCircleContact::b2ChainAndCircleContact(b2Fixture* fixtureA, int32 indexA, b2Fixture* fixtureB, int32 indexB) -: b2Contact(fixtureA, indexA, fixtureB, indexB) -{ - b2Assert(m_fixtureA->GetType() == b2Shape::e_chain); - b2Assert(m_fixtureB->GetType() == b2Shape::e_circle); -} - -void b2ChainAndCircleContact::Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB) -{ - b2ChainShape* chain = (b2ChainShape*)m_fixtureA->GetShape(); - b2EdgeShape edge; - chain->GetChildEdge(&edge, m_indexA); - b2CollideEdgeAndCircle( manifold, &edge, xfA, - (b2CircleShape*)m_fixtureB->GetShape(), xfB); -} diff --git a/Box2D/Dynamics/Contacts/b2ChainAndCircleContact.h b/Box2D/Dynamics/Contacts/b2ChainAndCircleContact.h deleted file mode 100644 index b93c30edc4..0000000000 --- a/Box2D/Dynamics/Contacts/b2ChainAndCircleContact.h +++ /dev/null @@ -1,39 +0,0 @@ -/* -* 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_CHAIN_AND_CIRCLE_CONTACT_H -#define B2_CHAIN_AND_CIRCLE_CONTACT_H - -#include - -class b2BlockAllocator; - -class b2ChainAndCircleContact : public b2Contact -{ -public: - static b2Contact* Create( b2Fixture* fixtureA, int32 indexA, - b2Fixture* fixtureB, int32 indexB, b2BlockAllocator* allocator); - static void Destroy(b2Contact* contact, b2BlockAllocator* allocator); - - b2ChainAndCircleContact(b2Fixture* fixtureA, int32 indexA, b2Fixture* fixtureB, int32 indexB); - ~b2ChainAndCircleContact() {} - - void Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB); -}; - -#endif diff --git a/Box2D/Dynamics/Contacts/b2ChainAndPolygonContact.cpp b/Box2D/Dynamics/Contacts/b2ChainAndPolygonContact.cpp deleted file mode 100644 index 4642fa46ab..0000000000 --- a/Box2D/Dynamics/Contacts/b2ChainAndPolygonContact.cpp +++ /dev/null @@ -1,53 +0,0 @@ -/* -* 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 -#include -#include -#include -#include - -#include - -b2Contact* b2ChainAndPolygonContact::Create(b2Fixture* fixtureA, int32 indexA, b2Fixture* fixtureB, int32 indexB, b2BlockAllocator* allocator) -{ - void* mem = allocator->Allocate(sizeof(b2ChainAndPolygonContact)); - return new (mem) b2ChainAndPolygonContact(fixtureA, indexA, fixtureB, indexB); -} - -void b2ChainAndPolygonContact::Destroy(b2Contact* contact, b2BlockAllocator* allocator) -{ - ((b2ChainAndPolygonContact*)contact)->~b2ChainAndPolygonContact(); - allocator->Free(contact, sizeof(b2ChainAndPolygonContact)); -} - -b2ChainAndPolygonContact::b2ChainAndPolygonContact(b2Fixture* fixtureA, int32 indexA, b2Fixture* fixtureB, int32 indexB) -: b2Contact(fixtureA, indexA, fixtureB, indexB) -{ - b2Assert(m_fixtureA->GetType() == b2Shape::e_chain); - b2Assert(m_fixtureB->GetType() == b2Shape::e_polygon); -} - -void b2ChainAndPolygonContact::Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB) -{ - b2ChainShape* chain = (b2ChainShape*)m_fixtureA->GetShape(); - b2EdgeShape edge; - chain->GetChildEdge(&edge, m_indexA); - b2CollideEdgeAndPolygon( manifold, &edge, xfA, - (b2PolygonShape*)m_fixtureB->GetShape(), xfB); -} diff --git a/Box2D/Dynamics/Contacts/b2ChainAndPolygonContact.h b/Box2D/Dynamics/Contacts/b2ChainAndPolygonContact.h deleted file mode 100644 index 0c09af68a1..0000000000 --- a/Box2D/Dynamics/Contacts/b2ChainAndPolygonContact.h +++ /dev/null @@ -1,39 +0,0 @@ -/* -* 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_CHAIN_AND_POLYGON_CONTACT_H -#define B2_CHAIN_AND_POLYGON_CONTACT_H - -#include - -class b2BlockAllocator; - -class b2ChainAndPolygonContact : public b2Contact -{ -public: - static b2Contact* Create( b2Fixture* fixtureA, int32 indexA, - b2Fixture* fixtureB, int32 indexB, b2BlockAllocator* allocator); - static void Destroy(b2Contact* contact, b2BlockAllocator* allocator); - - b2ChainAndPolygonContact(b2Fixture* fixtureA, int32 indexA, b2Fixture* fixtureB, int32 indexB); - ~b2ChainAndPolygonContact() {} - - void Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB); -}; - -#endif diff --git a/Box2D/Dynamics/Contacts/b2CircleContact.cpp b/Box2D/Dynamics/Contacts/b2CircleContact.cpp deleted file mode 100644 index b47f35aed0..0000000000 --- a/Box2D/Dynamics/Contacts/b2CircleContact.cpp +++ /dev/null @@ -1,52 +0,0 @@ -/* -* 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 -#include -#include -#include -#include -#include - -#include - -b2Contact* b2CircleContact::Create(b2Fixture* fixtureA, int32, b2Fixture* fixtureB, int32, b2BlockAllocator* allocator) -{ - void* mem = allocator->Allocate(sizeof(b2CircleContact)); - return new (mem) b2CircleContact(fixtureA, fixtureB); -} - -void b2CircleContact::Destroy(b2Contact* contact, b2BlockAllocator* allocator) -{ - ((b2CircleContact*)contact)->~b2CircleContact(); - allocator->Free(contact, sizeof(b2CircleContact)); -} - -b2CircleContact::b2CircleContact(b2Fixture* fixtureA, b2Fixture* fixtureB) - : b2Contact(fixtureA, 0, fixtureB, 0) -{ - b2Assert(m_fixtureA->GetType() == b2Shape::e_circle); - b2Assert(m_fixtureB->GetType() == b2Shape::e_circle); -} - -void b2CircleContact::Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB) -{ - b2CollideCircles(manifold, - (b2CircleShape*)m_fixtureA->GetShape(), xfA, - (b2CircleShape*)m_fixtureB->GetShape(), xfB); -} diff --git a/Box2D/Dynamics/Contacts/b2CircleContact.h b/Box2D/Dynamics/Contacts/b2CircleContact.h deleted file mode 100644 index 079d3abcdb..0000000000 --- a/Box2D/Dynamics/Contacts/b2CircleContact.h +++ /dev/null @@ -1,39 +0,0 @@ -/* -* 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_CONTACT_H -#define B2_CIRCLE_CONTACT_H - -#include - -class b2BlockAllocator; - -class b2CircleContact : public b2Contact -{ -public: - static b2Contact* Create( b2Fixture* fixtureA, int32 indexA, - b2Fixture* fixtureB, int32 indexB, b2BlockAllocator* allocator); - static void Destroy(b2Contact* contact, b2BlockAllocator* allocator); - - b2CircleContact(b2Fixture* fixtureA, b2Fixture* fixtureB); - ~b2CircleContact() {} - - void Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB); -}; - -#endif diff --git a/Box2D/Dynamics/Contacts/b2Contact.cpp b/Box2D/Dynamics/Contacts/b2Contact.cpp deleted file mode 100644 index d9f1021e4d..0000000000 --- a/Box2D/Dynamics/Contacts/b2Contact.cpp +++ /dev/null @@ -1,247 +0,0 @@ -/* -* 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 -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include - -b2ContactRegister b2Contact::s_registers[b2Shape::e_typeCount][b2Shape::e_typeCount]; -bool b2Contact::s_initialized = false; - -void b2Contact::InitializeRegisters() -{ - 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(b2EdgeAndCircleContact::Create, b2EdgeAndCircleContact::Destroy, b2Shape::e_edge, b2Shape::e_circle); - AddType(b2EdgeAndPolygonContact::Create, b2EdgeAndPolygonContact::Destroy, b2Shape::e_edge, b2Shape::e_polygon); - AddType(b2ChainAndCircleContact::Create, b2ChainAndCircleContact::Destroy, b2Shape::e_chain, b2Shape::e_circle); - AddType(b2ChainAndPolygonContact::Create, b2ChainAndPolygonContact::Destroy, b2Shape::e_chain, b2Shape::e_polygon); -} - -void b2Contact::AddType(b2ContactCreateFcn* createFcn, b2ContactDestroyFcn* destoryFcn, - b2Shape::Type type1, b2Shape::Type type2) -{ - b2Assert(0 <= type1 && type1 < b2Shape::e_typeCount); - b2Assert(0 <= type2 && type2 < b2Shape::e_typeCount); - - s_registers[type1][type2].createFcn = createFcn; - s_registers[type1][type2].destroyFcn = destoryFcn; - s_registers[type1][type2].primary = true; - - if (type1 != type2) - { - s_registers[type2][type1].createFcn = createFcn; - s_registers[type2][type1].destroyFcn = destoryFcn; - s_registers[type2][type1].primary = false; - } -} - -b2Contact* b2Contact::Create(b2Fixture* fixtureA, int32 indexA, b2Fixture* fixtureB, int32 indexB, b2BlockAllocator* allocator) -{ - if (s_initialized == false) - { - InitializeRegisters(); - s_initialized = true; - } - - b2Shape::Type type1 = fixtureA->GetType(); - b2Shape::Type type2 = fixtureB->GetType(); - - b2Assert(0 <= type1 && type1 < b2Shape::e_typeCount); - b2Assert(0 <= type2 && type2 < b2Shape::e_typeCount); - - b2ContactCreateFcn* createFcn = s_registers[type1][type2].createFcn; - if (createFcn) - { - if (s_registers[type1][type2].primary) - { - return createFcn(fixtureA, indexA, fixtureB, indexB, allocator); - } - else - { - return createFcn(fixtureB, indexB, fixtureA, indexA, allocator); - } - } - else - { - return NULL; - } -} - -void b2Contact::Destroy(b2Contact* contact, b2BlockAllocator* allocator) -{ - b2Assert(s_initialized == true); - - b2Fixture* fixtureA = contact->m_fixtureA; - b2Fixture* fixtureB = contact->m_fixtureB; - - if (contact->m_manifold.pointCount > 0 && - fixtureA->IsSensor() == false && - fixtureB->IsSensor() == false) - { - fixtureA->GetBody()->SetAwake(true); - fixtureB->GetBody()->SetAwake(true); - } - - b2Shape::Type typeA = fixtureA->GetType(); - b2Shape::Type typeB = fixtureB->GetType(); - - b2Assert(0 <= typeA && typeB < b2Shape::e_typeCount); - b2Assert(0 <= typeA && typeB < b2Shape::e_typeCount); - - b2ContactDestroyFcn* destroyFcn = s_registers[typeA][typeB].destroyFcn; - destroyFcn(contact, allocator); -} - -b2Contact::b2Contact(b2Fixture* fA, int32 indexA, b2Fixture* fB, int32 indexB) -{ - m_flags = e_enabledFlag; - - m_fixtureA = fA; - m_fixtureB = fB; - - m_indexA = indexA; - m_indexB = indexB; - - m_manifold.pointCount = 0; - - m_prev = NULL; - m_next = NULL; - - m_nodeA.contact = NULL; - m_nodeA.prev = NULL; - m_nodeA.next = NULL; - m_nodeA.other = NULL; - - m_nodeB.contact = NULL; - m_nodeB.prev = NULL; - m_nodeB.next = NULL; - m_nodeB.other = NULL; - - m_toiCount = 0; - - m_friction = b2MixFriction(m_fixtureA->m_friction, m_fixtureB->m_friction); - m_restitution = b2MixRestitution(m_fixtureA->m_restitution, m_fixtureB->m_restitution); - - m_tangentSpeed = 0.0f; -} - -// Update the contact manifold and touching status. -// Note: do not assume the fixture AABBs are overlapping or are valid. -void b2Contact::Update(b2ContactListener* listener) -{ - b2Manifold oldManifold = m_manifold; - - // Re-enable this contact. - m_flags |= e_enabledFlag; - - bool touching = false; - bool wasTouching = (m_flags & e_touchingFlag) == e_touchingFlag; - - bool sensorA = m_fixtureA->IsSensor(); - bool sensorB = m_fixtureB->IsSensor(); - bool sensor = sensorA || sensorB; - - b2Body* bodyA = m_fixtureA->GetBody(); - b2Body* bodyB = m_fixtureB->GetBody(); - const b2Transform& xfA = bodyA->GetTransform(); - const b2Transform& xfB = bodyB->GetTransform(); - - // Is this contact a sensor? - if (sensor) - { - const b2Shape* shapeA = m_fixtureA->GetShape(); - const b2Shape* shapeB = m_fixtureB->GetShape(); - touching = b2TestOverlap(shapeA, m_indexA, shapeB, m_indexB, xfA, xfB); - - // Sensors don't generate manifolds. - m_manifold.pointCount = 0; - } - else - { - Evaluate(&m_manifold, xfA, xfB); - touching = m_manifold.pointCount > 0; - - // Match old contact ids to new contact ids and copy the - // stored impulses to warm start the solver. - for (int32 i = 0; i < m_manifold.pointCount; ++i) - { - b2ManifoldPoint* mp2 = m_manifold.points + i; - mp2->normalImpulse = 0.0f; - mp2->tangentImpulse = 0.0f; - b2ContactID id2 = mp2->id; - - for (int32 j = 0; j < oldManifold.pointCount; ++j) - { - b2ManifoldPoint* mp1 = oldManifold.points + j; - - if (mp1->id.key == id2.key) - { - mp2->normalImpulse = mp1->normalImpulse; - mp2->tangentImpulse = mp1->tangentImpulse; - break; - } - } - } - - if (touching != wasTouching) - { - bodyA->SetAwake(true); - bodyB->SetAwake(true); - } - } - - if (touching) - { - m_flags |= e_touchingFlag; - } - else - { - m_flags &= ~e_touchingFlag; - } - - if (wasTouching == false && touching == true && listener) - { - listener->BeginContact(this); - } - - if (wasTouching == true && touching == false && listener) - { - listener->EndContact(this); - } - - if (sensor == false && touching && listener) - { - listener->PreSolve(this, &oldManifold); - } -} diff --git a/Box2D/Dynamics/Contacts/b2Contact.h b/Box2D/Dynamics/Contacts/b2Contact.h deleted file mode 100644 index 36e31e2566..0000000000 --- a/Box2D/Dynamics/Contacts/b2Contact.h +++ /dev/null @@ -1,349 +0,0 @@ -/* -* 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_CONTACT_H -#define B2_CONTACT_H - -#include -#include -#include -#include - -class b2Body; -class b2Contact; -class b2Fixture; -class b2World; -class b2BlockAllocator; -class b2StackAllocator; -class b2ContactListener; - -/// Friction mixing law. The idea is to allow either fixture to drive the restitution to zero. -/// For example, anything slides on ice. -inline float32 b2MixFriction(float32 friction1, float32 friction2) -{ - return b2Sqrt(friction1 * friction2); -} - -/// Restitution mixing law. The idea is allow for anything to bounce off an inelastic surface. -/// For example, a superball bounces on anything. -inline float32 b2MixRestitution(float32 restitution1, float32 restitution2) -{ - return restitution1 > restitution2 ? restitution1 : restitution2; -} - -typedef b2Contact* b2ContactCreateFcn( b2Fixture* fixtureA, int32 indexA, - b2Fixture* fixtureB, int32 indexB, - b2BlockAllocator* allocator); -typedef void b2ContactDestroyFcn(b2Contact* contact, b2BlockAllocator* allocator); - -struct b2ContactRegister -{ - b2ContactCreateFcn* createFcn; - b2ContactDestroyFcn* destroyFcn; - bool primary; -}; - -/// A contact edge is used to connect bodies and contacts together -/// in a contact graph where each body is a node and each contact -/// is an edge. A contact edge belongs to a doubly linked list -/// maintained in each attached body. Each contact has two contact -/// nodes, one for each attached body. -struct b2ContactEdge -{ - b2Body* other; ///< provides quick access to the other body attached. - b2Contact* contact; ///< the contact - b2ContactEdge* prev; ///< the previous contact edge in the body's contact list - b2ContactEdge* next; ///< the next contact edge in the body's contact list -}; - -/// The class manages contact between two shapes. A contact exists for each overlapping -/// AABB in the broad-phase (except if filtered). Therefore a contact object may exist -/// that has no contact points. -class b2Contact -{ -public: - - /// Get the contact manifold. Do not modify the manifold unless you understand the - /// internals of Box2D. - b2Manifold* GetManifold(); - const b2Manifold* GetManifold() const; - - /// Get the world manifold. - void GetWorldManifold(b2WorldManifold* worldManifold) const; - - /// Is this contact touching? - bool IsTouching() const; - - /// Enable/disable this contact. This can be used inside the pre-solve - /// contact listener. The contact is only disabled for the current - /// time step (or sub-step in continuous collisions). - void SetEnabled(bool flag); - - /// Has this contact been disabled? - bool IsEnabled() const; - - /// Get the next contact in the world's contact list. - b2Contact* GetNext(); - const b2Contact* GetNext() const; - - /// Get fixture A in this contact. - b2Fixture* GetFixtureA(); - const b2Fixture* GetFixtureA() const; - - /// Get the child primitive index for fixture A. - int32 GetChildIndexA() const; - - /// Get fixture B in this contact. - b2Fixture* GetFixtureB(); - const b2Fixture* GetFixtureB() const; - - /// Get the child primitive index for fixture B. - int32 GetChildIndexB() const; - - /// Override the default friction mixture. You can call this in b2ContactListener::PreSolve. - /// This value persists until set or reset. - void SetFriction(float32 friction); - - /// Get the friction. - float32 GetFriction() const; - - /// Reset the friction mixture to the default value. - void ResetFriction(); - - /// Override the default restitution mixture. You can call this in b2ContactListener::PreSolve. - /// The value persists until you set or reset. - void SetRestitution(float32 restitution); - - /// Get the restitution. - float32 GetRestitution() const; - - /// Reset the restitution to the default value. - void ResetRestitution(); - - /// Set the desired tangent speed for a conveyor belt behavior. In meters per second. - void SetTangentSpeed(float32 speed); - - /// Get the desired tangent speed. In meters per second. - float32 GetTangentSpeed() const; - - /// Evaluate this contact with your own manifold and transforms. - virtual void Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB) = 0; - -protected: - friend class b2ContactManager; - friend class b2World; - friend class b2ContactSolver; - friend class b2Body; - friend class b2Fixture; - - // Flags stored in m_flags - enum - { - // Used when crawling contact graph when forming islands. - e_islandFlag = 0x0001, - - // Set when the shapes are touching. - e_touchingFlag = 0x0002, - - // This contact can be disabled (by user) - e_enabledFlag = 0x0004, - - // This contact needs filtering because a fixture filter was changed. - e_filterFlag = 0x0008, - - // This bullet contact had a TOI event - e_bulletHitFlag = 0x0010, - - // This contact has a valid TOI in m_toi - e_toiFlag = 0x0020 - }; - - /// Flag this contact for filtering. Filtering will occur the next time step. - void FlagForFiltering(); - - static void AddType(b2ContactCreateFcn* createFcn, b2ContactDestroyFcn* destroyFcn, - b2Shape::Type typeA, b2Shape::Type typeB); - static void InitializeRegisters(); - static b2Contact* Create(b2Fixture* fixtureA, int32 indexA, b2Fixture* fixtureB, int32 indexB, b2BlockAllocator* allocator); - static void Destroy(b2Contact* contact, b2Shape::Type typeA, b2Shape::Type typeB, b2BlockAllocator* allocator); - static void Destroy(b2Contact* contact, b2BlockAllocator* allocator); - - b2Contact() : m_fixtureA(NULL), m_fixtureB(NULL) {} - b2Contact(b2Fixture* fixtureA, int32 indexA, b2Fixture* fixtureB, int32 indexB); - virtual ~b2Contact() {} - - void Update(b2ContactListener* listener); - - static b2ContactRegister s_registers[b2Shape::e_typeCount][b2Shape::e_typeCount]; - static bool s_initialized; - - uint32 m_flags; - - // World pool and list pointers. - b2Contact* m_prev; - b2Contact* m_next; - - // Nodes for connecting bodies. - b2ContactEdge m_nodeA; - b2ContactEdge m_nodeB; - - b2Fixture* m_fixtureA; - b2Fixture* m_fixtureB; - - int32 m_indexA; - int32 m_indexB; - - b2Manifold m_manifold; - - int32 m_toiCount; - float32 m_toi; - - float32 m_friction; - float32 m_restitution; - - float32 m_tangentSpeed; -}; - -inline b2Manifold* b2Contact::GetManifold() -{ - return &m_manifold; -} - -inline const b2Manifold* b2Contact::GetManifold() const -{ - return &m_manifold; -} - -inline void b2Contact::GetWorldManifold(b2WorldManifold* worldManifold) const -{ - const b2Body* bodyA = m_fixtureA->GetBody(); - const b2Body* bodyB = m_fixtureB->GetBody(); - const b2Shape* shapeA = m_fixtureA->GetShape(); - const b2Shape* shapeB = m_fixtureB->GetShape(); - - worldManifold->Initialize(&m_manifold, bodyA->GetTransform(), shapeA->m_radius, bodyB->GetTransform(), shapeB->m_radius); -} - -inline void b2Contact::SetEnabled(bool flag) -{ - if (flag) - { - m_flags |= e_enabledFlag; - } - else - { - m_flags &= ~e_enabledFlag; - } -} - -inline bool b2Contact::IsEnabled() const -{ - return (m_flags & e_enabledFlag) == e_enabledFlag; -} - -inline bool b2Contact::IsTouching() const -{ - return (m_flags & e_touchingFlag) == e_touchingFlag; -} - -inline b2Contact* b2Contact::GetNext() -{ - return m_next; -} - -inline const b2Contact* b2Contact::GetNext() const -{ - return m_next; -} - -inline b2Fixture* b2Contact::GetFixtureA() -{ - return m_fixtureA; -} - -inline const b2Fixture* b2Contact::GetFixtureA() const -{ - return m_fixtureA; -} - -inline b2Fixture* b2Contact::GetFixtureB() -{ - return m_fixtureB; -} - -inline int32 b2Contact::GetChildIndexA() const -{ - return m_indexA; -} - -inline const b2Fixture* b2Contact::GetFixtureB() const -{ - return m_fixtureB; -} - -inline int32 b2Contact::GetChildIndexB() const -{ - return m_indexB; -} - -inline void b2Contact::FlagForFiltering() -{ - m_flags |= e_filterFlag; -} - -inline void b2Contact::SetFriction(float32 friction) -{ - m_friction = friction; -} - -inline float32 b2Contact::GetFriction() const -{ - return m_friction; -} - -inline void b2Contact::ResetFriction() -{ - m_friction = b2MixFriction(m_fixtureA->m_friction, m_fixtureB->m_friction); -} - -inline void b2Contact::SetRestitution(float32 restitution) -{ - m_restitution = restitution; -} - -inline float32 b2Contact::GetRestitution() const -{ - return m_restitution; -} - -inline void b2Contact::ResetRestitution() -{ - m_restitution = b2MixRestitution(m_fixtureA->m_restitution, m_fixtureB->m_restitution); -} - -inline void b2Contact::SetTangentSpeed(float32 speed) -{ - m_tangentSpeed = speed; -} - -inline float32 b2Contact::GetTangentSpeed() const -{ - return m_tangentSpeed; -} - -#endif diff --git a/Box2D/Dynamics/Contacts/b2ContactSolver.cpp b/Box2D/Dynamics/Contacts/b2ContactSolver.cpp deleted file mode 100644 index 8224fdc2db..0000000000 --- a/Box2D/Dynamics/Contacts/b2ContactSolver.cpp +++ /dev/null @@ -1,833 +0,0 @@ -/* -* Copyright (c) 2006-2011 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 - -#include -#include -#include -#include -#include - -#define B2_DEBUG_SOLVER 0 - -struct b2ContactPositionConstraint -{ - b2Vec2 localPoints[b2_maxManifoldPoints]; - b2Vec2 localNormal; - b2Vec2 localPoint; - int32 indexA; - int32 indexB; - float32 invMassA, invMassB; - b2Vec2 localCenterA, localCenterB; - float32 invIA, invIB; - b2Manifold::Type type; - float32 radiusA, radiusB; - int32 pointCount; -}; - -b2ContactSolver::b2ContactSolver(b2ContactSolverDef* def) -{ - m_step = def->step; - m_allocator = def->allocator; - m_count = def->count; - m_positionConstraints = (b2ContactPositionConstraint*)m_allocator->Allocate(m_count * sizeof(b2ContactPositionConstraint)); - m_velocityConstraints = (b2ContactVelocityConstraint*)m_allocator->Allocate(m_count * sizeof(b2ContactVelocityConstraint)); - m_positions = def->positions; - m_velocities = def->velocities; - m_contacts = def->contacts; - - // Initialize position independent portions of the constraints. - for (int32 i = 0; i < m_count; ++i) - { - b2Contact* contact = m_contacts[i]; - - b2Fixture* fixtureA = contact->m_fixtureA; - b2Fixture* fixtureB = contact->m_fixtureB; - b2Shape* shapeA = fixtureA->GetShape(); - b2Shape* shapeB = fixtureB->GetShape(); - float32 radiusA = shapeA->m_radius; - float32 radiusB = shapeB->m_radius; - b2Body* bodyA = fixtureA->GetBody(); - b2Body* bodyB = fixtureB->GetBody(); - b2Manifold* manifold = contact->GetManifold(); - - int32 pointCount = manifold->pointCount; - b2Assert(pointCount > 0); - - b2ContactVelocityConstraint* vc = m_velocityConstraints + i; - vc->friction = contact->m_friction; - vc->restitution = contact->m_restitution; - vc->tangentSpeed = contact->m_tangentSpeed; - vc->indexA = bodyA->m_islandIndex; - vc->indexB = bodyB->m_islandIndex; - vc->invMassA = bodyA->m_invMass; - vc->invMassB = bodyB->m_invMass; - vc->invIA = bodyA->m_invI; - vc->invIB = bodyB->m_invI; - vc->contactIndex = i; - vc->pointCount = pointCount; - vc->K.SetZero(); - vc->normalMass.SetZero(); - - b2ContactPositionConstraint* pc = m_positionConstraints + i; - pc->indexA = bodyA->m_islandIndex; - pc->indexB = bodyB->m_islandIndex; - pc->invMassA = bodyA->m_invMass; - pc->invMassB = bodyB->m_invMass; - pc->localCenterA = bodyA->m_sweep.localCenter; - pc->localCenterB = bodyB->m_sweep.localCenter; - pc->invIA = bodyA->m_invI; - pc->invIB = bodyB->m_invI; - pc->localNormal = manifold->localNormal; - pc->localPoint = manifold->localPoint; - pc->pointCount = pointCount; - pc->radiusA = radiusA; - pc->radiusB = radiusB; - pc->type = manifold->type; - - for (int32 j = 0; j < pointCount; ++j) - { - b2ManifoldPoint* cp = manifold->points + j; - b2VelocityConstraintPoint* vcp = vc->points + j; - - if (m_step.warmStarting) - { - vcp->normalImpulse = m_step.dtRatio * cp->normalImpulse; - vcp->tangentImpulse = m_step.dtRatio * cp->tangentImpulse; - } - else - { - vcp->normalImpulse = 0.0f; - vcp->tangentImpulse = 0.0f; - } - - vcp->rA.SetZero(); - vcp->rB.SetZero(); - vcp->normalMass = 0.0f; - vcp->tangentMass = 0.0f; - vcp->velocityBias = 0.0f; - - pc->localPoints[j] = cp->localPoint; - } - } -} - -b2ContactSolver::~b2ContactSolver() -{ - m_allocator->Free(m_velocityConstraints); - m_allocator->Free(m_positionConstraints); -} - -// Initialize position dependent portions of the velocity constraints. -void b2ContactSolver::InitializeVelocityConstraints() -{ - for (int32 i = 0; i < m_count; ++i) - { - b2ContactVelocityConstraint* vc = m_velocityConstraints + i; - b2ContactPositionConstraint* pc = m_positionConstraints + i; - - float32 radiusA = pc->radiusA; - float32 radiusB = pc->radiusB; - b2Manifold* manifold = m_contacts[vc->contactIndex]->GetManifold(); - - int32 indexA = vc->indexA; - int32 indexB = vc->indexB; - - float32 mA = vc->invMassA; - float32 mB = vc->invMassB; - float32 iA = vc->invIA; - float32 iB = vc->invIB; - b2Vec2 localCenterA = pc->localCenterA; - b2Vec2 localCenterB = pc->localCenterB; - - b2Vec2 cA = m_positions[indexA].c; - float32 aA = m_positions[indexA].a; - b2Vec2 vA = m_velocities[indexA].v; - float32 wA = m_velocities[indexA].w; - - b2Vec2 cB = m_positions[indexB].c; - float32 aB = m_positions[indexB].a; - b2Vec2 vB = m_velocities[indexB].v; - float32 wB = m_velocities[indexB].w; - - b2Assert(manifold->pointCount > 0); - - b2Transform xfA, xfB; - xfA.q.Set(aA); - xfB.q.Set(aB); - xfA.p = cA - b2Mul(xfA.q, localCenterA); - xfB.p = cB - b2Mul(xfB.q, localCenterB); - - b2WorldManifold worldManifold; - worldManifold.Initialize(manifold, xfA, radiusA, xfB, radiusB); - - vc->normal = worldManifold.normal; - - int32 pointCount = vc->pointCount; - for (int32 j = 0; j < pointCount; ++j) - { - b2VelocityConstraintPoint* vcp = vc->points + j; - - vcp->rA = worldManifold.points[j] - cA; - vcp->rB = worldManifold.points[j] - cB; - - float32 rnA = b2Cross(vcp->rA, vc->normal); - float32 rnB = b2Cross(vcp->rB, vc->normal); - - float32 kNormal = mA + mB + iA * rnA * rnA + iB * rnB * rnB; - - vcp->normalMass = kNormal > 0.0f ? 1.0f / kNormal : 0.0f; - - b2Vec2 tangent = b2Cross(vc->normal, 1.0f); - - float32 rtA = b2Cross(vcp->rA, tangent); - float32 rtB = b2Cross(vcp->rB, tangent); - - float32 kTangent = mA + mB + iA * rtA * rtA + iB * rtB * rtB; - - vcp->tangentMass = kTangent > 0.0f ? 1.0f / kTangent : 0.0f; - - // Setup a velocity bias for restitution. - vcp->velocityBias = 0.0f; - float32 vRel = b2Dot(vc->normal, vB + b2Cross(wB, vcp->rB) - vA - b2Cross(wA, vcp->rA)); - if (vRel < -b2_velocityThreshold) - { - vcp->velocityBias = -vc->restitution * vRel; - } - } - - // If we have two points, then prepare the block solver. - if (vc->pointCount == 2) - { - b2VelocityConstraintPoint* vcp1 = vc->points + 0; - b2VelocityConstraintPoint* vcp2 = vc->points + 1; - - float32 rn1A = b2Cross(vcp1->rA, vc->normal); - float32 rn1B = b2Cross(vcp1->rB, vc->normal); - float32 rn2A = b2Cross(vcp2->rA, vc->normal); - float32 rn2B = b2Cross(vcp2->rB, vc->normal); - - float32 k11 = mA + mB + iA * rn1A * rn1A + iB * rn1B * rn1B; - float32 k22 = mA + mB + iA * rn2A * rn2A + iB * rn2B * rn2B; - float32 k12 = mA + mB + iA * rn1A * rn2A + iB * rn1B * rn2B; - - // Ensure a reasonable condition number. - const float32 k_maxConditionNumber = 1000.0f; - if (k11 * k11 < k_maxConditionNumber * (k11 * k22 - k12 * k12)) - { - // K is safe to invert. - vc->K.ex.Set(k11, k12); - vc->K.ey.Set(k12, k22); - vc->normalMass = vc->K.GetInverse(); - } - else - { - // The constraints are redundant, just use one. - // TODO_ERIN use deepest? - vc->pointCount = 1; - } - } - } -} - -void b2ContactSolver::WarmStart() -{ - // Warm start. - for (int32 i = 0; i < m_count; ++i) - { - b2ContactVelocityConstraint* vc = m_velocityConstraints + i; - - int32 indexA = vc->indexA; - int32 indexB = vc->indexB; - float32 mA = vc->invMassA; - float32 iA = vc->invIA; - float32 mB = vc->invMassB; - float32 iB = vc->invIB; - int32 pointCount = vc->pointCount; - - b2Vec2 vA = m_velocities[indexA].v; - float32 wA = m_velocities[indexA].w; - b2Vec2 vB = m_velocities[indexB].v; - float32 wB = m_velocities[indexB].w; - - b2Vec2 normal = vc->normal; - b2Vec2 tangent = b2Cross(normal, 1.0f); - - for (int32 j = 0; j < pointCount; ++j) - { - b2VelocityConstraintPoint* vcp = vc->points + j; - b2Vec2 P = vcp->normalImpulse * normal + vcp->tangentImpulse * tangent; - wA -= iA * b2Cross(vcp->rA, P); - vA -= mA * P; - wB += iB * b2Cross(vcp->rB, P); - vB += mB * P; - } - - m_velocities[indexA].v = vA; - m_velocities[indexA].w = wA; - m_velocities[indexB].v = vB; - m_velocities[indexB].w = wB; - } -} - -void b2ContactSolver::SolveVelocityConstraints() -{ - for (int32 i = 0; i < m_count; ++i) - { - b2ContactVelocityConstraint* vc = m_velocityConstraints + i; - - int32 indexA = vc->indexA; - int32 indexB = vc->indexB; - float32 mA = vc->invMassA; - float32 iA = vc->invIA; - float32 mB = vc->invMassB; - float32 iB = vc->invIB; - int32 pointCount = vc->pointCount; - - b2Vec2 vA = m_velocities[indexA].v; - float32 wA = m_velocities[indexA].w; - b2Vec2 vB = m_velocities[indexB].v; - float32 wB = m_velocities[indexB].w; - - b2Vec2 normal = vc->normal; - b2Vec2 tangent = b2Cross(normal, 1.0f); - float32 friction = vc->friction; - - b2Assert(pointCount == 1 || pointCount == 2); - - // Solve tangent constraints first because non-penetration is more important - // than friction. - for (int32 j = 0; j < pointCount; ++j) - { - b2VelocityConstraintPoint* vcp = vc->points + j; - - // Relative velocity at contact - b2Vec2 dv = vB + b2Cross(wB, vcp->rB) - vA - b2Cross(wA, vcp->rA); - - // Compute tangent force - float32 vt = b2Dot(dv, tangent) - vc->tangentSpeed; - float32 lambda = vcp->tangentMass * (-vt); - - // b2Clamp the accumulated force - float32 maxFriction = friction * vcp->normalImpulse; - float32 newImpulse = b2Clamp(vcp->tangentImpulse + lambda, -maxFriction, maxFriction); - lambda = newImpulse - vcp->tangentImpulse; - vcp->tangentImpulse = newImpulse; - - // Apply contact impulse - b2Vec2 P = lambda * tangent; - - vA -= mA * P; - wA -= iA * b2Cross(vcp->rA, P); - - vB += mB * P; - wB += iB * b2Cross(vcp->rB, P); - } - - // Solve normal constraints - if (vc->pointCount == 1) - { - b2VelocityConstraintPoint* vcp = vc->points + 0; - - // Relative velocity at contact - b2Vec2 dv = vB + b2Cross(wB, vcp->rB) - vA - b2Cross(wA, vcp->rA); - - // Compute normal impulse - float32 vn = b2Dot(dv, normal); - float32 lambda = -vcp->normalMass * (vn - vcp->velocityBias); - - // b2Clamp the accumulated impulse - float32 newImpulse = b2Max(vcp->normalImpulse + lambda, 0.0f); - lambda = newImpulse - vcp->normalImpulse; - vcp->normalImpulse = newImpulse; - - // Apply contact impulse - b2Vec2 P = lambda * normal; - vA -= mA * P; - wA -= iA * b2Cross(vcp->rA, P); - - vB += mB * P; - wB += iB * b2Cross(vcp->rB, P); - } - else - { - // Block solver developed in collaboration with Dirk Gregorius (back in 01/07 on Box2D_Lite). - // Build the mini LCP for this contact patch - // - // vn = A * x + b, vn >= 0, , vn >= 0, x >= 0 and vn_i * x_i = 0 with i = 1..2 - // - // A = J * W * JT and J = ( -n, -r1 x n, n, r2 x n ) - // b = vn0 - velocityBias - // - // The system is solved using the "Total enumeration method" (s. Murty). The complementary constraint vn_i * x_i - // implies that we must have in any solution either vn_i = 0 or x_i = 0. So for the 2D contact problem the cases - // vn1 = 0 and vn2 = 0, x1 = 0 and x2 = 0, x1 = 0 and vn2 = 0, x2 = 0 and vn1 = 0 need to be tested. The first valid - // solution that satisfies the problem is chosen. - // - // In order to account of the accumulated impulse 'a' (because of the iterative nature of the solver which only requires - // that the accumulated impulse is clamped and not the incremental impulse) we change the impulse variable (x_i). - // - // Substitute: - // - // x = a + d - // - // a := old total impulse - // x := new total impulse - // d := incremental impulse - // - // For the current iteration we extend the formula for the incremental impulse - // to compute the new total impulse: - // - // vn = A * d + b - // = A * (x - a) + b - // = A * x + b - A * a - // = A * x + b' - // b' = b - A * a; - - b2VelocityConstraintPoint* cp1 = vc->points + 0; - b2VelocityConstraintPoint* cp2 = vc->points + 1; - - b2Vec2 a(cp1->normalImpulse, cp2->normalImpulse); - b2Assert(a.x >= 0.0f && a.y >= 0.0f); - - // Relative velocity at contact - b2Vec2 dv1 = vB + b2Cross(wB, cp1->rB) - vA - b2Cross(wA, cp1->rA); - b2Vec2 dv2 = vB + b2Cross(wB, cp2->rB) - vA - b2Cross(wA, cp2->rA); - - // Compute normal velocity - float32 vn1 = b2Dot(dv1, normal); - float32 vn2 = b2Dot(dv2, normal); - - b2Vec2 b; - b.x = vn1 - cp1->velocityBias; - b.y = vn2 - cp2->velocityBias; - - // Compute b' - b -= b2Mul(vc->K, a); - - const float32 k_errorTol = 1e-3f; - B2_NOT_USED(k_errorTol); - - for (;;) - { - // - // Case 1: vn = 0 - // - // 0 = A * x + b' - // - // Solve for x: - // - // x = - inv(A) * b' - // - b2Vec2 x = - b2Mul(vc->normalMass, b); - - if (x.x >= 0.0f && x.y >= 0.0f) - { - // Get the incremental impulse - b2Vec2 d = x - a; - - // Apply incremental impulse - b2Vec2 P1 = d.x * normal; - b2Vec2 P2 = d.y * normal; - vA -= mA * (P1 + P2); - wA -= iA * (b2Cross(cp1->rA, P1) + b2Cross(cp2->rA, P2)); - - vB += mB * (P1 + P2); - wB += iB * (b2Cross(cp1->rB, P1) + b2Cross(cp2->rB, P2)); - - // Accumulate - cp1->normalImpulse = x.x; - cp2->normalImpulse = x.y; - -#if B2_DEBUG_SOLVER == 1 - // Postconditions - dv1 = vB + b2Cross(wB, cp1->rB) - vA - b2Cross(wA, cp1->rA); - dv2 = vB + b2Cross(wB, cp2->rB) - vA - b2Cross(wA, cp2->rA); - - // Compute normal velocity - vn1 = b2Dot(dv1, normal); - vn2 = b2Dot(dv2, normal); - - b2Assert(b2Abs(vn1 - cp1->velocityBias) < k_errorTol); - b2Assert(b2Abs(vn2 - cp2->velocityBias) < k_errorTol); -#endif - break; - } - - // - // Case 2: vn1 = 0 and x2 = 0 - // - // 0 = a11 * x1 + a12 * 0 + b1' - // vn2 = a21 * x1 + a22 * 0 + b2' - // - x.x = - cp1->normalMass * b.x; - x.y = 0.0f; - vn1 = 0.0f; - vn2 = vc->K.ex.y * x.x + b.y; - - if (x.x >= 0.0f && vn2 >= 0.0f) - { - // Get the incremental impulse - b2Vec2 d = x - a; - - // Apply incremental impulse - b2Vec2 P1 = d.x * normal; - b2Vec2 P2 = d.y * normal; - vA -= mA * (P1 + P2); - wA -= iA * (b2Cross(cp1->rA, P1) + b2Cross(cp2->rA, P2)); - - vB += mB * (P1 + P2); - wB += iB * (b2Cross(cp1->rB, P1) + b2Cross(cp2->rB, P2)); - - // Accumulate - cp1->normalImpulse = x.x; - cp2->normalImpulse = x.y; - -#if B2_DEBUG_SOLVER == 1 - // Postconditions - dv1 = vB + b2Cross(wB, cp1->rB) - vA - b2Cross(wA, cp1->rA); - - // Compute normal velocity - vn1 = b2Dot(dv1, normal); - - b2Assert(b2Abs(vn1 - cp1->velocityBias) < k_errorTol); -#endif - break; - } - - - // - // Case 3: vn2 = 0 and x1 = 0 - // - // vn1 = a11 * 0 + a12 * x2 + b1' - // 0 = a21 * 0 + a22 * x2 + b2' - // - x.x = 0.0f; - x.y = - cp2->normalMass * b.y; - vn1 = vc->K.ey.x * x.y + b.x; - vn2 = 0.0f; - - if (x.y >= 0.0f && vn1 >= 0.0f) - { - // Resubstitute for the incremental impulse - b2Vec2 d = x - a; - - // Apply incremental impulse - b2Vec2 P1 = d.x * normal; - b2Vec2 P2 = d.y * normal; - vA -= mA * (P1 + P2); - wA -= iA * (b2Cross(cp1->rA, P1) + b2Cross(cp2->rA, P2)); - - vB += mB * (P1 + P2); - wB += iB * (b2Cross(cp1->rB, P1) + b2Cross(cp2->rB, P2)); - - // Accumulate - cp1->normalImpulse = x.x; - cp2->normalImpulse = x.y; - -#if B2_DEBUG_SOLVER == 1 - // Postconditions - dv2 = vB + b2Cross(wB, cp2->rB) - vA - b2Cross(wA, cp2->rA); - - // Compute normal velocity - vn2 = b2Dot(dv2, normal); - - b2Assert(b2Abs(vn2 - cp2->velocityBias) < k_errorTol); -#endif - break; - } - - // - // Case 4: x1 = 0 and x2 = 0 - // - // vn1 = b1 - // vn2 = b2; - x.x = 0.0f; - x.y = 0.0f; - vn1 = b.x; - vn2 = b.y; - - if (vn1 >= 0.0f && vn2 >= 0.0f ) - { - // Resubstitute for the incremental impulse - b2Vec2 d = x - a; - - // Apply incremental impulse - b2Vec2 P1 = d.x * normal; - b2Vec2 P2 = d.y * normal; - vA -= mA * (P1 + P2); - wA -= iA * (b2Cross(cp1->rA, P1) + b2Cross(cp2->rA, P2)); - - vB += mB * (P1 + P2); - wB += iB * (b2Cross(cp1->rB, P1) + b2Cross(cp2->rB, P2)); - - // Accumulate - cp1->normalImpulse = x.x; - cp2->normalImpulse = x.y; - - break; - } - - // No solution, give up. This is hit sometimes, but it doesn't seem to matter. - break; - } - } - - m_velocities[indexA].v = vA; - m_velocities[indexA].w = wA; - m_velocities[indexB].v = vB; - m_velocities[indexB].w = wB; - } -} - -void b2ContactSolver::StoreImpulses() -{ - for (int32 i = 0; i < m_count; ++i) - { - b2ContactVelocityConstraint* vc = m_velocityConstraints + i; - b2Manifold* manifold = m_contacts[vc->contactIndex]->GetManifold(); - - for (int32 j = 0; j < vc->pointCount; ++j) - { - manifold->points[j].normalImpulse = vc->points[j].normalImpulse; - manifold->points[j].tangentImpulse = vc->points[j].tangentImpulse; - } - } -} - -struct b2PositionSolverManifold -{ - void Initialize(b2ContactPositionConstraint* pc, const b2Transform& xfA, const b2Transform& xfB, int32 index) - { - b2Assert(pc->pointCount > 0); - - switch (pc->type) - { - case b2Manifold::e_circles: - { - b2Vec2 pointA = b2Mul(xfA, pc->localPoint); - b2Vec2 pointB = b2Mul(xfB, pc->localPoints[0]); - normal = pointB - pointA; - normal.Normalize(); - point = 0.5f * (pointA + pointB); - separation = b2Dot(pointB - pointA, normal) - pc->radiusA - pc->radiusB; - } - break; - - case b2Manifold::e_faceA: - { - normal = b2Mul(xfA.q, pc->localNormal); - b2Vec2 planePoint = b2Mul(xfA, pc->localPoint); - - b2Vec2 clipPoint = b2Mul(xfB, pc->localPoints[index]); - separation = b2Dot(clipPoint - planePoint, normal) - pc->radiusA - pc->radiusB; - point = clipPoint; - } - break; - - case b2Manifold::e_faceB: - { - normal = b2Mul(xfB.q, pc->localNormal); - b2Vec2 planePoint = b2Mul(xfB, pc->localPoint); - - b2Vec2 clipPoint = b2Mul(xfA, pc->localPoints[index]); - separation = b2Dot(clipPoint - planePoint, normal) - pc->radiusA - pc->radiusB; - point = clipPoint; - - // Ensure normal points from A to B - normal = -normal; - } - break; - } - } - - b2Vec2 normal; - b2Vec2 point; - float32 separation; -}; - -// Sequential solver. -bool b2ContactSolver::SolvePositionConstraints() -{ - float32 minSeparation = 0.0f; - - for (int32 i = 0; i < m_count; ++i) - { - b2ContactPositionConstraint* pc = m_positionConstraints + i; - - int32 indexA = pc->indexA; - int32 indexB = pc->indexB; - b2Vec2 localCenterA = pc->localCenterA; - float32 mA = pc->invMassA; - float32 iA = pc->invIA; - b2Vec2 localCenterB = pc->localCenterB; - float32 mB = pc->invMassB; - float32 iB = pc->invIB; - int32 pointCount = pc->pointCount; - - b2Vec2 cA = m_positions[indexA].c; - float32 aA = m_positions[indexA].a; - - b2Vec2 cB = m_positions[indexB].c; - float32 aB = m_positions[indexB].a; - - // Solve normal constraints - for (int32 j = 0; j < pointCount; ++j) - { - b2Transform xfA, xfB; - xfA.q.Set(aA); - xfB.q.Set(aB); - xfA.p = cA - b2Mul(xfA.q, localCenterA); - xfB.p = cB - b2Mul(xfB.q, localCenterB); - - b2PositionSolverManifold psm; - psm.Initialize(pc, xfA, xfB, j); - b2Vec2 normal = psm.normal; - - b2Vec2 point = psm.point; - float32 separation = psm.separation; - - b2Vec2 rA = point - cA; - b2Vec2 rB = point - cB; - - // Track max constraint error. - minSeparation = b2Min(minSeparation, separation); - - // Prevent large corrections and allow slop. - float32 C = b2Clamp(b2_baumgarte * (separation + b2_linearSlop), -b2_maxLinearCorrection, 0.0f); - - // Compute the effective mass. - float32 rnA = b2Cross(rA, normal); - float32 rnB = b2Cross(rB, normal); - float32 K = mA + mB + iA * rnA * rnA + iB * rnB * rnB; - - // Compute normal impulse - float32 impulse = K > 0.0f ? - C / K : 0.0f; - - b2Vec2 P = impulse * normal; - - cA -= mA * P; - aA -= iA * b2Cross(rA, P); - - cB += mB * P; - aB += iB * b2Cross(rB, P); - } - - m_positions[indexA].c = cA; - m_positions[indexA].a = aA; - - m_positions[indexB].c = cB; - m_positions[indexB].a = aB; - } - - // We can't expect minSpeparation >= -b2_linearSlop because we don't - // push the separation above -b2_linearSlop. - return minSeparation >= -3.0f * b2_linearSlop; -} - -// Sequential position solver for position constraints. -bool b2ContactSolver::SolveTOIPositionConstraints(int32 toiIndexA, int32 toiIndexB) -{ - float32 minSeparation = 0.0f; - - for (int32 i = 0; i < m_count; ++i) - { - b2ContactPositionConstraint* pc = m_positionConstraints + i; - - int32 indexA = pc->indexA; - int32 indexB = pc->indexB; - b2Vec2 localCenterA = pc->localCenterA; - b2Vec2 localCenterB = pc->localCenterB; - int32 pointCount = pc->pointCount; - - float32 mA = 0.0f; - float32 iA = 0.0f; - if (indexA == toiIndexA || indexA == toiIndexB) - { - mA = pc->invMassA; - iA = pc->invIA; - } - - float32 mB = 0.0f; - float32 iB = 0.; - if (indexB == toiIndexA || indexB == toiIndexB) - { - mB = pc->invMassB; - iB = pc->invIB; - } - - b2Vec2 cA = m_positions[indexA].c; - float32 aA = m_positions[indexA].a; - - b2Vec2 cB = m_positions[indexB].c; - float32 aB = m_positions[indexB].a; - - // Solve normal constraints - for (int32 j = 0; j < pointCount; ++j) - { - b2Transform xfA, xfB; - xfA.q.Set(aA); - xfB.q.Set(aB); - xfA.p = cA - b2Mul(xfA.q, localCenterA); - xfB.p = cB - b2Mul(xfB.q, localCenterB); - - b2PositionSolverManifold psm; - psm.Initialize(pc, xfA, xfB, j); - b2Vec2 normal = psm.normal; - - b2Vec2 point = psm.point; - float32 separation = psm.separation; - - b2Vec2 rA = point - cA; - b2Vec2 rB = point - cB; - - // Track max constraint error. - minSeparation = b2Min(minSeparation, separation); - - // Prevent large corrections and allow slop. - float32 C = b2Clamp(b2_toiBaugarte * (separation + b2_linearSlop), -b2_maxLinearCorrection, 0.0f); - - // Compute the effective mass. - float32 rnA = b2Cross(rA, normal); - float32 rnB = b2Cross(rB, normal); - float32 K = mA + mB + iA * rnA * rnA + iB * rnB * rnB; - - // Compute normal impulse - float32 impulse = K > 0.0f ? - C / K : 0.0f; - - b2Vec2 P = impulse * normal; - - cA -= mA * P; - aA -= iA * b2Cross(rA, P); - - cB += mB * P; - aB += iB * b2Cross(rB, P); - } - - m_positions[indexA].c = cA; - m_positions[indexA].a = aA; - - m_positions[indexB].c = cB; - m_positions[indexB].a = aB; - } - - // We can't expect minSpeparation >= -b2_linearSlop because we don't - // push the separation above -b2_linearSlop. - return minSeparation >= -1.5f * b2_linearSlop; -} diff --git a/Box2D/Dynamics/Contacts/b2ContactSolver.h b/Box2D/Dynamics/Contacts/b2ContactSolver.h deleted file mode 100644 index a622e30d83..0000000000 --- a/Box2D/Dynamics/Contacts/b2ContactSolver.h +++ /dev/null @@ -1,95 +0,0 @@ -/* -* 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_CONTACT_SOLVER_H -#define B2_CONTACT_SOLVER_H - -#include -#include -#include - -class b2Contact; -class b2Body; -class b2StackAllocator; -struct b2ContactPositionConstraint; - -struct b2VelocityConstraintPoint -{ - b2Vec2 rA; - b2Vec2 rB; - float32 normalImpulse; - float32 tangentImpulse; - float32 normalMass; - float32 tangentMass; - float32 velocityBias; -}; - -struct b2ContactVelocityConstraint -{ - b2VelocityConstraintPoint points[b2_maxManifoldPoints]; - b2Vec2 normal; - b2Mat22 normalMass; - b2Mat22 K; - int32 indexA; - int32 indexB; - float32 invMassA, invMassB; - float32 invIA, invIB; - float32 friction; - float32 restitution; - float32 tangentSpeed; - int32 pointCount; - int32 contactIndex; -}; - -struct b2ContactSolverDef -{ - b2TimeStep step; - b2Contact** contacts; - int32 count; - b2Position* positions; - b2Velocity* velocities; - b2StackAllocator* allocator; -}; - -class b2ContactSolver -{ -public: - b2ContactSolver(b2ContactSolverDef* def); - ~b2ContactSolver(); - - void InitializeVelocityConstraints(); - - void WarmStart(); - void SolveVelocityConstraints(); - void StoreImpulses(); - - bool SolvePositionConstraints(); - bool SolveTOIPositionConstraints(int32 toiIndexA, int32 toiIndexB); - - b2TimeStep m_step; - b2Position* m_positions; - b2Velocity* m_velocities; - b2StackAllocator* m_allocator; - b2ContactPositionConstraint* m_positionConstraints; - b2ContactVelocityConstraint* m_velocityConstraints; - b2Contact** m_contacts; - int m_count; -}; - -#endif - diff --git a/Box2D/Dynamics/Contacts/b2EdgeAndCircleContact.cpp b/Box2D/Dynamics/Contacts/b2EdgeAndCircleContact.cpp deleted file mode 100644 index d1fd3295ce..0000000000 --- a/Box2D/Dynamics/Contacts/b2EdgeAndCircleContact.cpp +++ /dev/null @@ -1,49 +0,0 @@ -/* -* 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 -#include -#include - -#include - -b2Contact* b2EdgeAndCircleContact::Create(b2Fixture* fixtureA, int32, b2Fixture* fixtureB, int32, b2BlockAllocator* allocator) -{ - void* mem = allocator->Allocate(sizeof(b2EdgeAndCircleContact)); - return new (mem) b2EdgeAndCircleContact(fixtureA, fixtureB); -} - -void b2EdgeAndCircleContact::Destroy(b2Contact* contact, b2BlockAllocator* allocator) -{ - ((b2EdgeAndCircleContact*)contact)->~b2EdgeAndCircleContact(); - allocator->Free(contact, sizeof(b2EdgeAndCircleContact)); -} - -b2EdgeAndCircleContact::b2EdgeAndCircleContact(b2Fixture* fixtureA, b2Fixture* fixtureB) -: b2Contact(fixtureA, 0, fixtureB, 0) -{ - b2Assert(m_fixtureA->GetType() == b2Shape::e_edge); - b2Assert(m_fixtureB->GetType() == b2Shape::e_circle); -} - -void b2EdgeAndCircleContact::Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB) -{ - b2CollideEdgeAndCircle( manifold, - (b2EdgeShape*)m_fixtureA->GetShape(), xfA, - (b2CircleShape*)m_fixtureB->GetShape(), xfB); -} diff --git a/Box2D/Dynamics/Contacts/b2EdgeAndCircleContact.h b/Box2D/Dynamics/Contacts/b2EdgeAndCircleContact.h deleted file mode 100644 index e7a34553c1..0000000000 --- a/Box2D/Dynamics/Contacts/b2EdgeAndCircleContact.h +++ /dev/null @@ -1,39 +0,0 @@ -/* -* 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_EDGE_AND_CIRCLE_CONTACT_H -#define B2_EDGE_AND_CIRCLE_CONTACT_H - -#include - -class b2BlockAllocator; - -class b2EdgeAndCircleContact : public b2Contact -{ -public: - static b2Contact* Create( b2Fixture* fixtureA, int32 indexA, - b2Fixture* fixtureB, int32 indexB, b2BlockAllocator* allocator); - static void Destroy(b2Contact* contact, b2BlockAllocator* allocator); - - b2EdgeAndCircleContact(b2Fixture* fixtureA, b2Fixture* fixtureB); - ~b2EdgeAndCircleContact() {} - - void Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB); -}; - -#endif diff --git a/Box2D/Dynamics/Contacts/b2EdgeAndPolygonContact.cpp b/Box2D/Dynamics/Contacts/b2EdgeAndPolygonContact.cpp deleted file mode 100644 index ba536bc39d..0000000000 --- a/Box2D/Dynamics/Contacts/b2EdgeAndPolygonContact.cpp +++ /dev/null @@ -1,49 +0,0 @@ -/* -* 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 -#include -#include - -#include - -b2Contact* b2EdgeAndPolygonContact::Create(b2Fixture* fixtureA, int32, b2Fixture* fixtureB, int32, b2BlockAllocator* allocator) -{ - void* mem = allocator->Allocate(sizeof(b2EdgeAndPolygonContact)); - return new (mem) b2EdgeAndPolygonContact(fixtureA, fixtureB); -} - -void b2EdgeAndPolygonContact::Destroy(b2Contact* contact, b2BlockAllocator* allocator) -{ - ((b2EdgeAndPolygonContact*)contact)->~b2EdgeAndPolygonContact(); - allocator->Free(contact, sizeof(b2EdgeAndPolygonContact)); -} - -b2EdgeAndPolygonContact::b2EdgeAndPolygonContact(b2Fixture* fixtureA, b2Fixture* fixtureB) -: b2Contact(fixtureA, 0, fixtureB, 0) -{ - b2Assert(m_fixtureA->GetType() == b2Shape::e_edge); - b2Assert(m_fixtureB->GetType() == b2Shape::e_polygon); -} - -void b2EdgeAndPolygonContact::Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB) -{ - b2CollideEdgeAndPolygon( manifold, - (b2EdgeShape*)m_fixtureA->GetShape(), xfA, - (b2PolygonShape*)m_fixtureB->GetShape(), xfB); -} diff --git a/Box2D/Dynamics/Contacts/b2EdgeAndPolygonContact.h b/Box2D/Dynamics/Contacts/b2EdgeAndPolygonContact.h deleted file mode 100644 index 3b40f12163..0000000000 --- a/Box2D/Dynamics/Contacts/b2EdgeAndPolygonContact.h +++ /dev/null @@ -1,39 +0,0 @@ -/* -* 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_EDGE_AND_POLYGON_CONTACT_H -#define B2_EDGE_AND_POLYGON_CONTACT_H - -#include - -class b2BlockAllocator; - -class b2EdgeAndPolygonContact : public b2Contact -{ -public: - static b2Contact* Create( b2Fixture* fixtureA, int32 indexA, - b2Fixture* fixtureB, int32 indexB, b2BlockAllocator* allocator); - static void Destroy(b2Contact* contact, b2BlockAllocator* allocator); - - b2EdgeAndPolygonContact(b2Fixture* fixtureA, b2Fixture* fixtureB); - ~b2EdgeAndPolygonContact() {} - - void Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB); -}; - -#endif diff --git a/Box2D/Dynamics/Contacts/b2PolygonAndCircleContact.cpp b/Box2D/Dynamics/Contacts/b2PolygonAndCircleContact.cpp deleted file mode 100644 index 7c9cbcc926..0000000000 --- a/Box2D/Dynamics/Contacts/b2PolygonAndCircleContact.cpp +++ /dev/null @@ -1,49 +0,0 @@ -/* -* 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 -#include -#include - -#include - -b2Contact* b2PolygonAndCircleContact::Create(b2Fixture* fixtureA, int32, b2Fixture* fixtureB, int32, b2BlockAllocator* allocator) -{ - void* mem = allocator->Allocate(sizeof(b2PolygonAndCircleContact)); - return new (mem) b2PolygonAndCircleContact(fixtureA, fixtureB); -} - -void b2PolygonAndCircleContact::Destroy(b2Contact* contact, b2BlockAllocator* allocator) -{ - ((b2PolygonAndCircleContact*)contact)->~b2PolygonAndCircleContact(); - allocator->Free(contact, sizeof(b2PolygonAndCircleContact)); -} - -b2PolygonAndCircleContact::b2PolygonAndCircleContact(b2Fixture* fixtureA, b2Fixture* fixtureB) -: b2Contact(fixtureA, 0, fixtureB, 0) -{ - b2Assert(m_fixtureA->GetType() == b2Shape::e_polygon); - b2Assert(m_fixtureB->GetType() == b2Shape::e_circle); -} - -void b2PolygonAndCircleContact::Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB) -{ - b2CollidePolygonAndCircle( manifold, - (b2PolygonShape*)m_fixtureA->GetShape(), xfA, - (b2CircleShape*)m_fixtureB->GetShape(), xfB); -} diff --git a/Box2D/Dynamics/Contacts/b2PolygonAndCircleContact.h b/Box2D/Dynamics/Contacts/b2PolygonAndCircleContact.h deleted file mode 100644 index fe0645e080..0000000000 --- a/Box2D/Dynamics/Contacts/b2PolygonAndCircleContact.h +++ /dev/null @@ -1,38 +0,0 @@ -/* -* 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_AND_CIRCLE_CONTACT_H -#define B2_POLYGON_AND_CIRCLE_CONTACT_H - -#include - -class b2BlockAllocator; - -class b2PolygonAndCircleContact : public b2Contact -{ -public: - static b2Contact* Create(b2Fixture* fixtureA, int32 indexA, b2Fixture* fixtureB, int32 indexB, b2BlockAllocator* allocator); - static void Destroy(b2Contact* contact, b2BlockAllocator* allocator); - - b2PolygonAndCircleContact(b2Fixture* fixtureA, b2Fixture* fixtureB); - ~b2PolygonAndCircleContact() {} - - void Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB); -}; - -#endif diff --git a/Box2D/Dynamics/Contacts/b2PolygonContact.cpp b/Box2D/Dynamics/Contacts/b2PolygonContact.cpp deleted file mode 100644 index b5706ca104..0000000000 --- a/Box2D/Dynamics/Contacts/b2PolygonContact.cpp +++ /dev/null @@ -1,52 +0,0 @@ -/* -* 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 -#include -#include -#include -#include -#include - -#include - -b2Contact* b2PolygonContact::Create(b2Fixture* fixtureA, int32, b2Fixture* fixtureB, int32, b2BlockAllocator* allocator) -{ - void* mem = allocator->Allocate(sizeof(b2PolygonContact)); - return new (mem) b2PolygonContact(fixtureA, fixtureB); -} - -void b2PolygonContact::Destroy(b2Contact* contact, b2BlockAllocator* allocator) -{ - ((b2PolygonContact*)contact)->~b2PolygonContact(); - allocator->Free(contact, sizeof(b2PolygonContact)); -} - -b2PolygonContact::b2PolygonContact(b2Fixture* fixtureA, b2Fixture* fixtureB) - : b2Contact(fixtureA, 0, fixtureB, 0) -{ - b2Assert(m_fixtureA->GetType() == b2Shape::e_polygon); - b2Assert(m_fixtureB->GetType() == b2Shape::e_polygon); -} - -void b2PolygonContact::Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB) -{ - b2CollidePolygons( manifold, - (b2PolygonShape*)m_fixtureA->GetShape(), xfA, - (b2PolygonShape*)m_fixtureB->GetShape(), xfB); -} diff --git a/Box2D/Dynamics/Contacts/b2PolygonContact.h b/Box2D/Dynamics/Contacts/b2PolygonContact.h deleted file mode 100644 index 434e6c9266..0000000000 --- a/Box2D/Dynamics/Contacts/b2PolygonContact.h +++ /dev/null @@ -1,39 +0,0 @@ -/* -* 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_CONTACT_H -#define B2_POLYGON_CONTACT_H - -#include - -class b2BlockAllocator; - -class b2PolygonContact : public b2Contact -{ -public: - static b2Contact* Create( b2Fixture* fixtureA, int32 indexA, - b2Fixture* fixtureB, int32 indexB, b2BlockAllocator* allocator); - static void Destroy(b2Contact* contact, b2BlockAllocator* allocator); - - b2PolygonContact(b2Fixture* fixtureA, b2Fixture* fixtureB); - ~b2PolygonContact() {} - - void Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB); -}; - -#endif diff --git a/Box2D/Dynamics/Joints/b2DistanceJoint.cpp b/Box2D/Dynamics/Joints/b2DistanceJoint.cpp deleted file mode 100644 index ba2b6c0099..0000000000 --- a/Box2D/Dynamics/Joints/b2DistanceJoint.cpp +++ /dev/null @@ -1,260 +0,0 @@ -/* -* Copyright (c) 2006-2011 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 -#include -#include - -// 1-D constrained system -// m (v2 - v1) = lambda -// v2 + (beta/h) * x1 + gamma * lambda = 0, gamma has units of inverse mass. -// x2 = x1 + h * v2 - -// 1-D mass-damper-spring system -// m (v2 - v1) + h * d * v2 + h * k * - -// C = norm(p2 - p1) - L -// u = (p2 - p1) / norm(p2 - p1) -// Cdot = dot(u, v2 + cross(w2, r2) - v1 - cross(w1, r1)) -// J = [-u -cross(r1, u) u cross(r2, u)] -// K = J * invM * JT -// = invMass1 + invI1 * cross(r1, u)^2 + invMass2 + invI2 * cross(r2, u)^2 - -void b2DistanceJointDef::Initialize(b2Body* b1, b2Body* b2, - const b2Vec2& anchor1, const b2Vec2& anchor2) -{ - bodyA = b1; - bodyB = b2; - localAnchorA = bodyA->GetLocalPoint(anchor1); - localAnchorB = bodyB->GetLocalPoint(anchor2); - b2Vec2 d = anchor2 - anchor1; - length = d.Length(); -} - -b2DistanceJoint::b2DistanceJoint(const b2DistanceJointDef* def) -: b2Joint(def) -{ - m_localAnchorA = def->localAnchorA; - m_localAnchorB = def->localAnchorB; - m_length = def->length; - m_frequencyHz = def->frequencyHz; - m_dampingRatio = def->dampingRatio; - m_impulse = 0.0f; - m_gamma = 0.0f; - m_bias = 0.0f; -} - -void b2DistanceJoint::InitVelocityConstraints(const b2SolverData& data) -{ - m_indexA = m_bodyA->m_islandIndex; - m_indexB = m_bodyB->m_islandIndex; - m_localCenterA = m_bodyA->m_sweep.localCenter; - m_localCenterB = m_bodyB->m_sweep.localCenter; - m_invMassA = m_bodyA->m_invMass; - m_invMassB = m_bodyB->m_invMass; - m_invIA = m_bodyA->m_invI; - m_invIB = m_bodyB->m_invI; - - b2Vec2 cA = data.positions[m_indexA].c; - float32 aA = data.positions[m_indexA].a; - b2Vec2 vA = data.velocities[m_indexA].v; - float32 wA = data.velocities[m_indexA].w; - - b2Vec2 cB = data.positions[m_indexB].c; - float32 aB = data.positions[m_indexB].a; - b2Vec2 vB = data.velocities[m_indexB].v; - float32 wB = data.velocities[m_indexB].w; - - b2Rot qA(aA), qB(aB); - - m_rA = b2Mul(qA, m_localAnchorA - m_localCenterA); - m_rB = b2Mul(qB, m_localAnchorB - m_localCenterB); - m_u = cB + m_rB - cA - m_rA; - - // Handle singularity. - float32 length = m_u.Length(); - if (length > b2_linearSlop) - { - m_u *= 1.0f / length; - } - else - { - m_u.Set(0.0f, 0.0f); - } - - float32 crAu = b2Cross(m_rA, m_u); - float32 crBu = b2Cross(m_rB, m_u); - float32 invMass = m_invMassA + m_invIA * crAu * crAu + m_invMassB + m_invIB * crBu * crBu; - - // Compute the effective mass matrix. - m_mass = invMass != 0.0f ? 1.0f / invMass : 0.0f; - - if (m_frequencyHz > 0.0f) - { - float32 C = length - m_length; - - // Frequency - float32 omega = 2.0f * b2_pi * m_frequencyHz; - - // Damping coefficient - float32 d = 2.0f * m_mass * m_dampingRatio * omega; - - // Spring stiffness - float32 k = m_mass * omega * omega; - - // magic formulas - float32 h = data.step.dt; - m_gamma = h * (d + h * k); - m_gamma = m_gamma != 0.0f ? 1.0f / m_gamma : 0.0f; - m_bias = C * h * k * m_gamma; - - invMass += m_gamma; - m_mass = invMass != 0.0f ? 1.0f / invMass : 0.0f; - } - else - { - m_gamma = 0.0f; - m_bias = 0.0f; - } - - if (data.step.warmStarting) - { - // Scale the impulse to support a variable time step. - m_impulse *= data.step.dtRatio; - - b2Vec2 P = m_impulse * m_u; - vA -= m_invMassA * P; - wA -= m_invIA * b2Cross(m_rA, P); - vB += m_invMassB * P; - wB += m_invIB * b2Cross(m_rB, P); - } - else - { - m_impulse = 0.0f; - } - - data.velocities[m_indexA].v = vA; - data.velocities[m_indexA].w = wA; - data.velocities[m_indexB].v = vB; - data.velocities[m_indexB].w = wB; -} - -void b2DistanceJoint::SolveVelocityConstraints(const b2SolverData& data) -{ - b2Vec2 vA = data.velocities[m_indexA].v; - float32 wA = data.velocities[m_indexA].w; - b2Vec2 vB = data.velocities[m_indexB].v; - float32 wB = data.velocities[m_indexB].w; - - // Cdot = dot(u, v + cross(w, r)) - b2Vec2 vpA = vA + b2Cross(wA, m_rA); - b2Vec2 vpB = vB + b2Cross(wB, m_rB); - float32 Cdot = b2Dot(m_u, vpB - vpA); - - float32 impulse = -m_mass * (Cdot + m_bias + m_gamma * m_impulse); - m_impulse += impulse; - - b2Vec2 P = impulse * m_u; - vA -= m_invMassA * P; - wA -= m_invIA * b2Cross(m_rA, P); - vB += m_invMassB * P; - wB += m_invIB * b2Cross(m_rB, P); - - data.velocities[m_indexA].v = vA; - data.velocities[m_indexA].w = wA; - data.velocities[m_indexB].v = vB; - data.velocities[m_indexB].w = wB; -} - -bool b2DistanceJoint::SolvePositionConstraints(const b2SolverData& data) -{ - if (m_frequencyHz > 0.0f) - { - // There is no position correction for soft distance constraints. - return true; - } - - b2Vec2 cA = data.positions[m_indexA].c; - float32 aA = data.positions[m_indexA].a; - b2Vec2 cB = data.positions[m_indexB].c; - float32 aB = data.positions[m_indexB].a; - - b2Rot qA(aA), qB(aB); - - b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_localCenterA); - b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_localCenterB); - b2Vec2 u = cB + rB - cA - rA; - - float32 length = u.Normalize(); - float32 C = length - m_length; - C = b2Clamp(C, -b2_maxLinearCorrection, b2_maxLinearCorrection); - - float32 impulse = -m_mass * C; - b2Vec2 P = impulse * u; - - cA -= m_invMassA * P; - aA -= m_invIA * b2Cross(rA, P); - cB += m_invMassB * P; - aB += m_invIB * b2Cross(rB, P); - - data.positions[m_indexA].c = cA; - data.positions[m_indexA].a = aA; - data.positions[m_indexB].c = cB; - data.positions[m_indexB].a = aB; - - return b2Abs(C) < b2_linearSlop; -} - -b2Vec2 b2DistanceJoint::GetAnchorA() const -{ - return m_bodyA->GetWorldPoint(m_localAnchorA); -} - -b2Vec2 b2DistanceJoint::GetAnchorB() const -{ - return m_bodyB->GetWorldPoint(m_localAnchorB); -} - -b2Vec2 b2DistanceJoint::GetReactionForce(float32 inv_dt) const -{ - b2Vec2 F = (inv_dt * m_impulse) * m_u; - return F; -} - -float32 b2DistanceJoint::GetReactionTorque(float32 inv_dt) const -{ - B2_NOT_USED(inv_dt); - return 0.0f; -} - -void b2DistanceJoint::Dump() -{ - int32 indexA = m_bodyA->m_islandIndex; - int32 indexB = m_bodyB->m_islandIndex; - - b2Log(" b2DistanceJointDef jd;\n"); - b2Log(" jd.bodyA = bodies[%d];\n", indexA); - b2Log(" jd.bodyB = bodies[%d];\n", indexB); - b2Log(" jd.collideConnected = bool(%d);\n", m_collideConnected); - b2Log(" jd.localAnchorA.Set(%.15lef, %.15lef);\n", m_localAnchorA.x, m_localAnchorA.y); - b2Log(" jd.localAnchorB.Set(%.15lef, %.15lef);\n", m_localAnchorB.x, m_localAnchorB.y); - b2Log(" jd.length = %.15lef;\n", m_length); - b2Log(" jd.frequencyHz = %.15lef;\n", m_frequencyHz); - b2Log(" jd.dampingRatio = %.15lef;\n", m_dampingRatio); - b2Log(" joints[%d] = m_world->CreateJoint(&jd);\n", m_index); -} diff --git a/Box2D/Dynamics/Joints/b2DistanceJoint.h b/Box2D/Dynamics/Joints/b2DistanceJoint.h deleted file mode 100644 index 2faba9d479..0000000000 --- a/Box2D/Dynamics/Joints/b2DistanceJoint.h +++ /dev/null @@ -1,169 +0,0 @@ -/* -* Copyright (c) 2006-2007 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_DISTANCE_JOINT_H -#define B2_DISTANCE_JOINT_H - -#include - -/// Distance joint definition. This requires defining an -/// anchor point on both bodies and the non-zero length of the -/// distance joint. The definition uses local anchor points -/// so that the initial configuration can violate the constraint -/// slightly. This helps when saving and loading a game. -/// @warning Do not use a zero or short length. -struct b2DistanceJointDef : public b2JointDef -{ - b2DistanceJointDef() - { - type = e_distanceJoint; - localAnchorA.Set(0.0f, 0.0f); - localAnchorB.Set(0.0f, 0.0f); - length = 1.0f; - frequencyHz = 0.0f; - dampingRatio = 0.0f; - } - - /// Initialize the bodies, anchors, and length using the world - /// anchors. - void Initialize(b2Body* bodyA, b2Body* bodyB, - const b2Vec2& anchorA, const b2Vec2& anchorB); - - /// The local anchor point relative to bodyA's origin. - b2Vec2 localAnchorA; - - /// The local anchor point relative to bodyB's origin. - b2Vec2 localAnchorB; - - /// The natural length between the anchor points. - float32 length; - - /// The mass-spring-damper frequency in Hertz. A value of 0 - /// disables softness. - float32 frequencyHz; - - /// The damping ratio. 0 = no damping, 1 = critical damping. - float32 dampingRatio; -}; - -/// A distance joint constrains two points on two bodies -/// to remain at a fixed distance from each other. You can view -/// this as a massless, rigid rod. -class b2DistanceJoint : public b2Joint -{ -public: - - b2Vec2 GetAnchorA() const; - b2Vec2 GetAnchorB() const; - - /// Get the reaction force given the inverse time step. - /// Unit is N. - b2Vec2 GetReactionForce(float32 inv_dt) const; - - /// Get the reaction torque given the inverse time step. - /// Unit is N*m. This is always zero for a distance joint. - float32 GetReactionTorque(float32 inv_dt) const; - - /// The local anchor point relative to bodyA's origin. - const b2Vec2& GetLocalAnchorA() const { return m_localAnchorA; } - - /// The local anchor point relative to bodyB's origin. - const b2Vec2& GetLocalAnchorB() const { return m_localAnchorB; } - - /// Set/get the natural length. - /// Manipulating the length can lead to non-physical behavior when the frequency is zero. - void SetLength(float32 length); - float32 GetLength() const; - - /// Set/get frequency in Hz. - void SetFrequency(float32 hz); - float32 GetFrequency() const; - - /// Set/get damping ratio. - void SetDampingRatio(float32 ratio); - float32 GetDampingRatio() const; - - /// Dump joint to dmLog - void Dump(); - -protected: - - friend class b2Joint; - b2DistanceJoint(const b2DistanceJointDef* data); - - void InitVelocityConstraints(const b2SolverData& data); - void SolveVelocityConstraints(const b2SolverData& data); - bool SolvePositionConstraints(const b2SolverData& data); - - float32 m_frequencyHz; - float32 m_dampingRatio; - float32 m_bias; - - // Solver shared - b2Vec2 m_localAnchorA; - b2Vec2 m_localAnchorB; - float32 m_gamma; - float32 m_impulse; - float32 m_length; - - // Solver temp - int32 m_indexA; - int32 m_indexB; - b2Vec2 m_u; - b2Vec2 m_rA; - b2Vec2 m_rB; - b2Vec2 m_localCenterA; - b2Vec2 m_localCenterB; - float32 m_invMassA; - float32 m_invMassB; - float32 m_invIA; - float32 m_invIB; - float32 m_mass; -}; - -inline void b2DistanceJoint::SetLength(float32 length) -{ - m_length = length; -} - -inline float32 b2DistanceJoint::GetLength() const -{ - return m_length; -} - -inline void b2DistanceJoint::SetFrequency(float32 hz) -{ - m_frequencyHz = hz; -} - -inline float32 b2DistanceJoint::GetFrequency() const -{ - return m_frequencyHz; -} - -inline void b2DistanceJoint::SetDampingRatio(float32 ratio) -{ - m_dampingRatio = ratio; -} - -inline float32 b2DistanceJoint::GetDampingRatio() const -{ - return m_dampingRatio; -} - -#endif diff --git a/Box2D/Dynamics/Joints/b2FrictionJoint.cpp b/Box2D/Dynamics/Joints/b2FrictionJoint.cpp deleted file mode 100644 index 412d257a78..0000000000 --- a/Box2D/Dynamics/Joints/b2FrictionJoint.cpp +++ /dev/null @@ -1,251 +0,0 @@ -/* -* Copyright (c) 2006-2011 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 -#include -#include - -// Point-to-point constraint -// Cdot = v2 - v1 -// = v2 + cross(w2, r2) - v1 - cross(w1, r1) -// J = [-I -r1_skew I r2_skew ] -// Identity used: -// w k % (rx i + ry j) = w * (-ry i + rx j) - -// Angle constraint -// Cdot = w2 - w1 -// J = [0 0 -1 0 0 1] -// K = invI1 + invI2 - -void b2FrictionJointDef::Initialize(b2Body* bA, b2Body* bB, const b2Vec2& anchor) -{ - bodyA = bA; - bodyB = bB; - localAnchorA = bodyA->GetLocalPoint(anchor); - localAnchorB = bodyB->GetLocalPoint(anchor); -} - -b2FrictionJoint::b2FrictionJoint(const b2FrictionJointDef* def) -: b2Joint(def) -{ - m_localAnchorA = def->localAnchorA; - m_localAnchorB = def->localAnchorB; - - m_linearImpulse.SetZero(); - m_angularImpulse = 0.0f; - - m_maxForce = def->maxForce; - m_maxTorque = def->maxTorque; -} - -void b2FrictionJoint::InitVelocityConstraints(const b2SolverData& data) -{ - m_indexA = m_bodyA->m_islandIndex; - m_indexB = m_bodyB->m_islandIndex; - m_localCenterA = m_bodyA->m_sweep.localCenter; - m_localCenterB = m_bodyB->m_sweep.localCenter; - m_invMassA = m_bodyA->m_invMass; - m_invMassB = m_bodyB->m_invMass; - m_invIA = m_bodyA->m_invI; - m_invIB = m_bodyB->m_invI; - - float32 aA = data.positions[m_indexA].a; - b2Vec2 vA = data.velocities[m_indexA].v; - float32 wA = data.velocities[m_indexA].w; - - float32 aB = data.positions[m_indexB].a; - b2Vec2 vB = data.velocities[m_indexB].v; - float32 wB = data.velocities[m_indexB].w; - - b2Rot qA(aA), qB(aB); - - // Compute the effective mass matrix. - m_rA = b2Mul(qA, m_localAnchorA - m_localCenterA); - m_rB = b2Mul(qB, m_localAnchorB - m_localCenterB); - - // J = [-I -r1_skew I r2_skew] - // [ 0 -1 0 1] - // r_skew = [-ry; rx] - - // Matlab - // K = [ mA+r1y^2*iA+mB+r2y^2*iB, -r1y*iA*r1x-r2y*iB*r2x, -r1y*iA-r2y*iB] - // [ -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB, r1x*iA+r2x*iB] - // [ -r1y*iA-r2y*iB, r1x*iA+r2x*iB, iA+iB] - - float32 mA = m_invMassA, mB = m_invMassB; - float32 iA = m_invIA, iB = m_invIB; - - b2Mat22 K; - K.ex.x = mA + mB + iA * m_rA.y * m_rA.y + iB * m_rB.y * m_rB.y; - K.ex.y = -iA * m_rA.x * m_rA.y - iB * m_rB.x * m_rB.y; - K.ey.x = K.ex.y; - K.ey.y = mA + mB + iA * m_rA.x * m_rA.x + iB * m_rB.x * m_rB.x; - - m_linearMass = K.GetInverse(); - - m_angularMass = iA + iB; - if (m_angularMass > 0.0f) - { - m_angularMass = 1.0f / m_angularMass; - } - - if (data.step.warmStarting) - { - // Scale impulses to support a variable time step. - m_linearImpulse *= data.step.dtRatio; - m_angularImpulse *= data.step.dtRatio; - - b2Vec2 P(m_linearImpulse.x, m_linearImpulse.y); - vA -= mA * P; - wA -= iA * (b2Cross(m_rA, P) + m_angularImpulse); - vB += mB * P; - wB += iB * (b2Cross(m_rB, P) + m_angularImpulse); - } - else - { - m_linearImpulse.SetZero(); - m_angularImpulse = 0.0f; - } - - data.velocities[m_indexA].v = vA; - data.velocities[m_indexA].w = wA; - data.velocities[m_indexB].v = vB; - data.velocities[m_indexB].w = wB; -} - -void b2FrictionJoint::SolveVelocityConstraints(const b2SolverData& data) -{ - b2Vec2 vA = data.velocities[m_indexA].v; - float32 wA = data.velocities[m_indexA].w; - b2Vec2 vB = data.velocities[m_indexB].v; - float32 wB = data.velocities[m_indexB].w; - - float32 mA = m_invMassA, mB = m_invMassB; - float32 iA = m_invIA, iB = m_invIB; - - float32 h = data.step.dt; - - // Solve angular friction - { - float32 Cdot = wB - wA; - float32 impulse = -m_angularMass * Cdot; - - float32 oldImpulse = m_angularImpulse; - float32 maxImpulse = h * m_maxTorque; - m_angularImpulse = b2Clamp(m_angularImpulse + impulse, -maxImpulse, maxImpulse); - impulse = m_angularImpulse - oldImpulse; - - wA -= iA * impulse; - wB += iB * impulse; - } - - // Solve linear friction - { - b2Vec2 Cdot = vB + b2Cross(wB, m_rB) - vA - b2Cross(wA, m_rA); - - b2Vec2 impulse = -b2Mul(m_linearMass, Cdot); - b2Vec2 oldImpulse = m_linearImpulse; - m_linearImpulse += impulse; - - float32 maxImpulse = h * m_maxForce; - - if (m_linearImpulse.LengthSquared() > maxImpulse * maxImpulse) - { - m_linearImpulse.Normalize(); - m_linearImpulse *= maxImpulse; - } - - impulse = m_linearImpulse - oldImpulse; - - vA -= mA * impulse; - wA -= iA * b2Cross(m_rA, impulse); - - vB += mB * impulse; - wB += iB * b2Cross(m_rB, impulse); - } - - data.velocities[m_indexA].v = vA; - data.velocities[m_indexA].w = wA; - data.velocities[m_indexB].v = vB; - data.velocities[m_indexB].w = wB; -} - -bool b2FrictionJoint::SolvePositionConstraints(const b2SolverData& data) -{ - B2_NOT_USED(data); - - return true; -} - -b2Vec2 b2FrictionJoint::GetAnchorA() const -{ - return m_bodyA->GetWorldPoint(m_localAnchorA); -} - -b2Vec2 b2FrictionJoint::GetAnchorB() const -{ - return m_bodyB->GetWorldPoint(m_localAnchorB); -} - -b2Vec2 b2FrictionJoint::GetReactionForce(float32 inv_dt) const -{ - return inv_dt * m_linearImpulse; -} - -float32 b2FrictionJoint::GetReactionTorque(float32 inv_dt) const -{ - return inv_dt * m_angularImpulse; -} - -void b2FrictionJoint::SetMaxForce(float32 force) -{ - b2Assert(b2IsValid(force) && force >= 0.0f); - m_maxForce = force; -} - -float32 b2FrictionJoint::GetMaxForce() const -{ - return m_maxForce; -} - -void b2FrictionJoint::SetMaxTorque(float32 torque) -{ - b2Assert(b2IsValid(torque) && torque >= 0.0f); - m_maxTorque = torque; -} - -float32 b2FrictionJoint::GetMaxTorque() const -{ - return m_maxTorque; -} - -void b2FrictionJoint::Dump() -{ - int32 indexA = m_bodyA->m_islandIndex; - int32 indexB = m_bodyB->m_islandIndex; - - b2Log(" b2FrictionJointDef jd;\n"); - b2Log(" jd.bodyA = bodies[%d];\n", indexA); - b2Log(" jd.bodyB = bodies[%d];\n", indexB); - b2Log(" jd.collideConnected = bool(%d);\n", m_collideConnected); - b2Log(" jd.localAnchorA.Set(%.15lef, %.15lef);\n", m_localAnchorA.x, m_localAnchorA.y); - b2Log(" jd.localAnchorB.Set(%.15lef, %.15lef);\n", m_localAnchorB.x, m_localAnchorB.y); - b2Log(" jd.maxForce = %.15lef;\n", m_maxForce); - b2Log(" jd.maxTorque = %.15lef;\n", m_maxTorque); - b2Log(" joints[%d] = m_world->CreateJoint(&jd);\n", m_index); -} diff --git a/Box2D/Dynamics/Joints/b2FrictionJoint.h b/Box2D/Dynamics/Joints/b2FrictionJoint.h deleted file mode 100644 index d57abe80d3..0000000000 --- a/Box2D/Dynamics/Joints/b2FrictionJoint.h +++ /dev/null @@ -1,119 +0,0 @@ -/* -* Copyright (c) 2006-2007 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_FRICTION_JOINT_H -#define B2_FRICTION_JOINT_H - -#include - -/// Friction joint definition. -struct b2FrictionJointDef : public b2JointDef -{ - b2FrictionJointDef() - { - type = e_frictionJoint; - localAnchorA.SetZero(); - localAnchorB.SetZero(); - maxForce = 0.0f; - maxTorque = 0.0f; - } - - /// Initialize the bodies, anchors, axis, and reference angle using the world - /// anchor and world axis. - void Initialize(b2Body* bodyA, b2Body* bodyB, const b2Vec2& anchor); - - /// The local anchor point relative to bodyA's origin. - b2Vec2 localAnchorA; - - /// The local anchor point relative to bodyB's origin. - b2Vec2 localAnchorB; - - /// The maximum friction force in N. - float32 maxForce; - - /// The maximum friction torque in N-m. - float32 maxTorque; -}; - -/// Friction joint. This is used for top-down friction. -/// It provides 2D translational friction and angular friction. -class b2FrictionJoint : public b2Joint -{ -public: - b2Vec2 GetAnchorA() const; - b2Vec2 GetAnchorB() const; - - b2Vec2 GetReactionForce(float32 inv_dt) const; - float32 GetReactionTorque(float32 inv_dt) const; - - /// The local anchor point relative to bodyA's origin. - const b2Vec2& GetLocalAnchorA() const { return m_localAnchorA; } - - /// The local anchor point relative to bodyB's origin. - const b2Vec2& GetLocalAnchorB() const { return m_localAnchorB; } - - /// Set the maximum friction force in N. - void SetMaxForce(float32 force); - - /// Get the maximum friction force in N. - float32 GetMaxForce() const; - - /// Set the maximum friction torque in N*m. - void SetMaxTorque(float32 torque); - - /// Get the maximum friction torque in N*m. - float32 GetMaxTorque() const; - - /// Dump joint to dmLog - void Dump(); - -protected: - - friend class b2Joint; - - b2FrictionJoint(const b2FrictionJointDef* def); - - void InitVelocityConstraints(const b2SolverData& data); - void SolveVelocityConstraints(const b2SolverData& data); - bool SolvePositionConstraints(const b2SolverData& data); - - b2Vec2 m_localAnchorA; - b2Vec2 m_localAnchorB; - - // Solver shared - b2Vec2 m_linearImpulse; - float32 m_angularImpulse; - float32 m_maxForce; - float32 m_maxTorque; - - // Solver temp - int32 m_indexA; - int32 m_indexB; - b2Vec2 m_rA; - b2Vec2 m_rB; - b2Vec2 m_localCenterA; - b2Vec2 m_localCenterB; - float32 m_invMassA; - float32 m_invMassB; - float32 m_invIA; - float32 m_invIB; - b2Mat22 m_linearMass; - float32 m_angularMass; -}; - -#endif diff --git a/Box2D/Dynamics/Joints/b2GearJoint.cpp b/Box2D/Dynamics/Joints/b2GearJoint.cpp deleted file mode 100644 index 155eb0c1de..0000000000 --- a/Box2D/Dynamics/Joints/b2GearJoint.cpp +++ /dev/null @@ -1,419 +0,0 @@ -/* -* Copyright (c) 2007-2011 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 -#include -#include -#include -#include - -// Gear Joint: -// C0 = (coordinate1 + ratio * coordinate2)_initial -// C = (coordinate1 + ratio * coordinate2) - C0 = 0 -// J = [J1 ratio * J2] -// K = J * invM * JT -// = J1 * invM1 * J1T + ratio * ratio * J2 * invM2 * J2T -// -// Revolute: -// coordinate = rotation -// Cdot = angularVelocity -// J = [0 0 1] -// K = J * invM * JT = invI -// -// Prismatic: -// coordinate = dot(p - pg, ug) -// Cdot = dot(v + cross(w, r), ug) -// J = [ug cross(r, ug)] -// K = J * invM * JT = invMass + invI * cross(r, ug)^2 - -b2GearJoint::b2GearJoint(const b2GearJointDef* def) -: b2Joint(def) -{ - m_joint1 = def->joint1; - m_joint2 = def->joint2; - - m_typeA = m_joint1->GetType(); - m_typeB = m_joint2->GetType(); - - b2Assert(m_typeA == e_revoluteJoint || m_typeA == e_prismaticJoint); - b2Assert(m_typeB == e_revoluteJoint || m_typeB == e_prismaticJoint); - - float32 coordinateA, coordinateB; - - // TODO_ERIN there might be some problem with the joint edges in b2Joint. - - m_bodyC = m_joint1->GetBodyA(); - m_bodyA = m_joint1->GetBodyB(); - - // Get geometry of joint1 - b2Transform xfA = m_bodyA->m_xf; - float32 aA = m_bodyA->m_sweep.a; - b2Transform xfC = m_bodyC->m_xf; - float32 aC = m_bodyC->m_sweep.a; - - if (m_typeA == e_revoluteJoint) - { - b2RevoluteJoint* revolute = (b2RevoluteJoint*)def->joint1; - m_localAnchorC = revolute->m_localAnchorA; - m_localAnchorA = revolute->m_localAnchorB; - m_referenceAngleA = revolute->m_referenceAngle; - m_localAxisC.SetZero(); - - coordinateA = aA - aC - m_referenceAngleA; - } - else - { - b2PrismaticJoint* prismatic = (b2PrismaticJoint*)def->joint1; - m_localAnchorC = prismatic->m_localAnchorA; - m_localAnchorA = prismatic->m_localAnchorB; - m_referenceAngleA = prismatic->m_referenceAngle; - m_localAxisC = prismatic->m_localXAxisA; - - b2Vec2 pC = m_localAnchorC; - b2Vec2 pA = b2MulT(xfC.q, b2Mul(xfA.q, m_localAnchorA) + (xfA.p - xfC.p)); - coordinateA = b2Dot(pA - pC, m_localAxisC); - } - - m_bodyD = m_joint2->GetBodyA(); - m_bodyB = m_joint2->GetBodyB(); - - // Get geometry of joint2 - b2Transform xfB = m_bodyB->m_xf; - float32 aB = m_bodyB->m_sweep.a; - b2Transform xfD = m_bodyD->m_xf; - float32 aD = m_bodyD->m_sweep.a; - - if (m_typeB == e_revoluteJoint) - { - b2RevoluteJoint* revolute = (b2RevoluteJoint*)def->joint2; - m_localAnchorD = revolute->m_localAnchorA; - m_localAnchorB = revolute->m_localAnchorB; - m_referenceAngleB = revolute->m_referenceAngle; - m_localAxisD.SetZero(); - - coordinateB = aB - aD - m_referenceAngleB; - } - else - { - b2PrismaticJoint* prismatic = (b2PrismaticJoint*)def->joint2; - m_localAnchorD = prismatic->m_localAnchorA; - m_localAnchorB = prismatic->m_localAnchorB; - m_referenceAngleB = prismatic->m_referenceAngle; - m_localAxisD = prismatic->m_localXAxisA; - - b2Vec2 pD = m_localAnchorD; - b2Vec2 pB = b2MulT(xfD.q, b2Mul(xfB.q, m_localAnchorB) + (xfB.p - xfD.p)); - coordinateB = b2Dot(pB - pD, m_localAxisD); - } - - m_ratio = def->ratio; - - m_constant = coordinateA + m_ratio * coordinateB; - - m_impulse = 0.0f; -} - -void b2GearJoint::InitVelocityConstraints(const b2SolverData& data) -{ - m_indexA = m_bodyA->m_islandIndex; - m_indexB = m_bodyB->m_islandIndex; - m_indexC = m_bodyC->m_islandIndex; - m_indexD = m_bodyD->m_islandIndex; - m_lcA = m_bodyA->m_sweep.localCenter; - m_lcB = m_bodyB->m_sweep.localCenter; - m_lcC = m_bodyC->m_sweep.localCenter; - m_lcD = m_bodyD->m_sweep.localCenter; - m_mA = m_bodyA->m_invMass; - m_mB = m_bodyB->m_invMass; - m_mC = m_bodyC->m_invMass; - m_mD = m_bodyD->m_invMass; - m_iA = m_bodyA->m_invI; - m_iB = m_bodyB->m_invI; - m_iC = m_bodyC->m_invI; - m_iD = m_bodyD->m_invI; - - float32 aA = data.positions[m_indexA].a; - b2Vec2 vA = data.velocities[m_indexA].v; - float32 wA = data.velocities[m_indexA].w; - - float32 aB = data.positions[m_indexB].a; - b2Vec2 vB = data.velocities[m_indexB].v; - float32 wB = data.velocities[m_indexB].w; - - float32 aC = data.positions[m_indexC].a; - b2Vec2 vC = data.velocities[m_indexC].v; - float32 wC = data.velocities[m_indexC].w; - - float32 aD = data.positions[m_indexD].a; - b2Vec2 vD = data.velocities[m_indexD].v; - float32 wD = data.velocities[m_indexD].w; - - b2Rot qA(aA), qB(aB), qC(aC), qD(aD); - - m_mass = 0.0f; - - if (m_typeA == e_revoluteJoint) - { - m_JvAC.SetZero(); - m_JwA = 1.0f; - m_JwC = 1.0f; - m_mass += m_iA + m_iC; - } - else - { - b2Vec2 u = b2Mul(qC, m_localAxisC); - b2Vec2 rC = b2Mul(qC, m_localAnchorC - m_lcC); - b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_lcA); - m_JvAC = u; - m_JwC = b2Cross(rC, u); - m_JwA = b2Cross(rA, u); - m_mass += m_mC + m_mA + m_iC * m_JwC * m_JwC + m_iA * m_JwA * m_JwA; - } - - if (m_typeB == e_revoluteJoint) - { - m_JvBD.SetZero(); - m_JwB = m_ratio; - m_JwD = m_ratio; - m_mass += m_ratio * m_ratio * (m_iB + m_iD); - } - else - { - b2Vec2 u = b2Mul(qD, m_localAxisD); - b2Vec2 rD = b2Mul(qD, m_localAnchorD - m_lcD); - b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_lcB); - m_JvBD = m_ratio * u; - m_JwD = m_ratio * b2Cross(rD, u); - m_JwB = m_ratio * b2Cross(rB, u); - m_mass += m_ratio * m_ratio * (m_mD + m_mB) + m_iD * m_JwD * m_JwD + m_iB * m_JwB * m_JwB; - } - - // Compute effective mass. - m_mass = m_mass > 0.0f ? 1.0f / m_mass : 0.0f; - - if (data.step.warmStarting) - { - vA += (m_mA * m_impulse) * m_JvAC; - wA += m_iA * m_impulse * m_JwA; - vB += (m_mB * m_impulse) * m_JvBD; - wB += m_iB * m_impulse * m_JwB; - vC -= (m_mC * m_impulse) * m_JvAC; - wC -= m_iC * m_impulse * m_JwC; - vD -= (m_mD * m_impulse) * m_JvBD; - wD -= m_iD * m_impulse * m_JwD; - } - else - { - m_impulse = 0.0f; - } - - data.velocities[m_indexA].v = vA; - data.velocities[m_indexA].w = wA; - data.velocities[m_indexB].v = vB; - data.velocities[m_indexB].w = wB; - data.velocities[m_indexC].v = vC; - data.velocities[m_indexC].w = wC; - data.velocities[m_indexD].v = vD; - data.velocities[m_indexD].w = wD; -} - -void b2GearJoint::SolveVelocityConstraints(const b2SolverData& data) -{ - b2Vec2 vA = data.velocities[m_indexA].v; - float32 wA = data.velocities[m_indexA].w; - b2Vec2 vB = data.velocities[m_indexB].v; - float32 wB = data.velocities[m_indexB].w; - b2Vec2 vC = data.velocities[m_indexC].v; - float32 wC = data.velocities[m_indexC].w; - b2Vec2 vD = data.velocities[m_indexD].v; - float32 wD = data.velocities[m_indexD].w; - - float32 Cdot = b2Dot(m_JvAC, vA - vC) + b2Dot(m_JvBD, vB - vD); - Cdot += (m_JwA * wA - m_JwC * wC) + (m_JwB * wB - m_JwD * wD); - - float32 impulse = -m_mass * Cdot; - m_impulse += impulse; - - vA += (m_mA * impulse) * m_JvAC; - wA += m_iA * impulse * m_JwA; - vB += (m_mB * impulse) * m_JvBD; - wB += m_iB * impulse * m_JwB; - vC -= (m_mC * impulse) * m_JvAC; - wC -= m_iC * impulse * m_JwC; - vD -= (m_mD * impulse) * m_JvBD; - wD -= m_iD * impulse * m_JwD; - - data.velocities[m_indexA].v = vA; - data.velocities[m_indexA].w = wA; - data.velocities[m_indexB].v = vB; - data.velocities[m_indexB].w = wB; - data.velocities[m_indexC].v = vC; - data.velocities[m_indexC].w = wC; - data.velocities[m_indexD].v = vD; - data.velocities[m_indexD].w = wD; -} - -bool b2GearJoint::SolvePositionConstraints(const b2SolverData& data) -{ - b2Vec2 cA = data.positions[m_indexA].c; - float32 aA = data.positions[m_indexA].a; - b2Vec2 cB = data.positions[m_indexB].c; - float32 aB = data.positions[m_indexB].a; - b2Vec2 cC = data.positions[m_indexC].c; - float32 aC = data.positions[m_indexC].a; - b2Vec2 cD = data.positions[m_indexD].c; - float32 aD = data.positions[m_indexD].a; - - b2Rot qA(aA), qB(aB), qC(aC), qD(aD); - - float32 linearError = 0.0f; - - float32 coordinateA, coordinateB; - - b2Vec2 JvAC, JvBD; - float32 JwA, JwB, JwC, JwD; - float32 mass = 0.0f; - - if (m_typeA == e_revoluteJoint) - { - JvAC.SetZero(); - JwA = 1.0f; - JwC = 1.0f; - mass += m_iA + m_iC; - - coordinateA = aA - aC - m_referenceAngleA; - } - else - { - b2Vec2 u = b2Mul(qC, m_localAxisC); - b2Vec2 rC = b2Mul(qC, m_localAnchorC - m_lcC); - b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_lcA); - JvAC = u; - JwC = b2Cross(rC, u); - JwA = b2Cross(rA, u); - mass += m_mC + m_mA + m_iC * JwC * JwC + m_iA * JwA * JwA; - - b2Vec2 pC = m_localAnchorC - m_lcC; - b2Vec2 pA = b2MulT(qC, rA + (cA - cC)); - coordinateA = b2Dot(pA - pC, m_localAxisC); - } - - if (m_typeB == e_revoluteJoint) - { - JvBD.SetZero(); - JwB = m_ratio; - JwD = m_ratio; - mass += m_ratio * m_ratio * (m_iB + m_iD); - - coordinateB = aB - aD - m_referenceAngleB; - } - else - { - b2Vec2 u = b2Mul(qD, m_localAxisD); - b2Vec2 rD = b2Mul(qD, m_localAnchorD - m_lcD); - b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_lcB); - JvBD = m_ratio * u; - JwD = m_ratio * b2Cross(rD, u); - JwB = m_ratio * b2Cross(rB, u); - mass += m_ratio * m_ratio * (m_mD + m_mB) + m_iD * JwD * JwD + m_iB * JwB * JwB; - - b2Vec2 pD = m_localAnchorD - m_lcD; - b2Vec2 pB = b2MulT(qD, rB + (cB - cD)); - coordinateB = b2Dot(pB - pD, m_localAxisD); - } - - float32 C = (coordinateA + m_ratio * coordinateB) - m_constant; - - float32 impulse = 0.0f; - if (mass > 0.0f) - { - impulse = -C / mass; - } - - cA += m_mA * impulse * JvAC; - aA += m_iA * impulse * JwA; - cB += m_mB * impulse * JvBD; - aB += m_iB * impulse * JwB; - cC -= m_mC * impulse * JvAC; - aC -= m_iC * impulse * JwC; - cD -= m_mD * impulse * JvBD; - aD -= m_iD * impulse * JwD; - - data.positions[m_indexA].c = cA; - data.positions[m_indexA].a = aA; - data.positions[m_indexB].c = cB; - data.positions[m_indexB].a = aB; - data.positions[m_indexC].c = cC; - data.positions[m_indexC].a = aC; - data.positions[m_indexD].c = cD; - data.positions[m_indexD].a = aD; - - // TODO_ERIN not implemented - return linearError < b2_linearSlop; -} - -b2Vec2 b2GearJoint::GetAnchorA() const -{ - return m_bodyA->GetWorldPoint(m_localAnchorA); -} - -b2Vec2 b2GearJoint::GetAnchorB() const -{ - return m_bodyB->GetWorldPoint(m_localAnchorB); -} - -b2Vec2 b2GearJoint::GetReactionForce(float32 inv_dt) const -{ - b2Vec2 P = m_impulse * m_JvAC; - return inv_dt * P; -} - -float32 b2GearJoint::GetReactionTorque(float32 inv_dt) const -{ - float32 L = m_impulse * m_JwA; - return inv_dt * L; -} - -void b2GearJoint::SetRatio(float32 ratio) -{ - b2Assert(b2IsValid(ratio)); - m_ratio = ratio; -} - -float32 b2GearJoint::GetRatio() const -{ - return m_ratio; -} - -void b2GearJoint::Dump() -{ - int32 indexA = m_bodyA->m_islandIndex; - int32 indexB = m_bodyB->m_islandIndex; - - int32 index1 = m_joint1->m_index; - int32 index2 = m_joint2->m_index; - - b2Log(" b2GearJointDef jd;\n"); - b2Log(" jd.bodyA = bodies[%d];\n", indexA); - b2Log(" jd.bodyB = bodies[%d];\n", indexB); - b2Log(" jd.collideConnected = bool(%d);\n", m_collideConnected); - b2Log(" jd.joint1 = joints[%d];\n", index1); - b2Log(" jd.joint2 = joints[%d];\n", index2); - b2Log(" jd.ratio = %.15lef;\n", m_ratio); - b2Log(" joints[%d] = m_world->CreateJoint(&jd);\n", m_index); -} diff --git a/Box2D/Dynamics/Joints/b2GearJoint.h b/Box2D/Dynamics/Joints/b2GearJoint.h deleted file mode 100644 index fd3d7b2fa3..0000000000 --- a/Box2D/Dynamics/Joints/b2GearJoint.h +++ /dev/null @@ -1,125 +0,0 @@ -/* -* Copyright (c) 2006-2011 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_GEAR_JOINT_H -#define B2_GEAR_JOINT_H - -#include - -/// Gear joint definition. This definition requires two existing -/// revolute or prismatic joints (any combination will work). -struct b2GearJointDef : public b2JointDef -{ - b2GearJointDef() - { - type = e_gearJoint; - joint1 = NULL; - joint2 = NULL; - ratio = 1.0f; - } - - /// The first revolute/prismatic joint attached to the gear joint. - b2Joint* joint1; - - /// The second revolute/prismatic joint attached to the gear joint. - b2Joint* joint2; - - /// The gear ratio. - /// @see b2GearJoint for explanation. - float32 ratio; -}; - -/// A gear joint is used to connect two joints together. Either joint -/// can be a revolute or prismatic joint. You specify a gear ratio -/// to bind the motions together: -/// coordinate1 + ratio * coordinate2 = constant -/// The ratio can be negative or positive. If one joint is a revolute joint -/// and the other joint is a prismatic joint, then the ratio will have units -/// of length or units of 1/length. -/// @warning You have to manually destroy the gear joint if joint1 or joint2 -/// is destroyed. -class b2GearJoint : public b2Joint -{ -public: - b2Vec2 GetAnchorA() const; - b2Vec2 GetAnchorB() const; - - b2Vec2 GetReactionForce(float32 inv_dt) const; - float32 GetReactionTorque(float32 inv_dt) const; - - /// Get the first joint. - b2Joint* GetJoint1() { return m_joint1; } - - /// Get the second joint. - b2Joint* GetJoint2() { return m_joint2; } - - /// Set/Get the gear ratio. - void SetRatio(float32 ratio); - float32 GetRatio() const; - - /// Dump joint to dmLog - void Dump(); - -protected: - - friend class b2Joint; - b2GearJoint(const b2GearJointDef* data); - - void InitVelocityConstraints(const b2SolverData& data); - void SolveVelocityConstraints(const b2SolverData& data); - bool SolvePositionConstraints(const b2SolverData& data); - - b2Joint* m_joint1; - b2Joint* m_joint2; - - b2JointType m_typeA; - b2JointType m_typeB; - - // Body A is connected to body C - // Body B is connected to body D - b2Body* m_bodyC; - b2Body* m_bodyD; - - // Solver shared - b2Vec2 m_localAnchorA; - b2Vec2 m_localAnchorB; - b2Vec2 m_localAnchorC; - b2Vec2 m_localAnchorD; - - b2Vec2 m_localAxisC; - b2Vec2 m_localAxisD; - - float32 m_referenceAngleA; - float32 m_referenceAngleB; - - float32 m_constant; - float32 m_ratio; - - float32 m_impulse; - - // Solver temp - int32 m_indexA, m_indexB, m_indexC, m_indexD; - b2Vec2 m_lcA, m_lcB, m_lcC, m_lcD; - float32 m_mA, m_mB, m_mC, m_mD; - float32 m_iA, m_iB, m_iC, m_iD; - b2Vec2 m_JvAC, m_JvBD; - float32 m_JwA, m_JwB, m_JwC, m_JwD; - float32 m_mass; -}; - -#endif diff --git a/Box2D/Dynamics/Joints/b2Joint.cpp b/Box2D/Dynamics/Joints/b2Joint.cpp deleted file mode 100644 index dd55be94fa..0000000000 --- a/Box2D/Dynamics/Joints/b2Joint.cpp +++ /dev/null @@ -1,211 +0,0 @@ -/* -* Copyright (c) 2006-2007 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 -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include - -b2Joint* b2Joint::Create(const b2JointDef* def, b2BlockAllocator* allocator) -{ - b2Joint* joint = NULL; - - switch (def->type) - { - case e_distanceJoint: - { - void* mem = allocator->Allocate(sizeof(b2DistanceJoint)); - joint = new (mem) b2DistanceJoint(static_cast(def)); - } - break; - - case e_mouseJoint: - { - void* mem = allocator->Allocate(sizeof(b2MouseJoint)); - joint = new (mem) b2MouseJoint(static_cast(def)); - } - break; - - case e_prismaticJoint: - { - void* mem = allocator->Allocate(sizeof(b2PrismaticJoint)); - joint = new (mem) b2PrismaticJoint(static_cast(def)); - } - break; - - case e_revoluteJoint: - { - void* mem = allocator->Allocate(sizeof(b2RevoluteJoint)); - joint = new (mem) b2RevoluteJoint(static_cast(def)); - } - break; - - case e_pulleyJoint: - { - void* mem = allocator->Allocate(sizeof(b2PulleyJoint)); - joint = new (mem) b2PulleyJoint(static_cast(def)); - } - break; - - case e_gearJoint: - { - void* mem = allocator->Allocate(sizeof(b2GearJoint)); - joint = new (mem) b2GearJoint(static_cast(def)); - } - break; - - case e_wheelJoint: - { - void* mem = allocator->Allocate(sizeof(b2WheelJoint)); - joint = new (mem) b2WheelJoint(static_cast(def)); - } - break; - - case e_weldJoint: - { - void* mem = allocator->Allocate(sizeof(b2WeldJoint)); - joint = new (mem) b2WeldJoint(static_cast(def)); - } - break; - - case e_frictionJoint: - { - void* mem = allocator->Allocate(sizeof(b2FrictionJoint)); - joint = new (mem) b2FrictionJoint(static_cast(def)); - } - break; - - case e_ropeJoint: - { - void* mem = allocator->Allocate(sizeof(b2RopeJoint)); - joint = new (mem) b2RopeJoint(static_cast(def)); - } - break; - - case e_motorJoint: - { - void* mem = allocator->Allocate(sizeof(b2MotorJoint)); - joint = new (mem) b2MotorJoint(static_cast(def)); - } - break; - - default: - b2Assert(false); - break; - } - - return joint; -} - -void b2Joint::Destroy(b2Joint* joint, b2BlockAllocator* allocator) -{ - joint->~b2Joint(); - switch (joint->m_type) - { - case e_distanceJoint: - allocator->Free(joint, sizeof(b2DistanceJoint)); - break; - - case e_mouseJoint: - allocator->Free(joint, sizeof(b2MouseJoint)); - break; - - case e_prismaticJoint: - allocator->Free(joint, sizeof(b2PrismaticJoint)); - break; - - case e_revoluteJoint: - allocator->Free(joint, sizeof(b2RevoluteJoint)); - break; - - case e_pulleyJoint: - allocator->Free(joint, sizeof(b2PulleyJoint)); - break; - - case e_gearJoint: - allocator->Free(joint, sizeof(b2GearJoint)); - break; - - case e_wheelJoint: - allocator->Free(joint, sizeof(b2WheelJoint)); - break; - - case e_weldJoint: - allocator->Free(joint, sizeof(b2WeldJoint)); - break; - - case e_frictionJoint: - allocator->Free(joint, sizeof(b2FrictionJoint)); - break; - - case e_ropeJoint: - allocator->Free(joint, sizeof(b2RopeJoint)); - break; - - case e_motorJoint: - allocator->Free(joint, sizeof(b2MotorJoint)); - break; - - default: - b2Assert(false); - break; - } -} - -b2Joint::b2Joint(const b2JointDef* def) -{ - b2Assert(def->bodyA != def->bodyB); - - m_type = def->type; - m_prev = NULL; - m_next = NULL; - m_bodyA = def->bodyA; - m_bodyB = def->bodyB; - m_index = 0; - m_collideConnected = def->collideConnected; - m_islandFlag = false; - m_userData = def->userData; - - m_edgeA.joint = NULL; - m_edgeA.other = NULL; - m_edgeA.prev = NULL; - m_edgeA.next = NULL; - - m_edgeB.joint = NULL; - m_edgeB.other = NULL; - m_edgeB.prev = NULL; - m_edgeB.next = NULL; -} - -bool b2Joint::IsActive() const -{ - return m_bodyA->IsActive() && m_bodyB->IsActive(); -} diff --git a/Box2D/Dynamics/Joints/b2Joint.h b/Box2D/Dynamics/Joints/b2Joint.h deleted file mode 100644 index dd076b6b21..0000000000 --- a/Box2D/Dynamics/Joints/b2Joint.h +++ /dev/null @@ -1,226 +0,0 @@ -/* -* Copyright (c) 2006-2007 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_JOINT_H -#define B2_JOINT_H - -#include - -class b2Body; -class b2Joint; -struct b2SolverData; -class b2BlockAllocator; - -enum b2JointType -{ - e_unknownJoint, - e_revoluteJoint, - e_prismaticJoint, - e_distanceJoint, - e_pulleyJoint, - e_mouseJoint, - e_gearJoint, - e_wheelJoint, - e_weldJoint, - e_frictionJoint, - e_ropeJoint, - e_motorJoint -}; - -enum b2LimitState -{ - e_inactiveLimit, - e_atLowerLimit, - e_atUpperLimit, - e_equalLimits -}; - -struct b2Jacobian -{ - b2Vec2 linear; - float32 angularA; - float32 angularB; -}; - -/// A joint edge is used to connect bodies and joints together -/// in a joint graph where each body is a node and each joint -/// is an edge. A joint edge belongs to a doubly linked list -/// maintained in each attached body. Each joint has two joint -/// nodes, one for each attached body. -struct b2JointEdge -{ - b2Body* other; ///< provides quick access to the other body attached. - b2Joint* joint; ///< the joint - b2JointEdge* prev; ///< the previous joint edge in the body's joint list - b2JointEdge* next; ///< the next joint edge in the body's joint list -}; - -/// Joint definitions are used to construct joints. -struct b2JointDef -{ - b2JointDef() - { - type = e_unknownJoint; - userData = NULL; - bodyA = NULL; - bodyB = NULL; - collideConnected = false; - } - - /// The joint type is set automatically for concrete joint types. - b2JointType type; - - /// Use this to attach application specific data to your joints. - void* userData; - - /// The first attached body. - b2Body* bodyA; - - /// The second attached body. - b2Body* bodyB; - - /// Set this flag to true if the attached bodies should collide. - bool collideConnected; -}; - -/// The base joint class. Joints are used to constraint two bodies together in -/// various fashions. Some joints also feature limits and motors. -class b2Joint -{ -public: - - /// Get the type of the concrete joint. - b2JointType GetType() const; - - /// Get the first body attached to this joint. - b2Body* GetBodyA(); - - /// Get the second body attached to this joint. - b2Body* GetBodyB(); - - /// Get the anchor point on bodyA in world coordinates. - virtual b2Vec2 GetAnchorA() const = 0; - - /// Get the anchor point on bodyB in world coordinates. - virtual b2Vec2 GetAnchorB() const = 0; - - /// Get the reaction force on bodyB at the joint anchor in Newtons. - virtual b2Vec2 GetReactionForce(float32 inv_dt) const = 0; - - /// Get the reaction torque on bodyB in N*m. - virtual float32 GetReactionTorque(float32 inv_dt) const = 0; - - /// Get the next joint the world joint list. - b2Joint* GetNext(); - const b2Joint* GetNext() const; - - /// Get the user data pointer. - void* GetUserData() const; - - /// Set the user data pointer. - void SetUserData(void* data); - - /// Short-cut function to determine if either body is inactive. - bool IsActive() const; - - /// Get collide connected. - /// Note: modifying the collide connect flag won't work correctly because - /// the flag is only checked when fixture AABBs begin to overlap. - bool GetCollideConnected() const; - - /// Dump this joint to the log file. - virtual void Dump() { b2Log("// Dump is not supported for this joint type.\n"); } - - /// Shift the origin for any points stored in world coordinates. - virtual void ShiftOrigin(const b2Vec2& newOrigin) { B2_NOT_USED(newOrigin); } - -protected: - friend class b2World; - friend class b2Body; - friend class b2Island; - friend class b2GearJoint; - - static b2Joint* Create(const b2JointDef* def, b2BlockAllocator* allocator); - static void Destroy(b2Joint* joint, b2BlockAllocator* allocator); - - b2Joint(const b2JointDef* def); - virtual ~b2Joint() {} - - virtual void InitVelocityConstraints(const b2SolverData& data) = 0; - virtual void SolveVelocityConstraints(const b2SolverData& data) = 0; - - // This returns true if the position errors are within tolerance. - virtual bool SolvePositionConstraints(const b2SolverData& data) = 0; - - b2JointType m_type; - b2Joint* m_prev; - b2Joint* m_next; - b2JointEdge m_edgeA; - b2JointEdge m_edgeB; - b2Body* m_bodyA; - b2Body* m_bodyB; - - int32 m_index; - - bool m_islandFlag; - bool m_collideConnected; - - void* m_userData; -}; - -inline b2JointType b2Joint::GetType() const -{ - return m_type; -} - -inline b2Body* b2Joint::GetBodyA() -{ - return m_bodyA; -} - -inline b2Body* b2Joint::GetBodyB() -{ - return m_bodyB; -} - -inline b2Joint* b2Joint::GetNext() -{ - return m_next; -} - -inline const b2Joint* b2Joint::GetNext() const -{ - return m_next; -} - -inline void* b2Joint::GetUserData() const -{ - return m_userData; -} - -inline void b2Joint::SetUserData(void* data) -{ - m_userData = data; -} - -inline bool b2Joint::GetCollideConnected() const -{ - return m_collideConnected; -} - -#endif diff --git a/Box2D/Dynamics/Joints/b2MotorJoint.cpp b/Box2D/Dynamics/Joints/b2MotorJoint.cpp deleted file mode 100644 index e5bbcb165e..0000000000 --- a/Box2D/Dynamics/Joints/b2MotorJoint.cpp +++ /dev/null @@ -1,304 +0,0 @@ -/* -* Copyright (c) 2006-2012 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 -#include -#include - -// Point-to-point constraint -// Cdot = v2 - v1 -// = v2 + cross(w2, r2) - v1 - cross(w1, r1) -// J = [-I -r1_skew I r2_skew ] -// Identity used: -// w k % (rx i + ry j) = w * (-ry i + rx j) - -// Angle constraint -// Cdot = w2 - w1 -// J = [0 0 -1 0 0 1] -// K = invI1 + invI2 - -void b2MotorJointDef::Initialize(b2Body* bA, b2Body* bB) -{ - bodyA = bA; - bodyB = bB; - b2Vec2 xB = bodyB->GetPosition(); - linearOffset = bodyA->GetLocalPoint(xB); - - float32 angleA = bodyA->GetAngle(); - float32 angleB = bodyB->GetAngle(); - angularOffset = angleB - angleA; -} - -b2MotorJoint::b2MotorJoint(const b2MotorJointDef* def) -: b2Joint(def) -{ - m_linearOffset = def->linearOffset; - m_angularOffset = def->angularOffset; - - m_linearImpulse.SetZero(); - m_angularImpulse = 0.0f; - - m_maxForce = def->maxForce; - m_maxTorque = def->maxTorque; - m_correctionFactor = def->correctionFactor; -} - -void b2MotorJoint::InitVelocityConstraints(const b2SolverData& data) -{ - m_indexA = m_bodyA->m_islandIndex; - m_indexB = m_bodyB->m_islandIndex; - m_localCenterA = m_bodyA->m_sweep.localCenter; - m_localCenterB = m_bodyB->m_sweep.localCenter; - m_invMassA = m_bodyA->m_invMass; - m_invMassB = m_bodyB->m_invMass; - m_invIA = m_bodyA->m_invI; - m_invIB = m_bodyB->m_invI; - - b2Vec2 cA = data.positions[m_indexA].c; - float32 aA = data.positions[m_indexA].a; - b2Vec2 vA = data.velocities[m_indexA].v; - float32 wA = data.velocities[m_indexA].w; - - b2Vec2 cB = data.positions[m_indexB].c; - float32 aB = data.positions[m_indexB].a; - b2Vec2 vB = data.velocities[m_indexB].v; - float32 wB = data.velocities[m_indexB].w; - - b2Rot qA(aA), qB(aB); - - // Compute the effective mass matrix. - m_rA = b2Mul(qA, -m_localCenterA); - m_rB = b2Mul(qB, -m_localCenterB); - - // J = [-I -r1_skew I r2_skew] - // [ 0 -1 0 1] - // r_skew = [-ry; rx] - - // Matlab - // K = [ mA+r1y^2*iA+mB+r2y^2*iB, -r1y*iA*r1x-r2y*iB*r2x, -r1y*iA-r2y*iB] - // [ -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB, r1x*iA+r2x*iB] - // [ -r1y*iA-r2y*iB, r1x*iA+r2x*iB, iA+iB] - - float32 mA = m_invMassA, mB = m_invMassB; - float32 iA = m_invIA, iB = m_invIB; - - b2Mat22 K; - K.ex.x = mA + mB + iA * m_rA.y * m_rA.y + iB * m_rB.y * m_rB.y; - K.ex.y = -iA * m_rA.x * m_rA.y - iB * m_rB.x * m_rB.y; - K.ey.x = K.ex.y; - K.ey.y = mA + mB + iA * m_rA.x * m_rA.x + iB * m_rB.x * m_rB.x; - - m_linearMass = K.GetInverse(); - - m_angularMass = iA + iB; - if (m_angularMass > 0.0f) - { - m_angularMass = 1.0f / m_angularMass; - } - - m_linearError = cB + m_rB - cA - m_rA - b2Mul(qA, m_linearOffset); - m_angularError = aB - aA - m_angularOffset; - - if (data.step.warmStarting) - { - // Scale impulses to support a variable time step. - m_linearImpulse *= data.step.dtRatio; - m_angularImpulse *= data.step.dtRatio; - - b2Vec2 P(m_linearImpulse.x, m_linearImpulse.y); - vA -= mA * P; - wA -= iA * (b2Cross(m_rA, P) + m_angularImpulse); - vB += mB * P; - wB += iB * (b2Cross(m_rB, P) + m_angularImpulse); - } - else - { - m_linearImpulse.SetZero(); - m_angularImpulse = 0.0f; - } - - data.velocities[m_indexA].v = vA; - data.velocities[m_indexA].w = wA; - data.velocities[m_indexB].v = vB; - data.velocities[m_indexB].w = wB; -} - -void b2MotorJoint::SolveVelocityConstraints(const b2SolverData& data) -{ - b2Vec2 vA = data.velocities[m_indexA].v; - float32 wA = data.velocities[m_indexA].w; - b2Vec2 vB = data.velocities[m_indexB].v; - float32 wB = data.velocities[m_indexB].w; - - float32 mA = m_invMassA, mB = m_invMassB; - float32 iA = m_invIA, iB = m_invIB; - - float32 h = data.step.dt; - float32 inv_h = data.step.inv_dt; - - // Solve angular friction - { - float32 Cdot = wB - wA + inv_h * m_correctionFactor * m_angularError; - float32 impulse = -m_angularMass * Cdot; - - float32 oldImpulse = m_angularImpulse; - float32 maxImpulse = h * m_maxTorque; - m_angularImpulse = b2Clamp(m_angularImpulse + impulse, -maxImpulse, maxImpulse); - impulse = m_angularImpulse - oldImpulse; - - wA -= iA * impulse; - wB += iB * impulse; - } - - // Solve linear friction - { - b2Vec2 Cdot = vB + b2Cross(wB, m_rB) - vA - b2Cross(wA, m_rA) + inv_h * m_correctionFactor * m_linearError; - - b2Vec2 impulse = -b2Mul(m_linearMass, Cdot); - b2Vec2 oldImpulse = m_linearImpulse; - m_linearImpulse += impulse; - - float32 maxImpulse = h * m_maxForce; - - if (m_linearImpulse.LengthSquared() > maxImpulse * maxImpulse) - { - m_linearImpulse.Normalize(); - m_linearImpulse *= maxImpulse; - } - - impulse = m_linearImpulse - oldImpulse; - - vA -= mA * impulse; - wA -= iA * b2Cross(m_rA, impulse); - - vB += mB * impulse; - wB += iB * b2Cross(m_rB, impulse); - } - - data.velocities[m_indexA].v = vA; - data.velocities[m_indexA].w = wA; - data.velocities[m_indexB].v = vB; - data.velocities[m_indexB].w = wB; -} - -bool b2MotorJoint::SolvePositionConstraints(const b2SolverData& data) -{ - B2_NOT_USED(data); - - return true; -} - -b2Vec2 b2MotorJoint::GetAnchorA() const -{ - return m_bodyA->GetPosition(); -} - -b2Vec2 b2MotorJoint::GetAnchorB() const -{ - return m_bodyB->GetPosition(); -} - -b2Vec2 b2MotorJoint::GetReactionForce(float32 inv_dt) const -{ - return inv_dt * m_linearImpulse; -} - -float32 b2MotorJoint::GetReactionTorque(float32 inv_dt) const -{ - return inv_dt * m_angularImpulse; -} - -void b2MotorJoint::SetMaxForce(float32 force) -{ - b2Assert(b2IsValid(force) && force >= 0.0f); - m_maxForce = force; -} - -float32 b2MotorJoint::GetMaxForce() const -{ - return m_maxForce; -} - -void b2MotorJoint::SetMaxTorque(float32 torque) -{ - b2Assert(b2IsValid(torque) && torque >= 0.0f); - m_maxTorque = torque; -} - -float32 b2MotorJoint::GetMaxTorque() const -{ - return m_maxTorque; -} - -void b2MotorJoint::SetCorrectionFactor(float32 factor) -{ - b2Assert(b2IsValid(factor) && 0.0f <= factor && factor <= 1.0f); - m_correctionFactor = factor; -} - -float32 b2MotorJoint::GetCorrectionFactor() const -{ - return m_correctionFactor; -} - -void b2MotorJoint::SetLinearOffset(const b2Vec2& linearOffset) -{ - if (linearOffset.x != m_linearOffset.x || linearOffset.y != m_linearOffset.y) - { - m_bodyA->SetAwake(true); - m_bodyB->SetAwake(true); - m_linearOffset = linearOffset; - } -} - -const b2Vec2& b2MotorJoint::GetLinearOffset() const -{ - return m_linearOffset; -} - -void b2MotorJoint::SetAngularOffset(float32 angularOffset) -{ - if (angularOffset != m_angularOffset) - { - m_bodyA->SetAwake(true); - m_bodyB->SetAwake(true); - m_angularOffset = angularOffset; - } -} - -float32 b2MotorJoint::GetAngularOffset() const -{ - return m_angularOffset; -} - -void b2MotorJoint::Dump() -{ - int32 indexA = m_bodyA->m_islandIndex; - int32 indexB = m_bodyB->m_islandIndex; - - b2Log(" b2MotorJointDef jd;\n"); - b2Log(" jd.bodyA = bodies[%d];\n", indexA); - b2Log(" jd.bodyB = bodies[%d];\n", indexB); - b2Log(" jd.collideConnected = bool(%d);\n", m_collideConnected); - b2Log(" jd.linearOffset.Set(%.15lef, %.15lef);\n", m_linearOffset.x, m_linearOffset.y); - b2Log(" jd.angularOffset = %.15lef;\n", m_angularOffset); - b2Log(" jd.maxForce = %.15lef;\n", m_maxForce); - b2Log(" jd.maxTorque = %.15lef;\n", m_maxTorque); - b2Log(" jd.correctionFactor = %.15lef;\n", m_correctionFactor); - b2Log(" joints[%d] = m_world->CreateJoint(&jd);\n", m_index); -} diff --git a/Box2D/Dynamics/Joints/b2MotorJoint.h b/Box2D/Dynamics/Joints/b2MotorJoint.h deleted file mode 100644 index 0b76d6fe7e..0000000000 --- a/Box2D/Dynamics/Joints/b2MotorJoint.h +++ /dev/null @@ -1,133 +0,0 @@ -/* -* Copyright (c) 2006-2012 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_MOTOR_JOINT_H -#define B2_MOTOR_JOINT_H - -#include - -/// Motor joint definition. -struct b2MotorJointDef : public b2JointDef -{ - b2MotorJointDef() - { - type = e_motorJoint; - linearOffset.SetZero(); - angularOffset = 0.0f; - maxForce = 1.0f; - maxTorque = 1.0f; - correctionFactor = 0.3f; - } - - /// Initialize the bodies and offsets using the current transforms. - void Initialize(b2Body* bodyA, b2Body* bodyB); - - /// Position of bodyB minus the position of bodyA, in bodyA's frame, in meters. - b2Vec2 linearOffset; - - /// The bodyB angle minus bodyA angle in radians. - float32 angularOffset; - - /// The maximum motor force in N. - float32 maxForce; - - /// The maximum motor torque in N-m. - float32 maxTorque; - - /// Position correction factor in the range [0,1]. - float32 correctionFactor; -}; - -/// A motor joint is used to control the relative motion -/// between two bodies. A typical usage is to control the movement -/// of a dynamic body with respect to the ground. -class b2MotorJoint : public b2Joint -{ -public: - b2Vec2 GetAnchorA() const; - b2Vec2 GetAnchorB() const; - - b2Vec2 GetReactionForce(float32 inv_dt) const; - float32 GetReactionTorque(float32 inv_dt) const; - - /// Set/get the target linear offset, in frame A, in meters. - void SetLinearOffset(const b2Vec2& linearOffset); - const b2Vec2& GetLinearOffset() const; - - /// Set/get the target angular offset, in radians. - void SetAngularOffset(float32 angularOffset); - float32 GetAngularOffset() const; - - /// Set the maximum friction force in N. - void SetMaxForce(float32 force); - - /// Get the maximum friction force in N. - float32 GetMaxForce() const; - - /// Set the maximum friction torque in N*m. - void SetMaxTorque(float32 torque); - - /// Get the maximum friction torque in N*m. - float32 GetMaxTorque() const; - - /// Set the position correction factor in the range [0,1]. - void SetCorrectionFactor(float32 factor); - - /// Get the position correction factor in the range [0,1]. - float32 GetCorrectionFactor() const; - - /// Dump to b2Log - void Dump(); - -protected: - - friend class b2Joint; - - b2MotorJoint(const b2MotorJointDef* def); - - void InitVelocityConstraints(const b2SolverData& data); - void SolveVelocityConstraints(const b2SolverData& data); - bool SolvePositionConstraints(const b2SolverData& data); - - // Solver shared - b2Vec2 m_linearOffset; - float32 m_angularOffset; - b2Vec2 m_linearImpulse; - float32 m_angularImpulse; - float32 m_maxForce; - float32 m_maxTorque; - float32 m_correctionFactor; - - // Solver temp - int32 m_indexA; - int32 m_indexB; - b2Vec2 m_rA; - b2Vec2 m_rB; - b2Vec2 m_localCenterA; - b2Vec2 m_localCenterB; - b2Vec2 m_linearError; - float32 m_angularError; - float32 m_invMassA; - float32 m_invMassB; - float32 m_invIA; - float32 m_invIB; - b2Mat22 m_linearMass; - float32 m_angularMass; -}; - -#endif diff --git a/Box2D/Dynamics/Joints/b2MouseJoint.cpp b/Box2D/Dynamics/Joints/b2MouseJoint.cpp deleted file mode 100644 index ea96edde9b..0000000000 --- a/Box2D/Dynamics/Joints/b2MouseJoint.cpp +++ /dev/null @@ -1,222 +0,0 @@ -/* -* Copyright (c) 2006-2007 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 -#include -#include - -// p = attached point, m = mouse point -// C = p - m -// Cdot = v -// = v + cross(w, r) -// J = [I r_skew] -// Identity used: -// w k % (rx i + ry j) = w * (-ry i + rx j) - -b2MouseJoint::b2MouseJoint(const b2MouseJointDef* def) -: b2Joint(def) -{ - b2Assert(def->target.IsValid()); - b2Assert(b2IsValid(def->maxForce) && def->maxForce >= 0.0f); - b2Assert(b2IsValid(def->frequencyHz) && def->frequencyHz >= 0.0f); - b2Assert(b2IsValid(def->dampingRatio) && def->dampingRatio >= 0.0f); - - m_targetA = def->target; - m_localAnchorB = b2MulT(m_bodyB->GetTransform(), m_targetA); - - m_maxForce = def->maxForce; - m_impulse.SetZero(); - - m_frequencyHz = def->frequencyHz; - m_dampingRatio = def->dampingRatio; - - m_beta = 0.0f; - m_gamma = 0.0f; -} - -void b2MouseJoint::SetTarget(const b2Vec2& target) -{ - if (m_bodyB->IsAwake() == false) - { - m_bodyB->SetAwake(true); - } - m_targetA = target; -} - -const b2Vec2& b2MouseJoint::GetTarget() const -{ - return m_targetA; -} - -void b2MouseJoint::SetMaxForce(float32 force) -{ - m_maxForce = force; -} - -float32 b2MouseJoint::GetMaxForce() const -{ - return m_maxForce; -} - -void b2MouseJoint::SetFrequency(float32 hz) -{ - m_frequencyHz = hz; -} - -float32 b2MouseJoint::GetFrequency() const -{ - return m_frequencyHz; -} - -void b2MouseJoint::SetDampingRatio(float32 ratio) -{ - m_dampingRatio = ratio; -} - -float32 b2MouseJoint::GetDampingRatio() const -{ - return m_dampingRatio; -} - -void b2MouseJoint::InitVelocityConstraints(const b2SolverData& data) -{ - m_indexB = m_bodyB->m_islandIndex; - m_localCenterB = m_bodyB->m_sweep.localCenter; - m_invMassB = m_bodyB->m_invMass; - m_invIB = m_bodyB->m_invI; - - b2Vec2 cB = data.positions[m_indexB].c; - float32 aB = data.positions[m_indexB].a; - b2Vec2 vB = data.velocities[m_indexB].v; - float32 wB = data.velocities[m_indexB].w; - - b2Rot qB(aB); - - float32 mass = m_bodyB->GetMass(); - - // Frequency - float32 omega = 2.0f * b2_pi * m_frequencyHz; - - // Damping coefficient - float32 d = 2.0f * mass * m_dampingRatio * omega; - - // Spring stiffness - float32 k = mass * (omega * omega); - - // magic formulas - // gamma has units of inverse mass. - // beta has units of inverse time. - float32 h = data.step.dt; - b2Assert(d + h * k > b2_epsilon); - m_gamma = h * (d + h * k); - if (m_gamma != 0.0f) - { - m_gamma = 1.0f / m_gamma; - } - m_beta = h * k * m_gamma; - - // Compute the effective mass matrix. - m_rB = b2Mul(qB, m_localAnchorB - m_localCenterB); - - // K = [(1/m1 + 1/m2) * eye(2) - skew(r1) * invI1 * skew(r1) - skew(r2) * invI2 * skew(r2)] - // = [1/m1+1/m2 0 ] + invI1 * [r1.y*r1.y -r1.x*r1.y] + invI2 * [r1.y*r1.y -r1.x*r1.y] - // [ 0 1/m1+1/m2] [-r1.x*r1.y r1.x*r1.x] [-r1.x*r1.y r1.x*r1.x] - b2Mat22 K; - K.ex.x = m_invMassB + m_invIB * m_rB.y * m_rB.y + m_gamma; - K.ex.y = -m_invIB * m_rB.x * m_rB.y; - K.ey.x = K.ex.y; - K.ey.y = m_invMassB + m_invIB * m_rB.x * m_rB.x + m_gamma; - - m_mass = K.GetInverse(); - - m_C = cB + m_rB - m_targetA; - m_C *= m_beta; - - // Cheat with some damping - wB *= 0.98f; - - if (data.step.warmStarting) - { - m_impulse *= data.step.dtRatio; - vB += m_invMassB * m_impulse; - wB += m_invIB * b2Cross(m_rB, m_impulse); - } - else - { - m_impulse.SetZero(); - } - - data.velocities[m_indexB].v = vB; - data.velocities[m_indexB].w = wB; -} - -void b2MouseJoint::SolveVelocityConstraints(const b2SolverData& data) -{ - b2Vec2 vB = data.velocities[m_indexB].v; - float32 wB = data.velocities[m_indexB].w; - - // Cdot = v + cross(w, r) - b2Vec2 Cdot = vB + b2Cross(wB, m_rB); - b2Vec2 impulse = b2Mul(m_mass, -(Cdot + m_C + m_gamma * m_impulse)); - - b2Vec2 oldImpulse = m_impulse; - m_impulse += impulse; - float32 maxImpulse = data.step.dt * m_maxForce; - if (m_impulse.LengthSquared() > maxImpulse * maxImpulse) - { - m_impulse *= maxImpulse / m_impulse.Length(); - } - impulse = m_impulse - oldImpulse; - - vB += m_invMassB * impulse; - wB += m_invIB * b2Cross(m_rB, impulse); - - data.velocities[m_indexB].v = vB; - data.velocities[m_indexB].w = wB; -} - -bool b2MouseJoint::SolvePositionConstraints(const b2SolverData& data) -{ - B2_NOT_USED(data); - return true; -} - -b2Vec2 b2MouseJoint::GetAnchorA() const -{ - return m_targetA; -} - -b2Vec2 b2MouseJoint::GetAnchorB() const -{ - return m_bodyB->GetWorldPoint(m_localAnchorB); -} - -b2Vec2 b2MouseJoint::GetReactionForce(float32 inv_dt) const -{ - return inv_dt * m_impulse; -} - -float32 b2MouseJoint::GetReactionTorque(float32 inv_dt) const -{ - return inv_dt * 0.0f; -} - -void b2MouseJoint::ShiftOrigin(const b2Vec2& newOrigin) -{ - m_targetA -= newOrigin; -} diff --git a/Box2D/Dynamics/Joints/b2MouseJoint.h b/Box2D/Dynamics/Joints/b2MouseJoint.h deleted file mode 100644 index 8b50a0e149..0000000000 --- a/Box2D/Dynamics/Joints/b2MouseJoint.h +++ /dev/null @@ -1,129 +0,0 @@ -/* -* Copyright (c) 2006-2007 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_MOUSE_JOINT_H -#define B2_MOUSE_JOINT_H - -#include - -/// Mouse joint definition. This requires a world target point, -/// tuning parameters, and the time step. -struct b2MouseJointDef : public b2JointDef -{ - b2MouseJointDef() - { - type = e_mouseJoint; - target.Set(0.0f, 0.0f); - maxForce = 0.0f; - frequencyHz = 5.0f; - dampingRatio = 0.7f; - } - - /// The initial world target point. This is assumed - /// to coincide with the body anchor initially. - b2Vec2 target; - - /// The maximum constraint force that can be exerted - /// to move the candidate body. Usually you will express - /// as some multiple of the weight (multiplier * mass * gravity). - float32 maxForce; - - /// The response speed. - float32 frequencyHz; - - /// The damping ratio. 0 = no damping, 1 = critical damping. - float32 dampingRatio; -}; - -/// A mouse joint is used to make a point on a body track a -/// specified world point. This a soft constraint with a maximum -/// force. This allows the constraint to stretch and without -/// applying huge forces. -/// NOTE: this joint is not documented in the manual because it was -/// developed to be used in the testbed. If you want to learn how to -/// use the mouse joint, look at the testbed. -class b2MouseJoint : public b2Joint -{ -public: - - /// Implements b2Joint. - b2Vec2 GetAnchorA() const; - - /// Implements b2Joint. - b2Vec2 GetAnchorB() const; - - /// Implements b2Joint. - b2Vec2 GetReactionForce(float32 inv_dt) const; - - /// Implements b2Joint. - float32 GetReactionTorque(float32 inv_dt) const; - - /// Use this to update the target point. - void SetTarget(const b2Vec2& target); - const b2Vec2& GetTarget() const; - - /// Set/get the maximum force in Newtons. - void SetMaxForce(float32 force); - float32 GetMaxForce() const; - - /// Set/get the frequency in Hertz. - void SetFrequency(float32 hz); - float32 GetFrequency() const; - - /// Set/get the damping ratio (dimensionless). - void SetDampingRatio(float32 ratio); - float32 GetDampingRatio() const; - - /// The mouse joint does not support dumping. - void Dump() { b2Log("Mouse joint dumping is not supported.\n"); } - - /// Implement b2Joint::ShiftOrigin - void ShiftOrigin(const b2Vec2& newOrigin); - -protected: - friend class b2Joint; - - b2MouseJoint(const b2MouseJointDef* def); - - void InitVelocityConstraints(const b2SolverData& data); - void SolveVelocityConstraints(const b2SolverData& data); - bool SolvePositionConstraints(const b2SolverData& data); - - b2Vec2 m_localAnchorB; - b2Vec2 m_targetA; - float32 m_frequencyHz; - float32 m_dampingRatio; - float32 m_beta; - - // Solver shared - b2Vec2 m_impulse; - float32 m_maxForce; - float32 m_gamma; - - // Solver temp - int32 m_indexA; - int32 m_indexB; - b2Vec2 m_rB; - b2Vec2 m_localCenterB; - float32 m_invMassB; - float32 m_invIB; - b2Mat22 m_mass; - b2Vec2 m_C; -}; - -#endif diff --git a/Box2D/Dynamics/Joints/b2PrismaticJoint.cpp b/Box2D/Dynamics/Joints/b2PrismaticJoint.cpp deleted file mode 100644 index 26f5396816..0000000000 --- a/Box2D/Dynamics/Joints/b2PrismaticJoint.cpp +++ /dev/null @@ -1,626 +0,0 @@ -/* -* Copyright (c) 2006-2011 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 -#include -#include - -// Linear constraint (point-to-line) -// d = p2 - p1 = x2 + r2 - x1 - r1 -// C = dot(perp, d) -// Cdot = dot(d, cross(w1, perp)) + dot(perp, v2 + cross(w2, r2) - v1 - cross(w1, r1)) -// = -dot(perp, v1) - dot(cross(d + r1, perp), w1) + dot(perp, v2) + dot(cross(r2, perp), v2) -// J = [-perp, -cross(d + r1, perp), perp, cross(r2,perp)] -// -// Angular constraint -// C = a2 - a1 + a_initial -// Cdot = w2 - w1 -// J = [0 0 -1 0 0 1] -// -// K = J * invM * JT -// -// J = [-a -s1 a s2] -// [0 -1 0 1] -// a = perp -// s1 = cross(d + r1, a) = cross(p2 - x1, a) -// s2 = cross(r2, a) = cross(p2 - x2, a) - - -// Motor/Limit linear constraint -// C = dot(ax1, d) -// Cdot = = -dot(ax1, v1) - dot(cross(d + r1, ax1), w1) + dot(ax1, v2) + dot(cross(r2, ax1), v2) -// J = [-ax1 -cross(d+r1,ax1) ax1 cross(r2,ax1)] - -// Block Solver -// We develop a block solver that includes the joint limit. This makes the limit stiff (inelastic) even -// when the mass has poor distribution (leading to large torques about the joint anchor points). -// -// The Jacobian has 3 rows: -// J = [-uT -s1 uT s2] // linear -// [0 -1 0 1] // angular -// [-vT -a1 vT a2] // limit -// -// u = perp -// v = axis -// s1 = cross(d + r1, u), s2 = cross(r2, u) -// a1 = cross(d + r1, v), a2 = cross(r2, v) - -// M * (v2 - v1) = JT * df -// J * v2 = bias -// -// v2 = v1 + invM * JT * df -// J * (v1 + invM * JT * df) = bias -// K * df = bias - J * v1 = -Cdot -// K = J * invM * JT -// Cdot = J * v1 - bias -// -// Now solve for f2. -// df = f2 - f1 -// K * (f2 - f1) = -Cdot -// f2 = invK * (-Cdot) + f1 -// -// Clamp accumulated limit impulse. -// lower: f2(3) = max(f2(3), 0) -// upper: f2(3) = min(f2(3), 0) -// -// Solve for correct f2(1:2) -// K(1:2, 1:2) * f2(1:2) = -Cdot(1:2) - K(1:2,3) * f2(3) + K(1:2,1:3) * f1 -// = -Cdot(1:2) - K(1:2,3) * f2(3) + K(1:2,1:2) * f1(1:2) + K(1:2,3) * f1(3) -// K(1:2, 1:2) * f2(1:2) = -Cdot(1:2) - K(1:2,3) * (f2(3) - f1(3)) + K(1:2,1:2) * f1(1:2) -// f2(1:2) = invK(1:2,1:2) * (-Cdot(1:2) - K(1:2,3) * (f2(3) - f1(3))) + f1(1:2) -// -// Now compute impulse to be applied: -// df = f2 - f1 - -void b2PrismaticJointDef::Initialize(b2Body* bA, b2Body* bB, const b2Vec2& anchor, const b2Vec2& axis) -{ - bodyA = bA; - bodyB = bB; - localAnchorA = bodyA->GetLocalPoint(anchor); - localAnchorB = bodyB->GetLocalPoint(anchor); - localAxisA = bodyA->GetLocalVector(axis); - referenceAngle = bodyB->GetAngle() - bodyA->GetAngle(); -} - -b2PrismaticJoint::b2PrismaticJoint(const b2PrismaticJointDef* def) -: b2Joint(def) -{ - m_localAnchorA = def->localAnchorA; - m_localAnchorB = def->localAnchorB; - m_localXAxisA = def->localAxisA; - m_localXAxisA.Normalize(); - m_localYAxisA = b2Cross(1.0f, m_localXAxisA); - m_referenceAngle = def->referenceAngle; - - m_impulse.SetZero(); - m_motorMass = 0.0f; - m_motorImpulse = 0.0f; - - m_lowerTranslation = def->lowerTranslation; - m_upperTranslation = def->upperTranslation; - m_maxMotorForce = def->maxMotorForce; - m_motorSpeed = def->motorSpeed; - m_enableLimit = def->enableLimit; - m_enableMotor = def->enableMotor; - m_limitState = e_inactiveLimit; - - m_axis.SetZero(); - m_perp.SetZero(); -} - -void b2PrismaticJoint::InitVelocityConstraints(const b2SolverData& data) -{ - m_indexA = m_bodyA->m_islandIndex; - m_indexB = m_bodyB->m_islandIndex; - m_localCenterA = m_bodyA->m_sweep.localCenter; - m_localCenterB = m_bodyB->m_sweep.localCenter; - m_invMassA = m_bodyA->m_invMass; - m_invMassB = m_bodyB->m_invMass; - m_invIA = m_bodyA->m_invI; - m_invIB = m_bodyB->m_invI; - - b2Vec2 cA = data.positions[m_indexA].c; - float32 aA = data.positions[m_indexA].a; - b2Vec2 vA = data.velocities[m_indexA].v; - float32 wA = data.velocities[m_indexA].w; - - b2Vec2 cB = data.positions[m_indexB].c; - float32 aB = data.positions[m_indexB].a; - b2Vec2 vB = data.velocities[m_indexB].v; - float32 wB = data.velocities[m_indexB].w; - - b2Rot qA(aA), qB(aB); - - // Compute the effective masses. - b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_localCenterA); - b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_localCenterB); - b2Vec2 d = (cB - cA) + rB - rA; - - float32 mA = m_invMassA, mB = m_invMassB; - float32 iA = m_invIA, iB = m_invIB; - - // Compute motor Jacobian and effective mass. - { - m_axis = b2Mul(qA, m_localXAxisA); - m_a1 = b2Cross(d + rA, m_axis); - m_a2 = b2Cross(rB, m_axis); - - m_motorMass = mA + mB + iA * m_a1 * m_a1 + iB * m_a2 * m_a2; - if (m_motorMass > 0.0f) - { - m_motorMass = 1.0f / m_motorMass; - } - } - - // Prismatic constraint. - { - m_perp = b2Mul(qA, m_localYAxisA); - - m_s1 = b2Cross(d + rA, m_perp); - m_s2 = b2Cross(rB, m_perp); - - float32 k11 = mA + mB + iA * m_s1 * m_s1 + iB * m_s2 * m_s2; - float32 k12 = iA * m_s1 + iB * m_s2; - float32 k13 = iA * m_s1 * m_a1 + iB * m_s2 * m_a2; - float32 k22 = iA + iB; - if (k22 == 0.0f) - { - // For bodies with fixed rotation. - k22 = 1.0f; - } - float32 k23 = iA * m_a1 + iB * m_a2; - float32 k33 = mA + mB + iA * m_a1 * m_a1 + iB * m_a2 * m_a2; - - m_K.ex.Set(k11, k12, k13); - m_K.ey.Set(k12, k22, k23); - m_K.ez.Set(k13, k23, k33); - } - - // Compute motor and limit terms. - if (m_enableLimit) - { - float32 jointTranslation = b2Dot(m_axis, d); - if (b2Abs(m_upperTranslation - m_lowerTranslation) < 2.0f * b2_linearSlop) - { - m_limitState = e_equalLimits; - } - else if (jointTranslation <= m_lowerTranslation) - { - if (m_limitState != e_atLowerLimit) - { - m_limitState = e_atLowerLimit; - m_impulse.z = 0.0f; - } - } - else if (jointTranslation >= m_upperTranslation) - { - if (m_limitState != e_atUpperLimit) - { - m_limitState = e_atUpperLimit; - m_impulse.z = 0.0f; - } - } - else - { - m_limitState = e_inactiveLimit; - m_impulse.z = 0.0f; - } - } - else - { - m_limitState = e_inactiveLimit; - m_impulse.z = 0.0f; - } - - if (m_enableMotor == false) - { - m_motorImpulse = 0.0f; - } - - if (data.step.warmStarting) - { - // Account for variable time step. - m_impulse *= data.step.dtRatio; - m_motorImpulse *= data.step.dtRatio; - - b2Vec2 P = m_impulse.x * m_perp + (m_motorImpulse + m_impulse.z) * m_axis; - float32 LA = m_impulse.x * m_s1 + m_impulse.y + (m_motorImpulse + m_impulse.z) * m_a1; - float32 LB = m_impulse.x * m_s2 + m_impulse.y + (m_motorImpulse + m_impulse.z) * m_a2; - - vA -= mA * P; - wA -= iA * LA; - - vB += mB * P; - wB += iB * LB; - } - else - { - m_impulse.SetZero(); - m_motorImpulse = 0.0f; - } - - data.velocities[m_indexA].v = vA; - data.velocities[m_indexA].w = wA; - data.velocities[m_indexB].v = vB; - data.velocities[m_indexB].w = wB; -} - -void b2PrismaticJoint::SolveVelocityConstraints(const b2SolverData& data) -{ - b2Vec2 vA = data.velocities[m_indexA].v; - float32 wA = data.velocities[m_indexA].w; - b2Vec2 vB = data.velocities[m_indexB].v; - float32 wB = data.velocities[m_indexB].w; - - float32 mA = m_invMassA, mB = m_invMassB; - float32 iA = m_invIA, iB = m_invIB; - - // Solve linear motor constraint. - if (m_enableMotor && m_limitState != e_equalLimits) - { - float32 Cdot = b2Dot(m_axis, vB - vA) + m_a2 * wB - m_a1 * wA; - float32 impulse = m_motorMass * (m_motorSpeed - Cdot); - float32 oldImpulse = m_motorImpulse; - float32 maxImpulse = data.step.dt * m_maxMotorForce; - m_motorImpulse = b2Clamp(m_motorImpulse + impulse, -maxImpulse, maxImpulse); - impulse = m_motorImpulse - oldImpulse; - - b2Vec2 P = impulse * m_axis; - float32 LA = impulse * m_a1; - float32 LB = impulse * m_a2; - - vA -= mA * P; - wA -= iA * LA; - - vB += mB * P; - wB += iB * LB; - } - - b2Vec2 Cdot1; - Cdot1.x = b2Dot(m_perp, vB - vA) + m_s2 * wB - m_s1 * wA; - Cdot1.y = wB - wA; - - if (m_enableLimit && m_limitState != e_inactiveLimit) - { - // Solve prismatic and limit constraint in block form. - float32 Cdot2; - Cdot2 = b2Dot(m_axis, vB - vA) + m_a2 * wB - m_a1 * wA; - b2Vec3 Cdot(Cdot1.x, Cdot1.y, Cdot2); - - b2Vec3 f1 = m_impulse; - b2Vec3 df = m_K.Solve33(-Cdot); - m_impulse += df; - - if (m_limitState == e_atLowerLimit) - { - m_impulse.z = b2Max(m_impulse.z, 0.0f); - } - else if (m_limitState == e_atUpperLimit) - { - m_impulse.z = b2Min(m_impulse.z, 0.0f); - } - - // f2(1:2) = invK(1:2,1:2) * (-Cdot(1:2) - K(1:2,3) * (f2(3) - f1(3))) + f1(1:2) - b2Vec2 b = -Cdot1 - (m_impulse.z - f1.z) * b2Vec2(m_K.ez.x, m_K.ez.y); - b2Vec2 f2r = m_K.Solve22(b) + b2Vec2(f1.x, f1.y); - m_impulse.x = f2r.x; - m_impulse.y = f2r.y; - - df = m_impulse - f1; - - b2Vec2 P = df.x * m_perp + df.z * m_axis; - float32 LA = df.x * m_s1 + df.y + df.z * m_a1; - float32 LB = df.x * m_s2 + df.y + df.z * m_a2; - - vA -= mA * P; - wA -= iA * LA; - - vB += mB * P; - wB += iB * LB; - } - else - { - // Limit is inactive, just solve the prismatic constraint in block form. - b2Vec2 df = m_K.Solve22(-Cdot1); - m_impulse.x += df.x; - m_impulse.y += df.y; - - b2Vec2 P = df.x * m_perp; - float32 LA = df.x * m_s1 + df.y; - float32 LB = df.x * m_s2 + df.y; - - vA -= mA * P; - wA -= iA * LA; - - vB += mB * P; - wB += iB * LB; - } - - data.velocities[m_indexA].v = vA; - data.velocities[m_indexA].w = wA; - data.velocities[m_indexB].v = vB; - data.velocities[m_indexB].w = wB; -} - -bool b2PrismaticJoint::SolvePositionConstraints(const b2SolverData& data) -{ - b2Vec2 cA = data.positions[m_indexA].c; - float32 aA = data.positions[m_indexA].a; - b2Vec2 cB = data.positions[m_indexB].c; - float32 aB = data.positions[m_indexB].a; - - b2Rot qA(aA), qB(aB); - - float32 mA = m_invMassA, mB = m_invMassB; - float32 iA = m_invIA, iB = m_invIB; - - // Compute fresh Jacobians - b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_localCenterA); - b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_localCenterB); - b2Vec2 d = cB + rB - cA - rA; - - b2Vec2 axis = b2Mul(qA, m_localXAxisA); - float32 a1 = b2Cross(d + rA, axis); - float32 a2 = b2Cross(rB, axis); - b2Vec2 perp = b2Mul(qA, m_localYAxisA); - - float32 s1 = b2Cross(d + rA, perp); - float32 s2 = b2Cross(rB, perp); - - b2Vec3 impulse; - b2Vec2 C1; - C1.x = b2Dot(perp, d); - C1.y = aB - aA - m_referenceAngle; - - float32 linearError = b2Abs(C1.x); - float32 angularError = b2Abs(C1.y); - - bool active = false; - float32 C2 = 0.0f; - if (m_enableLimit) - { - float32 translation = b2Dot(axis, d); - if (b2Abs(m_upperTranslation - m_lowerTranslation) < 2.0f * b2_linearSlop) - { - // Prevent large angular corrections - C2 = b2Clamp(translation, -b2_maxLinearCorrection, b2_maxLinearCorrection); - linearError = b2Max(linearError, b2Abs(translation)); - active = true; - } - else if (translation <= m_lowerTranslation) - { - // Prevent large linear corrections and allow some slop. - C2 = b2Clamp(translation - m_lowerTranslation + b2_linearSlop, -b2_maxLinearCorrection, 0.0f); - linearError = b2Max(linearError, m_lowerTranslation - translation); - active = true; - } - else if (translation >= m_upperTranslation) - { - // Prevent large linear corrections and allow some slop. - C2 = b2Clamp(translation - m_upperTranslation - b2_linearSlop, 0.0f, b2_maxLinearCorrection); - linearError = b2Max(linearError, translation - m_upperTranslation); - active = true; - } - } - - if (active) - { - float32 k11 = mA + mB + iA * s1 * s1 + iB * s2 * s2; - float32 k12 = iA * s1 + iB * s2; - float32 k13 = iA * s1 * a1 + iB * s2 * a2; - float32 k22 = iA + iB; - if (k22 == 0.0f) - { - // For fixed rotation - k22 = 1.0f; - } - float32 k23 = iA * a1 + iB * a2; - float32 k33 = mA + mB + iA * a1 * a1 + iB * a2 * a2; - - b2Mat33 K; - K.ex.Set(k11, k12, k13); - K.ey.Set(k12, k22, k23); - K.ez.Set(k13, k23, k33); - - b2Vec3 C; - C.x = C1.x; - C.y = C1.y; - C.z = C2; - - impulse = K.Solve33(-C); - } - else - { - float32 k11 = mA + mB + iA * s1 * s1 + iB * s2 * s2; - float32 k12 = iA * s1 + iB * s2; - float32 k22 = iA + iB; - if (k22 == 0.0f) - { - k22 = 1.0f; - } - - b2Mat22 K; - K.ex.Set(k11, k12); - K.ey.Set(k12, k22); - - b2Vec2 impulse1 = K.Solve(-C1); - impulse.x = impulse1.x; - impulse.y = impulse1.y; - impulse.z = 0.0f; - } - - b2Vec2 P = impulse.x * perp + impulse.z * axis; - float32 LA = impulse.x * s1 + impulse.y + impulse.z * a1; - float32 LB = impulse.x * s2 + impulse.y + impulse.z * a2; - - cA -= mA * P; - aA -= iA * LA; - cB += mB * P; - aB += iB * LB; - - data.positions[m_indexA].c = cA; - data.positions[m_indexA].a = aA; - data.positions[m_indexB].c = cB; - data.positions[m_indexB].a = aB; - - return linearError <= b2_linearSlop && angularError <= b2_angularSlop; -} - -b2Vec2 b2PrismaticJoint::GetAnchorA() const -{ - return m_bodyA->GetWorldPoint(m_localAnchorA); -} - -b2Vec2 b2PrismaticJoint::GetAnchorB() const -{ - return m_bodyB->GetWorldPoint(m_localAnchorB); -} - -b2Vec2 b2PrismaticJoint::GetReactionForce(float32 inv_dt) const -{ - return inv_dt * (m_impulse.x * m_perp + (m_motorImpulse + m_impulse.z) * m_axis); -} - -float32 b2PrismaticJoint::GetReactionTorque(float32 inv_dt) const -{ - return inv_dt * m_impulse.y; -} - -float32 b2PrismaticJoint::GetJointTranslation() const -{ - b2Vec2 pA = m_bodyA->GetWorldPoint(m_localAnchorA); - b2Vec2 pB = m_bodyB->GetWorldPoint(m_localAnchorB); - b2Vec2 d = pB - pA; - b2Vec2 axis = m_bodyA->GetWorldVector(m_localXAxisA); - - float32 translation = b2Dot(d, axis); - return translation; -} - -float32 b2PrismaticJoint::GetJointSpeed() const -{ - b2Body* bA = m_bodyA; - b2Body* bB = m_bodyB; - - b2Vec2 rA = b2Mul(bA->m_xf.q, m_localAnchorA - bA->m_sweep.localCenter); - b2Vec2 rB = b2Mul(bB->m_xf.q, m_localAnchorB - bB->m_sweep.localCenter); - b2Vec2 p1 = bA->m_sweep.c + rA; - b2Vec2 p2 = bB->m_sweep.c + rB; - b2Vec2 d = p2 - p1; - b2Vec2 axis = b2Mul(bA->m_xf.q, m_localXAxisA); - - b2Vec2 vA = bA->m_linearVelocity; - b2Vec2 vB = bB->m_linearVelocity; - float32 wA = bA->m_angularVelocity; - float32 wB = bB->m_angularVelocity; - - float32 speed = b2Dot(d, b2Cross(wA, axis)) + b2Dot(axis, vB + b2Cross(wB, rB) - vA - b2Cross(wA, rA)); - return speed; -} - -bool b2PrismaticJoint::IsLimitEnabled() const -{ - return m_enableLimit; -} - -void b2PrismaticJoint::EnableLimit(bool flag) -{ - if (flag != m_enableLimit) - { - m_bodyA->SetAwake(true); - m_bodyB->SetAwake(true); - m_enableLimit = flag; - m_impulse.z = 0.0f; - } -} - -float32 b2PrismaticJoint::GetLowerLimit() const -{ - return m_lowerTranslation; -} - -float32 b2PrismaticJoint::GetUpperLimit() const -{ - return m_upperTranslation; -} - -void b2PrismaticJoint::SetLimits(float32 lower, float32 upper) -{ - b2Assert(lower <= upper); - if (lower != m_lowerTranslation || upper != m_upperTranslation) - { - m_bodyA->SetAwake(true); - m_bodyB->SetAwake(true); - m_lowerTranslation = lower; - m_upperTranslation = upper; - m_impulse.z = 0.0f; - } -} - -bool b2PrismaticJoint::IsMotorEnabled() const -{ - return m_enableMotor; -} - -void b2PrismaticJoint::EnableMotor(bool flag) -{ - m_bodyA->SetAwake(true); - m_bodyB->SetAwake(true); - m_enableMotor = flag; -} - -void b2PrismaticJoint::SetMotorSpeed(float32 speed) -{ - m_bodyA->SetAwake(true); - m_bodyB->SetAwake(true); - m_motorSpeed = speed; -} - -void b2PrismaticJoint::SetMaxMotorForce(float32 force) -{ - m_bodyA->SetAwake(true); - m_bodyB->SetAwake(true); - m_maxMotorForce = force; -} - -float32 b2PrismaticJoint::GetMotorForce(float32 inv_dt) const -{ - return inv_dt * m_motorImpulse; -} - -void b2PrismaticJoint::Dump() -{ - int32 indexA = m_bodyA->m_islandIndex; - int32 indexB = m_bodyB->m_islandIndex; - - b2Log(" b2PrismaticJointDef jd;\n"); - b2Log(" jd.bodyA = bodies[%d];\n", indexA); - b2Log(" jd.bodyB = bodies[%d];\n", indexB); - b2Log(" jd.collideConnected = bool(%d);\n", m_collideConnected); - b2Log(" jd.localAnchorA.Set(%.15lef, %.15lef);\n", m_localAnchorA.x, m_localAnchorA.y); - b2Log(" jd.localAnchorB.Set(%.15lef, %.15lef);\n", m_localAnchorB.x, m_localAnchorB.y); - b2Log(" jd.localAxisA.Set(%.15lef, %.15lef);\n", m_localXAxisA.x, m_localXAxisA.y); - b2Log(" jd.referenceAngle = %.15lef;\n", m_referenceAngle); - b2Log(" jd.enableLimit = bool(%d);\n", m_enableLimit); - b2Log(" jd.lowerTranslation = %.15lef;\n", m_lowerTranslation); - b2Log(" jd.upperTranslation = %.15lef;\n", m_upperTranslation); - b2Log(" jd.enableMotor = bool(%d);\n", m_enableMotor); - b2Log(" jd.motorSpeed = %.15lef;\n", m_motorSpeed); - b2Log(" jd.maxMotorForce = %.15lef;\n", m_maxMotorForce); - b2Log(" joints[%d] = m_world->CreateJoint(&jd);\n", m_index); -} diff --git a/Box2D/Dynamics/Joints/b2PrismaticJoint.h b/Box2D/Dynamics/Joints/b2PrismaticJoint.h deleted file mode 100644 index 6ef5d3ec04..0000000000 --- a/Box2D/Dynamics/Joints/b2PrismaticJoint.h +++ /dev/null @@ -1,196 +0,0 @@ -/* -* Copyright (c) 2006-2011 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_PRISMATIC_JOINT_H -#define B2_PRISMATIC_JOINT_H - -#include - -/// Prismatic joint definition. This requires defining a line of -/// motion using an axis and an anchor point. The definition uses local -/// anchor points and a local axis so that the initial configuration -/// can violate the constraint slightly. The joint translation is zero -/// when the local anchor points coincide in world space. Using local -/// anchors and a local axis helps when saving and loading a game. -struct b2PrismaticJointDef : public b2JointDef -{ - b2PrismaticJointDef() - { - type = e_prismaticJoint; - localAnchorA.SetZero(); - localAnchorB.SetZero(); - localAxisA.Set(1.0f, 0.0f); - referenceAngle = 0.0f; - enableLimit = false; - lowerTranslation = 0.0f; - upperTranslation = 0.0f; - enableMotor = false; - maxMotorForce = 0.0f; - motorSpeed = 0.0f; - } - - /// Initialize the bodies, anchors, axis, and reference angle using the world - /// anchor and unit world axis. - void Initialize(b2Body* bodyA, b2Body* bodyB, const b2Vec2& anchor, const b2Vec2& axis); - - /// The local anchor point relative to bodyA's origin. - b2Vec2 localAnchorA; - - /// The local anchor point relative to bodyB's origin. - b2Vec2 localAnchorB; - - /// The local translation unit axis in bodyA. - b2Vec2 localAxisA; - - /// The constrained angle between the bodies: bodyB_angle - bodyA_angle. - float32 referenceAngle; - - /// Enable/disable the joint limit. - bool enableLimit; - - /// The lower translation limit, usually in meters. - float32 lowerTranslation; - - /// The upper translation limit, usually in meters. - float32 upperTranslation; - - /// Enable/disable the joint motor. - bool enableMotor; - - /// The maximum motor torque, usually in N-m. - float32 maxMotorForce; - - /// The desired motor speed in radians per second. - float32 motorSpeed; -}; - -/// A prismatic joint. This joint provides one degree of freedom: translation -/// along an axis fixed in bodyA. Relative rotation is prevented. You can -/// use a joint limit to restrict the range of motion and a joint motor to -/// drive the motion or to model joint friction. -class b2PrismaticJoint : public b2Joint -{ -public: - b2Vec2 GetAnchorA() const; - b2Vec2 GetAnchorB() const; - - b2Vec2 GetReactionForce(float32 inv_dt) const; - float32 GetReactionTorque(float32 inv_dt) const; - - /// The local anchor point relative to bodyA's origin. - const b2Vec2& GetLocalAnchorA() const { return m_localAnchorA; } - - /// The local anchor point relative to bodyB's origin. - const b2Vec2& GetLocalAnchorB() const { return m_localAnchorB; } - - /// The local joint axis relative to bodyA. - const b2Vec2& GetLocalAxisA() const { return m_localXAxisA; } - - /// Get the reference angle. - float32 GetReferenceAngle() const { return m_referenceAngle; } - - /// Get the current joint translation, usually in meters. - float32 GetJointTranslation() const; - - /// Get the current joint translation speed, usually in meters per second. - float32 GetJointSpeed() const; - - /// Is the joint limit enabled? - bool IsLimitEnabled() const; - - /// Enable/disable the joint limit. - void EnableLimit(bool flag); - - /// Get the lower joint limit, usually in meters. - float32 GetLowerLimit() const; - - /// Get the upper joint limit, usually in meters. - float32 GetUpperLimit() const; - - /// Set the joint limits, usually in meters. - void SetLimits(float32 lower, float32 upper); - - /// Is the joint motor enabled? - bool IsMotorEnabled() const; - - /// Enable/disable the joint motor. - void EnableMotor(bool flag); - - /// Set the motor speed, usually in meters per second. - void SetMotorSpeed(float32 speed); - - /// Get the motor speed, usually in meters per second. - float32 GetMotorSpeed() const; - - /// Set the maximum motor force, usually in N. - void SetMaxMotorForce(float32 force); - float32 GetMaxMotorForce() const { return m_maxMotorForce; } - - /// Get the current motor force given the inverse time step, usually in N. - float32 GetMotorForce(float32 inv_dt) const; - - /// Dump to b2Log - void Dump(); - -protected: - friend class b2Joint; - friend class b2GearJoint; - b2PrismaticJoint(const b2PrismaticJointDef* def); - - void InitVelocityConstraints(const b2SolverData& data); - void SolveVelocityConstraints(const b2SolverData& data); - bool SolvePositionConstraints(const b2SolverData& data); - - // Solver shared - b2Vec2 m_localAnchorA; - b2Vec2 m_localAnchorB; - b2Vec2 m_localXAxisA; - b2Vec2 m_localYAxisA; - float32 m_referenceAngle; - b2Vec3 m_impulse; - float32 m_motorImpulse; - float32 m_lowerTranslation; - float32 m_upperTranslation; - float32 m_maxMotorForce; - float32 m_motorSpeed; - bool m_enableLimit; - bool m_enableMotor; - b2LimitState m_limitState; - - // Solver temp - int32 m_indexA; - int32 m_indexB; - b2Vec2 m_localCenterA; - b2Vec2 m_localCenterB; - float32 m_invMassA; - float32 m_invMassB; - float32 m_invIA; - float32 m_invIB; - b2Vec2 m_axis, m_perp; - float32 m_s1, m_s2; - float32 m_a1, m_a2; - b2Mat33 m_K; - float32 m_motorMass; -}; - -inline float32 b2PrismaticJoint::GetMotorSpeed() const -{ - return m_motorSpeed; -} - -#endif diff --git a/Box2D/Dynamics/Joints/b2PulleyJoint.cpp b/Box2D/Dynamics/Joints/b2PulleyJoint.cpp deleted file mode 100644 index 9cd340f1b7..0000000000 --- a/Box2D/Dynamics/Joints/b2PulleyJoint.cpp +++ /dev/null @@ -1,348 +0,0 @@ -/* -* Copyright (c) 2007 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 -#include -#include - -// Pulley: -// length1 = norm(p1 - s1) -// length2 = norm(p2 - s2) -// C0 = (length1 + ratio * length2)_initial -// C = C0 - (length1 + ratio * length2) -// u1 = (p1 - s1) / norm(p1 - s1) -// u2 = (p2 - s2) / norm(p2 - s2) -// Cdot = -dot(u1, v1 + cross(w1, r1)) - ratio * dot(u2, v2 + cross(w2, r2)) -// J = -[u1 cross(r1, u1) ratio * u2 ratio * cross(r2, u2)] -// K = J * invM * JT -// = invMass1 + invI1 * cross(r1, u1)^2 + ratio^2 * (invMass2 + invI2 * cross(r2, u2)^2) - -void b2PulleyJointDef::Initialize(b2Body* bA, b2Body* bB, - const b2Vec2& groundA, const b2Vec2& groundB, - const b2Vec2& anchorA, const b2Vec2& anchorB, - float32 r) -{ - bodyA = bA; - bodyB = bB; - groundAnchorA = groundA; - groundAnchorB = groundB; - localAnchorA = bodyA->GetLocalPoint(anchorA); - localAnchorB = bodyB->GetLocalPoint(anchorB); - b2Vec2 dA = anchorA - groundA; - lengthA = dA.Length(); - b2Vec2 dB = anchorB - groundB; - lengthB = dB.Length(); - ratio = r; - b2Assert(ratio > b2_epsilon); -} - -b2PulleyJoint::b2PulleyJoint(const b2PulleyJointDef* def) -: b2Joint(def) -{ - m_groundAnchorA = def->groundAnchorA; - m_groundAnchorB = def->groundAnchorB; - m_localAnchorA = def->localAnchorA; - m_localAnchorB = def->localAnchorB; - - m_lengthA = def->lengthA; - m_lengthB = def->lengthB; - - b2Assert(def->ratio != 0.0f); - m_ratio = def->ratio; - - m_constant = def->lengthA + m_ratio * def->lengthB; - - m_impulse = 0.0f; -} - -void b2PulleyJoint::InitVelocityConstraints(const b2SolverData& data) -{ - m_indexA = m_bodyA->m_islandIndex; - m_indexB = m_bodyB->m_islandIndex; - m_localCenterA = m_bodyA->m_sweep.localCenter; - m_localCenterB = m_bodyB->m_sweep.localCenter; - m_invMassA = m_bodyA->m_invMass; - m_invMassB = m_bodyB->m_invMass; - m_invIA = m_bodyA->m_invI; - m_invIB = m_bodyB->m_invI; - - b2Vec2 cA = data.positions[m_indexA].c; - float32 aA = data.positions[m_indexA].a; - b2Vec2 vA = data.velocities[m_indexA].v; - float32 wA = data.velocities[m_indexA].w; - - b2Vec2 cB = data.positions[m_indexB].c; - float32 aB = data.positions[m_indexB].a; - b2Vec2 vB = data.velocities[m_indexB].v; - float32 wB = data.velocities[m_indexB].w; - - b2Rot qA(aA), qB(aB); - - m_rA = b2Mul(qA, m_localAnchorA - m_localCenterA); - m_rB = b2Mul(qB, m_localAnchorB - m_localCenterB); - - // Get the pulley axes. - m_uA = cA + m_rA - m_groundAnchorA; - m_uB = cB + m_rB - m_groundAnchorB; - - float32 lengthA = m_uA.Length(); - float32 lengthB = m_uB.Length(); - - if (lengthA > 10.0f * b2_linearSlop) - { - m_uA *= 1.0f / lengthA; - } - else - { - m_uA.SetZero(); - } - - if (lengthB > 10.0f * b2_linearSlop) - { - m_uB *= 1.0f / lengthB; - } - else - { - m_uB.SetZero(); - } - - // Compute effective mass. - float32 ruA = b2Cross(m_rA, m_uA); - float32 ruB = b2Cross(m_rB, m_uB); - - float32 mA = m_invMassA + m_invIA * ruA * ruA; - float32 mB = m_invMassB + m_invIB * ruB * ruB; - - m_mass = mA + m_ratio * m_ratio * mB; - - if (m_mass > 0.0f) - { - m_mass = 1.0f / m_mass; - } - - if (data.step.warmStarting) - { - // Scale impulses to support variable time steps. - m_impulse *= data.step.dtRatio; - - // Warm starting. - b2Vec2 PA = -(m_impulse) * m_uA; - b2Vec2 PB = (-m_ratio * m_impulse) * m_uB; - - vA += m_invMassA * PA; - wA += m_invIA * b2Cross(m_rA, PA); - vB += m_invMassB * PB; - wB += m_invIB * b2Cross(m_rB, PB); - } - else - { - m_impulse = 0.0f; - } - - data.velocities[m_indexA].v = vA; - data.velocities[m_indexA].w = wA; - data.velocities[m_indexB].v = vB; - data.velocities[m_indexB].w = wB; -} - -void b2PulleyJoint::SolveVelocityConstraints(const b2SolverData& data) -{ - b2Vec2 vA = data.velocities[m_indexA].v; - float32 wA = data.velocities[m_indexA].w; - b2Vec2 vB = data.velocities[m_indexB].v; - float32 wB = data.velocities[m_indexB].w; - - b2Vec2 vpA = vA + b2Cross(wA, m_rA); - b2Vec2 vpB = vB + b2Cross(wB, m_rB); - - float32 Cdot = -b2Dot(m_uA, vpA) - m_ratio * b2Dot(m_uB, vpB); - float32 impulse = -m_mass * Cdot; - m_impulse += impulse; - - b2Vec2 PA = -impulse * m_uA; - b2Vec2 PB = -m_ratio * impulse * m_uB; - vA += m_invMassA * PA; - wA += m_invIA * b2Cross(m_rA, PA); - vB += m_invMassB * PB; - wB += m_invIB * b2Cross(m_rB, PB); - - data.velocities[m_indexA].v = vA; - data.velocities[m_indexA].w = wA; - data.velocities[m_indexB].v = vB; - data.velocities[m_indexB].w = wB; -} - -bool b2PulleyJoint::SolvePositionConstraints(const b2SolverData& data) -{ - b2Vec2 cA = data.positions[m_indexA].c; - float32 aA = data.positions[m_indexA].a; - b2Vec2 cB = data.positions[m_indexB].c; - float32 aB = data.positions[m_indexB].a; - - b2Rot qA(aA), qB(aB); - - b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_localCenterA); - b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_localCenterB); - - // Get the pulley axes. - b2Vec2 uA = cA + rA - m_groundAnchorA; - b2Vec2 uB = cB + rB - m_groundAnchorB; - - float32 lengthA = uA.Length(); - float32 lengthB = uB.Length(); - - if (lengthA > 10.0f * b2_linearSlop) - { - uA *= 1.0f / lengthA; - } - else - { - uA.SetZero(); - } - - if (lengthB > 10.0f * b2_linearSlop) - { - uB *= 1.0f / lengthB; - } - else - { - uB.SetZero(); - } - - // Compute effective mass. - float32 ruA = b2Cross(rA, uA); - float32 ruB = b2Cross(rB, uB); - - float32 mA = m_invMassA + m_invIA * ruA * ruA; - float32 mB = m_invMassB + m_invIB * ruB * ruB; - - float32 mass = mA + m_ratio * m_ratio * mB; - - if (mass > 0.0f) - { - mass = 1.0f / mass; - } - - float32 C = m_constant - lengthA - m_ratio * lengthB; - float32 linearError = b2Abs(C); - - float32 impulse = -mass * C; - - b2Vec2 PA = -impulse * uA; - b2Vec2 PB = -m_ratio * impulse * uB; - - cA += m_invMassA * PA; - aA += m_invIA * b2Cross(rA, PA); - cB += m_invMassB * PB; - aB += m_invIB * b2Cross(rB, PB); - - data.positions[m_indexA].c = cA; - data.positions[m_indexA].a = aA; - data.positions[m_indexB].c = cB; - data.positions[m_indexB].a = aB; - - return linearError < b2_linearSlop; -} - -b2Vec2 b2PulleyJoint::GetAnchorA() const -{ - return m_bodyA->GetWorldPoint(m_localAnchorA); -} - -b2Vec2 b2PulleyJoint::GetAnchorB() const -{ - return m_bodyB->GetWorldPoint(m_localAnchorB); -} - -b2Vec2 b2PulleyJoint::GetReactionForce(float32 inv_dt) const -{ - b2Vec2 P = m_impulse * m_uB; - return inv_dt * P; -} - -float32 b2PulleyJoint::GetReactionTorque(float32 inv_dt) const -{ - B2_NOT_USED(inv_dt); - return 0.0f; -} - -b2Vec2 b2PulleyJoint::GetGroundAnchorA() const -{ - return m_groundAnchorA; -} - -b2Vec2 b2PulleyJoint::GetGroundAnchorB() const -{ - return m_groundAnchorB; -} - -float32 b2PulleyJoint::GetLengthA() const -{ - return m_lengthA; -} - -float32 b2PulleyJoint::GetLengthB() const -{ - return m_lengthB; -} - -float32 b2PulleyJoint::GetRatio() const -{ - return m_ratio; -} - -float32 b2PulleyJoint::GetCurrentLengthA() const -{ - b2Vec2 p = m_bodyA->GetWorldPoint(m_localAnchorA); - b2Vec2 s = m_groundAnchorA; - b2Vec2 d = p - s; - return d.Length(); -} - -float32 b2PulleyJoint::GetCurrentLengthB() const -{ - b2Vec2 p = m_bodyB->GetWorldPoint(m_localAnchorB); - b2Vec2 s = m_groundAnchorB; - b2Vec2 d = p - s; - return d.Length(); -} - -void b2PulleyJoint::Dump() -{ - int32 indexA = m_bodyA->m_islandIndex; - int32 indexB = m_bodyB->m_islandIndex; - - b2Log(" b2PulleyJointDef jd;\n"); - b2Log(" jd.bodyA = bodies[%d];\n", indexA); - b2Log(" jd.bodyB = bodies[%d];\n", indexB); - b2Log(" jd.collideConnected = bool(%d);\n", m_collideConnected); - b2Log(" jd.groundAnchorA.Set(%.15lef, %.15lef);\n", m_groundAnchorA.x, m_groundAnchorA.y); - b2Log(" jd.groundAnchorB.Set(%.15lef, %.15lef);\n", m_groundAnchorB.x, m_groundAnchorB.y); - b2Log(" jd.localAnchorA.Set(%.15lef, %.15lef);\n", m_localAnchorA.x, m_localAnchorA.y); - b2Log(" jd.localAnchorB.Set(%.15lef, %.15lef);\n", m_localAnchorB.x, m_localAnchorB.y); - b2Log(" jd.lengthA = %.15lef;\n", m_lengthA); - b2Log(" jd.lengthB = %.15lef;\n", m_lengthB); - b2Log(" jd.ratio = %.15lef;\n", m_ratio); - b2Log(" joints[%d] = m_world->CreateJoint(&jd);\n", m_index); -} - -void b2PulleyJoint::ShiftOrigin(const b2Vec2& newOrigin) -{ - m_groundAnchorA -= newOrigin; - m_groundAnchorB -= newOrigin; -} diff --git a/Box2D/Dynamics/Joints/b2PulleyJoint.h b/Box2D/Dynamics/Joints/b2PulleyJoint.h deleted file mode 100644 index 44843e7b41..0000000000 --- a/Box2D/Dynamics/Joints/b2PulleyJoint.h +++ /dev/null @@ -1,152 +0,0 @@ -/* -* Copyright (c) 2006-2011 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_PULLEY_JOINT_H -#define B2_PULLEY_JOINT_H - -#include - -const float32 b2_minPulleyLength = 2.0f; - -/// Pulley joint definition. This requires two ground anchors, -/// two dynamic body anchor points, and a pulley ratio. -struct b2PulleyJointDef : public b2JointDef -{ - b2PulleyJointDef() - { - type = e_pulleyJoint; - groundAnchorA.Set(-1.0f, 1.0f); - groundAnchorB.Set(1.0f, 1.0f); - localAnchorA.Set(-1.0f, 0.0f); - localAnchorB.Set(1.0f, 0.0f); - lengthA = 0.0f; - lengthB = 0.0f; - ratio = 1.0f; - collideConnected = true; - } - - /// Initialize the bodies, anchors, lengths, max lengths, and ratio using the world anchors. - void Initialize(b2Body* bodyA, b2Body* bodyB, - const b2Vec2& groundAnchorA, const b2Vec2& groundAnchorB, - const b2Vec2& anchorA, const b2Vec2& anchorB, - float32 ratio); - - /// The first ground anchor in world coordinates. This point never moves. - b2Vec2 groundAnchorA; - - /// The second ground anchor in world coordinates. This point never moves. - b2Vec2 groundAnchorB; - - /// The local anchor point relative to bodyA's origin. - b2Vec2 localAnchorA; - - /// The local anchor point relative to bodyB's origin. - b2Vec2 localAnchorB; - - /// The a reference length for the segment attached to bodyA. - float32 lengthA; - - /// The a reference length for the segment attached to bodyB. - float32 lengthB; - - /// The pulley ratio, used to simulate a block-and-tackle. - float32 ratio; -}; - -/// The pulley joint is connected to two bodies and two fixed ground points. -/// The pulley supports a ratio such that: -/// length1 + ratio * length2 <= constant -/// Yes, the force transmitted is scaled by the ratio. -/// Warning: the pulley joint can get a bit squirrelly by itself. They often -/// work better when combined with prismatic joints. You should also cover the -/// the anchor points with static shapes to prevent one side from going to -/// zero length. -class b2PulleyJoint : public b2Joint -{ -public: - b2Vec2 GetAnchorA() const; - b2Vec2 GetAnchorB() const; - - b2Vec2 GetReactionForce(float32 inv_dt) const; - float32 GetReactionTorque(float32 inv_dt) const; - - /// Get the first ground anchor. - b2Vec2 GetGroundAnchorA() const; - - /// Get the second ground anchor. - b2Vec2 GetGroundAnchorB() const; - - /// Get the current length of the segment attached to bodyA. - float32 GetLengthA() const; - - /// Get the current length of the segment attached to bodyB. - float32 GetLengthB() const; - - /// Get the pulley ratio. - float32 GetRatio() const; - - /// Get the current length of the segment attached to bodyA. - float32 GetCurrentLengthA() const; - - /// Get the current length of the segment attached to bodyB. - float32 GetCurrentLengthB() const; - - /// Dump joint to dmLog - void Dump(); - - /// Implement b2Joint::ShiftOrigin - void ShiftOrigin(const b2Vec2& newOrigin); - -protected: - - friend class b2Joint; - b2PulleyJoint(const b2PulleyJointDef* data); - - void InitVelocityConstraints(const b2SolverData& data); - void SolveVelocityConstraints(const b2SolverData& data); - bool SolvePositionConstraints(const b2SolverData& data); - - b2Vec2 m_groundAnchorA; - b2Vec2 m_groundAnchorB; - float32 m_lengthA; - float32 m_lengthB; - - // Solver shared - b2Vec2 m_localAnchorA; - b2Vec2 m_localAnchorB; - float32 m_constant; - float32 m_ratio; - float32 m_impulse; - - // Solver temp - int32 m_indexA; - int32 m_indexB; - b2Vec2 m_uA; - b2Vec2 m_uB; - b2Vec2 m_rA; - b2Vec2 m_rB; - b2Vec2 m_localCenterA; - b2Vec2 m_localCenterB; - float32 m_invMassA; - float32 m_invMassB; - float32 m_invIA; - float32 m_invIB; - float32 m_mass; -}; - -#endif diff --git a/Box2D/Dynamics/Joints/b2RevoluteJoint.cpp b/Box2D/Dynamics/Joints/b2RevoluteJoint.cpp deleted file mode 100644 index 64bd19070d..0000000000 --- a/Box2D/Dynamics/Joints/b2RevoluteJoint.cpp +++ /dev/null @@ -1,502 +0,0 @@ -/* -* Copyright (c) 2006-2011 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 -#include -#include - -// Point-to-point constraint -// C = p2 - p1 -// Cdot = v2 - v1 -// = v2 + cross(w2, r2) - v1 - cross(w1, r1) -// J = [-I -r1_skew I r2_skew ] -// Identity used: -// w k % (rx i + ry j) = w * (-ry i + rx j) - -// Motor constraint -// Cdot = w2 - w1 -// J = [0 0 -1 0 0 1] -// K = invI1 + invI2 - -void b2RevoluteJointDef::Initialize(b2Body* bA, b2Body* bB, const b2Vec2& anchor) -{ - bodyA = bA; - bodyB = bB; - localAnchorA = bodyA->GetLocalPoint(anchor); - localAnchorB = bodyB->GetLocalPoint(anchor); - referenceAngle = bodyB->GetAngle() - bodyA->GetAngle(); -} - -b2RevoluteJoint::b2RevoluteJoint(const b2RevoluteJointDef* def) -: b2Joint(def) -{ - m_localAnchorA = def->localAnchorA; - m_localAnchorB = def->localAnchorB; - m_referenceAngle = def->referenceAngle; - - m_impulse.SetZero(); - m_motorImpulse = 0.0f; - - m_lowerAngle = def->lowerAngle; - m_upperAngle = def->upperAngle; - m_maxMotorTorque = def->maxMotorTorque; - m_motorSpeed = def->motorSpeed; - m_enableLimit = def->enableLimit; - m_enableMotor = def->enableMotor; - m_limitState = e_inactiveLimit; -} - -void b2RevoluteJoint::InitVelocityConstraints(const b2SolverData& data) -{ - m_indexA = m_bodyA->m_islandIndex; - m_indexB = m_bodyB->m_islandIndex; - m_localCenterA = m_bodyA->m_sweep.localCenter; - m_localCenterB = m_bodyB->m_sweep.localCenter; - m_invMassA = m_bodyA->m_invMass; - m_invMassB = m_bodyB->m_invMass; - m_invIA = m_bodyA->m_invI; - m_invIB = m_bodyB->m_invI; - - float32 aA = data.positions[m_indexA].a; - b2Vec2 vA = data.velocities[m_indexA].v; - float32 wA = data.velocities[m_indexA].w; - - float32 aB = data.positions[m_indexB].a; - b2Vec2 vB = data.velocities[m_indexB].v; - float32 wB = data.velocities[m_indexB].w; - - b2Rot qA(aA), qB(aB); - - m_rA = b2Mul(qA, m_localAnchorA - m_localCenterA); - m_rB = b2Mul(qB, m_localAnchorB - m_localCenterB); - - // J = [-I -r1_skew I r2_skew] - // [ 0 -1 0 1] - // r_skew = [-ry; rx] - - // Matlab - // K = [ mA+r1y^2*iA+mB+r2y^2*iB, -r1y*iA*r1x-r2y*iB*r2x, -r1y*iA-r2y*iB] - // [ -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB, r1x*iA+r2x*iB] - // [ -r1y*iA-r2y*iB, r1x*iA+r2x*iB, iA+iB] - - float32 mA = m_invMassA, mB = m_invMassB; - float32 iA = m_invIA, iB = m_invIB; - - bool fixedRotation = (iA + iB == 0.0f); - - m_mass.ex.x = mA + mB + m_rA.y * m_rA.y * iA + m_rB.y * m_rB.y * iB; - m_mass.ey.x = -m_rA.y * m_rA.x * iA - m_rB.y * m_rB.x * iB; - m_mass.ez.x = -m_rA.y * iA - m_rB.y * iB; - m_mass.ex.y = m_mass.ey.x; - m_mass.ey.y = mA + mB + m_rA.x * m_rA.x * iA + m_rB.x * m_rB.x * iB; - m_mass.ez.y = m_rA.x * iA + m_rB.x * iB; - m_mass.ex.z = m_mass.ez.x; - m_mass.ey.z = m_mass.ez.y; - m_mass.ez.z = iA + iB; - - m_motorMass = iA + iB; - if (m_motorMass > 0.0f) - { - m_motorMass = 1.0f / m_motorMass; - } - - if (m_enableMotor == false || fixedRotation) - { - m_motorImpulse = 0.0f; - } - - if (m_enableLimit && fixedRotation == false) - { - float32 jointAngle = aB - aA - m_referenceAngle; - if (b2Abs(m_upperAngle - m_lowerAngle) < 2.0f * b2_angularSlop) - { - m_limitState = e_equalLimits; - } - else if (jointAngle <= m_lowerAngle) - { - if (m_limitState != e_atLowerLimit) - { - m_impulse.z = 0.0f; - } - m_limitState = e_atLowerLimit; - } - else if (jointAngle >= m_upperAngle) - { - if (m_limitState != e_atUpperLimit) - { - m_impulse.z = 0.0f; - } - m_limitState = e_atUpperLimit; - } - else - { - m_limitState = e_inactiveLimit; - m_impulse.z = 0.0f; - } - } - else - { - m_limitState = e_inactiveLimit; - } - - if (data.step.warmStarting) - { - // Scale impulses to support a variable time step. - m_impulse *= data.step.dtRatio; - m_motorImpulse *= data.step.dtRatio; - - b2Vec2 P(m_impulse.x, m_impulse.y); - - vA -= mA * P; - wA -= iA * (b2Cross(m_rA, P) + m_motorImpulse + m_impulse.z); - - vB += mB * P; - wB += iB * (b2Cross(m_rB, P) + m_motorImpulse + m_impulse.z); - } - else - { - m_impulse.SetZero(); - m_motorImpulse = 0.0f; - } - - data.velocities[m_indexA].v = vA; - data.velocities[m_indexA].w = wA; - data.velocities[m_indexB].v = vB; - data.velocities[m_indexB].w = wB; -} - -void b2RevoluteJoint::SolveVelocityConstraints(const b2SolverData& data) -{ - b2Vec2 vA = data.velocities[m_indexA].v; - float32 wA = data.velocities[m_indexA].w; - b2Vec2 vB = data.velocities[m_indexB].v; - float32 wB = data.velocities[m_indexB].w; - - float32 mA = m_invMassA, mB = m_invMassB; - float32 iA = m_invIA, iB = m_invIB; - - bool fixedRotation = (iA + iB == 0.0f); - - // Solve motor constraint. - if (m_enableMotor && m_limitState != e_equalLimits && fixedRotation == false) - { - float32 Cdot = wB - wA - m_motorSpeed; - float32 impulse = -m_motorMass * Cdot; - float32 oldImpulse = m_motorImpulse; - float32 maxImpulse = data.step.dt * m_maxMotorTorque; - m_motorImpulse = b2Clamp(m_motorImpulse + impulse, -maxImpulse, maxImpulse); - impulse = m_motorImpulse - oldImpulse; - - wA -= iA * impulse; - wB += iB * impulse; - } - - // Solve limit constraint. - if (m_enableLimit && m_limitState != e_inactiveLimit && fixedRotation == false) - { - b2Vec2 Cdot1 = vB + b2Cross(wB, m_rB) - vA - b2Cross(wA, m_rA); - float32 Cdot2 = wB - wA; - b2Vec3 Cdot(Cdot1.x, Cdot1.y, Cdot2); - - b2Vec3 impulse = -m_mass.Solve33(Cdot); - - if (m_limitState == e_equalLimits) - { - m_impulse += impulse; - } - else if (m_limitState == e_atLowerLimit) - { - float32 newImpulse = m_impulse.z + impulse.z; - if (newImpulse < 0.0f) - { - b2Vec2 rhs = -Cdot1 + m_impulse.z * b2Vec2(m_mass.ez.x, m_mass.ez.y); - b2Vec2 reduced = m_mass.Solve22(rhs); - impulse.x = reduced.x; - impulse.y = reduced.y; - impulse.z = -m_impulse.z; - m_impulse.x += reduced.x; - m_impulse.y += reduced.y; - m_impulse.z = 0.0f; - } - else - { - m_impulse += impulse; - } - } - else if (m_limitState == e_atUpperLimit) - { - float32 newImpulse = m_impulse.z + impulse.z; - if (newImpulse > 0.0f) - { - b2Vec2 rhs = -Cdot1 + m_impulse.z * b2Vec2(m_mass.ez.x, m_mass.ez.y); - b2Vec2 reduced = m_mass.Solve22(rhs); - impulse.x = reduced.x; - impulse.y = reduced.y; - impulse.z = -m_impulse.z; - m_impulse.x += reduced.x; - m_impulse.y += reduced.y; - m_impulse.z = 0.0f; - } - else - { - m_impulse += impulse; - } - } - - b2Vec2 P(impulse.x, impulse.y); - - vA -= mA * P; - wA -= iA * (b2Cross(m_rA, P) + impulse.z); - - vB += mB * P; - wB += iB * (b2Cross(m_rB, P) + impulse.z); - } - else - { - // Solve point-to-point constraint - b2Vec2 Cdot = vB + b2Cross(wB, m_rB) - vA - b2Cross(wA, m_rA); - b2Vec2 impulse = m_mass.Solve22(-Cdot); - - m_impulse.x += impulse.x; - m_impulse.y += impulse.y; - - vA -= mA * impulse; - wA -= iA * b2Cross(m_rA, impulse); - - vB += mB * impulse; - wB += iB * b2Cross(m_rB, impulse); - } - - data.velocities[m_indexA].v = vA; - data.velocities[m_indexA].w = wA; - data.velocities[m_indexB].v = vB; - data.velocities[m_indexB].w = wB; -} - -bool b2RevoluteJoint::SolvePositionConstraints(const b2SolverData& data) -{ - b2Vec2 cA = data.positions[m_indexA].c; - float32 aA = data.positions[m_indexA].a; - b2Vec2 cB = data.positions[m_indexB].c; - float32 aB = data.positions[m_indexB].a; - - b2Rot qA(aA), qB(aB); - - float32 angularError = 0.0f; - float32 positionError = 0.0f; - - bool fixedRotation = (m_invIA + m_invIB == 0.0f); - - // Solve angular limit constraint. - if (m_enableLimit && m_limitState != e_inactiveLimit && fixedRotation == false) - { - float32 angle = aB - aA - m_referenceAngle; - float32 limitImpulse = 0.0f; - - if (m_limitState == e_equalLimits) - { - // Prevent large angular corrections - float32 C = b2Clamp(angle - m_lowerAngle, -b2_maxAngularCorrection, b2_maxAngularCorrection); - limitImpulse = -m_motorMass * C; - angularError = b2Abs(C); - } - else if (m_limitState == e_atLowerLimit) - { - float32 C = angle - m_lowerAngle; - angularError = -C; - - // Prevent large angular corrections and allow some slop. - C = b2Clamp(C + b2_angularSlop, -b2_maxAngularCorrection, 0.0f); - limitImpulse = -m_motorMass * C; - } - else if (m_limitState == e_atUpperLimit) - { - float32 C = angle - m_upperAngle; - angularError = C; - - // Prevent large angular corrections and allow some slop. - C = b2Clamp(C - b2_angularSlop, 0.0f, b2_maxAngularCorrection); - limitImpulse = -m_motorMass * C; - } - - aA -= m_invIA * limitImpulse; - aB += m_invIB * limitImpulse; - } - - // Solve point-to-point constraint. - { - qA.Set(aA); - qB.Set(aB); - b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_localCenterA); - b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_localCenterB); - - b2Vec2 C = cB + rB - cA - rA; - positionError = C.Length(); - - float32 mA = m_invMassA, mB = m_invMassB; - float32 iA = m_invIA, iB = m_invIB; - - b2Mat22 K; - K.ex.x = mA + mB + iA * rA.y * rA.y + iB * rB.y * rB.y; - K.ex.y = -iA * rA.x * rA.y - iB * rB.x * rB.y; - K.ey.x = K.ex.y; - K.ey.y = mA + mB + iA * rA.x * rA.x + iB * rB.x * rB.x; - - b2Vec2 impulse = -K.Solve(C); - - cA -= mA * impulse; - aA -= iA * b2Cross(rA, impulse); - - cB += mB * impulse; - aB += iB * b2Cross(rB, impulse); - } - - data.positions[m_indexA].c = cA; - data.positions[m_indexA].a = aA; - data.positions[m_indexB].c = cB; - data.positions[m_indexB].a = aB; - - return positionError <= b2_linearSlop && angularError <= b2_angularSlop; -} - -b2Vec2 b2RevoluteJoint::GetAnchorA() const -{ - return m_bodyA->GetWorldPoint(m_localAnchorA); -} - -b2Vec2 b2RevoluteJoint::GetAnchorB() const -{ - return m_bodyB->GetWorldPoint(m_localAnchorB); -} - -b2Vec2 b2RevoluteJoint::GetReactionForce(float32 inv_dt) const -{ - b2Vec2 P(m_impulse.x, m_impulse.y); - return inv_dt * P; -} - -float32 b2RevoluteJoint::GetReactionTorque(float32 inv_dt) const -{ - return inv_dt * m_impulse.z; -} - -float32 b2RevoluteJoint::GetJointAngle() const -{ - b2Body* bA = m_bodyA; - b2Body* bB = m_bodyB; - return bB->m_sweep.a - bA->m_sweep.a - m_referenceAngle; -} - -float32 b2RevoluteJoint::GetJointSpeed() const -{ - b2Body* bA = m_bodyA; - b2Body* bB = m_bodyB; - return bB->m_angularVelocity - bA->m_angularVelocity; -} - -bool b2RevoluteJoint::IsMotorEnabled() const -{ - return m_enableMotor; -} - -void b2RevoluteJoint::EnableMotor(bool flag) -{ - m_bodyA->SetAwake(true); - m_bodyB->SetAwake(true); - m_enableMotor = flag; -} - -float32 b2RevoluteJoint::GetMotorTorque(float32 inv_dt) const -{ - return inv_dt * m_motorImpulse; -} - -void b2RevoluteJoint::SetMotorSpeed(float32 speed) -{ - m_bodyA->SetAwake(true); - m_bodyB->SetAwake(true); - m_motorSpeed = speed; -} - -void b2RevoluteJoint::SetMaxMotorTorque(float32 torque) -{ - m_bodyA->SetAwake(true); - m_bodyB->SetAwake(true); - m_maxMotorTorque = torque; -} - -bool b2RevoluteJoint::IsLimitEnabled() const -{ - return m_enableLimit; -} - -void b2RevoluteJoint::EnableLimit(bool flag) -{ - if (flag != m_enableLimit) - { - m_bodyA->SetAwake(true); - m_bodyB->SetAwake(true); - m_enableLimit = flag; - m_impulse.z = 0.0f; - } -} - -float32 b2RevoluteJoint::GetLowerLimit() const -{ - return m_lowerAngle; -} - -float32 b2RevoluteJoint::GetUpperLimit() const -{ - return m_upperAngle; -} - -void b2RevoluteJoint::SetLimits(float32 lower, float32 upper) -{ - b2Assert(lower <= upper); - - if (lower != m_lowerAngle || upper != m_upperAngle) - { - m_bodyA->SetAwake(true); - m_bodyB->SetAwake(true); - m_impulse.z = 0.0f; - m_lowerAngle = lower; - m_upperAngle = upper; - } -} - -void b2RevoluteJoint::Dump() -{ - int32 indexA = m_bodyA->m_islandIndex; - int32 indexB = m_bodyB->m_islandIndex; - - b2Log(" b2RevoluteJointDef jd;\n"); - b2Log(" jd.bodyA = bodies[%d];\n", indexA); - b2Log(" jd.bodyB = bodies[%d];\n", indexB); - b2Log(" jd.collideConnected = bool(%d);\n", m_collideConnected); - b2Log(" jd.localAnchorA.Set(%.15lef, %.15lef);\n", m_localAnchorA.x, m_localAnchorA.y); - b2Log(" jd.localAnchorB.Set(%.15lef, %.15lef);\n", m_localAnchorB.x, m_localAnchorB.y); - b2Log(" jd.referenceAngle = %.15lef;\n", m_referenceAngle); - b2Log(" jd.enableLimit = bool(%d);\n", m_enableLimit); - b2Log(" jd.lowerAngle = %.15lef;\n", m_lowerAngle); - b2Log(" jd.upperAngle = %.15lef;\n", m_upperAngle); - b2Log(" jd.enableMotor = bool(%d);\n", m_enableMotor); - b2Log(" jd.motorSpeed = %.15lef;\n", m_motorSpeed); - b2Log(" jd.maxMotorTorque = %.15lef;\n", m_maxMotorTorque); - b2Log(" joints[%d] = m_world->CreateJoint(&jd);\n", m_index); -} diff --git a/Box2D/Dynamics/Joints/b2RevoluteJoint.h b/Box2D/Dynamics/Joints/b2RevoluteJoint.h deleted file mode 100644 index bbed4a6cb2..0000000000 --- a/Box2D/Dynamics/Joints/b2RevoluteJoint.h +++ /dev/null @@ -1,204 +0,0 @@ -/* -* Copyright (c) 2006-2011 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_REVOLUTE_JOINT_H -#define B2_REVOLUTE_JOINT_H - -#include - -/// Revolute joint definition. This requires defining an -/// anchor point where the bodies are joined. The definition -/// uses local anchor points so that the initial configuration -/// can violate the constraint slightly. You also need to -/// specify the initial relative angle for joint limits. This -/// helps when saving and loading a game. -/// The local anchor points are measured from the body's origin -/// rather than the center of mass because: -/// 1. you might not know where the center of mass will be. -/// 2. if you add/remove shapes from a body and recompute the mass, -/// the joints will be broken. -struct b2RevoluteJointDef : public b2JointDef -{ - b2RevoluteJointDef() - { - type = e_revoluteJoint; - localAnchorA.Set(0.0f, 0.0f); - localAnchorB.Set(0.0f, 0.0f); - referenceAngle = 0.0f; - lowerAngle = 0.0f; - upperAngle = 0.0f; - maxMotorTorque = 0.0f; - motorSpeed = 0.0f; - enableLimit = false; - enableMotor = false; - } - - /// Initialize the bodies, anchors, and reference angle using a world - /// anchor point. - void Initialize(b2Body* bodyA, b2Body* bodyB, const b2Vec2& anchor); - - /// The local anchor point relative to bodyA's origin. - b2Vec2 localAnchorA; - - /// The local anchor point relative to bodyB's origin. - b2Vec2 localAnchorB; - - /// The bodyB angle minus bodyA angle in the reference state (radians). - float32 referenceAngle; - - /// A flag to enable joint limits. - bool enableLimit; - - /// The lower angle for the joint limit (radians). - float32 lowerAngle; - - /// The upper angle for the joint limit (radians). - float32 upperAngle; - - /// A flag to enable the joint motor. - bool enableMotor; - - /// The desired motor speed. Usually in radians per second. - float32 motorSpeed; - - /// The maximum motor torque used to achieve the desired motor speed. - /// Usually in N-m. - float32 maxMotorTorque; -}; - -/// A revolute joint constrains two bodies to share a common point while they -/// are free to rotate about the point. The relative rotation about the shared -/// point is the joint angle. You can limit the relative rotation with -/// a joint limit that specifies a lower and upper angle. You can use a motor -/// to drive the relative rotation about the shared point. A maximum motor torque -/// is provided so that infinite forces are not generated. -class b2RevoluteJoint : public b2Joint -{ -public: - b2Vec2 GetAnchorA() const; - b2Vec2 GetAnchorB() const; - - /// The local anchor point relative to bodyA's origin. - const b2Vec2& GetLocalAnchorA() const { return m_localAnchorA; } - - /// The local anchor point relative to bodyB's origin. - const b2Vec2& GetLocalAnchorB() const { return m_localAnchorB; } - - /// Get the reference angle. - float32 GetReferenceAngle() const { return m_referenceAngle; } - - /// Get the current joint angle in radians. - float32 GetJointAngle() const; - - /// Get the current joint angle speed in radians per second. - float32 GetJointSpeed() const; - - /// Is the joint limit enabled? - bool IsLimitEnabled() const; - - /// Enable/disable the joint limit. - void EnableLimit(bool flag); - - /// Get the lower joint limit in radians. - float32 GetLowerLimit() const; - - /// Get the upper joint limit in radians. - float32 GetUpperLimit() const; - - /// Set the joint limits in radians. - void SetLimits(float32 lower, float32 upper); - - /// Is the joint motor enabled? - bool IsMotorEnabled() const; - - /// Enable/disable the joint motor. - void EnableMotor(bool flag); - - /// Set the motor speed in radians per second. - void SetMotorSpeed(float32 speed); - - /// Get the motor speed in radians per second. - float32 GetMotorSpeed() const; - - /// Set the maximum motor torque, usually in N-m. - void SetMaxMotorTorque(float32 torque); - float32 GetMaxMotorTorque() const { return m_maxMotorTorque; } - - /// Get the reaction force given the inverse time step. - /// Unit is N. - b2Vec2 GetReactionForce(float32 inv_dt) const; - - /// Get the reaction torque due to the joint limit given the inverse time step. - /// Unit is N*m. - float32 GetReactionTorque(float32 inv_dt) const; - - /// Get the current motor torque given the inverse time step. - /// Unit is N*m. - float32 GetMotorTorque(float32 inv_dt) const; - - /// Dump to b2Log. - void Dump(); - -protected: - - friend class b2Joint; - friend class b2GearJoint; - - b2RevoluteJoint(const b2RevoluteJointDef* def); - - void InitVelocityConstraints(const b2SolverData& data); - void SolveVelocityConstraints(const b2SolverData& data); - bool SolvePositionConstraints(const b2SolverData& data); - - // Solver shared - b2Vec2 m_localAnchorA; - b2Vec2 m_localAnchorB; - b2Vec3 m_impulse; - float32 m_motorImpulse; - - bool m_enableMotor; - float32 m_maxMotorTorque; - float32 m_motorSpeed; - - bool m_enableLimit; - float32 m_referenceAngle; - float32 m_lowerAngle; - float32 m_upperAngle; - - // Solver temp - int32 m_indexA; - int32 m_indexB; - b2Vec2 m_rA; - b2Vec2 m_rB; - b2Vec2 m_localCenterA; - b2Vec2 m_localCenterB; - float32 m_invMassA; - float32 m_invMassB; - float32 m_invIA; - float32 m_invIB; - b2Mat33 m_mass; // effective mass for point-to-point constraint. - float32 m_motorMass; // effective mass for motor/limit angular constraint. - b2LimitState m_limitState; -}; - -inline float32 b2RevoluteJoint::GetMotorSpeed() const -{ - return m_motorSpeed; -} - -#endif diff --git a/Box2D/Dynamics/Joints/b2RopeJoint.cpp b/Box2D/Dynamics/Joints/b2RopeJoint.cpp deleted file mode 100644 index 952ee7a3e1..0000000000 --- a/Box2D/Dynamics/Joints/b2RopeJoint.cpp +++ /dev/null @@ -1,241 +0,0 @@ -/* -* Copyright (c) 2007-2011 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 -#include -#include - - -// Limit: -// C = norm(pB - pA) - L -// u = (pB - pA) / norm(pB - pA) -// Cdot = dot(u, vB + cross(wB, rB) - vA - cross(wA, rA)) -// J = [-u -cross(rA, u) u cross(rB, u)] -// K = J * invM * JT -// = invMassA + invIA * cross(rA, u)^2 + invMassB + invIB * cross(rB, u)^2 - -b2RopeJoint::b2RopeJoint(const b2RopeJointDef* def) -: b2Joint(def) -{ - m_localAnchorA = def->localAnchorA; - m_localAnchorB = def->localAnchorB; - - m_maxLength = def->maxLength; - - m_mass = 0.0f; - m_impulse = 0.0f; - m_state = e_inactiveLimit; - m_length = 0.0f; -} - -void b2RopeJoint::InitVelocityConstraints(const b2SolverData& data) -{ - m_indexA = m_bodyA->m_islandIndex; - m_indexB = m_bodyB->m_islandIndex; - m_localCenterA = m_bodyA->m_sweep.localCenter; - m_localCenterB = m_bodyB->m_sweep.localCenter; - m_invMassA = m_bodyA->m_invMass; - m_invMassB = m_bodyB->m_invMass; - m_invIA = m_bodyA->m_invI; - m_invIB = m_bodyB->m_invI; - - b2Vec2 cA = data.positions[m_indexA].c; - float32 aA = data.positions[m_indexA].a; - b2Vec2 vA = data.velocities[m_indexA].v; - float32 wA = data.velocities[m_indexA].w; - - b2Vec2 cB = data.positions[m_indexB].c; - float32 aB = data.positions[m_indexB].a; - b2Vec2 vB = data.velocities[m_indexB].v; - float32 wB = data.velocities[m_indexB].w; - - b2Rot qA(aA), qB(aB); - - m_rA = b2Mul(qA, m_localAnchorA - m_localCenterA); - m_rB = b2Mul(qB, m_localAnchorB - m_localCenterB); - m_u = cB + m_rB - cA - m_rA; - - m_length = m_u.Length(); - - float32 C = m_length - m_maxLength; - if (C > 0.0f) - { - m_state = e_atUpperLimit; - } - else - { - m_state = e_inactiveLimit; - } - - if (m_length > b2_linearSlop) - { - m_u *= 1.0f / m_length; - } - else - { - m_u.SetZero(); - m_mass = 0.0f; - m_impulse = 0.0f; - return; - } - - // Compute effective mass. - float32 crA = b2Cross(m_rA, m_u); - float32 crB = b2Cross(m_rB, m_u); - float32 invMass = m_invMassA + m_invIA * crA * crA + m_invMassB + m_invIB * crB * crB; - - m_mass = invMass != 0.0f ? 1.0f / invMass : 0.0f; - - if (data.step.warmStarting) - { - // Scale the impulse to support a variable time step. - m_impulse *= data.step.dtRatio; - - b2Vec2 P = m_impulse * m_u; - vA -= m_invMassA * P; - wA -= m_invIA * b2Cross(m_rA, P); - vB += m_invMassB * P; - wB += m_invIB * b2Cross(m_rB, P); - } - else - { - m_impulse = 0.0f; - } - - data.velocities[m_indexA].v = vA; - data.velocities[m_indexA].w = wA; - data.velocities[m_indexB].v = vB; - data.velocities[m_indexB].w = wB; -} - -void b2RopeJoint::SolveVelocityConstraints(const b2SolverData& data) -{ - b2Vec2 vA = data.velocities[m_indexA].v; - float32 wA = data.velocities[m_indexA].w; - b2Vec2 vB = data.velocities[m_indexB].v; - float32 wB = data.velocities[m_indexB].w; - - // Cdot = dot(u, v + cross(w, r)) - b2Vec2 vpA = vA + b2Cross(wA, m_rA); - b2Vec2 vpB = vB + b2Cross(wB, m_rB); - float32 C = m_length - m_maxLength; - float32 Cdot = b2Dot(m_u, vpB - vpA); - - // Predictive constraint. - if (C < 0.0f) - { - Cdot += data.step.inv_dt * C; - } - - float32 impulse = -m_mass * Cdot; - float32 oldImpulse = m_impulse; - m_impulse = b2Min(0.0f, m_impulse + impulse); - impulse = m_impulse - oldImpulse; - - b2Vec2 P = impulse * m_u; - vA -= m_invMassA * P; - wA -= m_invIA * b2Cross(m_rA, P); - vB += m_invMassB * P; - wB += m_invIB * b2Cross(m_rB, P); - - data.velocities[m_indexA].v = vA; - data.velocities[m_indexA].w = wA; - data.velocities[m_indexB].v = vB; - data.velocities[m_indexB].w = wB; -} - -bool b2RopeJoint::SolvePositionConstraints(const b2SolverData& data) -{ - b2Vec2 cA = data.positions[m_indexA].c; - float32 aA = data.positions[m_indexA].a; - b2Vec2 cB = data.positions[m_indexB].c; - float32 aB = data.positions[m_indexB].a; - - b2Rot qA(aA), qB(aB); - - b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_localCenterA); - b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_localCenterB); - b2Vec2 u = cB + rB - cA - rA; - - float32 length = u.Normalize(); - float32 C = length - m_maxLength; - - C = b2Clamp(C, 0.0f, b2_maxLinearCorrection); - - float32 impulse = -m_mass * C; - b2Vec2 P = impulse * u; - - cA -= m_invMassA * P; - aA -= m_invIA * b2Cross(rA, P); - cB += m_invMassB * P; - aB += m_invIB * b2Cross(rB, P); - - data.positions[m_indexA].c = cA; - data.positions[m_indexA].a = aA; - data.positions[m_indexB].c = cB; - data.positions[m_indexB].a = aB; - - return length - m_maxLength < b2_linearSlop; -} - -b2Vec2 b2RopeJoint::GetAnchorA() const -{ - return m_bodyA->GetWorldPoint(m_localAnchorA); -} - -b2Vec2 b2RopeJoint::GetAnchorB() const -{ - return m_bodyB->GetWorldPoint(m_localAnchorB); -} - -b2Vec2 b2RopeJoint::GetReactionForce(float32 inv_dt) const -{ - b2Vec2 F = (inv_dt * m_impulse) * m_u; - return F; -} - -float32 b2RopeJoint::GetReactionTorque(float32 inv_dt) const -{ - B2_NOT_USED(inv_dt); - return 0.0f; -} - -float32 b2RopeJoint::GetMaxLength() const -{ - return m_maxLength; -} - -b2LimitState b2RopeJoint::GetLimitState() const -{ - return m_state; -} - -void b2RopeJoint::Dump() -{ - int32 indexA = m_bodyA->m_islandIndex; - int32 indexB = m_bodyB->m_islandIndex; - - b2Log(" b2RopeJointDef jd;\n"); - b2Log(" jd.bodyA = bodies[%d];\n", indexA); - b2Log(" jd.bodyB = bodies[%d];\n", indexB); - b2Log(" jd.collideConnected = bool(%d);\n", m_collideConnected); - b2Log(" jd.localAnchorA.Set(%.15lef, %.15lef);\n", m_localAnchorA.x, m_localAnchorA.y); - b2Log(" jd.localAnchorB.Set(%.15lef, %.15lef);\n", m_localAnchorB.x, m_localAnchorB.y); - b2Log(" jd.maxLength = %.15lef;\n", m_maxLength); - b2Log(" joints[%d] = m_world->CreateJoint(&jd);\n", m_index); -} diff --git a/Box2D/Dynamics/Joints/b2RopeJoint.h b/Box2D/Dynamics/Joints/b2RopeJoint.h deleted file mode 100644 index eaca575d06..0000000000 --- a/Box2D/Dynamics/Joints/b2RopeJoint.h +++ /dev/null @@ -1,114 +0,0 @@ -/* -* Copyright (c) 2006-2011 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_ROPE_JOINT_H -#define B2_ROPE_JOINT_H - -#include - -/// Rope joint definition. This requires two body anchor points and -/// a maximum lengths. -/// Note: by default the connected objects will not collide. -/// see collideConnected in b2JointDef. -struct b2RopeJointDef : public b2JointDef -{ - b2RopeJointDef() - { - type = e_ropeJoint; - localAnchorA.Set(-1.0f, 0.0f); - localAnchorB.Set(1.0f, 0.0f); - maxLength = 0.0f; - } - - /// The local anchor point relative to bodyA's origin. - b2Vec2 localAnchorA; - - /// The local anchor point relative to bodyB's origin. - b2Vec2 localAnchorB; - - /// The maximum length of the rope. - /// Warning: this must be larger than b2_linearSlop or - /// the joint will have no effect. - float32 maxLength; -}; - -/// A rope joint enforces a maximum distance between two points -/// on two bodies. It has no other effect. -/// Warning: if you attempt to change the maximum length during -/// the simulation you will get some non-physical behavior. -/// A model that would allow you to dynamically modify the length -/// would have some sponginess, so I chose not to implement it -/// that way. See b2DistanceJoint if you want to dynamically -/// control length. -class b2RopeJoint : public b2Joint -{ -public: - b2Vec2 GetAnchorA() const; - b2Vec2 GetAnchorB() const; - - b2Vec2 GetReactionForce(float32 inv_dt) const; - float32 GetReactionTorque(float32 inv_dt) const; - - /// The local anchor point relative to bodyA's origin. - const b2Vec2& GetLocalAnchorA() const { return m_localAnchorA; } - - /// The local anchor point relative to bodyB's origin. - const b2Vec2& GetLocalAnchorB() const { return m_localAnchorB; } - - /// Set/Get the maximum length of the rope. - void SetMaxLength(float32 length) { m_maxLength = length; } - float32 GetMaxLength() const; - - b2LimitState GetLimitState() const; - - /// Dump joint to dmLog - void Dump(); - -protected: - - friend class b2Joint; - b2RopeJoint(const b2RopeJointDef* data); - - void InitVelocityConstraints(const b2SolverData& data); - void SolveVelocityConstraints(const b2SolverData& data); - bool SolvePositionConstraints(const b2SolverData& data); - - // Solver shared - b2Vec2 m_localAnchorA; - b2Vec2 m_localAnchorB; - float32 m_maxLength; - float32 m_length; - float32 m_impulse; - - // Solver temp - int32 m_indexA; - int32 m_indexB; - b2Vec2 m_u; - b2Vec2 m_rA; - b2Vec2 m_rB; - b2Vec2 m_localCenterA; - b2Vec2 m_localCenterB; - float32 m_invMassA; - float32 m_invMassB; - float32 m_invIA; - float32 m_invIB; - float32 m_mass; - b2LimitState m_state; -}; - -#endif diff --git a/Box2D/Dynamics/Joints/b2WeldJoint.cpp b/Box2D/Dynamics/Joints/b2WeldJoint.cpp deleted file mode 100644 index 253f7216ef..0000000000 --- a/Box2D/Dynamics/Joints/b2WeldJoint.cpp +++ /dev/null @@ -1,328 +0,0 @@ -/* -* Copyright (c) 2006-2011 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 -#include -#include - -// Point-to-point constraint -// C = p2 - p1 -// Cdot = v2 - v1 -// = v2 + cross(w2, r2) - v1 - cross(w1, r1) -// J = [-I -r1_skew I r2_skew ] -// Identity used: -// w k % (rx i + ry j) = w * (-ry i + rx j) - -// Angle constraint -// C = angle2 - angle1 - referenceAngle -// Cdot = w2 - w1 -// J = [0 0 -1 0 0 1] -// K = invI1 + invI2 - -void b2WeldJointDef::Initialize(b2Body* bA, b2Body* bB, const b2Vec2& anchor) -{ - bodyA = bA; - bodyB = bB; - localAnchorA = bodyA->GetLocalPoint(anchor); - localAnchorB = bodyB->GetLocalPoint(anchor); - referenceAngle = bodyB->GetAngle() - bodyA->GetAngle(); -} - -b2WeldJoint::b2WeldJoint(const b2WeldJointDef* def) -: b2Joint(def) -{ - m_localAnchorA = def->localAnchorA; - m_localAnchorB = def->localAnchorB; - m_referenceAngle = def->referenceAngle; - m_frequencyHz = def->frequencyHz; - m_dampingRatio = def->dampingRatio; - - m_impulse.SetZero(); -} - -void b2WeldJoint::InitVelocityConstraints(const b2SolverData& data) -{ - m_indexA = m_bodyA->m_islandIndex; - m_indexB = m_bodyB->m_islandIndex; - m_localCenterA = m_bodyA->m_sweep.localCenter; - m_localCenterB = m_bodyB->m_sweep.localCenter; - m_invMassA = m_bodyA->m_invMass; - m_invMassB = m_bodyB->m_invMass; - m_invIA = m_bodyA->m_invI; - m_invIB = m_bodyB->m_invI; - - float32 aA = data.positions[m_indexA].a; - b2Vec2 vA = data.velocities[m_indexA].v; - float32 wA = data.velocities[m_indexA].w; - - float32 aB = data.positions[m_indexB].a; - b2Vec2 vB = data.velocities[m_indexB].v; - float32 wB = data.velocities[m_indexB].w; - - b2Rot qA(aA), qB(aB); - - m_rA = b2Mul(qA, m_localAnchorA - m_localCenterA); - m_rB = b2Mul(qB, m_localAnchorB - m_localCenterB); - - // J = [-I -r1_skew I r2_skew] - // [ 0 -1 0 1] - // r_skew = [-ry; rx] - - // Matlab - // K = [ mA+r1y^2*iA+mB+r2y^2*iB, -r1y*iA*r1x-r2y*iB*r2x, -r1y*iA-r2y*iB] - // [ -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB, r1x*iA+r2x*iB] - // [ -r1y*iA-r2y*iB, r1x*iA+r2x*iB, iA+iB] - - float32 mA = m_invMassA, mB = m_invMassB; - float32 iA = m_invIA, iB = m_invIB; - - b2Mat33 K; - K.ex.x = mA + mB + m_rA.y * m_rA.y * iA + m_rB.y * m_rB.y * iB; - K.ey.x = -m_rA.y * m_rA.x * iA - m_rB.y * m_rB.x * iB; - K.ez.x = -m_rA.y * iA - m_rB.y * iB; - K.ex.y = K.ey.x; - K.ey.y = mA + mB + m_rA.x * m_rA.x * iA + m_rB.x * m_rB.x * iB; - K.ez.y = m_rA.x * iA + m_rB.x * iB; - K.ex.z = K.ez.x; - K.ey.z = K.ez.y; - K.ez.z = iA + iB; - - if (m_frequencyHz > 0.0f) - { - K.GetInverse22(&m_mass); - - float32 invM = iA + iB; - float32 m = invM > 0.0f ? 1.0f / invM : 0.0f; - - float32 C = aB - aA - m_referenceAngle; - - // Frequency - float32 omega = 2.0f * b2_pi * m_frequencyHz; - - // Damping coefficient - float32 d = 2.0f * m * m_dampingRatio * omega; - - // Spring stiffness - float32 k = m * omega * omega; - - // magic formulas - float32 h = data.step.dt; - m_gamma = h * (d + h * k); - m_gamma = m_gamma != 0.0f ? 1.0f / m_gamma : 0.0f; - m_bias = C * h * k * m_gamma; - - invM += m_gamma; - m_mass.ez.z = invM != 0.0f ? 1.0f / invM : 0.0f; - } - else - { - K.GetSymInverse33(&m_mass); - m_gamma = 0.0f; - m_bias = 0.0f; - } - - if (data.step.warmStarting) - { - // Scale impulses to support a variable time step. - m_impulse *= data.step.dtRatio; - - b2Vec2 P(m_impulse.x, m_impulse.y); - - vA -= mA * P; - wA -= iA * (b2Cross(m_rA, P) + m_impulse.z); - - vB += mB * P; - wB += iB * (b2Cross(m_rB, P) + m_impulse.z); - } - else - { - m_impulse.SetZero(); - } - - data.velocities[m_indexA].v = vA; - data.velocities[m_indexA].w = wA; - data.velocities[m_indexB].v = vB; - data.velocities[m_indexB].w = wB; -} - -void b2WeldJoint::SolveVelocityConstraints(const b2SolverData& data) -{ - b2Vec2 vA = data.velocities[m_indexA].v; - float32 wA = data.velocities[m_indexA].w; - b2Vec2 vB = data.velocities[m_indexB].v; - float32 wB = data.velocities[m_indexB].w; - - float32 mA = m_invMassA, mB = m_invMassB; - float32 iA = m_invIA, iB = m_invIB; - - if (m_frequencyHz > 0.0f) - { - float32 Cdot2 = wB - wA; - - float32 impulse2 = -m_mass.ez.z * (Cdot2 + m_bias + m_gamma * m_impulse.z); - m_impulse.z += impulse2; - - wA -= iA * impulse2; - wB += iB * impulse2; - - b2Vec2 Cdot1 = vB + b2Cross(wB, m_rB) - vA - b2Cross(wA, m_rA); - - b2Vec2 impulse1 = -b2Mul22(m_mass, Cdot1); - m_impulse.x += impulse1.x; - m_impulse.y += impulse1.y; - - b2Vec2 P = impulse1; - - vA -= mA * P; - wA -= iA * b2Cross(m_rA, P); - - vB += mB * P; - wB += iB * b2Cross(m_rB, P); - } - else - { - b2Vec2 Cdot1 = vB + b2Cross(wB, m_rB) - vA - b2Cross(wA, m_rA); - float32 Cdot2 = wB - wA; - b2Vec3 Cdot(Cdot1.x, Cdot1.y, Cdot2); - - b2Vec3 impulse = -b2Mul(m_mass, Cdot); - m_impulse += impulse; - - b2Vec2 P(impulse.x, impulse.y); - - vA -= mA * P; - wA -= iA * (b2Cross(m_rA, P) + impulse.z); - - vB += mB * P; - wB += iB * (b2Cross(m_rB, P) + impulse.z); - } - - data.velocities[m_indexA].v = vA; - data.velocities[m_indexA].w = wA; - data.velocities[m_indexB].v = vB; - data.velocities[m_indexB].w = wB; -} - -bool b2WeldJoint::SolvePositionConstraints(const b2SolverData& data) -{ - b2Vec2 cA = data.positions[m_indexA].c; - float32 aA = data.positions[m_indexA].a; - b2Vec2 cB = data.positions[m_indexB].c; - float32 aB = data.positions[m_indexB].a; - - b2Rot qA(aA), qB(aB); - - float32 mA = m_invMassA, mB = m_invMassB; - float32 iA = m_invIA, iB = m_invIB; - - b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_localCenterA); - b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_localCenterB); - - float32 positionError, angularError; - - b2Mat33 K; - K.ex.x = mA + mB + rA.y * rA.y * iA + rB.y * rB.y * iB; - K.ey.x = -rA.y * rA.x * iA - rB.y * rB.x * iB; - K.ez.x = -rA.y * iA - rB.y * iB; - K.ex.y = K.ey.x; - K.ey.y = mA + mB + rA.x * rA.x * iA + rB.x * rB.x * iB; - K.ez.y = rA.x * iA + rB.x * iB; - K.ex.z = K.ez.x; - K.ey.z = K.ez.y; - K.ez.z = iA + iB; - - if (m_frequencyHz > 0.0f) - { - b2Vec2 C1 = cB + rB - cA - rA; - - positionError = C1.Length(); - angularError = 0.0f; - - b2Vec2 P = -K.Solve22(C1); - - cA -= mA * P; - aA -= iA * b2Cross(rA, P); - - cB += mB * P; - aB += iB * b2Cross(rB, P); - } - else - { - b2Vec2 C1 = cB + rB - cA - rA; - float32 C2 = aB - aA - m_referenceAngle; - - positionError = C1.Length(); - angularError = b2Abs(C2); - - b2Vec3 C(C1.x, C1.y, C2); - - b2Vec3 impulse = -K.Solve33(C); - b2Vec2 P(impulse.x, impulse.y); - - cA -= mA * P; - aA -= iA * (b2Cross(rA, P) + impulse.z); - - cB += mB * P; - aB += iB * (b2Cross(rB, P) + impulse.z); - } - - data.positions[m_indexA].c = cA; - data.positions[m_indexA].a = aA; - data.positions[m_indexB].c = cB; - data.positions[m_indexB].a = aB; - - return positionError <= b2_linearSlop && angularError <= b2_angularSlop; -} - -b2Vec2 b2WeldJoint::GetAnchorA() const -{ - return m_bodyA->GetWorldPoint(m_localAnchorA); -} - -b2Vec2 b2WeldJoint::GetAnchorB() const -{ - return m_bodyB->GetWorldPoint(m_localAnchorB); -} - -b2Vec2 b2WeldJoint::GetReactionForce(float32 inv_dt) const -{ - b2Vec2 P(m_impulse.x, m_impulse.y); - return inv_dt * P; -} - -float32 b2WeldJoint::GetReactionTorque(float32 inv_dt) const -{ - return inv_dt * m_impulse.z; -} - -void b2WeldJoint::Dump() -{ - int32 indexA = m_bodyA->m_islandIndex; - int32 indexB = m_bodyB->m_islandIndex; - - b2Log(" b2WeldJointDef jd;\n"); - b2Log(" jd.bodyA = bodies[%d];\n", indexA); - b2Log(" jd.bodyB = bodies[%d];\n", indexB); - b2Log(" jd.collideConnected = bool(%d);\n", m_collideConnected); - b2Log(" jd.localAnchorA.Set(%.15lef, %.15lef);\n", m_localAnchorA.x, m_localAnchorA.y); - b2Log(" jd.localAnchorB.Set(%.15lef, %.15lef);\n", m_localAnchorB.x, m_localAnchorB.y); - b2Log(" jd.referenceAngle = %.15lef;\n", m_referenceAngle); - b2Log(" jd.frequencyHz = %.15lef;\n", m_frequencyHz); - b2Log(" jd.dampingRatio = %.15lef;\n", m_dampingRatio); - b2Log(" joints[%d] = m_world->CreateJoint(&jd);\n", m_index); -} diff --git a/Box2D/Dynamics/Joints/b2WeldJoint.h b/Box2D/Dynamics/Joints/b2WeldJoint.h deleted file mode 100644 index eed9bc1032..0000000000 --- a/Box2D/Dynamics/Joints/b2WeldJoint.h +++ /dev/null @@ -1,126 +0,0 @@ -/* -* Copyright (c) 2006-2011 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_WELD_JOINT_H -#define B2_WELD_JOINT_H - -#include - -/// Weld joint definition. You need to specify local anchor points -/// where they are attached and the relative body angle. The position -/// of the anchor points is important for computing the reaction torque. -struct b2WeldJointDef : public b2JointDef -{ - b2WeldJointDef() - { - type = e_weldJoint; - localAnchorA.Set(0.0f, 0.0f); - localAnchorB.Set(0.0f, 0.0f); - referenceAngle = 0.0f; - frequencyHz = 0.0f; - dampingRatio = 0.0f; - } - - /// Initialize the bodies, anchors, and reference angle using a world - /// anchor point. - void Initialize(b2Body* bodyA, b2Body* bodyB, const b2Vec2& anchor); - - /// The local anchor point relative to bodyA's origin. - b2Vec2 localAnchorA; - - /// The local anchor point relative to bodyB's origin. - b2Vec2 localAnchorB; - - /// The bodyB angle minus bodyA angle in the reference state (radians). - float32 referenceAngle; - - /// The mass-spring-damper frequency in Hertz. Rotation only. - /// Disable softness with a value of 0. - float32 frequencyHz; - - /// The damping ratio. 0 = no damping, 1 = critical damping. - float32 dampingRatio; -}; - -/// A weld joint essentially glues two bodies together. A weld joint may -/// distort somewhat because the island constraint solver is approximate. -class b2WeldJoint : public b2Joint -{ -public: - b2Vec2 GetAnchorA() const; - b2Vec2 GetAnchorB() const; - - b2Vec2 GetReactionForce(float32 inv_dt) const; - float32 GetReactionTorque(float32 inv_dt) const; - - /// The local anchor point relative to bodyA's origin. - const b2Vec2& GetLocalAnchorA() const { return m_localAnchorA; } - - /// The local anchor point relative to bodyB's origin. - const b2Vec2& GetLocalAnchorB() const { return m_localAnchorB; } - - /// Get the reference angle. - float32 GetReferenceAngle() const { return m_referenceAngle; } - - /// Set/get frequency in Hz. - void SetFrequency(float32 hz) { m_frequencyHz = hz; } - float32 GetFrequency() const { return m_frequencyHz; } - - /// Set/get damping ratio. - void SetDampingRatio(float32 ratio) { m_dampingRatio = ratio; } - float32 GetDampingRatio() const { return m_dampingRatio; } - - /// Dump to b2Log - void Dump(); - -protected: - - friend class b2Joint; - - b2WeldJoint(const b2WeldJointDef* def); - - void InitVelocityConstraints(const b2SolverData& data); - void SolveVelocityConstraints(const b2SolverData& data); - bool SolvePositionConstraints(const b2SolverData& data); - - float32 m_frequencyHz; - float32 m_dampingRatio; - float32 m_bias; - - // Solver shared - b2Vec2 m_localAnchorA; - b2Vec2 m_localAnchorB; - float32 m_referenceAngle; - float32 m_gamma; - b2Vec3 m_impulse; - - // Solver temp - int32 m_indexA; - int32 m_indexB; - b2Vec2 m_rA; - b2Vec2 m_rB; - b2Vec2 m_localCenterA; - b2Vec2 m_localCenterB; - float32 m_invMassA; - float32 m_invMassB; - float32 m_invIA; - float32 m_invIB; - b2Mat33 m_mass; -}; - -#endif diff --git a/Box2D/Dynamics/Joints/b2WheelJoint.cpp b/Box2D/Dynamics/Joints/b2WheelJoint.cpp deleted file mode 100644 index c9f6de6005..0000000000 --- a/Box2D/Dynamics/Joints/b2WheelJoint.cpp +++ /dev/null @@ -1,419 +0,0 @@ -/* -* Copyright (c) 2006-2007 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 -#include -#include - -// Linear constraint (point-to-line) -// d = pB - pA = xB + rB - xA - rA -// C = dot(ay, d) -// Cdot = dot(d, cross(wA, ay)) + dot(ay, vB + cross(wB, rB) - vA - cross(wA, rA)) -// = -dot(ay, vA) - dot(cross(d + rA, ay), wA) + dot(ay, vB) + dot(cross(rB, ay), vB) -// J = [-ay, -cross(d + rA, ay), ay, cross(rB, ay)] - -// Spring linear constraint -// C = dot(ax, d) -// Cdot = = -dot(ax, vA) - dot(cross(d + rA, ax), wA) + dot(ax, vB) + dot(cross(rB, ax), vB) -// J = [-ax -cross(d+rA, ax) ax cross(rB, ax)] - -// Motor rotational constraint -// Cdot = wB - wA -// J = [0 0 -1 0 0 1] - -void b2WheelJointDef::Initialize(b2Body* bA, b2Body* bB, const b2Vec2& anchor, const b2Vec2& axis) -{ - bodyA = bA; - bodyB = bB; - localAnchorA = bodyA->GetLocalPoint(anchor); - localAnchorB = bodyB->GetLocalPoint(anchor); - localAxisA = bodyA->GetLocalVector(axis); -} - -b2WheelJoint::b2WheelJoint(const b2WheelJointDef* def) -: b2Joint(def) -{ - m_localAnchorA = def->localAnchorA; - m_localAnchorB = def->localAnchorB; - m_localXAxisA = def->localAxisA; - m_localYAxisA = b2Cross(1.0f, m_localXAxisA); - - m_mass = 0.0f; - m_impulse = 0.0f; - m_motorMass = 0.0f; - m_motorImpulse = 0.0f; - m_springMass = 0.0f; - m_springImpulse = 0.0f; - - m_maxMotorTorque = def->maxMotorTorque; - m_motorSpeed = def->motorSpeed; - m_enableMotor = def->enableMotor; - - m_frequencyHz = def->frequencyHz; - m_dampingRatio = def->dampingRatio; - - m_bias = 0.0f; - m_gamma = 0.0f; - - m_ax.SetZero(); - m_ay.SetZero(); -} - -void b2WheelJoint::InitVelocityConstraints(const b2SolverData& data) -{ - m_indexA = m_bodyA->m_islandIndex; - m_indexB = m_bodyB->m_islandIndex; - m_localCenterA = m_bodyA->m_sweep.localCenter; - m_localCenterB = m_bodyB->m_sweep.localCenter; - m_invMassA = m_bodyA->m_invMass; - m_invMassB = m_bodyB->m_invMass; - m_invIA = m_bodyA->m_invI; - m_invIB = m_bodyB->m_invI; - - float32 mA = m_invMassA, mB = m_invMassB; - float32 iA = m_invIA, iB = m_invIB; - - b2Vec2 cA = data.positions[m_indexA].c; - float32 aA = data.positions[m_indexA].a; - b2Vec2 vA = data.velocities[m_indexA].v; - float32 wA = data.velocities[m_indexA].w; - - b2Vec2 cB = data.positions[m_indexB].c; - float32 aB = data.positions[m_indexB].a; - b2Vec2 vB = data.velocities[m_indexB].v; - float32 wB = data.velocities[m_indexB].w; - - b2Rot qA(aA), qB(aB); - - // Compute the effective masses. - b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_localCenterA); - b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_localCenterB); - b2Vec2 d = cB + rB - cA - rA; - - // Point to line constraint - { - m_ay = b2Mul(qA, m_localYAxisA); - m_sAy = b2Cross(d + rA, m_ay); - m_sBy = b2Cross(rB, m_ay); - - m_mass = mA + mB + iA * m_sAy * m_sAy + iB * m_sBy * m_sBy; - - if (m_mass > 0.0f) - { - m_mass = 1.0f / m_mass; - } - } - - // Spring constraint - m_springMass = 0.0f; - m_bias = 0.0f; - m_gamma = 0.0f; - if (m_frequencyHz > 0.0f) - { - m_ax = b2Mul(qA, m_localXAxisA); - m_sAx = b2Cross(d + rA, m_ax); - m_sBx = b2Cross(rB, m_ax); - - float32 invMass = mA + mB + iA * m_sAx * m_sAx + iB * m_sBx * m_sBx; - - if (invMass > 0.0f) - { - m_springMass = 1.0f / invMass; - - float32 C = b2Dot(d, m_ax); - - // Frequency - float32 omega = 2.0f * b2_pi * m_frequencyHz; - - // Damping coefficient - float32 d = 2.0f * m_springMass * m_dampingRatio * omega; - - // Spring stiffness - float32 k = m_springMass * omega * omega; - - // magic formulas - float32 h = data.step.dt; - m_gamma = h * (d + h * k); - if (m_gamma > 0.0f) - { - m_gamma = 1.0f / m_gamma; - } - - m_bias = C * h * k * m_gamma; - - m_springMass = invMass + m_gamma; - if (m_springMass > 0.0f) - { - m_springMass = 1.0f / m_springMass; - } - } - } - else - { - m_springImpulse = 0.0f; - } - - // Rotational motor - if (m_enableMotor) - { - m_motorMass = iA + iB; - if (m_motorMass > 0.0f) - { - m_motorMass = 1.0f / m_motorMass; - } - } - else - { - m_motorMass = 0.0f; - m_motorImpulse = 0.0f; - } - - if (data.step.warmStarting) - { - // Account for variable time step. - m_impulse *= data.step.dtRatio; - m_springImpulse *= data.step.dtRatio; - m_motorImpulse *= data.step.dtRatio; - - b2Vec2 P = m_impulse * m_ay + m_springImpulse * m_ax; - float32 LA = m_impulse * m_sAy + m_springImpulse * m_sAx + m_motorImpulse; - float32 LB = m_impulse * m_sBy + m_springImpulse * m_sBx + m_motorImpulse; - - vA -= m_invMassA * P; - wA -= m_invIA * LA; - - vB += m_invMassB * P; - wB += m_invIB * LB; - } - else - { - m_impulse = 0.0f; - m_springImpulse = 0.0f; - m_motorImpulse = 0.0f; - } - - data.velocities[m_indexA].v = vA; - data.velocities[m_indexA].w = wA; - data.velocities[m_indexB].v = vB; - data.velocities[m_indexB].w = wB; -} - -void b2WheelJoint::SolveVelocityConstraints(const b2SolverData& data) -{ - float32 mA = m_invMassA, mB = m_invMassB; - float32 iA = m_invIA, iB = m_invIB; - - b2Vec2 vA = data.velocities[m_indexA].v; - float32 wA = data.velocities[m_indexA].w; - b2Vec2 vB = data.velocities[m_indexB].v; - float32 wB = data.velocities[m_indexB].w; - - // Solve spring constraint - { - float32 Cdot = b2Dot(m_ax, vB - vA) + m_sBx * wB - m_sAx * wA; - float32 impulse = -m_springMass * (Cdot + m_bias + m_gamma * m_springImpulse); - m_springImpulse += impulse; - - b2Vec2 P = impulse * m_ax; - float32 LA = impulse * m_sAx; - float32 LB = impulse * m_sBx; - - vA -= mA * P; - wA -= iA * LA; - - vB += mB * P; - wB += iB * LB; - } - - // Solve rotational motor constraint - { - float32 Cdot = wB - wA - m_motorSpeed; - float32 impulse = -m_motorMass * Cdot; - - float32 oldImpulse = m_motorImpulse; - float32 maxImpulse = data.step.dt * m_maxMotorTorque; - m_motorImpulse = b2Clamp(m_motorImpulse + impulse, -maxImpulse, maxImpulse); - impulse = m_motorImpulse - oldImpulse; - - wA -= iA * impulse; - wB += iB * impulse; - } - - // Solve point to line constraint - { - float32 Cdot = b2Dot(m_ay, vB - vA) + m_sBy * wB - m_sAy * wA; - float32 impulse = -m_mass * Cdot; - m_impulse += impulse; - - b2Vec2 P = impulse * m_ay; - float32 LA = impulse * m_sAy; - float32 LB = impulse * m_sBy; - - vA -= mA * P; - wA -= iA * LA; - - vB += mB * P; - wB += iB * LB; - } - - data.velocities[m_indexA].v = vA; - data.velocities[m_indexA].w = wA; - data.velocities[m_indexB].v = vB; - data.velocities[m_indexB].w = wB; -} - -bool b2WheelJoint::SolvePositionConstraints(const b2SolverData& data) -{ - b2Vec2 cA = data.positions[m_indexA].c; - float32 aA = data.positions[m_indexA].a; - b2Vec2 cB = data.positions[m_indexB].c; - float32 aB = data.positions[m_indexB].a; - - b2Rot qA(aA), qB(aB); - - b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_localCenterA); - b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_localCenterB); - b2Vec2 d = (cB - cA) + rB - rA; - - b2Vec2 ay = b2Mul(qA, m_localYAxisA); - - float32 sAy = b2Cross(d + rA, ay); - float32 sBy = b2Cross(rB, ay); - - float32 C = b2Dot(d, ay); - - float32 k = m_invMassA + m_invMassB + m_invIA * m_sAy * m_sAy + m_invIB * m_sBy * m_sBy; - - float32 impulse; - if (k != 0.0f) - { - impulse = - C / k; - } - else - { - impulse = 0.0f; - } - - b2Vec2 P = impulse * ay; - float32 LA = impulse * sAy; - float32 LB = impulse * sBy; - - cA -= m_invMassA * P; - aA -= m_invIA * LA; - cB += m_invMassB * P; - aB += m_invIB * LB; - - data.positions[m_indexA].c = cA; - data.positions[m_indexA].a = aA; - data.positions[m_indexB].c = cB; - data.positions[m_indexB].a = aB; - - return b2Abs(C) <= b2_linearSlop; -} - -b2Vec2 b2WheelJoint::GetAnchorA() const -{ - return m_bodyA->GetWorldPoint(m_localAnchorA); -} - -b2Vec2 b2WheelJoint::GetAnchorB() const -{ - return m_bodyB->GetWorldPoint(m_localAnchorB); -} - -b2Vec2 b2WheelJoint::GetReactionForce(float32 inv_dt) const -{ - return inv_dt * (m_impulse * m_ay + m_springImpulse * m_ax); -} - -float32 b2WheelJoint::GetReactionTorque(float32 inv_dt) const -{ - return inv_dt * m_motorImpulse; -} - -float32 b2WheelJoint::GetJointTranslation() const -{ - b2Body* bA = m_bodyA; - b2Body* bB = m_bodyB; - - b2Vec2 pA = bA->GetWorldPoint(m_localAnchorA); - b2Vec2 pB = bB->GetWorldPoint(m_localAnchorB); - b2Vec2 d = pB - pA; - b2Vec2 axis = bA->GetWorldVector(m_localXAxisA); - - float32 translation = b2Dot(d, axis); - return translation; -} - -float32 b2WheelJoint::GetJointSpeed() const -{ - float32 wA = m_bodyA->m_angularVelocity; - float32 wB = m_bodyB->m_angularVelocity; - return wB - wA; -} - -bool b2WheelJoint::IsMotorEnabled() const -{ - return m_enableMotor; -} - -void b2WheelJoint::EnableMotor(bool flag) -{ - m_bodyA->SetAwake(true); - m_bodyB->SetAwake(true); - m_enableMotor = flag; -} - -void b2WheelJoint::SetMotorSpeed(float32 speed) -{ - m_bodyA->SetAwake(true); - m_bodyB->SetAwake(true); - m_motorSpeed = speed; -} - -void b2WheelJoint::SetMaxMotorTorque(float32 torque) -{ - m_bodyA->SetAwake(true); - m_bodyB->SetAwake(true); - m_maxMotorTorque = torque; -} - -float32 b2WheelJoint::GetMotorTorque(float32 inv_dt) const -{ - return inv_dt * m_motorImpulse; -} - -void b2WheelJoint::Dump() -{ - int32 indexA = m_bodyA->m_islandIndex; - int32 indexB = m_bodyB->m_islandIndex; - - b2Log(" b2WheelJointDef jd;\n"); - b2Log(" jd.bodyA = bodies[%d];\n", indexA); - b2Log(" jd.bodyB = bodies[%d];\n", indexB); - b2Log(" jd.collideConnected = bool(%d);\n", m_collideConnected); - b2Log(" jd.localAnchorA.Set(%.15lef, %.15lef);\n", m_localAnchorA.x, m_localAnchorA.y); - b2Log(" jd.localAnchorB.Set(%.15lef, %.15lef);\n", m_localAnchorB.x, m_localAnchorB.y); - b2Log(" jd.localAxisA.Set(%.15lef, %.15lef);\n", m_localXAxisA.x, m_localXAxisA.y); - b2Log(" jd.enableMotor = bool(%d);\n", m_enableMotor); - b2Log(" jd.motorSpeed = %.15lef;\n", m_motorSpeed); - b2Log(" jd.maxMotorTorque = %.15lef;\n", m_maxMotorTorque); - b2Log(" jd.frequencyHz = %.15lef;\n", m_frequencyHz); - b2Log(" jd.dampingRatio = %.15lef;\n", m_dampingRatio); - b2Log(" joints[%d] = m_world->CreateJoint(&jd);\n", m_index); -} diff --git a/Box2D/Dynamics/Joints/b2WheelJoint.h b/Box2D/Dynamics/Joints/b2WheelJoint.h deleted file mode 100644 index ca59d7a64a..0000000000 --- a/Box2D/Dynamics/Joints/b2WheelJoint.h +++ /dev/null @@ -1,211 +0,0 @@ -/* -* Copyright (c) 2006-2011 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_WHEEL_JOINT_H -#define B2_WHEEL_JOINT_H - -#include - -/// Wheel joint definition. This requires defining a line of -/// motion using an axis and an anchor point. The definition uses local -/// anchor points and a local axis so that the initial configuration -/// can violate the constraint slightly. The joint translation is zero -/// when the local anchor points coincide in world space. Using local -/// anchors and a local axis helps when saving and loading a game. -struct b2WheelJointDef : public b2JointDef -{ - b2WheelJointDef() - { - type = e_wheelJoint; - localAnchorA.SetZero(); - localAnchorB.SetZero(); - localAxisA.Set(1.0f, 0.0f); - enableMotor = false; - maxMotorTorque = 0.0f; - motorSpeed = 0.0f; - frequencyHz = 2.0f; - dampingRatio = 0.7f; - } - - /// Initialize the bodies, anchors, axis, and reference angle using the world - /// anchor and world axis. - void Initialize(b2Body* bodyA, b2Body* bodyB, const b2Vec2& anchor, const b2Vec2& axis); - - /// The local anchor point relative to bodyA's origin. - b2Vec2 localAnchorA; - - /// The local anchor point relative to bodyB's origin. - b2Vec2 localAnchorB; - - /// The local translation axis in bodyA. - b2Vec2 localAxisA; - - /// Enable/disable the joint motor. - bool enableMotor; - - /// The maximum motor torque, usually in N-m. - float32 maxMotorTorque; - - /// The desired motor speed in radians per second. - float32 motorSpeed; - - /// Suspension frequency, zero indicates no suspension - float32 frequencyHz; - - /// Suspension damping ratio, one indicates critical damping - float32 dampingRatio; -}; - -/// A wheel joint. This joint provides two degrees of freedom: translation -/// along an axis fixed in bodyA and rotation in the plane. You can use a -/// joint limit to restrict the range of motion and a joint motor to drive -/// the rotation or to model rotational friction. -/// This joint is designed for vehicle suspensions. -class b2WheelJoint : public b2Joint -{ -public: - b2Vec2 GetAnchorA() const; - b2Vec2 GetAnchorB() const; - - b2Vec2 GetReactionForce(float32 inv_dt) const; - float32 GetReactionTorque(float32 inv_dt) const; - - /// The local anchor point relative to bodyA's origin. - const b2Vec2& GetLocalAnchorA() const { return m_localAnchorA; } - - /// The local anchor point relative to bodyB's origin. - const b2Vec2& GetLocalAnchorB() const { return m_localAnchorB; } - - /// The local joint axis relative to bodyA. - const b2Vec2& GetLocalAxisA() const { return m_localXAxisA; } - - /// Get the current joint translation, usually in meters. - float32 GetJointTranslation() const; - - /// Get the current joint translation speed, usually in meters per second. - float32 GetJointSpeed() const; - - /// Is the joint motor enabled? - bool IsMotorEnabled() const; - - /// Enable/disable the joint motor. - void EnableMotor(bool flag); - - /// Set the motor speed, usually in radians per second. - void SetMotorSpeed(float32 speed); - - /// Get the motor speed, usually in radians per second. - float32 GetMotorSpeed() const; - - /// Set/Get the maximum motor force, usually in N-m. - void SetMaxMotorTorque(float32 torque); - float32 GetMaxMotorTorque() const; - - /// Get the current motor torque given the inverse time step, usually in N-m. - float32 GetMotorTorque(float32 inv_dt) const; - - /// Set/Get the spring frequency in hertz. Setting the frequency to zero disables the spring. - void SetSpringFrequencyHz(float32 hz); - float32 GetSpringFrequencyHz() const; - - /// Set/Get the spring damping ratio - void SetSpringDampingRatio(float32 ratio); - float32 GetSpringDampingRatio() const; - - /// Dump to b2Log - void Dump(); - -protected: - - friend class b2Joint; - b2WheelJoint(const b2WheelJointDef* def); - - void InitVelocityConstraints(const b2SolverData& data); - void SolveVelocityConstraints(const b2SolverData& data); - bool SolvePositionConstraints(const b2SolverData& data); - - float32 m_frequencyHz; - float32 m_dampingRatio; - - // Solver shared - b2Vec2 m_localAnchorA; - b2Vec2 m_localAnchorB; - b2Vec2 m_localXAxisA; - b2Vec2 m_localYAxisA; - - float32 m_impulse; - float32 m_motorImpulse; - float32 m_springImpulse; - - float32 m_maxMotorTorque; - float32 m_motorSpeed; - bool m_enableMotor; - - // Solver temp - int32 m_indexA; - int32 m_indexB; - b2Vec2 m_localCenterA; - b2Vec2 m_localCenterB; - float32 m_invMassA; - float32 m_invMassB; - float32 m_invIA; - float32 m_invIB; - - b2Vec2 m_ax, m_ay; - float32 m_sAx, m_sBx; - float32 m_sAy, m_sBy; - - float32 m_mass; - float32 m_motorMass; - float32 m_springMass; - - float32 m_bias; - float32 m_gamma; -}; - -inline float32 b2WheelJoint::GetMotorSpeed() const -{ - return m_motorSpeed; -} - -inline float32 b2WheelJoint::GetMaxMotorTorque() const -{ - return m_maxMotorTorque; -} - -inline void b2WheelJoint::SetSpringFrequencyHz(float32 hz) -{ - m_frequencyHz = hz; -} - -inline float32 b2WheelJoint::GetSpringFrequencyHz() const -{ - return m_frequencyHz; -} - -inline void b2WheelJoint::SetSpringDampingRatio(float32 ratio) -{ - m_dampingRatio = ratio; -} - -inline float32 b2WheelJoint::GetSpringDampingRatio() const -{ - return m_dampingRatio; -} - -#endif diff --git a/Box2D/Dynamics/b2Body.cpp b/Box2D/Dynamics/b2Body.cpp deleted file mode 100644 index b6550531f9..0000000000 --- a/Box2D/Dynamics/b2Body.cpp +++ /dev/null @@ -1,549 +0,0 @@ -/* -* Copyright (c) 2006-2007 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 -#include -#include -#include -#include - -b2Body::b2Body(const b2BodyDef* bd, b2World* world) -{ - b2Assert(bd->position.IsValid()); - b2Assert(bd->linearVelocity.IsValid()); - b2Assert(b2IsValid(bd->angle)); - b2Assert(b2IsValid(bd->angularVelocity)); - b2Assert(b2IsValid(bd->angularDamping) && bd->angularDamping >= 0.0f); - b2Assert(b2IsValid(bd->linearDamping) && bd->linearDamping >= 0.0f); - - m_flags = 0; - - if (bd->bullet) - { - m_flags |= e_bulletFlag; - } - if (bd->fixedRotation) - { - m_flags |= e_fixedRotationFlag; - } - if (bd->allowSleep) - { - m_flags |= e_autoSleepFlag; - } - if (bd->awake) - { - m_flags |= e_awakeFlag; - } - if (bd->active) - { - m_flags |= e_activeFlag; - } - - m_world = world; - - m_xf.p = bd->position; - m_xf.q.Set(bd->angle); - - m_sweep.localCenter.SetZero(); - m_sweep.c0 = m_xf.p; - m_sweep.c = m_xf.p; - m_sweep.a0 = bd->angle; - m_sweep.a = bd->angle; - m_sweep.alpha0 = 0.0f; - - m_jointList = NULL; - m_contactList = NULL; - m_prev = NULL; - m_next = NULL; - - m_linearVelocity = bd->linearVelocity; - m_angularVelocity = bd->angularVelocity; - - m_linearDamping = bd->linearDamping; - m_angularDamping = bd->angularDamping; - m_gravityScale = bd->gravityScale; - - m_force.SetZero(); - m_torque = 0.0f; - - m_sleepTime = 0.0f; - - m_type = bd->type; - - if (m_type == b2_dynamicBody) - { - m_mass = 1.0f; - m_invMass = 1.0f; - } - else - { - m_mass = 0.0f; - m_invMass = 0.0f; - } - - m_I = 0.0f; - m_invI = 0.0f; - - m_userData = bd->userData; - - m_fixtureList = NULL; - m_fixtureCount = 0; -} - -b2Body::~b2Body() -{ - // shapes and joints are destroyed in b2World::Destroy -} - -void b2Body::SetType(b2BodyType type) -{ - b2Assert(m_world->IsLocked() == false); - if (m_world->IsLocked() == true) - { - return; - } - - if (m_type == type) - { - return; - } - - m_type = type; - - ResetMassData(); - - if (m_type == b2_staticBody) - { - m_linearVelocity.SetZero(); - m_angularVelocity = 0.0f; - m_sweep.a0 = m_sweep.a; - m_sweep.c0 = m_sweep.c; - SynchronizeFixtures(); - } - - SetAwake(true); - - m_force.SetZero(); - m_torque = 0.0f; - - // Delete the attached contacts. - b2ContactEdge* ce = m_contactList; - while (ce) - { - b2ContactEdge* ce0 = ce; - ce = ce->next; - m_world->m_contactManager.Destroy(ce0->contact); - } - m_contactList = NULL; - - // Touch the proxies so that new contacts will be created (when appropriate) - b2BroadPhase* broadPhase = &m_world->m_contactManager.m_broadPhase; - for (b2Fixture* f = m_fixtureList; f; f = f->m_next) - { - int32 proxyCount = f->m_proxyCount; - for (int32 i = 0; i < proxyCount; ++i) - { - broadPhase->TouchProxy(f->m_proxies[i].proxyId); - } - } -} - -b2Fixture* b2Body::CreateFixture(const b2FixtureDef* def) -{ - b2Assert(m_world->IsLocked() == false); - if (m_world->IsLocked() == true) - { - return NULL; - } - - b2BlockAllocator* allocator = &m_world->m_blockAllocator; - - void* memory = allocator->Allocate(sizeof(b2Fixture)); - b2Fixture* fixture = new (memory) b2Fixture; - fixture->Create(allocator, this, def); - - if (m_flags & e_activeFlag) - { - b2BroadPhase* broadPhase = &m_world->m_contactManager.m_broadPhase; - fixture->CreateProxies(broadPhase, m_xf); - } - - fixture->m_next = m_fixtureList; - m_fixtureList = fixture; - ++m_fixtureCount; - - fixture->m_body = this; - - // Adjust mass properties if needed. - if (fixture->m_density > 0.0f) - { - ResetMassData(); - } - - // Let the world know we have a new fixture. This will cause new contacts - // to be created at the beginning of the next time step. - m_world->m_flags |= b2World::e_newFixture; - - return fixture; -} - -b2Fixture* b2Body::CreateFixture(const b2Shape* shape, float32 density) -{ - b2FixtureDef def; - def.shape = shape; - def.density = density; - - return CreateFixture(&def); -} - -void b2Body::DestroyFixture(b2Fixture* fixture) -{ - b2Assert(m_world->IsLocked() == false); - if (m_world->IsLocked() == true) - { - return; - } - - b2Assert(fixture->m_body == this); - - // Remove the fixture from this body's singly linked list. - b2Assert(m_fixtureCount > 0); - b2Fixture** node = &m_fixtureList; - bool found = false; - while (*node != NULL) - { - if (*node == fixture) - { - *node = fixture->m_next; - found = true; - break; - } - - node = &(*node)->m_next; - } - - // You tried to remove a shape that is not attached to this body. - b2Assert(found); - - // Destroy any contacts associated with the fixture. - b2ContactEdge* edge = m_contactList; - while (edge) - { - b2Contact* c = edge->contact; - edge = edge->next; - - b2Fixture* fixtureA = c->GetFixtureA(); - b2Fixture* fixtureB = c->GetFixtureB(); - - if (fixture == fixtureA || fixture == fixtureB) - { - // This destroys the contact and removes it from - // this body's contact list. - m_world->m_contactManager.Destroy(c); - } - } - - b2BlockAllocator* allocator = &m_world->m_blockAllocator; - - if (m_flags & e_activeFlag) - { - b2BroadPhase* broadPhase = &m_world->m_contactManager.m_broadPhase; - fixture->DestroyProxies(broadPhase); - } - - fixture->Destroy(allocator); - fixture->m_body = NULL; - fixture->m_next = NULL; - fixture->~b2Fixture(); - allocator->Free(fixture, sizeof(b2Fixture)); - - --m_fixtureCount; - - // Reset the mass data. - ResetMassData(); -} - -void b2Body::ResetMassData() -{ - // Compute mass data from shapes. Each shape has its own density. - m_mass = 0.0f; - m_invMass = 0.0f; - m_I = 0.0f; - m_invI = 0.0f; - m_sweep.localCenter.SetZero(); - - // Static and kinematic bodies have zero mass. - if (m_type == b2_staticBody || m_type == b2_kinematicBody) - { - m_sweep.c0 = m_xf.p; - m_sweep.c = m_xf.p; - m_sweep.a0 = m_sweep.a; - return; - } - - b2Assert(m_type == b2_dynamicBody); - - // Accumulate mass over all fixtures. - b2Vec2 localCenter = b2Vec2_zero; - for (b2Fixture* f = m_fixtureList; f; f = f->m_next) - { - if (f->m_density == 0.0f) - { - continue; - } - - b2MassData massData; - f->GetMassData(&massData); - m_mass += massData.mass; - localCenter += massData.mass * massData.center; - m_I += massData.I; - } - - // Compute center of mass. - if (m_mass > 0.0f) - { - m_invMass = 1.0f / m_mass; - localCenter *= m_invMass; - } - else - { - // Force all dynamic bodies to have a positive mass. - m_mass = 1.0f; - m_invMass = 1.0f; - } - - if (m_I > 0.0f && (m_flags & e_fixedRotationFlag) == 0) - { - // Center the inertia about the center of mass. - m_I -= m_mass * b2Dot(localCenter, localCenter); - b2Assert(m_I > 0.0f); - m_invI = 1.0f / m_I; - - } - else - { - m_I = 0.0f; - m_invI = 0.0f; - } - - // Move center of mass. - b2Vec2 oldCenter = m_sweep.c; - m_sweep.localCenter = localCenter; - m_sweep.c0 = m_sweep.c = b2Mul(m_xf, m_sweep.localCenter); - - // Update center of mass velocity. - m_linearVelocity += b2Cross(m_angularVelocity, m_sweep.c - oldCenter); -} - -void b2Body::SetMassData(const b2MassData* massData) -{ - b2Assert(m_world->IsLocked() == false); - if (m_world->IsLocked() == true) - { - return; - } - - if (m_type != b2_dynamicBody) - { - return; - } - - m_invMass = 0.0f; - m_I = 0.0f; - m_invI = 0.0f; - - m_mass = massData->mass; - if (m_mass <= 0.0f) - { - m_mass = 1.0f; - } - - m_invMass = 1.0f / m_mass; - - if (massData->I > 0.0f && (m_flags & b2Body::e_fixedRotationFlag) == 0) - { - m_I = massData->I - m_mass * b2Dot(massData->center, massData->center); - b2Assert(m_I > 0.0f); - m_invI = 1.0f / m_I; - } - - // Move center of mass. - b2Vec2 oldCenter = m_sweep.c; - m_sweep.localCenter = massData->center; - m_sweep.c0 = m_sweep.c = b2Mul(m_xf, m_sweep.localCenter); - - // Update center of mass velocity. - m_linearVelocity += b2Cross(m_angularVelocity, m_sweep.c - oldCenter); -} - -bool b2Body::ShouldCollide(const b2Body* other) const -{ - // At least one body should be dynamic. - if (m_type != b2_dynamicBody && other->m_type != b2_dynamicBody) - { - return false; - } - - // Does a joint prevent collision? - for (b2JointEdge* jn = m_jointList; jn; jn = jn->next) - { - if (jn->other == other) - { - if (jn->joint->m_collideConnected == false) - { - return false; - } - } - } - - return true; -} - -void b2Body::SetTransform(const b2Vec2& position, float32 angle) -{ - b2Assert(m_world->IsLocked() == false); - if (m_world->IsLocked() == true) - { - return; - } - - m_xf.q.Set(angle); - m_xf.p = position; - - m_sweep.c = b2Mul(m_xf, m_sweep.localCenter); - m_sweep.a = angle; - - m_sweep.c0 = m_sweep.c; - m_sweep.a0 = angle; - - b2BroadPhase* broadPhase = &m_world->m_contactManager.m_broadPhase; - for (b2Fixture* f = m_fixtureList; f; f = f->m_next) - { - f->Synchronize(broadPhase, m_xf, m_xf); - } -} - -void b2Body::SynchronizeFixtures() -{ - b2Transform xf1; - xf1.q.Set(m_sweep.a0); - xf1.p = m_sweep.c0 - b2Mul(xf1.q, m_sweep.localCenter); - - b2BroadPhase* broadPhase = &m_world->m_contactManager.m_broadPhase; - for (b2Fixture* f = m_fixtureList; f; f = f->m_next) - { - f->Synchronize(broadPhase, xf1, m_xf); - } -} - -void b2Body::SetActive(bool flag) -{ - b2Assert(m_world->IsLocked() == false); - - if (flag == IsActive()) - { - return; - } - - if (flag) - { - m_flags |= e_activeFlag; - - // Create all proxies. - b2BroadPhase* broadPhase = &m_world->m_contactManager.m_broadPhase; - for (b2Fixture* f = m_fixtureList; f; f = f->m_next) - { - f->CreateProxies(broadPhase, m_xf); - } - - // Contacts are created the next time step. - } - else - { - m_flags &= ~e_activeFlag; - - // Destroy all proxies. - b2BroadPhase* broadPhase = &m_world->m_contactManager.m_broadPhase; - for (b2Fixture* f = m_fixtureList; f; f = f->m_next) - { - f->DestroyProxies(broadPhase); - } - - // Destroy the attached contacts. - b2ContactEdge* ce = m_contactList; - while (ce) - { - b2ContactEdge* ce0 = ce; - ce = ce->next; - m_world->m_contactManager.Destroy(ce0->contact); - } - m_contactList = NULL; - } -} - -void b2Body::SetFixedRotation(bool flag) -{ - bool status = (m_flags & e_fixedRotationFlag) == e_fixedRotationFlag; - if (status == flag) - { - return; - } - - if (flag) - { - m_flags |= e_fixedRotationFlag; - } - else - { - m_flags &= ~e_fixedRotationFlag; - } - - m_angularVelocity = 0.0f; - - ResetMassData(); -} - -void b2Body::Dump() -{ - int32 bodyIndex = m_islandIndex; - - b2Log("{\n"); - b2Log(" b2BodyDef bd;\n"); - b2Log(" bd.type = b2BodyType(%d);\n", m_type); - b2Log(" bd.position.Set(%.15lef, %.15lef);\n", m_xf.p.x, m_xf.p.y); - b2Log(" bd.angle = %.15lef;\n", m_sweep.a); - b2Log(" bd.linearVelocity.Set(%.15lef, %.15lef);\n", m_linearVelocity.x, m_linearVelocity.y); - b2Log(" bd.angularVelocity = %.15lef;\n", m_angularVelocity); - b2Log(" bd.linearDamping = %.15lef;\n", m_linearDamping); - b2Log(" bd.angularDamping = %.15lef;\n", m_angularDamping); - b2Log(" bd.allowSleep = bool(%d);\n", m_flags & e_autoSleepFlag); - b2Log(" bd.awake = bool(%d);\n", m_flags & e_awakeFlag); - b2Log(" bd.fixedRotation = bool(%d);\n", m_flags & e_fixedRotationFlag); - b2Log(" bd.bullet = bool(%d);\n", m_flags & e_bulletFlag); - b2Log(" bd.active = bool(%d);\n", m_flags & e_activeFlag); - b2Log(" bd.gravityScale = %.15lef;\n", m_gravityScale); - b2Log(" bodies[%d] = m_world->CreateBody(&bd);\n", m_islandIndex); - b2Log("\n"); - for (b2Fixture* f = m_fixtureList; f; f = f->m_next) - { - b2Log(" {\n"); - f->Dump(bodyIndex); - b2Log(" }\n"); - } - b2Log("}\n"); -} diff --git a/Box2D/Dynamics/b2Body.h b/Box2D/Dynamics/b2Body.h deleted file mode 100644 index 24bae64039..0000000000 --- a/Box2D/Dynamics/b2Body.h +++ /dev/null @@ -1,860 +0,0 @@ -/* -* Copyright (c) 2006-2011 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_BODY_H -#define B2_BODY_H - -#include -#include -#include - -class b2Fixture; -class b2Joint; -class b2Contact; -class b2Controller; -class b2World; -struct b2FixtureDef; -struct b2JointEdge; -struct b2ContactEdge; - -/// The body type. -/// static: zero mass, zero velocity, may be manually moved -/// kinematic: zero mass, non-zero velocity set by user, moved by solver -/// dynamic: positive mass, non-zero velocity determined by forces, moved by solver -enum b2BodyType -{ - b2_staticBody = 0, - b2_kinematicBody, - b2_dynamicBody - - // TODO_ERIN - //b2_bulletBody, -}; - -/// A body definition holds all the data needed to construct a rigid body. -/// You can safely re-use body definitions. Shapes are added to a body after construction. -struct b2BodyDef -{ - /// This constructor sets the body definition default values. - b2BodyDef() - { - userData = NULL; - position.Set(0.0f, 0.0f); - angle = 0.0f; - linearVelocity.Set(0.0f, 0.0f); - angularVelocity = 0.0f; - linearDamping = 0.0f; - angularDamping = 0.0f; - allowSleep = true; - awake = true; - fixedRotation = false; - bullet = false; - type = b2_staticBody; - active = true; - gravityScale = 1.0f; - } - - /// The body type: static, kinematic, or dynamic. - /// Note: if a dynamic body would have zero mass, the mass is set to one. - b2BodyType type; - - /// The world position of the body. Avoid creating bodies at the origin - /// since this can lead to many overlapping shapes. - b2Vec2 position; - - /// The world angle of the body in radians. - float32 angle; - - /// The linear velocity of the body's origin in world co-ordinates. - b2Vec2 linearVelocity; - - /// The angular velocity of the body. - float32 angularVelocity; - - /// Linear damping is use to reduce the linear velocity. The damping parameter - /// can be larger than 1.0f but the damping effect becomes sensitive to the - /// time step when the damping parameter is large. - float32 linearDamping; - - /// Angular damping is use to reduce the angular velocity. The damping parameter - /// can be larger than 1.0f but the damping effect becomes sensitive to the - /// time step when the damping parameter is large. - float32 angularDamping; - - /// Set this flag to false if this body should never fall asleep. Note that - /// this increases CPU usage. - bool allowSleep; - - /// Is this body initially awake or sleeping? - bool awake; - - /// Should this body be prevented from rotating? Useful for characters. - bool fixedRotation; - - /// Is this a fast moving body that should be prevented from tunneling through - /// other moving bodies? Note that all bodies are prevented from tunneling through - /// kinematic and static bodies. This setting is only considered on dynamic bodies. - /// @warning You should use this flag sparingly since it increases processing time. - bool bullet; - - /// Does this body start out active? - bool active; - - /// Use this to store application specific body data. - void* userData; - - /// Scale the gravity applied to this body. - float32 gravityScale; -}; - -/// A rigid body. These are created via b2World::CreateBody. -class b2Body -{ -public: - /// Creates a fixture and attach it to this body. Use this function if you need - /// to set some fixture parameters, like friction. Otherwise you can create the - /// fixture directly from a shape. - /// If the density is non-zero, this function automatically updates the mass of the body. - /// Contacts are not created until the next time step. - /// @param def the fixture definition. - /// @warning This function is locked during callbacks. - b2Fixture* CreateFixture(const b2FixtureDef* def); - - /// Creates a fixture from a shape and attach it to this body. - /// This is a convenience function. Use b2FixtureDef if you need to set parameters - /// like friction, restitution, user data, or filtering. - /// If the density is non-zero, this function automatically updates the mass of the body. - /// @param shape the shape to be cloned. - /// @param density the shape density (set to zero for static bodies). - /// @warning This function is locked during callbacks. - b2Fixture* CreateFixture(const b2Shape* shape, float32 density); - - /// Destroy a fixture. This removes the fixture from the broad-phase and - /// destroys all contacts associated with this fixture. This will - /// automatically adjust the mass of the body if the body is dynamic and the - /// fixture has positive density. - /// All fixtures attached to a body are implicitly destroyed when the body is destroyed. - /// @param fixture the fixture to be removed. - /// @warning This function is locked during callbacks. - void DestroyFixture(b2Fixture* fixture); - - /// Set the position of the body's origin and rotation. - /// Manipulating a body's transform may cause non-physical behavior. - /// Note: contacts are updated on the next call to b2World::Step. - /// @param position the world position of the body's local origin. - /// @param angle the world rotation in radians. - void SetTransform(const b2Vec2& position, float32 angle); - - /// Get the body transform for the body's origin. - /// @return the world transform of the body's origin. - const b2Transform& GetTransform() const; - - /// Get the world body origin position. - /// @return the world position of the body's origin. - const b2Vec2& GetPosition() const; - - /// Get the angle in radians. - /// @return the current world rotation angle in radians. - float32 GetAngle() const; - - /// Get the world position of the center of mass. - const b2Vec2& GetWorldCenter() const; - - /// Get the local position of the center of mass. - const b2Vec2& GetLocalCenter() const; - - /// Set the linear velocity of the center of mass. - /// @param v the new linear velocity of the center of mass. - void SetLinearVelocity(const b2Vec2& v); - - /// Get the linear velocity of the center of mass. - /// @return the linear velocity of the center of mass. - const b2Vec2& GetLinearVelocity() const; - - /// Set the angular velocity. - /// @param omega the new angular velocity in radians/second. - void SetAngularVelocity(float32 omega); - - /// Get the angular velocity. - /// @return the angular velocity in radians/second. - float32 GetAngularVelocity() const; - - /// Apply a force at a world point. If the force is not - /// applied at the center of mass, it will generate a torque and - /// affect the angular velocity. This wakes up the body. - /// @param force the world force vector, usually in Newtons (N). - /// @param point the world position of the point of application. - /// @param wake also wake up the body - void ApplyForce(const b2Vec2& force, const b2Vec2& point, bool wake); - - /// Apply a force to the center of mass. This wakes up the body. - /// @param force the world force vector, usually in Newtons (N). - /// @param wake also wake up the body - void ApplyForceToCenter(const b2Vec2& force, bool wake); - - /// Apply a torque. This affects the angular velocity - /// without affecting the linear velocity of the center of mass. - /// This wakes up the body. - /// @param torque about the z-axis (out of the screen), usually in N-m. - /// @param wake also wake up the body - void ApplyTorque(float32 torque, bool wake); - - /// Apply an impulse at a point. This immediately modifies the velocity. - /// It also modifies the angular velocity if the point of application - /// is not at the center of mass. This wakes up the body. - /// @param impulse the world impulse vector, usually in N-seconds or kg-m/s. - /// @param point the world position of the point of application. - /// @param wake also wake up the body - void ApplyLinearImpulse(const b2Vec2& impulse, const b2Vec2& point, bool wake); - - /// Apply an angular impulse. - /// @param impulse the angular impulse in units of kg*m*m/s - /// @param wake also wake up the body - void ApplyAngularImpulse(float32 impulse, bool wake); - - /// Get the total mass of the body. - /// @return the mass, usually in kilograms (kg). - float32 GetMass() const; - - /// Get the rotational inertia of the body about the local origin. - /// @return the rotational inertia, usually in kg-m^2. - float32 GetInertia() const; - - /// Get the mass data of the body. - /// @return a struct containing the mass, inertia and center of the body. - void GetMassData(b2MassData* data) const; - - /// Set the mass properties to override the mass properties of the fixtures. - /// Note that this changes the center of mass position. - /// Note that creating or destroying fixtures can also alter the mass. - /// This function has no effect if the body isn't dynamic. - /// @param massData the mass properties. - void SetMassData(const b2MassData* data); - - /// This resets the mass properties to the sum of the mass properties of the fixtures. - /// This normally does not need to be called unless you called SetMassData to override - /// the mass and you later want to reset the mass. - void ResetMassData(); - - /// Get the world coordinates of a point given the local coordinates. - /// @param localPoint a point on the body measured relative the the body's origin. - /// @return the same point expressed in world coordinates. - b2Vec2 GetWorldPoint(const b2Vec2& localPoint) const; - - /// Get the world coordinates of a vector given the local coordinates. - /// @param localVector a vector fixed in the body. - /// @return the same vector expressed in world coordinates. - b2Vec2 GetWorldVector(const b2Vec2& localVector) const; - - /// Gets a local point relative to the body's origin given a world point. - /// @param a point in world coordinates. - /// @return the corresponding local point relative to the body's origin. - b2Vec2 GetLocalPoint(const b2Vec2& worldPoint) const; - - /// Gets a local vector given a world vector. - /// @param a vector in world coordinates. - /// @return the corresponding local vector. - b2Vec2 GetLocalVector(const b2Vec2& worldVector) const; - - /// Get the world linear velocity of a world point attached to this body. - /// @param a point in world coordinates. - /// @return the world velocity of a point. - b2Vec2 GetLinearVelocityFromWorldPoint(const b2Vec2& worldPoint) const; - - /// Get the world velocity of a local point. - /// @param a point in local coordinates. - /// @return the world velocity of a point. - b2Vec2 GetLinearVelocityFromLocalPoint(const b2Vec2& localPoint) const; - - /// Get the linear damping of the body. - float32 GetLinearDamping() const; - - /// Set the linear damping of the body. - void SetLinearDamping(float32 linearDamping); - - /// Get the angular damping of the body. - float32 GetAngularDamping() const; - - /// Set the angular damping of the body. - void SetAngularDamping(float32 angularDamping); - - /// Get the gravity scale of the body. - float32 GetGravityScale() const; - - /// Set the gravity scale of the body. - void SetGravityScale(float32 scale); - - /// Set the type of this body. This may alter the mass and velocity. - void SetType(b2BodyType type); - - /// Get the type of this body. - b2BodyType GetType() const; - - /// Should this body be treated like a bullet for continuous collision detection? - void SetBullet(bool flag); - - /// Is this body treated like a bullet for continuous collision detection? - bool IsBullet() const; - - /// You can disable sleeping on this body. If you disable sleeping, the - /// body will be woken. - void SetSleepingAllowed(bool flag); - - /// Is this body allowed to sleep - bool IsSleepingAllowed() const; - - /// Set the sleep state of the body. A sleeping body has very - /// low CPU cost. - /// @param flag set to true to wake the body, false to put it to sleep. - void SetAwake(bool flag); - - /// Get the sleeping state of this body. - /// @return true if the body is awake. - bool IsAwake() const; - - /// Set the active state of the body. An inactive body is not - /// simulated and cannot be collided with or woken up. - /// If you pass a flag of true, all fixtures will be added to the - /// broad-phase. - /// If you pass a flag of false, all fixtures will be removed from - /// the broad-phase and all contacts will be destroyed. - /// Fixtures and joints are otherwise unaffected. You may continue - /// to create/destroy fixtures and joints on inactive bodies. - /// Fixtures on an inactive body are implicitly inactive and will - /// not participate in collisions, ray-casts, or queries. - /// Joints connected to an inactive body are implicitly inactive. - /// An inactive body is still owned by a b2World object and remains - /// in the body list. - void SetActive(bool flag); - - /// Get the active state of the body. - bool IsActive() const; - - /// Set this body to have fixed rotation. This causes the mass - /// to be reset. - void SetFixedRotation(bool flag); - - /// Does this body have fixed rotation? - bool IsFixedRotation() const; - - /// Get the list of all fixtures attached to this body. - b2Fixture* GetFixtureList(); - const b2Fixture* GetFixtureList() const; - - /// Get the list of all joints attached to this body. - b2JointEdge* GetJointList(); - const b2JointEdge* GetJointList() const; - - /// Get the list of all contacts attached to this body. - /// @warning this list changes during the time step and you may - /// miss some collisions if you don't use b2ContactListener. - b2ContactEdge* GetContactList(); - const b2ContactEdge* GetContactList() const; - - /// Get the next body in the world's body list. - b2Body* GetNext(); - const b2Body* GetNext() const; - - /// Get the user data pointer that was provided in the body definition. - void* GetUserData() const; - - /// Set the user data. Use this to store your application specific data. - void SetUserData(void* data); - - /// Get the parent world of this body. - b2World* GetWorld(); - const b2World* GetWorld() const; - - /// Dump this body to a log file - void Dump(); - -private: - - friend class b2World; - friend class b2Island; - friend class b2ContactManager; - friend class b2ContactSolver; - friend class b2Contact; - - friend class b2DistanceJoint; - friend class b2FrictionJoint; - friend class b2GearJoint; - friend class b2MotorJoint; - friend class b2MouseJoint; - friend class b2PrismaticJoint; - friend class b2PulleyJoint; - friend class b2RevoluteJoint; - friend class b2RopeJoint; - friend class b2WeldJoint; - friend class b2WheelJoint; - - // m_flags - enum - { - e_islandFlag = 0x0001, - e_awakeFlag = 0x0002, - e_autoSleepFlag = 0x0004, - e_bulletFlag = 0x0008, - e_fixedRotationFlag = 0x0010, - e_activeFlag = 0x0020, - e_toiFlag = 0x0040 - }; - - b2Body(const b2BodyDef* bd, b2World* world); - ~b2Body(); - - void SynchronizeFixtures(); - void SynchronizeTransform(); - - // This is used to prevent connected bodies from colliding. - // It may lie, depending on the collideConnected flag. - bool ShouldCollide(const b2Body* other) const; - - void Advance(float32 t); - - b2BodyType m_type; - - uint16 m_flags; - - int32 m_islandIndex; - - b2Transform m_xf; // the body origin transform - b2Sweep m_sweep; // the swept motion for CCD - - b2Vec2 m_linearVelocity; - float32 m_angularVelocity; - - b2Vec2 m_force; - float32 m_torque; - - b2World* m_world; - b2Body* m_prev; - b2Body* m_next; - - b2Fixture* m_fixtureList; - int32 m_fixtureCount; - - b2JointEdge* m_jointList; - b2ContactEdge* m_contactList; - - float32 m_mass, m_invMass; - - // Rotational inertia about the center of mass. - float32 m_I, m_invI; - - float32 m_linearDamping; - float32 m_angularDamping; - float32 m_gravityScale; - - float32 m_sleepTime; - - void* m_userData; -}; - -inline b2BodyType b2Body::GetType() const -{ - return m_type; -} - -inline const b2Transform& b2Body::GetTransform() const -{ - return m_xf; -} - -inline const b2Vec2& b2Body::GetPosition() const -{ - return m_xf.p; -} - -inline float32 b2Body::GetAngle() const -{ - return m_sweep.a; -} - -inline const b2Vec2& b2Body::GetWorldCenter() const -{ - return m_sweep.c; -} - -inline const b2Vec2& b2Body::GetLocalCenter() const -{ - return m_sweep.localCenter; -} - -inline void b2Body::SetLinearVelocity(const b2Vec2& v) -{ - if (m_type == b2_staticBody) - { - return; - } - - if (b2Dot(v,v) > 0.0f) - { - SetAwake(true); - } - - m_linearVelocity = v; -} - -inline const b2Vec2& b2Body::GetLinearVelocity() const -{ - return m_linearVelocity; -} - -inline void b2Body::SetAngularVelocity(float32 w) -{ - if (m_type == b2_staticBody) - { - return; - } - - if (w * w > 0.0f) - { - SetAwake(true); - } - - m_angularVelocity = w; -} - -inline float32 b2Body::GetAngularVelocity() const -{ - return m_angularVelocity; -} - -inline float32 b2Body::GetMass() const -{ - return m_mass; -} - -inline float32 b2Body::GetInertia() const -{ - return m_I + m_mass * b2Dot(m_sweep.localCenter, m_sweep.localCenter); -} - -inline void b2Body::GetMassData(b2MassData* data) const -{ - data->mass = m_mass; - data->I = m_I + m_mass * b2Dot(m_sweep.localCenter, m_sweep.localCenter); - data->center = m_sweep.localCenter; -} - -inline b2Vec2 b2Body::GetWorldPoint(const b2Vec2& localPoint) const -{ - return b2Mul(m_xf, localPoint); -} - -inline b2Vec2 b2Body::GetWorldVector(const b2Vec2& localVector) const -{ - return b2Mul(m_xf.q, localVector); -} - -inline b2Vec2 b2Body::GetLocalPoint(const b2Vec2& worldPoint) const -{ - return b2MulT(m_xf, worldPoint); -} - -inline b2Vec2 b2Body::GetLocalVector(const b2Vec2& worldVector) const -{ - return b2MulT(m_xf.q, worldVector); -} - -inline b2Vec2 b2Body::GetLinearVelocityFromWorldPoint(const b2Vec2& worldPoint) const -{ - return m_linearVelocity + b2Cross(m_angularVelocity, worldPoint - m_sweep.c); -} - -inline b2Vec2 b2Body::GetLinearVelocityFromLocalPoint(const b2Vec2& localPoint) const -{ - return GetLinearVelocityFromWorldPoint(GetWorldPoint(localPoint)); -} - -inline float32 b2Body::GetLinearDamping() const -{ - return m_linearDamping; -} - -inline void b2Body::SetLinearDamping(float32 linearDamping) -{ - m_linearDamping = linearDamping; -} - -inline float32 b2Body::GetAngularDamping() const -{ - return m_angularDamping; -} - -inline void b2Body::SetAngularDamping(float32 angularDamping) -{ - m_angularDamping = angularDamping; -} - -inline float32 b2Body::GetGravityScale() const -{ - return m_gravityScale; -} - -inline void b2Body::SetGravityScale(float32 scale) -{ - m_gravityScale = scale; -} - -inline void b2Body::SetBullet(bool flag) -{ - if (flag) - { - m_flags |= e_bulletFlag; - } - else - { - m_flags &= ~e_bulletFlag; - } -} - -inline bool b2Body::IsBullet() const -{ - return (m_flags & e_bulletFlag) == e_bulletFlag; -} - -inline void b2Body::SetAwake(bool flag) -{ - if (flag) - { - if ((m_flags & e_awakeFlag) == 0) - { - m_flags |= e_awakeFlag; - m_sleepTime = 0.0f; - } - } - else - { - m_flags &= ~e_awakeFlag; - m_sleepTime = 0.0f; - m_linearVelocity.SetZero(); - m_angularVelocity = 0.0f; - m_force.SetZero(); - m_torque = 0.0f; - } -} - -inline bool b2Body::IsAwake() const -{ - return (m_flags & e_awakeFlag) == e_awakeFlag; -} - -inline bool b2Body::IsActive() const -{ - return (m_flags & e_activeFlag) == e_activeFlag; -} - -inline bool b2Body::IsFixedRotation() const -{ - return (m_flags & e_fixedRotationFlag) == e_fixedRotationFlag; -} - -inline void b2Body::SetSleepingAllowed(bool flag) -{ - if (flag) - { - m_flags |= e_autoSleepFlag; - } - else - { - m_flags &= ~e_autoSleepFlag; - SetAwake(true); - } -} - -inline bool b2Body::IsSleepingAllowed() const -{ - return (m_flags & e_autoSleepFlag) == e_autoSleepFlag; -} - -inline b2Fixture* b2Body::GetFixtureList() -{ - return m_fixtureList; -} - -inline const b2Fixture* b2Body::GetFixtureList() const -{ - return m_fixtureList; -} - -inline b2JointEdge* b2Body::GetJointList() -{ - return m_jointList; -} - -inline const b2JointEdge* b2Body::GetJointList() const -{ - return m_jointList; -} - -inline b2ContactEdge* b2Body::GetContactList() -{ - return m_contactList; -} - -inline const b2ContactEdge* b2Body::GetContactList() const -{ - return m_contactList; -} - -inline b2Body* b2Body::GetNext() -{ - return m_next; -} - -inline const b2Body* b2Body::GetNext() const -{ - return m_next; -} - -inline void b2Body::SetUserData(void* data) -{ - m_userData = data; -} - -inline void* b2Body::GetUserData() const -{ - return m_userData; -} - -inline void b2Body::ApplyForce(const b2Vec2& force, const b2Vec2& point, bool wake) -{ - if (m_type != b2_dynamicBody) - { - return; - } - - if (wake && (m_flags & e_awakeFlag) == 0) - { - SetAwake(true); - } - - // Don't accumulate a force if the body is sleeping. - if (m_flags & e_awakeFlag) - { - m_force += force; - m_torque += b2Cross(point - m_sweep.c, force); - } -} - -inline void b2Body::ApplyForceToCenter(const b2Vec2& force, bool wake) -{ - if (m_type != b2_dynamicBody) - { - return; - } - - if (wake && (m_flags & e_awakeFlag) == 0) - { - SetAwake(true); - } - - // Don't accumulate a force if the body is sleeping - if (m_flags & e_awakeFlag) - { - m_force += force; - } -} - -inline void b2Body::ApplyTorque(float32 torque, bool wake) -{ - if (m_type != b2_dynamicBody) - { - return; - } - - if (wake && (m_flags & e_awakeFlag) == 0) - { - SetAwake(true); - } - - // Don't accumulate a force if the body is sleeping - if (m_flags & e_awakeFlag) - { - m_torque += torque; - } -} - -inline void b2Body::ApplyLinearImpulse(const b2Vec2& impulse, const b2Vec2& point, bool wake) -{ - if (m_type != b2_dynamicBody) - { - return; - } - - if (wake && (m_flags & e_awakeFlag) == 0) - { - SetAwake(true); - } - - // Don't accumulate velocity if the body is sleeping - if (m_flags & e_awakeFlag) - { - m_linearVelocity += m_invMass * impulse; - m_angularVelocity += m_invI * b2Cross(point - m_sweep.c, impulse); - } -} - -inline void b2Body::ApplyAngularImpulse(float32 impulse, bool wake) -{ - if (m_type != b2_dynamicBody) - { - return; - } - - if (wake && (m_flags & e_awakeFlag) == 0) - { - SetAwake(true); - } - - // Don't accumulate velocity if the body is sleeping - if (m_flags & e_awakeFlag) - { - m_angularVelocity += m_invI * impulse; - } -} - -inline void b2Body::SynchronizeTransform() -{ - m_xf.q.Set(m_sweep.a); - m_xf.p = m_sweep.c - b2Mul(m_xf.q, m_sweep.localCenter); -} - -inline void b2Body::Advance(float32 alpha) -{ - // Advance to the new safe time. This doesn't sync the broad-phase. - m_sweep.Advance(alpha); - m_sweep.c = m_sweep.c0; - m_sweep.a = m_sweep.a0; - m_xf.q.Set(m_sweep.a); - m_xf.p = m_sweep.c - b2Mul(m_xf.q, m_sweep.localCenter); -} - -inline b2World* b2Body::GetWorld() -{ - return m_world; -} - -inline const b2World* b2Body::GetWorld() const -{ - return m_world; -} - -#endif diff --git a/Box2D/Dynamics/b2ContactManager.cpp b/Box2D/Dynamics/b2ContactManager.cpp deleted file mode 100644 index d8a189b74e..0000000000 --- a/Box2D/Dynamics/b2ContactManager.cpp +++ /dev/null @@ -1,296 +0,0 @@ -/* -* 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 -#include -#include -#include -#include - -b2ContactFilter b2_defaultFilter; -b2ContactListener b2_defaultListener; - -b2ContactManager::b2ContactManager() -{ - m_contactList = NULL; - m_contactCount = 0; - m_contactFilter = &b2_defaultFilter; - m_contactListener = &b2_defaultListener; - m_allocator = NULL; -} - -void b2ContactManager::Destroy(b2Contact* c) -{ - b2Fixture* fixtureA = c->GetFixtureA(); - b2Fixture* fixtureB = c->GetFixtureB(); - b2Body* bodyA = fixtureA->GetBody(); - b2Body* bodyB = fixtureB->GetBody(); - - if (m_contactListener && c->IsTouching()) - { - m_contactListener->EndContact(c); - } - - // Remove from the world. - if (c->m_prev) - { - c->m_prev->m_next = c->m_next; - } - - if (c->m_next) - { - c->m_next->m_prev = c->m_prev; - } - - if (c == m_contactList) - { - m_contactList = c->m_next; - } - - // Remove from body 1 - if (c->m_nodeA.prev) - { - c->m_nodeA.prev->next = c->m_nodeA.next; - } - - if (c->m_nodeA.next) - { - c->m_nodeA.next->prev = c->m_nodeA.prev; - } - - if (&c->m_nodeA == bodyA->m_contactList) - { - bodyA->m_contactList = c->m_nodeA.next; - } - - // Remove from body 2 - if (c->m_nodeB.prev) - { - c->m_nodeB.prev->next = c->m_nodeB.next; - } - - if (c->m_nodeB.next) - { - c->m_nodeB.next->prev = c->m_nodeB.prev; - } - - if (&c->m_nodeB == bodyB->m_contactList) - { - bodyB->m_contactList = c->m_nodeB.next; - } - - // Call the factory. - b2Contact::Destroy(c, m_allocator); - --m_contactCount; -} - -// This is the top level collision call for the time step. Here -// all the narrow phase collision is processed for the world -// contact list. -void b2ContactManager::Collide() -{ - // Update awake contacts. - b2Contact* c = m_contactList; - while (c) - { - b2Fixture* fixtureA = c->GetFixtureA(); - b2Fixture* fixtureB = c->GetFixtureB(); - int32 indexA = c->GetChildIndexA(); - int32 indexB = c->GetChildIndexB(); - b2Body* bodyA = fixtureA->GetBody(); - b2Body* bodyB = fixtureB->GetBody(); - - // Is this contact flagged for filtering? - if (c->m_flags & b2Contact::e_filterFlag) - { - // Should these bodies collide? - if (bodyB->ShouldCollide(bodyA) == false) - { - b2Contact* cNuke = c; - c = cNuke->GetNext(); - Destroy(cNuke); - continue; - } - - // Check user filtering. - if (m_contactFilter && m_contactFilter->ShouldCollide(fixtureA, fixtureB) == false) - { - b2Contact* cNuke = c; - c = cNuke->GetNext(); - Destroy(cNuke); - continue; - } - - // Clear the filtering flag. - c->m_flags &= ~b2Contact::e_filterFlag; - } - - bool activeA = bodyA->IsAwake() && bodyA->m_type != b2_staticBody; - bool activeB = bodyB->IsAwake() && bodyB->m_type != b2_staticBody; - - // At least one body must be awake and it must be dynamic or kinematic. - if (activeA == false && activeB == false) - { - c = c->GetNext(); - continue; - } - - int32 proxyIdA = fixtureA->m_proxies[indexA].proxyId; - int32 proxyIdB = fixtureB->m_proxies[indexB].proxyId; - bool overlap = m_broadPhase.TestOverlap(proxyIdA, proxyIdB); - - // Here we destroy contacts that cease to overlap in the broad-phase. - if (overlap == false) - { - b2Contact* cNuke = c; - c = cNuke->GetNext(); - Destroy(cNuke); - continue; - } - - // The contact persists. - c->Update(m_contactListener); - c = c->GetNext(); - } -} - -void b2ContactManager::FindNewContacts() -{ - m_broadPhase.UpdatePairs(this); -} - -void b2ContactManager::AddPair(void* proxyUserDataA, void* proxyUserDataB) -{ - b2FixtureProxy* proxyA = (b2FixtureProxy*)proxyUserDataA; - b2FixtureProxy* proxyB = (b2FixtureProxy*)proxyUserDataB; - - b2Fixture* fixtureA = proxyA->fixture; - b2Fixture* fixtureB = proxyB->fixture; - - int32 indexA = proxyA->childIndex; - int32 indexB = proxyB->childIndex; - - b2Body* bodyA = fixtureA->GetBody(); - b2Body* bodyB = fixtureB->GetBody(); - - // Are the fixtures on the same body? - if (bodyA == bodyB) - { - return; - } - - // TODO_ERIN use a hash table to remove a potential bottleneck when both - // bodies have a lot of contacts. - // Does a contact already exist? - b2ContactEdge* edge = bodyB->GetContactList(); - while (edge) - { - if (edge->other == bodyA) - { - b2Fixture* fA = edge->contact->GetFixtureA(); - b2Fixture* fB = edge->contact->GetFixtureB(); - int32 iA = edge->contact->GetChildIndexA(); - int32 iB = edge->contact->GetChildIndexB(); - - if (fA == fixtureA && fB == fixtureB && iA == indexA && iB == indexB) - { - // A contact already exists. - return; - } - - if (fA == fixtureB && fB == fixtureA && iA == indexB && iB == indexA) - { - // A contact already exists. - return; - } - } - - edge = edge->next; - } - - // Does a joint override collision? Is at least one body dynamic? - if (bodyB->ShouldCollide(bodyA) == false) - { - return; - } - - // Check user filtering. - if (m_contactFilter && m_contactFilter->ShouldCollide(fixtureA, fixtureB) == false) - { - return; - } - - // Call the factory. - b2Contact* c = b2Contact::Create(fixtureA, indexA, fixtureB, indexB, m_allocator); - if (c == NULL) - { - return; - } - - // Contact creation may swap fixtures. - fixtureA = c->GetFixtureA(); - fixtureB = c->GetFixtureB(); - indexA = c->GetChildIndexA(); - indexB = c->GetChildIndexB(); - bodyA = fixtureA->GetBody(); - bodyB = fixtureB->GetBody(); - - // Insert into the world. - c->m_prev = NULL; - c->m_next = m_contactList; - if (m_contactList != NULL) - { - m_contactList->m_prev = c; - } - m_contactList = c; - - // Connect to island graph. - - // Connect to body A - c->m_nodeA.contact = c; - c->m_nodeA.other = bodyB; - - c->m_nodeA.prev = NULL; - c->m_nodeA.next = bodyA->m_contactList; - if (bodyA->m_contactList != NULL) - { - bodyA->m_contactList->prev = &c->m_nodeA; - } - bodyA->m_contactList = &c->m_nodeA; - - // Connect to body B - c->m_nodeB.contact = c; - c->m_nodeB.other = bodyA; - - c->m_nodeB.prev = NULL; - c->m_nodeB.next = bodyB->m_contactList; - if (bodyB->m_contactList != NULL) - { - bodyB->m_contactList->prev = &c->m_nodeB; - } - bodyB->m_contactList = &c->m_nodeB; - - // Wake up the bodies - if (fixtureA->IsSensor() == false && fixtureB->IsSensor() == false) - { - bodyA->SetAwake(true); - bodyB->SetAwake(true); - } - - ++m_contactCount; -} diff --git a/Box2D/Dynamics/b2ContactManager.h b/Box2D/Dynamics/b2ContactManager.h deleted file mode 100644 index 6a1d072af0..0000000000 --- a/Box2D/Dynamics/b2ContactManager.h +++ /dev/null @@ -1,52 +0,0 @@ -/* -* 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_CONTACT_MANAGER_H -#define B2_CONTACT_MANAGER_H - -#include - -class b2Contact; -class b2ContactFilter; -class b2ContactListener; -class b2BlockAllocator; - -// Delegate of b2World. -class b2ContactManager -{ -public: - b2ContactManager(); - - // Broad-phase callback. - void AddPair(void* proxyUserDataA, void* proxyUserDataB); - - void FindNewContacts(); - - void Destroy(b2Contact* c); - - void Collide(); - - b2BroadPhase m_broadPhase; - b2Contact* m_contactList; - int32 m_contactCount; - b2ContactFilter* m_contactFilter; - b2ContactListener* m_contactListener; - b2BlockAllocator* m_allocator; -}; - -#endif diff --git a/Box2D/Dynamics/b2Fixture.cpp b/Box2D/Dynamics/b2Fixture.cpp deleted file mode 100644 index fb17f0a3e2..0000000000 --- a/Box2D/Dynamics/b2Fixture.cpp +++ /dev/null @@ -1,303 +0,0 @@ -/* -* 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 -#include -#include -#include -#include -#include -#include -#include -#include -#include - -b2Fixture::b2Fixture() -{ - m_userData = NULL; - m_body = NULL; - m_next = NULL; - m_proxies = NULL; - m_proxyCount = 0; - m_shape = NULL; - m_density = 0.0f; -} - -void b2Fixture::Create(b2BlockAllocator* allocator, b2Body* body, const b2FixtureDef* def) -{ - m_userData = def->userData; - m_friction = def->friction; - m_restitution = def->restitution; - - m_body = body; - m_next = NULL; - - m_filter = def->filter; - - m_isSensor = def->isSensor; - - m_shape = def->shape->Clone(allocator); - - // Reserve proxy space - int32 childCount = m_shape->GetChildCount(); - m_proxies = (b2FixtureProxy*)allocator->Allocate(childCount * sizeof(b2FixtureProxy)); - for (int32 i = 0; i < childCount; ++i) - { - m_proxies[i].fixture = NULL; - m_proxies[i].proxyId = b2BroadPhase::e_nullProxy; - } - m_proxyCount = 0; - - m_density = def->density; -} - -void b2Fixture::Destroy(b2BlockAllocator* allocator) -{ - // The proxies must be destroyed before calling this. - b2Assert(m_proxyCount == 0); - - // Free the proxy array. - int32 childCount = m_shape->GetChildCount(); - allocator->Free(m_proxies, childCount * sizeof(b2FixtureProxy)); - m_proxies = NULL; - - // Free the child shape. - switch (m_shape->m_type) - { - case b2Shape::e_circle: - { - b2CircleShape* s = (b2CircleShape*)m_shape; - s->~b2CircleShape(); - allocator->Free(s, sizeof(b2CircleShape)); - } - break; - - case b2Shape::e_edge: - { - b2EdgeShape* s = (b2EdgeShape*)m_shape; - s->~b2EdgeShape(); - allocator->Free(s, sizeof(b2EdgeShape)); - } - break; - - case b2Shape::e_polygon: - { - b2PolygonShape* s = (b2PolygonShape*)m_shape; - s->~b2PolygonShape(); - allocator->Free(s, sizeof(b2PolygonShape)); - } - break; - - case b2Shape::e_chain: - { - b2ChainShape* s = (b2ChainShape*)m_shape; - s->~b2ChainShape(); - allocator->Free(s, sizeof(b2ChainShape)); - } - break; - - default: - b2Assert(false); - break; - } - - m_shape = NULL; -} - -void b2Fixture::CreateProxies(b2BroadPhase* broadPhase, const b2Transform& xf) -{ - b2Assert(m_proxyCount == 0); - - // Create proxies in the broad-phase. - m_proxyCount = m_shape->GetChildCount(); - - for (int32 i = 0; i < m_proxyCount; ++i) - { - b2FixtureProxy* proxy = m_proxies + i; - m_shape->ComputeAABB(&proxy->aabb, xf, i); - proxy->proxyId = broadPhase->CreateProxy(proxy->aabb, proxy); - proxy->fixture = this; - proxy->childIndex = i; - } -} - -void b2Fixture::DestroyProxies(b2BroadPhase* broadPhase) -{ - // Destroy proxies in the broad-phase. - for (int32 i = 0; i < m_proxyCount; ++i) - { - b2FixtureProxy* proxy = m_proxies + i; - broadPhase->DestroyProxy(proxy->proxyId); - proxy->proxyId = b2BroadPhase::e_nullProxy; - } - - m_proxyCount = 0; -} - -void b2Fixture::Synchronize(b2BroadPhase* broadPhase, const b2Transform& transform1, const b2Transform& transform2) -{ - if (m_proxyCount == 0) - { - return; - } - - for (int32 i = 0; i < m_proxyCount; ++i) - { - b2FixtureProxy* proxy = m_proxies + i; - - // Compute an AABB that covers the swept shape (may miss some rotation effect). - b2AABB aabb1, aabb2; - m_shape->ComputeAABB(&aabb1, transform1, proxy->childIndex); - m_shape->ComputeAABB(&aabb2, transform2, proxy->childIndex); - - proxy->aabb.Combine(aabb1, aabb2); - - b2Vec2 displacement = transform2.p - transform1.p; - - broadPhase->MoveProxy(proxy->proxyId, proxy->aabb, displacement); - } -} - -void b2Fixture::SetFilterData(const b2Filter& filter) -{ - m_filter = filter; - - Refilter(); -} - -void b2Fixture::Refilter() -{ - if (m_body == NULL) - { - return; - } - - // Flag associated contacts for filtering. - b2ContactEdge* edge = m_body->GetContactList(); - while (edge) - { - b2Contact* contact = edge->contact; - b2Fixture* fixtureA = contact->GetFixtureA(); - b2Fixture* fixtureB = contact->GetFixtureB(); - if (fixtureA == this || fixtureB == this) - { - contact->FlagForFiltering(); - } - - edge = edge->next; - } - - b2World* world = m_body->GetWorld(); - - if (world == NULL) - { - return; - } - - // Touch each proxy so that new pairs may be created - b2BroadPhase* broadPhase = &world->m_contactManager.m_broadPhase; - for (int32 i = 0; i < m_proxyCount; ++i) - { - broadPhase->TouchProxy(m_proxies[i].proxyId); - } -} - -void b2Fixture::SetSensor(bool sensor) -{ - if (sensor != m_isSensor) - { - m_body->SetAwake(true); - m_isSensor = sensor; - } -} - -void b2Fixture::Dump(int32 bodyIndex) -{ - b2Log(" b2FixtureDef fd;\n"); - b2Log(" fd.friction = %.15lef;\n", m_friction); - b2Log(" fd.restitution = %.15lef;\n", m_restitution); - b2Log(" fd.density = %.15lef;\n", m_density); - b2Log(" fd.isSensor = bool(%d);\n", m_isSensor); - b2Log(" fd.filter.categoryBits = uint16(%d);\n", m_filter.categoryBits); - b2Log(" fd.filter.maskBits = uint16(%d);\n", m_filter.maskBits); - b2Log(" fd.filter.groupIndex = int16(%d);\n", m_filter.groupIndex); - - switch (m_shape->m_type) - { - case b2Shape::e_circle: - { - b2CircleShape* s = (b2CircleShape*)m_shape; - b2Log(" b2CircleShape shape;\n"); - b2Log(" shape.m_radius = %.15lef;\n", s->m_radius); - b2Log(" shape.m_p.Set(%.15lef, %.15lef);\n", s->m_p.x, s->m_p.y); - } - break; - - case b2Shape::e_edge: - { - b2EdgeShape* s = (b2EdgeShape*)m_shape; - b2Log(" b2EdgeShape shape;\n"); - b2Log(" shape.m_radius = %.15lef;\n", s->m_radius); - b2Log(" shape.m_vertex0.Set(%.15lef, %.15lef);\n", s->m_vertex0.x, s->m_vertex0.y); - b2Log(" shape.m_vertex1.Set(%.15lef, %.15lef);\n", s->m_vertex1.x, s->m_vertex1.y); - b2Log(" shape.m_vertex2.Set(%.15lef, %.15lef);\n", s->m_vertex2.x, s->m_vertex2.y); - b2Log(" shape.m_vertex3.Set(%.15lef, %.15lef);\n", s->m_vertex3.x, s->m_vertex3.y); - b2Log(" shape.m_hasVertex0 = bool(%d);\n", s->m_hasVertex0); - b2Log(" shape.m_hasVertex3 = bool(%d);\n", s->m_hasVertex3); - } - break; - - case b2Shape::e_polygon: - { - b2PolygonShape* s = (b2PolygonShape*)m_shape; - b2Log(" b2PolygonShape shape;\n"); - b2Log(" b2Vec2 vs[%d];\n", b2_maxPolygonVertices); - for (int32 i = 0; i < s->m_count; ++i) - { - b2Log(" vs[%d].Set(%.15lef, %.15lef);\n", i, s->m_vertices[i].x, s->m_vertices[i].y); - } - b2Log(" shape.Set(vs, %d);\n", s->m_count); - } - break; - - case b2Shape::e_chain: - { - b2ChainShape* s = (b2ChainShape*)m_shape; - b2Log(" b2ChainShape shape;\n"); - b2Log(" b2Vec2 vs[%d];\n", s->m_count); - for (int32 i = 0; i < s->m_count; ++i) - { - b2Log(" vs[%d].Set(%.15lef, %.15lef);\n", i, s->m_vertices[i].x, s->m_vertices[i].y); - } - b2Log(" shape.CreateChain(vs, %d);\n", s->m_count); - b2Log(" shape.m_prevVertex.Set(%.15lef, %.15lef);\n", s->m_prevVertex.x, s->m_prevVertex.y); - b2Log(" shape.m_nextVertex.Set(%.15lef, %.15lef);\n", s->m_nextVertex.x, s->m_nextVertex.y); - b2Log(" shape.m_hasPrevVertex = bool(%d);\n", s->m_hasPrevVertex); - b2Log(" shape.m_hasNextVertex = bool(%d);\n", s->m_hasNextVertex); - } - break; - - default: - return; - } - - b2Log("\n"); - b2Log(" fd.shape = &shape;\n"); - b2Log("\n"); - b2Log(" bodies[%d]->CreateFixture(&fd);\n", bodyIndex); -} diff --git a/Box2D/Dynamics/b2Fixture.h b/Box2D/Dynamics/b2Fixture.h deleted file mode 100644 index 753127fbc5..0000000000 --- a/Box2D/Dynamics/b2Fixture.h +++ /dev/null @@ -1,345 +0,0 @@ -/* -* 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_FIXTURE_H -#define B2_FIXTURE_H - -#include -#include -#include - -class b2BlockAllocator; -class b2Body; -class b2BroadPhase; -class b2Fixture; - -/// This holds contact filtering data. -struct b2Filter -{ - b2Filter() - { - categoryBits = 0x0001; - maskBits = 0xFFFF; - groupIndex = 0; - } - - /// The collision category bits. Normally you would just set one bit. - uint16 categoryBits; - - /// The collision mask bits. This states the categories that this - /// shape would accept for collision. - uint16 maskBits; - - /// Collision groups allow a certain group of objects to never collide (negative) - /// or always collide (positive). Zero means no collision group. Non-zero group - /// filtering always wins against the mask bits. - int16 groupIndex; -}; - -/// A fixture definition is used to create a fixture. This class defines an -/// abstract fixture definition. You can reuse fixture definitions safely. -struct b2FixtureDef -{ - /// The constructor sets the default fixture definition values. - b2FixtureDef() - { - shape = NULL; - userData = NULL; - friction = 0.2f; - restitution = 0.0f; - density = 0.0f; - isSensor = false; - } - - /// The shape, this must be set. The shape will be cloned, so you - /// can create the shape on the stack. - const b2Shape* shape; - - /// Use this to store application specific fixture data. - void* userData; - - /// The friction coefficient, usually in the range [0,1]. - float32 friction; - - /// The restitution (elasticity) usually in the range [0,1]. - float32 restitution; - - /// The density, usually in kg/m^2. - float32 density; - - /// A sensor shape collects contact information but never generates a collision - /// response. - bool isSensor; - - /// Contact filtering data. - b2Filter filter; -}; - -/// This proxy is used internally to connect fixtures to the broad-phase. -struct b2FixtureProxy -{ - b2AABB aabb; - b2Fixture* fixture; - int32 childIndex; - int32 proxyId; -}; - -/// A fixture is used to attach a shape to a body for collision detection. A fixture -/// inherits its transform from its parent. Fixtures hold additional non-geometric data -/// such as friction, collision filters, etc. -/// Fixtures are created via b2Body::CreateFixture. -/// @warning you cannot reuse fixtures. -class b2Fixture -{ -public: - /// Get the type of the child shape. You can use this to down cast to the concrete shape. - /// @return the shape type. - b2Shape::Type GetType() const; - - /// Get the child shape. You can modify the child shape, however you should not change the - /// number of vertices because this will crash some collision caching mechanisms. - /// Manipulating the shape may lead to non-physical behavior. - b2Shape* GetShape(); - const b2Shape* GetShape() const; - - /// Set if this fixture is a sensor. - void SetSensor(bool sensor); - - /// Is this fixture a sensor (non-solid)? - /// @return the true if the shape is a sensor. - bool IsSensor() const; - - /// Set the contact filtering data. This will not update contacts until the next time - /// step when either parent body is active and awake. - /// This automatically calls Refilter. - void SetFilterData(const b2Filter& filter); - - /// Get the contact filtering data. - const b2Filter& GetFilterData() const; - - /// Call this if you want to establish collision that was previously disabled by b2ContactFilter::ShouldCollide. - void Refilter(); - - /// Get the parent body of this fixture. This is NULL if the fixture is not attached. - /// @return the parent body. - b2Body* GetBody(); - const b2Body* GetBody() const; - - /// Get the next fixture in the parent body's fixture list. - /// @return the next shape. - b2Fixture* GetNext(); - const b2Fixture* GetNext() const; - - /// Get the user data that was assigned in the fixture definition. Use this to - /// store your application specific data. - void* GetUserData() const; - - /// Set the user data. Use this to store your application specific data. - void SetUserData(void* data); - - /// Test a point for containment in this fixture. - /// @param p a point in world coordinates. - bool TestPoint(const b2Vec2& p) const; - - /// Cast a ray against this shape. - /// @param output the ray-cast results. - /// @param input the ray-cast input parameters. - bool RayCast(b2RayCastOutput* output, const b2RayCastInput& input, int32 childIndex) const; - - /// Get the mass data for this fixture. The mass data is based on the density and - /// the shape. The rotational inertia is about the shape's origin. This operation - /// may be expensive. - void GetMassData(b2MassData* massData) const; - - /// Set the density of this fixture. This will _not_ automatically adjust the mass - /// of the body. You must call b2Body::ResetMassData to update the body's mass. - void SetDensity(float32 density); - - /// Get the density of this fixture. - float32 GetDensity() const; - - /// Get the coefficient of friction. - float32 GetFriction() const; - - /// Set the coefficient of friction. This will _not_ change the friction of - /// existing contacts. - void SetFriction(float32 friction); - - /// Get the coefficient of restitution. - float32 GetRestitution() const; - - /// Set the coefficient of restitution. This will _not_ change the restitution of - /// existing contacts. - void SetRestitution(float32 restitution); - - /// Get the fixture's AABB. This AABB may be enlarge and/or stale. - /// If you need a more accurate AABB, compute it using the shape and - /// the body transform. - const b2AABB& GetAABB(int32 childIndex) const; - - /// Dump this fixture to the log file. - void Dump(int32 bodyIndex); - -protected: - - friend class b2Body; - friend class b2World; - friend class b2Contact; - friend class b2ContactManager; - - b2Fixture(); - - // We need separation create/destroy functions from the constructor/destructor because - // the destructor cannot access the allocator (no destructor arguments allowed by C++). - void Create(b2BlockAllocator* allocator, b2Body* body, const b2FixtureDef* def); - void Destroy(b2BlockAllocator* allocator); - - // These support body activation/deactivation. - void CreateProxies(b2BroadPhase* broadPhase, const b2Transform& xf); - void DestroyProxies(b2BroadPhase* broadPhase); - - void Synchronize(b2BroadPhase* broadPhase, const b2Transform& xf1, const b2Transform& xf2); - - float32 m_density; - - b2Fixture* m_next; - b2Body* m_body; - - b2Shape* m_shape; - - float32 m_friction; - float32 m_restitution; - - b2FixtureProxy* m_proxies; - int32 m_proxyCount; - - b2Filter m_filter; - - bool m_isSensor; - - void* m_userData; -}; - -inline b2Shape::Type b2Fixture::GetType() const -{ - return m_shape->GetType(); -} - -inline b2Shape* b2Fixture::GetShape() -{ - return m_shape; -} - -inline const b2Shape* b2Fixture::GetShape() const -{ - return m_shape; -} - -inline bool b2Fixture::IsSensor() const -{ - return m_isSensor; -} - -inline const b2Filter& b2Fixture::GetFilterData() const -{ - return m_filter; -} - -inline void* b2Fixture::GetUserData() const -{ - return m_userData; -} - -inline void b2Fixture::SetUserData(void* data) -{ - m_userData = data; -} - -inline b2Body* b2Fixture::GetBody() -{ - return m_body; -} - -inline const b2Body* b2Fixture::GetBody() const -{ - return m_body; -} - -inline b2Fixture* b2Fixture::GetNext() -{ - return m_next; -} - -inline const b2Fixture* b2Fixture::GetNext() const -{ - return m_next; -} - -inline void b2Fixture::SetDensity(float32 density) -{ - b2Assert(b2IsValid(density) && density >= 0.0f); - m_density = density; -} - -inline float32 b2Fixture::GetDensity() const -{ - return m_density; -} - -inline float32 b2Fixture::GetFriction() const -{ - return m_friction; -} - -inline void b2Fixture::SetFriction(float32 friction) -{ - m_friction = friction; -} - -inline float32 b2Fixture::GetRestitution() const -{ - return m_restitution; -} - -inline void b2Fixture::SetRestitution(float32 restitution) -{ - m_restitution = restitution; -} - -inline bool b2Fixture::TestPoint(const b2Vec2& p) const -{ - return m_shape->TestPoint(m_body->GetTransform(), p); -} - -inline bool b2Fixture::RayCast(b2RayCastOutput* output, const b2RayCastInput& input, int32 childIndex) const -{ - return m_shape->RayCast(output, input, m_body->GetTransform(), childIndex); -} - -inline void b2Fixture::GetMassData(b2MassData* massData) const -{ - m_shape->ComputeMass(massData, m_density); -} - -inline const b2AABB& b2Fixture::GetAABB(int32 childIndex) const -{ - b2Assert(0 <= childIndex && childIndex < m_proxyCount); - return m_proxies[childIndex].aabb; -} - -#endif diff --git a/Box2D/Dynamics/b2Island.cpp b/Box2D/Dynamics/b2Island.cpp deleted file mode 100644 index d46589dee9..0000000000 --- a/Box2D/Dynamics/b2Island.cpp +++ /dev/null @@ -1,539 +0,0 @@ -/* -* Copyright (c) 2006-2011 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 -#include -#include -#include -#include -#include -#include -#include -#include -#include - -/* -Position Correction Notes -========================= -I tried the several algorithms for position correction of the 2D revolute joint. -I looked at these systems: -- simple pendulum (1m diameter sphere on massless 5m stick) with initial angular velocity of 100 rad/s. -- suspension bridge with 30 1m long planks of length 1m. -- multi-link chain with 30 1m long links. - -Here are the algorithms: - -Baumgarte - A fraction of the position error is added to the velocity error. There is no -separate position solver. - -Pseudo Velocities - After the velocity solver and position integration, -the position error, Jacobian, and effective mass are recomputed. Then -the velocity constraints are solved with pseudo velocities and a fraction -of the position error is added to the pseudo velocity error. The pseudo -velocities are initialized to zero and there is no warm-starting. After -the position solver, the pseudo velocities are added to the positions. -This is also called the First Order World method or the Position LCP method. - -Modified Nonlinear Gauss-Seidel (NGS) - Like Pseudo Velocities except the -position error is re-computed for each constraint and the positions are updated -after the constraint is solved. The radius vectors (aka Jacobians) are -re-computed too (otherwise the algorithm has horrible instability). The pseudo -velocity states are not needed because they are effectively zero at the beginning -of each iteration. Since we have the current position error, we allow the -iterations to terminate early if the error becomes smaller than b2_linearSlop. - -Full NGS or just NGS - Like Modified NGS except the effective mass are re-computed -each time a constraint is solved. - -Here are the results: -Baumgarte - this is the cheapest algorithm but it has some stability problems, -especially with the bridge. The chain links separate easily close to the root -and they jitter as they struggle to pull together. This is one of the most common -methods in the field. The big drawback is that the position correction artificially -affects the momentum, thus leading to instabilities and false bounce. I used a -bias factor of 0.2. A larger bias factor makes the bridge less stable, a smaller -factor makes joints and contacts more spongy. - -Pseudo Velocities - the is more stable than the Baumgarte method. The bridge is -stable. However, joints still separate with large angular velocities. Drag the -simple pendulum in a circle quickly and the joint will separate. The chain separates -easily and does not recover. I used a bias factor of 0.2. A larger value lead to -the bridge collapsing when a heavy cube drops on it. - -Modified NGS - this algorithm is better in some ways than Baumgarte and Pseudo -Velocities, but in other ways it is worse. The bridge and chain are much more -stable, but the simple pendulum goes unstable at high angular velocities. - -Full NGS - stable in all tests. The joints display good stiffness. The bridge -still sags, but this is better than infinite forces. - -Recommendations -Pseudo Velocities are not really worthwhile because the bridge and chain cannot -recover from joint separation. In other cases the benefit over Baumgarte is small. - -Modified NGS is not a robust method for the revolute joint due to the violent -instability seen in the simple pendulum. Perhaps it is viable with other constraint -types, especially scalar constraints where the effective mass is a scalar. - -This leaves Baumgarte and Full NGS. Baumgarte has small, but manageable instabilities -and is very fast. I don't think we can escape Baumgarte, especially in highly -demanding cases where high constraint fidelity is not needed. - -Full NGS is robust and easy on the eyes. I recommend this as an option for -higher fidelity simulation and certainly for suspension bridges and long chains. -Full NGS might be a good choice for ragdolls, especially motorized ragdolls where -joint separation can be problematic. The number of NGS iterations can be reduced -for better performance without harming robustness much. - -Each joint in a can be handled differently in the position solver. So I recommend -a system where the user can select the algorithm on a per joint basis. I would -probably default to the slower Full NGS and let the user select the faster -Baumgarte method in performance critical scenarios. -*/ - -/* -Cache Performance - -The Box2D solvers are dominated by cache misses. Data structures are designed -to increase the number of cache hits. Much of misses are due to random access -to body data. The constraint structures are iterated over linearly, which leads -to few cache misses. - -The bodies are not accessed during iteration. Instead read only data, such as -the mass values are stored with the constraints. The mutable data are the constraint -impulses and the bodies velocities/positions. The impulses are held inside the -constraint structures. The body velocities/positions are held in compact, temporary -arrays to increase the number of cache hits. Linear and angular velocity are -stored in a single array since multiple arrays lead to multiple misses. -*/ - -/* -2D Rotation - -R = [cos(theta) -sin(theta)] - [sin(theta) cos(theta) ] - -thetaDot = omega - -Let q1 = cos(theta), q2 = sin(theta). -R = [q1 -q2] - [q2 q1] - -q1Dot = -thetaDot * q2 -q2Dot = thetaDot * q1 - -q1_new = q1_old - dt * w * q2 -q2_new = q2_old + dt * w * q1 -then normalize. - -This might be faster than computing sin+cos. -However, we can compute sin+cos of the same angle fast. -*/ - -b2Island::b2Island( - int32 bodyCapacity, - int32 contactCapacity, - int32 jointCapacity, - b2StackAllocator* allocator, - b2ContactListener* listener) -{ - m_bodyCapacity = bodyCapacity; - m_contactCapacity = contactCapacity; - m_jointCapacity = jointCapacity; - m_bodyCount = 0; - m_contactCount = 0; - m_jointCount = 0; - - m_allocator = allocator; - m_listener = listener; - - m_bodies = (b2Body**)m_allocator->Allocate(bodyCapacity * sizeof(b2Body*)); - m_contacts = (b2Contact**)m_allocator->Allocate(contactCapacity * sizeof(b2Contact*)); - m_joints = (b2Joint**)m_allocator->Allocate(jointCapacity * sizeof(b2Joint*)); - - m_velocities = (b2Velocity*)m_allocator->Allocate(m_bodyCapacity * sizeof(b2Velocity)); - m_positions = (b2Position*)m_allocator->Allocate(m_bodyCapacity * sizeof(b2Position)); -} - -b2Island::~b2Island() -{ - // Warning: the order should reverse the constructor order. - m_allocator->Free(m_positions); - m_allocator->Free(m_velocities); - m_allocator->Free(m_joints); - m_allocator->Free(m_contacts); - m_allocator->Free(m_bodies); -} - -void b2Island::Solve(b2Profile* profile, const b2TimeStep& step, const b2Vec2& gravity, bool allowSleep) -{ - b2Timer timer; - - float32 h = step.dt; - - // Integrate velocities and apply damping. Initialize the body state. - for (int32 i = 0; i < m_bodyCount; ++i) - { - b2Body* b = m_bodies[i]; - - b2Vec2 c = b->m_sweep.c; - float32 a = b->m_sweep.a; - b2Vec2 v = b->m_linearVelocity; - float32 w = b->m_angularVelocity; - - // Store positions for continuous collision. - b->m_sweep.c0 = b->m_sweep.c; - b->m_sweep.a0 = b->m_sweep.a; - - if (b->m_type == b2_dynamicBody) - { - // Integrate velocities. - v += h * (b->m_gravityScale * gravity + b->m_invMass * b->m_force); - w += h * b->m_invI * b->m_torque; - - // Apply damping. - // ODE: dv/dt + c * v = 0 - // Solution: v(t) = v0 * exp(-c * t) - // Time step: v(t + dt) = v0 * exp(-c * (t + dt)) = v0 * exp(-c * t) * exp(-c * dt) = v * exp(-c * dt) - // v2 = exp(-c * dt) * v1 - // Pade approximation: - // v2 = v1 * 1 / (1 + c * dt) - v *= 1.0f / (1.0f + h * b->m_linearDamping); - w *= 1.0f / (1.0f + h * b->m_angularDamping); - } - - m_positions[i].c = c; - m_positions[i].a = a; - m_velocities[i].v = v; - m_velocities[i].w = w; - } - - timer.Reset(); - - // Solver data - b2SolverData solverData; - solverData.step = step; - solverData.positions = m_positions; - solverData.velocities = m_velocities; - - // Initialize velocity constraints. - b2ContactSolverDef contactSolverDef; - contactSolverDef.step = step; - contactSolverDef.contacts = m_contacts; - contactSolverDef.count = m_contactCount; - contactSolverDef.positions = m_positions; - contactSolverDef.velocities = m_velocities; - contactSolverDef.allocator = m_allocator; - - b2ContactSolver contactSolver(&contactSolverDef); - contactSolver.InitializeVelocityConstraints(); - - if (step.warmStarting) - { - contactSolver.WarmStart(); - } - - for (int32 i = 0; i < m_jointCount; ++i) - { - m_joints[i]->InitVelocityConstraints(solverData); - } - - profile->solveInit = timer.GetMilliseconds(); - - // Solve velocity constraints - timer.Reset(); - for (int32 i = 0; i < step.velocityIterations; ++i) - { - for (int32 j = 0; j < m_jointCount; ++j) - { - m_joints[j]->SolveVelocityConstraints(solverData); - } - - contactSolver.SolveVelocityConstraints(); - } - - // Store impulses for warm starting - contactSolver.StoreImpulses(); - profile->solveVelocity = timer.GetMilliseconds(); - - // Integrate positions - for (int32 i = 0; i < m_bodyCount; ++i) - { - b2Vec2 c = m_positions[i].c; - float32 a = m_positions[i].a; - b2Vec2 v = m_velocities[i].v; - float32 w = m_velocities[i].w; - - // Check for large velocities - b2Vec2 translation = h * v; - if (b2Dot(translation, translation) > b2_maxTranslationSquared) - { - float32 ratio = b2_maxTranslation / translation.Length(); - v *= ratio; - } - - float32 rotation = h * w; - if (rotation * rotation > b2_maxRotationSquared) - { - float32 ratio = b2_maxRotation / b2Abs(rotation); - w *= ratio; - } - - // Integrate - c += h * v; - a += h * w; - - m_positions[i].c = c; - m_positions[i].a = a; - m_velocities[i].v = v; - m_velocities[i].w = w; - } - - // Solve position constraints - timer.Reset(); - bool positionSolved = false; - for (int32 i = 0; i < step.positionIterations; ++i) - { - bool contactsOkay = contactSolver.SolvePositionConstraints(); - - bool jointsOkay = true; - for (int32 i = 0; i < m_jointCount; ++i) - { - bool jointOkay = m_joints[i]->SolvePositionConstraints(solverData); - jointsOkay = jointsOkay && jointOkay; - } - - if (contactsOkay && jointsOkay) - { - // Exit early if the position errors are small. - positionSolved = true; - break; - } - } - - // Copy state buffers back to the bodies - for (int32 i = 0; i < m_bodyCount; ++i) - { - b2Body* body = m_bodies[i]; - body->m_sweep.c = m_positions[i].c; - body->m_sweep.a = m_positions[i].a; - body->m_linearVelocity = m_velocities[i].v; - body->m_angularVelocity = m_velocities[i].w; - body->SynchronizeTransform(); - } - - profile->solvePosition = timer.GetMilliseconds(); - - Report(contactSolver.m_velocityConstraints); - - if (allowSleep) - { - float32 minSleepTime = b2_maxFloat; - - const float32 linTolSqr = b2_linearSleepTolerance * b2_linearSleepTolerance; - const float32 angTolSqr = b2_angularSleepTolerance * b2_angularSleepTolerance; - - for (int32 i = 0; i < m_bodyCount; ++i) - { - b2Body* b = m_bodies[i]; - if (b->GetType() == b2_staticBody) - { - continue; - } - - if ((b->m_flags & b2Body::e_autoSleepFlag) == 0 || - b->m_angularVelocity * b->m_angularVelocity > angTolSqr || - b2Dot(b->m_linearVelocity, b->m_linearVelocity) > linTolSqr) - { - b->m_sleepTime = 0.0f; - minSleepTime = 0.0f; - } - else - { - b->m_sleepTime += h; - minSleepTime = b2Min(minSleepTime, b->m_sleepTime); - } - } - - if (minSleepTime >= b2_timeToSleep && positionSolved) - { - for (int32 i = 0; i < m_bodyCount; ++i) - { - b2Body* b = m_bodies[i]; - b->SetAwake(false); - } - } - } -} - -void b2Island::SolveTOI(const b2TimeStep& subStep, int32 toiIndexA, int32 toiIndexB) -{ - b2Assert(toiIndexA < m_bodyCount); - b2Assert(toiIndexB < m_bodyCount); - - // Initialize the body state. - for (int32 i = 0; i < m_bodyCount; ++i) - { - b2Body* b = m_bodies[i]; - m_positions[i].c = b->m_sweep.c; - m_positions[i].a = b->m_sweep.a; - m_velocities[i].v = b->m_linearVelocity; - m_velocities[i].w = b->m_angularVelocity; - } - - b2ContactSolverDef contactSolverDef; - contactSolverDef.contacts = m_contacts; - contactSolverDef.count = m_contactCount; - contactSolverDef.allocator = m_allocator; - contactSolverDef.step = subStep; - contactSolverDef.positions = m_positions; - contactSolverDef.velocities = m_velocities; - b2ContactSolver contactSolver(&contactSolverDef); - - // Solve position constraints. - for (int32 i = 0; i < subStep.positionIterations; ++i) - { - bool contactsOkay = contactSolver.SolveTOIPositionConstraints(toiIndexA, toiIndexB); - if (contactsOkay) - { - break; - } - } - -#if 0 - // Is the new position really safe? - for (int32 i = 0; i < m_contactCount; ++i) - { - b2Contact* c = m_contacts[i]; - b2Fixture* fA = c->GetFixtureA(); - b2Fixture* fB = c->GetFixtureB(); - - b2Body* bA = fA->GetBody(); - b2Body* bB = fB->GetBody(); - - int32 indexA = c->GetChildIndexA(); - int32 indexB = c->GetChildIndexB(); - - b2DistanceInput input; - input.proxyA.Set(fA->GetShape(), indexA); - input.proxyB.Set(fB->GetShape(), indexB); - input.transformA = bA->GetTransform(); - input.transformB = bB->GetTransform(); - input.useRadii = false; - - b2DistanceOutput output; - b2SimplexCache cache; - cache.count = 0; - b2Distance(&output, &cache, &input); - - if (output.distance == 0 || cache.count == 3) - { - cache.count += 0; - } - } -#endif - - // Leap of faith to new safe state. - m_bodies[toiIndexA]->m_sweep.c0 = m_positions[toiIndexA].c; - m_bodies[toiIndexA]->m_sweep.a0 = m_positions[toiIndexA].a; - m_bodies[toiIndexB]->m_sweep.c0 = m_positions[toiIndexB].c; - m_bodies[toiIndexB]->m_sweep.a0 = m_positions[toiIndexB].a; - - // No warm starting is needed for TOI events because warm - // starting impulses were applied in the discrete solver. - contactSolver.InitializeVelocityConstraints(); - - // Solve velocity constraints. - for (int32 i = 0; i < subStep.velocityIterations; ++i) - { - contactSolver.SolveVelocityConstraints(); - } - - // Don't store the TOI contact forces for warm starting - // because they can be quite large. - - float32 h = subStep.dt; - - // Integrate positions - for (int32 i = 0; i < m_bodyCount; ++i) - { - b2Vec2 c = m_positions[i].c; - float32 a = m_positions[i].a; - b2Vec2 v = m_velocities[i].v; - float32 w = m_velocities[i].w; - - // Check for large velocities - b2Vec2 translation = h * v; - if (b2Dot(translation, translation) > b2_maxTranslationSquared) - { - float32 ratio = b2_maxTranslation / translation.Length(); - v *= ratio; - } - - float32 rotation = h * w; - if (rotation * rotation > b2_maxRotationSquared) - { - float32 ratio = b2_maxRotation / b2Abs(rotation); - w *= ratio; - } - - // Integrate - c += h * v; - a += h * w; - - m_positions[i].c = c; - m_positions[i].a = a; - m_velocities[i].v = v; - m_velocities[i].w = w; - - // Sync bodies - b2Body* body = m_bodies[i]; - body->m_sweep.c = c; - body->m_sweep.a = a; - body->m_linearVelocity = v; - body->m_angularVelocity = w; - body->SynchronizeTransform(); - } - - Report(contactSolver.m_velocityConstraints); -} - -void b2Island::Report(const b2ContactVelocityConstraint* constraints) -{ - if (m_listener == NULL) - { - return; - } - - for (int32 i = 0; i < m_contactCount; ++i) - { - b2Contact* c = m_contacts[i]; - - const b2ContactVelocityConstraint* vc = constraints + i; - - b2ContactImpulse impulse; - impulse.count = vc->pointCount; - for (int32 j = 0; j < vc->pointCount; ++j) - { - impulse.normalImpulses[j] = vc->points[j].normalImpulse; - impulse.tangentImpulses[j] = vc->points[j].tangentImpulse; - } - - m_listener->PostSolve(c, &impulse); - } -} diff --git a/Box2D/Dynamics/b2Island.h b/Box2D/Dynamics/b2Island.h deleted file mode 100644 index 4a814c463e..0000000000 --- a/Box2D/Dynamics/b2Island.h +++ /dev/null @@ -1,93 +0,0 @@ -/* -* 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_ISLAND_H -#define B2_ISLAND_H - -#include -#include -#include - -class b2Contact; -class b2Joint; -class b2StackAllocator; -class b2ContactListener; -struct b2ContactVelocityConstraint; -struct b2Profile; - -/// This is an internal class. -class b2Island -{ -public: - b2Island(int32 bodyCapacity, int32 contactCapacity, int32 jointCapacity, - b2StackAllocator* allocator, b2ContactListener* listener); - ~b2Island(); - - void Clear() - { - m_bodyCount = 0; - m_contactCount = 0; - m_jointCount = 0; - } - - void Solve(b2Profile* profile, const b2TimeStep& step, const b2Vec2& gravity, bool allowSleep); - - void SolveTOI(const b2TimeStep& subStep, int32 toiIndexA, int32 toiIndexB); - - void Add(b2Body* body) - { - b2Assert(m_bodyCount < m_bodyCapacity); - body->m_islandIndex = m_bodyCount; - m_bodies[m_bodyCount] = body; - ++m_bodyCount; - } - - void Add(b2Contact* contact) - { - b2Assert(m_contactCount < m_contactCapacity); - m_contacts[m_contactCount++] = contact; - } - - void Add(b2Joint* joint) - { - b2Assert(m_jointCount < m_jointCapacity); - m_joints[m_jointCount++] = joint; - } - - void Report(const b2ContactVelocityConstraint* constraints); - - b2StackAllocator* m_allocator; - b2ContactListener* m_listener; - - b2Body** m_bodies; - b2Contact** m_contacts; - b2Joint** m_joints; - - b2Position* m_positions; - b2Velocity* m_velocities; - - int32 m_bodyCount; - int32 m_jointCount; - int32 m_contactCount; - - int32 m_bodyCapacity; - int32 m_contactCapacity; - int32 m_jointCapacity; -}; - -#endif diff --git a/Box2D/Dynamics/b2TimeStep.h b/Box2D/Dynamics/b2TimeStep.h deleted file mode 100644 index a3727624cd..0000000000 --- a/Box2D/Dynamics/b2TimeStep.h +++ /dev/null @@ -1,70 +0,0 @@ -/* -* Copyright (c) 2006-2011 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_TIME_STEP_H -#define B2_TIME_STEP_H - -#include - -/// Profiling data. Times are in milliseconds. -struct b2Profile -{ - float32 step; - float32 collide; - float32 solve; - float32 solveInit; - float32 solveVelocity; - float32 solvePosition; - float32 broadphase; - float32 solveTOI; -}; - -/// This is an internal structure. -struct b2TimeStep -{ - float32 dt; // time step - float32 inv_dt; // inverse time step (0 if dt == 0). - float32 dtRatio; // dt * inv_dt0 - int32 velocityIterations; - int32 positionIterations; - bool warmStarting; -}; - -/// This is an internal structure. -struct b2Position -{ - b2Vec2 c; - float32 a; -}; - -/// This is an internal structure. -struct b2Velocity -{ - b2Vec2 v; - float32 w; -}; - -/// Solver Data -struct b2SolverData -{ - b2TimeStep step; - b2Position* positions; - b2Velocity* velocities; -}; - -#endif diff --git a/Box2D/Dynamics/b2World.cpp b/Box2D/Dynamics/b2World.cpp deleted file mode 100644 index a3a6a1e413..0000000000 --- a/Box2D/Dynamics/b2World.cpp +++ /dev/null @@ -1,1339 +0,0 @@ -/* -* Copyright (c) 2006-2011 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 -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -b2World::b2World(const b2Vec2& gravity) -{ - m_destructionListener = NULL; - m_debugDraw = NULL; - - m_bodyList = NULL; - m_jointList = NULL; - - m_bodyCount = 0; - m_jointCount = 0; - - m_warmStarting = true; - m_continuousPhysics = true; - m_subStepping = false; - - m_stepComplete = true; - - m_allowSleep = true; - m_gravity = gravity; - - m_flags = e_clearForces; - - m_inv_dt0 = 0.0f; - - m_contactManager.m_allocator = &m_blockAllocator; - - memset(&m_profile, 0, sizeof(b2Profile)); -} - -b2World::~b2World() -{ - // Some shapes allocate using b2Alloc. - b2Body* b = m_bodyList; - while (b) - { - b2Body* bNext = b->m_next; - - b2Fixture* f = b->m_fixtureList; - while (f) - { - b2Fixture* fNext = f->m_next; - f->m_proxyCount = 0; - f->Destroy(&m_blockAllocator); - f = fNext; - } - - b = bNext; - } -} - -void b2World::SetDestructionListener(b2DestructionListener* listener) -{ - m_destructionListener = listener; -} - -void b2World::SetContactFilter(b2ContactFilter* filter) -{ - m_contactManager.m_contactFilter = filter; -} - -void b2World::SetContactListener(b2ContactListener* listener) -{ - m_contactManager.m_contactListener = listener; -} - -void b2World::SetDebugDraw(b2Draw* debugDraw) -{ - m_debugDraw = debugDraw; -} - -b2Body* b2World::CreateBody(const b2BodyDef* def) -{ - b2Assert(IsLocked() == false); - if (IsLocked()) - { - return NULL; - } - - void* mem = m_blockAllocator.Allocate(sizeof(b2Body)); - b2Body* b = new (mem) b2Body(def, this); - - // Add to world doubly linked list. - b->m_prev = NULL; - b->m_next = m_bodyList; - if (m_bodyList) - { - m_bodyList->m_prev = b; - } - m_bodyList = b; - ++m_bodyCount; - - return b; -} - -void b2World::DestroyBody(b2Body* b) -{ - b2Assert(m_bodyCount > 0); - b2Assert(IsLocked() == false); - if (IsLocked()) - { - return; - } - - // Delete the attached joints. - b2JointEdge* je = b->m_jointList; - while (je) - { - b2JointEdge* je0 = je; - je = je->next; - - if (m_destructionListener) - { - m_destructionListener->SayGoodbye(je0->joint); - } - - DestroyJoint(je0->joint); - - b->m_jointList = je; - } - b->m_jointList = NULL; - - // Delete the attached contacts. - b2ContactEdge* ce = b->m_contactList; - while (ce) - { - b2ContactEdge* ce0 = ce; - ce = ce->next; - m_contactManager.Destroy(ce0->contact); - } - b->m_contactList = NULL; - - // Delete the attached fixtures. This destroys broad-phase proxies. - b2Fixture* f = b->m_fixtureList; - while (f) - { - b2Fixture* f0 = f; - f = f->m_next; - - if (m_destructionListener) - { - m_destructionListener->SayGoodbye(f0); - } - - f0->DestroyProxies(&m_contactManager.m_broadPhase); - f0->Destroy(&m_blockAllocator); - f0->~b2Fixture(); - m_blockAllocator.Free(f0, sizeof(b2Fixture)); - - b->m_fixtureList = f; - b->m_fixtureCount -= 1; - } - b->m_fixtureList = NULL; - b->m_fixtureCount = 0; - - // Remove world body list. - if (b->m_prev) - { - b->m_prev->m_next = b->m_next; - } - - if (b->m_next) - { - b->m_next->m_prev = b->m_prev; - } - - if (b == m_bodyList) - { - m_bodyList = b->m_next; - } - - --m_bodyCount; - b->~b2Body(); - m_blockAllocator.Free(b, sizeof(b2Body)); -} - -b2Joint* b2World::CreateJoint(const b2JointDef* def) -{ - b2Assert(IsLocked() == false); - if (IsLocked()) - { - return NULL; - } - - b2Joint* j = b2Joint::Create(def, &m_blockAllocator); - - // Connect to the world list. - j->m_prev = NULL; - j->m_next = m_jointList; - if (m_jointList) - { - m_jointList->m_prev = j; - } - m_jointList = j; - ++m_jointCount; - - // Connect to the bodies' doubly linked lists. - j->m_edgeA.joint = j; - j->m_edgeA.other = j->m_bodyB; - j->m_edgeA.prev = NULL; - j->m_edgeA.next = j->m_bodyA->m_jointList; - if (j->m_bodyA->m_jointList) j->m_bodyA->m_jointList->prev = &j->m_edgeA; - j->m_bodyA->m_jointList = &j->m_edgeA; - - j->m_edgeB.joint = j; - j->m_edgeB.other = j->m_bodyA; - j->m_edgeB.prev = NULL; - j->m_edgeB.next = j->m_bodyB->m_jointList; - if (j->m_bodyB->m_jointList) j->m_bodyB->m_jointList->prev = &j->m_edgeB; - j->m_bodyB->m_jointList = &j->m_edgeB; - - b2Body* bodyA = def->bodyA; - b2Body* bodyB = def->bodyB; - - // If the joint prevents collisions, then flag any contacts for filtering. - if (def->collideConnected == false) - { - b2ContactEdge* edge = bodyB->GetContactList(); - while (edge) - { - if (edge->other == bodyA) - { - // Flag the contact for filtering at the next time step (where either - // body is awake). - edge->contact->FlagForFiltering(); - } - - edge = edge->next; - } - } - - // Note: creating a joint doesn't wake the bodies. - - return j; -} - -void b2World::DestroyJoint(b2Joint* j) -{ - b2Assert(IsLocked() == false); - if (IsLocked()) - { - return; - } - - bool collideConnected = j->m_collideConnected; - - // Remove from the doubly linked list. - if (j->m_prev) - { - j->m_prev->m_next = j->m_next; - } - - if (j->m_next) - { - j->m_next->m_prev = j->m_prev; - } - - if (j == m_jointList) - { - m_jointList = j->m_next; - } - - // Disconnect from island graph. - b2Body* bodyA = j->m_bodyA; - b2Body* bodyB = j->m_bodyB; - - // Wake up connected bodies. - bodyA->SetAwake(true); - bodyB->SetAwake(true); - - // Remove from body 1. - if (j->m_edgeA.prev) - { - j->m_edgeA.prev->next = j->m_edgeA.next; - } - - if (j->m_edgeA.next) - { - j->m_edgeA.next->prev = j->m_edgeA.prev; - } - - if (&j->m_edgeA == bodyA->m_jointList) - { - bodyA->m_jointList = j->m_edgeA.next; - } - - j->m_edgeA.prev = NULL; - j->m_edgeA.next = NULL; - - // Remove from body 2 - if (j->m_edgeB.prev) - { - j->m_edgeB.prev->next = j->m_edgeB.next; - } - - if (j->m_edgeB.next) - { - j->m_edgeB.next->prev = j->m_edgeB.prev; - } - - if (&j->m_edgeB == bodyB->m_jointList) - { - bodyB->m_jointList = j->m_edgeB.next; - } - - j->m_edgeB.prev = NULL; - j->m_edgeB.next = NULL; - - b2Joint::Destroy(j, &m_blockAllocator); - - b2Assert(m_jointCount > 0); - --m_jointCount; - - // If the joint prevents collisions, then flag any contacts for filtering. - if (collideConnected == false) - { - b2ContactEdge* edge = bodyB->GetContactList(); - while (edge) - { - if (edge->other == bodyA) - { - // Flag the contact for filtering at the next time step (where either - // body is awake). - edge->contact->FlagForFiltering(); - } - - edge = edge->next; - } - } -} - -// -void b2World::SetAllowSleeping(bool flag) -{ - if (flag == m_allowSleep) - { - return; - } - - m_allowSleep = flag; - if (m_allowSleep == false) - { - for (b2Body* b = m_bodyList; b; b = b->m_next) - { - b->SetAwake(true); - } - } -} - -// Find islands, integrate and solve constraints, solve position constraints -void b2World::Solve(const b2TimeStep& step) -{ - m_profile.solveInit = 0.0f; - m_profile.solveVelocity = 0.0f; - m_profile.solvePosition = 0.0f; - - // Size the island for the worst case. - b2Island island(m_bodyCount, - m_contactManager.m_contactCount, - m_jointCount, - &m_stackAllocator, - m_contactManager.m_contactListener); - - // Clear all the island flags. - for (b2Body* b = m_bodyList; b; b = b->m_next) - { - b->m_flags &= ~b2Body::e_islandFlag; - } - for (b2Contact* c = m_contactManager.m_contactList; c; c = c->m_next) - { - c->m_flags &= ~b2Contact::e_islandFlag; - } - for (b2Joint* j = m_jointList; j; j = j->m_next) - { - j->m_islandFlag = false; - } - - // Build and simulate all awake islands. - int32 stackSize = m_bodyCount; - b2Body** stack = (b2Body**)m_stackAllocator.Allocate(stackSize * sizeof(b2Body*)); - for (b2Body* seed = m_bodyList; seed; seed = seed->m_next) - { - if (seed->m_flags & b2Body::e_islandFlag) - { - continue; - } - - if (seed->IsAwake() == false || seed->IsActive() == false) - { - continue; - } - - // The seed can be dynamic or kinematic. - if (seed->GetType() == b2_staticBody) - { - continue; - } - - // Reset island and stack. - island.Clear(); - int32 stackCount = 0; - stack[stackCount++] = seed; - seed->m_flags |= b2Body::e_islandFlag; - - // Perform a depth first search (DFS) on the constraint graph. - while (stackCount > 0) - { - // Grab the next body off the stack and add it to the island. - b2Body* b = stack[--stackCount]; - b2Assert(b->IsActive() == true); - island.Add(b); - - // Make sure the body is awake. - b->SetAwake(true); - - // To keep islands as small as possible, we don't - // propagate islands across static bodies. - if (b->GetType() == b2_staticBody) - { - continue; - } - - // Search all contacts connected to this body. - for (b2ContactEdge* ce = b->m_contactList; ce; ce = ce->next) - { - b2Contact* contact = ce->contact; - - // Has this contact already been added to an island? - if (contact->m_flags & b2Contact::e_islandFlag) - { - continue; - } - - // Is this contact solid and touching? - if (contact->IsEnabled() == false || - contact->IsTouching() == false) - { - continue; - } - - // Skip sensors. - bool sensorA = contact->m_fixtureA->m_isSensor; - bool sensorB = contact->m_fixtureB->m_isSensor; - if (sensorA || sensorB) - { - continue; - } - - island.Add(contact); - contact->m_flags |= b2Contact::e_islandFlag; - - b2Body* other = ce->other; - - // Was the other body already added to this island? - if (other->m_flags & b2Body::e_islandFlag) - { - continue; - } - - b2Assert(stackCount < stackSize); - stack[stackCount++] = other; - other->m_flags |= b2Body::e_islandFlag; - } - - // Search all joints connect to this body. - for (b2JointEdge* je = b->m_jointList; je; je = je->next) - { - if (je->joint->m_islandFlag == true) - { - continue; - } - - b2Body* other = je->other; - - // Don't simulate joints connected to inactive bodies. - if (other->IsActive() == false) - { - continue; - } - - island.Add(je->joint); - je->joint->m_islandFlag = true; - - if (other->m_flags & b2Body::e_islandFlag) - { - continue; - } - - b2Assert(stackCount < stackSize); - stack[stackCount++] = other; - other->m_flags |= b2Body::e_islandFlag; - } - } - - b2Profile profile; - island.Solve(&profile, step, m_gravity, m_allowSleep); - m_profile.solveInit += profile.solveInit; - m_profile.solveVelocity += profile.solveVelocity; - m_profile.solvePosition += profile.solvePosition; - - // Post solve cleanup. - for (int32 i = 0; i < island.m_bodyCount; ++i) - { - // Allow static bodies to participate in other islands. - b2Body* b = island.m_bodies[i]; - if (b->GetType() == b2_staticBody) - { - b->m_flags &= ~b2Body::e_islandFlag; - } - } - } - - m_stackAllocator.Free(stack); - - { - b2Timer timer; - // Synchronize fixtures, check for out of range bodies. - for (b2Body* b = m_bodyList; b; b = b->GetNext()) - { - // If a body was not in an island then it did not move. - if ((b->m_flags & b2Body::e_islandFlag) == 0) - { - continue; - } - - if (b->GetType() == b2_staticBody) - { - continue; - } - - // Update fixtures (for broad-phase). - b->SynchronizeFixtures(); - } - - // Look for new contacts. - m_contactManager.FindNewContacts(); - m_profile.broadphase = timer.GetMilliseconds(); - } -} - -// Find TOI contacts and solve them. -void b2World::SolveTOI(const b2TimeStep& step) -{ - b2Island island(2 * b2_maxTOIContacts, b2_maxTOIContacts, 0, &m_stackAllocator, m_contactManager.m_contactListener); - - if (m_stepComplete) - { - for (b2Body* b = m_bodyList; b; b = b->m_next) - { - b->m_flags &= ~b2Body::e_islandFlag; - b->m_sweep.alpha0 = 0.0f; - } - - for (b2Contact* c = m_contactManager.m_contactList; c; c = c->m_next) - { - // Invalidate TOI - c->m_flags &= ~(b2Contact::e_toiFlag | b2Contact::e_islandFlag); - c->m_toiCount = 0; - c->m_toi = 1.0f; - } - } - - // Find TOI events and solve them. - for (;;) - { - // Find the first TOI. - b2Contact* minContact = NULL; - float32 minAlpha = 1.0f; - - for (b2Contact* c = m_contactManager.m_contactList; c; c = c->m_next) - { - // Is this contact disabled? - if (c->IsEnabled() == false) - { - continue; - } - - // Prevent excessive sub-stepping. - if (c->m_toiCount > b2_maxSubSteps) - { - continue; - } - - float32 alpha = 1.0f; - if (c->m_flags & b2Contact::e_toiFlag) - { - // This contact has a valid cached TOI. - alpha = c->m_toi; - } - else - { - b2Fixture* fA = c->GetFixtureA(); - b2Fixture* fB = c->GetFixtureB(); - - // Is there a sensor? - if (fA->IsSensor() || fB->IsSensor()) - { - continue; - } - - b2Body* bA = fA->GetBody(); - b2Body* bB = fB->GetBody(); - - b2BodyType typeA = bA->m_type; - b2BodyType typeB = bB->m_type; - b2Assert(typeA == b2_dynamicBody || typeB == b2_dynamicBody); - - bool activeA = bA->IsAwake() && typeA != b2_staticBody; - bool activeB = bB->IsAwake() && typeB != b2_staticBody; - - // Is at least one body active (awake and dynamic or kinematic)? - if (activeA == false && activeB == false) - { - continue; - } - - bool collideA = bA->IsBullet() || typeA != b2_dynamicBody; - bool collideB = bB->IsBullet() || typeB != b2_dynamicBody; - - // Are these two non-bullet dynamic bodies? - if (collideA == false && collideB == false) - { - continue; - } - - // Compute the TOI for this contact. - // Put the sweeps onto the same time interval. - float32 alpha0 = bA->m_sweep.alpha0; - - if (bA->m_sweep.alpha0 < bB->m_sweep.alpha0) - { - alpha0 = bB->m_sweep.alpha0; - bA->m_sweep.Advance(alpha0); - } - else if (bB->m_sweep.alpha0 < bA->m_sweep.alpha0) - { - alpha0 = bA->m_sweep.alpha0; - bB->m_sweep.Advance(alpha0); - } - - b2Assert(alpha0 < 1.0f); - - int32 indexA = c->GetChildIndexA(); - int32 indexB = c->GetChildIndexB(); - - // Compute the time of impact in interval [0, minTOI] - b2TOIInput input; - input.proxyA.Set(fA->GetShape(), indexA); - input.proxyB.Set(fB->GetShape(), indexB); - input.sweepA = bA->m_sweep; - input.sweepB = bB->m_sweep; - input.tMax = 1.0f; - - b2TOIOutput output; - b2TimeOfImpact(&output, &input); - - // Beta is the fraction of the remaining portion of the . - float32 beta = output.t; - if (output.state == b2TOIOutput::e_touching) - { - alpha = b2Min(alpha0 + (1.0f - alpha0) * beta, 1.0f); - } - else - { - alpha = 1.0f; - } - - c->m_toi = alpha; - c->m_flags |= b2Contact::e_toiFlag; - } - - if (alpha < minAlpha) - { - // This is the minimum TOI found so far. - minContact = c; - minAlpha = alpha; - } - } - - if (minContact == NULL || 1.0f - 10.0f * b2_epsilon < minAlpha) - { - // No more TOI events. Done! - m_stepComplete = true; - break; - } - - // Advance the bodies to the TOI. - b2Fixture* fA = minContact->GetFixtureA(); - b2Fixture* fB = minContact->GetFixtureB(); - b2Body* bA = fA->GetBody(); - b2Body* bB = fB->GetBody(); - - b2Sweep backup1 = bA->m_sweep; - b2Sweep backup2 = bB->m_sweep; - - bA->Advance(minAlpha); - bB->Advance(minAlpha); - - // The TOI contact likely has some new contact points. - minContact->Update(m_contactManager.m_contactListener); - minContact->m_flags &= ~b2Contact::e_toiFlag; - ++minContact->m_toiCount; - - // Is the contact solid? - if (minContact->IsEnabled() == false || minContact->IsTouching() == false) - { - // Restore the sweeps. - minContact->SetEnabled(false); - bA->m_sweep = backup1; - bB->m_sweep = backup2; - bA->SynchronizeTransform(); - bB->SynchronizeTransform(); - continue; - } - - bA->SetAwake(true); - bB->SetAwake(true); - - // Build the island - island.Clear(); - island.Add(bA); - island.Add(bB); - island.Add(minContact); - - bA->m_flags |= b2Body::e_islandFlag; - bB->m_flags |= b2Body::e_islandFlag; - minContact->m_flags |= b2Contact::e_islandFlag; - - // Get contacts on bodyA and bodyB. - b2Body* bodies[2] = {bA, bB}; - for (int32 i = 0; i < 2; ++i) - { - b2Body* body = bodies[i]; - if (body->m_type == b2_dynamicBody) - { - for (b2ContactEdge* ce = body->m_contactList; ce; ce = ce->next) - { - if (island.m_bodyCount == island.m_bodyCapacity) - { - break; - } - - if (island.m_contactCount == island.m_contactCapacity) - { - break; - } - - b2Contact* contact = ce->contact; - - // Has this contact already been added to the island? - if (contact->m_flags & b2Contact::e_islandFlag) - { - continue; - } - - // Only add static, kinematic, or bullet bodies. - b2Body* other = ce->other; - if (other->m_type == b2_dynamicBody && - body->IsBullet() == false && other->IsBullet() == false) - { - continue; - } - - // Skip sensors. - bool sensorA = contact->m_fixtureA->m_isSensor; - bool sensorB = contact->m_fixtureB->m_isSensor; - if (sensorA || sensorB) - { - continue; - } - - // Tentatively advance the body to the TOI. - b2Sweep backup = other->m_sweep; - if ((other->m_flags & b2Body::e_islandFlag) == 0) - { - other->Advance(minAlpha); - } - - // Update the contact points - contact->Update(m_contactManager.m_contactListener); - - // Was the contact disabled by the user? - if (contact->IsEnabled() == false) - { - other->m_sweep = backup; - other->SynchronizeTransform(); - continue; - } - - // Are there contact points? - if (contact->IsTouching() == false) - { - other->m_sweep = backup; - other->SynchronizeTransform(); - continue; - } - - // Add the contact to the island - contact->m_flags |= b2Contact::e_islandFlag; - island.Add(contact); - - // Has the other body already been added to the island? - if (other->m_flags & b2Body::e_islandFlag) - { - continue; - } - - // Add the other body to the island. - other->m_flags |= b2Body::e_islandFlag; - - if (other->m_type != b2_staticBody) - { - other->SetAwake(true); - } - - island.Add(other); - } - } - } - - b2TimeStep subStep; - subStep.dt = (1.0f - minAlpha) * step.dt; - subStep.inv_dt = 1.0f / subStep.dt; - subStep.dtRatio = 1.0f; - subStep.positionIterations = 20; - subStep.velocityIterations = step.velocityIterations; - subStep.warmStarting = false; - island.SolveTOI(subStep, bA->m_islandIndex, bB->m_islandIndex); - - // Reset island flags and synchronize broad-phase proxies. - for (int32 i = 0; i < island.m_bodyCount; ++i) - { - b2Body* body = island.m_bodies[i]; - body->m_flags &= ~b2Body::e_islandFlag; - - if (body->m_type != b2_dynamicBody) - { - continue; - } - - body->SynchronizeFixtures(); - - // Invalidate all contact TOIs on this displaced body. - for (b2ContactEdge* ce = body->m_contactList; ce; ce = ce->next) - { - ce->contact->m_flags &= ~(b2Contact::e_toiFlag | b2Contact::e_islandFlag); - } - } - - // Commit fixture proxy movements to the broad-phase so that new contacts are created. - // Also, some contacts can be destroyed. - m_contactManager.FindNewContacts(); - - if (m_subStepping) - { - m_stepComplete = false; - break; - } - } -} - -void b2World::Step(float32 dt, int32 velocityIterations, int32 positionIterations) -{ - b2Timer stepTimer; - - // If new fixtures were added, we need to find the new contacts. - if (m_flags & e_newFixture) - { - m_contactManager.FindNewContacts(); - m_flags &= ~e_newFixture; - } - - m_flags |= e_locked; - - b2TimeStep step; - step.dt = dt; - step.velocityIterations = velocityIterations; - step.positionIterations = positionIterations; - if (dt > 0.0f) - { - step.inv_dt = 1.0f / dt; - } - else - { - step.inv_dt = 0.0f; - } - - step.dtRatio = m_inv_dt0 * dt; - - step.warmStarting = m_warmStarting; - - // Update contacts. This is where some contacts are destroyed. - { - b2Timer timer; - m_contactManager.Collide(); - m_profile.collide = timer.GetMilliseconds(); - } - - // Integrate velocities, solve velocity constraints, and integrate positions. - if (m_stepComplete && step.dt > 0.0f) - { - b2Timer timer; - Solve(step); - m_profile.solve = timer.GetMilliseconds(); - } - - // Handle TOI events. - if (m_continuousPhysics && step.dt > 0.0f) - { - b2Timer timer; - SolveTOI(step); - m_profile.solveTOI = timer.GetMilliseconds(); - } - - if (step.dt > 0.0f) - { - m_inv_dt0 = step.inv_dt; - } - - if (m_flags & e_clearForces) - { - ClearForces(); - } - - m_flags &= ~e_locked; - - m_profile.step = stepTimer.GetMilliseconds(); -} - -void b2World::ClearForces() -{ - for (b2Body* body = m_bodyList; body; body = body->GetNext()) - { - body->m_force.SetZero(); - body->m_torque = 0.0f; - } -} - -struct b2WorldQueryWrapper -{ - bool QueryCallback(int32 proxyId) - { - b2FixtureProxy* proxy = (b2FixtureProxy*)broadPhase->GetUserData(proxyId); - return callback->ReportFixture(proxy->fixture); - } - - const b2BroadPhase* broadPhase; - b2QueryCallback* callback; -}; - -void b2World::QueryAABB(b2QueryCallback* callback, const b2AABB& aabb) const -{ - b2WorldQueryWrapper wrapper; - wrapper.broadPhase = &m_contactManager.m_broadPhase; - wrapper.callback = callback; - m_contactManager.m_broadPhase.Query(&wrapper, aabb); -} - -struct b2WorldRayCastWrapper -{ - float32 RayCastCallback(const b2RayCastInput& input, int32 proxyId) - { - void* userData = broadPhase->GetUserData(proxyId); - b2FixtureProxy* proxy = (b2FixtureProxy*)userData; - b2Fixture* fixture = proxy->fixture; - int32 index = proxy->childIndex; - b2RayCastOutput output; - bool hit = fixture->RayCast(&output, input, index); - - if (hit) - { - float32 fraction = output.fraction; - b2Vec2 point = (1.0f - fraction) * input.p1 + fraction * input.p2; - return callback->ReportFixture(fixture, point, output.normal, fraction); - } - - return input.maxFraction; - } - - const b2BroadPhase* broadPhase; - b2RayCastCallback* callback; -}; - -void b2World::RayCast(b2RayCastCallback* callback, const b2Vec2& point1, const b2Vec2& point2) const -{ - b2WorldRayCastWrapper wrapper; - wrapper.broadPhase = &m_contactManager.m_broadPhase; - wrapper.callback = callback; - b2RayCastInput input; - input.maxFraction = 1.0f; - input.p1 = point1; - input.p2 = point2; - m_contactManager.m_broadPhase.RayCast(&wrapper, input); -} - -void b2World::DrawShape(b2Fixture* fixture, const b2Transform& xf, const b2Color& color) -{ - switch (fixture->GetType()) - { - case b2Shape::e_circle: - { - b2CircleShape* circle = (b2CircleShape*)fixture->GetShape(); - - b2Vec2 center = b2Mul(xf, circle->m_p); - float32 radius = circle->m_radius; - b2Vec2 axis = b2Mul(xf.q, b2Vec2(1.0f, 0.0f)); - - m_debugDraw->DrawSolidCircle(center, radius, axis, color); - } - break; - - case b2Shape::e_edge: - { - b2EdgeShape* edge = (b2EdgeShape*)fixture->GetShape(); - b2Vec2 v1 = b2Mul(xf, edge->m_vertex1); - b2Vec2 v2 = b2Mul(xf, edge->m_vertex2); - m_debugDraw->DrawSegment(v1, v2, color); - } - break; - - case b2Shape::e_chain: - { - b2ChainShape* chain = (b2ChainShape*)fixture->GetShape(); - int32 count = chain->m_count; - const b2Vec2* vertices = chain->m_vertices; - - b2Vec2 v1 = b2Mul(xf, vertices[0]); - for (int32 i = 1; i < count; ++i) - { - b2Vec2 v2 = b2Mul(xf, vertices[i]); - m_debugDraw->DrawSegment(v1, v2, color); - m_debugDraw->DrawCircle(v1, 0.05f, color); - v1 = v2; - } - } - break; - - case b2Shape::e_polygon: - { - b2PolygonShape* poly = (b2PolygonShape*)fixture->GetShape(); - int32 vertexCount = poly->m_count; - b2Assert(vertexCount <= b2_maxPolygonVertices); - b2Vec2 vertices[b2_maxPolygonVertices]; - - for (int32 i = 0; i < vertexCount; ++i) - { - vertices[i] = b2Mul(xf, poly->m_vertices[i]); - } - - m_debugDraw->DrawSolidPolygon(vertices, vertexCount, color); - } - break; - - default: - break; - } -} - -void b2World::DrawJoint(b2Joint* joint) -{ - b2Body* bodyA = joint->GetBodyA(); - b2Body* bodyB = joint->GetBodyB(); - const b2Transform& xf1 = bodyA->GetTransform(); - const b2Transform& xf2 = bodyB->GetTransform(); - b2Vec2 x1 = xf1.p; - b2Vec2 x2 = xf2.p; - b2Vec2 p1 = joint->GetAnchorA(); - b2Vec2 p2 = joint->GetAnchorB(); - - b2Color color(0.5f, 0.8f, 0.8f); - - switch (joint->GetType()) - { - case e_distanceJoint: - m_debugDraw->DrawSegment(p1, p2, color); - break; - - case e_pulleyJoint: - { - b2PulleyJoint* pulley = (b2PulleyJoint*)joint; - b2Vec2 s1 = pulley->GetGroundAnchorA(); - b2Vec2 s2 = pulley->GetGroundAnchorB(); - m_debugDraw->DrawSegment(s1, p1, color); - m_debugDraw->DrawSegment(s2, p2, color); - m_debugDraw->DrawSegment(s1, s2, color); - } - break; - - case e_mouseJoint: - // don't draw this - break; - - default: - m_debugDraw->DrawSegment(x1, p1, color); - m_debugDraw->DrawSegment(p1, p2, color); - m_debugDraw->DrawSegment(x2, p2, color); - } -} - -void b2World::DrawDebugData() -{ - if (m_debugDraw == NULL) - { - return; - } - - uint32 flags = m_debugDraw->GetFlags(); - - if (flags & b2Draw::e_shapeBit) - { - for (b2Body* b = m_bodyList; b; b = b->GetNext()) - { - const b2Transform& xf = b->GetTransform(); - for (b2Fixture* f = b->GetFixtureList(); f; f = f->GetNext()) - { - if (b->IsActive() == false) - { - DrawShape(f, xf, b2Color(0.5f, 0.5f, 0.3f)); - } - else if (b->GetType() == b2_staticBody) - { - DrawShape(f, xf, b2Color(0.5f, 0.9f, 0.5f)); - } - else if (b->GetType() == b2_kinematicBody) - { - DrawShape(f, xf, b2Color(0.5f, 0.5f, 0.9f)); - } - else if (b->IsAwake() == false) - { - DrawShape(f, xf, b2Color(0.6f, 0.6f, 0.6f)); - } - else - { - DrawShape(f, xf, b2Color(0.9f, 0.7f, 0.7f)); - } - } - } - } - - if (flags & b2Draw::e_jointBit) - { - for (b2Joint* j = m_jointList; j; j = j->GetNext()) - { - DrawJoint(j); - } - } - - if (flags & b2Draw::e_pairBit) - { - b2Color color(0.3f, 0.9f, 0.9f); - for (b2Contact* c = m_contactManager.m_contactList; c; c = c->GetNext()) - { - //b2Fixture* fixtureA = c->GetFixtureA(); - //b2Fixture* fixtureB = c->GetFixtureB(); - - //b2Vec2 cA = fixtureA->GetAABB().GetCenter(); - //b2Vec2 cB = fixtureB->GetAABB().GetCenter(); - - //m_debugDraw->DrawSegment(cA, cB, color); - } - } - - if (flags & b2Draw::e_aabbBit) - { - b2Color color(0.9f, 0.3f, 0.9f); - b2BroadPhase* bp = &m_contactManager.m_broadPhase; - - for (b2Body* b = m_bodyList; b; b = b->GetNext()) - { - if (b->IsActive() == false) - { - continue; - } - - for (b2Fixture* f = b->GetFixtureList(); f; f = f->GetNext()) - { - for (int32 i = 0; i < f->m_proxyCount; ++i) - { - b2FixtureProxy* proxy = f->m_proxies + i; - b2AABB aabb = bp->GetFatAABB(proxy->proxyId); - b2Vec2 vs[4]; - vs[0].Set(aabb.lowerBound.x, aabb.lowerBound.y); - vs[1].Set(aabb.upperBound.x, aabb.lowerBound.y); - vs[2].Set(aabb.upperBound.x, aabb.upperBound.y); - vs[3].Set(aabb.lowerBound.x, aabb.upperBound.y); - - m_debugDraw->DrawPolygon(vs, 4, color); - } - } - } - } - - if (flags & b2Draw::e_centerOfMassBit) - { - for (b2Body* b = m_bodyList; b; b = b->GetNext()) - { - b2Transform xf = b->GetTransform(); - xf.p = b->GetWorldCenter(); - m_debugDraw->DrawTransform(xf); - } - } -} - -int32 b2World::GetProxyCount() const -{ - return m_contactManager.m_broadPhase.GetProxyCount(); -} - -int32 b2World::GetTreeHeight() const -{ - return m_contactManager.m_broadPhase.GetTreeHeight(); -} - -int32 b2World::GetTreeBalance() const -{ - return m_contactManager.m_broadPhase.GetTreeBalance(); -} - -float32 b2World::GetTreeQuality() const -{ - return m_contactManager.m_broadPhase.GetTreeQuality(); -} - -void b2World::ShiftOrigin(const b2Vec2& newOrigin) -{ - b2Assert((m_flags & e_locked) == 0); - if ((m_flags & e_locked) == e_locked) - { - return; - } - - for (b2Body* b = m_bodyList; b; b = b->m_next) - { - b->m_xf.p -= newOrigin; - b->m_sweep.c0 -= newOrigin; - b->m_sweep.c -= newOrigin; - } - - for (b2Joint* j = m_jointList; j; j = j->m_next) - { - j->ShiftOrigin(newOrigin); - } - - m_contactManager.m_broadPhase.ShiftOrigin(newOrigin); -} - -void b2World::Dump() -{ - if ((m_flags & e_locked) == e_locked) - { - return; - } - - b2Log("b2Vec2 g(%.15lef, %.15lef);\n", m_gravity.x, m_gravity.y); - b2Log("m_world->SetGravity(g);\n"); - - b2Log("b2Body** bodies = (b2Body**)b2Alloc(%d * sizeof(b2Body*));\n", m_bodyCount); - b2Log("b2Joint** joints = (b2Joint**)b2Alloc(%d * sizeof(b2Joint*));\n", m_jointCount); - int32 i = 0; - for (b2Body* b = m_bodyList; b; b = b->m_next) - { - b->m_islandIndex = i; - b->Dump(); - ++i; - } - - i = 0; - for (b2Joint* j = m_jointList; j; j = j->m_next) - { - j->m_index = i; - ++i; - } - - // First pass on joints, skip gear joints. - for (b2Joint* j = m_jointList; j; j = j->m_next) - { - if (j->m_type == e_gearJoint) - { - continue; - } - - b2Log("{\n"); - j->Dump(); - b2Log("}\n"); - } - - // Second pass on joints, only gear joints. - for (b2Joint* j = m_jointList; j; j = j->m_next) - { - if (j->m_type != e_gearJoint) - { - continue; - } - - b2Log("{\n"); - j->Dump(); - b2Log("}\n"); - } - - b2Log("b2Free(joints);\n"); - b2Log("b2Free(bodies);\n"); - b2Log("joints = NULL;\n"); - b2Log("bodies = NULL;\n"); -} diff --git a/Box2D/Dynamics/b2World.h b/Box2D/Dynamics/b2World.h deleted file mode 100644 index 4f94f6efe4..0000000000 --- a/Box2D/Dynamics/b2World.h +++ /dev/null @@ -1,354 +0,0 @@ -/* -* Copyright (c) 2006-2011 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_WORLD_H -#define B2_WORLD_H - -#include -#include -#include -#include -#include -#include - -struct b2AABB; -struct b2BodyDef; -struct b2Color; -struct b2JointDef; -class b2Body; -class b2Draw; -class b2Fixture; -class b2Joint; - -/// The world class manages all physics entities, dynamic simulation, -/// and asynchronous queries. The world also contains efficient memory -/// management facilities. -class b2World -{ -public: - /// Construct a world object. - /// @param gravity the world gravity vector. - b2World(const b2Vec2& gravity); - - /// Destruct the world. All physics entities are destroyed and all heap memory is released. - ~b2World(); - - /// Register a destruction listener. The listener is owned by you and must - /// remain in scope. - void SetDestructionListener(b2DestructionListener* listener); - - /// Register a contact filter to provide specific control over collision. - /// Otherwise the default filter is used (b2_defaultFilter). The listener is - /// owned by you and must remain in scope. - void SetContactFilter(b2ContactFilter* filter); - - /// Register a contact event listener. The listener is owned by you and must - /// remain in scope. - void SetContactListener(b2ContactListener* listener); - - /// Register a routine for debug drawing. The debug draw functions are called - /// inside with b2World::DrawDebugData method. The debug draw object is owned - /// by you and must remain in scope. - void SetDebugDraw(b2Draw* debugDraw); - - /// Create a rigid body given a definition. No reference to the definition - /// is retained. - /// @warning This function is locked during callbacks. - b2Body* CreateBody(const b2BodyDef* def); - - /// Destroy a rigid body given a definition. No reference to the definition - /// is retained. This function is locked during callbacks. - /// @warning This automatically deletes all associated shapes and joints. - /// @warning This function is locked during callbacks. - void DestroyBody(b2Body* body); - - /// Create a joint to constrain bodies together. No reference to the definition - /// is retained. This may cause the connected bodies to cease colliding. - /// @warning This function is locked during callbacks. - b2Joint* CreateJoint(const b2JointDef* def); - - /// Destroy a joint. This may cause the connected bodies to begin colliding. - /// @warning This function is locked during callbacks. - void DestroyJoint(b2Joint* joint); - - /// Take a time step. This performs collision detection, integration, - /// and constraint solution. - /// @param timeStep the amount of time to simulate, this should not vary. - /// @param velocityIterations for the velocity constraint solver. - /// @param positionIterations for the position constraint solver. - void Step( float32 timeStep, - int32 velocityIterations, - int32 positionIterations); - - /// Manually clear the force buffer on all bodies. By default, forces are cleared automatically - /// after each call to Step. The default behavior is modified by calling SetAutoClearForces. - /// The purpose of this function is to support sub-stepping. Sub-stepping is often used to maintain - /// a fixed sized time step under a variable frame-rate. - /// When you perform sub-stepping you will disable auto clearing of forces and instead call - /// ClearForces after all sub-steps are complete in one pass of your game loop. - /// @see SetAutoClearForces - void ClearForces(); - - /// Call this to draw shapes and other debug draw data. This is intentionally non-const. - void DrawDebugData(); - - /// Query the world for all fixtures that potentially overlap the - /// provided AABB. - /// @param callback a user implemented callback class. - /// @param aabb the query box. - void QueryAABB(b2QueryCallback* callback, const b2AABB& aabb) const; - - /// Ray-cast the world for all fixtures in the path of the ray. Your callback - /// controls whether you get the closest point, any point, or n-points. - /// The ray-cast ignores shapes that contain the starting point. - /// @param callback a user implemented callback class. - /// @param point1 the ray starting point - /// @param point2 the ray ending point - void RayCast(b2RayCastCallback* callback, const b2Vec2& point1, const b2Vec2& point2) const; - - /// Get the world body list. With the returned body, use b2Body::GetNext to get - /// the next body in the world list. A NULL body indicates the end of the list. - /// @return the head of the world body list. - b2Body* GetBodyList(); - const b2Body* GetBodyList() const; - - /// Get the world joint list. With the returned joint, use b2Joint::GetNext to get - /// the next joint in the world list. A NULL joint indicates the end of the list. - /// @return the head of the world joint list. - b2Joint* GetJointList(); - const b2Joint* GetJointList() const; - - /// Get the world contact list. With the returned contact, use b2Contact::GetNext to get - /// the next contact in the world list. A NULL contact indicates the end of the list. - /// @return the head of the world contact list. - /// @warning contacts are created and destroyed in the middle of a time step. - /// Use b2ContactListener to avoid missing contacts. - b2Contact* GetContactList(); - const b2Contact* GetContactList() const; - - /// Enable/disable sleep. - void SetAllowSleeping(bool flag); - bool GetAllowSleeping() const { return m_allowSleep; } - - /// Enable/disable warm starting. For testing. - void SetWarmStarting(bool flag) { m_warmStarting = flag; } - bool GetWarmStarting() const { return m_warmStarting; } - - /// Enable/disable continuous physics. For testing. - void SetContinuousPhysics(bool flag) { m_continuousPhysics = flag; } - bool GetContinuousPhysics() const { return m_continuousPhysics; } - - /// Enable/disable single stepped continuous physics. For testing. - void SetSubStepping(bool flag) { m_subStepping = flag; } - bool GetSubStepping() const { return m_subStepping; } - - /// Get the number of broad-phase proxies. - int32 GetProxyCount() const; - - /// Get the number of bodies. - int32 GetBodyCount() const; - - /// Get the number of joints. - int32 GetJointCount() const; - - /// Get the number of contacts (each may have 0 or more contact points). - int32 GetContactCount() const; - - /// Get the height of the dynamic tree. - int32 GetTreeHeight() const; - - /// Get the balance of the dynamic tree. - int32 GetTreeBalance() const; - - /// Get the quality metric of the dynamic tree. The smaller the better. - /// The minimum is 1. - float32 GetTreeQuality() const; - - /// Change the global gravity vector. - void SetGravity(const b2Vec2& gravity); - - /// Get the global gravity vector. - b2Vec2 GetGravity() const; - - /// Is the world locked (in the middle of a time step). - bool IsLocked() const; - - /// Set flag to control automatic clearing of forces after each time step. - void SetAutoClearForces(bool flag); - - /// Get the flag that controls automatic clearing of forces after each time step. - bool GetAutoClearForces() const; - - /// Shift the world origin. Useful for large worlds. - /// The body shift formula is: position -= newOrigin - /// @param newOrigin the new origin with respect to the old origin - void ShiftOrigin(const b2Vec2& newOrigin); - - /// Get the contact manager for testing. - const b2ContactManager& GetContactManager() const; - - /// Get the current profile. - const b2Profile& GetProfile() const; - - /// Dump the world into the log file. - /// @warning this should be called outside of a time step. - void Dump(); - -private: - - // m_flags - enum - { - e_newFixture = 0x0001, - e_locked = 0x0002, - e_clearForces = 0x0004 - }; - - friend class b2Body; - friend class b2Fixture; - friend class b2ContactManager; - friend class b2Controller; - - void Solve(const b2TimeStep& step); - void SolveTOI(const b2TimeStep& step); - - void DrawJoint(b2Joint* joint); - void DrawShape(b2Fixture* shape, const b2Transform& xf, const b2Color& color); - - b2BlockAllocator m_blockAllocator; - b2StackAllocator m_stackAllocator; - - int32 m_flags; - - b2ContactManager m_contactManager; - - b2Body* m_bodyList; - b2Joint* m_jointList; - - int32 m_bodyCount; - int32 m_jointCount; - - b2Vec2 m_gravity; - bool m_allowSleep; - - b2DestructionListener* m_destructionListener; - b2Draw* m_debugDraw; - - // This is used to compute the time step ratio to - // support a variable time step. - float32 m_inv_dt0; - - // These are for debugging the solver. - bool m_warmStarting; - bool m_continuousPhysics; - bool m_subStepping; - - bool m_stepComplete; - - b2Profile m_profile; -}; - -inline b2Body* b2World::GetBodyList() -{ - return m_bodyList; -} - -inline const b2Body* b2World::GetBodyList() const -{ - return m_bodyList; -} - -inline b2Joint* b2World::GetJointList() -{ - return m_jointList; -} - -inline const b2Joint* b2World::GetJointList() const -{ - return m_jointList; -} - -inline b2Contact* b2World::GetContactList() -{ - return m_contactManager.m_contactList; -} - -inline const b2Contact* b2World::GetContactList() const -{ - return m_contactManager.m_contactList; -} - -inline int32 b2World::GetBodyCount() const -{ - return m_bodyCount; -} - -inline int32 b2World::GetJointCount() const -{ - return m_jointCount; -} - -inline int32 b2World::GetContactCount() const -{ - return m_contactManager.m_contactCount; -} - -inline void b2World::SetGravity(const b2Vec2& gravity) -{ - m_gravity = gravity; -} - -inline b2Vec2 b2World::GetGravity() const -{ - return m_gravity; -} - -inline bool b2World::IsLocked() const -{ - return (m_flags & e_locked) == e_locked; -} - -inline void b2World::SetAutoClearForces(bool flag) -{ - if (flag) - { - m_flags |= e_clearForces; - } - else - { - m_flags &= ~e_clearForces; - } -} - -/// Get the flag that controls automatic clearing of forces after each time step. -inline bool b2World::GetAutoClearForces() const -{ - return (m_flags & e_clearForces) == e_clearForces; -} - -inline const b2ContactManager& b2World::GetContactManager() const -{ - return m_contactManager; -} - -inline const b2Profile& b2World::GetProfile() const -{ - return m_profile; -} - -#endif diff --git a/Box2D/Dynamics/b2WorldCallbacks.cpp b/Box2D/Dynamics/b2WorldCallbacks.cpp deleted file mode 100644 index 0f4b9c53c0..0000000000 --- a/Box2D/Dynamics/b2WorldCallbacks.cpp +++ /dev/null @@ -1,36 +0,0 @@ -/* -* 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 -#include - -// Return true if contact calculations should be performed between these two shapes. -// If you implement your own collision filter you may want to build from this implementation. -bool b2ContactFilter::ShouldCollide(b2Fixture* fixtureA, b2Fixture* fixtureB) -{ - const b2Filter& filterA = fixtureA->GetFilterData(); - const b2Filter& filterB = fixtureB->GetFilterData(); - - if (filterA.groupIndex == filterB.groupIndex && filterA.groupIndex != 0) - { - return filterA.groupIndex > 0; - } - - bool collide = (filterA.maskBits & filterB.categoryBits) != 0 && (filterA.categoryBits & filterB.maskBits) != 0; - return collide; -} diff --git a/Box2D/Dynamics/b2WorldCallbacks.h b/Box2D/Dynamics/b2WorldCallbacks.h deleted file mode 100644 index 82ffc02295..0000000000 --- a/Box2D/Dynamics/b2WorldCallbacks.h +++ /dev/null @@ -1,155 +0,0 @@ -/* -* 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_WORLD_CALLBACKS_H -#define B2_WORLD_CALLBACKS_H - -#include - -struct b2Vec2; -struct b2Transform; -class b2Fixture; -class b2Body; -class b2Joint; -class b2Contact; -struct b2ContactResult; -struct b2Manifold; - -/// Joints and fixtures are destroyed when their associated -/// body is destroyed. Implement this listener so that you -/// may nullify references to these joints and shapes. -class b2DestructionListener -{ -public: - virtual ~b2DestructionListener() {} - - /// Called when any joint is about to be destroyed due - /// to the destruction of one of its attached bodies. - virtual void SayGoodbye(b2Joint* joint) = 0; - - /// Called when any fixture is about to be destroyed due - /// to the destruction of its parent body. - virtual void SayGoodbye(b2Fixture* fixture) = 0; -}; - -/// Implement this class to provide collision filtering. In other words, you can implement -/// this class if you want finer control over contact creation. -class b2ContactFilter -{ -public: - virtual ~b2ContactFilter() {} - - /// Return true if contact calculations should be performed between these two shapes. - /// @warning for performance reasons this is only called when the AABBs begin to overlap. - virtual bool ShouldCollide(b2Fixture* fixtureA, b2Fixture* fixtureB); -}; - -/// Contact impulses for reporting. Impulses are used instead of forces because -/// sub-step forces may approach infinity for rigid body collisions. These -/// match up one-to-one with the contact points in b2Manifold. -struct b2ContactImpulse -{ - float32 normalImpulses[b2_maxManifoldPoints]; - float32 tangentImpulses[b2_maxManifoldPoints]; - int32 count; -}; - -/// Implement this class to get contact information. You can use these results for -/// things like sounds and game logic. You can also get contact results by -/// traversing the contact lists after the time step. However, you might miss -/// some contacts because continuous physics leads to sub-stepping. -/// Additionally you may receive multiple callbacks for the same contact in a -/// single time step. -/// You should strive to make your callbacks efficient because there may be -/// many callbacks per time step. -/// @warning You cannot create/destroy Box2D entities inside these callbacks. -class b2ContactListener -{ -public: - virtual ~b2ContactListener() {} - - /// Called when two fixtures begin to touch. - virtual void BeginContact(b2Contact* contact) { B2_NOT_USED(contact); } - - /// Called when two fixtures cease to touch. - virtual void EndContact(b2Contact* contact) { B2_NOT_USED(contact); } - - /// This is called after a contact is updated. This allows you to inspect a - /// contact before it goes to the solver. If you are careful, you can modify the - /// contact manifold (e.g. disable contact). - /// A copy of the old manifold is provided so that you can detect changes. - /// Note: this is called only for awake bodies. - /// Note: this is called even when the number of contact points is zero. - /// Note: this is not called for sensors. - /// Note: if you set the number of contact points to zero, you will not - /// get an EndContact callback. However, you may get a BeginContact callback - /// the next step. - virtual void PreSolve(b2Contact* contact, const b2Manifold* oldManifold) - { - B2_NOT_USED(contact); - B2_NOT_USED(oldManifold); - } - - /// This lets you inspect a contact after the solver is finished. This is useful - /// for inspecting impulses. - /// Note: the contact manifold does not include time of impact impulses, which can be - /// arbitrarily large if the sub-step is small. Hence the impulse is provided explicitly - /// in a separate data structure. - /// Note: this is only called for contacts that are touching, solid, and awake. - virtual void PostSolve(b2Contact* contact, const b2ContactImpulse* impulse) - { - B2_NOT_USED(contact); - B2_NOT_USED(impulse); - } -}; - -/// Callback class for AABB queries. -/// See b2World::Query -class b2QueryCallback -{ -public: - virtual ~b2QueryCallback() {} - - /// Called for each fixture found in the query AABB. - /// @return false to terminate the query. - virtual bool ReportFixture(b2Fixture* fixture) = 0; -}; - -/// Callback class for ray casts. -/// See b2World::RayCast -class b2RayCastCallback -{ -public: - virtual ~b2RayCastCallback() {} - - /// Called for each fixture found in the query. You control how the ray cast - /// proceeds by returning a float: - /// return -1: ignore this fixture and continue - /// return 0: terminate the ray cast - /// return fraction: clip the ray to this point - /// return 1: don't clip the ray and continue - /// @param fixture the fixture hit by the ray - /// @param point the point of initial intersection - /// @param normal the normal vector at the point of intersection - /// @return -1 to filter, 0 to terminate, fraction to clip the ray for - /// closest hit, 1 to continue - virtual float32 ReportFixture( b2Fixture* fixture, const b2Vec2& point, - const b2Vec2& normal, float32 fraction) = 0; -}; - -#endif diff --git a/Box2D/Rope/b2Rope.cpp b/Box2D/Rope/b2Rope.cpp deleted file mode 100644 index 7af3a6cf2f..0000000000 --- a/Box2D/Rope/b2Rope.cpp +++ /dev/null @@ -1,259 +0,0 @@ -/* -* Copyright (c) 2011 Erin Catto http://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 -#include - -b2Rope::b2Rope() -{ - m_count = 0; - m_ps = NULL; - m_p0s = NULL; - m_vs = NULL; - m_ims = NULL; - m_Ls = NULL; - m_as = NULL; - m_gravity.SetZero(); - m_k2 = 1.0f; - m_k3 = 0.1f; -} - -b2Rope::~b2Rope() -{ - b2Free(m_ps); - b2Free(m_p0s); - b2Free(m_vs); - b2Free(m_ims); - b2Free(m_Ls); - b2Free(m_as); -} - -void b2Rope::Initialize(const b2RopeDef* def) -{ - b2Assert(def->count >= 3); - m_count = def->count; - m_ps = (b2Vec2*)b2Alloc(m_count * sizeof(b2Vec2)); - m_p0s = (b2Vec2*)b2Alloc(m_count * sizeof(b2Vec2)); - m_vs = (b2Vec2*)b2Alloc(m_count * sizeof(b2Vec2)); - m_ims = (float32*)b2Alloc(m_count * sizeof(float32)); - - for (int32 i = 0; i < m_count; ++i) - { - m_ps[i] = def->vertices[i]; - m_p0s[i] = def->vertices[i]; - m_vs[i].SetZero(); - - float32 m = def->masses[i]; - if (m > 0.0f) - { - m_ims[i] = 1.0f / m; - } - else - { - m_ims[i] = 0.0f; - } - } - - int32 count2 = m_count - 1; - int32 count3 = m_count - 2; - m_Ls = (float32*)b2Alloc(count2 * sizeof(float32)); - m_as = (float32*)b2Alloc(count3 * sizeof(float32)); - - for (int32 i = 0; i < count2; ++i) - { - b2Vec2 p1 = m_ps[i]; - b2Vec2 p2 = m_ps[i+1]; - m_Ls[i] = b2Distance(p1, p2); - } - - for (int32 i = 0; i < count3; ++i) - { - b2Vec2 p1 = m_ps[i]; - b2Vec2 p2 = m_ps[i + 1]; - b2Vec2 p3 = m_ps[i + 2]; - - b2Vec2 d1 = p2 - p1; - b2Vec2 d2 = p3 - p2; - - float32 a = b2Cross(d1, d2); - float32 b = b2Dot(d1, d2); - - m_as[i] = b2Atan2(a, b); - } - - m_gravity = def->gravity; - m_damping = def->damping; - m_k2 = def->k2; - m_k3 = def->k3; -} - -void b2Rope::Step(float32 h, int32 iterations) -{ - if (h == 0.0) - { - return; - } - - float32 d = expf(- h * m_damping); - - for (int32 i = 0; i < m_count; ++i) - { - m_p0s[i] = m_ps[i]; - if (m_ims[i] > 0.0f) - { - m_vs[i] += h * m_gravity; - } - m_vs[i] *= d; - m_ps[i] += h * m_vs[i]; - - } - - for (int32 i = 0; i < iterations; ++i) - { - SolveC2(); - SolveC3(); - SolveC2(); - } - - float32 inv_h = 1.0f / h; - for (int32 i = 0; i < m_count; ++i) - { - m_vs[i] = inv_h * (m_ps[i] - m_p0s[i]); - } -} - -void b2Rope::SolveC2() -{ - int32 count2 = m_count - 1; - - for (int32 i = 0; i < count2; ++i) - { - b2Vec2 p1 = m_ps[i]; - b2Vec2 p2 = m_ps[i + 1]; - - b2Vec2 d = p2 - p1; - float32 L = d.Normalize(); - - float32 im1 = m_ims[i]; - float32 im2 = m_ims[i + 1]; - - if (im1 + im2 == 0.0f) - { - continue; - } - - float32 s1 = im1 / (im1 + im2); - float32 s2 = im2 / (im1 + im2); - - p1 -= m_k2 * s1 * (m_Ls[i] - L) * d; - p2 += m_k2 * s2 * (m_Ls[i] - L) * d; - - m_ps[i] = p1; - m_ps[i + 1] = p2; - } -} - -void b2Rope::SetAngle(float32 angle) -{ - int32 count3 = m_count - 2; - for (int32 i = 0; i < count3; ++i) - { - m_as[i] = angle; - } -} - -void b2Rope::SolveC3() -{ - int32 count3 = m_count - 2; - - for (int32 i = 0; i < count3; ++i) - { - b2Vec2 p1 = m_ps[i]; - b2Vec2 p2 = m_ps[i + 1]; - b2Vec2 p3 = m_ps[i + 2]; - - float32 m1 = m_ims[i]; - float32 m2 = m_ims[i + 1]; - float32 m3 = m_ims[i + 2]; - - b2Vec2 d1 = p2 - p1; - b2Vec2 d2 = p3 - p2; - - float32 L1sqr = d1.LengthSquared(); - float32 L2sqr = d2.LengthSquared(); - - if (L1sqr * L2sqr == 0.0f) - { - continue; - } - - float32 a = b2Cross(d1, d2); - float32 b = b2Dot(d1, d2); - - float32 angle = b2Atan2(a, b); - - b2Vec2 Jd1 = (-1.0f / L1sqr) * d1.Skew(); - b2Vec2 Jd2 = (1.0f / L2sqr) * d2.Skew(); - - b2Vec2 J1 = -Jd1; - b2Vec2 J2 = Jd1 - Jd2; - b2Vec2 J3 = Jd2; - - float32 mass = m1 * b2Dot(J1, J1) + m2 * b2Dot(J2, J2) + m3 * b2Dot(J3, J3); - if (mass == 0.0f) - { - continue; - } - - mass = 1.0f / mass; - - float32 C = angle - m_as[i]; - - while (C > b2_pi) - { - angle -= 2 * b2_pi; - C = angle - m_as[i]; - } - - while (C < -b2_pi) - { - angle += 2.0f * b2_pi; - C = angle - m_as[i]; - } - - float32 impulse = - m_k3 * mass * C; - - p1 += (m1 * impulse) * J1; - p2 += (m2 * impulse) * J2; - p3 += (m3 * impulse) * J3; - - m_ps[i] = p1; - m_ps[i + 1] = p2; - m_ps[i + 2] = p3; - } -} - -void b2Rope::Draw(b2Draw* draw) const -{ - b2Color c(0.4f, 0.5f, 0.7f); - - for (int32 i = 0; i < m_count - 1; ++i) - { - draw->DrawSegment(m_ps[i], m_ps[i+1], c); - } -} diff --git a/Box2D/Rope/b2Rope.h b/Box2D/Rope/b2Rope.h deleted file mode 100644 index 246cf923f9..0000000000 --- a/Box2D/Rope/b2Rope.h +++ /dev/null @@ -1,115 +0,0 @@ -/* -* Copyright (c) 2011 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_ROPE_H -#define B2_ROPE_H - -#include - -class b2Draw; - -/// -struct b2RopeDef -{ - b2RopeDef() - { - vertices = NULL; - count = 0; - masses = NULL; - gravity.SetZero(); - damping = 0.1f; - k2 = 0.9f; - k3 = 0.1f; - } - - /// - b2Vec2* vertices; - - /// - int32 count; - - /// - float32* masses; - - /// - b2Vec2 gravity; - - /// - float32 damping; - - /// Stretching stiffness - float32 k2; - - /// Bending stiffness. Values above 0.5 can make the simulation blow up. - float32 k3; -}; - -/// -class b2Rope -{ -public: - b2Rope(); - ~b2Rope(); - - /// - void Initialize(const b2RopeDef* def); - - /// - void Step(float32 timeStep, int32 iterations); - - /// - int32 GetVertexCount() const - { - return m_count; - } - - /// - const b2Vec2* GetVertices() const - { - return m_ps; - } - - /// - void Draw(b2Draw* draw) const; - - /// - void SetAngle(float32 angle); - -private: - - void SolveC2(); - void SolveC3(); - - int32 m_count; - b2Vec2* m_ps; - b2Vec2* m_p0s; - b2Vec2* m_vs; - - float32* m_ims; - - float32* m_Ls; - float32* m_as; - - b2Vec2 m_gravity; - float32 m_damping; - - float32 m_k2; - float32 m_k3; -}; - -#endif diff --git a/Box2D/proj.win32/Box2D.vcxproj b/Box2D/proj.win32/Box2D.vcxproj deleted file mode 100644 index ed59a2460d..0000000000 --- a/Box2D/proj.win32/Box2D.vcxproj +++ /dev/null @@ -1,196 +0,0 @@ - - - - - Debug - Win32 - - - Release - Win32 - - - - libBox2D - {929480E7-23C0-4DF6-8456-096D71547116} - Box2D.win32 - Win32Proj - - - - StaticLibrary - Unicode - v100 - v110 - v110_xp - - - StaticLibrary - Unicode - v100 - v110 - v110_xp - - - - - - - - - - - - - <_ProjectFileVersion>10.0.40219.1 - $(SolutionDir)$(Configuration).win32\ - $(Configuration).win32\ - $(SolutionDir)$(Configuration).win32\ - $(Configuration).win32\ - AllRules.ruleset - - - AllRules.ruleset - - - - - $(MSBuildProgramFiles32)\Microsoft SDKs\Windows\v7.1A\lib;$(LibraryPath) - - - $(MSBuildProgramFiles32)\Microsoft SDKs\Windows\v7.1A\lib;$(LibraryPath) - - - - Disabled - $(MSBuildProgramFiles32)\Microsoft SDKs\Windows\v7.1A\include;../../;%(AdditionalIncludeDirectories) - WIN32;_DEBUG;_LIB;%(PreprocessorDefinitions) - false - EnableFastChecks - MultiThreadedDebugDLL - - - Level3 - EditAndContinue - true - - - $(OutDir)$(ProjectName).lib - - - - - $(MSBuildProgramFiles32)\Microsoft SDKs\Windows\v7.1A\include;../../;%(AdditionalIncludeDirectories) - WIN32;NDEBUG;_LIB;%(PreprocessorDefinitions) - MultiThreadedDLL - - - Level3 - ProgramDatabase - true - - - $(OutDir)$(ProjectName).lib - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/Box2D/proj.win32/Box2D.vcxproj.filters b/Box2D/proj.win32/Box2D.vcxproj.filters deleted file mode 100644 index d9f5647a06..0000000000 --- a/Box2D/proj.win32/Box2D.vcxproj.filters +++ /dev/null @@ -1,307 +0,0 @@ - - - - - {bdf099c5-cc32-468b-b53f-ec010d7f823f} - - - {92792f88-4e5c-46ee-8d87-002f3b2297ae} - - - {f41a6fa6-5ac1-4514-9ac0-6f3a38f35b4d} - - - {0b56913a-34b7-410a-b386-869d6f7a20be} - - - {f0e7b230-79cc-49b8-9ed7-9dbfa062f2a4} - - - {932f2008-afbe-42f5-993d-a1df0ec67756} - - - {f8937e09-93a4-49fa-8f3e-dfc36da764df} - - - - - Collision - - - Collision - - - Collision - - - Collision - - - Collision - - - Collision - - - Collision - - - Collision - - - Collision\Shapes - - - Collision\Shapes - - - Collision\Shapes - - - Collision\Shapes - - - Common - - - Common - - - Common - - - Common - - - Common - - - Common - - - Dynamics - - - Dynamics - - - Dynamics - - - Dynamics - - - Dynamics - - - Dynamics - - - Dynamics\Contacts - - - Dynamics\Contacts - - - Dynamics\Contacts - - - Dynamics\Contacts - - - Dynamics\Contacts - - - Dynamics\Contacts - - - Dynamics\Contacts - - - Dynamics\Contacts - - - Dynamics\Contacts - - - Dynamics\Joints - - - Dynamics\Joints - - - Dynamics\Joints - - - Dynamics\Joints - - - Dynamics\Joints - - - Dynamics\Joints - - - Dynamics\Joints - - - Dynamics\Joints - - - Dynamics\Joints - - - Dynamics\Joints - - - Dynamics\Joints - - - Rope - - - Dynamics\Joints - - - - - Collision - - - Collision - - - Collision - - - Collision - - - Collision - - - Collision\Shapes - - - Collision\Shapes - - - Collision\Shapes - - - Collision\Shapes - - - Collision\Shapes - - - Common - - - Common - - - Common - - - Common - - - Common - - - Common - - - Common - - - Dynamics - - - Dynamics - - - Dynamics - - - Dynamics - - - Dynamics - - - Dynamics - - - Dynamics - - - Dynamics\Contacts - - - Dynamics\Contacts - - - Dynamics\Contacts - - - Dynamics\Contacts - - - Dynamics\Contacts - - - Dynamics\Contacts - - - Dynamics\Contacts - - - Dynamics\Contacts - - - Dynamics\Contacts - - - Dynamics\Joints - - - Dynamics\Joints - - - Dynamics\Joints - - - Dynamics\Joints - - - Dynamics\Joints - - - Dynamics\Joints - - - Dynamics\Joints - - - Dynamics\Joints - - - Dynamics\Joints - - - Dynamics\Joints - - - Dynamics\Joints - - - Rope - - - - Dynamics\Joints - - - \ No newline at end of file diff --git a/Box2D/proj.win32/Box2D.vcxproj.user b/Box2D/proj.win32/Box2D.vcxproj.user deleted file mode 100644 index ace9a86acb..0000000000 --- a/Box2D/proj.win32/Box2D.vcxproj.user +++ /dev/null @@ -1,3 +0,0 @@ - - - \ No newline at end of file diff --git a/Box2D/proj.wp8/Box2D.vcxproj b/Box2D/proj.wp8/Box2D.vcxproj deleted file mode 100644 index 0ee8c9ec3f..0000000000 --- a/Box2D/proj.wp8/Box2D.vcxproj +++ /dev/null @@ -1,224 +0,0 @@ - - - - - Debug - Win32 - - - Debug - ARM - - - Release - Win32 - - - Release - ARM - - - - {C55734A3-702C-4FA1-B950-32C8E169302F} - Box2D.win32 - en-US - 11.0 - - - - StaticLibrary - true - v110_wp80 - - - StaticLibrary - true - v110_wp80 - - - StaticLibrary - false - true - v110_wp80 - - - StaticLibrary - false - true - v110_wp80 - - - - - - - - false - - - - _LIB;%(PreprocessorDefinitions) - NotUsing - pch.h - $(WindowsSDK_MetadataPath);$(AdditionalUsingDirectories) - ../../;%(AdditionalIncludeDirectories) - WIN32;_DEBUG;_LIB;%(PreprocessorDefinitions) - - - Console - false - - - - - _LIB;NDEBUG;%(PreprocessorDefinitions) - NotUsing - pch.h - $(WindowsSDK_MetadataPath);$(AdditionalUsingDirectories) - ../../;%(AdditionalIncludeDirectories) - WIN32;NDEBUG;_LIB;%(PreprocessorDefinitions) - - - Console - false - - - - - _LIB;%(PreprocessorDefinitions) - NotUsing - pch.h - $(WindowsSDK_MetadataPath);$(AdditionalUsingDirectories) - ../../;%(AdditionalIncludeDirectories) - WIN32;_DEBUG;_LIB;%(PreprocessorDefinitions) - - - Console - false - - - - - _LIB;NDEBUG;%(PreprocessorDefinitions) - NotUsing - pch.h - $(WindowsSDK_MetadataPath);$(AdditionalUsingDirectories) - ../../;%(AdditionalIncludeDirectories) - WIN32;NDEBUG;_LIB;%(PreprocessorDefinitions) - - - Console - false - - - - - true - - - true - false - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/Box2D/proj.wp8/Box2D.vcxproj.filters b/Box2D/proj.wp8/Box2D.vcxproj.filters deleted file mode 100644 index d9f5647a06..0000000000 --- a/Box2D/proj.wp8/Box2D.vcxproj.filters +++ /dev/null @@ -1,307 +0,0 @@ - - - - - {bdf099c5-cc32-468b-b53f-ec010d7f823f} - - - {92792f88-4e5c-46ee-8d87-002f3b2297ae} - - - {f41a6fa6-5ac1-4514-9ac0-6f3a38f35b4d} - - - {0b56913a-34b7-410a-b386-869d6f7a20be} - - - {f0e7b230-79cc-49b8-9ed7-9dbfa062f2a4} - - - {932f2008-afbe-42f5-993d-a1df0ec67756} - - - {f8937e09-93a4-49fa-8f3e-dfc36da764df} - - - - - Collision - - - Collision - - - Collision - - - Collision - - - Collision - - - Collision - - - Collision - - - Collision - - - Collision\Shapes - - - Collision\Shapes - - - Collision\Shapes - - - Collision\Shapes - - - Common - - - Common - - - Common - - - Common - - - Common - - - Common - - - Dynamics - - - Dynamics - - - Dynamics - - - Dynamics - - - Dynamics - - - Dynamics - - - Dynamics\Contacts - - - Dynamics\Contacts - - - Dynamics\Contacts - - - Dynamics\Contacts - - - Dynamics\Contacts - - - Dynamics\Contacts - - - Dynamics\Contacts - - - Dynamics\Contacts - - - Dynamics\Contacts - - - Dynamics\Joints - - - Dynamics\Joints - - - Dynamics\Joints - - - Dynamics\Joints - - - Dynamics\Joints - - - Dynamics\Joints - - - Dynamics\Joints - - - Dynamics\Joints - - - Dynamics\Joints - - - Dynamics\Joints - - - Dynamics\Joints - - - Rope - - - Dynamics\Joints - - - - - Collision - - - Collision - - - Collision - - - Collision - - - Collision - - - Collision\Shapes - - - Collision\Shapes - - - Collision\Shapes - - - Collision\Shapes - - - Collision\Shapes - - - Common - - - Common - - - Common - - - Common - - - Common - - - Common - - - Common - - - Dynamics - - - Dynamics - - - Dynamics - - - Dynamics - - - Dynamics - - - Dynamics - - - Dynamics - - - Dynamics\Contacts - - - Dynamics\Contacts - - - Dynamics\Contacts - - - Dynamics\Contacts - - - Dynamics\Contacts - - - Dynamics\Contacts - - - Dynamics\Contacts - - - Dynamics\Contacts - - - Dynamics\Contacts - - - Dynamics\Joints - - - Dynamics\Joints - - - Dynamics\Joints - - - Dynamics\Joints - - - Dynamics\Joints - - - Dynamics\Joints - - - Dynamics\Joints - - - Dynamics\Joints - - - Dynamics\Joints - - - Dynamics\Joints - - - Dynamics\Joints - - - Rope - - - - Dynamics\Joints - - - \ No newline at end of file diff --git a/Box2D/proj.wp8/Box2D.vcxproj.user b/Box2D/proj.wp8/Box2D.vcxproj.user deleted file mode 100644 index 17f726ed50..0000000000 --- a/Box2D/proj.wp8/Box2D.vcxproj.user +++ /dev/null @@ -1,6 +0,0 @@ - - - - true - - \ No newline at end of file diff --git a/chipmunk/.gitignore b/chipmunk/.gitignore deleted file mode 100644 index d9f94512e8..0000000000 --- a/chipmunk/.gitignore +++ /dev/null @@ -1,9 +0,0 @@ -.DS_Store -doc/index.html -xcode/Chipmunk6.xcodeproj/*.pbxuser -xcode/Chipmunk6.xcodeproj/*.perspectivev3 -xcode/Chipmunk6.xcodeproj/xcuserdata -xcode/Chipmunk6.xcodeproj/project.xcworkspace -xcode/DerivedData -build -msvc diff --git a/chipmunk/Android.mk b/chipmunk/Android.mk deleted file mode 100644 index e8441cd0d7..0000000000 --- a/chipmunk/Android.mk +++ /dev/null @@ -1,45 +0,0 @@ -LOCAL_PATH := $(call my-dir) - -include $(CLEAR_VARS) - -LOCAL_MODULE := chipmunk_static - -LOCAL_MODULE_FILENAME := libchipmunk - -LOCAL_SRC_FILES := \ -src/chipmunk.c \ -src/cpArbiter.c \ -src/cpArray.c \ -src/cpBB.c \ -src/cpBBTree.c \ -src/cpBody.c \ -src/cpCollision.c \ -src/cpHashSet.c \ -src/cpPolyShape.c \ -src/cpShape.c \ -src/cpSpace.c \ -src/cpSpaceComponent.c \ -src/cpSpaceHash.c \ -src/cpSpaceQuery.c \ -src/cpSpaceStep.c \ -src/cpSpatialIndex.c \ -src/cpSweep1D.c \ -src/cpVect.c \ -src/constraints/cpConstraint.c \ -src/constraints/cpDampedRotarySpring.c \ -src/constraints/cpDampedSpring.c \ -src/constraints/cpGearJoint.c \ -src/constraints/cpGrooveJoint.c \ -src/constraints/cpPinJoint.c \ -src/constraints/cpPivotJoint.c \ -src/constraints/cpRatchetJoint.c \ -src/constraints/cpRotaryLimitJoint.c \ -src/constraints/cpSimpleMotor.c \ -src/constraints/cpSlideJoint.c - -LOCAL_EXPORT_C_INCLUDES := $(LOCAL_PATH)/include/chipmunk - -LOCAL_C_INCLUDES := $(LOCAL_PATH)/include/chipmunk -LOCAL_CFLAGS := -std=c99 - -include $(BUILD_STATIC_LIBRARY) diff --git a/chipmunk/LICENSE.txt b/chipmunk/LICENSE.txt deleted file mode 100644 index e6234f7b06..0000000000 --- a/chipmunk/LICENSE.txt +++ /dev/null @@ -1,19 +0,0 @@ -Copyright (c) 2007 Scott Lembcke and Howling Moon Software - -Permission is hereby granted, free of charge, to any person obtaining a copy -of this software and associated documentation files (the "Software"), to deal -in the Software without restriction, including without limitation the rights -to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -copies of the Software, and to permit persons to whom the Software is -furnished to do so, subject to the following conditions: - -The above copyright notice and this permission notice shall be included in -all copies or substantial portions of the Software. - -THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -SOFTWARE. diff --git a/chipmunk/README.txt b/chipmunk/README.txt deleted file mode 100644 index 5ef29de217..0000000000 --- a/chipmunk/README.txt +++ /dev/null @@ -1,46 +0,0 @@ -ABOUT: -Chipmunk is a simple, lightweight and fast 2D rigid body physics library written in C. It's licensed under the unrestrictive, OSI approved MIT license. My aim is to give 2D developers access the same quality of physics you find in newer 3D games. I hope you enjoy using Chipmunk, and please consider donating to help make it worth our time to continue to support Chipmunk with great new features. - -CONTRACTING: -Howling Moon Software (my company) is available for contracting if you want to make the physics in your game really stand out. Given our unique experience with the library, we can help you use Chipmunk to it's fullest potential. Feel free to contact us through our webpage: http://howlingmoonsoftware.com/contracting.php - -BUILDING: -Mac OS X: There is an included XCode project file for building the static library and demo application. Alteratively you could use the CMake files. - -iPhone: The XCode project can build a static library with all the proper compiler settings. Additionally, if you run the iphonestatic.sh script in the macosx/ directory, it will build you a fat library compiled as release for the device and debug for the simulator. After building that, just copy the static lib and include/chipmunk/ directory to your project. - -UNIX: A forum user was kind enough to make a set of CMake files for Chipmunk. This will require you to have CMake installed. To build run 'cmake .' then 'make'. This should build a dynamic library, a static library, and the demo application. - -Windows: There is an included MSVC project for building the library and demo application. I do not personally maintain the MSVC project and it apparently hasn't worked for a while. If you are a MSVC whiz, consider lending a hand. - -Ruby: I've been using maintaining a Ruby extension for Chipmunk, but at this time is not up to date with all the latest changes. A forum member has been working on an FFI based extention, and that may be a better way to take advantage of Chipmunk from Ruby. Another forum user has offered to maintain the non-FFI version of the extension. Stay tuned. - -GETTING STARTED: -First of all, you can find the C API documentation here: http://code.google.com/p/chipmunk-physics/wiki/Documentation - -A good starting point is to take a look at the included Demo application. The demos all just set up a Chipmunk simulation space and the demo app draws the graphics directly out of that. This makes it easy to see how the Chipmunk API works without worrying about the graphics code. You are free to use the demo drawing routines in your own projects, though it is certainly not the recommended way of drawing Chipmunk objects as it pokes around at the undocumented parts of Chipmunk. - -FORUM: -http://www.slembcke.net/forums - -CONTACT: -slembcke@gmail.com (also on Google Talk) - -CHANGES SINCE 5.0.0: -* Fixed a NaN issue where raycasts for horizontal or vertical lines would end up in an infinite loop. -* Fixed a number of memory leaks. -* Fixed a number of warnings for various compiler/OS combinations. -* Fixed a number of API issues: - * Rejecting a collision from a begin() callback permanently rejects the collision until separation. - * Erroneous collision type parameterns removed from cpSpaceDefaulteCollisionHandler(). -* Moved FFI declarations of inlined functions into their own header. -* Rearranged the project structure to separate out the header files into a separate include/ directory. -* Added a static library target for the iPhone. -* Type changes when building on the iPhone to make it friendlier to other APIs. -* Added an AABB query to complement point and segment queries. -* Added CP_NO_GROUP and CP_ALL_LAYERS constants. - -CHANGES SINCE 4.x: -* Brand new Joint/Constraint API: New constraints can be added easily and are much more flexible than the old joint system -* Efficient Segment Queries - Like raycasting, but with line segments. -* Brand new collision callback API: Collision begin/separate events, API for removal of objects within callbacks, more programable control over collision handling. \ No newline at end of file diff --git a/chipmunk/chipmunk-docs.html b/chipmunk/chipmunk-docs.html deleted file mode 100644 index 4ed673ef9f..0000000000 --- a/chipmunk/chipmunk-docs.html +++ /dev/null @@ -1,944 +0,0 @@ - - - - - - Chipmunk Game Dynamics Documentation - - - - -

Chipmunk Game Dynamics

- - -

Introduction

- - -

First of all, Chipmunk is a 2D rigid body physics library distributed under the MIT license. Though not yet complete, it is intended to be fast, numerically stable, and easy to use.

- - -

It’s been a long time in coming, but I’ve finally made a stable and usable physics implementation. While not feature complete yet, it’s well on it’s way. I would like to give a Erin Catto a big thank you, as the most of the ideas for the constraint solver come from his Box2D example code. (gPhysics Website). His contact persistence idea allows for stable stacks of objects with very few iterations of the contact solution. Couldn’t have gotten that working without his help.

- - -

Overview

- - -
    -
  • rigid bodies: A rigid body holds the physical properties of an object. (mass, position, rotation, velocity, etc.) It does not have a shape by itself. If you’ve done physics with particles before, rigid bodies differ mostly in that they are able to rotate.
    • -
  • collision shapes: By attaching shapes to bodies, you can define the a body’s shape. You can attach many shapes to a single body to define a complex shape, or none if it doesn’t require a shape.
    • -
  • joints: You can attach joints between two bodies to constrain their behavior.
    • -
  • spaces: Spaces are the basic simulation unit in Chipmunk. You add bodies, shapes and joints to a space, and then update the space as a whole.
    • -
- - -

Rigid bodies, collision shapes and sprites:

- - -

There is often confusion between rigid bodies and their collision shapes in Chipmunk and how they relate to sprites. A sprite would be a visual representation of an object, the sprite is drawn at the position of the rigid body. The collision shape would be the material representation of the object, and how it should collide with other objects. A sprite and collision shape have little to do with one another other than you probably want the collision shape to match the sprite’s shape.

- - -

C API Documentation

- - -

Initializing Chipmunk

- - -

Initializing Chipmunk is an extremely complicated process. The following code snippet steps you through the process:

- - -

-cpInitChipmunk(); /* Actually, that's pretty much it */
-
- -

Chipmunk memory management

- - -

For many of the structures you will use, Chipmunk uses a more or less standard set of memory management functions. For instance:

- - -
    -
  • cpSpaceAlloc() allocates but does not initialize a cpSpace struct.
    • -
  • cpSpaceInit(space, other_args) initializes a cpSpace struct.
    • -
  • cpSpaceNew(args) allocates and initializes a cpSpace struct using args.
    • -
  • cpSpaceDestroy(space) frees all dependancies, but does not free the cpSpace struct.
    • -
  • cpSpaceFree(space) frees all dependancies and the cpSpace struct.
    • -
- - -

While you will probably use the new/free versions exclusively, the others can be helpful when writing language extensions.

- - -

In general, you are responsible for freeing any structs that you allocate. The only exception is that cpShapeDestroy() also destroys the specific shape struct that was passed to the constructor.

- - -

Chipmunk floats: cpFloat

- - -

-typedef float cpFloat;
-
- -

All Chipmunk code should be double safe, so feel free to redefine this.

- - -

Chipmunk vectors: cpVect

- - -

User accessible fields:

- - -

-typedef struct cpVect{
-    cpFloat x,y;
-} cpVect
-
- -

Simply a 2D vector packed into a struct. May change in the future to take advantage of SIMD.

- - -

-#define cpvzero ((cpVect){0.0f, 0.0f})
-
- -

Constant for the zero vector.

- - -

-cpVect cpv(const cpFloat x, const cpFloat y)
-
- -

Convenience constructor for creating new cpVect structs.

- - -

-cpVect cpvadd(const cpVect v1, const cpVect v2)
-cpVect cpvsub(const cpVect v1, const cpVect v2)
-
- -

Add or subtract two vectors.

- - -

-cpVect cpvneg(const cpVect v)
-
- -

Negate a vector.

- - -

-cpVect cpvmult(const cpVect v, const cpFloat s)
-
- -

Scalar multiplication.

- - -

-cpFloat cpvdot(const cpVect v1, const cpVect v2)
-
- -

Vector dot product.

- - -

-cpFloat cpvcross(const cpVect v1, const cpVect v2)
-
- -

2D vector cross product analog. The cross product of 2D vectors exists only in the z component, so only that value is returned.

- - -

-cpVect cpvperp(const cpVect v)
-
- -

Returns the perpendicular vector. (90 degree rotation)

- - -

-cpVect cpvproject(const cpVect v1, const cpVect v2)
-
- -

Returns the vector projection of v1 onto v2.

- - -

-cpVect cpvrotate(const cpVect v1, const cpVect v2)
-
- -

Uses complex multiplication to rotate (and scale) v1 by v2.

- - -

-cpVect cpvunrotate(const cpVect v1, const cpVect v2)
-
- -

Inverse of cpvrotate().

- - -

-cpFloat cpvlength(const cpVect v)
-
- -

Returns the length of v.

- - -

-cpFloat cpvlengthsq(const cpVect v)
-
- -

Returns the squared length of v. Faster than cpvlength() when you only need to compare lengths.

- - -

-cpVect cpvnormalize(const cpVect v)
-
- -

Returns a normalized copy of v.

- - -

-cpVect cpvforangle(const cpFloat a)
-
- -

Returns the unit length vector for the given angle (in radians).

- - -

-cpFloat cpvtoangle(const cpVect v)
-
- -

Returns the angular direction v is pointing in (in radians).

- - -

-*cpvstr(const cpVect v)
-
- -

Returns a string representation of v. NOTE: The string points to a static local and is reset every time the function is called.

- - -

Chipmunk bounding boxes: cpBB

- - -

User accessible fields:

- - -

-typedef struct cpBB{
-    cpFloat l, b, r ,t;
-} cpBB
-
- -

Simple bounding box struct. Stored as left, bottom, right, top values.

- - -

-cpBB cpBBNew(const cpFloat l, const cpFloat b, const cpFloat r, const cpFloat t)
-
- -

Convenience constructor for cpBB structs.

- - -

-int cpBBintersects(const cpBB a, const cpBB b)
-
- -

Returns true if the bounding boxes intersect.

- - -

-int cpBBcontainsBB(const cpBB bb, const cpBB other)
-
- -

Returns true if bb completely contains other.

- - -

-int cpBBcontainsVect(const cpBB bb, const cpVect v)
-
- -

Returns true if bb contains v.

- - -

-cpVect cpBBClampVect(const cpBB bb, const cpVect v)
-
- -

Returns a copy of v clamped to the bounding box.

- - -

-cpVect cpBBWrapVect(const cpBB bb, const cpVect v)
-
- -

Returns a copy of v wrapped to the bounding box.

- - -

Chipmunk spatial hashes: cpSpaceHash

- - -

The spatial hash isn’t yet ready for user use. However, it has been made in a generic manner that would allow it to be used for more than just Chipmunk’s collision detection needs.

- - -

Chipmunk rigid bodies: cpBody

- - -

User accessible fields:

- - -

-typedef struct cpBody{
-    cpFloat m, m_inv;
-    cpFloat i, i_inv;
-
-    cpVect p, v, f;
-    cpFloat a, w, t;
-    cpVect rot;
-} cpBody
-
- -
    -
  • m, and m_inv are the mass and its inverse.
    • -
  • i, and i_inv are the moment of inertia and its inverse.
    • -
  • p, v, and f are the position, velocity and force respectively.
    • -
  • a, w, and t are the angle (in radians), angular velocity (rad/sec), and torque respectively.
    • -
  • rot is the rotation of the body as a unit length vector. (can be used with cpvrotate())
    • -
- - -

-cpBody *cpBodyAlloc(void)
-cpBody *cpBodyInit(cpBody *body, cpFloat m, cpFloat i)
-cpBody *cpBodyNew(cpFloat m, cpFloat i)
-
-void cpBodyDestroy(cpBody *body)
-void cpBodyFree(cpBody *body)
-
- -

Uses the standard suite of Chipmunk memory functions. m and i are the mass and moment of inertia for the body.

- - -

-void cpBodySetMass(cpBody *body, cpFloat m);
-void cpBodySetMoment(cpBody *body, cpFloat i);
-void cpBodySetAngle(cpBody *body, cpFloat a);
-
- -

Because several of the values are linked, (m/m_inv, i/i_inv, a/rot) don’t set them explicitly, use these setter functions instead.

- - -

-cpVect cpBodyLocal2World(cpBody *body, cpVect v)
-
- -

Convert from body local coordinates to world space coordinates.

- - -

-cpVect cpBodyWorld2Local(cpBody *body, cpVect v)
-
- -

Convert from world space coordinates to body local coordinates.

- - -

-void cpBodyApplyImpulse(cpBody *body, cpVect j, cpVect r)
-
- -

Apply the impulse j to body with offset r. Both j and r should be in world coordinates.

- - -

-void cpBodyResetForces(cpBody *body)
-
- -

Zero both the forces and torques accumulated on body.

- - -

-void cpBodyApplyForce(cpBody *body, cpVect f, cpVect r)
-
- -

Apply (accumulate) the force f on body with offset r. Both f and r should be in world coordinates.

- - -

-void cpBodyUpdateVelocity(cpBody *body, cpVect gravity, cpFloat damping, cpFloat dt)
-
- -

Updates the velocity of the body using Euler integration. You don’t need to call this unless you are managing the object manually instead of adding it to a cpSpace.

- - -

-void cpBodyUpdatePosition(cpBody *body, cpFloat dt)
-
- -

Updates the position of the body using Euler integration. Like cpBodyUpdateVelocity() you shouldn’t normally need to call this yourself.

- - -

-void cpDampedSpring(cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2, cpFloat rlen, cpFloat k, cpFloat dmp, cpFloat dt)
-
- -

Apply a spring force between bodies a and b at anchors anchr1 and anchr2 respectively. k is the spring constant (force/distance), rlen is the rest length of the spring, dmp is the damping constant (force/velocity), and dt is the time step to apply the force over. Note: not solving the damping forces in the impulse solver causes problems with large damping values. This function will eventually be replaced by a new constraint (joint) type.

- - -

Notes:

- - -
    -
  • Use forces to modify the rigid bodies if possible. This is likely to be the most stable.
    • -
  • Modifying a body’s velocity shouldn’t necessarily be avoided, but applying large changes can cause strange results in the simulation. Experiment freely, but be warned.
    • -
  • Don’t modify a body’s position every step unless you really know what you are doing. Otherwise you’re likely to get the position/velocity badly out of sync.
    • -
- - -

Chipmunk collision shapes: cpShape

- - -

There are currently 3 possible collision shapes:

- - -
    -
  • Circles: Fastest collision shape. They also roll smoothly.
    • -
  • Line segments: Meant mainly as a static shape. They can be attached to moving bodies, but they don’t generate collisions with other line segments.
    • -
  • Convex polygons: Slowest, but most flexible collision shape.
    • -
- - -

User accessible fields:

- - -

-typedef struct cpShape{
-    cpBB bb;
-
-    unsigned long collision_type;
-    unsigned long group;
-    unsigned long layers;
-
-    void *data;
-
-    cpBody *body;
-    cpFloat e, u;
-    cpVect surface_v;
-} cpShape;
-
- -
    -
  • bb: The bounding box of the shape. Only guaranteed to be valid after cpShapeCacheBB() is called.
    • -
  • collision_type: A user definable field, see the collision pair function section below for more information.
    • -
  • group: Shapes in the same non-zero group do not generate collisions. Useful when creating an object out of many shapes that you don’t want to self collide. Defaults to 0;
    • -
  • layers: Shapes only collide if they are in the same bit-planes. i.e. (a->layers & b->layers) != 0 By default, a shape occupies all 32 bit-planes.
    • -
  • data: A user definable field.
    • -
  • body: The rigid body the shape is attached to.
    • -
  • e: Elasticity of the shape. A value of 0.0 gives no bounce, while a value of 1.0 will give a “perfect” bounce. However due to inaccuracies in the simulation using 1.0 or greater is not recommended however. See the notes at the end of the section.
    • -
  • u: Friction coefficient. Chipmunk uses the Coulomb friction model, a value of 0.0 is frictionless. Tables of friction coefficients. See the notes at the end of the section.
    • -
  • surface_v: The surface velocity of the object. Useful for creating conveyor belts or players that move around. This value is only used when calculating friction, not the collision.
    • -
- - -

-void cpShapeDestroy(cpShape *shape)
-void cpShapeFree(cpShape *shape)
-
- -

Destroy and Free functions are shared by all shape types.

- - -

-cpBB cpShapeCacheBB(cpShape *shape)
-
- -

Updates and returns the bounding box of shape.

- - -

-void cpResetShapeIdCounter(void)
-
- -

Chipmunk keeps a counter so that every new shape is given a unique hash value to be used in the spatial hash. Because this affects the order in which the collisions are found and handled, you should reset the shape counter every time you populate a space with new shapes. If you don’t, there might be (very) slight differences in the simulation.

- - -

-cpCircleShape *cpCircleShapeAlloc(void)
-cpCircleShape *cpCircleShapeInit(cpCircleShape *circle, cpBody *body, cpVect offset, cpFloat radius)
-cpShape *cpCircleShapeNew(cpBody *body, cpVect offset, cpFloat radius)
-
- -

body is the body to attach the circle to, offset is the offset from the body’s center of gravity in body local coordinates.

- - -

-cpSegmentShape* cpSegmentShapeAlloc(void)
-cpSegmentShape* cpSegmentShapeInit(cpSegmentShape *seg, cpBody *body, cpVect a, cpVect b, cpFloat radius)
-cpShape* cpSegmentShapeNew(cpBody *body, cpVect a, cpVect b, cpFloat radius)
-
- -

body is the body to attach the segment to, a and b are the endpoints, and radius is the thickness of the segment.

- - -

-cpPolyShape *cpPolyShapeAlloc(void)
-cpPolyShape *cpPolyShapeInit(cpPolyShape *poly, cpBody *body, int numVerts, cpVect *verts, cpVect offset)
-cpShape *cpPolyShapeNew(cpBody *body, int numVerts, cpVect *verts, cpVect offset)
-
- -

body is the body to attach the poly to, verts is an array of cpVect’s defining a convex hull with a counterclockwise winding, offset is the offset from the body’s center of gravity in body local coordinates.

- - -Notes: -
    -
  • You can attach multiple collision shapes to a rigid body. This should allow you to create almost any shape you could possibly need.
    • -
  • Shapes attached to the same rigid body will never generate collisions. You don’t have to worry about overlap when attaching multiple shapes to a rigid body.
    • -
  • The amount of elasticity applied during a collision is determined by multiplying the elasticity of both shapes together. The same is done for determining the friction.
    • -
  • Make sure you add both the body and it’s collision shapes to a space. The exception is when you want to have a static body or a body that you integrate yourself. In that case, only add the shape.
    • -
- - -

Chipmunk joints: cpJoint

- - -

There are currently 4 kinds of joints:

- - -
    -
  • Pin Joints connect two rigid bodies with a solid pin or rod. It keeps the anchor points at a set distance from one another.
    • -
  • Slide Joints are like pin joints, but have a minimum and maximum distance. A chain could be modeled using this joint. It keeps the anchor points from getting to far apart, but will allow them to get closer together.
    • -
  • Pivot Joints simply allow two objects to pivot about a single point.
    • -
  • Groove Joints attach a point on one body to a groove on the other. Think of it as a sliding pivot joint.
    • -
- - -

-void cpJointDestroy(cpJoint *joint)
-void cpJointFree(cpJoint *joint)
-
- -

Destroy and Free functions are shared by all joint types.

- - -

-cpPinJoint *cpPinJointAlloc(void)
-cpPinJoint *cpPinJointInit(cpPinJoint *joint, cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2)
-cpJoint *cpPinJointNew(cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2)
-
- -

a and b are the two bodies to connect, and anchr1 and anchr2 are the anchor points on those bodies.

- - -

-cpSlideJoint *cpSlideJointAlloc(void)
-cpSlideJoint *cpSlideJointInit(cpSlideJoint *joint, cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2, cpFloat min, cpFloat max)
-cpJoint *cpSlideJointNew(cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2, cpFloat min, cpFloat max)
-
- -

a and b are the two bodies to connect, anchr1 and anchr2 are the anchor points on those bodies, and min and max define the allowed distances of the anchor points.

- - -

-cpPivotJoint *cpPivotJointAlloc(void)
-cpPivotJoint *cpPivotJointInit(cpPivotJoint *joint, cpBody *a, cpBody *b, cpVect pivot)
-cpJoint *cpPivotJointNew(cpBody *a, cpBody *b, cpVect pivot)
-
- -

a and b are the two bodies to connect, and pivot is the point in world coordinates of the pivot. Because the pivot location is given in world coordinates, you must have the bodies moved into the correct positions already.

- - -

-cpGrooveJoint *cpGrooveJointAlloc(void)
-cpGrooveJoint *cpGrooveJointInit(cpGrooveJoint *joint, cpBody *a, cpBody *b, cpVect groove_a, cpVect groove_b, cpVect anchr2)
-cpJoint *cpGrooveJointNew(cpBody *a, cpBody *b, cpVect groove_a, cpVect groove_b, cpVect anchr2)
-
- -

The groove goes from groov_a to groove_b on body a_, and the pivot is attached to _anchr2 on body b. All coordinates are body local.

- - -

Notes:

- - -
    -
  • You can add multiple joints between two bodies, but make sure that they don’t fight. It can cause the bodies to explode.
    • -
  • Make sure you add both of the connected bodies and the joint to a space.
    • -
- - -

Chipmunk spaces: cpSpace

- - -

User accessible fields:

- - -

-typedef struct cpSpace{
-    int iterations;
-
-    cpVect gravity;
-    cpFloat damping;
-
-    int stamp;
-} cpSpace;
-
- -
    -
  • iterations: The number of iterations to use when solving constraints (collisions and joints). Defaults to 10.
    • -
  • gravity: The amount of gravity applied to the system.
    • -
  • damping: The amount of viscous damping applied to the system.
    • -
  • stamp: The tick stamp. Incremented every time cpSpaceStep() is called. read only
    • -
- - -

-void cpSpaceFreeChildren(cpSpace *space)
-
- -

Frees all bodies, shapes and joints added to the system.

- - -

-cpSpace* cpSpaceAlloc(void)
-cpSpace* cpSpaceInit(cpSpace *space, int iterations)
-cpSpace* cpSpaceNew(int iterations)
-
-void cpSpaceDestroy(cpSpace *space)
-void cpSpaceFree(cpSpace *space)
-
- -

More standard Chipmunk memory functions.

- - -

-void cpSpaceFreeChildren(cpSpace *space)
-
- -

This function will free all of the shapes, bodies and joints that have been added to space.

- - -

-void cpSpaceAddShape(cpSpace *space, cpShape *shape)
-void cpSpaceAddStaticShape(cpSpace *space, cpShape *shape)
-void cpSpaceAddBody(cpSpace *space, cpBody *body)
-void cpSpaceAddJoint(cpSpace *space, cpJoint *joint)
-
-void cpSpaceRemoveShape(cpSpace *space, cpShape *shape)
-void cpSpaceRemoveStaticShape(cpSpace *space, cpShape *shape)
-void cpSpaceRemoveBody(cpSpace *space, cpBody *body)
-void cpSpaceRemoveJoint(cpSpace *space, cpJoint *joint)
-
- -

These functions add and remove shapes, bodies and joints from space. Shapes added as static are assumed not to move. Static shapes should be be attached to a rigid body with an infinite mass and moment of inertia. Also, don’t add the rigid body used to the space, as that will cause it to fall under the effects of gravity.

- - -

-void cpSpaceResizeStaticHash(cpSpace *space, cpFloat dim, int count)
-void cpSpaceResizeActiveHash(cpSpace *space, cpFloat dim, int count)
-
- -

The spatial hashes used by Chipmunk’s collision detection are fairly size sensitive. dim is the size of the hash cells. Setting dim to the average objects size is likely to give the best performance.

- - -

count is the suggested minimum number of cells in the hash table. Bigger is better, but only to a point. Setting count to ~10x the number of objects in the hash is probably a good starting point.

- - -

By default, dim is 100.0, and count is 1000.

- - -

-void cpSpaceRehashStatic(cpSpace *space)
-
- -

Rehashes the shapes in the static spatial hash. You only need to call this if you move one of the static shapes.

- - -

-void cpSpaceStep(cpSpace *space, cpFloat dt)
-
- -

Update the space for the given time step. Using a fixed time step is highly recommended. Doing so will increase the efficiency of the contact persistence, requiring an order of magnitude fewer iterations to resolve the collisions in the usual case.

- - -

Notes:

- - -
    -
  • When removing objects from the space, make sure you remove any other objects that reference it. For instance, when you remove a body, remove the joints and shapes attached to it.
    • -
  • The number of iterations, and the size of the time step determine the quality of the simulation. More iterations, or smaller time steps increase the quality.
    • -
  • Because static shapes are only rehashed when you request it, it’s possible to use a much higher count argument to cpHashResizeStaticHash() than to cpHashResizeStaticHash(). Doing so will use more memory though.
    • -
- - -

Miscellaneous.

- - -

-cpFloat cpMomentForCircle(cpFloat m, cpFloat r1, cpFloat r2, cpVect offset)
-
- -

Calculate the moment of inertia for a circle. Arguments are similar to cpCircleShapeInit(). m_ is the mass, _r1 and r2 define an inner and outer radius, and offset is the offset of the center from the center of gravity of the rigid body.

- - -

-cpFloat cpMomentForPoly(cpFloat m, int numVerts, cpVect *verts, cpVect offset)
-
- -

Calculate the moment of inertia for a poly. Arguments are similar to cpPolyShapeInit() m_ is the mass, _numVerts is the number of vertexes in verts, and offset is the offset of the poly coordinates from the center of gravity of the rigid body.

- - -

Collision pair functions

- - -

Collision pair functions allow you to add callbacks for certain collision events. Each cpShape structure has a user definable collision_type field that is used to identify its type. For instance, you could define an enumeration of collision types such as bullets and players, and then register a collision pair function to reduce the players health when a collision is found between the two.

- - -

Additionally, the return value of a collision pair function determines whether or not a collision will be processed. If the function returns false, the collision will be ignored. One use for this functionality is to allow a rock object to break a vase object. If the approximated energy of the collision is above a certain level, flag the vase to be removed from the space, apply an impulse to the rock to slow it down, and return false. After the cpSpaceStep() returns, remove the vase from the space.

- - -

WARNING: It is not safe for collision pair functions to remove or free shapes or bodies from a space. Doing so will likely end in a segfault as an earlier collision may already be referencing the shape or body. You must wait until after the cpSpaceStep() function returns.

- - -

-typedef struct cpContact{
-    cpVect p, n;
-    cpFloat dist;
-
-    cpFloat jnAcc, jtAcc;
-} cpContact;
-
- -

An array of cpContact structs are passed to collision pair functions. Some user accessible fields include:

- - -
    -
  • p: position of the collision.
    • -
  • n: normal of the collision.
    • -
  • dist: penetration distance of the collision.
    • -
  • jnAcc and jtAcc: The normal and tangential components of the accumulated (final) impulse applied to resolve the collision. Values will not be valid until after the call to cpSpaceStep() returns.
    • -
- - -

-typedef int (*cpCollFunc)(cpShape *a, cpShape *b, cpContact *contacts, int numContacts, cpFloat normal_coef, void *data)
-
- -

Prototype for a collision callback function. The two colliding shapes are passed as a and b, along with the contact points, and a user definable pointer are passed as arguments. The shapes are passed in the same order as their types were registered using cpSpaceAddCollisionPairFunc(). Because Chipmunk may swap the shapes to accommodate your collision pair function the normals may be backwards. Always multiply the normals by normal_coef before using the values. The contacts array may be freed on the next call to cpSpaceStep().

- - -

-void cpSpaceAddCollisionPairFunc(cpSpace *space, unsigned long a, unsigned long b, cpCollFunc func, void *data)
-
- -

Register func to be called when a collision is found between a shapes with collision_type fields that match a and b. data is passed to func as a parameter. The ordering of the collision types will match the ordering passed to the callback function.

- - -

Passing NULL for func will reject any collision with the given collision type pair.

- - -

-void cpSpaceRemoveCollisionPairFunc(cpSpace *space, unsigned long a, unsigned long b)
-
- -

Remove the function for the given collision type pair. The order of a and b must match the original order used with cpSpaceAddCollisionPairFunc().

- - -

-void cpSpaceSetDefaultCollisionPairFunc(cpSpace *space, cpCollFunc func, void *data)
-
- -

The default function is called when no collision pair function is specified. By default, the default function simply accepts all collisions. Passing NULL for func will reset the default function back to the default. (You know what I mean.)

- - -

Passing NULL for func will reject collisions by default.

- - -

Advanced topics

- - -

Advanced collision processing

- - -

Using collision pair functions, it’s possible to get information about a collision such as the locations of the contact points and the normals, but you can’t get the impulse applied to each contact point at the time the callback is called. Because Chipmunk caches the impulses, it is possible to access them after the call to cpSpaceStep() returns.

- - -

The impulse information is stored in the jnAcc and jtAcc fields of the cpContact structures passed to the collision pair function. So you must store a reference to the contacts array in the collision pair function so that you can access it later once the impulses have been calculated.

- - -

-cpVect cpContactsSumImpulses(cpContact *contacts, int numContacts);
-cpVect cpContactsSumImpulsesWithFriction(cpContact *contacts, int numContacts);
-
- -

Sums the impulses applied to the the given contact points. cpContactsSumImpulses() sums only the normal components, while cpContactsSumImpulsesWithFriction() sums the normal and tangential componets.

- - -

Notes:

- - -
    -
  • The contact array will either be destroyed or out of date after the next call to cpSpaceStep(). If you need to store the collision information permanently, you’ll have to copy it.
    • -
- - -

Collision pair functions, groups, and layers

- - -

There are three ways that you can specify which shapes are allowed to collide in Chipmunk: collision pair functions, groups, and layers. What are their intended purposes?

- - -

Collision pair functions: More than just a callback, collision pair functions can conditionally allow a collision between two shapes. This is the only choice if you need to perform some logic based on the shapes or contact information before deciding to allow the collision.

- - -

Groups: Groups filter out collisions between objects in the same non-zero groups. A good example of when groups would be useful is to create a multi-body, multi-shape object such as a ragdoll. You don’t want the parts of the ragdoll to collide with itself, but you do want it to collide with other ragdolls.

- - -

Layers: Layers are another way of grouping shapes. Collision between shapes that don’t occupy one or more of the same layers are filtered out. Layers are implemented using a bitmask on an unsigned long, so there are up to 32 layers available.

- - -

To be clear, for a collision to occur, all of the tests have to pass. Additionally, collisions between static shapes are not considered nor are collisions between shapes connected to the same rigid body.

- - -

Mysteries of cpSpaceStep() explained.

- - -

The cpSpaceStep() function is really the workhorse of Chipmunk. So what exactly does it do?

- - -
    -
  • Persistent contacts with a stamp that is out of date by more than cp_contact_persistence are filtered out.
    • -
  • Velocities of all rigid bodies in the space are integrated using cpBodyUpdateVelocity().
    • -
  • All active shapes have their bounding boxes calculated and cached.
    • -
  • Collisions between the active shapes and static shapes are found. (collision pair functions are called)
    • -
  • Collisions between the active shapes are found. (collision pair functions are called)
    • -
  • Information about all constraints (collisions and joints) is pre-calculated.
    • -
  • Impulses are found and applied to solve all constraints (joints and collisions).
    • -
  • Positions of all rigid bodies in the space are integrated using cpBodyUpdatePosition().
    • -
- - -

Chipmunk does a lot of processing on the shapes in the system in order to provide robust and fast collision detection, but the same is not true of the rigid bodies. Really all cpSpaceStep() does to the bodies in the system is to integrate them and apply impulses to them.

- - -

The integration step is really just for convenience. If you integrate the velocity of your rigid bodies before calling cpSpaceStep() and integrate their positions afterward, you don’t have to add them to the space at all. In fact, there are good reasons for not doing so. All bodies in the space are integrated using the same gravity and damping, and this prevents you from providing interesting objects such as moving platforms.

- - -

If you are going to do the integration for your rigid bodies manually, I would highly recommend that you use cpBodyUpdatePosition() to integrate the position of the objects and only integrate the velocity. cpBodyUpdatePosition() is a required step for penetration resolution, and weird things could happen if it’s not called. Chipmunk uses Euler integration, so calculating the velocity required to move a body to a specific position is trivial if that is what you wish to do.

- - -

Chipmunk globals.

- - -

Chipmunk was designed with multithreading in mind, so very few globals are used. This shouldn’t be a problem in most cases as it’s likely you’ll only need to set them on a per application basis (if at all).

- - -

int cp_contact_persistence: This determines how long contacts should persist. This number should be fairly small as the cached contacts will only be close for a short time. cp_contact_persistence defaults to 3 as it is large enough to help prevent oscillating contacts, but doesn’t allow stale contact information to be used.

- - -

cpFloat cp_collision_slop: The amount that shapes are allowed to penetrate. Setting this to zero will work just fine, but using a small positive amount will help prevent oscillating contacts. cp_collision_slop defaults to 0.1.

- - -

cpFloat cp_bias_coef: The amount of penetration to reduce in each step. Values should range from 0 to 1. Using large values will eliminate penetration in fewer steps, but can cause vibration. cp_bias_coef defaults to 0.1.

- - -

cpFloat cp_joint_bias_coef: Similar to cp_bias_coef, but for joints. Defaults to 0.1. In the future, joints might have their own bias coefficient instead.

- - -

Known Problems

- - -
    -
  • Fast moving objects sometimes pass right through one another. This is because I’m not doing swept volume collisions. Use smaller time steps if this is a problem.
    • -
  • Pointy or thin polygons aren’t handled well. Collision points are generated at the poly’s vertexes. I haven’t decided what to do about this yet.
    • -
  • Elastic shapes don’t stack well.
    • -
- - -

Performance/Quality tuning

- - -

There are a number of things you can do to increase the performance of Chipmunk:

- - -
    -
  • Use simpler collision shapes when possible. It’s easier on the collision detection, you’ll generate fewer contact points, and require fewer iterations.
    • -
  • Make sure you have your spatial hashes tuned properly. It may take some experimenting to find the optimal dim/count parameters.
    • -
  • Use fewer iterations.
    • -
  • Use a larger time step.
    • -
  • Use a fixed time step. This allows contact persistence to work properly, reducing the number of iterations you need by an order of magnitude.
    • -
- - -

If you have problems with jittery or vibrating objects, you need to do mostly the opposite:

- - -
    -
  • Use simpler shapes. Simpler shapes generate fewer contacts, increasing the efficiency of each iteration.
    • -
  • Use more iterations.
    • -
  • Use a smaller time step.
    • -
  • Use a fixed time step. By allowing the contact persistence to work properly, you can use more iterations without increasing the computational cost.
    • -
- - -

To do

- - -There are a number of things left on my todo list: -
    -
  • More joint types.
    • -
  • Post collision queries. Being able to query for all the impulses that were applied to an object would be useful.
    • -
  • Python binding. (possibly others if I can get help)
    • -
- - -

Contact information

- - -

Drop me a line. Tell me how great you think Chipmunk is or how terribly buggy it is. I’d love to hear about any project that people are using Chipmunk for.

- - -

AIM: slembcke@mac.com

- - -

Email: lemb0029(at)morris(dot)umn(dot)edu or slembcke(at)gmail(dot)com

- - -

I can also be found lurking about the iDevGames forums.

- - -

License

- - -

Copyright© 2007 Scott Lembcke

- - -

Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the “Software”), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:

- - -

The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.

- - -THE SOFTWARE IS PROVIDED “AS IS”, WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. - - - diff --git a/chipmunk/include/chipmunk/chipmunk.h b/chipmunk/include/chipmunk/chipmunk.h deleted file mode 100644 index 6337fb1a13..0000000000 --- a/chipmunk/include/chipmunk/chipmunk.h +++ /dev/null @@ -1,222 +0,0 @@ -/* Copyright (c) 2007 Scott Lembcke - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -#ifndef CHIPMUNK_HEADER -#define CHIPMUNK_HEADER - -#ifdef _MSC_VER - #define _USE_MATH_DEFINES -#endif - -#include -#include - -#ifdef __cplusplus -extern "C" { -#endif - -#ifndef CP_ALLOW_PRIVATE_ACCESS - #define CP_ALLOW_PRIVATE_ACCESS 0 -#endif - -#if CP_ALLOW_PRIVATE_ACCESS == 1 - #define CP_PRIVATE(__symbol__) __symbol__ -#else - #define CP_PRIVATE(__symbol__) __symbol__##_private -#endif - -void cpMessage(const char *condition, const char *file, int line, int isError, int isHardError, const char *message, ...); -#ifdef NDEBUG - #define cpAssertWarn(__condition__, ...) -#else - #define cpAssertWarn(__condition__, ...) if(!(__condition__)) cpMessage(#__condition__, __FILE__, __LINE__, 0, 0, __VA_ARGS__) -#endif - -#ifdef NDEBUG - #define cpAssertSoft(__condition__, ...) -#else - #define cpAssertSoft(__condition__, ...) if(!(__condition__)) cpMessage(#__condition__, __FILE__, __LINE__, 1, 0, __VA_ARGS__) -#endif - -// Hard assertions are important and cheap to execute. They are not disabled by compiling as debug. -#define cpAssertHard(__condition__, ...) if(!(__condition__)) cpMessage(#__condition__, __FILE__, __LINE__, 1, 1, __VA_ARGS__) - - -#include "chipmunk_types.h" - -/// @defgroup misc Misc -/// @{ - -/// Allocated size for various Chipmunk buffers -#ifndef CP_BUFFER_BYTES - #define CP_BUFFER_BYTES (32*1024) -#endif - -#ifndef cpcalloc - /// Chipmunk calloc() alias. - #define cpcalloc calloc -#endif - -#ifndef cprealloc - /// Chipmunk realloc() alias. - #define cprealloc realloc -#endif - -#ifndef cpfree - /// Chipmunk free() alias. - #define cpfree free -#endif - -typedef struct cpArray cpArray; -typedef struct cpHashSet cpHashSet; - -typedef struct cpBody cpBody; -typedef struct cpShape cpShape; -typedef struct cpConstraint cpConstraint; - -typedef struct cpCollisionHandler cpCollisionHandler; -typedef struct cpArbiter cpArbiter; - -typedef struct cpSpace cpSpace; - -#include "cpVect.h" -#include "cpBB.h" -#include "cpSpatialIndex.h" - -#include "cpBody.h" -#include "cpShape.h" -#include "cpPolyShape.h" - -#include "cpArbiter.h" -#include "constraints/cpConstraint.h" - -#include "cpSpace.h" - -// Chipmunk 6.2.1 -#define CP_VERSION_MAJOR 6 -#define CP_VERSION_MINOR 2 -#define CP_VERSION_RELEASE 1 - -/// Version string. -extern const char *cpVersionString; - -/// @deprecated -void cpInitChipmunk(void); - -/// Enables segment to segment shape collisions. -void cpEnableSegmentToSegmentCollisions(void); - - -/// Calculate the moment of inertia for a circle. -/// @c r1 and @c r2 are the inner and outer diameters. A solid circle has an inner diameter of 0. -cpFloat cpMomentForCircle(cpFloat m, cpFloat r1, cpFloat r2, cpVect offset); - -/// Calculate area of a hollow circle. -/// @c r1 and @c r2 are the inner and outer diameters. A solid circle has an inner diameter of 0. -cpFloat cpAreaForCircle(cpFloat r1, cpFloat r2); - -/// Calculate the moment of inertia for a line segment. -/// Beveling radius is not supported. -cpFloat cpMomentForSegment(cpFloat m, cpVect a, cpVect b); - -/// Calculate the area of a fattened (capsule shaped) line segment. -cpFloat cpAreaForSegment(cpVect a, cpVect b, cpFloat r); - -/// Calculate the moment of inertia for a solid polygon shape assuming it's center of gravity is at it's centroid. The offset is added to each vertex. -cpFloat cpMomentForPoly(cpFloat m, int numVerts, const cpVect *verts, cpVect offset); - -/// Calculate the signed area of a polygon. A Clockwise winding gives positive area. -/// This is probably backwards from what you expect, but matches Chipmunk's the winding for poly shapes. -cpFloat cpAreaForPoly(const int numVerts, const cpVect *verts); - -/// Calculate the natural centroid of a polygon. -cpVect cpCentroidForPoly(const int numVerts, const cpVect *verts); - -/// Center the polygon on the origin. (Subtracts the centroid of the polygon from each vertex) -void cpRecenterPoly(const int numVerts, cpVect *verts); - -/// Calculate the moment of inertia for a solid box. -cpFloat cpMomentForBox(cpFloat m, cpFloat width, cpFloat height); - -/// Calculate the moment of inertia for a solid box. -cpFloat cpMomentForBox2(cpFloat m, cpBB box); - -/// Calculate the convex hull of a given set of points. Returns the count of points in the hull. -/// @c result must be a pointer to a @c cpVect array with at least @c count elements. If @c result is @c NULL, then @c verts will be reduced instead. -/// @c first is an optional pointer to an integer to store where the first vertex in the hull came from (i.e. verts[first] == result[0]) -/// @c tol is the allowed amount to shrink the hull when simplifying it. A tolerance of 0.0 creates an exact hull. -int cpConvexHull(int count, cpVect *verts, cpVect *result, int *first, cpFloat tol); - -#ifdef _MSC_VER -#include "malloc.h" -#endif - -/// Convenience macro to work with cpConvexHull. -/// @c count and @c verts is the input array passed to cpConvexHull(). -/// @c count_var and @c verts_var are the names of the variables the macro creates to store the result. -/// The output vertex array is allocated on the stack using alloca() so it will be freed automatically, but cannot be returned from the current scope. -#define CP_CONVEX_HULL(__count__, __verts__, __count_var__, __verts_var__) \ -cpVect *__verts_var__ = (cpVect *)alloca(__count__*sizeof(cpVect)); \ -int __count_var__ = cpConvexHull(__count__, __verts__, __verts_var__, NULL, 0.0); \ - -#if defined(__has_extension) -#if __has_extension(blocks) -// Define alternate block based alternatives for a few of the callback heavy functions. -// Collision handlers are post-step callbacks are not included to avoid memory management issues. -// If you want to use blocks for those and are aware of how to correctly manage the memory, the implementation is trivial. - -void cpSpaceEachBody_b(cpSpace *space, void (^block)(cpBody *body)); -void cpSpaceEachShape_b(cpSpace *space, void (^block)(cpShape *shape)); -void cpSpaceEachConstraint_b(cpSpace *space, void (^block)(cpConstraint *constraint)); - -void cpBodyEachShape_b(cpBody *body, void (^block)(cpShape *shape)); -void cpBodyEachConstraint_b(cpBody *body, void (^block)(cpConstraint *constraint)); -void cpBodyEachArbiter_b(cpBody *body, void (^block)(cpArbiter *arbiter)); - -typedef void (^cpSpaceNearestPointQueryBlock)(cpShape *shape, cpFloat distance, cpVect point); -void cpSpaceNearestPointQuery_b(cpSpace *space, cpVect point, cpFloat maxDistance, cpLayers layers, cpGroup group, cpSpaceNearestPointQueryBlock block); - -typedef void (^cpSpaceSegmentQueryBlock)(cpShape *shape, cpFloat t, cpVect n); -void cpSpaceSegmentQuery_b(cpSpace *space, cpVect start, cpVect end, cpLayers layers, cpGroup group, cpSpaceSegmentQueryBlock block); - -typedef void (^cpSpaceBBQueryBlock)(cpShape *shape); -void cpSpaceBBQuery_b(cpSpace *space, cpBB bb, cpLayers layers, cpGroup group, cpSpaceBBQueryBlock block); - -typedef void (^cpSpaceShapeQueryBlock)(cpShape *shape, cpContactPointSet *points); -cpBool cpSpaceShapeQuery_b(cpSpace *space, cpShape *shape, cpSpaceShapeQueryBlock block); - -#endif -#endif - - -//@} - -#ifdef __cplusplus -} - -static inline cpVect operator *(const cpVect v, const cpFloat s){return cpvmult(v, s);} -static inline cpVect operator +(const cpVect v1, const cpVect v2){return cpvadd(v1, v2);} -static inline cpVect operator -(const cpVect v1, const cpVect v2){return cpvsub(v1, v2);} -static inline cpBool operator ==(const cpVect v1, const cpVect v2){return cpveql(v1, v2);} -static inline cpVect operator -(const cpVect v){return cpvneg(v);} - -#endif -#endif diff --git a/chipmunk/include/chipmunk/chipmunk_ffi.h b/chipmunk/include/chipmunk/chipmunk_ffi.h deleted file mode 100644 index 53ac6937c2..0000000000 --- a/chipmunk/include/chipmunk/chipmunk_ffi.h +++ /dev/null @@ -1,177 +0,0 @@ -#ifdef CHIPMUNK_FFI - -// Create non static inlined copies of Chipmunk functions, useful for working with dynamic FFIs -// This file should only be included in chipmunk.c - -#ifdef _MSC_VER - #if _MSC_VER >= 1600 - #define MAKE_REF(name) decltype(name) *_##name = name - #else - #define MAKE_REF(name) - #endif -#else - #define MAKE_REF(name) __typeof__(name) *_##name = name -#endif - -#define MAKE_PROPERTIES_REF(struct, property) \ - MAKE_REF(struct##Get##property); MAKE_REF(struct##Set##property) - -MAKE_REF(cpv); // makes a variable named _cpv that contains the function pointer for cpv() -MAKE_REF(cpveql); -MAKE_REF(cpvadd); -MAKE_REF(cpvneg); -MAKE_REF(cpvsub); -MAKE_REF(cpvmult); -MAKE_REF(cpvdot); -MAKE_REF(cpvcross); -MAKE_REF(cpvperp); -MAKE_REF(cpvrperp); -MAKE_REF(cpvproject); -MAKE_REF(cpvforangle); -MAKE_REF(cpvtoangle); -MAKE_REF(cpvrotate); -MAKE_REF(cpvunrotate); -MAKE_REF(cpvlengthsq); -MAKE_REF(cpvlength); -MAKE_REF(cpvlerp); -MAKE_REF(cpvnormalize); -MAKE_REF(cpvnormalize_safe); -MAKE_REF(cpvclamp); -MAKE_REF(cpvlerpconst); -MAKE_REF(cpvdist); -MAKE_REF(cpvdistsq); -MAKE_REF(cpvnear); - -MAKE_REF(cpfmax); -MAKE_REF(cpfmin); -MAKE_REF(cpfabs); -MAKE_REF(cpfclamp); -MAKE_REF(cpflerp); -MAKE_REF(cpflerpconst); - -MAKE_REF(cpBBNew); -MAKE_REF(cpBBNewForCircle); -MAKE_REF(cpBBIntersects); -MAKE_REF(cpBBContainsBB); -MAKE_REF(cpBBContainsVect); -MAKE_REF(cpBBMerge); -MAKE_REF(cpBBExpand); -MAKE_REF(cpBBArea); -MAKE_REF(cpBBMergedArea); -MAKE_REF(cpBBSegmentQuery); -MAKE_REF(cpBBIntersectsSegment); -MAKE_REF(cpBBClampVect); - -MAKE_REF(cpBodyGetMass); -MAKE_REF(cpBodyGetMoment); -MAKE_REF(cpBodyGetPos); -MAKE_REF(cpBodyGetAngle); -MAKE_REF(cpBodyGetRot); -MAKE_PROPERTIES_REF(cpBody, Vel); -MAKE_PROPERTIES_REF(cpBody, Force); -MAKE_PROPERTIES_REF(cpBody, AngVel); -MAKE_PROPERTIES_REF(cpBody, Torque); -MAKE_PROPERTIES_REF(cpBody, VelLimit); -MAKE_PROPERTIES_REF(cpBody, AngVelLimit); -MAKE_PROPERTIES_REF(cpBody, UserData); -MAKE_REF(cpBodyIsSleeping); -MAKE_REF(cpBodyIsStatic); -MAKE_REF(cpBodyIsRogue); -MAKE_REF(cpBodyLocal2World); -MAKE_REF(cpBodyWorld2Local); -MAKE_REF(cpBodyKineticEnergy); - -MAKE_REF(cpShapeGetBB); -MAKE_PROPERTIES_REF(cpShape, Body); -MAKE_PROPERTIES_REF(cpShape, Sensor); -MAKE_PROPERTIES_REF(cpShape, Elasticity); -MAKE_PROPERTIES_REF(cpShape, Friction); -MAKE_PROPERTIES_REF(cpShape, SurfaceVelocity); -MAKE_PROPERTIES_REF(cpShape, UserData); -MAKE_PROPERTIES_REF(cpShape, CollisionType); -MAKE_PROPERTIES_REF(cpShape, Group); -MAKE_PROPERTIES_REF(cpShape, Layers); - -MAKE_REF(cpArbiterGetShapes); -MAKE_REF(cpArbiterGetBodies); -MAKE_REF(cpArbiterIsFirstContact); -MAKE_REF(cpArbiterGetCount); - -MAKE_REF(cpConstraintGetA); -MAKE_REF(cpConstraintGetB); -MAKE_PROPERTIES_REF(cpConstraint, MaxForce); -MAKE_PROPERTIES_REF(cpConstraint, ErrorBias); -MAKE_PROPERTIES_REF(cpConstraint, MaxBias); -MAKE_PROPERTIES_REF(cpConstraint, UserData); -MAKE_REF(cpConstraintGetImpulse); - -MAKE_PROPERTIES_REF(cpDampedRotarySpring, RestAngle); -MAKE_PROPERTIES_REF(cpDampedRotarySpring, Stiffness); -MAKE_PROPERTIES_REF(cpDampedRotarySpring, Damping); -//MAKE_PROPERTIES_REF(cpDampedRotarySpring, SpringTorqueFunc); - -MAKE_PROPERTIES_REF(cpDampedSpring, Anchr1); -MAKE_PROPERTIES_REF(cpDampedSpring, Anchr2); -MAKE_PROPERTIES_REF(cpDampedSpring, RestLength); -MAKE_PROPERTIES_REF(cpDampedSpring, Stiffness); -MAKE_PROPERTIES_REF(cpDampedSpring, Damping); -//MAKE_PROPERTIES_REF(cpDampedSpring, SpringForceFunc); - -MAKE_PROPERTIES_REF(cpGearJoint, Phase); -MAKE_REF(cpGearJointGetRatio); - -MAKE_PROPERTIES_REF(cpGrooveJoint, Anchr2); -MAKE_REF(cpGrooveJointGetGrooveA); -MAKE_REF(cpGrooveJointGetGrooveB); - -MAKE_PROPERTIES_REF(cpPinJoint, Anchr1); -MAKE_PROPERTIES_REF(cpPinJoint, Anchr2); -MAKE_PROPERTIES_REF(cpPinJoint, Dist); - -MAKE_PROPERTIES_REF(cpPivotJoint, Anchr1); -MAKE_PROPERTIES_REF(cpPivotJoint, Anchr2); - -MAKE_PROPERTIES_REF(cpRatchetJoint, Angle); -MAKE_PROPERTIES_REF(cpRatchetJoint, Phase); -MAKE_PROPERTIES_REF(cpRatchetJoint, Ratchet); - -MAKE_PROPERTIES_REF(cpRotaryLimitJoint, Min); -MAKE_PROPERTIES_REF(cpRotaryLimitJoint, Max); - -MAKE_PROPERTIES_REF(cpSimpleMotor, Rate); - -MAKE_PROPERTIES_REF(cpSlideJoint, Anchr1); -MAKE_PROPERTIES_REF(cpSlideJoint, Anchr2); -MAKE_PROPERTIES_REF(cpSlideJoint, Min); -MAKE_PROPERTIES_REF(cpSlideJoint, Max); - -MAKE_REF(cpSegmentQueryHitPoint); -MAKE_REF(cpSegmentQueryHitDist); - -MAKE_REF(cpSpatialIndexDestroy); -MAKE_REF(cpSpatialIndexCount); -MAKE_REF(cpSpatialIndexEach); -MAKE_REF(cpSpatialIndexContains); -MAKE_REF(cpSpatialIndexInsert); -MAKE_REF(cpSpatialIndexRemove); -MAKE_REF(cpSpatialIndexReindex); -MAKE_REF(cpSpatialIndexReindexObject); -MAKE_REF(cpSpatialIndexSegmentQuery); -MAKE_REF(cpSpatialIndexQuery); -MAKE_REF(cpSpatialIndexReindexQuery); - -MAKE_PROPERTIES_REF(cpSpace, Iterations); -MAKE_PROPERTIES_REF(cpSpace, Gravity); -MAKE_PROPERTIES_REF(cpSpace, Damping); -MAKE_PROPERTIES_REF(cpSpace, IdleSpeedThreshold); -MAKE_PROPERTIES_REF(cpSpace, SleepTimeThreshold); -MAKE_PROPERTIES_REF(cpSpace, CollisionSlop); -MAKE_PROPERTIES_REF(cpSpace, CollisionBias); -MAKE_PROPERTIES_REF(cpSpace, CollisionPersistence); -MAKE_PROPERTIES_REF(cpSpace, EnableContactGraph); -MAKE_PROPERTIES_REF(cpSpace, UserData); -MAKE_REF(cpSpaceGetStaticBody); -MAKE_REF(cpSpaceGetCurrentTimeStep); -MAKE_REF(cpSpaceIsLocked); - -#endif diff --git a/chipmunk/include/chipmunk/chipmunk_private.h b/chipmunk/include/chipmunk/chipmunk_private.h deleted file mode 100644 index f676345bc9..0000000000 --- a/chipmunk/include/chipmunk/chipmunk_private.h +++ /dev/null @@ -1,254 +0,0 @@ -/* Copyright (c) 2007 Scott Lembcke - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -#define CP_ALLOW_PRIVATE_ACCESS 1 -#include "chipmunk.h" - -#define CP_HASH_COEF (3344921057ul) -#define CP_HASH_PAIR(A, B) ((cpHashValue)(A)*CP_HASH_COEF ^ (cpHashValue)(B)*CP_HASH_COEF) - -// TODO: Eww. Magic numbers. -#define MAGIC_EPSILON 1e-5 - -//MARK: cpArray - -struct cpArray { - int num, max; - void **arr; -}; - -cpArray *cpArrayNew(int size); - -void cpArrayFree(cpArray *arr); - -void cpArrayPush(cpArray *arr, void *object); -void *cpArrayPop(cpArray *arr); -void cpArrayDeleteObj(cpArray *arr, void *obj); -cpBool cpArrayContains(cpArray *arr, void *ptr); - -void cpArrayFreeEach(cpArray *arr, void (freeFunc)(void*)); - - -//MARK: Foreach loops - -static inline cpConstraint * -cpConstraintNext(cpConstraint *node, cpBody *body) -{ - return (node->a == body ? node->next_a : node->next_b); -} - -#define CP_BODY_FOREACH_CONSTRAINT(bdy, var)\ - for(cpConstraint *var = bdy->constraintList; var; var = cpConstraintNext(var, bdy)) - -static inline cpArbiter * -cpArbiterNext(cpArbiter *node, cpBody *body) -{ - return (node->body_a == body ? node->thread_a.next : node->thread_b.next); -} - -#define CP_BODY_FOREACH_ARBITER(bdy, var)\ - for(cpArbiter *var = bdy->arbiterList; var; var = cpArbiterNext(var, bdy)) - -#define CP_BODY_FOREACH_SHAPE(body, var)\ - for(cpShape *var = body->shapeList; var; var = var->next) - -#define CP_BODY_FOREACH_COMPONENT(root, var)\ - for(cpBody *var = root; var; var = var->node.next) - - -//MARK: cpHashSet - -typedef cpBool (*cpHashSetEqlFunc)(void *ptr, void *elt); -typedef void *(*cpHashSetTransFunc)(void *ptr, void *data); - -cpHashSet *cpHashSetNew(int size, cpHashSetEqlFunc eqlFunc); -void cpHashSetSetDefaultValue(cpHashSet *set, void *default_value); - -void cpHashSetFree(cpHashSet *set); - -int cpHashSetCount(cpHashSet *set); -void *cpHashSetInsert(cpHashSet *set, cpHashValue hash, void *ptr, void *data, cpHashSetTransFunc trans); -void *cpHashSetRemove(cpHashSet *set, cpHashValue hash, void *ptr); -void *cpHashSetFind(cpHashSet *set, cpHashValue hash, void *ptr); - -typedef void (*cpHashSetIteratorFunc)(void *elt, void *data); -void cpHashSetEach(cpHashSet *set, cpHashSetIteratorFunc func, void *data); - -typedef cpBool (*cpHashSetFilterFunc)(void *elt, void *data); -void cpHashSetFilter(cpHashSet *set, cpHashSetFilterFunc func, void *data); - - -//MARK: Body Functions - -void cpBodyAddShape(cpBody *body, cpShape *shape); -void cpBodyRemoveShape(cpBody *body, cpShape *shape); -void cpBodyRemoveConstraint(cpBody *body, cpConstraint *constraint); - - -//MARK: Shape/Collision Functions - -// TODO should move this to the cpVect API. It's pretty useful. -static inline cpVect -cpClosetPointOnSegment(const cpVect p, const cpVect a, const cpVect b) -{ - cpVect delta = cpvsub(a, b); - cpFloat t = cpfclamp01(cpvdot(delta, cpvsub(p, b))/cpvlengthsq(delta)); - return cpvadd(b, cpvmult(delta, t)); -} - -cpShape* cpShapeInit(cpShape *shape, const cpShapeClass *klass, cpBody *body); - -static inline cpBool -cpShapeActive(cpShape *shape) -{ - return shape->prev || (shape->body && shape->body->shapeList == shape); -} - -int cpCollideShapes(const cpShape *a, const cpShape *b, cpCollisionID *id, cpContact *arr); - -static inline void -CircleSegmentQuery(cpShape *shape, cpVect center, cpFloat r, cpVect a, cpVect b, cpSegmentQueryInfo *info) -{ - cpVect da = cpvsub(a, center); - cpVect db = cpvsub(b, center); - - cpFloat qa = cpvdot(da, da) - 2.0f*cpvdot(da, db) + cpvdot(db, db); - cpFloat qb = -2.0f*cpvdot(da, da) + 2.0f*cpvdot(da, db); - cpFloat qc = cpvdot(da, da) - r*r; - - cpFloat det = qb*qb - 4.0f*qa*qc; - - if(det >= 0.0f){ - cpFloat t = (-qb - cpfsqrt(det))/(2.0f*qa); - if(0.0f<= t && t <= 1.0f){ - info->shape = shape; - info->t = t; - info->n = cpvnormalize(cpvlerp(da, db, t)); - } - } -} - -// TODO doesn't really need to be inline, but need a better place to put this function -static inline cpSplittingPlane -cpSplittingPlaneNew(cpVect a, cpVect b) -{ - cpVect n = cpvnormalize(cpvperp(cpvsub(b, a))); - cpSplittingPlane plane = {n, cpvdot(n, a)}; - return plane; -} - -static inline cpFloat -cpSplittingPlaneCompare(cpSplittingPlane plane, cpVect v) -{ - return cpvdot(plane.n, v) - plane.d; -} - -void cpLoopIndexes(cpVect *verts, int count, int *start, int *end); - - -//MARK: Spatial Index Functions - -cpSpatialIndex *cpSpatialIndexInit(cpSpatialIndex *index, cpSpatialIndexClass *klass, cpSpatialIndexBBFunc bbfunc, cpSpatialIndex *staticIndex); - - -//MARK: Space Functions - -extern cpCollisionHandler cpDefaultCollisionHandler; -void cpSpaceProcessComponents(cpSpace *space, cpFloat dt); - -void cpSpacePushFreshContactBuffer(cpSpace *space); -cpContact *cpContactBufferGetArray(cpSpace *space); -void cpSpacePushContacts(cpSpace *space, int count); - -typedef struct cpPostStepCallback { - cpPostStepFunc func; - void *key; - void *data; -} cpPostStepCallback; - -cpPostStepCallback *cpSpaceGetPostStepCallback(cpSpace *space, void *key); - -cpBool cpSpaceArbiterSetFilter(cpArbiter *arb, cpSpace *space); -void cpSpaceFilterArbiters(cpSpace *space, cpBody *body, cpShape *filter); - -void cpSpaceActivateBody(cpSpace *space, cpBody *body); -void cpSpaceLock(cpSpace *space); -void cpSpaceUnlock(cpSpace *space, cpBool runPostStep); - -static inline cpCollisionHandler * -cpSpaceLookupHandler(cpSpace *space, cpCollisionType a, cpCollisionType b) -{ - cpCollisionType types[] = {a, b}; - return (cpCollisionHandler *)cpHashSetFind(space->collisionHandlers, CP_HASH_PAIR(a, b), types); -} - -static inline void -cpSpaceUncacheArbiter(cpSpace *space, cpArbiter *arb) -{ - cpShape *a = arb->a, *b = arb->b; - cpShape *shape_pair[] = {a, b}; - cpHashValue arbHashID = CP_HASH_PAIR((cpHashValue)a, (cpHashValue)b); - cpHashSetRemove(space->cachedArbiters, arbHashID, shape_pair); - cpArrayDeleteObj(space->arbiters, arb); -} - -void cpShapeUpdateFunc(cpShape *shape, void *unused); -cpCollisionID cpSpaceCollideShapes(cpShape *a, cpShape *b, cpCollisionID id, cpSpace *space); - - -//MARK: Arbiters - -struct cpContact { - cpVect p, n; - cpFloat dist; - - cpVect r1, r2; - cpFloat nMass, tMass, bounce; - - cpFloat jnAcc, jtAcc, jBias; - cpFloat bias; - - cpHashValue hash; -}; - -cpContact* cpContactInit(cpContact *con, cpVect p, cpVect n, cpFloat dist, cpHashValue hash); -cpArbiter* cpArbiterInit(cpArbiter *arb, cpShape *a, cpShape *b); - -static inline void -cpArbiterCallSeparate(cpArbiter *arb, cpSpace *space) -{ - // The handler needs to be looked up again as the handler cached on the arbiter may have been deleted since the last step. - cpCollisionHandler *handler = cpSpaceLookupHandler(space, arb->a->collision_type, arb->b->collision_type); - handler->separate(arb, space, handler->data); -} - -static inline struct cpArbiterThread * -cpArbiterThreadForBody(cpArbiter *arb, cpBody *body) -{ - return (arb->body_a == body ? &arb->thread_a : &arb->thread_b); -} - -void cpArbiterUnthread(cpArbiter *arb); - -void cpArbiterUpdate(cpArbiter *arb, cpContact *contacts, int numContacts, struct cpCollisionHandler *handler, cpShape *a, cpShape *b); -void cpArbiterPreStep(cpArbiter *arb, cpFloat dt, cpFloat bias, cpFloat slop); -void cpArbiterApplyCachedImpulse(cpArbiter *arb, cpFloat dt_coef); -void cpArbiterApplyImpulse(cpArbiter *arb); diff --git a/chipmunk/include/chipmunk/chipmunk_types.h b/chipmunk/include/chipmunk/chipmunk_types.h deleted file mode 100644 index fdfb6d1eef..0000000000 --- a/chipmunk/include/chipmunk/chipmunk_types.h +++ /dev/null @@ -1,222 +0,0 @@ -#include -#include - -#ifdef __APPLE__ - #include "TargetConditionals.h" -#endif - -//#if ((TARGET_OS_IPHONE == 1) || (TARGET_OS_MAC == 1)) && (!defined CP_USE_CGPOINTS) -// #define CP_USE_CGPOINTS 1 -//#endif - -#if CP_USE_CGPOINTS == 1 - #if TARGET_OS_IPHONE - #import - #elif TARGET_OS_MAC - #include - #endif - - #if defined(__LP64__) && __LP64__ - #define CP_USE_DOUBLES 1 - #else - #define CP_USE_DOUBLES 0 - #endif -#endif - -#ifndef CP_USE_DOUBLES - // use doubles by default for higher precision - #define CP_USE_DOUBLES 1 -#endif - -/// @defgroup basicTypes Basic Types -/// Most of these types can be configured at compile time. -/// @{ - -#if CP_USE_DOUBLES -/// Chipmunk's floating point type. -/// Can be reconfigured at compile time. - typedef double cpFloat; - #define cpfsqrt sqrt - #define cpfsin sin - #define cpfcos cos - #define cpfacos acos - #define cpfatan2 atan2 - #define cpfmod fmod - #define cpfexp exp - #define cpfpow pow - #define cpffloor floor - #define cpfceil ceil - #define CPFLOAT_MIN DBL_MIN -#else - typedef float cpFloat; - #define cpfsqrt sqrtf - #define cpfsin sinf - #define cpfcos cosf - #define cpfacos acosf - #define cpfatan2 atan2f - #define cpfmod fmodf - #define cpfexp expf - #define cpfpow powf - #define cpffloor floorf - #define cpfceil ceilf - #define CPFLOAT_MIN FLT_MIN -#endif - -#ifndef INFINITY - #ifdef _MSC_VER - union MSVC_EVIL_FLOAT_HACK - { - unsigned __int8 Bytes[4]; - float Value; - }; - static union MSVC_EVIL_FLOAT_HACK INFINITY_HACK = {{0x00, 0x00, 0x80, 0x7F}}; - #define INFINITY (INFINITY_HACK.Value) - #endif - - #ifdef __GNUC__ - #define INFINITY (__builtin_inf()) - #endif - - #ifndef INFINITY - #define INFINITY (1e1000) - #endif -#endif - -#ifndef M_PI - #define M_PI 3.14159265358979323846264338327950288 -#endif - -#ifndef M_E - #define M_E 2.71828182845904523536028747135266250 -#endif - - -/// Return the max of two cpFloats. -static inline cpFloat cpfmax(cpFloat a, cpFloat b) -{ - return (a > b) ? a : b; -} - -/// Return the min of two cpFloats. -static inline cpFloat cpfmin(cpFloat a, cpFloat b) -{ - return (a < b) ? a : b; -} - -/// Return the absolute value of a cpFloat. -static inline cpFloat cpfabs(cpFloat f) -{ - return (f < 0) ? -f : f; -} - -/// Clamp @c f to be between @c min and @c max. -static inline cpFloat cpfclamp(cpFloat f, cpFloat min, cpFloat max) -{ - return cpfmin(cpfmax(f, min), max); -} - -/// Clamp @c f to be between 0 and 1. -static inline cpFloat cpfclamp01(cpFloat f) -{ - return cpfmax(0.0f, cpfmin(f, 1.0f)); -} - - - -/// Linearly interpolate (or extrapolate) between @c f1 and @c f2 by @c t percent. -static inline cpFloat cpflerp(cpFloat f1, cpFloat f2, cpFloat t) -{ - return f1*(1.0f - t) + f2*t; -} - -/// Linearly interpolate from @c f1 to @c f2 by no more than @c d. -static inline cpFloat cpflerpconst(cpFloat f1, cpFloat f2, cpFloat d) -{ - return f1 + cpfclamp(f2 - f1, -d, d); -} - -/// Hash value type. -typedef uintptr_t cpHashValue; - -/// Type used internally to cache colliding object info for cpCollideShapes(). -/// Should be at least 32 bits. -typedef uint32_t cpCollisionID; - -// Oh C, how we love to define our own boolean types to get compiler compatibility -/// Chipmunk's boolean type. -#ifdef CP_BOOL_TYPE - typedef CP_BOOL_TYPE cpBool; -#else - typedef int cpBool; -#endif - -#ifndef cpTrue -/// true value. - #define cpTrue 1 -#endif - -#ifndef cpFalse -/// false value. - #define cpFalse 0 -#endif - -#ifdef CP_DATA_POINTER_TYPE - typedef CP_DATA_POINTER_TYPE cpDataPointer; -#else -/// Type used for user data pointers. - typedef void * cpDataPointer; -#endif - -#ifdef CP_COLLISION_TYPE_TYPE - typedef CP_COLLISION_TYPE_TYPE cpCollisionType; -#else -/// Type used for cpSpace.collision_type. - typedef uintptr_t cpCollisionType; -#endif - -#ifdef CP_GROUP_TYPE - typedef CP_GROUP_TYPE cpGroup; -#else -/// Type used for cpShape.group. - typedef uintptr_t cpGroup; -#endif - -#ifdef CP_LAYERS_TYPE - typedef CP_LAYERS_TYPE cpLayers; -#else -/// Type used for cpShape.layers. - typedef unsigned int cpLayers; -#endif - -#ifdef CP_TIMESTAMP_TYPE - typedef CP_TIMESTAMP_TYPE cpTimestamp; -#else -/// Type used for various timestamps in Chipmunk. - typedef unsigned int cpTimestamp; -#endif - -#ifndef CP_NO_GROUP -/// Value for cpShape.group signifying that a shape is in no group. - #define CP_NO_GROUP ((cpGroup)0) -#endif - -#ifndef CP_ALL_LAYERS -/// Value for cpShape.layers signifying that a shape is in every layer. - #define CP_ALL_LAYERS (~(cpLayers)0) -#endif -/// @} - -// CGPoints are structurally the same, and allow -// easy interoperability with other Cocoa libraries -#if CP_USE_CGPOINTS - typedef CGPoint cpVect; -#else -/// Chipmunk's 2D vector type. -/// @addtogroup cpVect - typedef struct cpVect{cpFloat x,y;} cpVect; -#endif - -typedef struct cpMat2x2 { - // Row major [[a, b][c d]] - cpFloat a, b, c, d; -} cpMat2x2; diff --git a/chipmunk/include/chipmunk/chipmunk_unsafe.h b/chipmunk/include/chipmunk/chipmunk_unsafe.h deleted file mode 100644 index 4428814c07..0000000000 --- a/chipmunk/include/chipmunk/chipmunk_unsafe.h +++ /dev/null @@ -1,65 +0,0 @@ -/* Copyright (c) 2007 Scott Lembcke - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/* This header defines a number of "unsafe" operations on Chipmunk objects. - * In this case "unsafe" is referring to operations which may reduce the - * physical accuracy or numerical stability of the simulation, but will not - * cause crashes. - * - * The prime example is mutating collision shapes. Chipmunk does not support - * this directly. Mutating shapes using this API will caused objects in contact - * to be pushed apart using Chipmunk's overlap solver, but not using real - * persistent velocities. Probably not what you meant, but perhaps close enough. - */ - -/// @defgroup unsafe Chipmunk Unsafe Shape Operations -/// These functions are used for mutating collision shapes. -/// Chipmunk does not have any way to get velocity information on changing shapes, -/// so the results will be unrealistic. You must explicity include the chipmunk_unsafe.h header to use them. -/// @{ - -#ifndef CHIPMUNK_UNSAFE_HEADER -#define CHIPMUNK_UNSAFE_HEADER - -#ifdef __cplusplus -extern "C" { -#endif - -/// Set the radius of a circle shape. -void cpCircleShapeSetRadius(cpShape *shape, cpFloat radius); -/// Set the offset of a circle shape. -void cpCircleShapeSetOffset(cpShape *shape, cpVect offset); - -/// Set the endpoints of a segment shape. -void cpSegmentShapeSetEndpoints(cpShape *shape, cpVect a, cpVect b); -/// Set the radius of a segment shape. -void cpSegmentShapeSetRadius(cpShape *shape, cpFloat radius); - -/// Set the vertexes of a poly shape. -void cpPolyShapeSetVerts(cpShape *shape, int numVerts, cpVect *verts, cpVect offset); -/// Set the radius of a poly shape. -void cpPolyShapeSetRadius(cpShape *shape, cpFloat radius); - -#ifdef __cplusplus -} -#endif -#endif -/// @} diff --git a/chipmunk/include/chipmunk/constraints/cpConstraint.h b/chipmunk/include/chipmunk/constraints/cpConstraint.h deleted file mode 100644 index d1059fffab..0000000000 --- a/chipmunk/include/chipmunk/constraints/cpConstraint.h +++ /dev/null @@ -1,161 +0,0 @@ -/* Copyright (c) 2007 Scott Lembcke - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/// @defgroup cpConstraint cpConstraint -/// @{ - -typedef struct cpConstraintClass cpConstraintClass; - -typedef void (*cpConstraintPreStepImpl)(cpConstraint *constraint, cpFloat dt); -typedef void (*cpConstraintApplyCachedImpulseImpl)(cpConstraint *constraint, cpFloat dt_coef); -typedef void (*cpConstraintApplyImpulseImpl)(cpConstraint *constraint, cpFloat dt); -typedef cpFloat (*cpConstraintGetImpulseImpl)(cpConstraint *constraint); - -/// @private -struct cpConstraintClass { - cpConstraintPreStepImpl preStep; - cpConstraintApplyCachedImpulseImpl applyCachedImpulse; - cpConstraintApplyImpulseImpl applyImpulse; - cpConstraintGetImpulseImpl getImpulse; -}; - -/// Callback function type that gets called before solving a joint. -typedef void (*cpConstraintPreSolveFunc)(cpConstraint *constraint, cpSpace *space); -/// Callback function type that gets called after solving a joint. -typedef void (*cpConstraintPostSolveFunc)(cpConstraint *constraint, cpSpace *space); - - -/// Opaque cpConstraint struct. -struct cpConstraint { - CP_PRIVATE(const cpConstraintClass *klass); - - /// The first body connected to this constraint. - cpBody *a; - /// The second body connected to this constraint. - cpBody *b; - - CP_PRIVATE(cpSpace *space); - - CP_PRIVATE(cpConstraint *next_a); - CP_PRIVATE(cpConstraint *next_b); - - /// The maximum force that this constraint is allowed to use. - /// Defaults to infinity. - cpFloat maxForce; - /// The rate at which joint error is corrected. - /// Defaults to pow(1.0 - 0.1, 60.0) meaning that it will - /// correct 10% of the error every 1/60th of a second. - cpFloat errorBias; - /// The maximum rate at which joint error is corrected. - /// Defaults to infinity. - cpFloat maxBias; - - /// Function called before the solver runs. - /// Animate your joint anchors, update your motor torque, etc. - cpConstraintPreSolveFunc preSolve; - - /// Function called after the solver runs. - /// Use the applied impulse to perform effects like breakable joints. - cpConstraintPostSolveFunc postSolve; - - /// User definable data pointer. - /// Generally this points to your the game object class so you can access it - /// when given a cpConstraint reference in a callback. - cpDataPointer data; -}; - -/// Destroy a constraint. -void cpConstraintDestroy(cpConstraint *constraint); -/// Destroy and free a constraint. -void cpConstraintFree(cpConstraint *constraint); - -/// @private -static inline void cpConstraintActivateBodies(cpConstraint *constraint) -{ - cpBody *a = constraint->a; if(a) cpBodyActivate(a); - cpBody *b = constraint->b; if(b) cpBodyActivate(b); -} - -/// @private -#define CP_DefineConstraintStructGetter(type, member, name) \ -static inline type cpConstraint##Get##name(const cpConstraint *constraint){return constraint->member;} - -/// @private -#define CP_DefineConstraintStructSetter(type, member, name) \ -static inline void cpConstraint##Set##name(cpConstraint *constraint, type value){ \ - cpConstraintActivateBodies(constraint); \ - constraint->member = value; \ -} - -/// @private -#define CP_DefineConstraintStructProperty(type, member, name) \ -CP_DefineConstraintStructGetter(type, member, name) \ -CP_DefineConstraintStructSetter(type, member, name) - -CP_DefineConstraintStructGetter(cpSpace*, CP_PRIVATE(space), Space) - -CP_DefineConstraintStructGetter(cpBody*, a, A) -CP_DefineConstraintStructGetter(cpBody*, b, B) -CP_DefineConstraintStructProperty(cpFloat, maxForce, MaxForce) -CP_DefineConstraintStructProperty(cpFloat, errorBias, ErrorBias) -CP_DefineConstraintStructProperty(cpFloat, maxBias, MaxBias) -CP_DefineConstraintStructProperty(cpConstraintPreSolveFunc, preSolve, PreSolveFunc) -CP_DefineConstraintStructProperty(cpConstraintPostSolveFunc, postSolve, PostSolveFunc) -CP_DefineConstraintStructProperty(cpDataPointer, data, UserData) - -// Get the last impulse applied by this constraint. -static inline cpFloat cpConstraintGetImpulse(cpConstraint *constraint) -{ - return constraint->CP_PRIVATE(klass)->getImpulse(constraint); -} - -/// @} - -#define cpConstraintCheckCast(constraint, struct) \ - cpAssertHard(constraint->CP_PRIVATE(klass) == struct##GetClass(), "Constraint is not a "#struct) - -#define CP_DefineConstraintGetter(struct, type, member, name) \ -static inline type struct##Get##name(const cpConstraint *constraint){ \ - cpConstraintCheckCast(constraint, struct); \ - return ((struct *)constraint)->member; \ -} - -#define CP_DefineConstraintSetter(struct, type, member, name) \ -static inline void struct##Set##name(cpConstraint *constraint, type value){ \ - cpConstraintCheckCast(constraint, struct); \ - cpConstraintActivateBodies(constraint); \ - ((struct *)constraint)->member = value; \ -} - -#define CP_DefineConstraintProperty(struct, type, member, name) \ -CP_DefineConstraintGetter(struct, type, member, name) \ -CP_DefineConstraintSetter(struct, type, member, name) - -#include "cpPinJoint.h" -#include "cpSlideJoint.h" -#include "cpPivotJoint.h" -#include "cpGrooveJoint.h" -#include "cpDampedSpring.h" -#include "cpDampedRotarySpring.h" -#include "cpRotaryLimitJoint.h" -#include "cpRatchetJoint.h" -#include "cpGearJoint.h" -#include "cpSimpleMotor.h" diff --git a/chipmunk/include/chipmunk/constraints/cpDampedRotarySpring.h b/chipmunk/include/chipmunk/constraints/cpDampedRotarySpring.h deleted file mode 100644 index 86cf915b2d..0000000000 --- a/chipmunk/include/chipmunk/constraints/cpDampedRotarySpring.h +++ /dev/null @@ -1,56 +0,0 @@ -/* Copyright (c) 2007 Scott Lembcke - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/// @defgroup cpDampedRotarySpring cpDampedRotarySpring -/// @{ - -typedef cpFloat (*cpDampedRotarySpringTorqueFunc)(struct cpConstraint *spring, cpFloat relativeAngle); - -const cpConstraintClass *cpDampedRotarySpringGetClass(void); - -/// @private -typedef struct cpDampedRotarySpring { - cpConstraint constraint; - cpFloat restAngle; - cpFloat stiffness; - cpFloat damping; - cpDampedRotarySpringTorqueFunc springTorqueFunc; - - cpFloat target_wrn; - cpFloat w_coef; - - cpFloat iSum; - cpFloat jAcc; -} cpDampedRotarySpring; - -/// Allocate a damped rotary spring. -cpDampedRotarySpring* cpDampedRotarySpringAlloc(void); -/// Initialize a damped rotary spring. -cpDampedRotarySpring* cpDampedRotarySpringInit(cpDampedRotarySpring *joint, cpBody *a, cpBody *b, cpFloat restAngle, cpFloat stiffness, cpFloat damping); -/// Allocate and initialize a damped rotary spring. -cpConstraint* cpDampedRotarySpringNew(cpBody *a, cpBody *b, cpFloat restAngle, cpFloat stiffness, cpFloat damping); - -CP_DefineConstraintProperty(cpDampedRotarySpring, cpFloat, restAngle, RestAngle) -CP_DefineConstraintProperty(cpDampedRotarySpring, cpFloat, stiffness, Stiffness) -CP_DefineConstraintProperty(cpDampedRotarySpring, cpFloat, damping, Damping) -CP_DefineConstraintProperty(cpDampedRotarySpring, cpDampedRotarySpringTorqueFunc, springTorqueFunc, SpringTorqueFunc) - -/// @} diff --git a/chipmunk/include/chipmunk/constraints/cpDampedSpring.h b/chipmunk/include/chipmunk/constraints/cpDampedSpring.h deleted file mode 100644 index f5120647f9..0000000000 --- a/chipmunk/include/chipmunk/constraints/cpDampedSpring.h +++ /dev/null @@ -1,64 +0,0 @@ -/* Copyright (c) 2007 Scott Lembcke - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/// @defgroup cpDampedSpring cpDampedSpring -/// @{ - -typedef struct cpDampedSpring cpDampedSpring; - -typedef cpFloat (*cpDampedSpringForceFunc)(cpConstraint *spring, cpFloat dist); - -const cpConstraintClass *cpDampedSpringGetClass(void); - -/// @private -struct cpDampedSpring { - cpConstraint constraint; - cpVect anchr1, anchr2; - cpFloat restLength; - cpFloat stiffness; - cpFloat damping; - cpDampedSpringForceFunc springForceFunc; - - cpFloat target_vrn; - cpFloat v_coef; - - cpVect r1, r2; - cpFloat nMass; - cpVect n; - - cpFloat jAcc; -}; - -/// Allocate a damped spring. -cpDampedSpring* cpDampedSpringAlloc(void); -/// Initialize a damped spring. -cpDampedSpring* cpDampedSpringInit(cpDampedSpring *joint, cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2, cpFloat restLength, cpFloat stiffness, cpFloat damping); -/// Allocate and initialize a damped spring. -cpConstraint* cpDampedSpringNew(cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2, cpFloat restLength, cpFloat stiffness, cpFloat damping); - -CP_DefineConstraintProperty(cpDampedSpring, cpVect, anchr1, Anchr1) -CP_DefineConstraintProperty(cpDampedSpring, cpVect, anchr2, Anchr2) -CP_DefineConstraintProperty(cpDampedSpring, cpFloat, restLength, RestLength) -CP_DefineConstraintProperty(cpDampedSpring, cpFloat, stiffness, Stiffness) -CP_DefineConstraintProperty(cpDampedSpring, cpFloat, damping, Damping) -CP_DefineConstraintProperty(cpDampedSpring, cpDampedSpringForceFunc, springForceFunc, SpringForceFunc) - -/// @} diff --git a/chipmunk/include/chipmunk/constraints/cpGearJoint.h b/chipmunk/include/chipmunk/constraints/cpGearJoint.h deleted file mode 100644 index c9ebf94220..0000000000 --- a/chipmunk/include/chipmunk/constraints/cpGearJoint.h +++ /dev/null @@ -1,51 +0,0 @@ -/* Copyright (c) 2007 Scott Lembcke - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/// @defgroup cpGearJoint cpGearJoint -/// @{ - -const cpConstraintClass *cpGearJointGetClass(void); - -/// @private -typedef struct cpGearJoint { - cpConstraint constraint; - cpFloat phase, ratio; - cpFloat ratio_inv; - - cpFloat iSum; - - cpFloat bias; - cpFloat jAcc; -} cpGearJoint; - -/// Allocate a gear joint. -cpGearJoint* cpGearJointAlloc(void); -/// Initialize a gear joint. -cpGearJoint* cpGearJointInit(cpGearJoint *joint, cpBody *a, cpBody *b, cpFloat phase, cpFloat ratio); -/// Allocate and initialize a gear joint. -cpConstraint* cpGearJointNew(cpBody *a, cpBody *b, cpFloat phase, cpFloat ratio); - -CP_DefineConstraintProperty(cpGearJoint, cpFloat, phase, Phase) -CP_DefineConstraintGetter(cpGearJoint, cpFloat, ratio, Ratio) -/// Set the ratio of a gear joint. -void cpGearJointSetRatio(cpConstraint *constraint, cpFloat value); - -/// @} diff --git a/chipmunk/include/chipmunk/constraints/cpGrooveJoint.h b/chipmunk/include/chipmunk/constraints/cpGrooveJoint.h deleted file mode 100644 index b260218f47..0000000000 --- a/chipmunk/include/chipmunk/constraints/cpGrooveJoint.h +++ /dev/null @@ -1,57 +0,0 @@ -/* Copyright (c) 2007 Scott Lembcke - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/// @defgroup cpGrooveJoint cpGrooveJoint -/// @{ - -const cpConstraintClass *cpGrooveJointGetClass(void); - -/// @private -typedef struct cpGrooveJoint { - cpConstraint constraint; - cpVect grv_n, grv_a, grv_b; - cpVect anchr2; - - cpVect grv_tn; - cpFloat clamp; - cpVect r1, r2; - cpMat2x2 k; - - cpVect jAcc; - cpVect bias; -} cpGrooveJoint; - -/// Allocate a groove joint. -cpGrooveJoint* cpGrooveJointAlloc(void); -/// Initialize a groove joint. -cpGrooveJoint* cpGrooveJointInit(cpGrooveJoint *joint, cpBody *a, cpBody *b, cpVect groove_a, cpVect groove_b, cpVect anchr2); -/// Allocate and initialize a groove joint. -cpConstraint* cpGrooveJointNew(cpBody *a, cpBody *b, cpVect groove_a, cpVect groove_b, cpVect anchr2); - -CP_DefineConstraintGetter(cpGrooveJoint, cpVect, grv_a, GrooveA) -/// Set endpoint a of a groove joint's groove -void cpGrooveJointSetGrooveA(cpConstraint *constraint, cpVect value); -CP_DefineConstraintGetter(cpGrooveJoint, cpVect, grv_b, GrooveB) -/// Set endpoint b of a groove joint's groove -void cpGrooveJointSetGrooveB(cpConstraint *constraint, cpVect value); -CP_DefineConstraintProperty(cpGrooveJoint, cpVect, anchr2, Anchr2) - -/// @} diff --git a/chipmunk/include/chipmunk/constraints/cpPinJoint.h b/chipmunk/include/chipmunk/constraints/cpPinJoint.h deleted file mode 100644 index 2413e84a1d..0000000000 --- a/chipmunk/include/chipmunk/constraints/cpPinJoint.h +++ /dev/null @@ -1,52 +0,0 @@ -/* Copyright (c) 2007 Scott Lembcke - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/// @defgroup cpPinJoint cpPinJoint -/// @{ - -const cpConstraintClass *cpPinJointGetClass(void); - -/// @private -typedef struct cpPinJoint { - cpConstraint constraint; - cpVect anchr1, anchr2; - cpFloat dist; - - cpVect r1, r2; - cpVect n; - cpFloat nMass; - - cpFloat jnAcc; - cpFloat bias; -} cpPinJoint; - -/// Allocate a pin joint. -cpPinJoint* cpPinJointAlloc(void); -/// Initialize a pin joint. -cpPinJoint* cpPinJointInit(cpPinJoint *joint, cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2); -/// Allocate and initialize a pin joint. -cpConstraint* cpPinJointNew(cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2); - -CP_DefineConstraintProperty(cpPinJoint, cpVect, anchr1, Anchr1) -CP_DefineConstraintProperty(cpPinJoint, cpVect, anchr2, Anchr2) -CP_DefineConstraintProperty(cpPinJoint, cpFloat, dist, Dist) - -///@} diff --git a/chipmunk/include/chipmunk/constraints/cpPivotJoint.h b/chipmunk/include/chipmunk/constraints/cpPivotJoint.h deleted file mode 100644 index a5d33174fc..0000000000 --- a/chipmunk/include/chipmunk/constraints/cpPivotJoint.h +++ /dev/null @@ -1,51 +0,0 @@ -/* Copyright (c) 2007 Scott Lembcke - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/// @defgroup cpPivotJoint cpPivotJoint -/// @{ - -const cpConstraintClass *cpPivotJointGetClass(void); - -/// @private -typedef struct cpPivotJoint { - cpConstraint constraint; - cpVect anchr1, anchr2; - - cpVect r1, r2; - cpMat2x2 k; - - cpVect jAcc; - cpVect bias; -} cpPivotJoint; - -/// Allocate a pivot joint -cpPivotJoint* cpPivotJointAlloc(void); -/// Initialize a pivot joint. -cpPivotJoint* cpPivotJointInit(cpPivotJoint *joint, cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2); -/// Allocate and initialize a pivot joint. -cpConstraint* cpPivotJointNew(cpBody *a, cpBody *b, cpVect pivot); -/// Allocate and initialize a pivot joint with specific anchors. -cpConstraint* cpPivotJointNew2(cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2); - -CP_DefineConstraintProperty(cpPivotJoint, cpVect, anchr1, Anchr1) -CP_DefineConstraintProperty(cpPivotJoint, cpVect, anchr2, Anchr2) - -/// @} diff --git a/chipmunk/include/chipmunk/constraints/cpRatchetJoint.h b/chipmunk/include/chipmunk/constraints/cpRatchetJoint.h deleted file mode 100644 index e371b10881..0000000000 --- a/chipmunk/include/chipmunk/constraints/cpRatchetJoint.h +++ /dev/null @@ -1,49 +0,0 @@ -/* Copyright (c) 2007 Scott Lembcke - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/// @defgroup cpRatchetJoint cpRatchetJoint -/// @{ - -const cpConstraintClass *cpRatchetJointGetClass(void); - -/// @private -typedef struct cpRatchetJoint { - cpConstraint constraint; - cpFloat angle, phase, ratchet; - - cpFloat iSum; - - cpFloat bias; - cpFloat jAcc; -} cpRatchetJoint; - -/// Allocate a ratchet joint. -cpRatchetJoint* cpRatchetJointAlloc(void); -/// Initialize a ratched joint. -cpRatchetJoint* cpRatchetJointInit(cpRatchetJoint *joint, cpBody *a, cpBody *b, cpFloat phase, cpFloat ratchet); -/// Allocate and initialize a ratchet joint. -cpConstraint* cpRatchetJointNew(cpBody *a, cpBody *b, cpFloat phase, cpFloat ratchet); - -CP_DefineConstraintProperty(cpRatchetJoint, cpFloat, angle, Angle) -CP_DefineConstraintProperty(cpRatchetJoint, cpFloat, phase, Phase) -CP_DefineConstraintProperty(cpRatchetJoint, cpFloat, ratchet, Ratchet) - -/// @} diff --git a/chipmunk/include/chipmunk/constraints/cpRotaryLimitJoint.h b/chipmunk/include/chipmunk/constraints/cpRotaryLimitJoint.h deleted file mode 100644 index 2277e40f58..0000000000 --- a/chipmunk/include/chipmunk/constraints/cpRotaryLimitJoint.h +++ /dev/null @@ -1,48 +0,0 @@ -/* Copyright (c) 2007 Scott Lembcke - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/// @defgroup cpRotaryLimitJoint cpRotaryLimitJoint -/// @{ - -const cpConstraintClass *cpRotaryLimitJointGetClass(void); - -/// @private -typedef struct cpRotaryLimitJoint { - cpConstraint constraint; - cpFloat min, max; - - cpFloat iSum; - - cpFloat bias; - cpFloat jAcc; -} cpRotaryLimitJoint; - -/// Allocate a damped rotary limit joint. -cpRotaryLimitJoint* cpRotaryLimitJointAlloc(void); -/// Initialize a damped rotary limit joint. -cpRotaryLimitJoint* cpRotaryLimitJointInit(cpRotaryLimitJoint *joint, cpBody *a, cpBody *b, cpFloat min, cpFloat max); -/// Allocate and initialize a damped rotary limit joint. -cpConstraint* cpRotaryLimitJointNew(cpBody *a, cpBody *b, cpFloat min, cpFloat max); - -CP_DefineConstraintProperty(cpRotaryLimitJoint, cpFloat, min, Min) -CP_DefineConstraintProperty(cpRotaryLimitJoint, cpFloat, max, Max) - -/// @} diff --git a/chipmunk/include/chipmunk/constraints/cpSimpleMotor.h b/chipmunk/include/chipmunk/constraints/cpSimpleMotor.h deleted file mode 100644 index 37d28c5de5..0000000000 --- a/chipmunk/include/chipmunk/constraints/cpSimpleMotor.h +++ /dev/null @@ -1,46 +0,0 @@ -/* Copyright (c) 2007 Scott Lembcke - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/// @defgroup cpSimpleMotor cpSimpleMotor -/// @{ - -const cpConstraintClass *cpSimpleMotorGetClass(void); - -/// @private -typedef struct cpSimpleMotor { - cpConstraint constraint; - cpFloat rate; - - cpFloat iSum; - - cpFloat jAcc; -} cpSimpleMotor; - -/// Allocate a simple motor. -cpSimpleMotor* cpSimpleMotorAlloc(void); -/// initialize a simple motor. -cpSimpleMotor* cpSimpleMotorInit(cpSimpleMotor *joint, cpBody *a, cpBody *b, cpFloat rate); -/// Allocate and initialize a simple motor. -cpConstraint* cpSimpleMotorNew(cpBody *a, cpBody *b, cpFloat rate); - -CP_DefineConstraintProperty(cpSimpleMotor, cpFloat, rate, Rate) - -/// @} diff --git a/chipmunk/include/chipmunk/constraints/cpSlideJoint.h b/chipmunk/include/chipmunk/constraints/cpSlideJoint.h deleted file mode 100644 index 40db8bba2b..0000000000 --- a/chipmunk/include/chipmunk/constraints/cpSlideJoint.h +++ /dev/null @@ -1,53 +0,0 @@ -/* Copyright (c) 2007 Scott Lembcke - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/// @defgroup cpSlideJoint cpSlideJoint -/// @{ - -const cpConstraintClass *cpSlideJointGetClass(void); - -/// @private -typedef struct cpSlideJoint { - cpConstraint constraint; - cpVect anchr1, anchr2; - cpFloat min, max; - - cpVect r1, r2; - cpVect n; - cpFloat nMass; - - cpFloat jnAcc; - cpFloat bias; -} cpSlideJoint; - -/// Allocate a slide joint. -cpSlideJoint* cpSlideJointAlloc(void); -/// Initialize a slide joint. -cpSlideJoint* cpSlideJointInit(cpSlideJoint *joint, cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2, cpFloat min, cpFloat max); -/// Allocate and initialize a slide joint. -cpConstraint* cpSlideJointNew(cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2, cpFloat min, cpFloat max); - -CP_DefineConstraintProperty(cpSlideJoint, cpVect, anchr1, Anchr1) -CP_DefineConstraintProperty(cpSlideJoint, cpVect, anchr2, Anchr2) -CP_DefineConstraintProperty(cpSlideJoint, cpFloat, min, Min) -CP_DefineConstraintProperty(cpSlideJoint, cpFloat, max, Max) - -/// @} diff --git a/chipmunk/include/chipmunk/constraints/util.h b/chipmunk/include/chipmunk/constraints/util.h deleted file mode 100644 index 0a6204758a..0000000000 --- a/chipmunk/include/chipmunk/constraints/util.h +++ /dev/null @@ -1,126 +0,0 @@ -/* Copyright (c) 2007 Scott Lembcke - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -// These are utility routines to use when creating custom constraints. -// I'm not sure if this should be part of the private API or not. -// I should probably clean up the naming conventions if it is... - -#define CP_DefineClassGetter(t) const cpConstraintClass * t##GetClass(void){return (cpConstraintClass *)&klass;} - -void cpConstraintInit(cpConstraint *constraint, const cpConstraintClass *klass, cpBody *a, cpBody *b); - -static inline cpVect -relative_velocity(cpBody *a, cpBody *b, cpVect r1, cpVect r2){ - cpVect v1_sum = cpvadd(a->v, cpvmult(cpvperp(r1), a->w)); - cpVect v2_sum = cpvadd(b->v, cpvmult(cpvperp(r2), b->w)); - - return cpvsub(v2_sum, v1_sum); -} - -static inline cpFloat -normal_relative_velocity(cpBody *a, cpBody *b, cpVect r1, cpVect r2, cpVect n){ - return cpvdot(relative_velocity(a, b, r1, r2), n); -} - -static inline void -apply_impulse(cpBody *body, cpVect j, cpVect r){ - body->v = cpvadd(body->v, cpvmult(j, body->m_inv)); - body->w += body->i_inv*cpvcross(r, j); -} - -static inline void -apply_impulses(cpBody *a , cpBody *b, cpVect r1, cpVect r2, cpVect j) -{ - apply_impulse(a, cpvneg(j), r1); - apply_impulse(b, j, r2); -} - -static inline void -apply_bias_impulse(cpBody *body, cpVect j, cpVect r) -{ - body->CP_PRIVATE(v_bias) = cpvadd(body->CP_PRIVATE(v_bias), cpvmult(j, body->m_inv)); - body->CP_PRIVATE(w_bias) += body->i_inv*cpvcross(r, j); -} - -static inline void -apply_bias_impulses(cpBody *a , cpBody *b, cpVect r1, cpVect r2, cpVect j) -{ - apply_bias_impulse(a, cpvneg(j), r1); - apply_bias_impulse(b, j, r2); -} - -static inline cpFloat -k_scalar_body(cpBody *body, cpVect r, cpVect n) -{ - cpFloat rcn = cpvcross(r, n); - return body->m_inv + body->i_inv*rcn*rcn; -} - -static inline cpFloat -k_scalar(cpBody *a, cpBody *b, cpVect r1, cpVect r2, cpVect n) -{ - cpFloat value = k_scalar_body(a, r1, n) + k_scalar_body(b, r2, n); - cpAssertSoft(value != 0.0, "Unsolvable collision or constraint."); - - return value; -} - -static inline cpMat2x2 -k_tensor(cpBody *a, cpBody *b, cpVect r1, cpVect r2) -{ - cpFloat m_sum = a->m_inv + b->m_inv; - - // start with Identity*m_sum - cpFloat k11 = m_sum, k12 = 0.0f; - cpFloat k21 = 0.0f, k22 = m_sum; - - // add the influence from r1 - cpFloat a_i_inv = a->i_inv; - cpFloat r1xsq = r1.x * r1.x * a_i_inv; - cpFloat r1ysq = r1.y * r1.y * a_i_inv; - cpFloat r1nxy = -r1.x * r1.y * a_i_inv; - k11 += r1ysq; k12 += r1nxy; - k21 += r1nxy; k22 += r1xsq; - - // add the influnce from r2 - cpFloat b_i_inv = b->i_inv; - cpFloat r2xsq = r2.x * r2.x * b_i_inv; - cpFloat r2ysq = r2.y * r2.y * b_i_inv; - cpFloat r2nxy = -r2.x * r2.y * b_i_inv; - k11 += r2ysq; k12 += r2nxy; - k21 += r2nxy; k22 += r2xsq; - - // invert - cpFloat det = k11*k22 - k12*k21; - cpAssertSoft(det != 0.0, "Unsolvable constraint."); - - cpFloat det_inv = 1.0f/det; - return cpMat2x2New( - k22*det_inv, -k12*det_inv, - -k21*det_inv, k11*det_inv - ); -} - -static inline cpFloat -bias_coef(cpFloat errorBias, cpFloat dt) -{ - return 1.0f - cpfpow(errorBias, dt); -} diff --git a/chipmunk/include/chipmunk/cpArbiter.h b/chipmunk/include/chipmunk/cpArbiter.h deleted file mode 100644 index 1ccb693d3a..0000000000 --- a/chipmunk/include/chipmunk/cpArbiter.h +++ /dev/null @@ -1,207 +0,0 @@ -/* Copyright (c) 2007 Scott Lembcke - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/// @defgroup cpArbiter cpArbiter -/// The cpArbiter struct controls pairs of colliding shapes. -/// They are also used in conjuction with collision handler callbacks -/// allowing you to retrieve information on the collision and control it. -/// @{ - -/// Collision begin event function callback type. -/// Returning false from a begin callback causes the collision to be ignored until -/// the the separate callback is called when the objects stop colliding. -typedef cpBool (*cpCollisionBeginFunc)(cpArbiter *arb, cpSpace *space, void *data); -/// Collision pre-solve event function callback type. -/// Returning false from a pre-step callback causes the collision to be ignored until the next step. -typedef cpBool (*cpCollisionPreSolveFunc)(cpArbiter *arb, cpSpace *space, void *data); -/// Collision post-solve event function callback type. -typedef void (*cpCollisionPostSolveFunc)(cpArbiter *arb, cpSpace *space, void *data); -/// Collision separate event function callback type. -typedef void (*cpCollisionSeparateFunc)(cpArbiter *arb, cpSpace *space, void *data); - -/// @private -struct cpCollisionHandler { - cpCollisionType a; - cpCollisionType b; - cpCollisionBeginFunc begin; - cpCollisionPreSolveFunc preSolve; - cpCollisionPostSolveFunc postSolve; - cpCollisionSeparateFunc separate; - void *data; -}; - -typedef struct cpContact cpContact; - -#define CP_MAX_CONTACTS_PER_ARBITER 2 - -/// @private -typedef enum cpArbiterState { - // Arbiter is active and its the first collision. - cpArbiterStateFirstColl, - // Arbiter is active and its not the first collision. - cpArbiterStateNormal, - // Collision has been explicitly ignored. - // Either by returning false from a begin collision handler or calling cpArbiterIgnore(). - cpArbiterStateIgnore, - // Collison is no longer active. A space will cache an arbiter for up to cpSpace.collisionPersistence more steps. - cpArbiterStateCached, -} cpArbiterState; - -/// @private -struct cpArbiterThread { - // Links to next and previous arbiters in the contact graph. - struct cpArbiter *next, *prev; -}; - -/// A colliding pair of shapes. -struct cpArbiter { - /// Calculated value to use for the elasticity coefficient. - /// Override in a pre-solve collision handler for custom behavior. - cpFloat e; - /// Calculated value to use for the friction coefficient. - /// Override in a pre-solve collision handler for custom behavior. - cpFloat u; - /// Calculated value to use for applying surface velocities. - /// Override in a pre-solve collision handler for custom behavior. - cpVect surface_vr; - - /// User definable data pointer. - /// The value will persist for the pair of shapes until the separate() callback is called. - /// NOTE: If you need to clean up this pointer, you should implement the separate() callback to do it. - cpDataPointer data; - - CP_PRIVATE(cpShape *a); - CP_PRIVATE(cpShape *b); - CP_PRIVATE(cpBody *body_a); - CP_PRIVATE(cpBody *body_b); - - CP_PRIVATE(struct cpArbiterThread thread_a); - CP_PRIVATE(struct cpArbiterThread thread_b); - - CP_PRIVATE(int numContacts); - CP_PRIVATE(cpContact *contacts); - - CP_PRIVATE(cpTimestamp stamp); - CP_PRIVATE(cpCollisionHandler *handler); - CP_PRIVATE(cpBool swappedColl); - CP_PRIVATE(cpArbiterState state); -}; - -#define CP_DefineArbiterStructGetter(type, member, name) \ -static inline type cpArbiterGet##name(const cpArbiter *arb){return arb->member;} - -#define CP_DefineArbiterStructSetter(type, member, name) \ -static inline void cpArbiterSet##name(cpArbiter *arb, type value){arb->member = value;} - -#define CP_DefineArbiterStructProperty(type, member, name) \ -CP_DefineArbiterStructGetter(type, member, name) \ -CP_DefineArbiterStructSetter(type, member, name) - -CP_DefineArbiterStructProperty(cpFloat, e, Elasticity) -CP_DefineArbiterStructProperty(cpFloat, u, Friction) - -// Get the relative surface velocity of the two shapes in contact. -cpVect cpArbiterGetSurfaceVelocity(cpArbiter *arb); - -// Override the relative surface velocity of the two shapes in contact. -// By default this is calculated to be the difference of the two -// surface velocities clamped to the tangent plane. -void cpArbiterSetSurfaceVelocity(cpArbiter *arb, cpVect vr); - -CP_DefineArbiterStructProperty(cpDataPointer, data, UserData) - -/// Calculate the total impulse that was applied by this arbiter. -/// This function should only be called from a post-solve, post-step or cpBodyEachArbiter callback. -cpVect cpArbiterTotalImpulse(const cpArbiter *arb); -/// Calculate the total impulse including the friction that was applied by this arbiter. -/// This function should only be called from a post-solve, post-step or cpBodyEachArbiter callback. -cpVect cpArbiterTotalImpulseWithFriction(const cpArbiter *arb); -/// Calculate the amount of energy lost in a collision including static, but not dynamic friction. -/// This function should only be called from a post-solve, post-step or cpBodyEachArbiter callback. -cpFloat cpArbiterTotalKE(const cpArbiter *arb); - - -/// Causes a collision pair to be ignored as if you returned false from a begin callback. -/// If called from a pre-step callback, you will still need to return false -/// if you want it to be ignored in the current step. -void cpArbiterIgnore(cpArbiter *arb); - -/// Return the colliding shapes involved for this arbiter. -/// The order of their cpSpace.collision_type values will match -/// the order set when the collision handler was registered. -static inline void cpArbiterGetShapes(const cpArbiter *arb, cpShape **a, cpShape **b) -{ - if(arb->CP_PRIVATE(swappedColl)){ - (*a) = arb->CP_PRIVATE(b), (*b) = arb->CP_PRIVATE(a); - } else { - (*a) = arb->CP_PRIVATE(a), (*b) = arb->CP_PRIVATE(b); - } -} -/// A macro shortcut for defining and retrieving the shapes from an arbiter. -#define CP_ARBITER_GET_SHAPES(__arb__, __a__, __b__) cpShape *__a__, *__b__; cpArbiterGetShapes(__arb__, &__a__, &__b__); - -/// Return the colliding bodies involved for this arbiter. -/// The order of the cpSpace.collision_type the bodies are associated with values will match -/// the order set when the collision handler was registered. -static inline void cpArbiterGetBodies(const cpArbiter *arb, cpBody **a, cpBody **b) -{ - CP_ARBITER_GET_SHAPES(arb, shape_a, shape_b); - (*a) = shape_a->body; - (*b) = shape_b->body; -} -/// A macro shortcut for defining and retrieving the bodies from an arbiter. -#define CP_ARBITER_GET_BODIES(__arb__, __a__, __b__) cpBody *__a__, *__b__; cpArbiterGetBodies(__arb__, &__a__, &__b__); - -/// A struct that wraps up the important collision data for an arbiter. -typedef struct cpContactPointSet { - /// The number of contact points in the set. - int count; - - /// The array of contact points. - struct { - /// The position of the contact point. - cpVect point; - /// The normal of the contact point. - cpVect normal; - /// The depth of the contact point. - cpFloat dist; - } points[CP_MAX_CONTACTS_PER_ARBITER]; -} cpContactPointSet; - -/// Return a contact set from an arbiter. -cpContactPointSet cpArbiterGetContactPointSet(const cpArbiter *arb); - -/// Replace the contact point set for an arbiter. -/// This can be a very powerful feature, but use it with caution! -void cpArbiterSetContactPointSet(cpArbiter *arb, cpContactPointSet *set); - -/// Returns true if this is the first step a pair of objects started colliding. -cpBool cpArbiterIsFirstContact(const cpArbiter *arb); -/// Get the number of contact points for this arbiter. -int cpArbiterGetCount(const cpArbiter *arb); -/// Get the normal of the @c ith contact point. -cpVect cpArbiterGetNormal(const cpArbiter *arb, int i); -/// Get the position of the @c ith contact point. -cpVect cpArbiterGetPoint(const cpArbiter *arb, int i); -/// Get the depth of the @c ith contact point. -cpFloat cpArbiterGetDepth(const cpArbiter *arb, int i); - -/// @} diff --git a/chipmunk/include/chipmunk/cpBB.h b/chipmunk/include/chipmunk/cpBB.h deleted file mode 100644 index 4e59c2d495..0000000000 --- a/chipmunk/include/chipmunk/cpBB.h +++ /dev/null @@ -1,143 +0,0 @@ -/* Copyright (c) 2007 Scott Lembcke - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/// @defgroup cpBBB cpBB -/// Chipmunk's axis-aligned 2D bounding box type along with a few handy routines. -/// @{ - -/// Chipmunk's axis-aligned 2D bounding box type. (left, bottom, right, top) -typedef struct cpBB{ - cpFloat l, b, r ,t; -} cpBB; - -/// Convenience constructor for cpBB structs. -static inline cpBB cpBBNew(const cpFloat l, const cpFloat b, const cpFloat r, const cpFloat t) -{ - cpBB bb = {l, b, r, t}; - return bb; -} - -/// Constructs a cpBB for a circle with the given position and radius. -static inline cpBB cpBBNewForCircle(const cpVect p, const cpFloat r) -{ - return cpBBNew(p.x - r, p.y - r, p.x + r, p.y + r); -} - -/// Returns true if @c a and @c b intersect. -static inline cpBool cpBBIntersects(const cpBB a, const cpBB b) -{ - return (a.l <= b.r && b.l <= a.r && a.b <= b.t && b.b <= a.t); -} - -/// Returns true if @c other lies completely within @c bb. -static inline cpBool cpBBContainsBB(const cpBB bb, const cpBB other) -{ - return (bb.l <= other.l && bb.r >= other.r && bb.b <= other.b && bb.t >= other.t); -} - -/// Returns true if @c bb contains @c v. -static inline cpBool cpBBContainsVect(const cpBB bb, const cpVect v) -{ - return (bb.l <= v.x && bb.r >= v.x && bb.b <= v.y && bb.t >= v.y); -} - -/// Returns a bounding box that holds both bounding boxes. -static inline cpBB cpBBMerge(const cpBB a, const cpBB b){ - return cpBBNew( - cpfmin(a.l, b.l), - cpfmin(a.b, b.b), - cpfmax(a.r, b.r), - cpfmax(a.t, b.t) - ); -} - -/// Returns a bounding box that holds both @c bb and @c v. -static inline cpBB cpBBExpand(const cpBB bb, const cpVect v){ - return cpBBNew( - cpfmin(bb.l, v.x), - cpfmin(bb.b, v.y), - cpfmax(bb.r, v.x), - cpfmax(bb.t, v.y) - ); -} - -/// Returns the center of a bounding box. -static inline cpVect -cpBBCenter(cpBB bb) -{ - return cpvlerp(cpv(bb.l, bb.b), cpv(bb.r, bb.t), 0.5f); -} - -/// Returns the area of the bounding box. -static inline cpFloat cpBBArea(cpBB bb) -{ - return (bb.r - bb.l)*(bb.t - bb.b); -} - -/// Merges @c a and @c b and returns the area of the merged bounding box. -static inline cpFloat cpBBMergedArea(cpBB a, cpBB b) -{ - return (cpfmax(a.r, b.r) - cpfmin(a.l, b.l))*(cpfmax(a.t, b.t) - cpfmin(a.b, b.b)); -} - -/// Returns the fraction along the segment query the cpBB is hit. Returns INFINITY if it doesn't hit. -static inline cpFloat cpBBSegmentQuery(cpBB bb, cpVect a, cpVect b) -{ - cpFloat idx = 1.0f/(b.x - a.x); - cpFloat tx1 = (bb.l == a.x ? -INFINITY : (bb.l - a.x)*idx); - cpFloat tx2 = (bb.r == a.x ? INFINITY : (bb.r - a.x)*idx); - cpFloat txmin = cpfmin(tx1, tx2); - cpFloat txmax = cpfmax(tx1, tx2); - - cpFloat idy = 1.0f/(b.y - a.y); - cpFloat ty1 = (bb.b == a.y ? -INFINITY : (bb.b - a.y)*idy); - cpFloat ty2 = (bb.t == a.y ? INFINITY : (bb.t - a.y)*idy); - cpFloat tymin = cpfmin(ty1, ty2); - cpFloat tymax = cpfmax(ty1, ty2); - - if(tymin <= txmax && txmin <= tymax){ - cpFloat min = cpfmax(txmin, tymin); - cpFloat max = cpfmin(txmax, tymax); - - if(0.0 <= max && min <= 1.0) return cpfmax(min, 0.0); - } - - return INFINITY; -} - -/// Return true if the bounding box intersects the line segment with ends @c a and @c b. -static inline cpBool cpBBIntersectsSegment(cpBB bb, cpVect a, cpVect b) -{ - return (cpBBSegmentQuery(bb, a, b) != INFINITY); -} - -/// Clamp a vector to a bounding box. -static inline cpVect -cpBBClampVect(const cpBB bb, const cpVect v) -{ - return cpv(cpfclamp(v.x, bb.l, bb.r), cpfclamp(v.y, bb.b, bb.t)); -} - -// TODO edge case issue -/// Wrap a vector to a bounding box. -cpVect cpBBWrapVect(const cpBB bb, const cpVect v); // wrap a vector to a bbox - -///@} diff --git a/chipmunk/include/chipmunk/cpBody.h b/chipmunk/include/chipmunk/cpBody.h deleted file mode 100644 index 6168b0fe67..0000000000 --- a/chipmunk/include/chipmunk/cpBody.h +++ /dev/null @@ -1,251 +0,0 @@ -/* Copyright (c) 2007 Scott Lembcke - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/// @defgroup cpBody cpBody -/// Chipmunk's rigid body type. Rigid bodies hold the physical properties of an object like -/// it's mass, and position and velocity of it's center of gravity. They don't have an shape on their own. -/// They are given a shape by creating collision shapes (cpShape) that point to the body. -/// @{ - -/// Rigid body velocity update function type. -typedef void (*cpBodyVelocityFunc)(cpBody *body, cpVect gravity, cpFloat damping, cpFloat dt); -/// Rigid body position update function type. -typedef void (*cpBodyPositionFunc)(cpBody *body, cpFloat dt); - -/// Used internally to track information on the collision graph. -/// @private -typedef struct cpComponentNode { - cpBody *root; - cpBody *next; - cpFloat idleTime; -} cpComponentNode; - -/// Chipmunk's rigid body struct. -struct cpBody { - /// Function that is called to integrate the body's velocity. (Defaults to cpBodyUpdateVelocity) - cpBodyVelocityFunc velocity_func; - - /// Function that is called to integrate the body's position. (Defaults to cpBodyUpdatePosition) - cpBodyPositionFunc position_func; - - /// Mass of the body. - /// Must agree with cpBody.m_inv! Use cpBodySetMass() when changing the mass for this reason. - cpFloat m; - /// Mass inverse. - cpFloat m_inv; - - /// Moment of inertia of the body. - /// Must agree with cpBody.i_inv! Use cpBodySetMoment() when changing the moment for this reason. - cpFloat i; - /// Moment of inertia inverse. - cpFloat i_inv; - - /// Position of the rigid body's center of gravity. - cpVect p; - /// Velocity of the rigid body's center of gravity. - cpVect v; - /// Force acting on the rigid body's center of gravity. - cpVect f; - - /// Rotation of the body around it's center of gravity in radians. - /// Must agree with cpBody.rot! Use cpBodySetAngle() when changing the angle for this reason. - cpFloat a; - /// Angular velocity of the body around it's center of gravity in radians/second. - cpFloat w; - /// Torque applied to the body around it's center of gravity. - cpFloat t; - - /// Cached unit length vector representing the angle of the body. - /// Used for fast rotations using cpvrotate(). - cpVect rot; - - /// User definable data pointer. - /// Generally this points to your the game object class so you can access it - /// when given a cpBody reference in a callback. - cpDataPointer data; - - /// Maximum velocity allowed when updating the velocity. - cpFloat v_limit; - /// Maximum rotational rate (in radians/second) allowed when updating the angular velocity. - cpFloat w_limit; - - CP_PRIVATE(cpVect v_bias); - CP_PRIVATE(cpFloat w_bias); - - CP_PRIVATE(cpSpace *space); - - CP_PRIVATE(cpShape *shapeList); - CP_PRIVATE(cpArbiter *arbiterList); - CP_PRIVATE(cpConstraint *constraintList); - - CP_PRIVATE(cpComponentNode node); -}; - -/// Allocate a cpBody. -cpBody* cpBodyAlloc(void); -/// Initialize a cpBody. -cpBody* cpBodyInit(cpBody *body, cpFloat m, cpFloat i); -/// Allocate and initialize a cpBody. -cpBody* cpBodyNew(cpFloat m, cpFloat i); - -/// Initialize a static cpBody. -cpBody* cpBodyInitStatic(cpBody *body); -/// Allocate and initialize a static cpBody. -cpBody* cpBodyNewStatic(void); - -/// Destroy a cpBody. -void cpBodyDestroy(cpBody *body); -/// Destroy and free a cpBody. -void cpBodyFree(cpBody *body); - -/// Check that the properties of a body is sane. (Only in debug mode) -#ifdef NDEBUG - #define cpBodyAssertSane(body) -#else - void cpBodySanityCheck(cpBody *body); - #define cpBodyAssertSane(body) cpBodySanityCheck(body) -#endif - -// Defined in cpSpace.c -/// Wake up a sleeping or idle body. -void cpBodyActivate(cpBody *body); -/// Wake up any sleeping or idle bodies touching a static body. -void cpBodyActivateStatic(cpBody *body, cpShape *filter); - -/// Force a body to fall asleep immediately. -void cpBodySleep(cpBody *body); -/// Force a body to fall asleep immediately along with other bodies in a group. -void cpBodySleepWithGroup(cpBody *body, cpBody *group); - -/// Returns true if the body is sleeping. -static inline cpBool cpBodyIsSleeping(const cpBody *body) -{ - return (CP_PRIVATE(body->node).root != ((cpBody*)0)); -} - -/// Returns true if the body is static. -static inline cpBool cpBodyIsStatic(const cpBody *body) -{ - return CP_PRIVATE(body->node).idleTime == INFINITY; -} - -/// Returns true if the body has not been added to a space. -/// Note: Static bodies are a subtype of rogue bodies. -static inline cpBool cpBodyIsRogue(const cpBody *body) -{ - return (body->CP_PRIVATE(space) == ((cpSpace*)0)); -} - - -#define CP_DefineBodyStructGetter(type, member, name) \ -static inline type cpBodyGet##name(const cpBody *body){return body->member;} - -#define CP_DefineBodyStructSetter(type, member, name) \ -static inline void cpBodySet##name(cpBody *body, const type value){ \ - cpBodyActivate(body); \ - body->member = value; \ - cpBodyAssertSane(body); \ -} - -#define CP_DefineBodyStructProperty(type, member, name) \ -CP_DefineBodyStructGetter(type, member, name) \ -CP_DefineBodyStructSetter(type, member, name) - -// TODO add to docs -CP_DefineBodyStructGetter(cpSpace*, CP_PRIVATE(space), Space) - -CP_DefineBodyStructGetter(cpFloat, m, Mass) -/// Set the mass of a body. -void cpBodySetMass(cpBody *body, cpFloat m); - -CP_DefineBodyStructGetter(cpFloat, i, Moment) -/// Set the moment of a body. -void cpBodySetMoment(cpBody *body, cpFloat i); - -CP_DefineBodyStructGetter(cpVect, p, Pos) -/// Set the position of a body. -void cpBodySetPos(cpBody *body, cpVect pos); -CP_DefineBodyStructProperty(cpVect, v, Vel) -CP_DefineBodyStructProperty(cpVect, f, Force) -CP_DefineBodyStructGetter(cpFloat, a, Angle) -/// Set the angle of a body. -void cpBodySetAngle(cpBody *body, cpFloat a); -CP_DefineBodyStructProperty(cpFloat, w, AngVel) -CP_DefineBodyStructProperty(cpFloat, t, Torque) -CP_DefineBodyStructGetter(cpVect, rot, Rot) -CP_DefineBodyStructProperty(cpFloat, v_limit, VelLimit) -CP_DefineBodyStructProperty(cpFloat, w_limit, AngVelLimit) -CP_DefineBodyStructProperty(cpDataPointer, data, UserData) - -/// Default Integration functions. -void cpBodyUpdateVelocity(cpBody *body, cpVect gravity, cpFloat damping, cpFloat dt); -void cpBodyUpdatePosition(cpBody *body, cpFloat dt); - -/// Convert body relative/local coordinates to absolute/world coordinates. -static inline cpVect cpBodyLocal2World(const cpBody *body, const cpVect v) -{ - return cpvadd(body->p, cpvrotate(v, body->rot)); -} - -/// Convert body absolute/world coordinates to relative/local coordinates. -static inline cpVect cpBodyWorld2Local(const cpBody *body, const cpVect v) -{ - return cpvunrotate(cpvsub(v, body->p), body->rot); -} - -/// Set the forces and torque or a body to zero. -void cpBodyResetForces(cpBody *body); -/// Apply an force (in world coordinates) to the body at a point relative to the center of gravity (also in world coordinates). -void cpBodyApplyForce(cpBody *body, const cpVect f, const cpVect r); -/// Apply an impulse (in world coordinates) to the body at a point relative to the center of gravity (also in world coordinates). -void cpBodyApplyImpulse(cpBody *body, const cpVect j, const cpVect r); - -/// Get the velocity on a body (in world units) at a point on the body in world coordinates. -cpVect cpBodyGetVelAtWorldPoint(cpBody *body, cpVect point); -/// Get the velocity on a body (in world units) at a point on the body in local coordinates. -cpVect cpBodyGetVelAtLocalPoint(cpBody *body, cpVect point); - - -/// Get the kinetic energy of a body. -static inline cpFloat cpBodyKineticEnergy(const cpBody *body) -{ - // Need to do some fudging to avoid NaNs - cpFloat vsq = cpvdot(body->v, body->v); - cpFloat wsq = body->w*body->w; - return (vsq ? vsq*body->m : 0.0f) + (wsq ? wsq*body->i : 0.0f); -} - -/// Body/shape iterator callback function type. -typedef void (*cpBodyShapeIteratorFunc)(cpBody *body, cpShape *shape, void *data); -/// Call @c func once for each shape attached to @c body and added to the space. -void cpBodyEachShape(cpBody *body, cpBodyShapeIteratorFunc func, void *data); - -/// Body/constraint iterator callback function type. -typedef void (*cpBodyConstraintIteratorFunc)(cpBody *body, cpConstraint *constraint, void *data); -/// Call @c func once for each constraint attached to @c body and added to the space. -void cpBodyEachConstraint(cpBody *body, cpBodyConstraintIteratorFunc func, void *data); - -/// Body/arbiter iterator callback function type. -typedef void (*cpBodyArbiterIteratorFunc)(cpBody *body, cpArbiter *arbiter, void *data); -/// Call @c func once for each arbiter that is currently active on the body. -void cpBodyEachArbiter(cpBody *body, cpBodyArbiterIteratorFunc func, void *data); - -///@} diff --git a/chipmunk/include/chipmunk/cpPolyShape.h b/chipmunk/include/chipmunk/cpPolyShape.h deleted file mode 100644 index a5587ba250..0000000000 --- a/chipmunk/include/chipmunk/cpPolyShape.h +++ /dev/null @@ -1,81 +0,0 @@ -/* Copyright (c) 2007 Scott Lembcke - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/// @defgroup cpPolyShape cpPolyShape -/// @{ - -/// @private -typedef struct cpSplittingPlane { - cpVect n; - cpFloat d; -} cpSplittingPlane; - -/// @private -typedef struct cpPolyShape { - cpShape shape; - - int numVerts; - cpVect *verts, *tVerts; - cpSplittingPlane *planes, *tPlanes; - - cpFloat r; -} cpPolyShape; - -/// Allocate a polygon shape. -cpPolyShape* cpPolyShapeAlloc(void); -/// Initialize a polygon shape. -/// A convex hull will be created from the vertexes. -cpPolyShape* cpPolyShapeInit(cpPolyShape *poly, cpBody *body, int numVerts, const cpVect *verts, cpVect offset); -/// Initialize a polygon shape. -/// A convex hull will be created from the vertexes. -cpPolyShape* cpPolyShapeInit2(cpPolyShape *poly, cpBody *body, int numVerts, const cpVect *verts, cpVect offset, cpFloat radius); -/// Allocate and initialize a polygon shape. -/// A convex hull will be created from the vertexes. -cpShape* cpPolyShapeNew(cpBody *body, int numVerts, const cpVect *verts, cpVect offset); -/// Allocate and initialize a polygon shape. -/// A convex hull will be created from the vertexes. -cpShape* cpPolyShapeNew2(cpBody *body, int numVerts, const cpVect *verts, cpVect offset, cpFloat radius); - -/// Initialize a box shaped polygon shape. -cpPolyShape* cpBoxShapeInit(cpPolyShape *poly, cpBody *body, cpFloat width, cpFloat height); -/// Initialize an offset box shaped polygon shape. -cpPolyShape* cpBoxShapeInit2(cpPolyShape *poly, cpBody *body, cpBB box); -/// Initialize an offset box shaped polygon shape. -cpPolyShape* cpBoxShapeInit3(cpPolyShape *poly, cpBody *body, cpBB box, cpFloat radius); -/// Allocate and initialize a box shaped polygon shape. -cpShape* cpBoxShapeNew(cpBody *body, cpFloat width, cpFloat height); -/// Allocate and initialize an offset box shaped polygon shape. -cpShape* cpBoxShapeNew2(cpBody *body, cpBB box); -/// Allocate and initialize an offset box shaped polygon shape. -cpShape* cpBoxShapeNew3(cpBody *body, cpBB box, cpFloat radius); - -/// Check that a set of vertexes is convex and has a clockwise winding. -/// NOTE: Due to floating point precision issues, hulls created with cpQuickHull() are not guaranteed to validate! -cpBool cpPolyValidate(const cpVect *verts, const int numVerts); - -/// Get the number of verts in a polygon shape. -int cpPolyShapeGetNumVerts(const cpShape *shape); -/// Get the @c ith vertex of a polygon shape. -cpVect cpPolyShapeGetVert(const cpShape *shape, int idx); -/// Get the radius of a polygon shape. -cpFloat cpPolyShapeGetRadius(const cpShape *shape); - -/// @} diff --git a/chipmunk/include/chipmunk/cpShape.h b/chipmunk/include/chipmunk/cpShape.h deleted file mode 100644 index 62920c371c..0000000000 --- a/chipmunk/include/chipmunk/cpShape.h +++ /dev/null @@ -1,232 +0,0 @@ -/* Copyright (c) 2007 Scott Lembcke - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/// @defgroup cpShape cpShape -/// The cpShape struct defines the shape of a rigid body. -/// @{ - -typedef struct cpShapeClass cpShapeClass; - -/// Nearest point query info struct. -typedef struct cpNearestPointQueryInfo { - /// The nearest shape, NULL if no shape was within range. - cpShape *shape; - /// The closest point on the shape's surface. (in world space coordinates) - cpVect p; - /// The distance to the point. The distance is negative if the point is inside the shape. - cpFloat d; - /// The gradient of the signed distance function. - /// The same as info.p/info.d, but accurate even for very small values of info.d. - cpVect g; -} cpNearestPointQueryInfo; - -/// Segment query info struct. -typedef struct cpSegmentQueryInfo { - /// The shape that was hit, NULL if no collision occured. - cpShape *shape; - /// The normalized distance along the query segment in the range [0, 1]. - cpFloat t; - /// The normal of the surface hit. - cpVect n; -} cpSegmentQueryInfo; - -/// @private -typedef enum cpShapeType{ - CP_CIRCLE_SHAPE, - CP_SEGMENT_SHAPE, - CP_POLY_SHAPE, - CP_NUM_SHAPES -} cpShapeType; - -typedef cpBB (*cpShapeCacheDataImpl)(cpShape *shape, cpVect p, cpVect rot); -typedef void (*cpShapeDestroyImpl)(cpShape *shape); -typedef void (*cpShapeNearestPointQueryImpl)(cpShape *shape, cpVect p, cpNearestPointQueryInfo *info); -typedef void (*cpShapeSegmentQueryImpl)(cpShape *shape, cpVect a, cpVect b, cpSegmentQueryInfo *info); - -/// @private -struct cpShapeClass { - cpShapeType type; - - cpShapeCacheDataImpl cacheData; - cpShapeDestroyImpl destroy; - cpShapeNearestPointQueryImpl nearestPointQuery; - cpShapeSegmentQueryImpl segmentQuery; -}; - -/// Opaque collision shape struct. -struct cpShape { - CP_PRIVATE(const cpShapeClass *klass); - - /// The rigid body this collision shape is attached to. - cpBody *body; - - /// The current bounding box of the shape. - cpBB bb; - - /// Sensor flag. - /// Sensor shapes call collision callbacks but don't produce collisions. - cpBool sensor; - - /// Coefficient of restitution. (elasticity) - cpFloat e; - /// Coefficient of friction. - cpFloat u; - /// Surface velocity used when solving for friction. - cpVect surface_v; - - /// User definable data pointer. - /// Generally this points to your the game object class so you can access it - /// when given a cpShape reference in a callback. - cpDataPointer data; - - /// Collision type of this shape used when picking collision handlers. - cpCollisionType collision_type; - /// Group of this shape. Shapes in the same group don't collide. - cpGroup group; - // Layer bitmask for this shape. Shapes only collide if the bitwise and of their layers is non-zero. - cpLayers layers; - - CP_PRIVATE(cpSpace *space); - - CP_PRIVATE(cpShape *next); - CP_PRIVATE(cpShape *prev); - - CP_PRIVATE(cpHashValue hashid); -}; - -/// Destroy a shape. -void cpShapeDestroy(cpShape *shape); -/// Destroy and Free a shape. -void cpShapeFree(cpShape *shape); - -/// Update, cache and return the bounding box of a shape based on the body it's attached to. -cpBB cpShapeCacheBB(cpShape *shape); -/// Update, cache and return the bounding box of a shape with an explicit transformation. -cpBB cpShapeUpdate(cpShape *shape, cpVect pos, cpVect rot); - -/// Test if a point lies within a shape. -cpBool cpShapePointQuery(cpShape *shape, cpVect p); - -/// Perform a nearest point query. It finds the closest point on the surface of shape to a specific point. -/// The value returned is the distance between the points. A negative distance means the point is inside the shape. -cpFloat cpShapeNearestPointQuery(cpShape *shape, cpVect p, cpNearestPointQueryInfo *out); - -/// Perform a segment query against a shape. @c info must be a pointer to a valid cpSegmentQueryInfo structure. -cpBool cpShapeSegmentQuery(cpShape *shape, cpVect a, cpVect b, cpSegmentQueryInfo *info); - -/// Get the hit point for a segment query. -static inline cpVect cpSegmentQueryHitPoint(const cpVect start, const cpVect end, const cpSegmentQueryInfo info) -{ - return cpvlerp(start, end, info.t); -} - -/// Get the hit distance for a segment query. -static inline cpFloat cpSegmentQueryHitDist(const cpVect start, const cpVect end, const cpSegmentQueryInfo info) -{ - return cpvdist(start, end)*info.t; -} - -#define CP_DefineShapeStructGetter(type, member, name) \ -static inline type cpShapeGet##name(const cpShape *shape){return shape->member;} - -#define CP_DefineShapeStructSetter(type, member, name, activates) \ -static inline void cpShapeSet##name(cpShape *shape, type value){ \ - if(activates && shape->body) cpBodyActivate(shape->body); \ - shape->member = value; \ -} - -#define CP_DefineShapeStructProperty(type, member, name, activates) \ -CP_DefineShapeStructGetter(type, member, name) \ -CP_DefineShapeStructSetter(type, member, name, activates) - -CP_DefineShapeStructGetter(cpSpace*, CP_PRIVATE(space), Space) - -CP_DefineShapeStructGetter(cpBody*, body, Body) -void cpShapeSetBody(cpShape *shape, cpBody *body); - -CP_DefineShapeStructGetter(cpBB, bb, BB) -CP_DefineShapeStructProperty(cpBool, sensor, Sensor, cpTrue) -CP_DefineShapeStructProperty(cpFloat, e, Elasticity, cpFalse) -CP_DefineShapeStructProperty(cpFloat, u, Friction, cpTrue) -CP_DefineShapeStructProperty(cpVect, surface_v, SurfaceVelocity, cpTrue) -CP_DefineShapeStructProperty(cpDataPointer, data, UserData, cpFalse) -CP_DefineShapeStructProperty(cpCollisionType, collision_type, CollisionType, cpTrue) -CP_DefineShapeStructProperty(cpGroup, group, Group, cpTrue) -CP_DefineShapeStructProperty(cpLayers, layers, Layers, cpTrue) - -/// When initializing a shape, it's hash value comes from a counter. -/// Because the hash value may affect iteration order, you can reset the shape ID counter -/// when recreating a space. This will make the simulation be deterministic. -void cpResetShapeIdCounter(void); - -#define CP_DeclareShapeGetter(struct, type, name) type struct##Get##name(const cpShape *shape) - -/// @} -/// @defgroup cpCircleShape cpCircleShape - -/// @private -typedef struct cpCircleShape { - cpShape shape; - - cpVect c, tc; - cpFloat r; -} cpCircleShape; - -/// Allocate a circle shape. -cpCircleShape* cpCircleShapeAlloc(void); -/// Initialize a circle shape. -cpCircleShape* cpCircleShapeInit(cpCircleShape *circle, cpBody *body, cpFloat radius, cpVect offset); -/// Allocate and initialize a circle shape. -cpShape* cpCircleShapeNew(cpBody *body, cpFloat radius, cpVect offset); - -CP_DeclareShapeGetter(cpCircleShape, cpVect, Offset); -CP_DeclareShapeGetter(cpCircleShape, cpFloat, Radius); - -/// @} -/// @defgroup cpSegmentShape cpSegmentShape - -/// @private -typedef struct cpSegmentShape { - cpShape shape; - - cpVect a, b, n; - cpVect ta, tb, tn; - cpFloat r; - - cpVect a_tangent, b_tangent; -} cpSegmentShape; - -/// Allocate a segment shape. -cpSegmentShape* cpSegmentShapeAlloc(void); -/// Initialize a segment shape. -cpSegmentShape* cpSegmentShapeInit(cpSegmentShape *seg, cpBody *body, cpVect a, cpVect b, cpFloat radius); -/// Allocate and initialize a segment shape. -cpShape* cpSegmentShapeNew(cpBody *body, cpVect a, cpVect b, cpFloat radius); - -/// Let Chipmunk know about the geometry of adjacent segments to avoid colliding with endcaps. -void cpSegmentShapeSetNeighbors(cpShape *shape, cpVect prev, cpVect next); - -CP_DeclareShapeGetter(cpSegmentShape, cpVect, A); -CP_DeclareShapeGetter(cpSegmentShape, cpVect, B); -CP_DeclareShapeGetter(cpSegmentShape, cpVect, Normal); -CP_DeclareShapeGetter(cpSegmentShape, cpFloat, Radius); - -/// @} diff --git a/chipmunk/include/chipmunk/cpSpace.h b/chipmunk/include/chipmunk/cpSpace.h deleted file mode 100644 index 26439f76c3..0000000000 --- a/chipmunk/include/chipmunk/cpSpace.h +++ /dev/null @@ -1,283 +0,0 @@ -/* Copyright (c) 2007 Scott Lembcke - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/// @defgroup cpSpace cpSpace -/// @{ - -typedef struct cpContactBufferHeader cpContactBufferHeader; -typedef void (*cpSpaceArbiterApplyImpulseFunc)(cpArbiter *arb); - -/// Basic Unit of Simulation in Chipmunk -struct cpSpace { - /// Number of iterations to use in the impulse solver to solve contacts. - int iterations; - - /// Gravity to pass to rigid bodies when integrating velocity. - cpVect gravity; - - /// Damping rate expressed as the fraction of velocity bodies retain each second. - /// A value of 0.9 would mean that each body's velocity will drop 10% per second. - /// The default value is 1.0, meaning no damping is applied. - /// @note This damping value is different than those of cpDampedSpring and cpDampedRotarySpring. - cpFloat damping; - - /// Speed threshold for a body to be considered idle. - /// The default value of 0 means to let the space guess a good threshold based on gravity. - cpFloat idleSpeedThreshold; - - /// Time a group of bodies must remain idle in order to fall asleep. - /// Enabling sleeping also implicitly enables the the contact graph. - /// The default value of INFINITY disables the sleeping algorithm. - cpFloat sleepTimeThreshold; - - /// Amount of encouraged penetration between colliding shapes. - /// Used to reduce oscillating contacts and keep the collision cache warm. - /// Defaults to 0.1. If you have poor simulation quality, - /// increase this number as much as possible without allowing visible amounts of overlap. - cpFloat collisionSlop; - - /// Determines how fast overlapping shapes are pushed apart. - /// Expressed as a fraction of the error remaining after each second. - /// Defaults to pow(1.0 - 0.1, 60.0) meaning that Chipmunk fixes 10% of overlap each frame at 60Hz. - cpFloat collisionBias; - - /// Number of frames that contact information should persist. - /// Defaults to 3. There is probably never a reason to change this value. - cpTimestamp collisionPersistence; - - /// Rebuild the contact graph during each step. Must be enabled to use the cpBodyEachArbiter() function. - /// Disabled by default for a small performance boost. Enabled implicitly when the sleeping feature is enabled. - cpBool enableContactGraph; - - /// User definable data pointer. - /// Generally this points to your game's controller or game state - /// class so you can access it when given a cpSpace reference in a callback. - cpDataPointer data; - - /// The designated static body for this space. - /// You can modify this body, or replace it with your own static body. - /// By default it points to a statically allocated cpBody in the cpSpace struct. - cpBody *staticBody; - - CP_PRIVATE(cpTimestamp stamp); - CP_PRIVATE(cpFloat curr_dt); - - CP_PRIVATE(cpArray *bodies); - CP_PRIVATE(cpArray *rousedBodies); - CP_PRIVATE(cpArray *sleepingComponents); - - CP_PRIVATE(cpSpatialIndex *staticShapes); - CP_PRIVATE(cpSpatialIndex *activeShapes); - - CP_PRIVATE(cpArray *arbiters); - CP_PRIVATE(cpContactBufferHeader *contactBuffersHead); - CP_PRIVATE(cpHashSet *cachedArbiters); - CP_PRIVATE(cpArray *pooledArbiters); - CP_PRIVATE(cpArray *constraints); - - CP_PRIVATE(cpArray *allocatedBuffers); - CP_PRIVATE(int locked); - - CP_PRIVATE(cpHashSet *collisionHandlers); - CP_PRIVATE(cpCollisionHandler defaultHandler); - - CP_PRIVATE(cpBool skipPostStep); - CP_PRIVATE(cpArray *postStepCallbacks); - - CP_PRIVATE(cpBody _staticBody); -}; - -/// Allocate a cpSpace. -cpSpace* cpSpaceAlloc(void); -/// Initialize a cpSpace. -cpSpace* cpSpaceInit(cpSpace *space); -/// Allocate and initialize a cpSpace. -cpSpace* cpSpaceNew(void); - -/// Destroy a cpSpace. -void cpSpaceDestroy(cpSpace *space); -/// Destroy and free a cpSpace. -void cpSpaceFree(cpSpace *space); - -#define CP_DefineSpaceStructGetter(type, member, name) \ -static inline type cpSpaceGet##name(const cpSpace *space){return space->member;} - -#define CP_DefineSpaceStructSetter(type, member, name) \ -static inline void cpSpaceSet##name(cpSpace *space, type value){space->member = value;} - -#define CP_DefineSpaceStructProperty(type, member, name) \ -CP_DefineSpaceStructGetter(type, member, name) \ -CP_DefineSpaceStructSetter(type, member, name) - -CP_DefineSpaceStructProperty(int, iterations, Iterations) -CP_DefineSpaceStructProperty(cpVect, gravity, Gravity) -CP_DefineSpaceStructProperty(cpFloat, damping, Damping) -CP_DefineSpaceStructProperty(cpFloat, idleSpeedThreshold, IdleSpeedThreshold) -CP_DefineSpaceStructProperty(cpFloat, sleepTimeThreshold, SleepTimeThreshold) -CP_DefineSpaceStructProperty(cpFloat, collisionSlop, CollisionSlop) -CP_DefineSpaceStructProperty(cpFloat, collisionBias, CollisionBias) -CP_DefineSpaceStructProperty(cpTimestamp, collisionPersistence, CollisionPersistence) -CP_DefineSpaceStructProperty(cpBool, enableContactGraph, EnableContactGraph) -CP_DefineSpaceStructProperty(cpDataPointer, data, UserData) -CP_DefineSpaceStructGetter(cpBody*, staticBody, StaticBody) -CP_DefineSpaceStructGetter(cpFloat, CP_PRIVATE(curr_dt), CurrentTimeStep) - -/// returns true from inside a callback and objects cannot be added/removed. -static inline cpBool -cpSpaceIsLocked(cpSpace *space) -{ - return space->CP_PRIVATE(locked); -} - -/// Set a default collision handler for this space. -/// The default collision handler is invoked for each colliding pair of shapes -/// that isn't explicitly handled by a specific collision handler. -/// You can pass NULL for any function you don't want to implement. -void cpSpaceSetDefaultCollisionHandler( - cpSpace *space, - cpCollisionBeginFunc begin, - cpCollisionPreSolveFunc preSolve, - cpCollisionPostSolveFunc postSolve, - cpCollisionSeparateFunc separate, - void *data -); - -/// Set a collision handler to be used whenever the two shapes with the given collision types collide. -/// You can pass NULL for any function you don't want to implement. -void cpSpaceAddCollisionHandler( - cpSpace *space, - cpCollisionType a, cpCollisionType b, - cpCollisionBeginFunc begin, - cpCollisionPreSolveFunc preSolve, - cpCollisionPostSolveFunc postSolve, - cpCollisionSeparateFunc separate, - void *data -); - -/// Unset a collision handler. -void cpSpaceRemoveCollisionHandler(cpSpace *space, cpCollisionType a, cpCollisionType b); - -/// Add a collision shape to the simulation. -/// If the shape is attached to a static body, it will be added as a static shape. -cpShape* cpSpaceAddShape(cpSpace *space, cpShape *shape); -/// Explicity add a shape as a static shape to the simulation. -cpShape* cpSpaceAddStaticShape(cpSpace *space, cpShape *shape); -/// Add a rigid body to the simulation. -cpBody* cpSpaceAddBody(cpSpace *space, cpBody *body); -/// Add a constraint to the simulation. -cpConstraint* cpSpaceAddConstraint(cpSpace *space, cpConstraint *constraint); - -/// Remove a collision shape from the simulation. -void cpSpaceRemoveShape(cpSpace *space, cpShape *shape); -/// Remove a collision shape added using cpSpaceAddStaticShape() from the simulation. -void cpSpaceRemoveStaticShape(cpSpace *space, cpShape *shape); -/// Remove a rigid body from the simulation. -void cpSpaceRemoveBody(cpSpace *space, cpBody *body); -/// Remove a constraint from the simulation. -void cpSpaceRemoveConstraint(cpSpace *space, cpConstraint *constraint); - -/// Test if a collision shape has been added to the space. -cpBool cpSpaceContainsShape(cpSpace *space, cpShape *shape); -/// Test if a rigid body has been added to the space. -cpBool cpSpaceContainsBody(cpSpace *space, cpBody *body); -/// Test if a constraint has been added to the space. -cpBool cpSpaceContainsConstraint(cpSpace *space, cpConstraint *constraint); - -/// Convert a dynamic rogue body to a static one. -/// If the body is active, you must remove it from the space first. -void cpSpaceConvertBodyToStatic(cpSpace *space, cpBody *body); -/// Convert a body to a dynamic rogue body. -/// If you want the body to be active after the transition, you must add it to the space also. -void cpSpaceConvertBodyToDynamic(cpSpace *space, cpBody *body, cpFloat mass, cpFloat moment); - -/// Post Step callback function type. -typedef void (*cpPostStepFunc)(cpSpace *space, void *key, void *data); -/// Schedule a post-step callback to be called when cpSpaceStep() finishes. -/// You can only register one callback per unique value for @c key. -/// Returns true only if @c key has never been scheduled before. -/// It's possible to pass @c NULL for @c func if you only want to mark @c key as being used. -cpBool cpSpaceAddPostStepCallback(cpSpace *space, cpPostStepFunc func, void *key, void *data); - -/// Point query callback function type. -typedef void (*cpSpacePointQueryFunc)(cpShape *shape, void *data); -/// Query the space at a point and call @c func for each shape found. -void cpSpacePointQuery(cpSpace *space, cpVect point, cpLayers layers, cpGroup group, cpSpacePointQueryFunc func, void *data); -/// Query the space at a point and return the first shape found. Returns NULL if no shapes were found. -cpShape *cpSpacePointQueryFirst(cpSpace *space, cpVect point, cpLayers layers, cpGroup group); - -/// Nearest point query callback function type. -typedef void (*cpSpaceNearestPointQueryFunc)(cpShape *shape, cpFloat distance, cpVect point, void *data); -/// Query the space at a point and call @c func for each shape found. -void cpSpaceNearestPointQuery(cpSpace *space, cpVect point, cpFloat maxDistance, cpLayers layers, cpGroup group, cpSpaceNearestPointQueryFunc func, void *data); -/// Query the space at a point and return the nearest shape found. Returns NULL if no shapes were found. -cpShape *cpSpaceNearestPointQueryNearest(cpSpace *space, cpVect point, cpFloat maxDistance, cpLayers layers, cpGroup group, cpNearestPointQueryInfo *out); - -/// Segment query callback function type. -typedef void (*cpSpaceSegmentQueryFunc)(cpShape *shape, cpFloat t, cpVect n, void *data); -/// Perform a directed line segment query (like a raycast) against the space calling @c func for each shape intersected. -void cpSpaceSegmentQuery(cpSpace *space, cpVect start, cpVect end, cpLayers layers, cpGroup group, cpSpaceSegmentQueryFunc func, void *data); -/// Perform a directed line segment query (like a raycast) against the space and return the first shape hit. Returns NULL if no shapes were hit. -cpShape *cpSpaceSegmentQueryFirst(cpSpace *space, cpVect start, cpVect end, cpLayers layers, cpGroup group, cpSegmentQueryInfo *out); - -/// Rectangle Query callback function type. -typedef void (*cpSpaceBBQueryFunc)(cpShape *shape, void *data); -/// Perform a fast rectangle query on the space calling @c func for each shape found. -/// Only the shape's bounding boxes are checked for overlap, not their full shape. -void cpSpaceBBQuery(cpSpace *space, cpBB bb, cpLayers layers, cpGroup group, cpSpaceBBQueryFunc func, void *data); - -/// Shape query callback function type. -typedef void (*cpSpaceShapeQueryFunc)(cpShape *shape, cpContactPointSet *points, void *data); -/// Query a space for any shapes overlapping the given shape and call @c func for each shape found. -cpBool cpSpaceShapeQuery(cpSpace *space, cpShape *shape, cpSpaceShapeQueryFunc func, void *data); - -/// Call cpBodyActivate() for any shape that is overlaps the given shape. -void cpSpaceActivateShapesTouchingShape(cpSpace *space, cpShape *shape); - - -/// Space/body iterator callback function type. -typedef void (*cpSpaceBodyIteratorFunc)(cpBody *body, void *data); -/// Call @c func for each body in the space. -void cpSpaceEachBody(cpSpace *space, cpSpaceBodyIteratorFunc func, void *data); - -/// Space/body iterator callback function type. -typedef void (*cpSpaceShapeIteratorFunc)(cpShape *shape, void *data); -/// Call @c func for each shape in the space. -void cpSpaceEachShape(cpSpace *space, cpSpaceShapeIteratorFunc func, void *data); - -/// Space/constraint iterator callback function type. -typedef void (*cpSpaceConstraintIteratorFunc)(cpConstraint *constraint, void *data); -/// Call @c func for each shape in the space. -void cpSpaceEachConstraint(cpSpace *space, cpSpaceConstraintIteratorFunc func, void *data); - -/// Update the collision detection info for the static shapes in the space. -void cpSpaceReindexStatic(cpSpace *space); -/// Update the collision detection data for a specific shape in the space. -void cpSpaceReindexShape(cpSpace *space, cpShape *shape); -/// Update the collision detection data for all shapes attached to a body. -void cpSpaceReindexShapesForBody(cpSpace *space, cpBody *body); - -/// Switch the space to use a spatial has as it's spatial index. -void cpSpaceUseSpatialHash(cpSpace *space, cpFloat dim, int count); - -/// Step the space forward in time by @c dt. -void cpSpaceStep(cpSpace *space, cpFloat dt); - -/// @} diff --git a/chipmunk/include/chipmunk/cpSpatialIndex.h b/chipmunk/include/chipmunk/cpSpatialIndex.h deleted file mode 100644 index c279cad063..0000000000 --- a/chipmunk/include/chipmunk/cpSpatialIndex.h +++ /dev/null @@ -1,227 +0,0 @@ -/* Copyright (c) 2010 Scott Lembcke - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/** - @defgroup cpSpatialIndex cpSpatialIndex - - Spatial indexes are data structures that are used to accelerate collision detection - and spatial queries. Chipmunk provides a number of spatial index algorithms to pick from - and they are programmed in a generic way so that you can use them for holding more than - just cpShape structs. - - It works by using @c void pointers to the objects you add and using a callback to ask your code - for bounding boxes when it needs them. Several types of queries can be performed an index as well - as reindexing and full collision information. All communication to the spatial indexes is performed - through callback functions. - - Spatial indexes should be treated as opaque structs. - This meanns you shouldn't be reading any of the struct fields. - @{ -*/ - -//MARK: Spatial Index - -/// Spatial index bounding box callback function type. -/// The spatial index calls this function and passes you a pointer to an object you added -/// when it needs to get the bounding box associated with that object. -typedef cpBB (*cpSpatialIndexBBFunc)(void *obj); -/// Spatial index/object iterator callback function type. -typedef void (*cpSpatialIndexIteratorFunc)(void *obj, void *data); -/// Spatial query callback function type. -typedef cpCollisionID (*cpSpatialIndexQueryFunc)(void *obj1, void *obj2, cpCollisionID id, void *data); -/// Spatial segment query callback function type. -typedef cpFloat (*cpSpatialIndexSegmentQueryFunc)(void *obj1, void *obj2, void *data); - - -typedef struct cpSpatialIndexClass cpSpatialIndexClass; -typedef struct cpSpatialIndex cpSpatialIndex; - -/// @private -struct cpSpatialIndex { - cpSpatialIndexClass *klass; - - cpSpatialIndexBBFunc bbfunc; - - cpSpatialIndex *staticIndex, *dynamicIndex; -}; - - -//MARK: Spatial Hash - -typedef struct cpSpaceHash cpSpaceHash; - -/// Allocate a spatial hash. -cpSpaceHash* cpSpaceHashAlloc(void); -/// Initialize a spatial hash. -cpSpatialIndex* cpSpaceHashInit(cpSpaceHash *hash, cpFloat celldim, int numcells, cpSpatialIndexBBFunc bbfunc, cpSpatialIndex *staticIndex); -/// Allocate and initialize a spatial hash. -cpSpatialIndex* cpSpaceHashNew(cpFloat celldim, int cells, cpSpatialIndexBBFunc bbfunc, cpSpatialIndex *staticIndex); - -/// Change the cell dimensions and table size of the spatial hash to tune it. -/// The cell dimensions should roughly match the average size of your objects -/// and the table size should be ~10 larger than the number of objects inserted. -/// Some trial and error is required to find the optimum numbers for efficiency. -void cpSpaceHashResize(cpSpaceHash *hash, cpFloat celldim, int numcells); - -//MARK: AABB Tree - -typedef struct cpBBTree cpBBTree; - -/// Allocate a bounding box tree. -cpBBTree* cpBBTreeAlloc(void); -/// Initialize a bounding box tree. -cpSpatialIndex* cpBBTreeInit(cpBBTree *tree, cpSpatialIndexBBFunc bbfunc, cpSpatialIndex *staticIndex); -/// Allocate and initialize a bounding box tree. -cpSpatialIndex* cpBBTreeNew(cpSpatialIndexBBFunc bbfunc, cpSpatialIndex *staticIndex); - -/// Perform a static top down optimization of the tree. -void cpBBTreeOptimize(cpSpatialIndex *index); - -/// Bounding box tree velocity callback function. -/// This function should return an estimate for the object's velocity. -typedef cpVect (*cpBBTreeVelocityFunc)(void *obj); -/// Set the velocity function for the bounding box tree to enable temporal coherence. -void cpBBTreeSetVelocityFunc(cpSpatialIndex *index, cpBBTreeVelocityFunc func); - -//MARK: Single Axis Sweep - -typedef struct cpSweep1D cpSweep1D; - -/// Allocate a 1D sort and sweep broadphase. -cpSweep1D* cpSweep1DAlloc(void); -/// Initialize a 1D sort and sweep broadphase. -cpSpatialIndex* cpSweep1DInit(cpSweep1D *sweep, cpSpatialIndexBBFunc bbfunc, cpSpatialIndex *staticIndex); -/// Allocate and initialize a 1D sort and sweep broadphase. -cpSpatialIndex* cpSweep1DNew(cpSpatialIndexBBFunc bbfunc, cpSpatialIndex *staticIndex); - -//MARK: Spatial Index Implementation - -typedef void (*cpSpatialIndexDestroyImpl)(cpSpatialIndex *index); - -typedef int (*cpSpatialIndexCountImpl)(cpSpatialIndex *index); -typedef void (*cpSpatialIndexEachImpl)(cpSpatialIndex *index, cpSpatialIndexIteratorFunc func, void *data); - -typedef cpBool (*cpSpatialIndexContainsImpl)(cpSpatialIndex *index, void *obj, cpHashValue hashid); -typedef void (*cpSpatialIndexInsertImpl)(cpSpatialIndex *index, void *obj, cpHashValue hashid); -typedef void (*cpSpatialIndexRemoveImpl)(cpSpatialIndex *index, void *obj, cpHashValue hashid); - -typedef void (*cpSpatialIndexReindexImpl)(cpSpatialIndex *index); -typedef void (*cpSpatialIndexReindexObjectImpl)(cpSpatialIndex *index, void *obj, cpHashValue hashid); -typedef void (*cpSpatialIndexReindexQueryImpl)(cpSpatialIndex *index, cpSpatialIndexQueryFunc func, void *data); - -typedef void (*cpSpatialIndexQueryImpl)(cpSpatialIndex *index, void *obj, cpBB bb, cpSpatialIndexQueryFunc func, void *data); -typedef void (*cpSpatialIndexSegmentQueryImpl)(cpSpatialIndex *index, void *obj, cpVect a, cpVect b, cpFloat t_exit, cpSpatialIndexSegmentQueryFunc func, void *data); - -struct cpSpatialIndexClass { - cpSpatialIndexDestroyImpl destroy; - - cpSpatialIndexCountImpl count; - cpSpatialIndexEachImpl each; - - cpSpatialIndexContainsImpl contains; - cpSpatialIndexInsertImpl insert; - cpSpatialIndexRemoveImpl remove; - - cpSpatialIndexReindexImpl reindex; - cpSpatialIndexReindexObjectImpl reindexObject; - cpSpatialIndexReindexQueryImpl reindexQuery; - - cpSpatialIndexQueryImpl query; - cpSpatialIndexSegmentQueryImpl segmentQuery; -}; - -/// Destroy and free a spatial index. -void cpSpatialIndexFree(cpSpatialIndex *index); -/// Collide the objects in @c dynamicIndex against the objects in @c staticIndex using the query callback function. -void cpSpatialIndexCollideStatic(cpSpatialIndex *dynamicIndex, cpSpatialIndex *staticIndex, cpSpatialIndexQueryFunc func, void *data); - -/// Destroy a spatial index. -static inline void cpSpatialIndexDestroy(cpSpatialIndex *index) -{ - if(index->klass) index->klass->destroy(index); -} - -/// Get the number of objects in the spatial index. -static inline int cpSpatialIndexCount(cpSpatialIndex *index) -{ - return index->klass->count(index); -} - -/// Iterate the objects in the spatial index. @c func will be called once for each object. -static inline void cpSpatialIndexEach(cpSpatialIndex *index, cpSpatialIndexIteratorFunc func, void *data) -{ - index->klass->each(index, func, data); -} - -/// Returns true if the spatial index contains the given object. -/// Most spatial indexes use hashed storage, so you must provide a hash value too. -static inline cpBool cpSpatialIndexContains(cpSpatialIndex *index, void *obj, cpHashValue hashid) -{ - return index->klass->contains(index, obj, hashid); -} - -/// Add an object to a spatial index. -/// Most spatial indexes use hashed storage, so you must provide a hash value too. -static inline void cpSpatialIndexInsert(cpSpatialIndex *index, void *obj, cpHashValue hashid) -{ - index->klass->insert(index, obj, hashid); -} - -/// Remove an object from a spatial index. -/// Most spatial indexes use hashed storage, so you must provide a hash value too. -static inline void cpSpatialIndexRemove(cpSpatialIndex *index, void *obj, cpHashValue hashid) -{ - index->klass->remove(index, obj, hashid); -} - -/// Perform a full reindex of a spatial index. -static inline void cpSpatialIndexReindex(cpSpatialIndex *index) -{ - index->klass->reindex(index); -} - -/// Reindex a single object in the spatial index. -static inline void cpSpatialIndexReindexObject(cpSpatialIndex *index, void *obj, cpHashValue hashid) -{ - index->klass->reindexObject(index, obj, hashid); -} - -/// Perform a rectangle query against the spatial index, calling @c func for each potential match. -static inline void cpSpatialIndexQuery(cpSpatialIndex *index, void *obj, cpBB bb, cpSpatialIndexQueryFunc func, void *data) -{ - index->klass->query(index, obj, bb, func, data); -} - -/// Perform a segment query against the spatial index, calling @c func for each potential match. -static inline void cpSpatialIndexSegmentQuery(cpSpatialIndex *index, void *obj, cpVect a, cpVect b, cpFloat t_exit, cpSpatialIndexSegmentQueryFunc func, void *data) -{ - index->klass->segmentQuery(index, obj, a, b, t_exit, func, data); -} - -/// Simultaneously reindex and find all colliding objects. -/// @c func will be called once for each potentially overlapping pair of objects found. -/// If the spatial index was initialized with a static index, it will collide it's objects against that as well. -static inline void cpSpatialIndexReindexQuery(cpSpatialIndex *index, cpSpatialIndexQueryFunc func, void *data) -{ - index->klass->reindexQuery(index, func, data); -} - -///@} diff --git a/chipmunk/include/chipmunk/cpVect.h b/chipmunk/include/chipmunk/cpVect.h deleted file mode 100644 index 90855e56e2..0000000000 --- a/chipmunk/include/chipmunk/cpVect.h +++ /dev/null @@ -1,213 +0,0 @@ -/* Copyright (c) 2007 Scott Lembcke - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/// @defgroup cpVect cpVect -/// Chipmunk's 2D vector type along with a handy 2D vector math lib. -/// @{ - -/// Constant for the zero vector. -static const cpVect cpvzero = {0.0f,0.0f}; - -/// Convenience constructor for cpVect structs. -static inline cpVect cpv(const cpFloat x, const cpFloat y) -{ - cpVect v = {x, y}; - return v; -} - -/// Spherical linearly interpolate between v1 and v2. -cpVect cpvslerp(const cpVect v1, const cpVect v2, const cpFloat t); - -/// Spherical linearly interpolate between v1 towards v2 by no more than angle a radians -cpVect cpvslerpconst(const cpVect v1, const cpVect v2, const cpFloat a); - -/// Returns a string representation of v. Intended mostly for debugging purposes and not production use. -/// @attention The string points to a static local and is reset every time the function is called. -/// If you want to print more than one vector you will have to split up your printing onto separate lines. -char* cpvstr(const cpVect v); - -/// Check if two vectors are equal. (Be careful when comparing floating point numbers!) -static inline cpBool cpveql(const cpVect v1, const cpVect v2) -{ - return (v1.x == v2.x && v1.y == v2.y); -} - -/// Add two vectors -static inline cpVect cpvadd(const cpVect v1, const cpVect v2) -{ - return cpv(v1.x + v2.x, v1.y + v2.y); -} - -/// Subtract two vectors. -static inline cpVect cpvsub(const cpVect v1, const cpVect v2) -{ - return cpv(v1.x - v2.x, v1.y - v2.y); -} - -/// Negate a vector. -static inline cpVect cpvneg(const cpVect v) -{ - return cpv(-v.x, -v.y); -} - -/// Scalar multiplication. -static inline cpVect cpvmult(const cpVect v, const cpFloat s) -{ - return cpv(v.x*s, v.y*s); -} - -/// Vector dot product. -static inline cpFloat cpvdot(const cpVect v1, const cpVect v2) -{ - return v1.x*v2.x + v1.y*v2.y; -} - -/// 2D vector cross product analog. -/// The cross product of 2D vectors results in a 3D vector with only a z component. -/// This function returns the magnitude of the z value. -static inline cpFloat cpvcross(const cpVect v1, const cpVect v2) -{ - return v1.x*v2.y - v1.y*v2.x; -} - -/// Returns a perpendicular vector. (90 degree rotation) -static inline cpVect cpvperp(const cpVect v) -{ - return cpv(-v.y, v.x); -} - -/// Returns a perpendicular vector. (-90 degree rotation) -static inline cpVect cpvrperp(const cpVect v) -{ - return cpv(v.y, -v.x); -} - -/// Returns the vector projection of v1 onto v2. -static inline cpVect cpvproject(const cpVect v1, const cpVect v2) -{ - return cpvmult(v2, cpvdot(v1, v2)/cpvdot(v2, v2)); -} - -/// Returns the unit length vector for the given angle (in radians). -static inline cpVect cpvforangle(const cpFloat a) -{ - return cpv(cpfcos(a), cpfsin(a)); -} - -/// Returns the angular direction v is pointing in (in radians). -static inline cpFloat cpvtoangle(const cpVect v) -{ - return cpfatan2(v.y, v.x); -} - -/// Uses complex number multiplication to rotate v1 by v2. Scaling will occur if v1 is not a unit vector. -static inline cpVect cpvrotate(const cpVect v1, const cpVect v2) -{ - return cpv(v1.x*v2.x - v1.y*v2.y, v1.x*v2.y + v1.y*v2.x); -} - -/// Inverse of cpvrotate(). -static inline cpVect cpvunrotate(const cpVect v1, const cpVect v2) -{ - return cpv(v1.x*v2.x + v1.y*v2.y, v1.y*v2.x - v1.x*v2.y); -} - -/// Returns the squared length of v. Faster than cpvlength() when you only need to compare lengths. -static inline cpFloat cpvlengthsq(const cpVect v) -{ - return cpvdot(v, v); -} - -/// Returns the length of v. -static inline cpFloat cpvlength(const cpVect v) -{ - return cpfsqrt(cpvdot(v, v)); -} - -/// Linearly interpolate between v1 and v2. -static inline cpVect cpvlerp(const cpVect v1, const cpVect v2, const cpFloat t) -{ - return cpvadd(cpvmult(v1, 1.0f - t), cpvmult(v2, t)); -} - -/// Returns a normalized copy of v. -static inline cpVect cpvnormalize(const cpVect v) -{ - // Neat trick I saw somewhere to avoid div/0. - return cpvmult(v, 1.0f/(cpvlength(v) + CPFLOAT_MIN)); -} - -/// @deprecated Just an alias for cpvnormalize() now. -static inline cpVect cpvnormalize_safe(const cpVect v) -{ - return cpvnormalize(v); -} - -/// Clamp v to length len. -static inline cpVect cpvclamp(const cpVect v, const cpFloat len) -{ - return (cpvdot(v,v) > len*len) ? cpvmult(cpvnormalize(v), len) : v; -} - -/// Linearly interpolate between v1 towards v2 by distance d. -static inline cpVect cpvlerpconst(cpVect v1, cpVect v2, cpFloat d) -{ - return cpvadd(v1, cpvclamp(cpvsub(v2, v1), d)); -} - -/// Returns the distance between v1 and v2. -static inline cpFloat cpvdist(const cpVect v1, const cpVect v2) -{ - return cpvlength(cpvsub(v1, v2)); -} - -/// Returns the squared distance between v1 and v2. Faster than cpvdist() when you only need to compare distances. -static inline cpFloat cpvdistsq(const cpVect v1, const cpVect v2) -{ - return cpvlengthsq(cpvsub(v1, v2)); -} - -/// Returns true if the distance between v1 and v2 is less than dist. -static inline cpBool cpvnear(const cpVect v1, const cpVect v2, const cpFloat dist) -{ - return cpvdistsq(v1, v2) < dist*dist; -} - -/// @} - -/// @defgroup cpMat2x2 cpMat2x2 -/// 2x2 matrix type used for tensors and such. -/// @{ - -static inline cpMat2x2 -cpMat2x2New(cpFloat a, cpFloat b, cpFloat c, cpFloat d) -{ - cpMat2x2 m = {a, b, c, d}; - return m; -} - -static inline cpVect -cpMat2x2Transform(cpMat2x2 m, cpVect v) -{ - return cpv(v.x*m.a + v.y*m.b, v.x*m.c + v.y*m.d); -} - -///@} diff --git a/chipmunk/proj.win32/chipmunk.vcxproj b/chipmunk/proj.win32/chipmunk.vcxproj deleted file mode 100644 index 0cdc2e835b..0000000000 --- a/chipmunk/proj.win32/chipmunk.vcxproj +++ /dev/null @@ -1,163 +0,0 @@ - - - - - Debug - Win32 - - - Release - Win32 - - - - libchipmunk - {207BC7A9-CCF1-4F2F-A04D-45F72242AE25} - Win32Proj - - - - StaticLibrary - Unicode - v100 - v110 - v110_xp - - - StaticLibrary - Unicode - v100 - v110 - v110_xp - - - - - - - - - - - - - <_ProjectFileVersion>10.0.40219.1 - $(SolutionDir)$(Configuration).win32\ - $(Configuration).win32\ - $(SolutionDir)$(Configuration).win32\ - $(Configuration).win32\ - AllRules.ruleset - - - AllRules.ruleset - - - - - $(MSBuildProgramFiles32)\Microsoft SDKs\Windows\v7.1A\lib;$(LibraryPath) - - - $(MSBuildProgramFiles32)\Microsoft SDKs\Windows\v7.1A\lib;$(LibraryPath) - - - - Disabled - $(MSBuildProgramFiles32)\Microsoft SDKs\Windows\v7.1A\include;..\include\chipmunk;%(AdditionalIncludeDirectories) - WIN32;_DEBUG;_LIB;%(PreprocessorDefinitions) - false - EnableFastChecks - MultiThreadedDebugDLL - false - - - Level3 - EditAndContinue - CompileAsCpp - 4068;4996;%(DisableSpecificWarnings) - true - - - $(OutDir)$(ProjectName).lib - - - - - $(MSBuildProgramFiles32)\Microsoft SDKs\Windows\v7.1A\include;..\include\chipmunk;%(AdditionalIncludeDirectories) - WIN32;NDEBUG;_LIB;%(PreprocessorDefinitions) - MultiThreadedDLL - false - - - Level3 - ProgramDatabase - CompileAsCpp - 4068;4996;%(DisableSpecificWarnings) - true - - - $(OutDir)$(ProjectName).lib - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/chipmunk/proj.win32/chipmunk.vcxproj.filters b/chipmunk/proj.win32/chipmunk.vcxproj.filters deleted file mode 100644 index b3dd86f096..0000000000 --- a/chipmunk/proj.win32/chipmunk.vcxproj.filters +++ /dev/null @@ -1,188 +0,0 @@ - - - - - {93995380-89BD-4b04-88EB-625FBE52EBFB} - h;hpp;hxx;hm;inl;inc;xsd - - - {4dabab00-1a6f-41ee-be09-a30737358f86} - - - {4FC737F1-C7A5-4376-A066-2A32D752A2FF} - cpp;c;cc;cxx;def;odl;idl;hpj;bat;asm;asmx - - - {e573d383-f016-40f6-999a-d804311ffc62} - - - - - include - - - include - - - include - - - include - - - include - - - include - - - include - - - include - - - include - - - include - - - include - - - include - - - include - - - include\constraints - - - include\constraints - - - include\constraints - - - include\constraints - - - include\constraints - - - include\constraints - - - include\constraints - - - include\constraints - - - include\constraints - - - include\constraints - - - include\constraints - - - include\constraints - - - src - - - - - src - - - src - - - src - - - src - - - src - - - src - - - src - - - src - - - src - - - src - - - src - - - src - - - src - - - src - - - src - - - src - - - src - - - src - - - src\constraints - - - src\constraints - - - src\constraints - - - src\constraints - - - src\constraints - - - src\constraints - - - src\constraints - - - src\constraints - - - src\constraints - - - src\constraints - - - src\constraints - - - \ No newline at end of file diff --git a/chipmunk/proj.win32/chipmunk.vcxproj.user b/chipmunk/proj.win32/chipmunk.vcxproj.user deleted file mode 100644 index ace9a86acb..0000000000 --- a/chipmunk/proj.win32/chipmunk.vcxproj.user +++ /dev/null @@ -1,3 +0,0 @@ - - - \ No newline at end of file diff --git a/chipmunk/proj.winrt/chipmunk.vcxproj b/chipmunk/proj.winrt/chipmunk.vcxproj deleted file mode 100644 index 9ffaa7ecf6..0000000000 --- a/chipmunk/proj.winrt/chipmunk.vcxproj +++ /dev/null @@ -1,480 +0,0 @@ - - - - - Debug - ARM - - - Debug - Win32 - - - Debug - x64 - - - Release - ARM - - - Release - Win32 - - - Release - x64 - - - - {8B143765-5C1B-4076-9384-3E0EC3650B1A} - Win32Proj - libchipmunk - libchipmunk - en-US - 12.0 - true - Windows Store - 8.1 - - - - StaticLibrary - true - v120 - - - StaticLibrary - true - v120 - - - StaticLibrary - true - v120 - - - StaticLibrary - false - true - v120 - - - StaticLibrary - false - true - v120 - - - StaticLibrary - false - true - v120 - - - - - - - - - - - - - - - - - - - - - - - - false - $(SolutionDir)\winrt$(Configuration)\$(MSBuildProjectName)\ - winrt$(Configuration)\ - - - false - $(SolutionDir)\winrt$(Configuration)\$(MSBuildProjectName)\ - winrt$(Configuration)\ - - - false - $(SolutionDir)\winrt$(Configuration)\$(MSBuildProjectName)\ - winrt$(Configuration)\ - - - false - $(SolutionDir)\winrt$(Configuration)\$(MSBuildProjectName)\ - winrt$(Configuration)\ - - - false - $(SolutionDir)\winrt$(Configuration)\$(MSBuildProjectName)\ - winrt$(Configuration)\ - - - false - $(SolutionDir)\winrt$(Configuration)\$(MSBuildProjectName)\ - winrt$(Configuration)\ - - - - NotUsing - false - true - ..\include\chipmunk;%(AdditionalIncludeDirectories) - WIN32;_DEBUG;_LIB;%(PreprocessorDefinitions) - 4068;4996;4056;%(DisableSpecificWarnings) - - - Console - false - false - - - - - NotUsing - false - true - $(MSBuildProgramFiles32)\Microsoft SDKs\Windows\v7.1A\include;..\include\chipmunk;%(AdditionalIncludeDirectories) - WIN32;NDEBUG;_LIB;%(PreprocessorDefinitions) - 4068;4996;4056;%(DisableSpecificWarnings) - - - Console - false - false - - - - - NotUsing - false - true - $(MSBuildProgramFiles32)\Microsoft SDKs\Windows\v7.1A\include;..\include\chipmunk;%(AdditionalIncludeDirectories) - WIN32;_DEBUG;_LIB;%(PreprocessorDefinitions) - 4068;4996;4056;%(DisableSpecificWarnings) - - - Console - false - false - - - - - NotUsing - false - true - $(MSBuildProgramFiles32)\Microsoft SDKs\Windows\v7.1A\include;..\include\chipmunk;%(AdditionalIncludeDirectories) - WIN32;NDEBUG;_LIB;%(PreprocessorDefinitions) - 4068;4996;4056;%(DisableSpecificWarnings) - - - Console - false - false - - - - - NotUsing - false - true - $(MSBuildProgramFiles32)\Microsoft SDKs\Windows\v7.1A\include;..\include\chipmunk;%(AdditionalIncludeDirectories) - WIN32;_DEBUG;_LIB;%(PreprocessorDefinitions) - 4068;4996;4056;%(DisableSpecificWarnings) - - - Console - false - false - - - - - NotUsing - false - true - $(MSBuildProgramFiles32)\Microsoft SDKs\Windows\v7.1A\include;..\include\chipmunk;%(AdditionalIncludeDirectories) - WIN32;NDEBUG;_LIB;%(PreprocessorDefinitions) - 4068;4996;4056;%(DisableSpecificWarnings) - - - Console - false - false - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - CompileAsCpp - CompileAsCpp - CompileAsCpp - CompileAsCpp - CompileAsCpp - CompileAsCpp - - - CompileAsCpp - CompileAsCpp - CompileAsCpp - CompileAsCpp - CompileAsCpp - CompileAsCpp - - - CompileAsCpp - CompileAsCpp - CompileAsCpp - CompileAsCpp - CompileAsCpp - CompileAsCpp - - - CompileAsCpp - CompileAsCpp - CompileAsCpp - CompileAsCpp - CompileAsCpp - CompileAsCpp - - - CompileAsCpp - CompileAsCpp - CompileAsCpp - CompileAsCpp - CompileAsCpp - CompileAsCpp - - - CompileAsCpp - CompileAsCpp - CompileAsCpp - CompileAsCpp - CompileAsCpp - CompileAsCpp - - - CompileAsCpp - CompileAsCpp - CompileAsCpp - CompileAsCpp - CompileAsCpp - CompileAsCpp - - - CompileAsCpp - CompileAsCpp - CompileAsCpp - CompileAsCpp - CompileAsCpp - CompileAsCpp - - - CompileAsCpp - CompileAsCpp - CompileAsCpp - CompileAsCpp - CompileAsCpp - CompileAsCpp - - - CompileAsCpp - CompileAsCpp - CompileAsCpp - CompileAsCpp - CompileAsCpp - CompileAsCpp - - - CompileAsCpp - CompileAsCpp - CompileAsCpp - CompileAsCpp - CompileAsCpp - CompileAsCpp - - - CompileAsCpp - CompileAsCpp - CompileAsCpp - CompileAsCpp - CompileAsCpp - CompileAsCpp - - - CompileAsCpp - CompileAsCpp - CompileAsCpp - CompileAsCpp - CompileAsCpp - CompileAsCpp - - - CompileAsCpp - CompileAsCpp - CompileAsCpp - CompileAsCpp - CompileAsCpp - CompileAsCpp - - - CompileAsCpp - CompileAsCpp - CompileAsCpp - CompileAsCpp - CompileAsCpp - CompileAsCpp - - - CompileAsCpp - CompileAsCpp - CompileAsCpp - CompileAsCpp - CompileAsCpp - CompileAsCpp - - - CompileAsCpp - CompileAsCpp - CompileAsCpp - CompileAsCpp - CompileAsCpp - CompileAsCpp - - - CompileAsCpp - CompileAsCpp - CompileAsCpp - CompileAsCpp - CompileAsCpp - CompileAsCpp - - - CompileAsCpp - CompileAsCpp - CompileAsCpp - CompileAsCpp - CompileAsCpp - CompileAsCpp - - - CompileAsCpp - CompileAsCpp - CompileAsCpp - CompileAsCpp - CompileAsCpp - CompileAsCpp - - - CompileAsCpp - CompileAsCpp - CompileAsCpp - CompileAsCpp - CompileAsCpp - CompileAsCpp - - - CompileAsCpp - CompileAsCpp - CompileAsCpp - CompileAsCpp - CompileAsCpp - CompileAsCpp - - - CompileAsCpp - CompileAsCpp - CompileAsCpp - CompileAsCpp - CompileAsCpp - CompileAsCpp - - - CompileAsCpp - CompileAsCpp - CompileAsCpp - CompileAsCpp - CompileAsCpp - CompileAsCpp - - - CompileAsCpp - CompileAsCpp - CompileAsCpp - CompileAsCpp - CompileAsCpp - CompileAsCpp - - - CompileAsCpp - CompileAsCpp - CompileAsCpp - CompileAsCpp - CompileAsCpp - CompileAsCpp - - - CompileAsCpp - CompileAsCpp - CompileAsCpp - CompileAsCpp - CompileAsCpp - CompileAsCpp - - - CompileAsCpp - CompileAsCpp - CompileAsCpp - CompileAsCpp - CompileAsCpp - CompileAsCpp - - - CompileAsCpp - CompileAsCpp - CompileAsCpp - CompileAsCpp - CompileAsCpp - CompileAsCpp - - - \ No newline at end of file diff --git a/chipmunk/proj.winrt/chipmunk.vcxproj.filters b/chipmunk/proj.winrt/chipmunk.vcxproj.filters deleted file mode 100644 index b3dd86f096..0000000000 --- a/chipmunk/proj.winrt/chipmunk.vcxproj.filters +++ /dev/null @@ -1,188 +0,0 @@ - - - - - {93995380-89BD-4b04-88EB-625FBE52EBFB} - h;hpp;hxx;hm;inl;inc;xsd - - - {4dabab00-1a6f-41ee-be09-a30737358f86} - - - {4FC737F1-C7A5-4376-A066-2A32D752A2FF} - cpp;c;cc;cxx;def;odl;idl;hpj;bat;asm;asmx - - - {e573d383-f016-40f6-999a-d804311ffc62} - - - - - include - - - include - - - include - - - include - - - include - - - include - - - include - - - include - - - include - - - include - - - include - - - include - - - include - - - include\constraints - - - include\constraints - - - include\constraints - - - include\constraints - - - include\constraints - - - include\constraints - - - include\constraints - - - include\constraints - - - include\constraints - - - include\constraints - - - include\constraints - - - include\constraints - - - src - - - - - src - - - src - - - src - - - src - - - src - - - src - - - src - - - src - - - src - - - src - - - src - - - src - - - src - - - src - - - src - - - src - - - src - - - src - - - src\constraints - - - src\constraints - - - src\constraints - - - src\constraints - - - src\constraints - - - src\constraints - - - src\constraints - - - src\constraints - - - src\constraints - - - src\constraints - - - src\constraints - - - \ No newline at end of file diff --git a/chipmunk/proj.wp8/chipmunk.vcxproj b/chipmunk/proj.wp8/chipmunk.vcxproj deleted file mode 100644 index 176e839e64..0000000000 --- a/chipmunk/proj.wp8/chipmunk.vcxproj +++ /dev/null @@ -1,335 +0,0 @@ - - - - - Debug - Win32 - - - Debug - ARM - - - Release - Win32 - - - Release - ARM - - - - {BA568A33-98F9-4D2A-BFFF-C065ADCB447A} - libchipmunk - en-US - 11.0 - - - - StaticLibrary - true - v110_wp80 - - - StaticLibrary - true - v110_wp80 - - - StaticLibrary - false - true - v110_wp80 - - - StaticLibrary - false - true - v110_wp80 - - - - - - - - false - - - - _LIB;%(PreprocessorDefinitions) - NotUsing - pch.h - $(WindowsSDK_MetadataPath);$(AdditionalUsingDirectories) - ..\include\chipmunk;%(AdditionalIncludeDirectories) - WIN32;_DEBUG;_LIB;%(PreprocessorDefinitions) - 4068;4996;%(DisableSpecificWarnings) - - - Console - false - - - - - _LIB;NDEBUG;%(PreprocessorDefinitions) - NotUsing - pch.h - $(WindowsSDK_MetadataPath);$(AdditionalUsingDirectories) - ..\include\chipmunk;%(AdditionalIncludeDirectories) - WIN32;NDEBUG;_LIB;%(PreprocessorDefinitions) - 4068;4996;%(DisableSpecificWarnings) - - - Console - false - - - - - _LIB;%(PreprocessorDefinitions) - NotUsing - pch.h - $(WindowsSDK_MetadataPath);$(AdditionalUsingDirectories) - ..\include\chipmunk;%(AdditionalIncludeDirectories) - WIN32;_DEBUG;_LIB;%(PreprocessorDefinitions) - 4068;4996;%(DisableSpecificWarnings) - - - Console - false - - - - - _LIB;NDEBUG;%(PreprocessorDefinitions) - NotUsing - pch.h - $(WindowsSDK_MetadataPath);$(AdditionalUsingDirectories) - ..\include\chipmunk;%(AdditionalIncludeDirectories) - WIN32;NDEBUG;_LIB;%(PreprocessorDefinitions) - 4068;4996;%(DisableSpecificWarnings) - - - Console - false - - - - - true - - - true - false - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - CompileAsCpp - CompileAsCpp - CompileAsCpp - CompileAsCpp - - - CompileAsCpp - CompileAsCpp - CompileAsCpp - CompileAsCpp - - - CompileAsCpp - CompileAsCpp - CompileAsCpp - CompileAsCpp - - - CompileAsCpp - CompileAsCpp - CompileAsCpp - CompileAsCpp - - - CompileAsCpp - CompileAsCpp - CompileAsCpp - CompileAsCpp - - - CompileAsCpp - CompileAsCpp - CompileAsCpp - CompileAsCpp - - - CompileAsCpp - CompileAsCpp - CompileAsCpp - CompileAsCpp - - - CompileAsCpp - CompileAsCpp - CompileAsCpp - CompileAsCpp - - - CompileAsCpp - CompileAsCpp - CompileAsCpp - CompileAsCpp - - - CompileAsCpp - CompileAsCpp - CompileAsCpp - CompileAsCpp - - - CompileAsCpp - CompileAsCpp - CompileAsCpp - CompileAsCpp - - - CompileAsCpp - CompileAsCpp - CompileAsCpp - CompileAsCpp - - - CompileAsCpp - CompileAsCpp - CompileAsCpp - CompileAsCpp - - - CompileAsCpp - CompileAsCpp - CompileAsCpp - CompileAsCpp - - - CompileAsCpp - CompileAsCpp - CompileAsCpp - CompileAsCpp - - - CompileAsCpp - CompileAsCpp - CompileAsCpp - CompileAsCpp - - - CompileAsCpp - CompileAsCpp - CompileAsCpp - CompileAsCpp - - - CompileAsCpp - CompileAsCpp - CompileAsCpp - CompileAsCpp - - - CompileAsCpp - CompileAsCpp - CompileAsCpp - CompileAsCpp - - - CompileAsCpp - CompileAsCpp - CompileAsCpp - CompileAsCpp - - - CompileAsCpp - CompileAsCpp - CompileAsCpp - CompileAsCpp - - - CompileAsCpp - CompileAsCpp - CompileAsCpp - CompileAsCpp - - - CompileAsCpp - CompileAsCpp - CompileAsCpp - CompileAsCpp - - - CompileAsCpp - CompileAsCpp - CompileAsCpp - CompileAsCpp - - - CompileAsCpp - CompileAsCpp - CompileAsCpp - CompileAsCpp - - - CompileAsCpp - CompileAsCpp - CompileAsCpp - CompileAsCpp - - - CompileAsCpp - CompileAsCpp - CompileAsCpp - CompileAsCpp - - - CompileAsCpp - CompileAsCpp - CompileAsCpp - CompileAsCpp - - - CompileAsCpp - CompileAsCpp - CompileAsCpp - CompileAsCpp - - - \ No newline at end of file diff --git a/chipmunk/proj.wp8/chipmunk.vcxproj.filters b/chipmunk/proj.wp8/chipmunk.vcxproj.filters deleted file mode 100644 index b3dd86f096..0000000000 --- a/chipmunk/proj.wp8/chipmunk.vcxproj.filters +++ /dev/null @@ -1,188 +0,0 @@ - - - - - {93995380-89BD-4b04-88EB-625FBE52EBFB} - h;hpp;hxx;hm;inl;inc;xsd - - - {4dabab00-1a6f-41ee-be09-a30737358f86} - - - {4FC737F1-C7A5-4376-A066-2A32D752A2FF} - cpp;c;cc;cxx;def;odl;idl;hpj;bat;asm;asmx - - - {e573d383-f016-40f6-999a-d804311ffc62} - - - - - include - - - include - - - include - - - include - - - include - - - include - - - include - - - include - - - include - - - include - - - include - - - include - - - include - - - include\constraints - - - include\constraints - - - include\constraints - - - include\constraints - - - include\constraints - - - include\constraints - - - include\constraints - - - include\constraints - - - include\constraints - - - include\constraints - - - include\constraints - - - include\constraints - - - src - - - - - src - - - src - - - src - - - src - - - src - - - src - - - src - - - src - - - src - - - src - - - src - - - src - - - src - - - src - - - src - - - src - - - src - - - src - - - src\constraints - - - src\constraints - - - src\constraints - - - src\constraints - - - src\constraints - - - src\constraints - - - src\constraints - - - src\constraints - - - src\constraints - - - src\constraints - - - src\constraints - - - \ No newline at end of file diff --git a/chipmunk/proj.wp8/chipmunk.vcxproj.user b/chipmunk/proj.wp8/chipmunk.vcxproj.user deleted file mode 100644 index 17f726ed50..0000000000 --- a/chipmunk/proj.wp8/chipmunk.vcxproj.user +++ /dev/null @@ -1,6 +0,0 @@ - - - - true - - \ No newline at end of file diff --git a/chipmunk/src/CMakeLists.txt b/chipmunk/src/CMakeLists.txt deleted file mode 100644 index 4005c750f4..0000000000 --- a/chipmunk/src/CMakeLists.txt +++ /dev/null @@ -1,57 +0,0 @@ -set(BUILD_STATIC 1) - -file(GLOB chipmunk_source_files "*.c" "constraints/*.c") -file(GLOB chipmunk_public_header "${chipmunk_SOURCE_DIR}/include/chipmunk/*.h") -file(GLOB chipmunk_constraint_header "${chipmunk_SOURCE_DIR}/include/chipmunk/constraints/*.h") - -include_directories(${chipmunk_SOURCE_DIR}/include/chipmunk) - -if(BUILD_SHARED) - add_library(chipmunk SHARED - ${chipmunk_source_files} - ) - # Tell MSVC to compile the code as C++. - if(MSVC) - set_source_files_properties(${chipmunk_source_files} PROPERTIES LANGUAGE CXX) - set_target_properties(chipmunk PROPERTIES LINKER_LANGUAGE CXX) - endif(MSVC) - # set the lib's version number - # But avoid on Android because symlinks to version numbered .so's don't work with Android's Java-side loadLibrary. - if(NOT ANDROID) - set_target_properties(chipmunk PROPERTIES VERSION 6.2.1) - endif(NOT ANDROID) - if(ANDROID) - # need to explicitly link to the math library because the CMake/Android toolchains may not do it automatically - target_link_libraries(chipmunk m) - endif(ANDROID) - install(TARGETS chipmunk RUNTIME DESTINATION lib LIBRARY DESTINATION lib) -endif(BUILD_SHARED) - -if(BUILD_STATIC) - add_library(chipmunk_static STATIC - ${chipmunk_source_files} - ) - # Tell MSVC to compile the code as C++. - if(MSVC) - set_source_files_properties(${chipmunk_source_files} PROPERTIES LANGUAGE CXX) - set_target_properties(chipmunk_static PROPERTIES LINKER_LANGUAGE CXX) - endif(MSVC) - # Sets chipmunk_static to output "libchipmunk.a" not "libchipmunk_static.a" - set_target_properties(chipmunk_static PROPERTIES OUTPUT_NAME chipmunk) - if(INSTALL_STATIC) - install(TARGETS chipmunk_static ARCHIVE DESTINATION lib) - endif(INSTALL_STATIC) -endif(BUILD_STATIC) - -if(BUILD_SHARED OR INSTALL_STATIC) - # FIXME: change to PUBLIC_HEADER to allow building frameworks - install(FILES ${chipmunk_public_header} DESTINATION include/chipmunk) - install(FILES ${chipmunk_constraint_header} DESTINATION include/chipmunk/constraints) -endif(BUILD_SHARED OR INSTALL_STATIC) - -set_target_properties(chipmunk_static - PROPERTIES - ARCHIVE_OUTPUT_DIRECTORY "${CMAKE_BINARY_DIR}/lib" - LIBRARY_OUTPUT_DIRECTORY "${CMAKE_BINARY_DIR}/lib" -) - diff --git a/chipmunk/src/chipmunk.c b/chipmunk/src/chipmunk.c deleted file mode 100644 index 2c098df4f6..0000000000 --- a/chipmunk/src/chipmunk.c +++ /dev/null @@ -1,324 +0,0 @@ -/* Copyright (c) 2007 Scott Lembcke - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -#include -#include -#include - -#include "chipmunk_private.h" - -void -cpMessage(const char *condition, const char *file, int line, cpBool isError, cpBool isHardError, const char *message, ...) -{ - fprintf(stderr, (isError ? "Aborting due to Chipmunk error: " : "Chipmunk warning: ")); - - va_list vargs; - va_start(vargs, message); { - vfprintf(stderr, message, vargs); - fprintf(stderr, "\n"); - } va_end(vargs); - - fprintf(stderr, "\tFailed condition: %s\n", condition); - fprintf(stderr, "\tSource:%s:%d\n", file, line); - - if(isError) abort(); -} - -#define STR(s) #s -#define XSTR(s) STR(s) - -const char *cpVersionString = XSTR(CP_VERSION_MAJOR)"."XSTR(CP_VERSION_MINOR)"."XSTR(CP_VERSION_RELEASE); - -void -cpInitChipmunk(void) -{ - cpAssertWarn(cpFalse, "cpInitChipmunk is deprecated and no longer required. It will be removed in the future."); -} - -//MARK: Misc Functions - -cpFloat -cpMomentForCircle(cpFloat m, cpFloat r1, cpFloat r2, cpVect offset) -{ - return m*(0.5f*(r1*r1 + r2*r2) + cpvlengthsq(offset)); -} - -cpFloat -cpAreaForCircle(cpFloat r1, cpFloat r2) -{ - return (cpFloat)M_PI*cpfabs(r1*r1 - r2*r2); -} - -cpFloat -cpMomentForSegment(cpFloat m, cpVect a, cpVect b) -{ - cpVect offset = cpvmult(cpvadd(a, b), 0.5f); - return m*(cpvdistsq(b, a)/12.0f + cpvlengthsq(offset)); -} - -cpFloat -cpAreaForSegment(cpVect a, cpVect b, cpFloat r) -{ - return r*((cpFloat)M_PI*r + 2.0f*cpvdist(a, b)); -} - -cpFloat -cpMomentForPoly(cpFloat m, const int numVerts, const cpVect *verts, cpVect offset) -{ - if(numVerts == 2) return cpMomentForSegment(m, verts[0], verts[1]); - - cpFloat sum1 = 0.0f; - cpFloat sum2 = 0.0f; - for(int i=0; i max.x || (v.x == max.x && v.y > max.y)){ - max = v; - (*end) = i; - } - } -} - -#define SWAP(__A__, __B__) {cpVect __TMP__ = __A__; __A__ = __B__; __B__ = __TMP__;} - -static int -QHullPartition(cpVect *verts, int count, cpVect a, cpVect b, cpFloat tol) -{ - if(count == 0) return 0; - - cpFloat max = 0; - int pivot = 0; - - cpVect delta = cpvsub(b, a); - cpFloat valueTol = tol*cpvlength(delta); - - int head = 0; - for(int tail = count-1; head <= tail;){ - cpFloat value = cpvcross(delta, cpvsub(verts[head], a)); - if(value > valueTol){ - if(value > max){ - max = value; - pivot = head; - } - - head++; - } else { - SWAP(verts[head], verts[tail]); - tail--; - } - } - - // move the new pivot to the front if it's not already there. - if(pivot != 0) SWAP(verts[0], verts[pivot]); - return head; -} - -static int -QHullReduce(cpFloat tol, cpVect *verts, int count, cpVect a, cpVect pivot, cpVect b, cpVect *result) -{ - if(count < 0){ - return 0; - } else if(count == 0) { - result[0] = pivot; - return 1; - } else { - int left_count = QHullPartition(verts, count, a, pivot, tol); - int index = QHullReduce(tol, verts + 1, left_count - 1, a, verts[0], pivot, result); - - result[index++] = pivot; - - int right_count = QHullPartition(verts + left_count, count - left_count, pivot, b, tol); - return index + QHullReduce(tol, verts + left_count + 1, right_count - 1, pivot, verts[left_count], b, result + index); - } -} - -// QuickHull seemed like a neat algorithm, and efficient-ish for large input sets. -// My implementation performs an in place reduction using the result array as scratch space. -int -cpConvexHull(int count, cpVect *verts, cpVect *result, int *first, cpFloat tol) -{ - if(result){ - // Copy the line vertexes into the empty part of the result polyline to use as a scratch buffer. - memcpy(result, verts, count*sizeof(cpVect)); - } else { - // If a result array was not specified, reduce the input instead. - result = verts; - } - - // Degenerate case, all poins are the same. - int start, end; - cpLoopIndexes(verts, count, &start, &end); - if(start == end){ - if(first) (*first) = 0; - return 1; - } - - SWAP(result[0], result[start]); - SWAP(result[1], result[end == 0 ? start : end]); - - cpVect a = result[0]; - cpVect b = result[1]; - - if(first) (*first) = start; - int resultCount = QHullReduce(tol, result + 2, count - 2, a, b, a, result + 1) + 1; - cpAssertSoft(cpPolyValidate(result, resultCount), - "Internal error: cpConvexHull() and cpPolyValidate() did not agree." - "Please report this error with as much info as you can."); - return resultCount; -} - -//MARK: Alternate Block Iterators - -#if defined(__has_extension) -#if __has_extension(blocks) - -static void IteratorFunc(void *ptr, void (^block)(void *ptr)){block(ptr);} - -void cpSpaceEachBody_b(cpSpace *space, void (^block)(cpBody *body)){ - cpSpaceEachBody(space, (cpSpaceBodyIteratorFunc)IteratorFunc, block); -} - -void cpSpaceEachShape_b(cpSpace *space, void (^block)(cpShape *shape)){ - cpSpaceEachShape(space, (cpSpaceShapeIteratorFunc)IteratorFunc, block); -} - -void cpSpaceEachConstraint_b(cpSpace *space, void (^block)(cpConstraint *constraint)){ - cpSpaceEachConstraint(space, (cpSpaceConstraintIteratorFunc)IteratorFunc, block); -} - -static void BodyIteratorFunc(cpBody *body, void *ptr, void (^block)(void *ptr)){block(ptr);} - -void cpBodyEachShape_b(cpBody *body, void (^block)(cpShape *shape)){ - cpBodyEachShape(body, (cpBodyShapeIteratorFunc)BodyIteratorFunc, block); -} - -void cpBodyEachConstraint_b(cpBody *body, void (^block)(cpConstraint *constraint)){ - cpBodyEachConstraint(body, (cpBodyConstraintIteratorFunc)BodyIteratorFunc, block); -} - -void cpBodyEachArbiter_b(cpBody *body, void (^block)(cpArbiter *arbiter)){ - cpBodyEachArbiter(body, (cpBodyArbiterIteratorFunc)BodyIteratorFunc, block); -} - -static void NearestPointQueryIteratorFunc(cpShape *shape, cpFloat distance, cpVect point, cpSpaceNearestPointQueryBlock block){block(shape, distance, point);} -void cpSpaceNearestPointQuery_b(cpSpace *space, cpVect point, cpFloat maxDistance, cpLayers layers, cpGroup group, cpSpaceNearestPointQueryBlock block){ - cpSpaceNearestPointQuery(space, point, maxDistance, layers, group, (cpSpaceNearestPointQueryFunc)NearestPointQueryIteratorFunc, block); -} - -static void SegmentQueryIteratorFunc(cpShape *shape, cpFloat t, cpVect n, cpSpaceSegmentQueryBlock block){block(shape, t, n);} -void cpSpaceSegmentQuery_b(cpSpace *space, cpVect start, cpVect end, cpLayers layers, cpGroup group, cpSpaceSegmentQueryBlock block){ - cpSpaceSegmentQuery(space, start, end, layers, group, (cpSpaceSegmentQueryFunc)SegmentQueryIteratorFunc, block); -} - -void cpSpaceBBQuery_b(cpSpace *space, cpBB bb, cpLayers layers, cpGroup group, cpSpaceBBQueryBlock block){ - cpSpaceBBQuery(space, bb, layers, group, (cpSpaceBBQueryFunc)IteratorFunc, block); -} - -static void ShapeQueryIteratorFunc(cpShape *shape, cpContactPointSet *points, cpSpaceShapeQueryBlock block){block(shape, points);} -cpBool cpSpaceShapeQuery_b(cpSpace *space, cpShape *shape, cpSpaceShapeQueryBlock block){ - return cpSpaceShapeQuery(space, shape, (cpSpaceShapeQueryFunc)ShapeQueryIteratorFunc, block); -} - -#endif -#endif - -#include "chipmunk_ffi.h" diff --git a/chipmunk/src/constraints/cpConstraint.c b/chipmunk/src/constraints/cpConstraint.c deleted file mode 100644 index d3902f46fe..0000000000 --- a/chipmunk/src/constraints/cpConstraint.c +++ /dev/null @@ -1,58 +0,0 @@ -/* Copyright (c) 2007 Scott Lembcke - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -#include "chipmunk_private.h" -#include "constraints/util.h" - -// TODO: Comment me! - -void cpConstraintDestroy(cpConstraint *constraint){} - -void -cpConstraintFree(cpConstraint *constraint) -{ - if(constraint){ - cpConstraintDestroy(constraint); - cpfree(constraint); - } -} - -// *** declared in util.h TODO move declaration to chipmunk_private.h - -void -cpConstraintInit(cpConstraint *constraint, const cpConstraintClass *klass, cpBody *a, cpBody *b) -{ - constraint->klass = klass; - - constraint->a = a; - constraint->b = b; - constraint->space = NULL; - - constraint->next_a = NULL; - constraint->next_b = NULL; - - constraint->maxForce = (cpFloat)INFINITY; - constraint->errorBias = cpfpow(1.0f - 0.1f, 60.0f); - constraint->maxBias = (cpFloat)INFINITY; - - constraint->preSolve = NULL; - constraint->postSolve = NULL; -} diff --git a/chipmunk/src/constraints/cpDampedRotarySpring.c b/chipmunk/src/constraints/cpDampedRotarySpring.c deleted file mode 100644 index f57c289e7f..0000000000 --- a/chipmunk/src/constraints/cpDampedRotarySpring.c +++ /dev/null @@ -1,114 +0,0 @@ -/* Copyright (c) 2007 Scott Lembcke - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -#include "chipmunk_private.h" -#include "constraints/util.h" - -static cpFloat -defaultSpringTorque(cpDampedRotarySpring *spring, cpFloat relativeAngle){ - return (relativeAngle - spring->restAngle)*spring->stiffness; -} - -static void -preStep(cpDampedRotarySpring *spring, cpFloat dt) -{ - cpBody *a = spring->constraint.a; - cpBody *b = spring->constraint.b; - - cpFloat moment = a->i_inv + b->i_inv; - cpAssertSoft(moment != 0.0, "Unsolvable spring."); - spring->iSum = 1.0f/moment; - - spring->w_coef = 1.0f - cpfexp(-spring->damping*dt*moment); - spring->target_wrn = 0.0f; - - // apply spring torque - cpFloat j_spring = spring->springTorqueFunc((cpConstraint *)spring, a->a - b->a)*dt; - spring->jAcc = j_spring; - - a->w -= j_spring*a->i_inv; - b->w += j_spring*b->i_inv; -} - -static void applyCachedImpulse(cpDampedRotarySpring *spring, cpFloat dt_coef){} - -static void -applyImpulse(cpDampedRotarySpring *spring, cpFloat dt) -{ - cpBody *a = spring->constraint.a; - cpBody *b = spring->constraint.b; - - // compute relative velocity - cpFloat wrn = a->w - b->w;//normal_relative_velocity(a, b, r1, r2, n) - spring->target_vrn; - - // compute velocity loss from drag - // not 100% certain this is derived correctly, though it makes sense - cpFloat w_damp = (spring->target_wrn - wrn)*spring->w_coef; - spring->target_wrn = wrn + w_damp; - - //apply_impulses(a, b, spring->r1, spring->r2, cpvmult(spring->n, v_damp*spring->nMass)); - cpFloat j_damp = w_damp*spring->iSum; - spring->jAcc += j_damp; - - a->w += j_damp*a->i_inv; - b->w -= j_damp*b->i_inv; -} - -static cpFloat -getImpulse(cpDampedRotarySpring *spring) -{ - return spring->jAcc; -} - -static const cpConstraintClass klass = { - (cpConstraintPreStepImpl)preStep, - (cpConstraintApplyCachedImpulseImpl)applyCachedImpulse, - (cpConstraintApplyImpulseImpl)applyImpulse, - (cpConstraintGetImpulseImpl)getImpulse, -}; -CP_DefineClassGetter(cpDampedRotarySpring) - -cpDampedRotarySpring * -cpDampedRotarySpringAlloc(void) -{ - return (cpDampedRotarySpring *)cpcalloc(1, sizeof(cpDampedRotarySpring)); -} - -cpDampedRotarySpring * -cpDampedRotarySpringInit(cpDampedRotarySpring *spring, cpBody *a, cpBody *b, cpFloat restAngle, cpFloat stiffness, cpFloat damping) -{ - cpConstraintInit((cpConstraint *)spring, &klass, a, b); - - spring->restAngle = restAngle; - spring->stiffness = stiffness; - spring->damping = damping; - spring->springTorqueFunc = (cpDampedRotarySpringTorqueFunc)defaultSpringTorque; - - spring->jAcc = 0.0f; - - return spring; -} - -cpConstraint * -cpDampedRotarySpringNew(cpBody *a, cpBody *b, cpFloat restAngle, cpFloat stiffness, cpFloat damping) -{ - return (cpConstraint *)cpDampedRotarySpringInit(cpDampedRotarySpringAlloc(), a, b, restAngle, stiffness, damping); -} diff --git a/chipmunk/src/constraints/cpDampedSpring.c b/chipmunk/src/constraints/cpDampedSpring.c deleted file mode 100644 index 5a855f818d..0000000000 --- a/chipmunk/src/constraints/cpDampedSpring.c +++ /dev/null @@ -1,122 +0,0 @@ -/* Copyright (c) 2007 Scott Lembcke - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -#include "chipmunk_private.h" -#include "constraints/util.h" - -static cpFloat -defaultSpringForce(cpDampedSpring *spring, cpFloat dist){ - return (spring->restLength - dist)*spring->stiffness; -} - -static void -preStep(cpDampedSpring *spring, cpFloat dt) -{ - cpBody *a = spring->constraint.a; - cpBody *b = spring->constraint.b; - - spring->r1 = cpvrotate(spring->anchr1, a->rot); - spring->r2 = cpvrotate(spring->anchr2, b->rot); - - cpVect delta = cpvsub(cpvadd(b->p, spring->r2), cpvadd(a->p, spring->r1)); - cpFloat dist = cpvlength(delta); - spring->n = cpvmult(delta, 1.0f/(dist ? dist : INFINITY)); - - cpFloat k = k_scalar(a, b, spring->r1, spring->r2, spring->n); - cpAssertSoft(k != 0.0, "Unsolvable spring."); - spring->nMass = 1.0f/k; - - spring->target_vrn = 0.0f; - spring->v_coef = 1.0f - cpfexp(-spring->damping*dt*k); - - // apply spring force - cpFloat f_spring = spring->springForceFunc((cpConstraint *)spring, dist); - cpFloat j_spring = spring->jAcc = f_spring*dt; - apply_impulses(a, b, spring->r1, spring->r2, cpvmult(spring->n, j_spring)); -} - -static void applyCachedImpulse(cpDampedSpring *spring, cpFloat dt_coef){} - -static void -applyImpulse(cpDampedSpring *spring, cpFloat dt) -{ - cpBody *a = spring->constraint.a; - cpBody *b = spring->constraint.b; - - cpVect n = spring->n; - cpVect r1 = spring->r1; - cpVect r2 = spring->r2; - - // compute relative velocity - cpFloat vrn = normal_relative_velocity(a, b, r1, r2, n); - - // compute velocity loss from drag - cpFloat v_damp = (spring->target_vrn - vrn)*spring->v_coef; - spring->target_vrn = vrn + v_damp; - - cpFloat j_damp = v_damp*spring->nMass; - spring->jAcc += j_damp; - apply_impulses(a, b, spring->r1, spring->r2, cpvmult(spring->n, j_damp)); -} - -static cpFloat -getImpulse(cpDampedSpring *spring) -{ - return spring->jAcc; -} - -static const cpConstraintClass klass = { - (cpConstraintPreStepImpl)preStep, - (cpConstraintApplyCachedImpulseImpl)applyCachedImpulse, - (cpConstraintApplyImpulseImpl)applyImpulse, - (cpConstraintGetImpulseImpl)getImpulse, -}; -CP_DefineClassGetter(cpDampedSpring) - -cpDampedSpring * -cpDampedSpringAlloc(void) -{ - return (cpDampedSpring *)cpcalloc(1, sizeof(cpDampedSpring)); -} - -cpDampedSpring * -cpDampedSpringInit(cpDampedSpring *spring, cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2, cpFloat restLength, cpFloat stiffness, cpFloat damping) -{ - cpConstraintInit((cpConstraint *)spring, cpDampedSpringGetClass(), a, b); - - spring->anchr1 = anchr1; - spring->anchr2 = anchr2; - - spring->restLength = restLength; - spring->stiffness = stiffness; - spring->damping = damping; - spring->springForceFunc = (cpDampedSpringForceFunc)defaultSpringForce; - - spring->jAcc = 0.0f; - - return spring; -} - -cpConstraint * -cpDampedSpringNew(cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2, cpFloat restLength, cpFloat stiffness, cpFloat damping) -{ - return (cpConstraint *)cpDampedSpringInit(cpDampedSpringAlloc(), a, b, anchr1, anchr2, restLength, stiffness, damping); -} diff --git a/chipmunk/src/constraints/cpGearJoint.c b/chipmunk/src/constraints/cpGearJoint.c deleted file mode 100644 index 818c1b4660..0000000000 --- a/chipmunk/src/constraints/cpGearJoint.c +++ /dev/null @@ -1,119 +0,0 @@ -/* Copyright (c) 2007 Scott Lembcke - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -#include "chipmunk_private.h" -#include "constraints/util.h" - -static void -preStep(cpGearJoint *joint, cpFloat dt) -{ - cpBody *a = joint->constraint.a; - cpBody *b = joint->constraint.b; - - // calculate moment of inertia coefficient. - joint->iSum = 1.0f/(a->i_inv*joint->ratio_inv + joint->ratio*b->i_inv); - - // calculate bias velocity - cpFloat maxBias = joint->constraint.maxBias; - joint->bias = cpfclamp(-bias_coef(joint->constraint.errorBias, dt)*(b->a*joint->ratio - a->a - joint->phase)/dt, -maxBias, maxBias); -} - -static void -applyCachedImpulse(cpGearJoint *joint, cpFloat dt_coef) -{ - cpBody *a = joint->constraint.a; - cpBody *b = joint->constraint.b; - - cpFloat j = joint->jAcc*dt_coef; - a->w -= j*a->i_inv*joint->ratio_inv; - b->w += j*b->i_inv; -} - -static void -applyImpulse(cpGearJoint *joint, cpFloat dt) -{ - cpBody *a = joint->constraint.a; - cpBody *b = joint->constraint.b; - - // compute relative rotational velocity - cpFloat wr = b->w*joint->ratio - a->w; - - cpFloat jMax = joint->constraint.maxForce*dt; - - // compute normal impulse - cpFloat j = (joint->bias - wr)*joint->iSum; - cpFloat jOld = joint->jAcc; - joint->jAcc = cpfclamp(jOld + j, -jMax, jMax); - j = joint->jAcc - jOld; - - // apply impulse - a->w -= j*a->i_inv*joint->ratio_inv; - b->w += j*b->i_inv; -} - -static cpFloat -getImpulse(cpGearJoint *joint) -{ - return cpfabs(joint->jAcc); -} - -static const cpConstraintClass klass = { - (cpConstraintPreStepImpl)preStep, - (cpConstraintApplyCachedImpulseImpl)applyCachedImpulse, - (cpConstraintApplyImpulseImpl)applyImpulse, - (cpConstraintGetImpulseImpl)getImpulse, -}; -CP_DefineClassGetter(cpGearJoint) - -cpGearJoint * -cpGearJointAlloc(void) -{ - return (cpGearJoint *)cpcalloc(1, sizeof(cpGearJoint)); -} - -cpGearJoint * -cpGearJointInit(cpGearJoint *joint, cpBody *a, cpBody *b, cpFloat phase, cpFloat ratio) -{ - cpConstraintInit((cpConstraint *)joint, &klass, a, b); - - joint->phase = phase; - joint->ratio = ratio; - joint->ratio_inv = 1.0f/ratio; - - joint->jAcc = 0.0f; - - return joint; -} - -cpConstraint * -cpGearJointNew(cpBody *a, cpBody *b, cpFloat phase, cpFloat ratio) -{ - return (cpConstraint *)cpGearJointInit(cpGearJointAlloc(), a, b, phase, ratio); -} - -void -cpGearJointSetRatio(cpConstraint *constraint, cpFloat value) -{ - cpConstraintCheckCast(constraint, cpGearJoint); - ((cpGearJoint *)constraint)->ratio = value; - ((cpGearJoint *)constraint)->ratio_inv = 1.0f/value; - cpConstraintActivateBodies(constraint); -} diff --git a/chipmunk/src/constraints/cpGrooveJoint.c b/chipmunk/src/constraints/cpGrooveJoint.c deleted file mode 100644 index efe14aed71..0000000000 --- a/chipmunk/src/constraints/cpGrooveJoint.c +++ /dev/null @@ -1,165 +0,0 @@ -/* Copyright (c) 2007 Scott Lembcke - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -#include "chipmunk_private.h" -#include "constraints/util.h" - -static void -preStep(cpGrooveJoint *joint, cpFloat dt) -{ - cpBody *a = joint->constraint.a; - cpBody *b = joint->constraint.b; - - // calculate endpoints in worldspace - cpVect ta = cpBodyLocal2World(a, joint->grv_a); - cpVect tb = cpBodyLocal2World(a, joint->grv_b); - - // calculate axis - cpVect n = cpvrotate(joint->grv_n, a->rot); - cpFloat d = cpvdot(ta, n); - - joint->grv_tn = n; - joint->r2 = cpvrotate(joint->anchr2, b->rot); - - // calculate tangential distance along the axis of r2 - cpFloat td = cpvcross(cpvadd(b->p, joint->r2), n); - // calculate clamping factor and r2 - if(td <= cpvcross(ta, n)){ - joint->clamp = 1.0f; - joint->r1 = cpvsub(ta, a->p); - } else if(td >= cpvcross(tb, n)){ - joint->clamp = -1.0f; - joint->r1 = cpvsub(tb, a->p); - } else { - joint->clamp = 0.0f; - joint->r1 = cpvsub(cpvadd(cpvmult(cpvperp(n), -td), cpvmult(n, d)), a->p); - } - - // Calculate mass tensor - joint->k = k_tensor(a, b, joint->r1, joint->r2); - - // calculate bias velocity - cpVect delta = cpvsub(cpvadd(b->p, joint->r2), cpvadd(a->p, joint->r1)); - joint->bias = cpvclamp(cpvmult(delta, -bias_coef(joint->constraint.errorBias, dt)/dt), joint->constraint.maxBias); -} - -static void -applyCachedImpulse(cpGrooveJoint *joint, cpFloat dt_coef) -{ - cpBody *a = joint->constraint.a; - cpBody *b = joint->constraint.b; - - apply_impulses(a, b, joint->r1, joint->r2, cpvmult(joint->jAcc, dt_coef)); -} - -static inline cpVect -grooveConstrain(cpGrooveJoint *joint, cpVect j, cpFloat dt){ - cpVect n = joint->grv_tn; - cpVect jClamp = (joint->clamp*cpvcross(j, n) > 0.0f) ? j : cpvproject(j, n); - return cpvclamp(jClamp, joint->constraint.maxForce*dt); -} - -static void -applyImpulse(cpGrooveJoint *joint, cpFloat dt) -{ - cpBody *a = joint->constraint.a; - cpBody *b = joint->constraint.b; - - cpVect r1 = joint->r1; - cpVect r2 = joint->r2; - - // compute impulse - cpVect vr = relative_velocity(a, b, r1, r2); - - cpVect j = cpMat2x2Transform(joint->k, cpvsub(joint->bias, vr)); - cpVect jOld = joint->jAcc; - joint->jAcc = grooveConstrain(joint, cpvadd(jOld, j), dt); - j = cpvsub(joint->jAcc, jOld); - - // apply impulse - apply_impulses(a, b, joint->r1, joint->r2, j); -} - -static cpFloat -getImpulse(cpGrooveJoint *joint) -{ - return cpvlength(joint->jAcc); -} - -static const cpConstraintClass klass = { - (cpConstraintPreStepImpl)preStep, - (cpConstraintApplyCachedImpulseImpl)applyCachedImpulse, - (cpConstraintApplyImpulseImpl)applyImpulse, - (cpConstraintGetImpulseImpl)getImpulse, -}; -CP_DefineClassGetter(cpGrooveJoint) - -cpGrooveJoint * -cpGrooveJointAlloc(void) -{ - return (cpGrooveJoint *)cpcalloc(1, sizeof(cpGrooveJoint)); -} - -cpGrooveJoint * -cpGrooveJointInit(cpGrooveJoint *joint, cpBody *a, cpBody *b, cpVect groove_a, cpVect groove_b, cpVect anchr2) -{ - cpConstraintInit((cpConstraint *)joint, &klass, a, b); - - joint->grv_a = groove_a; - joint->grv_b = groove_b; - joint->grv_n = cpvperp(cpvnormalize(cpvsub(groove_b, groove_a))); - joint->anchr2 = anchr2; - - joint->jAcc = cpvzero; - - return joint; -} - -cpConstraint * -cpGrooveJointNew(cpBody *a, cpBody *b, cpVect groove_a, cpVect groove_b, cpVect anchr2) -{ - return (cpConstraint *)cpGrooveJointInit(cpGrooveJointAlloc(), a, b, groove_a, groove_b, anchr2); -} - -void -cpGrooveJointSetGrooveA(cpConstraint *constraint, cpVect value) -{ - cpGrooveJoint *g = (cpGrooveJoint *)constraint; - cpConstraintCheckCast(constraint, cpGrooveJoint); - - g->grv_a = value; - g->grv_n = cpvperp(cpvnormalize(cpvsub(g->grv_b, value))); - - cpConstraintActivateBodies(constraint); -} - -void -cpGrooveJointSetGrooveB(cpConstraint *constraint, cpVect value) -{ - cpGrooveJoint *g = (cpGrooveJoint *)constraint; - cpConstraintCheckCast(constraint, cpGrooveJoint); - - g->grv_b = value; - g->grv_n = cpvperp(cpvnormalize(cpvsub(value, g->grv_a))); - - cpConstraintActivateBodies(constraint); -} - diff --git a/chipmunk/src/constraints/cpPinJoint.c b/chipmunk/src/constraints/cpPinJoint.c deleted file mode 100644 index abff6cef29..0000000000 --- a/chipmunk/src/constraints/cpPinJoint.c +++ /dev/null @@ -1,123 +0,0 @@ -/* Copyright (c) 2007 Scott Lembcke - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -#include "chipmunk_private.h" -#include "constraints/util.h" - -static void -preStep(cpPinJoint *joint, cpFloat dt) -{ - cpBody *a = joint->constraint.a; - cpBody *b = joint->constraint.b; - - joint->r1 = cpvrotate(joint->anchr1, a->rot); - joint->r2 = cpvrotate(joint->anchr2, b->rot); - - cpVect delta = cpvsub(cpvadd(b->p, joint->r2), cpvadd(a->p, joint->r1)); - cpFloat dist = cpvlength(delta); - joint->n = cpvmult(delta, 1.0f/(dist ? dist : (cpFloat)INFINITY)); - - // calculate mass normal - joint->nMass = 1.0f/k_scalar(a, b, joint->r1, joint->r2, joint->n); - - // calculate bias velocity - cpFloat maxBias = joint->constraint.maxBias; - joint->bias = cpfclamp(-bias_coef(joint->constraint.errorBias, dt)*(dist - joint->dist)/dt, -maxBias, maxBias); -} - -static void -applyCachedImpulse(cpPinJoint *joint, cpFloat dt_coef) -{ - cpBody *a = joint->constraint.a; - cpBody *b = joint->constraint.b; - - cpVect j = cpvmult(joint->n, joint->jnAcc*dt_coef); - apply_impulses(a, b, joint->r1, joint->r2, j); -} - -static void -applyImpulse(cpPinJoint *joint, cpFloat dt) -{ - cpBody *a = joint->constraint.a; - cpBody *b = joint->constraint.b; - cpVect n = joint->n; - - // compute relative velocity - cpFloat vrn = normal_relative_velocity(a, b, joint->r1, joint->r2, n); - - cpFloat jnMax = joint->constraint.maxForce*dt; - - // compute normal impulse - cpFloat jn = (joint->bias - vrn)*joint->nMass; - cpFloat jnOld = joint->jnAcc; - joint->jnAcc = cpfclamp(jnOld + jn, -jnMax, jnMax); - jn = joint->jnAcc - jnOld; - - // apply impulse - apply_impulses(a, b, joint->r1, joint->r2, cpvmult(n, jn)); -} - -static cpFloat -getImpulse(cpPinJoint *joint) -{ - return cpfabs(joint->jnAcc); -} - -static const cpConstraintClass klass = { - (cpConstraintPreStepImpl)preStep, - (cpConstraintApplyCachedImpulseImpl)applyCachedImpulse, - (cpConstraintApplyImpulseImpl)applyImpulse, - (cpConstraintGetImpulseImpl)getImpulse, -}; -CP_DefineClassGetter(cpPinJoint) - - -cpPinJoint * -cpPinJointAlloc(void) -{ - return (cpPinJoint *)cpcalloc(1, sizeof(cpPinJoint)); -} - -cpPinJoint * -cpPinJointInit(cpPinJoint *joint, cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2) -{ - cpConstraintInit((cpConstraint *)joint, &klass, a, b); - - joint->anchr1 = anchr1; - joint->anchr2 = anchr2; - - // STATIC_BODY_CHECK - cpVect p1 = (a ? cpvadd(a->p, cpvrotate(anchr1, a->rot)) : anchr1); - cpVect p2 = (b ? cpvadd(b->p, cpvrotate(anchr2, b->rot)) : anchr2); - joint->dist = cpvlength(cpvsub(p2, p1)); - - cpAssertWarn(joint->dist > 0.0, "You created a 0 length pin joint. A pivot joint will be much more stable."); - - joint->jnAcc = 0.0f; - - return joint; -} - -cpConstraint * -cpPinJointNew(cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2) -{ - return (cpConstraint *)cpPinJointInit(cpPinJointAlloc(), a, b, anchr1, anchr2); -} diff --git a/chipmunk/src/constraints/cpPivotJoint.c b/chipmunk/src/constraints/cpPivotJoint.c deleted file mode 100644 index 5a042bc05e..0000000000 --- a/chipmunk/src/constraints/cpPivotJoint.c +++ /dev/null @@ -1,118 +0,0 @@ -/* Copyright (c) 2007 Scott Lembcke - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -#include "chipmunk_private.h" -#include "constraints/util.h" - -static void -preStep(cpPivotJoint *joint, cpFloat dt) -{ - cpBody *a = joint->constraint.a; - cpBody *b = joint->constraint.b; - - joint->r1 = cpvrotate(joint->anchr1, a->rot); - joint->r2 = cpvrotate(joint->anchr2, b->rot); - - // Calculate mass tensor - joint-> k = k_tensor(a, b, joint->r1, joint->r2); - - // calculate bias velocity - cpVect delta = cpvsub(cpvadd(b->p, joint->r2), cpvadd(a->p, joint->r1)); - joint->bias = cpvclamp(cpvmult(delta, -bias_coef(joint->constraint.errorBias, dt)/dt), joint->constraint.maxBias); -} - -static void -applyCachedImpulse(cpPivotJoint *joint, cpFloat dt_coef) -{ - cpBody *a = joint->constraint.a; - cpBody *b = joint->constraint.b; - - apply_impulses(a, b, joint->r1, joint->r2, cpvmult(joint->jAcc, dt_coef)); -} - -static void -applyImpulse(cpPivotJoint *joint, cpFloat dt) -{ - cpBody *a = joint->constraint.a; - cpBody *b = joint->constraint.b; - - cpVect r1 = joint->r1; - cpVect r2 = joint->r2; - - // compute relative velocity - cpVect vr = relative_velocity(a, b, r1, r2); - - // compute normal impulse - cpVect j = cpMat2x2Transform(joint->k, cpvsub(joint->bias, vr)); - cpVect jOld = joint->jAcc; - joint->jAcc = cpvclamp(cpvadd(joint->jAcc, j), joint->constraint.maxForce*dt); - j = cpvsub(joint->jAcc, jOld); - - // apply impulse - apply_impulses(a, b, joint->r1, joint->r2, j); -} - -static cpFloat -getImpulse(cpConstraint *joint) -{ - return cpvlength(((cpPivotJoint *)joint)->jAcc); -} - -static const cpConstraintClass klass = { - (cpConstraintPreStepImpl)preStep, - (cpConstraintApplyCachedImpulseImpl)applyCachedImpulse, - (cpConstraintApplyImpulseImpl)applyImpulse, - (cpConstraintGetImpulseImpl)getImpulse, -}; -CP_DefineClassGetter(cpPivotJoint) - -cpPivotJoint * -cpPivotJointAlloc(void) -{ - return (cpPivotJoint *)cpcalloc(1, sizeof(cpPivotJoint)); -} - -cpPivotJoint * -cpPivotJointInit(cpPivotJoint *joint, cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2) -{ - cpConstraintInit((cpConstraint *)joint, &klass, a, b); - - joint->anchr1 = anchr1; - joint->anchr2 = anchr2; - - joint->jAcc = cpvzero; - - return joint; -} - -cpConstraint * -cpPivotJointNew2(cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2) -{ - return (cpConstraint *)cpPivotJointInit(cpPivotJointAlloc(), a, b, anchr1, anchr2); -} - -cpConstraint * -cpPivotJointNew(cpBody *a, cpBody *b, cpVect pivot) -{ - cpVect anchr1 = (a ? cpBodyWorld2Local(a, pivot) : pivot); - cpVect anchr2 = (b ? cpBodyWorld2Local(b, pivot) : pivot); - return cpPivotJointNew2(a, b, anchr1, anchr2); -} diff --git a/chipmunk/src/constraints/cpRatchetJoint.c b/chipmunk/src/constraints/cpRatchetJoint.c deleted file mode 100644 index 4e30ba5c82..0000000000 --- a/chipmunk/src/constraints/cpRatchetJoint.c +++ /dev/null @@ -1,131 +0,0 @@ -/* Copyright (c) 2007 Scott Lembcke - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -#include "chipmunk_private.h" -#include "constraints/util.h" - -static void -preStep(cpRatchetJoint *joint, cpFloat dt) -{ - cpBody *a = joint->constraint.a; - cpBody *b = joint->constraint.b; - - cpFloat angle = joint->angle; - cpFloat phase = joint->phase; - cpFloat ratchet = joint->ratchet; - - cpFloat delta = b->a - a->a; - cpFloat diff = angle - delta; - cpFloat pdist = 0.0f; - - if(diff*ratchet > 0.0f){ - pdist = diff; - } else { - joint->angle = cpffloor((delta - phase)/ratchet)*ratchet + phase; - } - - // calculate moment of inertia coefficient. - joint->iSum = 1.0f/(a->i_inv + b->i_inv); - - // calculate bias velocity - cpFloat maxBias = joint->constraint.maxBias; - joint->bias = cpfclamp(-bias_coef(joint->constraint.errorBias, dt)*pdist/dt, -maxBias, maxBias); - - // If the bias is 0, the joint is not at a limit. Reset the impulse. - if(!joint->bias) joint->jAcc = 0.0f; -} - -static void -applyCachedImpulse(cpRatchetJoint *joint, cpFloat dt_coef) -{ - cpBody *a = joint->constraint.a; - cpBody *b = joint->constraint.b; - - cpFloat j = joint->jAcc*dt_coef; - a->w -= j*a->i_inv; - b->w += j*b->i_inv; -} - -static void -applyImpulse(cpRatchetJoint *joint, cpFloat dt) -{ - if(!joint->bias) return; // early exit - - cpBody *a = joint->constraint.a; - cpBody *b = joint->constraint.b; - - // compute relative rotational velocity - cpFloat wr = b->w - a->w; - cpFloat ratchet = joint->ratchet; - - cpFloat jMax = joint->constraint.maxForce*dt; - - // compute normal impulse - cpFloat j = -(joint->bias + wr)*joint->iSum; - cpFloat jOld = joint->jAcc; - joint->jAcc = cpfclamp((jOld + j)*ratchet, 0.0f, jMax*cpfabs(ratchet))/ratchet; - j = joint->jAcc - jOld; - - // apply impulse - a->w -= j*a->i_inv; - b->w += j*b->i_inv; -} - -static cpFloat -getImpulse(cpRatchetJoint *joint) -{ - return cpfabs(joint->jAcc); -} - -static const cpConstraintClass klass = { - (cpConstraintPreStepImpl)preStep, - (cpConstraintApplyCachedImpulseImpl)applyCachedImpulse, - (cpConstraintApplyImpulseImpl)applyImpulse, - (cpConstraintGetImpulseImpl)getImpulse, -}; -CP_DefineClassGetter(cpRatchetJoint) - -cpRatchetJoint * -cpRatchetJointAlloc(void) -{ - return (cpRatchetJoint *)cpcalloc(1, sizeof(cpRatchetJoint)); -} - -cpRatchetJoint * -cpRatchetJointInit(cpRatchetJoint *joint, cpBody *a, cpBody *b, cpFloat phase, cpFloat ratchet) -{ - cpConstraintInit((cpConstraint *)joint, &klass, a, b); - - joint->angle = 0.0f; - joint->phase = phase; - joint->ratchet = ratchet; - - // STATIC_BODY_CHECK - joint->angle = (b ? b->a : 0.0f) - (a ? a->a : 0.0f); - - return joint; -} - -cpConstraint * -cpRatchetJointNew(cpBody *a, cpBody *b, cpFloat phase, cpFloat ratchet) -{ - return (cpConstraint *)cpRatchetJointInit(cpRatchetJointAlloc(), a, b, phase, ratchet); -} diff --git a/chipmunk/src/constraints/cpRotaryLimitJoint.c b/chipmunk/src/constraints/cpRotaryLimitJoint.c deleted file mode 100644 index a39c93fcd1..0000000000 --- a/chipmunk/src/constraints/cpRotaryLimitJoint.c +++ /dev/null @@ -1,126 +0,0 @@ -/* Copyright (c) 2007 Scott Lembcke - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -#include "chipmunk_private.h" -#include "constraints/util.h" - -static void -preStep(cpRotaryLimitJoint *joint, cpFloat dt) -{ - cpBody *a = joint->constraint.a; - cpBody *b = joint->constraint.b; - - cpFloat dist = b->a - a->a; - cpFloat pdist = 0.0f; - if(dist > joint->max) { - pdist = joint->max - dist; - } else if(dist < joint->min) { - pdist = joint->min - dist; - } - - // calculate moment of inertia coefficient. - joint->iSum = 1.0f/(a->i_inv + b->i_inv); - - // calculate bias velocity - cpFloat maxBias = joint->constraint.maxBias; - joint->bias = cpfclamp(-bias_coef(joint->constraint.errorBias, dt)*pdist/dt, -maxBias, maxBias); - - // If the bias is 0, the joint is not at a limit. Reset the impulse. - if(!joint->bias) joint->jAcc = 0.0f; -} - -static void -applyCachedImpulse(cpRotaryLimitJoint *joint, cpFloat dt_coef) -{ - cpBody *a = joint->constraint.a; - cpBody *b = joint->constraint.b; - - cpFloat j = joint->jAcc*dt_coef; - a->w -= j*a->i_inv; - b->w += j*b->i_inv; -} - -static void -applyImpulse(cpRotaryLimitJoint *joint, cpFloat dt) -{ - if(!joint->bias) return; // early exit - - cpBody *a = joint->constraint.a; - cpBody *b = joint->constraint.b; - - // compute relative rotational velocity - cpFloat wr = b->w - a->w; - - cpFloat jMax = joint->constraint.maxForce*dt; - - // compute normal impulse - cpFloat j = -(joint->bias + wr)*joint->iSum; - cpFloat jOld = joint->jAcc; - if(joint->bias < 0.0f){ - joint->jAcc = cpfclamp(jOld + j, 0.0f, jMax); - } else { - joint->jAcc = cpfclamp(jOld + j, -jMax, 0.0f); - } - j = joint->jAcc - jOld; - - // apply impulse - a->w -= j*a->i_inv; - b->w += j*b->i_inv; -} - -static cpFloat -getImpulse(cpRotaryLimitJoint *joint) -{ - return cpfabs(joint->jAcc); -} - -static const cpConstraintClass klass = { - (cpConstraintPreStepImpl)preStep, - (cpConstraintApplyCachedImpulseImpl)applyCachedImpulse, - (cpConstraintApplyImpulseImpl)applyImpulse, - (cpConstraintGetImpulseImpl)getImpulse, -}; -CP_DefineClassGetter(cpRotaryLimitJoint) - -cpRotaryLimitJoint * -cpRotaryLimitJointAlloc(void) -{ - return (cpRotaryLimitJoint *)cpcalloc(1, sizeof(cpRotaryLimitJoint)); -} - -cpRotaryLimitJoint * -cpRotaryLimitJointInit(cpRotaryLimitJoint *joint, cpBody *a, cpBody *b, cpFloat min, cpFloat max) -{ - cpConstraintInit((cpConstraint *)joint, &klass, a, b); - - joint->min = min; - joint->max = max; - - joint->jAcc = 0.0f; - - return joint; -} - -cpConstraint * -cpRotaryLimitJointNew(cpBody *a, cpBody *b, cpFloat min, cpFloat max) -{ - return (cpConstraint *)cpRotaryLimitJointInit(cpRotaryLimitJointAlloc(), a, b, min, max); -} diff --git a/chipmunk/src/constraints/cpSimpleMotor.c b/chipmunk/src/constraints/cpSimpleMotor.c deleted file mode 100644 index b7a023bd15..0000000000 --- a/chipmunk/src/constraints/cpSimpleMotor.c +++ /dev/null @@ -1,104 +0,0 @@ -/* Copyright (c) 2007 Scott Lembcke - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -#include "chipmunk_private.h" -#include "constraints/util.h" - -static void -preStep(cpSimpleMotor *joint, cpFloat dt) -{ - cpBody *a = joint->constraint.a; - cpBody *b = joint->constraint.b; - - // calculate moment of inertia coefficient. - joint->iSum = 1.0f/(a->i_inv + b->i_inv); -} - -static void -applyCachedImpulse(cpSimpleMotor *joint, cpFloat dt_coef) -{ - cpBody *a = joint->constraint.a; - cpBody *b = joint->constraint.b; - - cpFloat j = joint->jAcc*dt_coef; - a->w -= j*a->i_inv; - b->w += j*b->i_inv; -} - -static void -applyImpulse(cpSimpleMotor *joint, cpFloat dt) -{ - cpBody *a = joint->constraint.a; - cpBody *b = joint->constraint.b; - - // compute relative rotational velocity - cpFloat wr = b->w - a->w + joint->rate; - - cpFloat jMax = joint->constraint.maxForce*dt; - - // compute normal impulse - cpFloat j = -wr*joint->iSum; - cpFloat jOld = joint->jAcc; - joint->jAcc = cpfclamp(jOld + j, -jMax, jMax); - j = joint->jAcc - jOld; - - // apply impulse - a->w -= j*a->i_inv; - b->w += j*b->i_inv; -} - -static cpFloat -getImpulse(cpSimpleMotor *joint) -{ - return cpfabs(joint->jAcc); -} - -static const cpConstraintClass klass = { - (cpConstraintPreStepImpl)preStep, - (cpConstraintApplyCachedImpulseImpl)applyCachedImpulse, - (cpConstraintApplyImpulseImpl)applyImpulse, - (cpConstraintGetImpulseImpl)getImpulse, -}; -CP_DefineClassGetter(cpSimpleMotor) - -cpSimpleMotor * -cpSimpleMotorAlloc(void) -{ - return (cpSimpleMotor *)cpcalloc(1, sizeof(cpSimpleMotor)); -} - -cpSimpleMotor * -cpSimpleMotorInit(cpSimpleMotor *joint, cpBody *a, cpBody *b, cpFloat rate) -{ - cpConstraintInit((cpConstraint *)joint, &klass, a, b); - - joint->rate = rate; - - joint->jAcc = 0.0f; - - return joint; -} - -cpConstraint * -cpSimpleMotorNew(cpBody *a, cpBody *b, cpFloat rate) -{ - return (cpConstraint *)cpSimpleMotorInit(cpSimpleMotorAlloc(), a, b, rate); -} diff --git a/chipmunk/src/constraints/cpSlideJoint.c b/chipmunk/src/constraints/cpSlideJoint.c deleted file mode 100644 index 87e691d188..0000000000 --- a/chipmunk/src/constraints/cpSlideJoint.c +++ /dev/null @@ -1,131 +0,0 @@ -/* Copyright (c) 2007 Scott Lembcke - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -#include "chipmunk_private.h" -#include "constraints/util.h" - -static void -preStep(cpSlideJoint *joint, cpFloat dt) -{ - cpBody *a = joint->constraint.a; - cpBody *b = joint->constraint.b; - - joint->r1 = cpvrotate(joint->anchr1, a->rot); - joint->r2 = cpvrotate(joint->anchr2, b->rot); - - cpVect delta = cpvsub(cpvadd(b->p, joint->r2), cpvadd(a->p, joint->r1)); - cpFloat dist = cpvlength(delta); - cpFloat pdist = 0.0f; - if(dist > joint->max) { - pdist = dist - joint->max; - joint->n = cpvnormalize_safe(delta); - } else if(dist < joint->min) { - pdist = joint->min - dist; - joint->n = cpvneg(cpvnormalize_safe(delta)); - } else { - joint->n = cpvzero; - joint->jnAcc = 0.0f; - } - - // calculate mass normal - joint->nMass = 1.0f/k_scalar(a, b, joint->r1, joint->r2, joint->n); - - // calculate bias velocity - cpFloat maxBias = joint->constraint.maxBias; - joint->bias = cpfclamp(-bias_coef(joint->constraint.errorBias, dt)*pdist/dt, -maxBias, maxBias); -} - -static void -applyCachedImpulse(cpSlideJoint *joint, cpFloat dt_coef) -{ - cpBody *a = joint->constraint.a; - cpBody *b = joint->constraint.b; - - cpVect j = cpvmult(joint->n, joint->jnAcc*dt_coef); - apply_impulses(a, b, joint->r1, joint->r2, j); -} - -static void -applyImpulse(cpSlideJoint *joint, cpFloat dt) -{ - if(cpveql(joint->n, cpvzero)) return; // early exit - - cpBody *a = joint->constraint.a; - cpBody *b = joint->constraint.b; - - cpVect n = joint->n; - cpVect r1 = joint->r1; - cpVect r2 = joint->r2; - - // compute relative velocity - cpVect vr = relative_velocity(a, b, r1, r2); - cpFloat vrn = cpvdot(vr, n); - - // compute normal impulse - cpFloat jn = (joint->bias - vrn)*joint->nMass; - cpFloat jnOld = joint->jnAcc; - joint->jnAcc = cpfclamp(jnOld + jn, -joint->constraint.maxForce*dt, 0.0f); - jn = joint->jnAcc - jnOld; - - // apply impulse - apply_impulses(a, b, joint->r1, joint->r2, cpvmult(n, jn)); -} - -static cpFloat -getImpulse(cpConstraint *joint) -{ - return cpfabs(((cpSlideJoint *)joint)->jnAcc); -} - -static const cpConstraintClass klass = { - (cpConstraintPreStepImpl)preStep, - (cpConstraintApplyCachedImpulseImpl)applyCachedImpulse, - (cpConstraintApplyImpulseImpl)applyImpulse, - (cpConstraintGetImpulseImpl)getImpulse, -}; -CP_DefineClassGetter(cpSlideJoint) - -cpSlideJoint * -cpSlideJointAlloc(void) -{ - return (cpSlideJoint *)cpcalloc(1, sizeof(cpSlideJoint)); -} - -cpSlideJoint * -cpSlideJointInit(cpSlideJoint *joint, cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2, cpFloat min, cpFloat max) -{ - cpConstraintInit((cpConstraint *)joint, &klass, a, b); - - joint->anchr1 = anchr1; - joint->anchr2 = anchr2; - joint->min = min; - joint->max = max; - - joint->jnAcc = 0.0f; - - return joint; -} - -cpConstraint * -cpSlideJointNew(cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2, cpFloat min, cpFloat max) -{ - return (cpConstraint *)cpSlideJointInit(cpSlideJointAlloc(), a, b, anchr1, anchr2, min, max); -} diff --git a/chipmunk/src/cpArbiter.c b/chipmunk/src/cpArbiter.c deleted file mode 100644 index ef6f09809c..0000000000 --- a/chipmunk/src/cpArbiter.c +++ /dev/null @@ -1,353 +0,0 @@ -/* Copyright (c) 2007 Scott Lembcke - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -#include "chipmunk_private.h" -#include "constraints/util.h" - -cpContact* -cpContactInit(cpContact *con, cpVect p, cpVect n, cpFloat dist, cpHashValue hash) -{ - con->p = p; - con->n = n; - con->dist = dist; - - con->jnAcc = 0.0f; - con->jtAcc = 0.0f; - con->jBias = 0.0f; - - con->hash = hash; - - return con; -} - -// TODO make this generic so I can reuse it for constraints also. -static inline void -unthreadHelper(cpArbiter *arb, cpBody *body) -{ - struct cpArbiterThread *thread = cpArbiterThreadForBody(arb, body); - cpArbiter *prev = thread->prev; - cpArbiter *next = thread->next; - - if(prev){ - cpArbiterThreadForBody(prev, body)->next = next; - } else if(body->arbiterList == arb) { - // IFF prev is NULL and body->arbiterList == arb, is arb at the head of the list. - // This function may be called for an arbiter that was never in a list. - // In that case, we need to protect it from wiping out the body->arbiterList pointer. - body->arbiterList = next; - } - - if(next) cpArbiterThreadForBody(next, body)->prev = prev; - - thread->prev = NULL; - thread->next = NULL; -} - -void -cpArbiterUnthread(cpArbiter *arb) -{ - unthreadHelper(arb, arb->body_a); - unthreadHelper(arb, arb->body_b); -} - -cpBool cpArbiterIsFirstContact(const cpArbiter *arb) -{ - return arb->CP_PRIVATE(state) == cpArbiterStateFirstColl; -} - -int cpArbiterGetCount(const cpArbiter *arb) -{ - // Return 0 contacts if we are in a separate callback. - return (arb->CP_PRIVATE(state) != cpArbiterStateCached ? arb->CP_PRIVATE(numContacts) : 0); -} - -cpVect -cpArbiterGetNormal(const cpArbiter *arb, int i) -{ - cpAssertHard(0 <= i && i < cpArbiterGetCount(arb), "Index error: The specified contact index is invalid for this arbiter"); - - cpVect n = arb->contacts[i].n; - return arb->swappedColl ? cpvneg(n) : n; -} - -cpVect -cpArbiterGetPoint(const cpArbiter *arb, int i) -{ - cpAssertHard(0 <= i && i < cpArbiterGetCount(arb), "Index error: The specified contact index is invalid for this arbiter"); - - return arb->CP_PRIVATE(contacts)[i].CP_PRIVATE(p); -} - -cpFloat -cpArbiterGetDepth(const cpArbiter *arb, int i) -{ - cpAssertHard(0 <= i && i < cpArbiterGetCount(arb), "Index error: The specified contact index is invalid for this arbiter"); - - return arb->CP_PRIVATE(contacts)[i].CP_PRIVATE(dist); -} - -cpContactPointSet -cpArbiterGetContactPointSet(const cpArbiter *arb) -{ - cpContactPointSet set; - set.count = cpArbiterGetCount(arb); - - for(int i=0; iCP_PRIVATE(contacts)[i].CP_PRIVATE(p); - set.points[i].normal = arb->CP_PRIVATE(contacts)[i].CP_PRIVATE(n); - set.points[i].dist = arb->CP_PRIVATE(contacts)[i].CP_PRIVATE(dist); - } - - return set; -} - -void -cpArbiterSetContactPointSet(cpArbiter *arb, cpContactPointSet *set) -{ - int count = set->count; - cpAssertHard(count == arb->numContacts, "The number of contact points cannot be changed."); - - for(int i=0; icontacts[i].p = set->points[i].point; - arb->contacts[i].n = set->points[i].normal; - arb->contacts[i].dist = set->points[i].dist; - } -} - -cpVect -cpArbiterTotalImpulse(const cpArbiter *arb) -{ - cpContact *contacts = arb->contacts; - cpVect sum = cpvzero; - - for(int i=0, count=cpArbiterGetCount(arb); in, con->jnAcc)); - } - - return (arb->swappedColl ? sum : cpvneg(sum)); -} - -cpVect -cpArbiterTotalImpulseWithFriction(const cpArbiter *arb) -{ - cpContact *contacts = arb->contacts; - cpVect sum = cpvzero; - - for(int i=0, count=cpArbiterGetCount(arb); in, cpv(con->jnAcc, con->jtAcc))); - } - - return (arb->swappedColl ? sum : cpvneg(sum)); -} - -cpFloat -cpArbiterTotalKE(const cpArbiter *arb) -{ - cpFloat eCoef = (1 - arb->e)/(1 + arb->e); - cpFloat sum = 0.0; - - cpContact *contacts = arb->contacts; - for(int i=0, count=cpArbiterGetCount(arb); ijnAcc; - cpFloat jtAcc = con->jtAcc; - - sum += eCoef*jnAcc*jnAcc/con->nMass + jtAcc*jtAcc/con->tMass; - } - - return sum; -} - -void -cpArbiterIgnore(cpArbiter *arb) -{ - arb->state = cpArbiterStateIgnore; -} - -cpVect -cpArbiterGetSurfaceVelocity(cpArbiter *arb) -{ - return cpvmult(arb->surface_vr, arb->swappedColl ? -1.0f : 1.0); -} - -void -cpArbiterSetSurfaceVelocity(cpArbiter *arb, cpVect vr) -{ - arb->surface_vr = cpvmult(vr, arb->swappedColl ? -1.0f : 1.0); -} - - -cpArbiter* -cpArbiterInit(cpArbiter *arb, cpShape *a, cpShape *b) -{ - arb->handler = NULL; - arb->swappedColl = cpFalse; - - arb->e = 0.0f; - arb->u = 0.0f; - arb->surface_vr = cpvzero; - - arb->numContacts = 0; - arb->contacts = NULL; - - arb->a = a; arb->body_a = a->body; - arb->b = b; arb->body_b = b->body; - - arb->thread_a.next = NULL; - arb->thread_b.next = NULL; - arb->thread_a.prev = NULL; - arb->thread_b.prev = NULL; - - arb->stamp = 0; - arb->state = cpArbiterStateFirstColl; - - arb->data = NULL; - - return arb; -} - -void -cpArbiterUpdate(cpArbiter *arb, cpContact *contacts, int numContacts, cpCollisionHandler *handler, cpShape *a, cpShape *b) -{ - // Iterate over the possible pairs to look for hash value matches. - for(int i=0; inumContacts; j++){ - cpContact *old = &arb->contacts[j]; - - // This could trigger false positives, but is fairly unlikely nor serious if it does. - if(con->hash == old->hash){ - // Copy the persistant contact information. - con->jnAcc = old->jnAcc; - con->jtAcc = old->jtAcc; - } - } - } - - arb->contacts = contacts; - arb->numContacts = numContacts; - - arb->handler = handler; - arb->swappedColl = (a->collision_type != handler->a); - - arb->e = a->e * b->e; - arb->u = a->u * b->u; - - // Currently all contacts will have the same normal. - // This may change in the future. - cpVect n = (numContacts ? contacts[0].n : cpvzero); - cpVect surface_vr = cpvsub(a->surface_v, b->surface_v); - arb->surface_vr = cpvsub(surface_vr, cpvmult(n, cpvdot(surface_vr, n))); - - // For collisions between two similar primitive types, the order could have been swapped. - arb->a = a; arb->body_a = a->body; - arb->b = b; arb->body_b = b->body; - - // mark it as new if it's been cached - if(arb->state == cpArbiterStateCached) arb->state = cpArbiterStateFirstColl; -} - -void -cpArbiterPreStep(cpArbiter *arb, cpFloat dt, cpFloat slop, cpFloat bias) -{ - cpBody *a = arb->body_a; - cpBody *b = arb->body_b; - - for(int i=0; inumContacts; i++){ - cpContact *con = &arb->contacts[i]; - - // Calculate the offsets. - con->r1 = cpvsub(con->p, a->p); - con->r2 = cpvsub(con->p, b->p); - - // Calculate the mass normal and mass tangent. - con->nMass = 1.0f/k_scalar(a, b, con->r1, con->r2, con->n); - con->tMass = 1.0f/k_scalar(a, b, con->r1, con->r2, cpvperp(con->n)); - - // Calculate the target bias velocity. - con->bias = -bias*cpfmin(0.0f, con->dist + slop)/dt; - con->jBias = 0.0f; - - // Calculate the target bounce velocity. - con->bounce = normal_relative_velocity(a, b, con->r1, con->r2, con->n)*arb->e; - } -} - -void -cpArbiterApplyCachedImpulse(cpArbiter *arb, cpFloat dt_coef) -{ - if(cpArbiterIsFirstContact(arb)) return; - - cpBody *a = arb->body_a; - cpBody *b = arb->body_b; - - for(int i=0; inumContacts; i++){ - cpContact *con = &arb->contacts[i]; - cpVect j = cpvrotate(con->n, cpv(con->jnAcc, con->jtAcc)); - apply_impulses(a, b, con->r1, con->r2, cpvmult(j, dt_coef)); - } -} - -// TODO is it worth splitting velocity/position correction? - -void -cpArbiterApplyImpulse(cpArbiter *arb) -{ - cpBody *a = arb->body_a; - cpBody *b = arb->body_b; - cpVect surface_vr = arb->surface_vr; - cpFloat friction = arb->u; - - for(int i=0; inumContacts; i++){ - cpContact *con = &arb->contacts[i]; - cpFloat nMass = con->nMass; - cpVect n = con->n; - cpVect r1 = con->r1; - cpVect r2 = con->r2; - - cpVect vb1 = cpvadd(a->v_bias, cpvmult(cpvperp(r1), a->w_bias)); - cpVect vb2 = cpvadd(b->v_bias, cpvmult(cpvperp(r2), b->w_bias)); - cpVect vr = cpvadd(relative_velocity(a, b, r1, r2), surface_vr); - - cpFloat vbn = cpvdot(cpvsub(vb2, vb1), n); - cpFloat vrn = cpvdot(vr, n); - cpFloat vrt = cpvdot(vr, cpvperp(n)); - - cpFloat jbn = (con->bias - vbn)*nMass; - cpFloat jbnOld = con->jBias; - con->jBias = cpfmax(jbnOld + jbn, 0.0f); - - cpFloat jn = -(con->bounce + vrn)*nMass; - cpFloat jnOld = con->jnAcc; - con->jnAcc = cpfmax(jnOld + jn, 0.0f); - - cpFloat jtMax = friction*con->jnAcc; - cpFloat jt = -vrt*con->tMass; - cpFloat jtOld = con->jtAcc; - con->jtAcc = cpfclamp(jtOld + jt, -jtMax, jtMax); - - apply_bias_impulses(a, b, r1, r2, cpvmult(n, con->jBias - jbnOld)); - apply_impulses(a, b, r1, r2, cpvrotate(n, cpv(con->jnAcc - jnOld, con->jtAcc - jtOld))); - } -} diff --git a/chipmunk/src/cpArray.c b/chipmunk/src/cpArray.c deleted file mode 100644 index 4d46b422cd..0000000000 --- a/chipmunk/src/cpArray.c +++ /dev/null @@ -1,124 +0,0 @@ -/* Copyright (c) 2007 Scott Lembcke - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -#include - -#include "chipmunk_private.h" - - -cpArray * -cpArrayNew(int size) -{ - cpArray *arr = (cpArray *)cpcalloc(1, sizeof(cpArray)); - - arr->num = 0; - arr->max = (size ? size : 4); - arr->arr = (void **)cpcalloc(arr->max, sizeof(void*)); - - return arr; -} - -void -cpArrayFree(cpArray *arr) -{ - if(arr){ - cpfree(arr->arr); - arr->arr = NULL; - - cpfree(arr); - } -} - -void -cpArrayPush(cpArray *arr, void *object) -{ - if(arr->num == arr->max){ - arr->max *= 2; - arr->arr = (void **)cprealloc(arr->arr, arr->max*sizeof(void*)); - } - - arr->arr[arr->num] = object; - arr->num++; -} - -void * -cpArrayPop(cpArray *arr) -{ - arr->num--; - - void *value = arr->arr[arr->num]; - arr->arr[arr->num] = NULL; - - return value; -} - -//static void -//cpArrayDeleteIndex(cpArray *arr, int idx) -//{ -// arr->num--; -// -// arr->arr[idx] = arr->arr[arr->num]; -// arr->arr[arr->num] = NULL; -//} - -void -cpArrayDeleteObj(cpArray *arr, void *obj) -{ - for(int i=0; inum; i++){ - if(arr->arr[i] == obj){ - arr->num--; - - arr->arr[i] = arr->arr[arr->num]; - arr->arr[arr->num] = NULL; - - return; - } - } -} - -//void -//cpArrayAppend(cpArray *arr, cpArray *other) -//{ -// void *tail = &arr->arr[arr->num]; -// -// arr->num += other->num; -// if(arr->num >= arr->max){ -// arr->max = arr->num; -// arr->arr = (void **)cprealloc(arr->arr, arr->max*sizeof(void**)); -// } -// -// memcpy(tail, other->arr, other->num*sizeof(void**)); -//} - -void -cpArrayFreeEach(cpArray *arr, void (freeFunc)(void*)) -{ - for(int i=0; inum; i++) freeFunc(arr->arr[i]); -} - -cpBool -cpArrayContains(cpArray *arr, void *ptr) -{ - for(int i=0; inum; i++) - if(arr->arr[i] == ptr) return cpTrue; - - return cpFalse; -} diff --git a/chipmunk/src/cpBB.c b/chipmunk/src/cpBB.c deleted file mode 100644 index 5a8d20a6a9..0000000000 --- a/chipmunk/src/cpBB.c +++ /dev/null @@ -1,36 +0,0 @@ -/* Copyright (c) 2007 Scott Lembcke - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -#include "chipmunk_private.h" - -cpVect -cpBBWrapVect(const cpBB bb, const cpVect v) -{ - cpFloat ix = cpfabs(bb.r - bb.l); - cpFloat modx = cpfmod(v.x - bb.l, ix); - cpFloat x = (modx > 0.0f) ? modx : modx + ix; - - cpFloat iy = cpfabs(bb.t - bb.b); - cpFloat mody = cpfmod(v.y - bb.b, iy); - cpFloat y = (mody > 0.0f) ? mody : mody + iy; - - return cpv(x + bb.l, y + bb.b); -} diff --git a/chipmunk/src/cpBBTree.c b/chipmunk/src/cpBBTree.c deleted file mode 100644 index 6d347e4d60..0000000000 --- a/chipmunk/src/cpBBTree.c +++ /dev/null @@ -1,894 +0,0 @@ -/* Copyright (c) 2009 Scott Lembcke - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -#include "stdlib.h" -#include "stdio.h" - -#include "chipmunk_private.h" - -static inline cpSpatialIndexClass *Klass(); - -typedef struct Node Node; -typedef struct Pair Pair; - -struct cpBBTree { - cpSpatialIndex spatialIndex; - cpBBTreeVelocityFunc velocityFunc; - - cpHashSet *leaves; - Node *root; - - Node *pooledNodes; - Pair *pooledPairs; - cpArray *allocatedBuffers; - - cpTimestamp stamp; -}; - -struct Node { - void *obj; - cpBB bb; - Node *parent; - - union { - // Internal nodes - struct { Node *a, *b; } children; - - // Leaves - struct { - cpTimestamp stamp; - Pair *pairs; - } leaf; - } node; -}; - -// Can't use anonymous unions and still get good x-compiler compatability -#define A node.children.a -#define B node.children.b -#define STAMP node.leaf.stamp -#define PAIRS node.leaf.pairs - -typedef struct Thread { - Pair *prev; - Node *leaf; - Pair *next; -} Thread; - -struct Pair { - Thread a, b; - cpCollisionID id; -}; - -//MARK: Misc Functions - -static inline cpBB -GetBB(cpBBTree *tree, void *obj) -{ - cpBB bb = tree->spatialIndex.bbfunc(obj); - - cpBBTreeVelocityFunc velocityFunc = tree->velocityFunc; - if(velocityFunc){ - cpFloat coef = 0.1f; - cpFloat x = (bb.r - bb.l)*coef; - cpFloat y = (bb.t - bb.b)*coef; - - cpVect v = cpvmult(velocityFunc(obj), 0.1f); - return cpBBNew(bb.l + cpfmin(-x, v.x), bb.b + cpfmin(-y, v.y), bb.r + cpfmax(x, v.x), bb.t + cpfmax(y, v.y)); - } else { - return bb; - } -} - -static inline cpBBTree * -GetTree(cpSpatialIndex *index) -{ - return (index && index->klass == Klass() ? (cpBBTree *)index : NULL); -} - -static inline Node * -GetRootIfTree(cpSpatialIndex *index){ - return (index && index->klass == Klass() ? ((cpBBTree *)index)->root : NULL); -} - -static inline cpBBTree * -GetMasterTree(cpBBTree *tree) -{ - cpBBTree *dynamicTree = GetTree(tree->spatialIndex.dynamicIndex); - return (dynamicTree ? dynamicTree : tree); -} - -static inline void -IncrementStamp(cpBBTree *tree) -{ - cpBBTree *dynamicTree = GetTree(tree->spatialIndex.dynamicIndex); - if(dynamicTree){ - dynamicTree->stamp++; - } else { - tree->stamp++; - } -} - -//MARK: Pair/Thread Functions - -static void -PairRecycle(cpBBTree *tree, Pair *pair) -{ - // Share the pool of the master tree. - // TODO would be lovely to move the pairs stuff into an external data structure. - tree = GetMasterTree(tree); - - pair->a.next = tree->pooledPairs; - tree->pooledPairs = pair; -} - -static Pair * -PairFromPool(cpBBTree *tree) -{ - // Share the pool of the master tree. - // TODO would be lovely to move the pairs stuff into an external data structure. - tree = GetMasterTree(tree); - - Pair *pair = tree->pooledPairs; - - if(pair){ - tree->pooledPairs = pair->a.next; - return pair; - } else { - // Pool is exhausted, make more - int count = CP_BUFFER_BYTES/sizeof(Pair); - cpAssertHard(count, "Internal Error: Buffer size is too small."); - - Pair *buffer = (Pair *)cpcalloc(1, CP_BUFFER_BYTES); - cpArrayPush(tree->allocatedBuffers, buffer); - - // push all but the first one, return the first instead - for(int i=1; ia.leaf == thread.leaf) next->a.prev = prev; else next->b.prev = prev; - } - - if(prev){ - if(prev->a.leaf == thread.leaf) prev->a.next = next; else prev->b.next = next; - } else { - thread.leaf->PAIRS = next; - } -} - -static void -PairsClear(Node *leaf, cpBBTree *tree) -{ - Pair *pair = leaf->PAIRS; - leaf->PAIRS = NULL; - - while(pair){ - if(pair->a.leaf == leaf){ - Pair *next = pair->a.next; - ThreadUnlink(pair->b); - PairRecycle(tree, pair); - pair = next; - } else { - Pair *next = pair->b.next; - ThreadUnlink(pair->a); - PairRecycle(tree, pair); - pair = next; - } - } -} - -static void -PairInsert(Node *a, Node *b, cpBBTree *tree) -{ - Pair *nextA = a->PAIRS, *nextB = b->PAIRS; - Pair *pair = PairFromPool(tree); - Pair temp = {{NULL, a, nextA},{NULL, b, nextB}, 0}; - - a->PAIRS = b->PAIRS = pair; - *pair = temp; - - if(nextA){ - if(nextA->a.leaf == a) nextA->a.prev = pair; else nextA->b.prev = pair; - } - - if(nextB){ - if(nextB->a.leaf == b) nextB->a.prev = pair; else nextB->b.prev = pair; - } -} - - -//MARK: Node Functions - -static void -NodeRecycle(cpBBTree *tree, Node *node) -{ - node->parent = tree->pooledNodes; - tree->pooledNodes = node; -} - -static Node * -NodeFromPool(cpBBTree *tree) -{ - Node *node = tree->pooledNodes; - - if(node){ - tree->pooledNodes = node->parent; - return node; - } else { - // Pool is exhausted, make more - int count = CP_BUFFER_BYTES/sizeof(Node); - cpAssertHard(count, "Internal Error: Buffer size is too small."); - - Node *buffer = (Node *)cpcalloc(1, CP_BUFFER_BYTES); - cpArrayPush(tree->allocatedBuffers, buffer); - - // push all but the first one, return the first instead - for(int i=1; iA = value; - value->parent = node; -} - -static inline void -NodeSetB(Node *node, Node *value) -{ - node->B = value; - value->parent = node; -} - -static Node * -NodeNew(cpBBTree *tree, Node *a, Node *b) -{ - Node *node = NodeFromPool(tree); - - node->obj = NULL; - node->bb = cpBBMerge(a->bb, b->bb); - node->parent = NULL; - - NodeSetA(node, a); - NodeSetB(node, b); - - return node; -} - -static inline cpBool -NodeIsLeaf(Node *node) -{ - return (node->obj != NULL); -} - -static inline Node * -NodeOther(Node *node, Node *child) -{ - return (node->A == child ? node->B : node->A); -} - -static inline void -NodeReplaceChild(Node *parent, Node *child, Node *value, cpBBTree *tree) -{ - cpAssertSoft(!NodeIsLeaf(parent), "Internal Error: Cannot replace child of a leaf."); - cpAssertSoft(child == parent->A || child == parent->B, "Internal Error: Node is not a child of parent."); - - if(parent->A == child){ - NodeRecycle(tree, parent->A); - NodeSetA(parent, value); - } else { - NodeRecycle(tree, parent->B); - NodeSetB(parent, value); - } - - for(Node *node=parent; node; node = node->parent){ - node->bb = cpBBMerge(node->A->bb, node->B->bb); - } -} - -//MARK: Subtree Functions - -static inline cpFloat -cpBBProximity(cpBB a, cpBB b) -{ - return cpfabs(a.l + a.r - b.l - b.r) + cpfabs(a.b + a.t - b.b - b.t); -} - -static Node * -SubtreeInsert(Node *subtree, Node *leaf, cpBBTree *tree) -{ - if(subtree == NULL){ - return leaf; - } else if(NodeIsLeaf(subtree)){ - return NodeNew(tree, leaf, subtree); - } else { - cpFloat cost_a = cpBBArea(subtree->B->bb) + cpBBMergedArea(subtree->A->bb, leaf->bb); - cpFloat cost_b = cpBBArea(subtree->A->bb) + cpBBMergedArea(subtree->B->bb, leaf->bb); - - if(cost_a == cost_b){ - cost_a = cpBBProximity(subtree->A->bb, leaf->bb); - cost_b = cpBBProximity(subtree->B->bb, leaf->bb); - } - - if(cost_b < cost_a){ - NodeSetB(subtree, SubtreeInsert(subtree->B, leaf, tree)); - } else { - NodeSetA(subtree, SubtreeInsert(subtree->A, leaf, tree)); - } - - subtree->bb = cpBBMerge(subtree->bb, leaf->bb); - return subtree; - } -} - -static void -SubtreeQuery(Node *subtree, void *obj, cpBB bb, cpSpatialIndexQueryFunc func, void *data) -{ - if(cpBBIntersects(subtree->bb, bb)){ - if(NodeIsLeaf(subtree)){ - func(obj, subtree->obj, 0, data); - } else { - SubtreeQuery(subtree->A, obj, bb, func, data); - SubtreeQuery(subtree->B, obj, bb, func, data); - } - } -} - - -static cpFloat -SubtreeSegmentQuery(Node *subtree, void *obj, cpVect a, cpVect b, cpFloat t_exit, cpSpatialIndexSegmentQueryFunc func, void *data) -{ - if(NodeIsLeaf(subtree)){ - return func(obj, subtree->obj, data); - } else { - cpFloat t_a = cpBBSegmentQuery(subtree->A->bb, a, b); - cpFloat t_b = cpBBSegmentQuery(subtree->B->bb, a, b); - - if(t_a < t_b){ - if(t_a < t_exit) t_exit = cpfmin(t_exit, SubtreeSegmentQuery(subtree->A, obj, a, b, t_exit, func, data)); - if(t_b < t_exit) t_exit = cpfmin(t_exit, SubtreeSegmentQuery(subtree->B, obj, a, b, t_exit, func, data)); - } else { - if(t_b < t_exit) t_exit = cpfmin(t_exit, SubtreeSegmentQuery(subtree->B, obj, a, b, t_exit, func, data)); - if(t_a < t_exit) t_exit = cpfmin(t_exit, SubtreeSegmentQuery(subtree->A, obj, a, b, t_exit, func, data)); - } - - return t_exit; - } -} - -static void -SubtreeRecycle(cpBBTree *tree, Node *node) -{ - if(!NodeIsLeaf(node)){ - SubtreeRecycle(tree, node->A); - SubtreeRecycle(tree, node->B); - NodeRecycle(tree, node); - } -} - -static inline Node * -SubtreeRemove(Node *subtree, Node *leaf, cpBBTree *tree) -{ - if(leaf == subtree){ - return NULL; - } else { - Node *parent = leaf->parent; - if(parent == subtree){ - Node *other = NodeOther(subtree, leaf); - other->parent = subtree->parent; - NodeRecycle(tree, subtree); - return other; - } else { - NodeReplaceChild(parent->parent, parent, NodeOther(parent, leaf), tree); - return subtree; - } - } -} - -//MARK: Marking Functions - -typedef struct MarkContext { - cpBBTree *tree; - Node *staticRoot; - cpSpatialIndexQueryFunc func; - void *data; -} MarkContext; - -static void -MarkLeafQuery(Node *subtree, Node *leaf, cpBool left, MarkContext *context) -{ - if(cpBBIntersects(leaf->bb, subtree->bb)){ - if(NodeIsLeaf(subtree)){ - if(left){ - PairInsert(leaf, subtree, context->tree); - } else { - if(subtree->STAMP < leaf->STAMP) PairInsert(subtree, leaf, context->tree); - context->func(leaf->obj, subtree->obj, 0, context->data); - } - } else { - MarkLeafQuery(subtree->A, leaf, left, context); - MarkLeafQuery(subtree->B, leaf, left, context); - } - } -} - -static void -MarkLeaf(Node *leaf, MarkContext *context) -{ - cpBBTree *tree = context->tree; - if(leaf->STAMP == GetMasterTree(tree)->stamp){ - Node *staticRoot = context->staticRoot; - if(staticRoot) MarkLeafQuery(staticRoot, leaf, cpFalse, context); - - for(Node *node = leaf; node->parent; node = node->parent){ - if(node == node->parent->A){ - MarkLeafQuery(node->parent->B, leaf, cpTrue, context); - } else { - MarkLeafQuery(node->parent->A, leaf, cpFalse, context); - } - } - } else { - Pair *pair = leaf->PAIRS; - while(pair){ - if(leaf == pair->b.leaf){ - pair->id = context->func(pair->a.leaf->obj, leaf->obj, pair->id, context->data); - pair = pair->b.next; - } else { - pair = pair->a.next; - } - } - } -} - -static void -MarkSubtree(Node *subtree, MarkContext *context) -{ - if(NodeIsLeaf(subtree)){ - MarkLeaf(subtree, context); - } else { - MarkSubtree(subtree->A, context); - MarkSubtree(subtree->B, context); // TODO Force TCO here? - } -} - -//MARK: Leaf Functions - -static Node * -LeafNew(cpBBTree *tree, void *obj, cpBB bb) -{ - Node *node = NodeFromPool(tree); - node->obj = obj; - node->bb = GetBB(tree, obj); - - node->parent = NULL; - node->STAMP = 0; - node->PAIRS = NULL; - - return node; -} - -static cpBool -LeafUpdate(Node *leaf, cpBBTree *tree) -{ - Node *root = tree->root; - cpBB bb = tree->spatialIndex.bbfunc(leaf->obj); - - if(!cpBBContainsBB(leaf->bb, bb)){ - leaf->bb = GetBB(tree, leaf->obj); - - root = SubtreeRemove(root, leaf, tree); - tree->root = SubtreeInsert(root, leaf, tree); - - PairsClear(leaf, tree); - leaf->STAMP = GetMasterTree(tree)->stamp; - - return cpTrue; - } else { - return cpFalse; - } -} - -static cpCollisionID VoidQueryFunc(void *obj1, void *obj2, cpCollisionID id, void *data){return id;} - -static void -LeafAddPairs(Node *leaf, cpBBTree *tree) -{ - cpSpatialIndex *dynamicIndex = tree->spatialIndex.dynamicIndex; - if(dynamicIndex){ - Node *dynamicRoot = GetRootIfTree(dynamicIndex); - if(dynamicRoot){ - cpBBTree *dynamicTree = GetTree(dynamicIndex); - MarkContext context = {dynamicTree, NULL, NULL, NULL}; - MarkLeafQuery(dynamicRoot, leaf, cpTrue, &context); - } - } else { - Node *staticRoot = GetRootIfTree(tree->spatialIndex.staticIndex); - MarkContext context = {tree, staticRoot, VoidQueryFunc, NULL}; - MarkLeaf(leaf, &context); - } -} - -//MARK: Memory Management Functions - -cpBBTree * -cpBBTreeAlloc(void) -{ - return (cpBBTree *)cpcalloc(1, sizeof(cpBBTree)); -} - -static int -leafSetEql(void *obj, Node *node) -{ - return (obj == node->obj); -} - -static void * -leafSetTrans(void *obj, cpBBTree *tree) -{ - return LeafNew(tree, obj, tree->spatialIndex.bbfunc(obj)); -} - -cpSpatialIndex * -cpBBTreeInit(cpBBTree *tree, cpSpatialIndexBBFunc bbfunc, cpSpatialIndex *staticIndex) -{ - cpSpatialIndexInit((cpSpatialIndex *)tree, Klass(), bbfunc, staticIndex); - - tree->velocityFunc = NULL; - - tree->leaves = cpHashSetNew(0, (cpHashSetEqlFunc)leafSetEql); - tree->root = NULL; - - tree->pooledNodes = NULL; - tree->allocatedBuffers = cpArrayNew(0); - - tree->stamp = 0; - - return (cpSpatialIndex *)tree; -} - -void -cpBBTreeSetVelocityFunc(cpSpatialIndex *index, cpBBTreeVelocityFunc func) -{ - if(index->klass != Klass()){ - cpAssertWarn(cpFalse, "Ignoring cpBBTreeSetVelocityFunc() call to non-tree spatial index."); - return; - } - - ((cpBBTree *)index)->velocityFunc = func; -} - -cpSpatialIndex * -cpBBTreeNew(cpSpatialIndexBBFunc bbfunc, cpSpatialIndex *staticIndex) -{ - return cpBBTreeInit(cpBBTreeAlloc(), bbfunc, staticIndex); -} - -static void -cpBBTreeDestroy(cpBBTree *tree) -{ - cpHashSetFree(tree->leaves); - - if(tree->allocatedBuffers) cpArrayFreeEach(tree->allocatedBuffers, cpfree); - cpArrayFree(tree->allocatedBuffers); -} - -//MARK: Insert/Remove - -static void -cpBBTreeInsert(cpBBTree *tree, void *obj, cpHashValue hashid) -{ - Node *leaf = (Node *)cpHashSetInsert(tree->leaves, hashid, obj, tree, (cpHashSetTransFunc)leafSetTrans); - - Node *root = tree->root; - tree->root = SubtreeInsert(root, leaf, tree); - - leaf->STAMP = GetMasterTree(tree)->stamp; - LeafAddPairs(leaf, tree); - IncrementStamp(tree); -} - -static void -cpBBTreeRemove(cpBBTree *tree, void *obj, cpHashValue hashid) -{ - Node *leaf = (Node *)cpHashSetRemove(tree->leaves, hashid, obj); - - tree->root = SubtreeRemove(tree->root, leaf, tree); - PairsClear(leaf, tree); - NodeRecycle(tree, leaf); -} - -static cpBool -cpBBTreeContains(cpBBTree *tree, void *obj, cpHashValue hashid) -{ - return (cpHashSetFind(tree->leaves, hashid, obj) != NULL); -} - -//MARK: Reindex - -static void -cpBBTreeReindexQuery(cpBBTree *tree, cpSpatialIndexQueryFunc func, void *data) -{ - if(!tree->root) return; - - // LeafUpdate() may modify tree->root. Don't cache it. - cpHashSetEach(tree->leaves, (cpHashSetIteratorFunc)LeafUpdate, tree); - - cpSpatialIndex *staticIndex = tree->spatialIndex.staticIndex; - Node *staticRoot = (staticIndex && staticIndex->klass == Klass() ? ((cpBBTree *)staticIndex)->root : NULL); - - MarkContext context = {tree, staticRoot, func, data}; - MarkSubtree(tree->root, &context); - if(staticIndex && !staticRoot) cpSpatialIndexCollideStatic((cpSpatialIndex *)tree, staticIndex, func, data); - - IncrementStamp(tree); -} - -static void -cpBBTreeReindex(cpBBTree *tree) -{ - cpBBTreeReindexQuery(tree, VoidQueryFunc, NULL); -} - -static void -cpBBTreeReindexObject(cpBBTree *tree, void *obj, cpHashValue hashid) -{ - Node *leaf = (Node *)cpHashSetFind(tree->leaves, hashid, obj); - if(leaf){ - if(LeafUpdate(leaf, tree)) LeafAddPairs(leaf, tree); - IncrementStamp(tree); - } -} - -//MARK: Query - -static void -cpBBTreeSegmentQuery(cpBBTree *tree, void *obj, cpVect a, cpVect b, cpFloat t_exit, cpSpatialIndexSegmentQueryFunc func, void *data) -{ - Node *root = tree->root; - if(root) SubtreeSegmentQuery(root, obj, a, b, t_exit, func, data); -} - -static void -cpBBTreeQuery(cpBBTree *tree, void *obj, cpBB bb, cpSpatialIndexQueryFunc func, void *data) -{ - if(tree->root) SubtreeQuery(tree->root, obj, bb, func, data); -} - -//MARK: Misc - -static int -cpBBTreeCount(cpBBTree *tree) -{ - return cpHashSetCount(tree->leaves); -} - -typedef struct eachContext { - cpSpatialIndexIteratorFunc func; - void *data; -} eachContext; - -static void each_helper(Node *node, eachContext *context){context->func(node->obj, context->data);} - -static void -cpBBTreeEach(cpBBTree *tree, cpSpatialIndexIteratorFunc func, void *data) -{ - eachContext context = {func, data}; - cpHashSetEach(tree->leaves, (cpHashSetIteratorFunc)each_helper, &context); -} - -static cpSpatialIndexClass klass = { - (cpSpatialIndexDestroyImpl)cpBBTreeDestroy, - - (cpSpatialIndexCountImpl)cpBBTreeCount, - (cpSpatialIndexEachImpl)cpBBTreeEach, - - (cpSpatialIndexContainsImpl)cpBBTreeContains, - (cpSpatialIndexInsertImpl)cpBBTreeInsert, - (cpSpatialIndexRemoveImpl)cpBBTreeRemove, - - (cpSpatialIndexReindexImpl)cpBBTreeReindex, - (cpSpatialIndexReindexObjectImpl)cpBBTreeReindexObject, - (cpSpatialIndexReindexQueryImpl)cpBBTreeReindexQuery, - - (cpSpatialIndexQueryImpl)cpBBTreeQuery, - (cpSpatialIndexSegmentQueryImpl)cpBBTreeSegmentQuery, -}; - -static inline cpSpatialIndexClass *Klass(){return &klass;} - - -//MARK: Tree Optimization - -static int -cpfcompare(const cpFloat *a, const cpFloat *b){ - return (*a < *b ? -1 : (*b < *a ? 1 : 0)); -} - -static void -fillNodeArray(Node *node, Node ***cursor){ - (**cursor) = node; - (*cursor)++; -} - -static Node * -partitionNodes(cpBBTree *tree, Node **nodes, int count) -{ - if(count == 1){ - return nodes[0]; - } else if(count == 2) { - return NodeNew(tree, nodes[0], nodes[1]); - } - - // Find the AABB for these nodes - cpBB bb = nodes[0]->bb; - for(int i=1; ibb); - - // Split it on it's longest axis - cpBool splitWidth = (bb.r - bb.l > bb.t - bb.b); - - // Sort the bounds and use the median as the splitting point - cpFloat *bounds = (cpFloat *)cpcalloc(count*2, sizeof(cpFloat)); - if(splitWidth){ - for(int i=0; ibb.l; - bounds[2*i + 1] = nodes[i]->bb.r; - } - } else { - for(int i=0; ibb.b; - bounds[2*i + 1] = nodes[i]->bb.t; - } - } - - qsort(bounds, count*2, sizeof(cpFloat), (int (*)(const void *, const void *))cpfcompare); - cpFloat split = (bounds[count - 1] + bounds[count])*0.5f; // use the medain as the split - cpfree(bounds); - - // Generate the child BBs - cpBB a = bb, b = bb; - if(splitWidth) a.r = b.l = split; else a.t = b.b = split; - - // Partition the nodes - int right = count; - for(int left=0; left < right;){ - Node *node = nodes[left]; - if(cpBBMergedArea(node->bb, b) < cpBBMergedArea(node->bb, a)){ -// if(cpBBProximity(node->bb, b) < cpBBProximity(node->bb, a)){ - right--; - nodes[left] = nodes[right]; - nodes[right] = node; - } else { - left++; - } - } - - if(right == count){ - Node *node = NULL; - for(int i=0; iroot; -// Node *node = root; -// int bit = 0; -// unsigned int path = tree->opath; -// -// while(!NodeIsLeaf(node)){ -// node = (path&(1<a : node->b); -// bit = (bit + 1)&(sizeof(unsigned int)*8 - 1); -// } -// -// root = subtreeRemove(root, node, tree); -// tree->root = subtreeInsert(root, node, tree); -// } -//} - -void -cpBBTreeOptimize(cpSpatialIndex *index) -{ - if(index->klass != &klass){ - cpAssertWarn(cpFalse, "Ignoring cpBBTreeOptimize() call to non-tree spatial index."); - return; - } - - cpBBTree *tree = (cpBBTree *)index; - Node *root = tree->root; - if(!root) return; - - int count = cpBBTreeCount(tree); - Node **nodes = (Node **)cpcalloc(count, sizeof(Node *)); - Node **cursor = nodes; - - cpHashSetEach(tree->leaves, (cpHashSetIteratorFunc)fillNodeArray, &cursor); - - SubtreeRecycle(tree, root); - tree->root = partitionNodes(tree, nodes, count); - cpfree(nodes); -} - -//MARK: Debug Draw - -//#define CP_BBTREE_DEBUG_DRAW -#ifdef CP_BBTREE_DEBUG_DRAW -#include "OpenGL/gl.h" -#include "OpenGL/glu.h" -#include - -static void -NodeRender(Node *node, int depth) -{ - if(!NodeIsLeaf(node) && depth <= 10){ - NodeRender(node->a, depth + 1); - NodeRender(node->b, depth + 1); - } - - cpBB bb = node->bb; - -// GLfloat v = depth/2.0f; -// glColor3f(1.0f - v, v, 0.0f); - glLineWidth(cpfmax(5.0f - depth, 1.0f)); - glBegin(GL_LINES); { - glVertex2f(bb.l, bb.b); - glVertex2f(bb.l, bb.t); - - glVertex2f(bb.l, bb.t); - glVertex2f(bb.r, bb.t); - - glVertex2f(bb.r, bb.t); - glVertex2f(bb.r, bb.b); - - glVertex2f(bb.r, bb.b); - glVertex2f(bb.l, bb.b); - }; glEnd(); -} - -void -cpBBTreeRenderDebug(cpSpatialIndex *index){ - if(index->klass != &klass){ - cpAssertWarn(cpFalse, "Ignoring cpBBTreeRenderDebug() call to non-tree spatial index."); - return; - } - - cpBBTree *tree = (cpBBTree *)index; - if(tree->root) NodeRender(tree->root, 0); -} -#endif diff --git a/chipmunk/src/cpBody.c b/chipmunk/src/cpBody.c deleted file mode 100644 index f4dc8420ff..0000000000 --- a/chipmunk/src/cpBody.c +++ /dev/null @@ -1,330 +0,0 @@ -/* Copyright (c) 2007 Scott Lembcke - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -#include - -#include "chipmunk_private.h" -#include "constraints/util.h" - -// initialized in cpInitChipmunk() -cpBody cpStaticBodySingleton; - -cpBody* -cpBodyAlloc(void) -{ - return (cpBody *)cpcalloc(1, sizeof(cpBody)); -} - -cpBody * -cpBodyInit(cpBody *body, cpFloat m, cpFloat i) -{ - body->space = NULL; - body->shapeList = NULL; - body->arbiterList = NULL; - body->constraintList = NULL; - - body->velocity_func = cpBodyUpdateVelocity; - body->position_func = cpBodyUpdatePosition; - - cpComponentNode node = {NULL, NULL, 0.0f}; - body->node = node; - - body->p = cpvzero; - body->v = cpvzero; - body->f = cpvzero; - - body->w = 0.0f; - body->t = 0.0f; - - body->v_bias = cpvzero; - body->w_bias = 0.0f; - - body->v_limit = (cpFloat)INFINITY; - body->w_limit = (cpFloat)INFINITY; - - body->data = NULL; - - // Setters must be called after full initialization so the sanity checks don't assert on garbage data. - cpBodySetMass(body, m); - cpBodySetMoment(body, i); - cpBodySetAngle(body, 0.0f); - - return body; -} - -cpBody* -cpBodyNew(cpFloat m, cpFloat i) -{ - return cpBodyInit(cpBodyAlloc(), m, i); -} - -cpBody * -cpBodyInitStatic(cpBody *body) -{ - cpBodyInit(body, (cpFloat)INFINITY, (cpFloat)INFINITY); - body->node.idleTime = (cpFloat)INFINITY; - - return body; -} - -cpBody * -cpBodyNewStatic(void) -{ - return cpBodyInitStatic(cpBodyAlloc()); -} - -void cpBodyDestroy(cpBody *body){} - -void -cpBodyFree(cpBody *body) -{ - if(body){ - cpBodyDestroy(body); - cpfree(body); - } -} - -static void cpv_assert_nan(cpVect v, char *message){cpAssertSoft(v.x == v.x && v.y == v.y, message);} -static void cpv_assert_infinite(cpVect v, char *message){cpAssertSoft(cpfabs(v.x) != INFINITY && cpfabs(v.y) != INFINITY, message);} -static void cpv_assert_sane(cpVect v, char *message){cpv_assert_nan(v, message); cpv_assert_infinite(v, message);} - -#ifdef __cplusplus -extern "C" { -#endif - -void -cpBodySanityCheck(cpBody *body) -{ - cpAssertSoft(body->m == body->m && body->m_inv == body->m_inv, "Body's mass is invalid."); - cpAssertSoft(body->i == body->i && body->i_inv == body->i_inv, "Body's moment is invalid."); - - cpv_assert_sane(body->p, "Body's position is invalid."); - cpv_assert_sane(body->v, "Body's velocity is invalid."); - cpv_assert_sane(body->f, "Body's force is invalid."); - - cpAssertSoft(body->a == body->a && cpfabs(body->a) != INFINITY, "Body's angle is invalid."); - cpAssertSoft(body->w == body->w && cpfabs(body->w) != INFINITY, "Body's angular velocity is invalid."); - cpAssertSoft(body->t == body->t && cpfabs(body->t) != INFINITY, "Body's torque is invalid."); - - cpv_assert_sane(body->rot, "Body's rotation vector is invalid."); - - cpAssertSoft(body->v_limit == body->v_limit, "Body's velocity limit is invalid."); - cpAssertSoft(body->w_limit == body->w_limit, "Body's angular velocity limit is invalid."); -} - -#ifdef __cplusplus -} -#endif - -void -cpBodySetMass(cpBody *body, cpFloat mass) -{ - cpAssertHard(mass > 0.0f, "Mass must be positive and non-zero."); - - cpBodyActivate(body); - body->m = mass; - body->m_inv = 1.0f/mass; - cpBodyAssertSane(body); -} - -void -cpBodySetMoment(cpBody *body, cpFloat moment) -{ - cpAssertHard(moment > 0.0f, "Moment of Inertia must be positive and non-zero."); - - cpBodyActivate(body); - body->i = moment; - body->i_inv = 1.0f/moment; - cpBodyAssertSane(body); -} - -void -cpBodyAddShape(cpBody *body, cpShape *shape) -{ - cpShape *next = body->shapeList; - if(next) next->prev = shape; - - shape->next = next; - body->shapeList = shape; -} - -void -cpBodyRemoveShape(cpBody *body, cpShape *shape) -{ - cpShape *prev = shape->prev; - cpShape *next = shape->next; - - if(prev){ - prev->next = next; - } else { - body->shapeList = next; - } - - if(next){ - next->prev = prev; - } - - shape->prev = NULL; - shape->next = NULL; -} - -static cpConstraint * -filterConstraints(cpConstraint *node, cpBody *body, cpConstraint *filter) -{ - if(node == filter){ - return cpConstraintNext(node, body); - } else if(node->a == body){ - node->next_a = filterConstraints(node->next_a, body, filter); - } else { - node->next_b = filterConstraints(node->next_b, body, filter); - } - - return node; -} - -void -cpBodyRemoveConstraint(cpBody *body, cpConstraint *constraint) -{ - body->constraintList = filterConstraints(body->constraintList, body, constraint); -} - -void -cpBodySetPos(cpBody *body, cpVect pos) -{ - cpBodyActivate(body); - body->p = pos; - cpBodyAssertSane(body); -} - -static inline void -setAngle(cpBody *body, cpFloat angle) -{ - body->a = angle;//fmod(a, (cpFloat)M_PI*2.0f); - body->rot = cpvforangle(angle); - cpBodyAssertSane(body); -} - -void -cpBodySetAngle(cpBody *body, cpFloat angle) -{ - cpBodyActivate(body); - setAngle(body, angle); -} - -void -cpBodyUpdateVelocity(cpBody *body, cpVect gravity, cpFloat damping, cpFloat dt) -{ - body->v = cpvclamp(cpvadd(cpvmult(body->v, damping), cpvmult(cpvadd(gravity, cpvmult(body->f, body->m_inv)), dt)), body->v_limit); - - cpFloat w_limit = body->w_limit; - body->w = cpfclamp(body->w*damping + body->t*body->i_inv*dt, -w_limit, w_limit); - - cpBodySanityCheck(body); -} - -void -cpBodyUpdatePosition(cpBody *body, cpFloat dt) -{ - body->p = cpvadd(body->p, cpvmult(cpvadd(body->v, body->v_bias), dt)); - setAngle(body, body->a + (body->w + body->w_bias)*dt); - - body->v_bias = cpvzero; - body->w_bias = 0.0f; - - cpBodySanityCheck(body); -} - -void -cpBodyResetForces(cpBody *body) -{ - cpBodyActivate(body); - body->f = cpvzero; - body->t = 0.0f; -} - -void -cpBodyApplyForce(cpBody *body, cpVect force, cpVect r) -{ - cpBodyActivate(body); - body->f = cpvadd(body->f, force); - body->t += cpvcross(r, force); -} - -void -cpBodyApplyImpulse(cpBody *body, const cpVect j, const cpVect r) -{ - cpBodyActivate(body); - apply_impulse(body, j, r); -} - -static inline cpVect -cpBodyGetVelAtPoint(cpBody *body, cpVect r) -{ - return cpvadd(body->v, cpvmult(cpvperp(r), body->w)); -} - -cpVect -cpBodyGetVelAtWorldPoint(cpBody *body, cpVect point) -{ - return cpBodyGetVelAtPoint(body, cpvsub(point, body->p)); -} - -cpVect -cpBodyGetVelAtLocalPoint(cpBody *body, cpVect point) -{ - return cpBodyGetVelAtPoint(body, cpvrotate(point, body->rot)); -} - -void -cpBodyEachShape(cpBody *body, cpBodyShapeIteratorFunc func, void *data) -{ - cpShape *shape = body->shapeList; - while(shape){ - cpShape *next = shape->next; - func(body, shape, data); - shape = next; - } -} - -void -cpBodyEachConstraint(cpBody *body, cpBodyConstraintIteratorFunc func, void *data) -{ - cpConstraint *constraint = body->constraintList; - while(constraint){ - cpConstraint *next = cpConstraintNext(constraint, body); - func(body, constraint, data); - constraint = next; - } -} - -void -cpBodyEachArbiter(cpBody *body, cpBodyArbiterIteratorFunc func, void *data) -{ - cpArbiter *arb = body->arbiterList; - while(arb){ - cpArbiter *next = cpArbiterNext(arb, body); - - arb->swappedColl = (body == arb->body_b); - func(body, arb, data); - - arb = next; - } -} diff --git a/chipmunk/src/cpCollision.c b/chipmunk/src/cpCollision.c deleted file mode 100644 index 3b3e4e1138..0000000000 --- a/chipmunk/src/cpCollision.c +++ /dev/null @@ -1,741 +0,0 @@ -/* Copyright (c) 2007 Scott Lembcke - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -#include -#include - -#ifndef WIN32 -#include -#endif - -#include "chipmunk_private.h" - -#if DEBUG && 0 -#include "ChipmunkDemo.h" -#define DRAW_ALL 0 -#define DRAW_GJK (0 || DRAW_ALL) -#define DRAW_EPA (0 || DRAW_ALL) -#define DRAW_CLOSEST (0 || DRAW_ALL) -#define DRAW_CLIP (0 || DRAW_ALL) - -#define PRINT_LOG 0 -#endif - -#define ENABLE_CACHING 1 - -#define MAX_GJK_ITERATIONS 30 -#define MAX_EPA_ITERATIONS 30 -#define WARN_GJK_ITERATIONS 20 -#define WARN_EPA_ITERATIONS 20 - -// Add contact points for circle to circle collisions. -// Used by several collision tests. -// TODO should accept hash parameter -static int -CircleToCircleQuery(const cpVect p1, const cpVect p2, const cpFloat r1, const cpFloat r2, cpHashValue hash, cpContact *con) -{ - cpFloat mindist = r1 + r2; - cpVect delta = cpvsub(p2, p1); - cpFloat distsq = cpvlengthsq(delta); - - if(distsq < mindist*mindist){ - cpFloat dist = cpfsqrt(distsq); - cpVect n = (dist ? cpvmult(delta, 1.0f/dist) : cpv(1.0f, 0.0f)); - cpContactInit(con, cpvlerp(p1, p2, r1/(r1 + r2)), n, dist - mindist, hash); - - return 1; - } else { - return 0; - } -} - -//MARK: Support Points and Edges: - -static inline int -PolySupportPointIndex(const int count, const cpVect *verts, const cpVect n) -{ - cpFloat max = -INFINITY; - int index = 0; - - for(int i=0; i max){ - max = d; - index = i; - } - } - - return index; -} - -struct SupportPoint { - cpVect p; - cpCollisionID id; -}; - -static inline struct SupportPoint -SupportPointNew(cpVect p, cpCollisionID id) -{ - struct SupportPoint point = {p, id}; - return point; -} - -typedef struct SupportPoint (*SupportPointFunc)(const cpShape *shape, const cpVect n); - -static inline struct SupportPoint -CircleSupportPoint(const cpCircleShape *circle, const cpVect n) -{ - return SupportPointNew(circle->tc, 0); -} - -static inline struct SupportPoint -SegmentSupportPoint(const cpSegmentShape *seg, const cpVect n) -{ - if(cpvdot(seg->ta, n) > cpvdot(seg->tb, n)){ - return SupportPointNew(seg->ta, 0); - } else { - return SupportPointNew(seg->tb, 1); - } -} - -static inline struct SupportPoint -PolySupportPoint(const cpPolyShape *poly, const cpVect n) -{ - const cpVect *verts = poly->tVerts; - int i = PolySupportPointIndex(poly->numVerts, verts, n); - return SupportPointNew(verts[i], i); -} - -struct MinkowskiPoint { - cpVect a, b; - cpVect ab; - cpCollisionID id; -}; - -static inline struct MinkowskiPoint -MinkowskiPointNew(const struct SupportPoint a, const struct SupportPoint b) -{ - struct MinkowskiPoint point = {a.p, b.p, cpvsub(b.p, a.p), (a.id & 0xFF)<<8 | (b.id & 0xFF)}; - return point; -} - -struct SupportContext { - const cpShape *shape1, *shape2; - SupportPointFunc func1, func2; -}; - -static inline struct MinkowskiPoint -Support(const struct SupportContext *ctx, const cpVect n) -{ - struct SupportPoint a = ctx->func1(ctx->shape1, cpvneg(n)); - struct SupportPoint b = ctx->func2(ctx->shape2, n); - return MinkowskiPointNew(a, b); -} - -struct EdgePoint { - cpVect p; - cpHashValue hash; -}; - -struct Edge { - struct EdgePoint a, b; - cpFloat r; - cpVect n; -}; - -static inline struct Edge -EdgeNew(cpVect va, cpVect vb, cpHashValue ha, cpHashValue hb, cpFloat r) -{ - struct Edge edge = {{va, ha}, {vb, hb}, r, cpvnormalize(cpvperp(cpvsub(vb, va)))}; - return edge; -} - -static struct Edge -SupportEdgeForPoly(const cpPolyShape *poly, const cpVect n) -{ - int numVerts = poly->numVerts; - int i1 = PolySupportPointIndex(poly->numVerts, poly->tVerts, n); - - // TODO get rid of mod eventually, very expensive on ARM - int i0 = (i1 - 1 + numVerts)%numVerts; - int i2 = (i1 + 1)%numVerts; - - cpVect *verts = poly->tVerts; - if(cpvdot(n, poly->tPlanes[i1].n) > cpvdot(n, poly->tPlanes[i2].n)){ - struct Edge edge = {{verts[i0], CP_HASH_PAIR(poly, i0)}, {verts[i1], CP_HASH_PAIR(poly, i1)}, poly->r, poly->tPlanes[i1].n}; - return edge; - } else { - struct Edge edge = {{verts[i1], CP_HASH_PAIR(poly, i1)}, {verts[i2], CP_HASH_PAIR(poly, i2)}, poly->r, poly->tPlanes[i2].n}; - return edge; - } -} - -static struct Edge -SupportEdgeForSegment(const cpSegmentShape *seg, const cpVect n) -{ - if(cpvdot(seg->tn, n) > 0.0){ - struct Edge edge = {{seg->ta, CP_HASH_PAIR(seg, 0)}, {seg->tb, CP_HASH_PAIR(seg, 1)}, seg->r, seg->tn}; - return edge; - } else { - struct Edge edge = {{seg->tb, CP_HASH_PAIR(seg, 1)}, {seg->ta, CP_HASH_PAIR(seg, 0)}, seg->r, cpvneg(seg->tn)}; - return edge; - } -} - -static inline cpFloat -ClosestT(const cpVect a, const cpVect b) -{ - cpVect delta = cpvsub(b, a); - return -cpfclamp(cpvdot(delta, cpvadd(a, b))/cpvlengthsq(delta), -1.0f, 1.0f); -} - -static inline cpVect -LerpT(const cpVect a, const cpVect b, const cpFloat t) -{ - cpFloat ht = 0.5f*t; - return cpvadd(cpvmult(a, 0.5f - ht), cpvmult(b, 0.5f + ht)); -} - -struct ClosestPoints { - cpVect a, b; - cpVect n; - cpFloat d; - cpCollisionID id; -}; - -static inline struct ClosestPoints -ClosestPointsNew(const struct MinkowskiPoint v0, const struct MinkowskiPoint v1) -{ - cpFloat t = ClosestT(v0.ab, v1.ab); - cpVect p = LerpT(v0.ab, v1.ab, t); - - cpVect pa = LerpT(v0.a, v1.a, t); - cpVect pb = LerpT(v0.b, v1.b, t); - cpCollisionID id = (v0.id & 0xFFFF)<<16 | (v1.id & 0xFFFF); - - cpVect delta = cpvsub(v1.ab, v0.ab); - cpVect n = cpvnormalize(cpvperp(delta)); - cpFloat d = -cpvdot(n, p); - - if(d <= 0.0f || (0.0f < t && t < 1.0f)){ - struct ClosestPoints points = {pa, pb, cpvneg(n), d, id}; - return points; - } else { - cpFloat d2 = cpvlength(p); - cpVect n2 = cpvmult(p, 1.0f/(d2 + CPFLOAT_MIN)); - - struct ClosestPoints points = {pa, pb, n2, d2, id}; - return points; - } -} - -//MARK: EPA Functions - -static inline cpFloat -ClosestDist(const cpVect v0,const cpVect v1) -{ - return cpvlengthsq(LerpT(v0, v1, ClosestT(v0, v1))); -} - -static struct ClosestPoints -EPARecurse(const struct SupportContext *ctx, const int count, const struct MinkowskiPoint *hull, const int iteration) -{ - int mini = 0; - cpFloat minDist = INFINITY; - - // TODO: precalculate this when building the hull and save a step. - for(int j=0, i=count-1; j 0.0f && iteration < MAX_EPA_ITERATIONS){ - int count2 = 1; - struct MinkowskiPoint *hull2 = (struct MinkowskiPoint *)alloca((count + 1)*sizeof(struct MinkowskiPoint)); - hull2[0] = p; - - for(int i=0; i 0.0f){ - hull2[count2] = hull[index]; - count2++; - } - } - - return EPARecurse(ctx, count2, hull2, iteration + 1); - } else { - cpAssertWarn(iteration < WARN_EPA_ITERATIONS, "High EPA iterations: %d", iteration); - return ClosestPointsNew(v0, v1); - } -} - -static struct ClosestPoints -EPA(const struct SupportContext *ctx, const struct MinkowskiPoint v0, const struct MinkowskiPoint v1, const struct MinkowskiPoint v2) -{ - // TODO: allocate a NxM array here and do an in place convex hull reduction in EPARecurse - struct MinkowskiPoint hull[3] = {v0, v1, v2}; - return EPARecurse(ctx, 3, hull, 1); -} - -//MARK: GJK Functions. - -static inline struct ClosestPoints -GJKRecurse(const struct SupportContext *ctx, const struct MinkowskiPoint v0, const struct MinkowskiPoint v1, const int iteration) -{ - if(iteration > MAX_GJK_ITERATIONS){ - cpAssertWarn(iteration < WARN_GJK_ITERATIONS, "High GJK iterations: %d", iteration); - return ClosestPointsNew(v0, v1); - } - - cpVect delta = cpvsub(v1.ab, v0.ab); - if(cpvcross(delta, cpvadd(v0.ab, v1.ab)) > 0.0f){ - // Origin is behind axis. Flip and try again. - return GJKRecurse(ctx, v1, v0, iteration + 1); - } else { - cpFloat t = ClosestT(v0.ab, v1.ab); - cpVect n = (-1.0f < t && t < 1.0f ? cpvperp(delta) : cpvneg(LerpT(v0.ab, v1.ab, t))); - struct MinkowskiPoint p = Support(ctx, n); - -#if DRAW_GJK - ChipmunkDebugDrawSegment(v0.ab, v1.ab, RGBAColor(1, 1, 1, 1)); - cpVect c = cpvlerp(v0.ab, v1.ab, 0.5); - ChipmunkDebugDrawSegment(c, cpvadd(c, cpvmult(cpvnormalize(n), 5.0)), RGBAColor(1, 0, 0, 1)); - - ChipmunkDebugDrawPoints(5.0, 1, &p.ab, RGBAColor(1, 1, 1, 1)); -#endif - - if( - cpvcross(cpvsub(v1.ab, p.ab), cpvadd(v1.ab, p.ab)) > 0.0f && - cpvcross(cpvsub(v0.ab, p.ab), cpvadd(v0.ab, p.ab)) < 0.0f - ){ - cpAssertWarn(iteration < WARN_GJK_ITERATIONS, "High GJK->EPA iterations: %d", iteration); - // The triangle v0, p, v1 contains the origin. Use EPA to find the MSA. - return EPA(ctx, v0, p, v1); - } else { - // The new point must be farther along the normal than the existing points. - if(cpvdot(p.ab, n) <= cpfmax(cpvdot(v0.ab, n), cpvdot(v1.ab, n))){ - cpAssertWarn(iteration < WARN_GJK_ITERATIONS, "High GJK iterations: %d", iteration); - return ClosestPointsNew(v0, v1); - } else { - if(ClosestDist(v0.ab, p.ab) < ClosestDist(p.ab, v1.ab)){ - return GJKRecurse(ctx, v0, p, iteration + 1); - } else { - return GJKRecurse(ctx, p, v1, iteration + 1); - } - } - } - } -} - -static struct SupportPoint -ShapePoint(const cpShape *shape, const int i) -{ - switch(shape->klass->type){ - case CP_CIRCLE_SHAPE: { - return SupportPointNew(((cpCircleShape *)shape)->tc, 0); - } case CP_SEGMENT_SHAPE: { - cpSegmentShape *seg = (cpSegmentShape *)shape; - return SupportPointNew(i == 0 ? seg->ta : seg->tb, i); - } case CP_POLY_SHAPE: { - cpPolyShape *poly = (cpPolyShape *)shape; - // Poly shapes may change vertex count. - int index = (i < poly->numVerts ? i : 0); - return SupportPointNew(poly->tVerts[index], index); - } default: { - return SupportPointNew(cpvzero, 0); - } - } -} - -static struct ClosestPoints -GJK(const struct SupportContext *ctx, cpCollisionID *id) -{ -#if DRAW_GJK || DRAW_EPA - // draw the minkowski difference origin - cpVect origin = cpvzero; - ChipmunkDebugDrawPoints(5.0, 1, &origin, RGBAColor(1,0,0,1)); - - int mdiffCount = ctx->count1*ctx->count2; - cpVect *mdiffVerts = alloca(mdiffCount*sizeof(cpVect)); - - for(int i=0; icount1; i++){ - for(int j=0; jcount2; j++){ - cpVect v1 = ShapePoint(ctx->count1, ctx->verts1, i).p; - cpVect v2 = ShapePoint(ctx->count2, ctx->verts2, j).p; - mdiffVerts[i*ctx->count2 + j] = cpvsub(v2, v1); - } - } - - cpVect *hullVerts = alloca(mdiffCount*sizeof(cpVect)); - int hullCount = cpConvexHull(mdiffCount, mdiffVerts, hullVerts, NULL, 0.0); - - ChipmunkDebugDrawPolygon(hullCount, hullVerts, RGBAColor(1, 0, 0, 1), RGBAColor(1, 0, 0, 0.25)); - ChipmunkDebugDrawPoints(2.0, mdiffCount, mdiffVerts, RGBAColor(1, 0, 0, 1)); -#endif - - struct MinkowskiPoint v0, v1; - if(*id && ENABLE_CACHING){ - v0 = MinkowskiPointNew(ShapePoint(ctx->shape1, (*id>>24)&0xFF), ShapePoint(ctx->shape2, (*id>>16)&0xFF)); - v1 = MinkowskiPointNew(ShapePoint(ctx->shape1, (*id>> 8)&0xFF), ShapePoint(ctx->shape2, (*id )&0xFF)); - } else { - cpVect axis = cpvperp(cpvsub(cpBBCenter(ctx->shape1->bb), cpBBCenter(ctx->shape2->bb))); - v0 = Support(ctx, axis); - v1 = Support(ctx, cpvneg(axis)); - } - - struct ClosestPoints points = GJKRecurse(ctx, v0, v1, 1); - *id = points.id; - return points; -} - -//MARK: Contact Clipping - -static inline void -Contact1(cpFloat dist, cpVect a, cpVect b, cpFloat refr, cpFloat incr, cpVect n, cpHashValue hash, cpContact *arr) -{ - cpFloat rsum = refr + incr; - cpFloat alpha = (rsum > 0.0f ? refr/rsum : 0.5f); - cpVect point = cpvlerp(a, b, alpha); - - cpContactInit(arr, point, n, dist - rsum, hash); -} - -static inline int -Contact2(cpVect refp, cpVect inca, cpVect incb, cpFloat refr, cpFloat incr, cpVect refn, cpVect n, cpHashValue hash, cpContact *arr) -{ - cpFloat cian = cpvcross(inca, refn); - cpFloat cibn = cpvcross(incb, refn); - cpFloat crpn = cpvcross(refp, refn); - cpFloat t = 1.0f - cpfclamp01((cibn - crpn)/(cibn - cian)); - - cpVect point = cpvlerp(inca, incb, t); - cpFloat pd = cpvdot(cpvsub(point, refp), refn); - - if(t > 0.0f && pd <= 0.0f){ - cpFloat rsum = refr + incr; - cpFloat alpha = (rsum > 0.0f ? incr*(1.0f - (rsum + pd)/rsum) : -0.5f*pd); - - cpContactInit(arr, cpvadd(point, cpvmult(refn, alpha)), n, pd, hash); - return 1; - } else { - return 0; - } -} - -static inline int -ClipContacts(const struct Edge ref, const struct Edge inc, const struct ClosestPoints points, const cpFloat nflip, cpContact *arr) -{ - cpVect inc_offs = cpvmult(inc.n, inc.r); - cpVect ref_offs = cpvmult(ref.n, ref.r); - - cpVect inca = cpvadd(inc.a.p, inc_offs); - cpVect incb = cpvadd(inc.b.p, inc_offs); - - cpVect closest_inca = cpClosetPointOnSegment(inc.a.p, ref.a.p, ref.b.p); - cpVect closest_incb = cpClosetPointOnSegment(inc.b.p, ref.a.p, ref.b.p); - - cpVect msa = cpvmult(points.n, nflip*points.d); - cpFloat cost_a = cpvdistsq(cpvsub(inc.a.p, closest_inca), msa); - cpFloat cost_b = cpvdistsq(cpvsub(inc.b.p, closest_incb), msa); - -#if DRAW_CLIP - ChipmunkDebugDrawSegment(ref.a.p, ref.b.p, RGBAColor(1, 0, 0, 1)); - ChipmunkDebugDrawSegment(inc.a.p, inc.b.p, RGBAColor(0, 1, 0, 1)); - ChipmunkDebugDrawSegment(inca, incb, RGBAColor(0, 1, 0, 1)); - - cpVect cref = cpvlerp(ref.a.p, ref.b.p, 0.5); - ChipmunkDebugDrawSegment(cref, cpvadd(cref, cpvmult(ref.n, 5.0)), RGBAColor(1, 0, 0, 1)); - - cpVect cinc = cpvlerp(inc.a.p, inc.b.p, 0.5); - ChipmunkDebugDrawSegment(cinc, cpvadd(cinc, cpvmult(inc.n, 5.0)), RGBAColor(1, 0, 0, 1)); - - ChipmunkDebugDrawPoints(5.0, 2, (cpVect[]){ref.a.p, inc.a.p}, RGBAColor(1, 1, 0, 1)); - ChipmunkDebugDrawPoints(5.0, 2, (cpVect[]){ref.b.p, inc.b.p}, RGBAColor(0, 1, 1, 1)); - - if(cost_a < cost_b){ - ChipmunkDebugDrawSegment(closest_inca, inc.a.p, RGBAColor(1, 0, 1, 1)); - } else { - ChipmunkDebugDrawSegment(closest_incb, inc.b.p, RGBAColor(1, 0, 1, 1)); - } -#endif - - cpHashValue hash_iarb = CP_HASH_PAIR(inc.a.hash, ref.b.hash); - cpHashValue hash_ibra = CP_HASH_PAIR(inc.b.hash, ref.a.hash); - - if(cost_a < cost_b){ - cpVect refp = cpvadd(ref.a.p, ref_offs); - Contact1(points.d, closest_inca, inc.a.p, ref.r, inc.r, points.n, hash_iarb, arr); - return Contact2(refp, inca, incb, ref.r, inc.r, ref.n, points.n, hash_ibra, arr + 1) + 1; - } else { - cpVect refp = cpvadd(ref.b.p, ref_offs); - Contact1(points.d, closest_incb, inc.b.p, ref.r, inc.r, points.n, hash_ibra, arr); - return Contact2(refp, incb, inca, ref.r, inc.r, ref.n, points.n, hash_iarb, arr + 1) + 1; - } -} - -static inline int -ContactPoints(const struct Edge e1, const struct Edge e2, const struct ClosestPoints points, cpContact *arr) -{ - cpFloat mindist = e1.r + e2.r; - if(points.d <= mindist){ - cpFloat pick = cpvdot(e1.n, points.n) + cpvdot(e2.n, points.n); - - if( - (pick != 0.0f && pick > 0.0f) || - // If the edges are both perfectly aligned weird things happen. - // This is *very* common at the start of a simulation. - // Pick the longest edge as the reference to break the tie. - (pick == 0.0f && (cpvdistsq(e1.a.p, e1.b.p) > cpvdistsq(e2.a.p, e2.b.p))) - ){ - return ClipContacts(e1, e2, points, 1.0f, arr); - } else { - return ClipContacts(e2, e1, points, -1.0f, arr); - } - } else { - return 0; - } -} - -//MARK: Collision Functions - -typedef int (*CollisionFunc)(const cpShape *a, const cpShape *b, cpCollisionID *id, cpContact *arr); - -// Collide circle shapes. -static int -CircleToCircle(const cpCircleShape *c1, const cpCircleShape *c2, cpCollisionID *id, cpContact *arr) -{ - return CircleToCircleQuery(c1->tc, c2->tc, c1->r, c2->r, 0, arr); -} - -static int -CircleToSegment(const cpCircleShape *circleShape, const cpSegmentShape *segmentShape, cpCollisionID *id, cpContact *con) -{ - cpVect seg_a = segmentShape->ta; - cpVect seg_b = segmentShape->tb; - cpVect center = circleShape->tc; - - cpVect seg_delta = cpvsub(seg_b, seg_a); - cpFloat closest_t = cpfclamp01(cpvdot(seg_delta, cpvsub(center, seg_a))/cpvlengthsq(seg_delta)); - cpVect closest = cpvadd(seg_a, cpvmult(seg_delta, closest_t)); - - if(CircleToCircleQuery(center, closest, circleShape->r, segmentShape->r, 0, con)){ - cpVect n = con[0].n; - - // Reject endcap collisions if tangents are provided. - if( - (closest_t != 0.0f || cpvdot(n, cpvrotate(segmentShape->a_tangent, segmentShape->shape.body->rot)) >= 0.0) && - (closest_t != 1.0f || cpvdot(n, cpvrotate(segmentShape->b_tangent, segmentShape->shape.body->rot)) >= 0.0) - ){ - return 1; - } - } - - return 0; -} - -static int -SegmentToSegment(const cpSegmentShape *seg1, const cpSegmentShape *seg2, cpCollisionID *id, cpContact *arr) -{ - struct SupportContext context = {(cpShape *)seg1, (cpShape *)seg2, (SupportPointFunc)SegmentSupportPoint, (SupportPointFunc)SegmentSupportPoint}; - struct ClosestPoints points = GJK(&context, id); - -#if DRAW_CLOSEST -#if PRINT_LOG -// ChipmunkDemoPrintString("Distance: %.2f\n", points.d); -#endif - - ChipmunkDebugDrawDot(6.0, points.a, RGBAColor(1, 1, 1, 1)); - ChipmunkDebugDrawDot(6.0, points.b, RGBAColor(1, 1, 1, 1)); - ChipmunkDebugDrawSegment(points.a, points.b, RGBAColor(1, 1, 1, 1)); - ChipmunkDebugDrawSegment(points.a, cpvadd(points.a, cpvmult(points.n, 10.0)), RGBAColor(1, 0, 0, 1)); -#endif - - cpVect n = points.n; - cpVect rot1 = seg1->shape.body->rot; - cpVect rot2 = seg2->shape.body->rot; - if( - points.d <= (seg1->r + seg2->r) && - ( - (!cpveql(points.a, seg1->ta) || cpvdot(n, cpvrotate(seg1->a_tangent, rot1)) <= 0.0) && - (!cpveql(points.a, seg1->tb) || cpvdot(n, cpvrotate(seg1->b_tangent, rot1)) <= 0.0) && - (!cpveql(points.b, seg2->ta) || cpvdot(n, cpvrotate(seg2->a_tangent, rot2)) >= 0.0) && - (!cpveql(points.b, seg2->tb) || cpvdot(n, cpvrotate(seg2->b_tangent, rot2)) >= 0.0) - ) - ){ - return ContactPoints(SupportEdgeForSegment(seg1, n), SupportEdgeForSegment(seg2, cpvneg(n)), points, arr); - } else { - return 0; - } -} - -static int -PolyToPoly(const cpPolyShape *poly1, const cpPolyShape *poly2, cpCollisionID *id, cpContact *arr) -{ - struct SupportContext context = {(cpShape *)poly1, (cpShape *)poly2, (SupportPointFunc)PolySupportPoint, (SupportPointFunc)PolySupportPoint}; - struct ClosestPoints points = GJK(&context, id); - -#if DRAW_CLOSEST -#if PRINT_LOG -// ChipmunkDemoPrintString("Distance: %.2f\n", points.d); -#endif - - ChipmunkDebugDrawDot(3.0, points.a, RGBAColor(1, 1, 1, 1)); - ChipmunkDebugDrawDot(3.0, points.b, RGBAColor(1, 1, 1, 1)); - ChipmunkDebugDrawSegment(points.a, points.b, RGBAColor(1, 1, 1, 1)); - ChipmunkDebugDrawSegment(points.a, cpvadd(points.a, cpvmult(points.n, 10.0)), RGBAColor(1, 0, 0, 1)); -#endif - - if(points.d - poly1->r - poly2->r <= 0.0){ - return ContactPoints(SupportEdgeForPoly(poly1, points.n), SupportEdgeForPoly(poly2, cpvneg(points.n)), points, arr); - } else { - return 0; - } -} - -static int -SegmentToPoly(const cpSegmentShape *seg, const cpPolyShape *poly, cpCollisionID *id, cpContact *arr) -{ - struct SupportContext context = {(cpShape *)seg, (cpShape *)poly, (SupportPointFunc)SegmentSupportPoint, (SupportPointFunc)PolySupportPoint}; - struct ClosestPoints points = GJK(&context, id); - -#if DRAW_CLOSEST -#if PRINT_LOG -// ChipmunkDemoPrintString("Distance: %.2f\n", points.d); -#endif - - ChipmunkDebugDrawDot(3.0, points.a, RGBAColor(1, 1, 1, 1)); - ChipmunkDebugDrawDot(3.0, points.b, RGBAColor(1, 1, 1, 1)); - ChipmunkDebugDrawSegment(points.a, points.b, RGBAColor(1, 1, 1, 1)); - ChipmunkDebugDrawSegment(points.a, cpvadd(points.a, cpvmult(points.n, 10.0)), RGBAColor(1, 0, 0, 1)); -#endif - - // Reject endcap collisions if tangents are provided. - cpVect n = points.n; - cpVect rot = seg->shape.body->rot; - if( - points.d - seg->r - poly->r <= 0.0 && - ( - (!cpveql(points.a, seg->ta) || cpvdot(n, cpvrotate(seg->a_tangent, rot)) <= 0.0) && - (!cpveql(points.a, seg->tb) || cpvdot(n, cpvrotate(seg->b_tangent, rot)) <= 0.0) - ) - ){ - return ContactPoints(SupportEdgeForSegment(seg, n), SupportEdgeForPoly(poly, cpvneg(n)), points, arr); - } else { - return 0; - } -} - -// This one is less gross, but still gross. -// TODO: Comment me! -static int -CircleToPoly(const cpCircleShape *circle, const cpPolyShape *poly, cpCollisionID *id, cpContact *con) -{ - struct SupportContext context = {(cpShape *)circle, (cpShape *)poly, (SupportPointFunc)CircleSupportPoint, (SupportPointFunc)PolySupportPoint}; - struct ClosestPoints points = GJK(&context, id); - -#if DRAW_CLOSEST - ChipmunkDebugDrawDot(3.0, points.a, RGBAColor(1, 1, 1, 1)); - ChipmunkDebugDrawDot(3.0, points.b, RGBAColor(1, 1, 1, 1)); - ChipmunkDebugDrawSegment(points.a, points.b, RGBAColor(1, 1, 1, 1)); - ChipmunkDebugDrawSegment(points.a, cpvadd(points.a, cpvmult(points.n, 10.0)), RGBAColor(1, 0, 0, 1)); -#endif - - cpFloat mindist = circle->r + poly->r; - if(points.d - mindist <= 0.0){ - cpVect p = cpvlerp(points.a, points.b, circle->r/(mindist)); - cpContactInit(con, p, points.n, points.d - mindist, 0); - return 1; - } else { - return 0; - } -} - -static const CollisionFunc builtinCollisionFuncs[9] = { - (CollisionFunc)CircleToCircle, - NULL, - NULL, - (CollisionFunc)CircleToSegment, - NULL, - NULL, - (CollisionFunc)CircleToPoly, - (CollisionFunc)SegmentToPoly, - (CollisionFunc)PolyToPoly, -}; -static const CollisionFunc *colfuncs = builtinCollisionFuncs; - -static const CollisionFunc segmentCollisions[9] = { - (CollisionFunc)CircleToCircle, - NULL, - NULL, - (CollisionFunc)CircleToSegment, - (CollisionFunc)SegmentToSegment, - NULL, - (CollisionFunc)CircleToPoly, - (CollisionFunc)SegmentToPoly, - (CollisionFunc)PolyToPoly, -}; - -void -cpEnableSegmentToSegmentCollisions(void) -{ - colfuncs = segmentCollisions; -} - -int -cpCollideShapes(const cpShape *a, const cpShape *b, cpCollisionID *id, cpContact *arr) -{ - // Their shape types must be in order. - cpAssertSoft(a->klass->type <= b->klass->type, "Internal Error: Collision shapes passed to cpCollideShapes() are not sorted."); - - CollisionFunc cfunc = colfuncs[a->klass->type + b->klass->type*CP_NUM_SHAPES]; - - int numContacts = (cfunc? cfunc(a, b, id, arr) : 0); - cpAssertSoft(numContacts <= CP_MAX_CONTACTS_PER_ARBITER, "Internal error: Too many contact points returned."); - - return numContacts; -} diff --git a/chipmunk/src/cpHashSet.c b/chipmunk/src/cpHashSet.c deleted file mode 100644 index 0d68a07476..0000000000 --- a/chipmunk/src/cpHashSet.c +++ /dev/null @@ -1,253 +0,0 @@ -/* Copyright (c) 2007 Scott Lembcke - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -#include "chipmunk_private.h" -#include "prime.h" - -typedef struct cpHashSetBin { - void *elt; - cpHashValue hash; - struct cpHashSetBin *next; -} cpHashSetBin; - -struct cpHashSet { - unsigned int entries, size; - - cpHashSetEqlFunc eql; - void *default_value; - - cpHashSetBin **table; - cpHashSetBin *pooledBins; - - cpArray *allocatedBuffers; -}; - -void -cpHashSetFree(cpHashSet *set) -{ - if(set){ - cpfree(set->table); - - cpArrayFreeEach(set->allocatedBuffers, cpfree); - cpArrayFree(set->allocatedBuffers); - - cpfree(set); - } -} - -cpHashSet * -cpHashSetNew(int size, cpHashSetEqlFunc eqlFunc) -{ - cpHashSet *set = (cpHashSet *)cpcalloc(1, sizeof(cpHashSet)); - - set->size = next_prime(size); - set->entries = 0; - - set->eql = eqlFunc; - set->default_value = NULL; - - set->table = (cpHashSetBin **)cpcalloc(set->size, sizeof(cpHashSetBin *)); - set->pooledBins = NULL; - - set->allocatedBuffers = cpArrayNew(0); - - return set; -} - -void -cpHashSetSetDefaultValue(cpHashSet *set, void *default_value) -{ - set->default_value = default_value; -} - -static int -setIsFull(cpHashSet *set) -{ - return (set->entries >= set->size); -} - -static void -cpHashSetResize(cpHashSet *set) -{ - // Get the next approximate doubled prime. - unsigned int newSize = next_prime(set->size + 1); - // Allocate a new table. - cpHashSetBin **newTable = (cpHashSetBin **)cpcalloc(newSize, sizeof(cpHashSetBin *)); - - // Iterate over the chains. - for(unsigned int i=0; isize; i++){ - // Rehash the bins into the new table. - cpHashSetBin *bin = set->table[i]; - while(bin){ - cpHashSetBin *next = bin->next; - - cpHashValue idx = bin->hash%newSize; - bin->next = newTable[idx]; - newTable[idx] = bin; - - bin = next; - } - } - - cpfree(set->table); - - set->table = newTable; - set->size = newSize; -} - -static inline void -recycleBin(cpHashSet *set, cpHashSetBin *bin) -{ - bin->next = set->pooledBins; - set->pooledBins = bin; - bin->elt = NULL; -} - -static cpHashSetBin * -getUnusedBin(cpHashSet *set) -{ - cpHashSetBin *bin = set->pooledBins; - - if(bin){ - set->pooledBins = bin->next; - return bin; - } else { - // Pool is exhausted, make more - int count = CP_BUFFER_BYTES/sizeof(cpHashSetBin); - cpAssertHard(count, "Internal Error: Buffer size is too small."); - - cpHashSetBin *buffer = (cpHashSetBin *)cpcalloc(1, CP_BUFFER_BYTES); - cpArrayPush(set->allocatedBuffers, buffer); - - // push all but the first one, return it instead - for(int i=1; ientries; -} - -void * -cpHashSetInsert(cpHashSet *set, cpHashValue hash, void *ptr, void *data, cpHashSetTransFunc trans) -{ - cpHashValue idx = hash%set->size; - - // Find the bin with the matching element. - cpHashSetBin *bin = set->table[idx]; - while(bin && !set->eql(ptr, bin->elt)) - bin = bin->next; - - // Create it if necessary. - if(!bin){ - bin = getUnusedBin(set); - bin->hash = hash; - bin->elt = (trans ? trans(ptr, data) : data); - - bin->next = set->table[idx]; - set->table[idx] = bin; - - set->entries++; - if(setIsFull(set)) cpHashSetResize(set); - } - - return bin->elt; -} - -void * -cpHashSetRemove(cpHashSet *set, cpHashValue hash, void *ptr) -{ - cpHashValue idx = hash%set->size; - - cpHashSetBin **prev_ptr = &set->table[idx]; - cpHashSetBin *bin = set->table[idx]; - - // Find the bin - while(bin && !set->eql(ptr, bin->elt)){ - prev_ptr = &bin->next; - bin = bin->next; - } - - // Remove it if it exists. - if(bin){ - // Update the previous linked list pointer - (*prev_ptr) = bin->next; - set->entries--; - - void *elt = bin->elt; - recycleBin(set, bin); - - return elt; - } - - return NULL; -} - -void * -cpHashSetFind(cpHashSet *set, cpHashValue hash, void *ptr) -{ - cpHashValue idx = hash%set->size; - cpHashSetBin *bin = set->table[idx]; - while(bin && !set->eql(ptr, bin->elt)) - bin = bin->next; - - return (bin ? bin->elt : set->default_value); -} - -void -cpHashSetEach(cpHashSet *set, cpHashSetIteratorFunc func, void *data) -{ - for(unsigned int i=0; isize; i++){ - cpHashSetBin *bin = set->table[i]; - while(bin){ - cpHashSetBin *next = bin->next; - func(bin->elt, data); - bin = next; - } - } -} - -void -cpHashSetFilter(cpHashSet *set, cpHashSetFilterFunc func, void *data) -{ - for(unsigned int i=0; isize; i++){ - // The rest works similarly to cpHashSetRemove() above. - cpHashSetBin **prev_ptr = &set->table[i]; - cpHashSetBin *bin = set->table[i]; - while(bin){ - cpHashSetBin *next = bin->next; - - if(func(bin->elt, data)){ - prev_ptr = &bin->next; - } else { - (*prev_ptr) = next; - - set->entries--; - recycleBin(set, bin); - } - - bin = next; - } - } -} diff --git a/chipmunk/src/cpPolyShape.c b/chipmunk/src/cpPolyShape.c deleted file mode 100644 index 12f2168c9a..0000000000 --- a/chipmunk/src/cpPolyShape.c +++ /dev/null @@ -1,329 +0,0 @@ -/* Copyright (c) 2007 Scott Lembcke - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -#include "chipmunk_private.h" -#include "chipmunk_unsafe.h" - -cpPolyShape * -cpPolyShapeAlloc(void) -{ - return (cpPolyShape *)cpcalloc(1, sizeof(cpPolyShape)); -} - -static cpBB -cpPolyShapeTransformVerts(cpPolyShape *poly, cpVect p, cpVect rot) -{ - cpVect *src = poly->verts; - cpVect *dst = poly->tVerts; - - cpFloat l = (cpFloat)INFINITY, r = -(cpFloat)INFINITY; - cpFloat b = (cpFloat)INFINITY, t = -(cpFloat)INFINITY; - - for(int i=0; inumVerts; i++){ - cpVect v = cpvadd(p, cpvrotate(src[i], rot)); - - dst[i] = v; - l = cpfmin(l, v.x); - r = cpfmax(r, v.x); - b = cpfmin(b, v.y); - t = cpfmax(t, v.y); - } - - cpFloat radius = poly->r; - return cpBBNew(l - radius, b - radius, r + radius, t + radius); -} - -static void -cpPolyShapeTransformAxes(cpPolyShape *poly, cpVect p, cpVect rot) -{ - cpSplittingPlane *src = poly->planes; - cpSplittingPlane *dst = poly->tPlanes; - - for(int i=0; inumVerts; i++){ - cpVect n = cpvrotate(src[i].n, rot); - dst[i].n = n; - dst[i].d = cpvdot(p, n) + src[i].d; - } -} - -static cpBB -cpPolyShapeCacheData(cpPolyShape *poly, cpVect p, cpVect rot) -{ - cpPolyShapeTransformAxes(poly, p, rot); - cpBB bb = poly->shape.bb = cpPolyShapeTransformVerts(poly, p, rot); - - return bb; -} - -static void -cpPolyShapeDestroy(cpPolyShape *poly) -{ - cpfree(poly->verts); - cpfree(poly->planes); -} - -static void -cpPolyShapeNearestPointQuery(cpPolyShape *poly, cpVect p, cpNearestPointQueryInfo *info){ - int count = poly->numVerts; - cpSplittingPlane *planes = poly->tPlanes; - cpVect *verts = poly->tVerts; - cpFloat r = poly->r; - - cpVect v0 = verts[count - 1]; - cpFloat minDist = INFINITY; - cpVect closestPoint = cpvzero; - cpVect closestNormal = cpvzero; - cpBool outside = cpFalse; - - for(int i=0; i 0.0f) outside = cpTrue; - - cpVect v1 = verts[i]; - cpVect closest = cpClosetPointOnSegment(p, v0, v1); - - cpFloat dist = cpvdist(p, closest); - if(dist < minDist){ - minDist = dist; - closestPoint = closest; - closestNormal = planes[i].n; - } - - v0 = v1; - } - - cpFloat dist = (outside ? minDist : -minDist); - cpVect g = cpvmult(cpvsub(p, closestPoint), 1.0f/dist); - - info->shape = (cpShape *)poly; - info->p = cpvadd(closestPoint, cpvmult(g, r)); - info->d = dist - r; - - // Use the normal of the closest segment if the distance is small. - info->g = (minDist > MAGIC_EPSILON ? g : closestNormal); -} - -static void -cpPolyShapeSegmentQuery(cpPolyShape *poly, cpVect a, cpVect b, cpSegmentQueryInfo *info) -{ - cpSplittingPlane *axes = poly->tPlanes; - cpVect *verts = poly->tVerts; - int numVerts = poly->numVerts; - cpFloat r = poly->r; - - for(int i=0; i 0.0f) continue; - - cpFloat bn = cpvdot(b, n); - cpFloat t = d/(bn - an); - if(t < 0.0f || 1.0f < t) continue; - - cpVect point = cpvlerp(a, b, t); - cpFloat dt = -cpvcross(n, point); - cpFloat dtMin = -cpvcross(n, verts[(i - 1 + numVerts)%numVerts]); - cpFloat dtMax = -cpvcross(n, verts[i]); - - if(dtMin <= dt && dt <= dtMax){ - info->shape = (cpShape *)poly; - info->t = t; - info->n = n; - } - } - - // Also check against the beveled vertexes. - if(r > 0.0f){ - for(int i=0; ishape, verts[i], r, a, b, &circle_info); - if(circle_info.t < info->t) (*info) = circle_info; - } - } -} - -static const cpShapeClass polyClass = { - CP_POLY_SHAPE, - (cpShapeCacheDataImpl)cpPolyShapeCacheData, - (cpShapeDestroyImpl)cpPolyShapeDestroy, - (cpShapeNearestPointQueryImpl)cpPolyShapeNearestPointQuery, - (cpShapeSegmentQueryImpl)cpPolyShapeSegmentQuery, -}; - -cpBool -cpPolyValidate(const cpVect *verts, const int numVerts) -{ - for(int i=0; i 0.0f){ - return cpFalse; - } - } - - return cpTrue; -} - -int -cpPolyShapeGetNumVerts(const cpShape *shape) -{ - cpAssertHard(shape->klass == &polyClass, "Shape is not a poly shape."); - return ((cpPolyShape *)shape)->numVerts; -} - -cpVect -cpPolyShapeGetVert(const cpShape *shape, int idx) -{ - cpAssertHard(shape->klass == &polyClass, "Shape is not a poly shape."); - cpAssertHard(0 <= idx && idx < cpPolyShapeGetNumVerts(shape), "Index out of range."); - - return ((cpPolyShape *)shape)->verts[idx]; -} - -cpFloat -cpPolyShapeGetRadius(const cpShape *shape) -{ - cpAssertHard(shape->klass == &polyClass, "Shape is not a poly shape."); - return ((cpPolyShape *)shape)->r; -} - - -static void -setUpVerts(cpPolyShape *poly, int numVerts, const cpVect *verts, cpVect offset) -{ - // Fail if the user attempts to pass a concave poly, or a bad winding. - cpAssertHard(cpPolyValidate(verts, numVerts), "Polygon is concave or has a reversed winding. Consider using cpConvexHull() or CP_CONVEX_HULL()."); - - poly->numVerts = numVerts; - poly->verts = (cpVect *)cpcalloc(2*numVerts, sizeof(cpVect)); - poly->planes = (cpSplittingPlane *)cpcalloc(2*numVerts, sizeof(cpSplittingPlane)); - poly->tVerts = poly->verts + numVerts; - poly->tPlanes = poly->planes + numVerts; - - for(int i=0; iverts[i] = a; - poly->planes[i].n = n; - poly->planes[i].d = cpvdot(n, a); - } - - // TODO: Why did I add this? It duplicates work from above. - for(int i=0; iplanes[i] = cpSplittingPlaneNew(poly->verts[(i - 1 + numVerts)%numVerts], poly->verts[i]); - } -} - -cpPolyShape * -cpPolyShapeInit(cpPolyShape *poly, cpBody *body, int numVerts, const cpVect *verts, cpVect offset) -{ - return cpPolyShapeInit2(poly, body, numVerts, verts, offset, 0.0f); -} - -cpPolyShape * -cpPolyShapeInit2(cpPolyShape *poly, cpBody *body, int numVerts, const cpVect *verts, cpVect offset, cpFloat radius) -{ - setUpVerts(poly, numVerts, verts, offset); - cpShapeInit((cpShape *)poly, &polyClass, body); - poly->r = radius; - - return poly; -} - - -cpShape * -cpPolyShapeNew(cpBody *body, int numVerts, const cpVect *verts, cpVect offset) -{ - return cpPolyShapeNew2(body, numVerts, verts, offset, 0.0f); -} - -cpShape * -cpPolyShapeNew2(cpBody *body, int numVerts, const cpVect *verts, cpVect offset, cpFloat radius) -{ - return (cpShape *)cpPolyShapeInit2(cpPolyShapeAlloc(), body, numVerts, verts, offset, radius); -} - -cpPolyShape * -cpBoxShapeInit(cpPolyShape *poly, cpBody *body, cpFloat width, cpFloat height) -{ - cpFloat hw = width/2.0f; - cpFloat hh = height/2.0f; - - return cpBoxShapeInit2(poly, body, cpBBNew(-hw, -hh, hw, hh)); -} - -cpPolyShape * -cpBoxShapeInit2(cpPolyShape *poly, cpBody *body, cpBB box) -{ - return cpBoxShapeInit3(poly, body, box, 0.0f); -} - -cpPolyShape * -cpBoxShapeInit3(cpPolyShape *poly, cpBody *body, cpBB box, cpFloat radius) -{ - cpVect verts[] = { - cpv(box.l, box.b), - cpv(box.l, box.t), - cpv(box.r, box.t), - cpv(box.r, box.b), - }; - - return cpPolyShapeInit2(poly, body, 4, verts, cpvzero, radius); -} - -cpShape * -cpBoxShapeNew(cpBody *body, cpFloat width, cpFloat height) -{ - return (cpShape *)cpBoxShapeInit(cpPolyShapeAlloc(), body, width, height); -} - -cpShape * -cpBoxShapeNew2(cpBody *body, cpBB box) -{ - return (cpShape *)cpBoxShapeInit2(cpPolyShapeAlloc(), body, box); -} - -cpShape * -cpBoxShapeNew3(cpBody *body, cpBB box, cpFloat radius) -{ - return (cpShape *)cpBoxShapeInit3(cpPolyShapeAlloc(), body, box, radius); -} - -// Unsafe API (chipmunk_unsafe.h) - -void -cpPolyShapeSetVerts(cpShape *shape, int numVerts, cpVect *verts, cpVect offset) -{ - cpAssertHard(shape->klass == &polyClass, "Shape is not a poly shape."); - cpPolyShapeDestroy((cpPolyShape *)shape); - setUpVerts((cpPolyShape *)shape, numVerts, verts, offset); -} - -void -cpPolyShapeSetRadius(cpShape *shape, cpFloat radius) -{ - cpAssertHard(shape->klass == &polyClass, "Shape is not a poly shape."); - ((cpPolyShape *)shape)->r = radius; -} diff --git a/chipmunk/src/cpShape.c b/chipmunk/src/cpShape.c deleted file mode 100644 index 56f00b066a..0000000000 --- a/chipmunk/src/cpShape.c +++ /dev/null @@ -1,387 +0,0 @@ -/* Copyright (c) 2007 Scott Lembcke - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -#include "chipmunk_private.h" -#include "chipmunk_unsafe.h" - -#define CP_DefineShapeGetter(struct, type, member, name) \ -CP_DeclareShapeGetter(struct, type, name){ \ - cpAssertHard(shape->klass == &struct##Class, "shape is not a "#struct); \ - return ((struct *)shape)->member; \ -} - -static cpHashValue cpShapeIDCounter = 0; - -void -cpResetShapeIdCounter(void) -{ - cpShapeIDCounter = 0; -} - - -cpShape* -cpShapeInit(cpShape *shape, const cpShapeClass *klass, cpBody *body) -{ - shape->klass = klass; - - shape->hashid = cpShapeIDCounter; - cpShapeIDCounter++; - - shape->body = body; - shape->sensor = 0; - - shape->e = 0.0f; - shape->u = 0.0f; - shape->surface_v = cpvzero; - - shape->collision_type = 0; - shape->group = CP_NO_GROUP; - shape->layers = CP_ALL_LAYERS; - - shape->data = NULL; - - shape->space = NULL; - - shape->next = NULL; - shape->prev = NULL; - - return shape; -} - -void -cpShapeDestroy(cpShape *shape) -{ - if(shape->klass && shape->klass->destroy) shape->klass->destroy(shape); -} - -void -cpShapeFree(cpShape *shape) -{ - if(shape){ - cpShapeDestroy(shape); - cpfree(shape); - } -} - -void -cpShapeSetBody(cpShape *shape, cpBody *body) -{ - cpAssertHard(!cpShapeActive(shape), "You cannot change the body on an active shape. You must remove the shape from the space before changing the body."); - shape->body = body; -} - -cpBB -cpShapeCacheBB(cpShape *shape) -{ - cpBody *body = shape->body; - return cpShapeUpdate(shape, body->p, body->rot); -} - -cpBB -cpShapeUpdate(cpShape *shape, cpVect pos, cpVect rot) -{ - return (shape->bb = shape->klass->cacheData(shape, pos, rot)); -} - -cpBool -cpShapePointQuery(cpShape *shape, cpVect p){ - cpNearestPointQueryInfo info = {NULL, cpvzero, INFINITY, cpvzero}; - cpShapeNearestPointQuery(shape, p, &info); - - return (info.d < 0.0f); -} - -cpFloat -cpShapeNearestPointQuery(cpShape *shape, cpVect p, cpNearestPointQueryInfo *info) -{ - cpNearestPointQueryInfo blank = {NULL, cpvzero, INFINITY, cpvzero}; - if(info){ - (*info) = blank; - } else { - info = ␣ - } - - shape->klass->nearestPointQuery(shape, p, info); - return info->d; -} - - -cpBool -cpShapeSegmentQuery(cpShape *shape, cpVect a, cpVect b, cpSegmentQueryInfo *info){ - cpSegmentQueryInfo blank = {NULL, 1.0f, cpvzero}; - if(info){ - (*info) = blank; - } else { - info = ␣ - } - - cpNearestPointQueryInfo nearest; - shape->klass->nearestPointQuery(shape, a, &nearest); - if(nearest.d <= 0.0){ - info->shape = shape; - info->t = 0.0; - info->n = cpvnormalize(cpvsub(a, nearest.p)); - } else { - shape->klass->segmentQuery(shape, a, b, info); - } - - return (info->shape != NULL); -} - -cpCircleShape * -cpCircleShapeAlloc(void) -{ - return (cpCircleShape *)cpcalloc(1, sizeof(cpCircleShape)); -} - -static cpBB -cpCircleShapeCacheData(cpCircleShape *circle, cpVect p, cpVect rot) -{ - cpVect c = circle->tc = cpvadd(p, cpvrotate(circle->c, rot)); - return cpBBNewForCircle(c, circle->r); -} - -static void -cpCicleShapeNearestPointQuery(cpCircleShape *circle, cpVect p, cpNearestPointQueryInfo *info) -{ - cpVect delta = cpvsub(p, circle->tc); - cpFloat d = cpvlength(delta); - cpFloat r = circle->r; - - info->shape = (cpShape *)circle; - info->p = cpvadd(circle->tc, cpvmult(delta, r/d)); // TODO div/0 - info->d = d - r; - - // Use up for the gradient if the distance is very small. - info->g = (d > MAGIC_EPSILON ? cpvmult(delta, 1.0f/d) : cpv(0.0f, 1.0f)); -} - -static void -cpCircleShapeSegmentQuery(cpCircleShape *circle, cpVect a, cpVect b, cpSegmentQueryInfo *info) -{ - CircleSegmentQuery((cpShape *)circle, circle->tc, circle->r, a, b, info); -} - -static const cpShapeClass cpCircleShapeClass = { - CP_CIRCLE_SHAPE, - (cpShapeCacheDataImpl)cpCircleShapeCacheData, - NULL, - (cpShapeNearestPointQueryImpl)cpCicleShapeNearestPointQuery, - (cpShapeSegmentQueryImpl)cpCircleShapeSegmentQuery, -}; - -cpCircleShape * -cpCircleShapeInit(cpCircleShape *circle, cpBody *body, cpFloat radius, cpVect offset) -{ - circle->c = offset; - circle->r = radius; - - cpShapeInit((cpShape *)circle, &cpCircleShapeClass, body); - - return circle; -} - -cpShape * -cpCircleShapeNew(cpBody *body, cpFloat radius, cpVect offset) -{ - return (cpShape *)cpCircleShapeInit(cpCircleShapeAlloc(), body, radius, offset); -} - -CP_DefineShapeGetter(cpCircleShape, cpVect, c, Offset) -CP_DefineShapeGetter(cpCircleShape, cpFloat, r, Radius) - -cpSegmentShape * -cpSegmentShapeAlloc(void) -{ - return (cpSegmentShape *)cpcalloc(1, sizeof(cpSegmentShape)); -} - -static cpBB -cpSegmentShapeCacheData(cpSegmentShape *seg, cpVect p, cpVect rot) -{ - seg->ta = cpvadd(p, cpvrotate(seg->a, rot)); - seg->tb = cpvadd(p, cpvrotate(seg->b, rot)); - seg->tn = cpvrotate(seg->n, rot); - - cpFloat l,r,b,t; - - if(seg->ta.x < seg->tb.x){ - l = seg->ta.x; - r = seg->tb.x; - } else { - l = seg->tb.x; - r = seg->ta.x; - } - - if(seg->ta.y < seg->tb.y){ - b = seg->ta.y; - t = seg->tb.y; - } else { - b = seg->tb.y; - t = seg->ta.y; - } - - cpFloat rad = seg->r; - return cpBBNew(l - rad, b - rad, r + rad, t + rad); -} - -static void -cpSegmentShapeNearestPointQuery(cpSegmentShape *seg, cpVect p, cpNearestPointQueryInfo *info) -{ - cpVect closest = cpClosetPointOnSegment(p, seg->ta, seg->tb); - - cpVect delta = cpvsub(p, closest); - cpFloat d = cpvlength(delta); - cpFloat r = seg->r; - cpVect g = cpvmult(delta, 1.0f/d); - - info->shape = (cpShape *)seg; - info->p = (d ? cpvadd(closest, cpvmult(g, r)) : closest); - info->d = d - r; - - // Use the segment's normal if the distance is very small. - info->g = (d > MAGIC_EPSILON ? g : seg->n); -} - -static void -cpSegmentShapeSegmentQuery(cpSegmentShape *seg, cpVect a, cpVect b, cpSegmentQueryInfo *info) -{ - cpVect n = seg->tn; - cpFloat d = cpvdot(cpvsub(seg->ta, a), n); - cpFloat r = seg->r; - - cpVect flipped_n = (d > 0.0f ? cpvneg(n) : n); - cpVect seg_offset = cpvsub(cpvmult(flipped_n, r), a); - - // Make the endpoints relative to 'a' and move them by the thickness of the segment. - cpVect seg_a = cpvadd(seg->ta, seg_offset); - cpVect seg_b = cpvadd(seg->tb, seg_offset); - cpVect delta = cpvsub(b, a); - - if(cpvcross(delta, seg_a)*cpvcross(delta, seg_b) <= 0.0f){ - cpFloat d_offset = d + (d > 0.0f ? -r : r); - cpFloat ad = -d_offset; - cpFloat bd = cpvdot(delta, n) - d_offset; - - if(ad*bd < 0.0f){ - info->shape = (cpShape *)seg; - info->t = ad/(ad - bd); - info->n = flipped_n; - } - } else if(r != 0.0f){ - cpSegmentQueryInfo info1 = {NULL, 1.0f, cpvzero}; - cpSegmentQueryInfo info2 = {NULL, 1.0f, cpvzero}; - CircleSegmentQuery((cpShape *)seg, seg->ta, seg->r, a, b, &info1); - CircleSegmentQuery((cpShape *)seg, seg->tb, seg->r, a, b, &info2); - - if(info1.t < info2.t){ - (*info) = info1; - } else { - (*info) = info2; - } - } -} - -static const cpShapeClass cpSegmentShapeClass = { - CP_SEGMENT_SHAPE, - (cpShapeCacheDataImpl)cpSegmentShapeCacheData, - NULL, - (cpShapeNearestPointQueryImpl)cpSegmentShapeNearestPointQuery, - (cpShapeSegmentQueryImpl)cpSegmentShapeSegmentQuery, -}; - -cpSegmentShape * -cpSegmentShapeInit(cpSegmentShape *seg, cpBody *body, cpVect a, cpVect b, cpFloat r) -{ - seg->a = a; - seg->b = b; - seg->n = cpvperp(cpvnormalize(cpvsub(b, a))); - - seg->r = r; - - seg->a_tangent = cpvzero; - seg->b_tangent = cpvzero; - - cpShapeInit((cpShape *)seg, &cpSegmentShapeClass, body); - - return seg; -} - -cpShape* -cpSegmentShapeNew(cpBody *body, cpVect a, cpVect b, cpFloat r) -{ - return (cpShape *)cpSegmentShapeInit(cpSegmentShapeAlloc(), body, a, b, r); -} - -CP_DefineShapeGetter(cpSegmentShape, cpVect, a, A) -CP_DefineShapeGetter(cpSegmentShape, cpVect, b, B) -CP_DefineShapeGetter(cpSegmentShape, cpVect, n, Normal) -CP_DefineShapeGetter(cpSegmentShape, cpFloat, r, Radius) - -void -cpSegmentShapeSetNeighbors(cpShape *shape, cpVect prev, cpVect next) -{ - cpAssertHard(shape->klass == &cpSegmentShapeClass, "Shape is not a segment shape."); - cpSegmentShape *seg = (cpSegmentShape *)shape; - - seg->a_tangent = cpvsub(prev, seg->a); - seg->b_tangent = cpvsub(next, seg->b); -} - -// Unsafe API (chipmunk_unsafe.h) - -void -cpCircleShapeSetRadius(cpShape *shape, cpFloat radius) -{ - cpAssertHard(shape->klass == &cpCircleShapeClass, "Shape is not a circle shape."); - cpCircleShape *circle = (cpCircleShape *)shape; - - circle->r = radius; -} - -void -cpCircleShapeSetOffset(cpShape *shape, cpVect offset) -{ - cpAssertHard(shape->klass == &cpCircleShapeClass, "Shape is not a circle shape."); - cpCircleShape *circle = (cpCircleShape *)shape; - - circle->c = offset; -} - -void -cpSegmentShapeSetEndpoints(cpShape *shape, cpVect a, cpVect b) -{ - cpAssertHard(shape->klass == &cpSegmentShapeClass, "Shape is not a segment shape."); - cpSegmentShape *seg = (cpSegmentShape *)shape; - - seg->a = a; - seg->b = b; - seg->n = cpvperp(cpvnormalize(cpvsub(b, a))); -} - -void -cpSegmentShapeSetRadius(cpShape *shape, cpFloat radius) -{ - cpAssertHard(shape->klass == &cpSegmentShapeClass, "Shape is not a segment shape."); - cpSegmentShape *seg = (cpSegmentShape *)shape; - - seg->r = radius; -} diff --git a/chipmunk/src/cpSpace.c b/chipmunk/src/cpSpace.c deleted file mode 100644 index 111d0d2444..0000000000 --- a/chipmunk/src/cpSpace.c +++ /dev/null @@ -1,599 +0,0 @@ -/* Copyright (c) 2007 Scott Lembcke - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -#include -#include - -#include "chipmunk_private.h" - -//MARK: Contact Set Helpers - -// Equal function for arbiterSet. -static cpBool -arbiterSetEql(cpShape **shapes, cpArbiter *arb) -{ - cpShape *a = shapes[0]; - cpShape *b = shapes[1]; - - return ((a == arb->a && b == arb->b) || (b == arb->a && a == arb->b)); -} - -//MARK: Collision Handler Set HelperFunctions - -// Equals function for collisionHandlers. -static cpBool -handlerSetEql(cpCollisionHandler *check, cpCollisionHandler *pair) -{ - return ((check->a == pair->a && check->b == pair->b) || (check->b == pair->a && check->a == pair->b)); -} - -// Transformation function for collisionHandlers. -static void * -handlerSetTrans(cpCollisionHandler *handler, void *unused) -{ - cpCollisionHandler *copy = (cpCollisionHandler *)cpcalloc(1, sizeof(cpCollisionHandler)); - (*copy) = (*handler); - - return copy; -} - -//MARK: Misc Helper Funcs - -// Default collision functions. -static cpBool alwaysCollide(cpArbiter *arb, cpSpace *space, void *data){return 1;} -static void nothing(cpArbiter *arb, cpSpace *space, void *data){} - -// function to get the estimated velocity of a shape for the cpBBTree. -static cpVect shapeVelocityFunc(cpShape *shape){return shape->body->v;} - -static void freeWrap(void *ptr, void *unused){cpfree(ptr);} - -//MARK: Memory Management Functions - -cpSpace * -cpSpaceAlloc(void) -{ - return (cpSpace *)cpcalloc(1, sizeof(cpSpace)); -} - -cpCollisionHandler cpDefaultCollisionHandler = {0, 0, alwaysCollide, alwaysCollide, nothing, nothing, NULL}; - -cpSpace* -cpSpaceInit(cpSpace *space) -{ -#ifndef NDEBUG - static cpBool done = cpFalse; - if(!done){ - printf("Initializing cpSpace - Chipmunk v%s (Debug Enabled)\n", cpVersionString); - printf("Compile with -DNDEBUG defined to disable debug mode and runtime assertion checks\n"); - done = cpTrue; - } -#endif - - space->iterations = 10; - - space->gravity = cpvzero; - space->damping = 1.0f; - - space->collisionSlop = 0.1f; - space->collisionBias = cpfpow(1.0f - 0.1f, 60.0f); - space->collisionPersistence = 3; - - space->locked = 0; - space->stamp = 0; - - space->staticShapes = cpBBTreeNew((cpSpatialIndexBBFunc)cpShapeGetBB, NULL); - space->activeShapes = cpBBTreeNew((cpSpatialIndexBBFunc)cpShapeGetBB, space->staticShapes); - cpBBTreeSetVelocityFunc(space->activeShapes, (cpBBTreeVelocityFunc)shapeVelocityFunc); - - space->allocatedBuffers = cpArrayNew(0); - - space->bodies = cpArrayNew(0); - space->sleepingComponents = cpArrayNew(0); - space->rousedBodies = cpArrayNew(0); - - space->sleepTimeThreshold = INFINITY; - space->idleSpeedThreshold = 0.0f; - space->enableContactGraph = cpFalse; - - space->arbiters = cpArrayNew(0); - space->pooledArbiters = cpArrayNew(0); - - space->contactBuffersHead = NULL; - space->cachedArbiters = cpHashSetNew(0, (cpHashSetEqlFunc)arbiterSetEql); - - space->constraints = cpArrayNew(0); - - space->defaultHandler = cpDefaultCollisionHandler; - space->collisionHandlers = cpHashSetNew(0, (cpHashSetEqlFunc)handlerSetEql); - cpHashSetSetDefaultValue(space->collisionHandlers, &cpDefaultCollisionHandler); - - space->postStepCallbacks = cpArrayNew(0); - space->skipPostStep = cpFalse; - - cpBodyInitStatic(&space->_staticBody); - space->staticBody = &space->_staticBody; - - return space; -} - -cpSpace* -cpSpaceNew(void) -{ - return cpSpaceInit(cpSpaceAlloc()); -} - -void -cpSpaceDestroy(cpSpace *space) -{ - cpSpaceEachBody(space, (cpSpaceBodyIteratorFunc)cpBodyActivate, NULL); - - cpSpatialIndexFree(space->staticShapes); - cpSpatialIndexFree(space->activeShapes); - - cpArrayFree(space->bodies); - cpArrayFree(space->sleepingComponents); - cpArrayFree(space->rousedBodies); - - cpArrayFree(space->constraints); - - cpHashSetFree(space->cachedArbiters); - - cpArrayFree(space->arbiters); - cpArrayFree(space->pooledArbiters); - - if(space->allocatedBuffers){ - cpArrayFreeEach(space->allocatedBuffers, cpfree); - cpArrayFree(space->allocatedBuffers); - } - - if(space->postStepCallbacks){ - cpArrayFreeEach(space->postStepCallbacks, cpfree); - cpArrayFree(space->postStepCallbacks); - } - - if(space->collisionHandlers) cpHashSetEach(space->collisionHandlers, freeWrap, NULL); - cpHashSetFree(space->collisionHandlers); -} - -void -cpSpaceFree(cpSpace *space) -{ - if(space){ - cpSpaceDestroy(space); - cpfree(space); - } -} - -#define cpAssertSpaceUnlocked(space) \ - cpAssertHard(!space->locked, \ - "This operation cannot be done safely during a call to cpSpaceStep() or during a query. " \ - "Put these calls into a post-step callback." \ - ); - -//MARK: Collision Handler Function Management - -void -cpSpaceAddCollisionHandler( - cpSpace *space, - cpCollisionType a, cpCollisionType b, - cpCollisionBeginFunc begin, - cpCollisionPreSolveFunc preSolve, - cpCollisionPostSolveFunc postSolve, - cpCollisionSeparateFunc separate, - void *data -){ - cpAssertSpaceUnlocked(space); - - // Remove any old function so the new one will get added. - cpSpaceRemoveCollisionHandler(space, a, b); - - cpCollisionHandler handler = { - a, b, - begin ? begin : alwaysCollide, - preSolve ? preSolve : alwaysCollide, - postSolve ? postSolve : nothing, - separate ? separate : nothing, - data - }; - - cpHashSetInsert(space->collisionHandlers, CP_HASH_PAIR(a, b), &handler, NULL, (cpHashSetTransFunc)handlerSetTrans); -} - -void -cpSpaceRemoveCollisionHandler(cpSpace *space, cpCollisionType a, cpCollisionType b) -{ - cpAssertSpaceUnlocked(space); - - struct { cpCollisionType a, b; } ids = {a, b}; - cpCollisionHandler *old_handler = (cpCollisionHandler *) cpHashSetRemove(space->collisionHandlers, CP_HASH_PAIR(a, b), &ids); - cpfree(old_handler); -} - -void -cpSpaceSetDefaultCollisionHandler( - cpSpace *space, - cpCollisionBeginFunc begin, - cpCollisionPreSolveFunc preSolve, - cpCollisionPostSolveFunc postSolve, - cpCollisionSeparateFunc separate, - void *data -){ - cpAssertSpaceUnlocked(space); - - cpCollisionHandler handler = { - 0, 0, - begin ? begin : alwaysCollide, - preSolve ? preSolve : alwaysCollide, - postSolve ? postSolve : nothing, - separate ? separate : nothing, - data - }; - - space->defaultHandler = handler; - cpHashSetSetDefaultValue(space->collisionHandlers, &space->defaultHandler); -} - -//MARK: Body, Shape, and Joint Management -cpShape * -cpSpaceAddShape(cpSpace *space, cpShape *shape) -{ - cpBody *body = shape->body; - if(cpBodyIsStatic(body)) return cpSpaceAddStaticShape(space, shape); - - cpAssertHard(shape->space != space, "You have already added this shape to this space. You must not add it a second time."); - cpAssertHard(!shape->space, "You have already added this shape to another space. You cannot add it to a second."); - cpAssertSpaceUnlocked(space); - - cpBodyActivate(body); - cpBodyAddShape(body, shape); - - cpShapeUpdate(shape, body->p, body->rot); - cpSpatialIndexInsert(space->activeShapes, shape, shape->hashid); - shape->space = space; - - return shape; -} - -cpShape * -cpSpaceAddStaticShape(cpSpace *space, cpShape *shape) -{ - cpAssertHard(shape->space != space, "You have already added this shape to this space. You must not add it a second time."); - cpAssertHard(!shape->space, "You have already added this shape to another space. You cannot add it to a second."); - cpAssertHard(cpBodyIsRogue(shape->body), "You are adding a static shape to a dynamic body. Did you mean to attach it to a static or rogue body? See the documentation for more information."); - cpAssertSpaceUnlocked(space); - - cpBody *body = shape->body; - cpBodyAddShape(body, shape); - cpShapeUpdate(shape, body->p, body->rot); - cpSpatialIndexInsert(space->staticShapes, shape, shape->hashid); - shape->space = space; - - return shape; -} - -cpBody * -cpSpaceAddBody(cpSpace *space, cpBody *body) -{ - cpAssertHard(!cpBodyIsStatic(body), "Do not add static bodies to a space. Static bodies do not move and should not be simulated."); - cpAssertHard(body->space != space, "You have already added this body to this space. You must not add it a second time."); - cpAssertHard(!body->space, "You have already added this body to another space. You cannot add it to a second."); - cpAssertSpaceUnlocked(space); - - cpArrayPush(space->bodies, body); - body->space = space; - - return body; -} - -cpConstraint * -cpSpaceAddConstraint(cpSpace *space, cpConstraint *constraint) -{ - cpAssertHard(constraint->space != space, "You have already added this constraint to this space. You must not add it a second time."); - cpAssertHard(!constraint->space, "You have already added this constraint to another space. You cannot add it to a second."); - cpAssertHard(constraint->a && constraint->b, "Constraint is attached to a NULL body."); - cpAssertSpaceUnlocked(space); - - cpBodyActivate(constraint->a); - cpBodyActivate(constraint->b); - cpArrayPush(space->constraints, constraint); - - // Push onto the heads of the bodies' constraint lists - cpBody *a = constraint->a, *b = constraint->b; - constraint->next_a = a->constraintList; a->constraintList = constraint; - constraint->next_b = b->constraintList; b->constraintList = constraint; - constraint->space = space; - - return constraint; -} - -struct arbiterFilterContext { - cpSpace *space; - cpBody *body; - cpShape *shape; -}; - -static cpBool -cachedArbitersFilter(cpArbiter *arb, struct arbiterFilterContext *context) -{ - cpShape *shape = context->shape; - cpBody *body = context->body; - - - // Match on the filter shape, or if it's NULL the filter body - if( - (body == arb->body_a && (shape == arb->a || shape == NULL)) || - (body == arb->body_b && (shape == arb->b || shape == NULL)) - ){ - // Call separate when removing shapes. - if(shape && arb->state != cpArbiterStateCached) cpArbiterCallSeparate(arb, context->space); - - cpArbiterUnthread(arb); - cpArrayDeleteObj(context->space->arbiters, arb); - cpArrayPush(context->space->pooledArbiters, arb); - - return cpFalse; - } - - return cpTrue; -} - -void -cpSpaceFilterArbiters(cpSpace *space, cpBody *body, cpShape *filter) -{ - cpSpaceLock(space); { - struct arbiterFilterContext context = {space, body, filter}; - cpHashSetFilter(space->cachedArbiters, (cpHashSetFilterFunc)cachedArbitersFilter, &context); - } cpSpaceUnlock(space, cpTrue); -} - -void -cpSpaceRemoveShape(cpSpace *space, cpShape *shape) -{ - cpBody *body = shape->body; - if(cpBodyIsStatic(body)){ - cpSpaceRemoveStaticShape(space, shape); - } else { - cpAssertHard(cpSpaceContainsShape(space, shape), "Cannot remove a shape that was not added to the space. (Removed twice maybe?)"); - cpAssertSpaceUnlocked(space); - - cpBodyActivate(body); - cpBodyRemoveShape(body, shape); - cpSpaceFilterArbiters(space, body, shape); - cpSpatialIndexRemove(space->activeShapes, shape, shape->hashid); - shape->space = NULL; - } -} - -void -cpSpaceRemoveStaticShape(cpSpace *space, cpShape *shape) -{ - cpAssertHard(cpSpaceContainsShape(space, shape), "Cannot remove a static or sleeping shape that was not added to the space. (Removed twice maybe?)"); - cpAssertSpaceUnlocked(space); - - cpBody *body = shape->body; - if(cpBodyIsStatic(body)) cpBodyActivateStatic(body, shape); - cpBodyRemoveShape(body, shape); - cpSpaceFilterArbiters(space, body, shape); - cpSpatialIndexRemove(space->staticShapes, shape, shape->hashid); - shape->space = NULL; -} - -void -cpSpaceRemoveBody(cpSpace *space, cpBody *body) -{ - cpAssertHard(cpSpaceContainsBody(space, body), "Cannot remove a body that was not added to the space. (Removed twice maybe?)"); - cpAssertSpaceUnlocked(space); - - cpBodyActivate(body); -// cpSpaceFilterArbiters(space, body, NULL); - cpArrayDeleteObj(space->bodies, body); - body->space = NULL; -} - -void -cpSpaceRemoveConstraint(cpSpace *space, cpConstraint *constraint) -{ - cpAssertHard(cpSpaceContainsConstraint(space, constraint), "Cannot remove a constraint that was not added to the space. (Removed twice maybe?)"); - cpAssertSpaceUnlocked(space); - - cpBodyActivate(constraint->a); - cpBodyActivate(constraint->b); - cpArrayDeleteObj(space->constraints, constraint); - - cpBodyRemoveConstraint(constraint->a, constraint); - cpBodyRemoveConstraint(constraint->b, constraint); - constraint->space = NULL; -} - -cpBool cpSpaceContainsShape(cpSpace *space, cpShape *shape) -{ - return (shape->space == space); -} - -cpBool cpSpaceContainsBody(cpSpace *space, cpBody *body) -{ - return (body->space == space); -} - -cpBool cpSpaceContainsConstraint(cpSpace *space, cpConstraint *constraint) -{ - return (constraint->space == space); -} - -//MARK: Static/rogue body conversion. - -void -cpSpaceConvertBodyToStatic(cpSpace *space, cpBody *body) -{ - cpAssertHard(!cpBodyIsStatic(body), "Body is already static."); - cpAssertHard(cpBodyIsRogue(body), "Remove the body from the space before calling this function."); - cpAssertSpaceUnlocked(space); - - cpBodySetMass(body, INFINITY); - cpBodySetMoment(body, INFINITY); - - cpBodySetVel(body, cpvzero); - cpBodySetAngVel(body, 0.0f); - - body->node.idleTime = INFINITY; - CP_BODY_FOREACH_SHAPE(body, shape){ - cpSpatialIndexRemove(space->activeShapes, shape, shape->hashid); - cpSpatialIndexInsert(space->staticShapes, shape, shape->hashid); - } -} - -void -cpSpaceConvertBodyToDynamic(cpSpace *space, cpBody *body, cpFloat m, cpFloat i) -{ - cpAssertHard(cpBodyIsStatic(body), "Body is already dynamic."); - cpAssertSpaceUnlocked(space); - - cpBodyActivateStatic(body, NULL); - - cpBodySetMass(body, m); - cpBodySetMoment(body, i); - - body->node.idleTime = 0.0f; - CP_BODY_FOREACH_SHAPE(body, shape){ - cpSpatialIndexRemove(space->staticShapes, shape, shape->hashid); - cpSpatialIndexInsert(space->activeShapes, shape, shape->hashid); - } -} - -//MARK: Iteration - -void -cpSpaceEachBody(cpSpace *space, cpSpaceBodyIteratorFunc func, void *data) -{ - cpSpaceLock(space); { - cpArray *bodies = space->bodies; - - for(int i=0; inum; i++){ - func((cpBody *)bodies->arr[i], data); - } - - cpArray *components = space->sleepingComponents; - for(int i=0; inum; i++){ - cpBody *root = (cpBody *)components->arr[i]; - - cpBody *body = root; - while(body){ - cpBody *next = body->node.next; - func(body, data); - body = next; - } - } - } cpSpaceUnlock(space, cpTrue); -} - -typedef struct spaceShapeContext { - cpSpaceShapeIteratorFunc func; - void *data; -} spaceShapeContext; - -static void -spaceEachShapeIterator(cpShape *shape, spaceShapeContext *context) -{ - context->func(shape, context->data); -} - -void -cpSpaceEachShape(cpSpace *space, cpSpaceShapeIteratorFunc func, void *data) -{ - cpSpaceLock(space); { - spaceShapeContext context = {func, data}; - cpSpatialIndexEach(space->activeShapes, (cpSpatialIndexIteratorFunc)spaceEachShapeIterator, &context); - cpSpatialIndexEach(space->staticShapes, (cpSpatialIndexIteratorFunc)spaceEachShapeIterator, &context); - } cpSpaceUnlock(space, cpTrue); -} - -void -cpSpaceEachConstraint(cpSpace *space, cpSpaceConstraintIteratorFunc func, void *data) -{ - cpSpaceLock(space); { - cpArray *constraints = space->constraints; - - for(int i=0; inum; i++){ - func((cpConstraint *)constraints->arr[i], data); - } - } cpSpaceUnlock(space, cpTrue); -} - -//MARK: Spatial Index Management - -static void -updateBBCache(cpShape *shape, void *unused) -{ - cpBody *body = shape->body; - cpShapeUpdate(shape, body->p, body->rot); -} - -void -cpSpaceReindexStatic(cpSpace *space) -{ - cpAssertHard(!space->locked, "You cannot manually reindex objects while the space is locked. Wait until the current query or step is complete."); - - cpSpatialIndexEach(space->staticShapes, (cpSpatialIndexIteratorFunc)&updateBBCache, NULL); - cpSpatialIndexReindex(space->staticShapes); -} - -void -cpSpaceReindexShape(cpSpace *space, cpShape *shape) -{ - cpAssertHard(!space->locked, "You cannot manually reindex objects while the space is locked. Wait until the current query or step is complete."); - - cpBody *body = shape->body; - cpShapeUpdate(shape, body->p, body->rot); - - // attempt to rehash the shape in both hashes - cpSpatialIndexReindexObject(space->activeShapes, shape, shape->hashid); - cpSpatialIndexReindexObject(space->staticShapes, shape, shape->hashid); -} - -void -cpSpaceReindexShapesForBody(cpSpace *space, cpBody *body) -{ - CP_BODY_FOREACH_SHAPE(body, shape) cpSpaceReindexShape(space, shape); -} - - -static void -copyShapes(cpShape *shape, cpSpatialIndex *index) -{ - cpSpatialIndexInsert(index, shape, shape->hashid); -} - -void -cpSpaceUseSpatialHash(cpSpace *space, cpFloat dim, int count) -{ - cpSpatialIndex *staticShapes = cpSpaceHashNew(dim, count, (cpSpatialIndexBBFunc)cpShapeGetBB, NULL); - cpSpatialIndex *activeShapes = cpSpaceHashNew(dim, count, (cpSpatialIndexBBFunc)cpShapeGetBB, staticShapes); - - cpSpatialIndexEach(space->staticShapes, (cpSpatialIndexIteratorFunc)copyShapes, staticShapes); - cpSpatialIndexEach(space->activeShapes, (cpSpatialIndexIteratorFunc)copyShapes, activeShapes); - - cpSpatialIndexFree(space->staticShapes); - cpSpatialIndexFree(space->activeShapes); - - space->staticShapes = staticShapes; - space->activeShapes = activeShapes; -} diff --git a/chipmunk/src/cpSpaceComponent.c b/chipmunk/src/cpSpaceComponent.c deleted file mode 100644 index fe3ac84ebb..0000000000 --- a/chipmunk/src/cpSpaceComponent.c +++ /dev/null @@ -1,357 +0,0 @@ -/* Copyright (c) 2007 Scott Lembcke - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -#include - -#include "chipmunk_private.h" - -//MARK: Sleeping Functions - -void -cpSpaceActivateBody(cpSpace *space, cpBody *body) -{ - cpAssertHard(!cpBodyIsRogue(body), "Internal error: Attempting to activate a rogue body."); - - if(space->locked){ - // cpSpaceActivateBody() is called again once the space is unlocked - if(!cpArrayContains(space->rousedBodies, body)) cpArrayPush(space->rousedBodies, body); - } else { - cpAssertSoft(body->node.root == NULL && body->node.next == NULL, "Internal error: Activating body non-NULL node pointers."); - cpArrayPush(space->bodies, body); - - CP_BODY_FOREACH_SHAPE(body, shape){ - cpSpatialIndexRemove(space->staticShapes, shape, shape->hashid); - cpSpatialIndexInsert(space->activeShapes, shape, shape->hashid); - } - - CP_BODY_FOREACH_ARBITER(body, arb){ - cpBody *bodyA = arb->body_a; - - // Arbiters are shared between two bodies that are always woken up together. - // You only want to restore the arbiter once, so bodyA is arbitrarily chosen to own the arbiter. - // The edge case is when static bodies are involved as the static bodies never actually sleep. - // If the static body is bodyB then all is good. If the static body is bodyA, that can easily be checked. - if(body == bodyA || cpBodyIsStatic(bodyA)){ - int numContacts = arb->numContacts; - cpContact *contacts = arb->contacts; - - // Restore contact values back to the space's contact buffer memory - arb->contacts = cpContactBufferGetArray(space); - memcpy(arb->contacts, contacts, numContacts*sizeof(cpContact)); - cpSpacePushContacts(space, numContacts); - - // Reinsert the arbiter into the arbiter cache - cpShape *a = arb->a, *b = arb->b; - cpShape *shape_pair[] = {a, b}; - cpHashValue arbHashID = CP_HASH_PAIR((cpHashValue)a, (cpHashValue)b); - cpHashSetInsert(space->cachedArbiters, arbHashID, shape_pair, arb, NULL); - - // Update the arbiter's state - arb->stamp = space->stamp; - arb->handler = cpSpaceLookupHandler(space, a->collision_type, b->collision_type); - cpArrayPush(space->arbiters, arb); - - cpfree(contacts); - } - } - - CP_BODY_FOREACH_CONSTRAINT(body, constraint){ - cpBody *bodyA = constraint->a; - if(body == bodyA || cpBodyIsStatic(bodyA)) cpArrayPush(space->constraints, constraint); - } - } -} - -static void -cpSpaceDeactivateBody(cpSpace *space, cpBody *body) -{ - cpAssertHard(!cpBodyIsRogue(body), "Internal error: Attempting to deactivate a rouge body."); - - cpArrayDeleteObj(space->bodies, body); - - CP_BODY_FOREACH_SHAPE(body, shape){ - cpSpatialIndexRemove(space->activeShapes, shape, shape->hashid); - cpSpatialIndexInsert(space->staticShapes, shape, shape->hashid); - } - - CP_BODY_FOREACH_ARBITER(body, arb){ - cpBody *bodyA = arb->body_a; - if(body == bodyA || cpBodyIsStatic(bodyA)){ - cpSpaceUncacheArbiter(space, arb); - - // Save contact values to a new block of memory so they won't time out - size_t bytes = arb->numContacts*sizeof(cpContact); - cpContact *contacts = (cpContact *)cpcalloc(1, bytes); - memcpy(contacts, arb->contacts, bytes); - arb->contacts = contacts; - } - } - - CP_BODY_FOREACH_CONSTRAINT(body, constraint){ - cpBody *bodyA = constraint->a; - if(body == bodyA || cpBodyIsStatic(bodyA)) cpArrayDeleteObj(space->constraints, constraint); - } -} - -static inline cpBody * -ComponentRoot(cpBody *body) -{ - return (body ? body->node.root : NULL); -} - -static inline void -ComponentActivate(cpBody *root) -{ - if(!root || !cpBodyIsSleeping(root)) return; - cpAssertHard(!cpBodyIsRogue(root), "Internal Error: ComponentActivate() called on a rogue body."); - - cpSpace *space = root->space; - cpBody *body = root; - while(body){ - cpBody *next = body->node.next; - - body->node.idleTime = 0.0f; - body->node.root = NULL; - body->node.next = NULL; - cpSpaceActivateBody(space, body); - - body = next; - } - - cpArrayDeleteObj(space->sleepingComponents, root); -} - -void -cpBodyActivate(cpBody *body) -{ - if(!cpBodyIsRogue(body)){ - body->node.idleTime = 0.0f; - ComponentActivate(ComponentRoot(body)); - } - - CP_BODY_FOREACH_ARBITER(body, arb){ - // Reset the idle timer of things the body is touching as well. - // That way things don't get left hanging in the air. - cpBody *other = (arb->body_a == body ? arb->body_b : arb->body_a); - if(!cpBodyIsStatic(other)) other->node.idleTime = 0.0f; - } -} - -void -cpBodyActivateStatic(cpBody *body, cpShape *filter) -{ - cpAssertHard(cpBodyIsStatic(body), "cpBodyActivateStatic() called on a non-static body."); - - CP_BODY_FOREACH_ARBITER(body, arb){ - if(!filter || filter == arb->a || filter == arb->b){ - cpBodyActivate(arb->body_a == body ? arb->body_b : arb->body_a); - } - } - - // TODO should also activate joints? -} - -static inline void -cpBodyPushArbiter(cpBody *body, cpArbiter *arb) -{ - cpAssertSoft(cpArbiterThreadForBody(arb, body)->next == NULL, "Internal Error: Dangling contact graph pointers detected. (A)"); - cpAssertSoft(cpArbiterThreadForBody(arb, body)->prev == NULL, "Internal Error: Dangling contact graph pointers detected. (B)"); - - cpArbiter *next = body->arbiterList; - cpAssertSoft(next == NULL || cpArbiterThreadForBody(next, body)->prev == NULL, "Internal Error: Dangling contact graph pointers detected. (C)"); - cpArbiterThreadForBody(arb, body)->next = next; - - if(next) cpArbiterThreadForBody(next, body)->prev = arb; - body->arbiterList = arb; -} - -static inline void -ComponentAdd(cpBody *root, cpBody *body){ - body->node.root = root; - - if(body != root){ - body->node.next = root->node.next; - root->node.next = body; - } -} - -static inline void -FloodFillComponent(cpBody *root, cpBody *body) -{ - // Rogue bodies cannot be put to sleep and prevent bodies they are touching from sleepining anyway. - // Static bodies (which are a type of rogue body) are effectively sleeping all the time. - if(!cpBodyIsRogue(body)){ - cpBody *other_root = ComponentRoot(body); - if(other_root == NULL){ - ComponentAdd(root, body); - CP_BODY_FOREACH_ARBITER(body, arb) FloodFillComponent(root, (body == arb->body_a ? arb->body_b : arb->body_a)); - CP_BODY_FOREACH_CONSTRAINT(body, constraint) FloodFillComponent(root, (body == constraint->a ? constraint->b : constraint->a)); - } else { - cpAssertSoft(other_root == root, "Internal Error: Inconsistency dectected in the contact graph."); - } - } -} - -static inline cpBool -ComponentActive(cpBody *root, cpFloat threshold) -{ - CP_BODY_FOREACH_COMPONENT(root, body){ - if(body->node.idleTime < threshold) return cpTrue; - } - - return cpFalse; -} - -void -cpSpaceProcessComponents(cpSpace *space, cpFloat dt) -{ - cpBool sleep = (space->sleepTimeThreshold != INFINITY); - cpArray *bodies = space->bodies; - -#ifndef NDEBUG - for(int i=0; inum; i++){ - cpBody *body = (cpBody*)bodies->arr[i]; - - cpAssertSoft(body->node.next == NULL, "Internal Error: Dangling next pointer detected in contact graph."); - cpAssertSoft(body->node.root == NULL, "Internal Error: Dangling root pointer detected in contact graph."); - } -#endif - - // Calculate the kinetic energy of all the bodies. - if(sleep){ - cpFloat dv = space->idleSpeedThreshold; - cpFloat dvsq = (dv ? dv*dv : cpvlengthsq(space->gravity)*dt*dt); - - // update idling and reset component nodes - for(int i=0; inum; i++){ - cpBody *body = (cpBody*)bodies->arr[i]; - - // Need to deal with infinite mass objects - cpFloat keThreshold = (dvsq ? body->m*dvsq : 0.0f); - body->node.idleTime = (cpBodyKineticEnergy(body) > keThreshold ? 0.0f : body->node.idleTime + dt); - } - } - - // Awaken any sleeping bodies found and then push arbiters to the bodies' lists. - cpArray *arbiters = space->arbiters; - for(int i=0, count=arbiters->num; iarr[i]; - cpBody *a = arb->body_a, *b = arb->body_b; - - if(sleep){ - if((cpBodyIsRogue(b) && !cpBodyIsStatic(b)) || cpBodyIsSleeping(a)) cpBodyActivate(a); - if((cpBodyIsRogue(a) && !cpBodyIsStatic(a)) || cpBodyIsSleeping(b)) cpBodyActivate(b); - } - - cpBodyPushArbiter(a, arb); - cpBodyPushArbiter(b, arb); - } - - if(sleep){ - // Bodies should be held active if connected by a joint to a non-static rouge body. - cpArray *constraints = space->constraints; - for(int i=0; inum; i++){ - cpConstraint *constraint = (cpConstraint *)constraints->arr[i]; - cpBody *a = constraint->a, *b = constraint->b; - - if(cpBodyIsRogue(b) && !cpBodyIsStatic(b)) cpBodyActivate(a); - if(cpBodyIsRogue(a) && !cpBodyIsStatic(a)) cpBodyActivate(b); - } - - // Generate components and deactivate sleeping ones - for(int i=0; inum;){ - cpBody *body = (cpBody*)bodies->arr[i]; - - if(ComponentRoot(body) == NULL){ - // Body not in a component yet. Perform a DFS to flood fill mark - // the component in the contact graph using this body as the root. - FloodFillComponent(body, body); - - // Check if the component should be put to sleep. - if(!ComponentActive(body, space->sleepTimeThreshold)){ - cpArrayPush(space->sleepingComponents, body); - CP_BODY_FOREACH_COMPONENT(body, other) cpSpaceDeactivateBody(space, other); - - // cpSpaceDeactivateBody() removed the current body from the list. - // Skip incrementing the index counter. - continue; - } - } - - i++; - - // Only sleeping bodies retain their component node pointers. - body->node.root = NULL; - body->node.next = NULL; - } - } -} - -void -cpBodySleep(cpBody *body) -{ - cpBodySleepWithGroup(body, NULL); -} - -void -cpBodySleepWithGroup(cpBody *body, cpBody *group){ - cpAssertHard(!cpBodyIsRogue(body), "Rogue (and static) bodies cannot be put to sleep."); - - cpSpace *space = body->space; - cpAssertHard(!space->locked, "Bodies cannot be put to sleep during a query or a call to cpSpaceStep(). Put these calls into a post-step callback."); - cpAssertHard(group == NULL || cpBodyIsSleeping(group), "Cannot use a non-sleeping body as a group identifier."); - - if(cpBodyIsSleeping(body)){ - cpAssertHard(ComponentRoot(body) == ComponentRoot(group), "The body is already sleeping and it's group cannot be reassigned."); - return; - } - - CP_BODY_FOREACH_SHAPE(body, shape) cpShapeUpdate(shape, body->p, body->rot); - cpSpaceDeactivateBody(space, body); - - if(group){ - cpBody *root = ComponentRoot(group); - - cpComponentNode node = {root, root->node.next, 0.0f}; - body->node = node; - - root->node.next = body; - } else { - cpComponentNode node = {body, NULL, 0.0f}; - body->node = node; - - cpArrayPush(space->sleepingComponents, body); - } - - cpArrayDeleteObj(space->bodies, body); -} - -static void -activateTouchingHelper(cpShape *shape, cpContactPointSet *points, cpShape *other){ - cpBodyActivate(shape->body); -} - -void -cpSpaceActivateShapesTouchingShape(cpSpace *space, cpShape *shape){ - if(space->sleepTimeThreshold != INFINITY){ - cpSpaceShapeQuery(space, shape, (cpSpaceShapeQueryFunc)activateTouchingHelper, shape); - } -} diff --git a/chipmunk/src/cpSpaceHash.c b/chipmunk/src/cpSpaceHash.c deleted file mode 100644 index 9e20715e69..0000000000 --- a/chipmunk/src/cpSpaceHash.c +++ /dev/null @@ -1,634 +0,0 @@ -/* Copyright (c) 2007 Scott Lembcke - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -#include "chipmunk_private.h" -#include "prime.h" - -typedef struct cpSpaceHashBin cpSpaceHashBin; -typedef struct cpHandle cpHandle; - -struct cpSpaceHash { - cpSpatialIndex spatialIndex; - - int numcells; - cpFloat celldim; - - cpSpaceHashBin **table; - cpHashSet *handleSet; - - cpSpaceHashBin *pooledBins; - cpArray *pooledHandles; - cpArray *allocatedBuffers; - - cpTimestamp stamp; -}; - - -//MARK: Handle Functions - -struct cpHandle { - void *obj; - int retain; - cpTimestamp stamp; -}; - -static cpHandle* -cpHandleInit(cpHandle *hand, void *obj) -{ - hand->obj = obj; - hand->retain = 0; - hand->stamp = 0; - - return hand; -} - -static inline void cpHandleRetain(cpHandle *hand){hand->retain++;} - -static inline void -cpHandleRelease(cpHandle *hand, cpArray *pooledHandles) -{ - hand->retain--; - if(hand->retain == 0) cpArrayPush(pooledHandles, hand); -} - -static int handleSetEql(void *obj, cpHandle *hand){return (obj == hand->obj);} - -static void * -handleSetTrans(void *obj, cpSpaceHash *hash) -{ - if(hash->pooledHandles->num == 0){ - // handle pool is exhausted, make more - int count = CP_BUFFER_BYTES/sizeof(cpHandle); - cpAssertHard(count, "Internal Error: Buffer size is too small."); - - cpHandle *buffer = (cpHandle *)cpcalloc(1, CP_BUFFER_BYTES); - cpArrayPush(hash->allocatedBuffers, buffer); - - for(int i=0; ipooledHandles, buffer + i); - } - - cpHandle *hand = cpHandleInit((cpHandle *)cpArrayPop(hash->pooledHandles), obj); - cpHandleRetain(hand); - - return hand; -} - -//MARK: Bin Functions - -struct cpSpaceHashBin { - cpHandle *handle; - cpSpaceHashBin *next; -}; - -static inline void -recycleBin(cpSpaceHash *hash, cpSpaceHashBin *bin) -{ - bin->next = hash->pooledBins; - hash->pooledBins = bin; -} - -static inline void -clearTableCell(cpSpaceHash *hash, int idx) -{ - cpSpaceHashBin *bin = hash->table[idx]; - while(bin){ - cpSpaceHashBin *next = bin->next; - - cpHandleRelease(bin->handle, hash->pooledHandles); - recycleBin(hash, bin); - - bin = next; - } - - hash->table[idx] = NULL; -} - -static void -clearTable(cpSpaceHash *hash) -{ - for(int i=0; inumcells; i++) clearTableCell(hash, i); -} - -// Get a recycled or new bin. -static inline cpSpaceHashBin * -getEmptyBin(cpSpaceHash *hash) -{ - cpSpaceHashBin *bin = hash->pooledBins; - - if(bin){ - hash->pooledBins = bin->next; - return bin; - } else { - // Pool is exhausted, make more - int count = CP_BUFFER_BYTES/sizeof(cpSpaceHashBin); - cpAssertHard(count, "Internal Error: Buffer size is too small."); - - cpSpaceHashBin *buffer = (cpSpaceHashBin *)cpcalloc(1, CP_BUFFER_BYTES); - cpArrayPush(hash->allocatedBuffers, buffer); - - // push all but the first one, return the first instead - for(int i=1; itable); - - hash->numcells = numcells; - hash->table = (cpSpaceHashBin **)cpcalloc(numcells, sizeof(cpSpaceHashBin *)); -} - -static inline cpSpatialIndexClass *Klass(); - -cpSpatialIndex * -cpSpaceHashInit(cpSpaceHash *hash, cpFloat celldim, int numcells, cpSpatialIndexBBFunc bbfunc, cpSpatialIndex *staticIndex) -{ - cpSpatialIndexInit((cpSpatialIndex *)hash, Klass(), bbfunc, staticIndex); - - cpSpaceHashAllocTable(hash, next_prime(numcells)); - hash->celldim = celldim; - - hash->handleSet = cpHashSetNew(0, (cpHashSetEqlFunc)handleSetEql); - - hash->pooledHandles = cpArrayNew(0); - - hash->pooledBins = NULL; - hash->allocatedBuffers = cpArrayNew(0); - - hash->stamp = 1; - - return (cpSpatialIndex *)hash; -} - -cpSpatialIndex * -cpSpaceHashNew(cpFloat celldim, int cells, cpSpatialIndexBBFunc bbfunc, cpSpatialIndex *staticIndex) -{ - return cpSpaceHashInit(cpSpaceHashAlloc(), celldim, cells, bbfunc, staticIndex); -} - -static void -cpSpaceHashDestroy(cpSpaceHash *hash) -{ - if(hash->table) clearTable(hash); - cpfree(hash->table); - - cpHashSetFree(hash->handleSet); - - cpArrayFreeEach(hash->allocatedBuffers, cpfree); - cpArrayFree(hash->allocatedBuffers); - cpArrayFree(hash->pooledHandles); -} - -//MARK: Helper Functions - -static inline cpBool -containsHandle(cpSpaceHashBin *bin, cpHandle *hand) -{ - while(bin){ - if(bin->handle == hand) return cpTrue; - bin = bin->next; - } - - return cpFalse; -} - -// The hash function itself. -static inline cpHashValue -hash_func(cpHashValue x, cpHashValue y, cpHashValue n) -{ - return (x*1640531513ul ^ y*2654435789ul) % n; -} - -// Much faster than (int)floor(f) -// Profiling showed floor() to be a sizable performance hog -static inline int -floor_int(cpFloat f) -{ - int i = (int)f; - return (f < 0.0f && f != i ? i - 1 : i); -} - -static inline void -hashHandle(cpSpaceHash *hash, cpHandle *hand, cpBB bb) -{ - // Find the dimensions in cell coordinates. - cpFloat dim = hash->celldim; - int l = floor_int(bb.l/dim); // Fix by ShiftZ - int r = floor_int(bb.r/dim); - int b = floor_int(bb.b/dim); - int t = floor_int(bb.t/dim); - - int n = hash->numcells; - for(int i=l; i<=r; i++){ - for(int j=b; j<=t; j++){ - cpHashValue idx = hash_func(i,j,n); - cpSpaceHashBin *bin = hash->table[idx]; - - // Don't add an object twice to the same cell. - if(containsHandle(bin, hand)) continue; - - cpHandleRetain(hand); - // Insert a new bin for the handle in this cell. - cpSpaceHashBin *newBin = getEmptyBin(hash); - newBin->handle = hand; - newBin->next = bin; - hash->table[idx] = newBin; - } - } -} - -//MARK: Basic Operations - -static void -cpSpaceHashInsert(cpSpaceHash *hash, void *obj, cpHashValue hashid) -{ - cpHandle *hand = (cpHandle *)cpHashSetInsert(hash->handleSet, hashid, obj, hash, (cpHashSetTransFunc)handleSetTrans); - hashHandle(hash, hand, hash->spatialIndex.bbfunc(obj)); -} - -static void -cpSpaceHashRehashObject(cpSpaceHash *hash, void *obj, cpHashValue hashid) -{ - cpHandle *hand = (cpHandle *)cpHashSetRemove(hash->handleSet, hashid, obj); - - if(hand){ - hand->obj = NULL; - cpHandleRelease(hand, hash->pooledHandles); - - cpSpaceHashInsert(hash, obj, hashid); - } -} - -static void -rehash_helper(cpHandle *hand, cpSpaceHash *hash) -{ - hashHandle(hash, hand, hash->spatialIndex.bbfunc(hand->obj)); -} - -static void -cpSpaceHashRehash(cpSpaceHash *hash) -{ - clearTable(hash); - cpHashSetEach(hash->handleSet, (cpHashSetIteratorFunc)rehash_helper, hash); -} - -static void -cpSpaceHashRemove(cpSpaceHash *hash, void *obj, cpHashValue hashid) -{ - cpHandle *hand = (cpHandle *)cpHashSetRemove(hash->handleSet, hashid, obj); - - if(hand){ - hand->obj = NULL; - cpHandleRelease(hand, hash->pooledHandles); - } -} - -typedef struct eachContext { - cpSpatialIndexIteratorFunc func; - void *data; -} eachContext; - -static void eachHelper(cpHandle *hand, eachContext *context){context->func(hand->obj, context->data);} - -static void -cpSpaceHashEach(cpSpaceHash *hash, cpSpatialIndexIteratorFunc func, void *data) -{ - eachContext context = {func, data}; - cpHashSetEach(hash->handleSet, (cpHashSetIteratorFunc)eachHelper, &context); -} - -static void -remove_orphaned_handles(cpSpaceHash *hash, cpSpaceHashBin **bin_ptr) -{ - cpSpaceHashBin *bin = *bin_ptr; - while(bin){ - cpHandle *hand = bin->handle; - cpSpaceHashBin *next = bin->next; - - if(!hand->obj){ - // orphaned handle, unlink and recycle the bin - (*bin_ptr) = bin->next; - recycleBin(hash, bin); - - cpHandleRelease(hand, hash->pooledHandles); - } else { - bin_ptr = &bin->next; - } - - bin = next; - } -} - -//MARK: Query Functions - -static inline void -query_helper(cpSpaceHash *hash, cpSpaceHashBin **bin_ptr, void *obj, cpSpatialIndexQueryFunc func, void *data) -{ - restart: - for(cpSpaceHashBin *bin = *bin_ptr; bin; bin = bin->next){ - cpHandle *hand = bin->handle; - void *other = hand->obj; - - if(hand->stamp == hash->stamp || obj == other){ - continue; - } else if(other){ - func(obj, other, 0, data); - hand->stamp = hash->stamp; - } else { - // The object for this handle has been removed - // cleanup this cell and restart the query - remove_orphaned_handles(hash, bin_ptr); - goto restart; // GCC not smart enough/able to tail call an inlined function. - } - } -} - -static void -cpSpaceHashQuery(cpSpaceHash *hash, void *obj, cpBB bb, cpSpatialIndexQueryFunc func, void *data) -{ - // Get the dimensions in cell coordinates. - cpFloat dim = hash->celldim; - int l = floor_int(bb.l/dim); // Fix by ShiftZ - int r = floor_int(bb.r/dim); - int b = floor_int(bb.b/dim); - int t = floor_int(bb.t/dim); - - int n = hash->numcells; - cpSpaceHashBin **table = hash->table; - - // Iterate over the cells and query them. - for(int i=l; i<=r; i++){ - for(int j=b; j<=t; j++){ - query_helper(hash, &table[hash_func(i,j,n)], obj, func, data); - } - } - - hash->stamp++; -} - -// Similar to struct eachPair above. -typedef struct queryRehashContext { - cpSpaceHash *hash; - cpSpatialIndexQueryFunc func; - void *data; -} queryRehashContext; - -// Hashset iterator func used with cpSpaceHashQueryRehash(). -static void -queryRehash_helper(cpHandle *hand, queryRehashContext *context) -{ - cpSpaceHash *hash = context->hash; - cpSpatialIndexQueryFunc func = context->func; - void *data = context->data; - - cpFloat dim = hash->celldim; - int n = hash->numcells; - - void *obj = hand->obj; - cpBB bb = hash->spatialIndex.bbfunc(obj); - - int l = floor_int(bb.l/dim); - int r = floor_int(bb.r/dim); - int b = floor_int(bb.b/dim); - int t = floor_int(bb.t/dim); - - cpSpaceHashBin **table = hash->table; - - for(int i=l; i<=r; i++){ - for(int j=b; j<=t; j++){ - cpHashValue idx = hash_func(i,j,n); - cpSpaceHashBin *bin = table[idx]; - - if(containsHandle(bin, hand)) continue; - - cpHandleRetain(hand); // this MUST be done first in case the object is removed in func() - query_helper(hash, &bin, obj, func, data); - - cpSpaceHashBin *newBin = getEmptyBin(hash); - newBin->handle = hand; - newBin->next = bin; - table[idx] = newBin; - } - } - - // Increment the stamp for each object hashed. - hash->stamp++; -} - -static void -cpSpaceHashReindexQuery(cpSpaceHash *hash, cpSpatialIndexQueryFunc func, void *data) -{ - clearTable(hash); - - queryRehashContext context = {hash, func, data}; - cpHashSetEach(hash->handleSet, (cpHashSetIteratorFunc)queryRehash_helper, &context); - - cpSpatialIndexCollideStatic((cpSpatialIndex *)hash, hash->spatialIndex.staticIndex, func, data); -} - -static inline cpFloat -segmentQuery_helper(cpSpaceHash *hash, cpSpaceHashBin **bin_ptr, void *obj, cpSpatialIndexSegmentQueryFunc func, void *data) -{ - cpFloat t = 1.0f; - - restart: - for(cpSpaceHashBin *bin = *bin_ptr; bin; bin = bin->next){ - cpHandle *hand = bin->handle; - void *other = hand->obj; - - // Skip over certain conditions - if(hand->stamp == hash->stamp){ - continue; - } else if(other){ - t = cpfmin(t, func(obj, other, data)); - hand->stamp = hash->stamp; - } else { - // The object for this handle has been removed - // cleanup this cell and restart the query - remove_orphaned_handles(hash, bin_ptr); - goto restart; // GCC not smart enough/able to tail call an inlined function. - } - } - - return t; -} - -// modified from http://playtechs.blogspot.com/2007/03/raytracing-on-grid.html -static void -cpSpaceHashSegmentQuery(cpSpaceHash *hash, void *obj, cpVect a, cpVect b, cpFloat t_exit, cpSpatialIndexSegmentQueryFunc func, void *data) -{ - a = cpvmult(a, 1.0f/hash->celldim); - b = cpvmult(b, 1.0f/hash->celldim); - - int cell_x = floor_int(a.x), cell_y = floor_int(a.y); - - cpFloat t = 0; - - int x_inc, y_inc; - cpFloat temp_v, temp_h; - - if (b.x > a.x){ - x_inc = 1; - temp_h = (cpffloor(a.x + 1.0f) - a.x); - } else { - x_inc = -1; - temp_h = (a.x - cpffloor(a.x)); - } - - if (b.y > a.y){ - y_inc = 1; - temp_v = (cpffloor(a.y + 1.0f) - a.y); - } else { - y_inc = -1; - temp_v = (a.y - cpffloor(a.y)); - } - - // Division by zero is *very* slow on ARM - cpFloat dx = cpfabs(b.x - a.x), dy = cpfabs(b.y - a.y); - cpFloat dt_dx = (dx ? 1.0f/dx : INFINITY), dt_dy = (dy ? 1.0f/dy : INFINITY); - - // fix NANs in horizontal directions - cpFloat next_h = (temp_h ? temp_h*dt_dx : dt_dx); - cpFloat next_v = (temp_v ? temp_v*dt_dy : dt_dy); - - int n = hash->numcells; - cpSpaceHashBin **table = hash->table; - - while(t < t_exit){ - cpHashValue idx = hash_func(cell_x, cell_y, n); - t_exit = cpfmin(t_exit, segmentQuery_helper(hash, &table[idx], obj, func, data)); - - if (next_v < next_h){ - cell_y += y_inc; - t = next_v; - next_v += dt_dy; - } else { - cell_x += x_inc; - t = next_h; - next_h += dt_dx; - } - } - - hash->stamp++; -} - -//MARK: Misc - -void -cpSpaceHashResize(cpSpaceHash *hash, cpFloat celldim, int numcells) -{ - if(hash->spatialIndex.klass != Klass()){ - cpAssertWarn(cpFalse, "Ignoring cpSpaceHashResize() call to non-cpSpaceHash spatial index."); - return; - } - - clearTable(hash); - - hash->celldim = celldim; - cpSpaceHashAllocTable(hash, next_prime(numcells)); -} - -static int -cpSpaceHashCount(cpSpaceHash *hash) -{ - return cpHashSetCount(hash->handleSet); -} - -static int -cpSpaceHashContains(cpSpaceHash *hash, void *obj, cpHashValue hashid) -{ - return cpHashSetFind(hash->handleSet, hashid, obj) != NULL; -} - -static cpSpatialIndexClass klass = { - (cpSpatialIndexDestroyImpl)cpSpaceHashDestroy, - - (cpSpatialIndexCountImpl)cpSpaceHashCount, - (cpSpatialIndexEachImpl)cpSpaceHashEach, - (cpSpatialIndexContainsImpl)cpSpaceHashContains, - - (cpSpatialIndexInsertImpl)cpSpaceHashInsert, - (cpSpatialIndexRemoveImpl)cpSpaceHashRemove, - - (cpSpatialIndexReindexImpl)cpSpaceHashRehash, - (cpSpatialIndexReindexObjectImpl)cpSpaceHashRehashObject, - (cpSpatialIndexReindexQueryImpl)cpSpaceHashReindexQuery, - - (cpSpatialIndexQueryImpl)cpSpaceHashQuery, - (cpSpatialIndexSegmentQueryImpl)cpSpaceHashSegmentQuery, -}; - -static inline cpSpatialIndexClass *Klass(){return &klass;} - -//MARK: Debug Drawing - -//#define CP_BBTREE_DEBUG_DRAW -#ifdef CP_BBTREE_DEBUG_DRAW -#include "OpenGL/gl.h" -#include "OpenGL/glu.h" -#include - -void -cpSpaceHashRenderDebug(cpSpatialIndex *index) -{ - if(index->klass != &klass){ - cpAssertWarn(cpFalse, "Ignoring cpSpaceHashRenderDebug() call to non-spatial hash spatial index."); - return; - } - - cpSpaceHash *hash = (cpSpaceHash *)index; - cpBB bb = cpBBNew(-320, -240, 320, 240); - - cpFloat dim = hash->celldim; - int n = hash->numcells; - - int l = (int)floor(bb.l/dim); - int r = (int)floor(bb.r/dim); - int b = (int)floor(bb.b/dim); - int t = (int)floor(bb.t/dim); - - for(int i=l; i<=r; i++){ - for(int j=b; j<=t; j++){ - int cell_count = 0; - - int index = hash_func(i,j,n); - for(cpSpaceHashBin *bin = hash->table[index]; bin; bin = bin->next) - cell_count++; - - GLfloat v = 1.0f - (GLfloat)cell_count/10.0f; - glColor3f(v,v,v); - glRectf(i*dim, j*dim, (i + 1)*dim, (j + 1)*dim); - } - } -} -#endif diff --git a/chipmunk/src/cpSpaceQuery.c b/chipmunk/src/cpSpaceQuery.c deleted file mode 100644 index 944bc4cec1..0000000000 --- a/chipmunk/src/cpSpaceQuery.c +++ /dev/null @@ -1,324 +0,0 @@ -/* Copyright (c) 2007 Scott Lembcke - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -#include "chipmunk_private.h" - -//MARK: Point Query Functions - -struct PointQueryContext { - cpVect point; - cpLayers layers; - cpGroup group; - cpSpacePointQueryFunc func; - void *data; -}; - -static cpCollisionID -PointQuery(struct PointQueryContext *context, cpShape *shape, cpCollisionID id, void *data) -{ - if( - !(shape->group && context->group == shape->group) && (context->layers&shape->layers) && - cpShapePointQuery(shape, context->point) - ){ - context->func(shape, context->data); - } - - return id; -} - -void -cpSpacePointQuery(cpSpace *space, cpVect point, cpLayers layers, cpGroup group, cpSpacePointQueryFunc func, void *data) -{ - struct PointQueryContext context = {point, layers, group, func, data}; - cpBB bb = cpBBNewForCircle(point, 0.0f); - - cpSpaceLock(space); { - cpSpatialIndexQuery(space->activeShapes, &context, bb, (cpSpatialIndexQueryFunc)PointQuery, data); - cpSpatialIndexQuery(space->staticShapes, &context, bb, (cpSpatialIndexQueryFunc)PointQuery, data); - } cpSpaceUnlock(space, cpTrue); -} - -static void -PointQueryFirst(cpShape *shape, cpShape **outShape) -{ - if(!shape->sensor) *outShape = shape; -} - -cpShape * -cpSpacePointQueryFirst(cpSpace *space, cpVect point, cpLayers layers, cpGroup group) -{ - cpShape *shape = NULL; - cpSpacePointQuery(space, point, layers, group, (cpSpacePointQueryFunc)PointQueryFirst, &shape); - - return shape; -} - -//MARK: Nearest Point Query Functions - -struct NearestPointQueryContext { - cpVect point; - cpFloat maxDistance; - cpLayers layers; - cpGroup group; - cpSpaceNearestPointQueryFunc func; -}; - -static cpCollisionID -NearestPointQuery(struct NearestPointQueryContext *context, cpShape *shape, cpCollisionID id, void *data) -{ - if( - !(shape->group && context->group == shape->group) && (context->layers&shape->layers) - ){ - cpNearestPointQueryInfo info; - cpShapeNearestPointQuery(shape, context->point, &info); - - if(info.shape && info.d < context->maxDistance) context->func(shape, info.d, info.p, data); - } - - return id; -} - -void -cpSpaceNearestPointQuery(cpSpace *space, cpVect point, cpFloat maxDistance, cpLayers layers, cpGroup group, cpSpaceNearestPointQueryFunc func, void *data) -{ - struct NearestPointQueryContext context = {point, maxDistance, layers, group, func}; - cpBB bb = cpBBNewForCircle(point, cpfmax(maxDistance, 0.0f)); - - cpSpaceLock(space); { - cpSpatialIndexQuery(space->activeShapes, &context, bb, (cpSpatialIndexQueryFunc)NearestPointQuery, data); - cpSpatialIndexQuery(space->staticShapes, &context, bb, (cpSpatialIndexQueryFunc)NearestPointQuery, data); - } cpSpaceUnlock(space, cpTrue); -} - -static cpCollisionID -NearestPointQueryNearest(struct NearestPointQueryContext *context, cpShape *shape, cpCollisionID id, cpNearestPointQueryInfo *out) -{ - if( - !(shape->group && context->group == shape->group) && (context->layers&shape->layers) && !shape->sensor - ){ - cpNearestPointQueryInfo info; - cpShapeNearestPointQuery(shape, context->point, &info); - - if(info.d < out->d) (*out) = info; - } - - return id; -} - -cpShape * -cpSpaceNearestPointQueryNearest(cpSpace *space, cpVect point, cpFloat maxDistance, cpLayers layers, cpGroup group, cpNearestPointQueryInfo *out) -{ - cpNearestPointQueryInfo info = {NULL, cpvzero, maxDistance, cpvzero}; - if(out){ - (*out) = info; - } else { - out = &info; - } - - struct NearestPointQueryContext context = { - point, maxDistance, - layers, group, - NULL - }; - - cpBB bb = cpBBNewForCircle(point, cpfmax(maxDistance, 0.0f)); - cpSpatialIndexQuery(space->activeShapes, &context, bb, (cpSpatialIndexQueryFunc)NearestPointQueryNearest, out); - cpSpatialIndexQuery(space->staticShapes, &context, bb, (cpSpatialIndexQueryFunc)NearestPointQueryNearest, out); - - return out->shape; -} - - -//MARK: Segment Query Functions - -struct SegmentQueryContext { - cpVect start, end; - cpLayers layers; - cpGroup group; - cpSpaceSegmentQueryFunc func; -}; - -static cpFloat -SegmentQuery(struct SegmentQueryContext *context, cpShape *shape, void *data) -{ - cpSegmentQueryInfo info; - - if( - !(shape->group && context->group == shape->group) && (context->layers&shape->layers) && - cpShapeSegmentQuery(shape, context->start, context->end, &info) - ){ - context->func(shape, info.t, info.n, data); - } - - return 1.0f; -} - -void -cpSpaceSegmentQuery(cpSpace *space, cpVect start, cpVect end, cpLayers layers, cpGroup group, cpSpaceSegmentQueryFunc func, void *data) -{ - struct SegmentQueryContext context = { - start, end, - layers, group, - func, - }; - - cpSpaceLock(space); { - cpSpatialIndexSegmentQuery(space->staticShapes, &context, start, end, 1.0f, (cpSpatialIndexSegmentQueryFunc)SegmentQuery, data); - cpSpatialIndexSegmentQuery(space->activeShapes, &context, start, end, 1.0f, (cpSpatialIndexSegmentQueryFunc)SegmentQuery, data); - } cpSpaceUnlock(space, cpTrue); -} - -static cpFloat -SegmentQueryFirst(struct SegmentQueryContext *context, cpShape *shape, cpSegmentQueryInfo *out) -{ - cpSegmentQueryInfo info; - - if( - !(shape->group && context->group == shape->group) && (context->layers&shape->layers) && - !shape->sensor && - cpShapeSegmentQuery(shape, context->start, context->end, &info) && - info.t < out->t - ){ - (*out) = info; - } - - return out->t; -} - -cpShape * -cpSpaceSegmentQueryFirst(cpSpace *space, cpVect start, cpVect end, cpLayers layers, cpGroup group, cpSegmentQueryInfo *out) -{ - cpSegmentQueryInfo info = {NULL, 1.0f, cpvzero}; - if(out){ - (*out) = info; - } else { - out = &info; - } - - struct SegmentQueryContext context = { - start, end, - layers, group, - NULL - }; - - cpSpatialIndexSegmentQuery(space->staticShapes, &context, start, end, 1.0f, (cpSpatialIndexSegmentQueryFunc)SegmentQueryFirst, out); - cpSpatialIndexSegmentQuery(space->activeShapes, &context, start, end, out->t, (cpSpatialIndexSegmentQueryFunc)SegmentQueryFirst, out); - - return out->shape; -} - -//MARK: BB Query Functions - -struct BBQueryContext { - cpBB bb; - cpLayers layers; - cpGroup group; - cpSpaceBBQueryFunc func; -}; - -static cpCollisionID -BBQuery(struct BBQueryContext *context, cpShape *shape, cpCollisionID id, void *data) -{ - if( - !(shape->group && context->group == shape->group) && (context->layers&shape->layers) && - cpBBIntersects(context->bb, shape->bb) - ){ - context->func(shape, data); - } - - return id; -} - -void -cpSpaceBBQuery(cpSpace *space, cpBB bb, cpLayers layers, cpGroup group, cpSpaceBBQueryFunc func, void *data) -{ - struct BBQueryContext context = {bb, layers, group, func}; - - cpSpaceLock(space); { - cpSpatialIndexQuery(space->activeShapes, &context, bb, (cpSpatialIndexQueryFunc)BBQuery, data); - cpSpatialIndexQuery(space->staticShapes, &context, bb, (cpSpatialIndexQueryFunc)BBQuery, data); - } cpSpaceUnlock(space, cpTrue); -} - -//MARK: Shape Query Functions - -struct ShapeQueryContext { - cpSpaceShapeQueryFunc func; - void *data; - cpBool anyCollision; -}; - -// Callback from the spatial hash. -static cpCollisionID -ShapeQuery(cpShape *a, cpShape *b, cpCollisionID id, struct ShapeQueryContext *context) -{ - // Reject any of the simple cases - if( - (a->group && a->group == b->group) || - !(a->layers & b->layers) || - a == b - ) return id; - - cpContact contacts[CP_MAX_CONTACTS_PER_ARBITER]; - int numContacts = 0; - - // Shape 'a' should have the lower shape type. (required by cpCollideShapes() ) - if(a->klass->type <= b->klass->type){ - numContacts = cpCollideShapes(a, b, &id, contacts); - } else { - numContacts = cpCollideShapes(b, a, &id, contacts); - for(int i=0; ianyCollision = !(a->sensor || b->sensor); - - if(context->func){ - cpContactPointSet set; - set.count = numContacts; - - for(int i=0; ifunc(b, &set, context->data); - } - } - - return id; -} - -cpBool -cpSpaceShapeQuery(cpSpace *space, cpShape *shape, cpSpaceShapeQueryFunc func, void *data) -{ - cpBody *body = shape->body; - cpBB bb = (body ? cpShapeUpdate(shape, body->p, body->rot) : shape->bb); - struct ShapeQueryContext context = {func, data, cpFalse}; - - cpSpaceLock(space); { - cpSpatialIndexQuery(space->activeShapes, shape, bb, (cpSpatialIndexQueryFunc)ShapeQuery, &context); - cpSpatialIndexQuery(space->staticShapes, shape, bb, (cpSpatialIndexQueryFunc)ShapeQuery, &context); - } cpSpaceUnlock(space, cpTrue); - - return context.anyCollision; -} diff --git a/chipmunk/src/cpSpaceStep.c b/chipmunk/src/cpSpaceStep.c deleted file mode 100644 index f6f6d050f0..0000000000 --- a/chipmunk/src/cpSpaceStep.c +++ /dev/null @@ -1,438 +0,0 @@ -/* Copyright (c) 2007 Scott Lembcke - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -#include "chipmunk_private.h" - -//MARK: Post Step Callback Functions - -cpPostStepCallback * -cpSpaceGetPostStepCallback(cpSpace *space, void *key) -{ - cpArray *arr = space->postStepCallbacks; - for(int i=0; inum; i++){ - cpPostStepCallback *callback = (cpPostStepCallback *)arr->arr[i]; - if(callback && callback->key == key) return callback; - } - - return NULL; -} - -static void PostStepDoNothing(cpSpace *space, void *obj, void *data){} - -cpBool -cpSpaceAddPostStepCallback(cpSpace *space, cpPostStepFunc func, void *key, void *data) -{ - cpAssertWarn(space->locked, - "Adding a post-step callback when the space is not locked is unnecessary. " - "Post-step callbacks will not called until the end of the next call to cpSpaceStep() or the next query."); - - if(!cpSpaceGetPostStepCallback(space, key)){ - cpPostStepCallback *callback = (cpPostStepCallback *)cpcalloc(1, sizeof(cpPostStepCallback)); - callback->func = (func ? func : PostStepDoNothing); - callback->key = key; - callback->data = data; - - cpArrayPush(space->postStepCallbacks, callback); - return cpTrue; - } else { - return cpFalse; - } -} - -//MARK: Locking Functions - -void -cpSpaceLock(cpSpace *space) -{ - space->locked++; -} - -void -cpSpaceUnlock(cpSpace *space, cpBool runPostStep) -{ - space->locked--; - cpAssertHard(space->locked >= 0, "Internal Error: Space lock underflow."); - - if(space->locked == 0){ - cpArray *waking = space->rousedBodies; - - for(int i=0, count=waking->num; iarr[i]); - waking->arr[i] = NULL; - } - - waking->num = 0; - - if(space->locked == 0 && runPostStep && !space->skipPostStep){ - space->skipPostStep = cpTrue; - - cpArray *arr = space->postStepCallbacks; - for(int i=0; inum; i++){ - cpPostStepCallback *callback = (cpPostStepCallback *)arr->arr[i]; - cpPostStepFunc func = callback->func; - - // Mark the func as NULL in case calling it calls cpSpaceRunPostStepCallbacks() again. - // TODO need more tests around this case I think. - callback->func = NULL; - if(func) func(space, callback->key, callback->data); - - arr->arr[i] = NULL; - cpfree(callback); - } - - arr->num = 0; - space->skipPostStep = cpFalse; - } - } -} - -//MARK: Contact Buffer Functions - -struct cpContactBufferHeader { - cpTimestamp stamp; - cpContactBufferHeader *next; - unsigned int numContacts; -}; - -#define CP_CONTACTS_BUFFER_SIZE ((CP_BUFFER_BYTES - sizeof(cpContactBufferHeader))/sizeof(cpContact)) -typedef struct cpContactBuffer { - cpContactBufferHeader header; - cpContact contacts[CP_CONTACTS_BUFFER_SIZE]; -} cpContactBuffer; - -static cpContactBufferHeader * -cpSpaceAllocContactBuffer(cpSpace *space) -{ - cpContactBuffer *buffer = (cpContactBuffer *)cpcalloc(1, sizeof(cpContactBuffer)); - cpArrayPush(space->allocatedBuffers, buffer); - return (cpContactBufferHeader *)buffer; -} - -static cpContactBufferHeader * -cpContactBufferHeaderInit(cpContactBufferHeader *header, cpTimestamp stamp, cpContactBufferHeader *splice) -{ - header->stamp = stamp; - header->next = (splice ? splice->next : header); - header->numContacts = 0; - - return header; -} - -void -cpSpacePushFreshContactBuffer(cpSpace *space) -{ - cpTimestamp stamp = space->stamp; - - cpContactBufferHeader *head = space->contactBuffersHead; - - if(!head){ - // No buffers have been allocated, make one - space->contactBuffersHead = cpContactBufferHeaderInit(cpSpaceAllocContactBuffer(space), stamp, NULL); - } else if(stamp - head->next->stamp > space->collisionPersistence){ - // The tail buffer is available, rotate the ring - cpContactBufferHeader *tail = head->next; - space->contactBuffersHead = cpContactBufferHeaderInit(tail, stamp, tail); - } else { - // Allocate a new buffer and push it into the ring - cpContactBufferHeader *buffer = cpContactBufferHeaderInit(cpSpaceAllocContactBuffer(space), stamp, head); - space->contactBuffersHead = head->next = buffer; - } -} - - -cpContact * -cpContactBufferGetArray(cpSpace *space) -{ - if(space->contactBuffersHead->numContacts + CP_MAX_CONTACTS_PER_ARBITER > CP_CONTACTS_BUFFER_SIZE){ - // contact buffer could overflow on the next collision, push a fresh one. - cpSpacePushFreshContactBuffer(space); - } - - cpContactBufferHeader *head = space->contactBuffersHead; - return ((cpContactBuffer *)head)->contacts + head->numContacts; -} - -void -cpSpacePushContacts(cpSpace *space, int count) -{ - cpAssertHard(count <= CP_MAX_CONTACTS_PER_ARBITER, "Internal Error: Contact buffer overflow!"); - space->contactBuffersHead->numContacts += count; -} - -static void -cpSpacePopContacts(cpSpace *space, int count){ - space->contactBuffersHead->numContacts -= count; -} - -//MARK: Collision Detection Functions - -static void * -cpSpaceArbiterSetTrans(cpShape **shapes, cpSpace *space) -{ - if(space->pooledArbiters->num == 0){ - // arbiter pool is exhausted, make more - int count = CP_BUFFER_BYTES/sizeof(cpArbiter); - cpAssertHard(count, "Internal Error: Buffer size too small."); - - cpArbiter *buffer = (cpArbiter *)cpcalloc(1, CP_BUFFER_BYTES); - cpArrayPush(space->allocatedBuffers, buffer); - - for(int i=0; ipooledArbiters, buffer + i); - } - - return cpArbiterInit((cpArbiter *)cpArrayPop(space->pooledArbiters), shapes[0], shapes[1]); -} - -static inline cpBool -queryReject(cpShape *a, cpShape *b) -{ - return ( - // BBoxes must overlap - !cpBBIntersects(a->bb, b->bb) - // Don't collide shapes attached to the same body. - || a->body == b->body - // Don't collide objects in the same non-zero group - || (a->group && a->group == b->group) - // Don't collide objects that don't share at least on layer. - || !(a->layers & b->layers) - // Don't collide infinite mass objects - || (a->body->m == INFINITY && b->body->m == INFINITY) - ); -} - -// Callback from the spatial hash. -cpCollisionID -cpSpaceCollideShapes(cpShape *a, cpShape *b, cpCollisionID id, cpSpace *space) -{ - // Reject any of the simple cases - if(queryReject(a,b)) return id; - - cpCollisionHandler *handler = cpSpaceLookupHandler(space, a->collision_type, b->collision_type); - - cpBool sensor = a->sensor || b->sensor; - if(sensor && handler == &cpDefaultCollisionHandler) return id; - - // Shape 'a' should have the lower shape type. (required by cpCollideShapes() ) - // TODO remove me: a < b comparison is for debugging collisions - if(a->klass->type > b->klass->type || (a->klass->type == b->klass->type && a < b)){ - cpShape *temp = a; - a = b; - b = temp; - } - - // Narrow-phase collision detection. - cpContact *contacts = cpContactBufferGetArray(space); - int numContacts = cpCollideShapes(a, b, &id, contacts); - if(!numContacts) return id; // Shapes are not colliding. - cpSpacePushContacts(space, numContacts); - - // Get an arbiter from space->arbiterSet for the two shapes. - // This is where the persistant contact magic comes from. - cpShape *shape_pair[] = {a, b}; - cpHashValue arbHashID = CP_HASH_PAIR((cpHashValue)a, (cpHashValue)b); - cpArbiter *arb = (cpArbiter *)cpHashSetInsert(space->cachedArbiters, arbHashID, shape_pair, space, (cpHashSetTransFunc)cpSpaceArbiterSetTrans); - cpArbiterUpdate(arb, contacts, numContacts, handler, a, b); - - // Call the begin function first if it's the first step - if(arb->state == cpArbiterStateFirstColl && !handler->begin(arb, space, handler->data)){ - cpArbiterIgnore(arb); // permanently ignore the collision until separation - } - - if( - // Ignore the arbiter if it has been flagged - (arb->state != cpArbiterStateIgnore) && - // Call preSolve - handler->preSolve(arb, space, handler->data) && - // Process, but don't add collisions for sensors. - !sensor - ){ - cpArrayPush(space->arbiters, arb); - } else { - cpSpacePopContacts(space, numContacts); - - arb->contacts = NULL; - arb->numContacts = 0; - - // Normally arbiters are set as used after calling the post-solve callback. - // However, post-solve callbacks are not called for sensors or arbiters rejected from pre-solve. - if(arb->state != cpArbiterStateIgnore) arb->state = cpArbiterStateNormal; - } - - // Time stamp the arbiter so we know it was used recently. - arb->stamp = space->stamp; - return id; -} - -// Hashset filter func to throw away old arbiters. -cpBool -cpSpaceArbiterSetFilter(cpArbiter *arb, cpSpace *space) -{ - cpTimestamp ticks = space->stamp - arb->stamp; - - cpBody *a = arb->body_a, *b = arb->body_b; - - // TODO should make an arbiter state for this so it doesn't require filtering arbiters for dangling body pointers on body removal. - // Preserve arbiters on sensors and rejected arbiters for sleeping objects. - // This prevents errant separate callbacks from happenening. - if( - (cpBodyIsStatic(a) || cpBodyIsSleeping(a)) && - (cpBodyIsStatic(b) || cpBodyIsSleeping(b)) - ){ - return cpTrue; - } - - // Arbiter was used last frame, but not this one - if(ticks >= 1 && arb->state != cpArbiterStateCached){ - arb->state = cpArbiterStateCached; - cpArbiterCallSeparate(arb, space); - } - - if(ticks >= space->collisionPersistence){ - arb->contacts = NULL; - arb->numContacts = 0; - - cpArrayPush(space->pooledArbiters, arb); - return cpFalse; - } - - return cpTrue; -} - -//MARK: All Important cpSpaceStep() Function - -void -cpShapeUpdateFunc(cpShape *shape, void *unused) -{ - cpBody *body = shape->body; - cpShapeUpdate(shape, body->p, body->rot); -} - -void -cpSpaceStep(cpSpace *space, cpFloat dt) -{ - // don't step if the timestep is 0! - if(dt == 0.0f) return; - - space->stamp++; - - cpFloat prev_dt = space->curr_dt; - space->curr_dt = dt; - - cpArray *bodies = space->bodies; - cpArray *constraints = space->constraints; - cpArray *arbiters = space->arbiters; - - // Reset and empty the arbiter lists. - for(int i=0; inum; i++){ - cpArbiter *arb = (cpArbiter *)arbiters->arr[i]; - arb->state = cpArbiterStateNormal; - - // If both bodies are awake, unthread the arbiter from the contact graph. - if(!cpBodyIsSleeping(arb->body_a) && !cpBodyIsSleeping(arb->body_b)){ - cpArbiterUnthread(arb); - } - } - arbiters->num = 0; - - cpSpaceLock(space); { - // Integrate positions - for(int i=0; inum; i++){ - cpBody *body = (cpBody *)bodies->arr[i]; - body->position_func(body, dt); - } - - // Find colliding pairs. - cpSpacePushFreshContactBuffer(space); - cpSpatialIndexEach(space->activeShapes, (cpSpatialIndexIteratorFunc)cpShapeUpdateFunc, NULL); - cpSpatialIndexReindexQuery(space->activeShapes, (cpSpatialIndexQueryFunc)cpSpaceCollideShapes, space); - } cpSpaceUnlock(space, cpFalse); - - // Rebuild the contact graph (and detect sleeping components if sleeping is enabled) - cpSpaceProcessComponents(space, dt); - - cpSpaceLock(space); { - // Clear out old cached arbiters and call separate callbacks - cpHashSetFilter(space->cachedArbiters, (cpHashSetFilterFunc)cpSpaceArbiterSetFilter, space); - - // Prestep the arbiters and constraints. - cpFloat slop = space->collisionSlop; - cpFloat biasCoef = 1.0f - cpfpow(space->collisionBias, dt); - for(int i=0; inum; i++){ - cpArbiterPreStep((cpArbiter *)arbiters->arr[i], dt, slop, biasCoef); - } - - for(int i=0; inum; i++){ - cpConstraint *constraint = (cpConstraint *)constraints->arr[i]; - - cpConstraintPreSolveFunc preSolve = constraint->preSolve; - if(preSolve) preSolve(constraint, space); - - constraint->klass->preStep(constraint, dt); - } - - // Integrate velocities. - cpFloat damping = cpfpow(space->damping, dt); - cpVect gravity = space->gravity; - for(int i=0; inum; i++){ - cpBody *body = (cpBody *)bodies->arr[i]; - body->velocity_func(body, gravity, damping, dt); - } - - // Apply cached impulses - cpFloat dt_coef = (prev_dt == 0.0f ? 0.0f : dt/prev_dt); - for(int i=0; inum; i++){ - cpArbiterApplyCachedImpulse((cpArbiter *)arbiters->arr[i], dt_coef); - } - - for(int i=0; inum; i++){ - cpConstraint *constraint = (cpConstraint *)constraints->arr[i]; - constraint->klass->applyCachedImpulse(constraint, dt_coef); - } - - // Run the impulse solver. - for(int i=0; iiterations; i++){ - for(int j=0; jnum; j++){ - cpArbiterApplyImpulse((cpArbiter *)arbiters->arr[j]); - } - - for(int j=0; jnum; j++){ - cpConstraint *constraint = (cpConstraint *)constraints->arr[j]; - constraint->klass->applyImpulse(constraint, dt); - } - } - - // Run the constraint post-solve callbacks - for(int i=0; inum; i++){ - cpConstraint *constraint = (cpConstraint *)constraints->arr[i]; - - cpConstraintPostSolveFunc postSolve = constraint->postSolve; - if(postSolve) postSolve(constraint, space); - } - - // run the post-solve callbacks - for(int i=0; inum; i++){ - cpArbiter *arb = (cpArbiter *) arbiters->arr[i]; - - cpCollisionHandler *handler = arb->handler; - handler->postSolve(arb, space, handler->data); - } - } cpSpaceUnlock(space, cpTrue); -} diff --git a/chipmunk/src/cpSpatialIndex.c b/chipmunk/src/cpSpatialIndex.c deleted file mode 100644 index e5c5c42919..0000000000 --- a/chipmunk/src/cpSpatialIndex.c +++ /dev/null @@ -1,69 +0,0 @@ -/* Copyright (c) 2010 Scott Lembcke - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -#include "chipmunk_private.h" - -void -cpSpatialIndexFree(cpSpatialIndex *index) -{ - if(index){ - cpSpatialIndexDestroy(index); - cpfree(index); - } -} - -cpSpatialIndex * -cpSpatialIndexInit(cpSpatialIndex *index, cpSpatialIndexClass *klass, cpSpatialIndexBBFunc bbfunc, cpSpatialIndex *staticIndex) -{ - index->klass = klass; - index->bbfunc = bbfunc; - index->staticIndex = staticIndex; - - if(staticIndex){ - cpAssertHard(!staticIndex->dynamicIndex, "This static index is already associated with a dynamic index."); - staticIndex->dynamicIndex = index; - } - - return index; -} - -typedef struct dynamicToStaticContext { - cpSpatialIndexBBFunc bbfunc; - cpSpatialIndex *staticIndex; - cpSpatialIndexQueryFunc queryFunc; - void *data; -} dynamicToStaticContext; - -static void -dynamicToStaticIter(void *obj, dynamicToStaticContext *context) -{ - cpSpatialIndexQuery(context->staticIndex, obj, context->bbfunc(obj), context->queryFunc, context->data); -} - -void -cpSpatialIndexCollideStatic(cpSpatialIndex *dynamicIndex, cpSpatialIndex *staticIndex, cpSpatialIndexQueryFunc func, void *data) -{ - if(staticIndex && cpSpatialIndexCount(staticIndex) > 0){ - dynamicToStaticContext context = {dynamicIndex->bbfunc, staticIndex, func, data}; - cpSpatialIndexEach(dynamicIndex, (cpSpatialIndexIteratorFunc)dynamicToStaticIter, &context); - } -} - diff --git a/chipmunk/src/cpSweep1D.c b/chipmunk/src/cpSweep1D.c deleted file mode 100644 index 80b9a4ac53..0000000000 --- a/chipmunk/src/cpSweep1D.c +++ /dev/null @@ -1,254 +0,0 @@ -/* Copyright (c) 2010 Scott Lembcke - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -#include "chipmunk_private.h" - -static inline cpSpatialIndexClass *Klass(); - -//MARK: Basic Structures - -typedef struct Bounds { - cpFloat min, max; -} Bounds; - -typedef struct TableCell { - void *obj; - Bounds bounds; -} TableCell; - -struct cpSweep1D -{ - cpSpatialIndex spatialIndex; - - int num; - int max; - TableCell *table; -}; - -static inline cpBool -BoundsOverlap(Bounds a, Bounds b) -{ - return (a.min <= b.max && b.min <= a.max); -} - -static inline Bounds -BBToBounds(cpSweep1D *sweep, cpBB bb) -{ - Bounds bounds = {bb.l, bb.r}; - return bounds; -} - -static inline TableCell -MakeTableCell(cpSweep1D *sweep, void *obj) -{ - TableCell cell = {obj, BBToBounds(sweep, sweep->spatialIndex.bbfunc(obj))}; - return cell; -} - -//MARK: Memory Management Functions - -cpSweep1D * -cpSweep1DAlloc(void) -{ - return (cpSweep1D *)cpcalloc(1, sizeof(cpSweep1D)); -} - -static void -ResizeTable(cpSweep1D *sweep, int size) -{ - sweep->max = size; - sweep->table = (TableCell *)cprealloc(sweep->table, size*sizeof(TableCell)); -} - -cpSpatialIndex * -cpSweep1DInit(cpSweep1D *sweep, cpSpatialIndexBBFunc bbfunc, cpSpatialIndex *staticIndex) -{ - cpSpatialIndexInit((cpSpatialIndex *)sweep, Klass(), bbfunc, staticIndex); - - sweep->num = 0; - ResizeTable(sweep, 32); - - return (cpSpatialIndex *)sweep; -} - -cpSpatialIndex * -cpSweep1DNew(cpSpatialIndexBBFunc bbfunc, cpSpatialIndex *staticIndex) -{ - return cpSweep1DInit(cpSweep1DAlloc(), bbfunc, staticIndex); -} - -static void -cpSweep1DDestroy(cpSweep1D *sweep) -{ - cpfree(sweep->table); - sweep->table = NULL; -} - -//MARK: Misc - -static int -cpSweep1DCount(cpSweep1D *sweep) -{ - return sweep->num; -} - -static void -cpSweep1DEach(cpSweep1D *sweep, cpSpatialIndexIteratorFunc func, void *data) -{ - TableCell *table = sweep->table; - for(int i=0, count=sweep->num; itable; - for(int i=0, count=sweep->num; inum == sweep->max) ResizeTable(sweep, sweep->max*2); - - sweep->table[sweep->num] = MakeTableCell(sweep, obj); - sweep->num++; -} - -static void -cpSweep1DRemove(cpSweep1D *sweep, void *obj, cpHashValue hashid) -{ - TableCell *table = sweep->table; - for(int i=0, count=sweep->num; inum; - - table[i] = table[num]; - table[num].obj = NULL; - - return; - } - } -} - -//MARK: Reindexing Functions - -static void -cpSweep1DReindexObject(cpSweep1D *sweep, void *obj, cpHashValue hashid) -{ - // Nothing to do here -} - -static void -cpSweep1DReindex(cpSweep1D *sweep) -{ - // Nothing to do here - // Could perform a sort, but queries are not accelerated anyway. -} - -//MARK: Query Functions - -static void -cpSweep1DQuery(cpSweep1D *sweep, void *obj, cpBB bb, cpSpatialIndexQueryFunc func, void *data) -{ - // Implementing binary search here would allow you to find an upper limit - // but not a lower limit. Probably not worth the hassle. - - Bounds bounds = BBToBounds(sweep, bb); - - TableCell *table = sweep->table; - for(int i=0, count=sweep->num; itable; - for(int i=0, count=sweep->num; ibounds.min < b->bounds.min ? -1 : (a->bounds.min > b->bounds.min ? 1 : 0)); -} - -static void -cpSweep1DReindexQuery(cpSweep1D *sweep, cpSpatialIndexQueryFunc func, void *data) -{ - TableCell *table = sweep->table; - int count = sweep->num; - - // Update bounds and sort - for(int i=0; ispatialIndex.staticIndex, func, data); -} - -static cpSpatialIndexClass klass = { - (cpSpatialIndexDestroyImpl)cpSweep1DDestroy, - - (cpSpatialIndexCountImpl)cpSweep1DCount, - (cpSpatialIndexEachImpl)cpSweep1DEach, - (cpSpatialIndexContainsImpl)cpSweep1DContains, - - (cpSpatialIndexInsertImpl)cpSweep1DInsert, - (cpSpatialIndexRemoveImpl)cpSweep1DRemove, - - (cpSpatialIndexReindexImpl)cpSweep1DReindex, - (cpSpatialIndexReindexObjectImpl)cpSweep1DReindexObject, - (cpSpatialIndexReindexQueryImpl)cpSweep1DReindexQuery, - - (cpSpatialIndexQueryImpl)cpSweep1DQuery, - (cpSpatialIndexSegmentQueryImpl)cpSweep1DSegmentQuery, -}; - -static inline cpSpatialIndexClass *Klass(){return &klass;} - diff --git a/chipmunk/src/cpVect.c b/chipmunk/src/cpVect.c deleted file mode 100644 index 64d16639ef..0000000000 --- a/chipmunk/src/cpVect.c +++ /dev/null @@ -1,56 +0,0 @@ -/* Copyright (c) 2007 Scott Lembcke - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -#include - -#include "chipmunk_private.h" - -cpVect -cpvslerp(const cpVect v1, const cpVect v2, const cpFloat t) -{ - cpFloat dot = cpvdot(cpvnormalize(v1), cpvnormalize(v2)); - cpFloat omega = cpfacos(cpfclamp(dot, -1.0f, 1.0f)); - - if(omega < 1e-3){ - // If the angle between two vectors is very small, lerp instead to avoid precision issues. - return cpvlerp(v1, v2, t); - } else { - cpFloat denom = 1.0f/cpfsin(omega); - return cpvadd(cpvmult(v1, cpfsin((1.0f - t)*omega)*denom), cpvmult(v2, cpfsin(t*omega)*denom)); - } -} - -cpVect -cpvslerpconst(const cpVect v1, const cpVect v2, const cpFloat a) -{ - cpFloat dot = cpvdot(cpvnormalize(v1), cpvnormalize(v2)); - cpFloat omega = cpfacos(cpfclamp(dot, -1.0f, 1.0f)); - - return cpvslerp(v1, v2, cpfmin(a, omega)/omega); -} - -char* -cpvstr(const cpVect v) -{ - static char str[256]; - sprintf(str, "(% .3f, % .3f)", v.x, v.y); - return str; -} diff --git a/chipmunk/src/prime.h b/chipmunk/src/prime.h deleted file mode 100644 index 419bdb89d6..0000000000 --- a/chipmunk/src/prime.h +++ /dev/null @@ -1,68 +0,0 @@ -/* Copyright (c) 2007 Scott Lembcke - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -// Used for resizing hash tables. -// Values approximately double. -// http://planetmath.org/encyclopedia/GoodHashTablePrimes.html -static int primes[] = { - 5, - 13, - 23, - 47, - 97, - 193, - 389, - 769, - 1543, - 3079, - 6151, - 12289, - 24593, - 49157, - 98317, - 196613, - 393241, - 786433, - 1572869, - 3145739, - 6291469, - 12582917, - 25165843, - 50331653, - 100663319, - 201326611, - 402653189, - 805306457, - 1610612741, - 0, -}; - -static inline int -next_prime(int n) -{ - int i = 0; - while(n > primes[i]){ - i++; - cpAssertHard(primes[i], "Tried to resize a hash table to a size greater than 1610612741 O_o"); // realistically this should never happen - } - - return primes[i]; -}