diff options
Diffstat (limited to 'Source/3rdParty/Box2D/Dynamics')
55 files changed, 13326 insertions, 0 deletions
diff --git a/Source/3rdParty/Box2D/Dynamics/Contacts/b2ChainAndCircleContact.cpp b/Source/3rdParty/Box2D/Dynamics/Contacts/b2ChainAndCircleContact.cpp new file mode 100644 index 0000000..c930255 --- /dev/null +++ b/Source/3rdParty/Box2D/Dynamics/Contacts/b2ChainAndCircleContact.cpp @@ -0,0 +1,53 @@ +/* +* Copyright (c) 2006-2010 Erin Catto http://www.box2d.org +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +#include "Box2D/Dynamics/Contacts/b2ChainAndCircleContact.h" +#include "Box2D/Common/b2BlockAllocator.h" +#include "Box2D/Dynamics/b2Fixture.h" +#include "Box2D/Collision/Shapes/b2ChainShape.h" +#include "Box2D/Collision/Shapes/b2EdgeShape.h" + +#include <new> + +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/Source/3rdParty/Box2D/Dynamics/Contacts/b2ChainAndCircleContact.h b/Source/3rdParty/Box2D/Dynamics/Contacts/b2ChainAndCircleContact.h new file mode 100644 index 0000000..1421f90 --- /dev/null +++ b/Source/3rdParty/Box2D/Dynamics/Contacts/b2ChainAndCircleContact.h @@ -0,0 +1,39 @@ +/* +* 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 "Box2D/Dynamics/Contacts/b2Contact.h" + +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) override; +}; + +#endif diff --git a/Source/3rdParty/Box2D/Dynamics/Contacts/b2ChainAndPolygonContact.cpp b/Source/3rdParty/Box2D/Dynamics/Contacts/b2ChainAndPolygonContact.cpp new file mode 100644 index 0000000..78431d5 --- /dev/null +++ b/Source/3rdParty/Box2D/Dynamics/Contacts/b2ChainAndPolygonContact.cpp @@ -0,0 +1,53 @@ +/* +* Copyright (c) 2006-2010 Erin Catto http://www.box2d.org +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +#include "Box2D/Dynamics/Contacts/b2ChainAndPolygonContact.h" +#include "Box2D/Common/b2BlockAllocator.h" +#include "Box2D/Dynamics/b2Fixture.h" +#include "Box2D/Collision/Shapes/b2ChainShape.h" +#include "Box2D/Collision/Shapes/b2EdgeShape.h" + +#include <new> + +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/Source/3rdParty/Box2D/Dynamics/Contacts/b2ChainAndPolygonContact.h b/Source/3rdParty/Box2D/Dynamics/Contacts/b2ChainAndPolygonContact.h new file mode 100644 index 0000000..89b8dd3 --- /dev/null +++ b/Source/3rdParty/Box2D/Dynamics/Contacts/b2ChainAndPolygonContact.h @@ -0,0 +1,39 @@ +/* +* 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 "Box2D/Dynamics/Contacts/b2Contact.h" + +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) override; +}; + +#endif diff --git a/Source/3rdParty/Box2D/Dynamics/Contacts/b2CircleContact.cpp b/Source/3rdParty/Box2D/Dynamics/Contacts/b2CircleContact.cpp new file mode 100644 index 0000000..3b6c50b --- /dev/null +++ b/Source/3rdParty/Box2D/Dynamics/Contacts/b2CircleContact.cpp @@ -0,0 +1,52 @@ +/* +* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +#include "Box2D/Dynamics/Contacts/b2CircleContact.h" +#include "Box2D/Dynamics/b2Body.h" +#include "Box2D/Dynamics/b2Fixture.h" +#include "Box2D/Dynamics/b2WorldCallbacks.h" +#include "Box2D/Common/b2BlockAllocator.h" +#include "Box2D/Collision/b2TimeOfImpact.h" + +#include <new> + +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/Source/3rdParty/Box2D/Dynamics/Contacts/b2CircleContact.h b/Source/3rdParty/Box2D/Dynamics/Contacts/b2CircleContact.h new file mode 100644 index 0000000..d40c7fb --- /dev/null +++ b/Source/3rdParty/Box2D/Dynamics/Contacts/b2CircleContact.h @@ -0,0 +1,39 @@ +/* +* 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 "Box2D/Dynamics/Contacts/b2Contact.h" + +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) override; +}; + +#endif diff --git a/Source/3rdParty/Box2D/Dynamics/Contacts/b2Contact.cpp b/Source/3rdParty/Box2D/Dynamics/Contacts/b2Contact.cpp new file mode 100644 index 0000000..41b0f78 --- /dev/null +++ b/Source/3rdParty/Box2D/Dynamics/Contacts/b2Contact.cpp @@ -0,0 +1,247 @@ +/* +* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +#include "Box2D/Dynamics/Contacts/b2Contact.h" +#include "Box2D/Dynamics/Contacts/b2CircleContact.h" +#include "Box2D/Dynamics/Contacts/b2PolygonAndCircleContact.h" +#include "Box2D/Dynamics/Contacts/b2PolygonContact.h" +#include "Box2D/Dynamics/Contacts/b2EdgeAndCircleContact.h" +#include "Box2D/Dynamics/Contacts/b2EdgeAndPolygonContact.h" +#include "Box2D/Dynamics/Contacts/b2ChainAndCircleContact.h" +#include "Box2D/Dynamics/Contacts/b2ChainAndPolygonContact.h" +#include "Box2D/Dynamics/Contacts/b2ContactSolver.h" + +#include "Box2D/Collision/b2Collision.h" +#include "Box2D/Collision/b2TimeOfImpact.h" +#include "Box2D/Collision/Shapes/b2Shape.h" +#include "Box2D/Common/b2BlockAllocator.h" +#include "Box2D/Dynamics/b2Body.h" +#include "Box2D/Dynamics/b2Fixture.h" +#include "Box2D/Dynamics/b2World.h" + +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 nullptr; + } +} + +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 = nullptr; + m_next = nullptr; + + m_nodeA.contact = nullptr; + m_nodeA.prev = nullptr; + m_nodeA.next = nullptr; + m_nodeA.other = nullptr; + + m_nodeB.contact = nullptr; + m_nodeB.prev = nullptr; + m_nodeB.next = nullptr; + m_nodeB.other = nullptr; + + 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/Source/3rdParty/Box2D/Dynamics/Contacts/b2Contact.h b/Source/3rdParty/Box2D/Dynamics/Contacts/b2Contact.h new file mode 100644 index 0000000..df23d3c --- /dev/null +++ b/Source/3rdParty/Box2D/Dynamics/Contacts/b2Contact.h @@ -0,0 +1,349 @@ +/* +* 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 "Box2D/Common/b2Math.h" +#include "Box2D/Collision/b2Collision.h" +#include "Box2D/Collision/Shapes/b2Shape.h" +#include "Box2D/Dynamics/b2Fixture.h" + +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 friction 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(nullptr), m_fixtureB(nullptr) {} + 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/Source/3rdParty/Box2D/Dynamics/Contacts/b2ContactSolver.cpp b/Source/3rdParty/Box2D/Dynamics/Contacts/b2ContactSolver.cpp new file mode 100644 index 0000000..147968c --- /dev/null +++ b/Source/3rdParty/Box2D/Dynamics/Contacts/b2ContactSolver.cpp @@ -0,0 +1,838 @@ +/* +* 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 "Box2D/Dynamics/Contacts/b2ContactSolver.h" + +#include "Box2D/Dynamics/Contacts/b2Contact.h" +#include "Box2D/Dynamics/b2Body.h" +#include "Box2D/Dynamics/b2Fixture.h" +#include "Box2D/Dynamics/b2World.h" +#include "Box2D/Common/b2StackAllocator.h" + +// Solver debugging is normally disabled because the block solver sometimes has to deal with a poorly conditioned effective mass matrix. +#define B2_DEBUG_SOLVER 0 + +bool g_blockSolve = true; + +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 && g_blockSolve) + { + 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 (pointCount == 1 || g_blockSolve == false) + { + 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 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, 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/Source/3rdParty/Box2D/Dynamics/Contacts/b2ContactSolver.h b/Source/3rdParty/Box2D/Dynamics/Contacts/b2ContactSolver.h new file mode 100644 index 0000000..ed98df5 --- /dev/null +++ b/Source/3rdParty/Box2D/Dynamics/Contacts/b2ContactSolver.h @@ -0,0 +1,95 @@ +/* +* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef B2_CONTACT_SOLVER_H +#define B2_CONTACT_SOLVER_H + +#include "Box2D/Common/b2Math.h" +#include "Box2D/Collision/b2Collision.h" +#include "Box2D/Dynamics/b2TimeStep.h" + +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/Source/3rdParty/Box2D/Dynamics/Contacts/b2EdgeAndCircleContact.cpp b/Source/3rdParty/Box2D/Dynamics/Contacts/b2EdgeAndCircleContact.cpp new file mode 100644 index 0000000..8d5933e --- /dev/null +++ b/Source/3rdParty/Box2D/Dynamics/Contacts/b2EdgeAndCircleContact.cpp @@ -0,0 +1,49 @@ +/* +* Copyright (c) 2006-2010 Erin Catto http://www.box2d.org +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +#include "Box2D/Dynamics/Contacts/b2EdgeAndCircleContact.h" +#include "Box2D/Common/b2BlockAllocator.h" +#include "Box2D/Dynamics/b2Fixture.h" + +#include <new> + +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/Source/3rdParty/Box2D/Dynamics/Contacts/b2EdgeAndCircleContact.h b/Source/3rdParty/Box2D/Dynamics/Contacts/b2EdgeAndCircleContact.h new file mode 100644 index 0000000..e241985 --- /dev/null +++ b/Source/3rdParty/Box2D/Dynamics/Contacts/b2EdgeAndCircleContact.h @@ -0,0 +1,39 @@ +/* +* 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 "Box2D/Dynamics/Contacts/b2Contact.h" + +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) override; +}; + +#endif diff --git a/Source/3rdParty/Box2D/Dynamics/Contacts/b2EdgeAndPolygonContact.cpp b/Source/3rdParty/Box2D/Dynamics/Contacts/b2EdgeAndPolygonContact.cpp new file mode 100644 index 0000000..6fab3f7 --- /dev/null +++ b/Source/3rdParty/Box2D/Dynamics/Contacts/b2EdgeAndPolygonContact.cpp @@ -0,0 +1,49 @@ +/* +* Copyright (c) 2006-2010 Erin Catto http://www.box2d.org +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +#include "Box2D/Dynamics/Contacts/b2EdgeAndPolygonContact.h" +#include "Box2D/Common/b2BlockAllocator.h" +#include "Box2D/Dynamics/b2Fixture.h" + +#include <new> + +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/Source/3rdParty/Box2D/Dynamics/Contacts/b2EdgeAndPolygonContact.h b/Source/3rdParty/Box2D/Dynamics/Contacts/b2EdgeAndPolygonContact.h new file mode 100644 index 0000000..ad92aac --- /dev/null +++ b/Source/3rdParty/Box2D/Dynamics/Contacts/b2EdgeAndPolygonContact.h @@ -0,0 +1,39 @@ +/* +* 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 "Box2D/Dynamics/Contacts/b2Contact.h" + +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) override; +}; + +#endif diff --git a/Source/3rdParty/Box2D/Dynamics/Contacts/b2PolygonAndCircleContact.cpp b/Source/3rdParty/Box2D/Dynamics/Contacts/b2PolygonAndCircleContact.cpp new file mode 100644 index 0000000..d3c3b94 --- /dev/null +++ b/Source/3rdParty/Box2D/Dynamics/Contacts/b2PolygonAndCircleContact.cpp @@ -0,0 +1,49 @@ +/* +* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +#include "Box2D/Dynamics/Contacts/b2PolygonAndCircleContact.h" +#include "Box2D/Common/b2BlockAllocator.h" +#include "Box2D/Dynamics/b2Fixture.h" + +#include <new> + +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/Source/3rdParty/Box2D/Dynamics/Contacts/b2PolygonAndCircleContact.h b/Source/3rdParty/Box2D/Dynamics/Contacts/b2PolygonAndCircleContact.h new file mode 100644 index 0000000..fc3573c --- /dev/null +++ b/Source/3rdParty/Box2D/Dynamics/Contacts/b2PolygonAndCircleContact.h @@ -0,0 +1,38 @@ +/* +* 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 "Box2D/Dynamics/Contacts/b2Contact.h" + +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) override; +}; + +#endif diff --git a/Source/3rdParty/Box2D/Dynamics/Contacts/b2PolygonContact.cpp b/Source/3rdParty/Box2D/Dynamics/Contacts/b2PolygonContact.cpp new file mode 100644 index 0000000..a9a6cdc --- /dev/null +++ b/Source/3rdParty/Box2D/Dynamics/Contacts/b2PolygonContact.cpp @@ -0,0 +1,52 @@ +/* +* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +#include "Box2D/Dynamics/Contacts/b2PolygonContact.h" +#include "Box2D/Common/b2BlockAllocator.h" +#include "Box2D/Collision/b2TimeOfImpact.h" +#include "Box2D/Dynamics/b2Body.h" +#include "Box2D/Dynamics/b2Fixture.h" +#include "Box2D/Dynamics/b2WorldCallbacks.h" + +#include <new> + +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/Source/3rdParty/Box2D/Dynamics/Contacts/b2PolygonContact.h b/Source/3rdParty/Box2D/Dynamics/Contacts/b2PolygonContact.h new file mode 100644 index 0000000..4755b4b --- /dev/null +++ b/Source/3rdParty/Box2D/Dynamics/Contacts/b2PolygonContact.h @@ -0,0 +1,39 @@ +/* +* 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 "Box2D/Dynamics/Contacts/b2Contact.h" + +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) override; +}; + +#endif diff --git a/Source/3rdParty/Box2D/Dynamics/Joints/b2DistanceJoint.cpp b/Source/3rdParty/Box2D/Dynamics/Joints/b2DistanceJoint.cpp new file mode 100644 index 0000000..126133c --- /dev/null +++ b/Source/3rdParty/Box2D/Dynamics/Joints/b2DistanceJoint.cpp @@ -0,0 +1,260 @@ +/* +* 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 "Box2D/Dynamics/Joints/b2DistanceJoint.h" +#include "Box2D/Dynamics/b2Body.h" +#include "Box2D/Dynamics/b2TimeStep.h" + +// 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/Source/3rdParty/Box2D/Dynamics/Joints/b2DistanceJoint.h b/Source/3rdParty/Box2D/Dynamics/Joints/b2DistanceJoint.h new file mode 100644 index 0000000..ba59210 --- /dev/null +++ b/Source/3rdParty/Box2D/Dynamics/Joints/b2DistanceJoint.h @@ -0,0 +1,169 @@ +/* +* 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 "Box2D/Dynamics/Joints/b2Joint.h" + +/// 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 override; + b2Vec2 GetAnchorB() const override; + + /// Get the reaction force given the inverse time step. + /// Unit is N. + b2Vec2 GetReactionForce(float32 inv_dt) const override; + + /// 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 override; + + /// 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() override; + +protected: + + friend class b2Joint; + b2DistanceJoint(const b2DistanceJointDef* data); + + void InitVelocityConstraints(const b2SolverData& data) override; + void SolveVelocityConstraints(const b2SolverData& data) override; + bool SolvePositionConstraints(const b2SolverData& data) override; + + 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/Source/3rdParty/Box2D/Dynamics/Joints/b2FrictionJoint.cpp b/Source/3rdParty/Box2D/Dynamics/Joints/b2FrictionJoint.cpp new file mode 100644 index 0000000..cb122eb --- /dev/null +++ b/Source/3rdParty/Box2D/Dynamics/Joints/b2FrictionJoint.cpp @@ -0,0 +1,251 @@ +/* +* 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 "Box2D/Dynamics/Joints/b2FrictionJoint.h" +#include "Box2D/Dynamics/b2Body.h" +#include "Box2D/Dynamics/b2TimeStep.h" + +// 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/Source/3rdParty/Box2D/Dynamics/Joints/b2FrictionJoint.h b/Source/3rdParty/Box2D/Dynamics/Joints/b2FrictionJoint.h new file mode 100644 index 0000000..d964f84 --- /dev/null +++ b/Source/3rdParty/Box2D/Dynamics/Joints/b2FrictionJoint.h @@ -0,0 +1,119 @@ +/* +* 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 "Box2D/Dynamics/Joints/b2Joint.h" + +/// 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 override; + b2Vec2 GetAnchorB() const override; + + b2Vec2 GetReactionForce(float32 inv_dt) const override; + float32 GetReactionTorque(float32 inv_dt) const override; + + /// 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() override; + +protected: + + friend class b2Joint; + + b2FrictionJoint(const b2FrictionJointDef* def); + + void InitVelocityConstraints(const b2SolverData& data) override; + void SolveVelocityConstraints(const b2SolverData& data) override; + bool SolvePositionConstraints(const b2SolverData& data) override; + + 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/Source/3rdParty/Box2D/Dynamics/Joints/b2GearJoint.cpp b/Source/3rdParty/Box2D/Dynamics/Joints/b2GearJoint.cpp new file mode 100644 index 0000000..1ce575b --- /dev/null +++ b/Source/3rdParty/Box2D/Dynamics/Joints/b2GearJoint.cpp @@ -0,0 +1,419 @@ +/* +* 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 "Box2D/Dynamics/Joints/b2GearJoint.h" +#include "Box2D/Dynamics/Joints/b2RevoluteJoint.h" +#include "Box2D/Dynamics/Joints/b2PrismaticJoint.h" +#include "Box2D/Dynamics/b2Body.h" +#include "Box2D/Dynamics/b2TimeStep.h" + +// 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/Source/3rdParty/Box2D/Dynamics/Joints/b2GearJoint.h b/Source/3rdParty/Box2D/Dynamics/Joints/b2GearJoint.h new file mode 100644 index 0000000..53f7e58 --- /dev/null +++ b/Source/3rdParty/Box2D/Dynamics/Joints/b2GearJoint.h @@ -0,0 +1,125 @@ +/* +* 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 "Box2D/Dynamics/Joints/b2Joint.h" + +/// 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 = nullptr; + joint2 = nullptr; + 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 override; + b2Vec2 GetAnchorB() const override; + + b2Vec2 GetReactionForce(float32 inv_dt) const override; + float32 GetReactionTorque(float32 inv_dt) const override; + + /// 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() override; + +protected: + + friend class b2Joint; + b2GearJoint(const b2GearJointDef* data); + + void InitVelocityConstraints(const b2SolverData& data) override; + void SolveVelocityConstraints(const b2SolverData& data) override; + bool SolvePositionConstraints(const b2SolverData& data) override; + + 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/Source/3rdParty/Box2D/Dynamics/Joints/b2Joint.cpp b/Source/3rdParty/Box2D/Dynamics/Joints/b2Joint.cpp new file mode 100644 index 0000000..8103b01 --- /dev/null +++ b/Source/3rdParty/Box2D/Dynamics/Joints/b2Joint.cpp @@ -0,0 +1,211 @@ +/* +* 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 "Box2D/Dynamics/Joints/b2Joint.h" +#include "Box2D/Dynamics/Joints/b2DistanceJoint.h" +#include "Box2D/Dynamics/Joints/b2WheelJoint.h" +#include "Box2D/Dynamics/Joints/b2MouseJoint.h" +#include "Box2D/Dynamics/Joints/b2RevoluteJoint.h" +#include "Box2D/Dynamics/Joints/b2PrismaticJoint.h" +#include "Box2D/Dynamics/Joints/b2PulleyJoint.h" +#include "Box2D/Dynamics/Joints/b2GearJoint.h" +#include "Box2D/Dynamics/Joints/b2WeldJoint.h" +#include "Box2D/Dynamics/Joints/b2FrictionJoint.h" +#include "Box2D/Dynamics/Joints/b2RopeJoint.h" +#include "Box2D/Dynamics/Joints/b2MotorJoint.h" +#include "Box2D/Dynamics/b2Body.h" +#include "Box2D/Dynamics/b2World.h" +#include "Box2D/Common/b2BlockAllocator.h" + +#include <new> + +b2Joint* b2Joint::Create(const b2JointDef* def, b2BlockAllocator* allocator) +{ + b2Joint* joint = nullptr; + + switch (def->type) + { + case e_distanceJoint: + { + void* mem = allocator->Allocate(sizeof(b2DistanceJoint)); + joint = new (mem) b2DistanceJoint(static_cast<const b2DistanceJointDef*>(def)); + } + break; + + case e_mouseJoint: + { + void* mem = allocator->Allocate(sizeof(b2MouseJoint)); + joint = new (mem) b2MouseJoint(static_cast<const b2MouseJointDef*>(def)); + } + break; + + case e_prismaticJoint: + { + void* mem = allocator->Allocate(sizeof(b2PrismaticJoint)); + joint = new (mem) b2PrismaticJoint(static_cast<const b2PrismaticJointDef*>(def)); + } + break; + + case e_revoluteJoint: + { + void* mem = allocator->Allocate(sizeof(b2RevoluteJoint)); + joint = new (mem) b2RevoluteJoint(static_cast<const b2RevoluteJointDef*>(def)); + } + break; + + case e_pulleyJoint: + { + void* mem = allocator->Allocate(sizeof(b2PulleyJoint)); + joint = new (mem) b2PulleyJoint(static_cast<const b2PulleyJointDef*>(def)); + } + break; + + case e_gearJoint: + { + void* mem = allocator->Allocate(sizeof(b2GearJoint)); + joint = new (mem) b2GearJoint(static_cast<const b2GearJointDef*>(def)); + } + break; + + case e_wheelJoint: + { + void* mem = allocator->Allocate(sizeof(b2WheelJoint)); + joint = new (mem) b2WheelJoint(static_cast<const b2WheelJointDef*>(def)); + } + break; + + case e_weldJoint: + { + void* mem = allocator->Allocate(sizeof(b2WeldJoint)); + joint = new (mem) b2WeldJoint(static_cast<const b2WeldJointDef*>(def)); + } + break; + + case e_frictionJoint: + { + void* mem = allocator->Allocate(sizeof(b2FrictionJoint)); + joint = new (mem) b2FrictionJoint(static_cast<const b2FrictionJointDef*>(def)); + } + break; + + case e_ropeJoint: + { + void* mem = allocator->Allocate(sizeof(b2RopeJoint)); + joint = new (mem) b2RopeJoint(static_cast<const b2RopeJointDef*>(def)); + } + break; + + case e_motorJoint: + { + void* mem = allocator->Allocate(sizeof(b2MotorJoint)); + joint = new (mem) b2MotorJoint(static_cast<const b2MotorJointDef*>(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 = nullptr; + m_next = nullptr; + 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 = nullptr; + m_edgeA.other = nullptr; + m_edgeA.prev = nullptr; + m_edgeA.next = nullptr; + + m_edgeB.joint = nullptr; + m_edgeB.other = nullptr; + m_edgeB.prev = nullptr; + m_edgeB.next = nullptr; +} + +bool b2Joint::IsActive() const +{ + return m_bodyA->IsActive() && m_bodyB->IsActive(); +} diff --git a/Source/3rdParty/Box2D/Dynamics/Joints/b2Joint.h b/Source/3rdParty/Box2D/Dynamics/Joints/b2Joint.h new file mode 100644 index 0000000..2ab5616 --- /dev/null +++ b/Source/3rdParty/Box2D/Dynamics/Joints/b2Joint.h @@ -0,0 +1,226 @@ +/* +* 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 "Box2D/Common/b2Math.h" + +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 = nullptr; + bodyA = nullptr; + bodyB = nullptr; + 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/Source/3rdParty/Box2D/Dynamics/Joints/b2MotorJoint.cpp b/Source/3rdParty/Box2D/Dynamics/Joints/b2MotorJoint.cpp new file mode 100644 index 0000000..7906845 --- /dev/null +++ b/Source/3rdParty/Box2D/Dynamics/Joints/b2MotorJoint.cpp @@ -0,0 +1,309 @@ +/* +* 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 "Box2D/Dynamics/Joints/b2MotorJoint.h" +#include "Box2D/Dynamics/b2Body.h" +#include "Box2D/Dynamics/b2TimeStep.h" + +// 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) +// +// r1 = offset - c1 +// r2 = -c2 + +// 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_linearOffset - m_localCenterA); + m_rB = b2Mul(qB, -m_localCenterB); + + // J = [-I -r1_skew I r2_skew] + // 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; + + // Upper 2 by 2 of K for point to point + 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; + 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/Source/3rdParty/Box2D/Dynamics/Joints/b2MotorJoint.h b/Source/3rdParty/Box2D/Dynamics/Joints/b2MotorJoint.h new file mode 100644 index 0000000..f384f41 --- /dev/null +++ b/Source/3rdParty/Box2D/Dynamics/Joints/b2MotorJoint.h @@ -0,0 +1,133 @@ +/* +* 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 "Box2D/Dynamics/Joints/b2Joint.h" + +/// 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 override; + b2Vec2 GetAnchorB() const override; + + b2Vec2 GetReactionForce(float32 inv_dt) const override; + float32 GetReactionTorque(float32 inv_dt) const override; + + /// 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() override; + +protected: + + friend class b2Joint; + + b2MotorJoint(const b2MotorJointDef* def); + + void InitVelocityConstraints(const b2SolverData& data) override; + void SolveVelocityConstraints(const b2SolverData& data) override; + bool SolvePositionConstraints(const b2SolverData& data) override; + + // 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/Source/3rdParty/Box2D/Dynamics/Joints/b2MouseJoint.cpp b/Source/3rdParty/Box2D/Dynamics/Joints/b2MouseJoint.cpp new file mode 100644 index 0000000..637e4cd --- /dev/null +++ b/Source/3rdParty/Box2D/Dynamics/Joints/b2MouseJoint.cpp @@ -0,0 +1,222 @@ +/* +* 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 "Box2D/Dynamics/Joints/b2MouseJoint.h" +#include "Box2D/Dynamics/b2Body.h" +#include "Box2D/Dynamics/b2TimeStep.h" + +// 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 (target != m_targetA) + { + 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/Source/3rdParty/Box2D/Dynamics/Joints/b2MouseJoint.h b/Source/3rdParty/Box2D/Dynamics/Joints/b2MouseJoint.h new file mode 100644 index 0000000..7441978 --- /dev/null +++ b/Source/3rdParty/Box2D/Dynamics/Joints/b2MouseJoint.h @@ -0,0 +1,129 @@ +/* +* 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 "Box2D/Dynamics/Joints/b2Joint.h" + +/// 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 override; + + /// Implements b2Joint. + b2Vec2 GetAnchorB() const override; + + /// Implements b2Joint. + b2Vec2 GetReactionForce(float32 inv_dt) const override; + + /// Implements b2Joint. + float32 GetReactionTorque(float32 inv_dt) const override; + + /// 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() override { b2Log("Mouse joint dumping is not supported.\n"); } + + /// Implement b2Joint::ShiftOrigin + void ShiftOrigin(const b2Vec2& newOrigin) override; + +protected: + friend class b2Joint; + + b2MouseJoint(const b2MouseJointDef* def); + + void InitVelocityConstraints(const b2SolverData& data) override; + void SolveVelocityConstraints(const b2SolverData& data) override; + bool SolvePositionConstraints(const b2SolverData& data) override; + + 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/Source/3rdParty/Box2D/Dynamics/Joints/b2PrismaticJoint.cpp b/Source/3rdParty/Box2D/Dynamics/Joints/b2PrismaticJoint.cpp new file mode 100644 index 0000000..5da19b6 --- /dev/null +++ b/Source/3rdParty/Box2D/Dynamics/Joints/b2PrismaticJoint.cpp @@ -0,0 +1,642 @@ +/* +* 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 "Box2D/Dynamics/Joints/b2PrismaticJoint.h" +#include "Box2D/Dynamics/b2Body.h" +#include "Box2D/Dynamics/b2TimeStep.h" + +// 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; +} + +// A velocity based solver computes reaction forces(impulses) using the velocity constraint solver.Under this context, +// the position solver is not there to resolve forces.It is only there to cope with integration error. +// +// Therefore, the pseudo impulses in the position solver do not have any physical meaning.Thus it is okay if they suck. +// +// We could take the active state from the velocity solver.However, the joint might push past the limit when the velocity +// solver indicates the limit is inactive. +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) +{ + if (flag != m_enableMotor) + { + m_bodyA->SetAwake(true); + m_bodyB->SetAwake(true); + m_enableMotor = flag; + } +} + +void b2PrismaticJoint::SetMotorSpeed(float32 speed) +{ + if (speed != m_motorSpeed) + { + m_bodyA->SetAwake(true); + m_bodyB->SetAwake(true); + m_motorSpeed = speed; + } +} + +void b2PrismaticJoint::SetMaxMotorForce(float32 force) +{ + if (force != m_maxMotorForce) + { + 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/Source/3rdParty/Box2D/Dynamics/Joints/b2PrismaticJoint.h b/Source/3rdParty/Box2D/Dynamics/Joints/b2PrismaticJoint.h new file mode 100644 index 0000000..131dffd --- /dev/null +++ b/Source/3rdParty/Box2D/Dynamics/Joints/b2PrismaticJoint.h @@ -0,0 +1,196 @@ +/* +* 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 "Box2D/Dynamics/Joints/b2Joint.h" + +/// 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 override; + b2Vec2 GetAnchorB() const override; + + b2Vec2 GetReactionForce(float32 inv_dt) const override; + float32 GetReactionTorque(float32 inv_dt) const override; + + /// 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() override; + +protected: + friend class b2Joint; + friend class b2GearJoint; + b2PrismaticJoint(const b2PrismaticJointDef* def); + + void InitVelocityConstraints(const b2SolverData& data) override; + void SolveVelocityConstraints(const b2SolverData& data) override; + bool SolvePositionConstraints(const b2SolverData& data) override; + + // 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/Source/3rdParty/Box2D/Dynamics/Joints/b2PulleyJoint.cpp b/Source/3rdParty/Box2D/Dynamics/Joints/b2PulleyJoint.cpp new file mode 100644 index 0000000..1525f41 --- /dev/null +++ b/Source/3rdParty/Box2D/Dynamics/Joints/b2PulleyJoint.cpp @@ -0,0 +1,348 @@ +/* +* 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 "Box2D/Dynamics/Joints/b2PulleyJoint.h" +#include "Box2D/Dynamics/b2Body.h" +#include "Box2D/Dynamics/b2TimeStep.h" + +// 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/Source/3rdParty/Box2D/Dynamics/Joints/b2PulleyJoint.h b/Source/3rdParty/Box2D/Dynamics/Joints/b2PulleyJoint.h new file mode 100644 index 0000000..71c759b --- /dev/null +++ b/Source/3rdParty/Box2D/Dynamics/Joints/b2PulleyJoint.h @@ -0,0 +1,152 @@ +/* +* 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 "Box2D/Dynamics/Joints/b2Joint.h" + +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 override; + b2Vec2 GetAnchorB() const override; + + b2Vec2 GetReactionForce(float32 inv_dt) const override; + float32 GetReactionTorque(float32 inv_dt) const override; + + /// 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() override; + + /// Implement b2Joint::ShiftOrigin + void ShiftOrigin(const b2Vec2& newOrigin) override; + +protected: + + friend class b2Joint; + b2PulleyJoint(const b2PulleyJointDef* data); + + void InitVelocityConstraints(const b2SolverData& data) override; + void SolveVelocityConstraints(const b2SolverData& data) override; + bool SolvePositionConstraints(const b2SolverData& data) override; + + 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/Source/3rdParty/Box2D/Dynamics/Joints/b2RevoluteJoint.cpp b/Source/3rdParty/Box2D/Dynamics/Joints/b2RevoluteJoint.cpp new file mode 100644 index 0000000..b3f7ee5 --- /dev/null +++ b/Source/3rdParty/Box2D/Dynamics/Joints/b2RevoluteJoint.cpp @@ -0,0 +1,511 @@ +/* +* 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 "Box2D/Dynamics/Joints/b2RevoluteJoint.h" +#include "Box2D/Dynamics/b2Body.h" +#include "Box2D/Dynamics/b2TimeStep.h" + +// 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) +{ + if (flag != m_enableMotor) + { + 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) +{ + if (speed != m_motorSpeed) + { + m_bodyA->SetAwake(true); + m_bodyB->SetAwake(true); + m_motorSpeed = speed; + } +} + +void b2RevoluteJoint::SetMaxMotorTorque(float32 torque) +{ + if (torque != m_maxMotorTorque) + { + 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/Source/3rdParty/Box2D/Dynamics/Joints/b2RevoluteJoint.h b/Source/3rdParty/Box2D/Dynamics/Joints/b2RevoluteJoint.h new file mode 100644 index 0000000..06b1455 --- /dev/null +++ b/Source/3rdParty/Box2D/Dynamics/Joints/b2RevoluteJoint.h @@ -0,0 +1,204 @@ +/* +* 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 "Box2D/Dynamics/Joints/b2Joint.h" + +/// 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 override; + b2Vec2 GetAnchorB() const override; + + /// 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 override; + + /// Get the reaction torque due to the joint limit given the inverse time step. + /// Unit is N*m. + float32 GetReactionTorque(float32 inv_dt) const override; + + /// 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() override; + +protected: + + friend class b2Joint; + friend class b2GearJoint; + + b2RevoluteJoint(const b2RevoluteJointDef* def); + + void InitVelocityConstraints(const b2SolverData& data) override; + void SolveVelocityConstraints(const b2SolverData& data) override; + bool SolvePositionConstraints(const b2SolverData& data) override; + + // 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/Source/3rdParty/Box2D/Dynamics/Joints/b2RopeJoint.cpp b/Source/3rdParty/Box2D/Dynamics/Joints/b2RopeJoint.cpp new file mode 100644 index 0000000..86d27e7 --- /dev/null +++ b/Source/3rdParty/Box2D/Dynamics/Joints/b2RopeJoint.cpp @@ -0,0 +1,241 @@ +/* +* 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 "Box2D/Dynamics/Joints/b2RopeJoint.h" +#include "Box2D/Dynamics/b2Body.h" +#include "Box2D/Dynamics/b2TimeStep.h" + + +// 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/Source/3rdParty/Box2D/Dynamics/Joints/b2RopeJoint.h b/Source/3rdParty/Box2D/Dynamics/Joints/b2RopeJoint.h new file mode 100644 index 0000000..ef5d6f7 --- /dev/null +++ b/Source/3rdParty/Box2D/Dynamics/Joints/b2RopeJoint.h @@ -0,0 +1,114 @@ +/* +* 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 "Box2D/Dynamics/Joints/b2Joint.h" + +/// 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 override; + b2Vec2 GetAnchorB() const override; + + b2Vec2 GetReactionForce(float32 inv_dt) const override; + float32 GetReactionTorque(float32 inv_dt) const override; + + /// 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() override; + +protected: + + friend class b2Joint; + b2RopeJoint(const b2RopeJointDef* data); + + void InitVelocityConstraints(const b2SolverData& data) override; + void SolveVelocityConstraints(const b2SolverData& data) override; + bool SolvePositionConstraints(const b2SolverData& data) override; + + // 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/Source/3rdParty/Box2D/Dynamics/Joints/b2WeldJoint.cpp b/Source/3rdParty/Box2D/Dynamics/Joints/b2WeldJoint.cpp new file mode 100644 index 0000000..b10cee8 --- /dev/null +++ b/Source/3rdParty/Box2D/Dynamics/Joints/b2WeldJoint.cpp @@ -0,0 +1,344 @@ +/* +* 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 "Box2D/Dynamics/Joints/b2WeldJoint.h" +#include "Box2D/Dynamics/b2Body.h" +#include "Box2D/Dynamics/b2TimeStep.h" + +// 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 if (K.ez.z == 0.0f) + { + K.GetInverse22(&m_mass); + m_gamma = 0.0f; + m_bias = 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; + if (K.ez.z > 0.0f) + { + impulse = -K.Solve33(C); + } + else + { + b2Vec2 impulse2 = -K.Solve22(C1); + impulse.Set(impulse2.x, impulse2.y, 0.0f); + } + + 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/Source/3rdParty/Box2D/Dynamics/Joints/b2WeldJoint.h b/Source/3rdParty/Box2D/Dynamics/Joints/b2WeldJoint.h new file mode 100644 index 0000000..81ba235 --- /dev/null +++ b/Source/3rdParty/Box2D/Dynamics/Joints/b2WeldJoint.h @@ -0,0 +1,126 @@ +/* +* 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 "Box2D/Dynamics/Joints/b2Joint.h" + +/// 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 override; + b2Vec2 GetAnchorB() const override; + + b2Vec2 GetReactionForce(float32 inv_dt) const override; + float32 GetReactionTorque(float32 inv_dt) const override; + + /// 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() override; + +protected: + + friend class b2Joint; + + b2WeldJoint(const b2WeldJointDef* def); + + void InitVelocityConstraints(const b2SolverData& data) override; + void SolveVelocityConstraints(const b2SolverData& data) override; + bool SolvePositionConstraints(const b2SolverData& data) override; + + 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/Source/3rdParty/Box2D/Dynamics/Joints/b2WheelJoint.cpp b/Source/3rdParty/Box2D/Dynamics/Joints/b2WheelJoint.cpp new file mode 100644 index 0000000..a95311e --- /dev/null +++ b/Source/3rdParty/Box2D/Dynamics/Joints/b2WheelJoint.cpp @@ -0,0 +1,456 @@ +/* +* 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 "Box2D/Dynamics/Joints/b2WheelJoint.h" +#include "Box2D/Dynamics/b2Body.h" +#include "Box2D/Dynamics/b2TimeStep.h" + +// 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 damp = 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 * (damp + 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::GetJointLinearSpeed() 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; +} + +float32 b2WheelJoint::GetJointAngle() const +{ + b2Body* bA = m_bodyA; + b2Body* bB = m_bodyB; + return bB->m_sweep.a - bA->m_sweep.a; +} + +float32 b2WheelJoint::GetJointAngularSpeed() 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) +{ + if (flag != m_enableMotor) + { + m_bodyA->SetAwake(true); + m_bodyB->SetAwake(true); + m_enableMotor = flag; + } +} + +void b2WheelJoint::SetMotorSpeed(float32 speed) +{ + if (speed != m_motorSpeed) + { + m_bodyA->SetAwake(true); + m_bodyB->SetAwake(true); + m_motorSpeed = speed; + } +} + +void b2WheelJoint::SetMaxMotorTorque(float32 torque) +{ + if (torque != m_maxMotorTorque) + { + 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/Source/3rdParty/Box2D/Dynamics/Joints/b2WheelJoint.h b/Source/3rdParty/Box2D/Dynamics/Joints/b2WheelJoint.h new file mode 100644 index 0000000..be7ad66 --- /dev/null +++ b/Source/3rdParty/Box2D/Dynamics/Joints/b2WheelJoint.h @@ -0,0 +1,216 @@ +/* +* 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 "Box2D/Dynamics/Joints/b2Joint.h" + +/// 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. In other words, it is a point to +/// line constraint with a rotational motor and a linear spring/damper. +/// This joint is designed for vehicle suspensions. +class b2WheelJoint : public b2Joint +{ +public: + b2Vec2 GetAnchorA() const override; + b2Vec2 GetAnchorB() const override; + + b2Vec2 GetReactionForce(float32 inv_dt) const override; + float32 GetReactionTorque(float32 inv_dt) const override; + + /// 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 linear speed, usually in meters per second. + float32 GetJointLinearSpeed() const; + + /// Get the current joint angle in radians. + float32 GetJointAngle() const; + + /// Get the current joint angular speed in radians per second. + float32 GetJointAngularSpeed() 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() override; + +protected: + + friend class b2Joint; + b2WheelJoint(const b2WheelJointDef* def); + + void InitVelocityConstraints(const b2SolverData& data) override; + void SolveVelocityConstraints(const b2SolverData& data) override; + bool SolvePositionConstraints(const b2SolverData& data) override; + + 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/Source/3rdParty/Box2D/Dynamics/b2Body.cpp b/Source/3rdParty/Box2D/Dynamics/b2Body.cpp new file mode 100644 index 0000000..54154b8 --- /dev/null +++ b/Source/3rdParty/Box2D/Dynamics/b2Body.cpp @@ -0,0 +1,554 @@ +/* +* 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 "Box2D/Dynamics/b2Body.h" +#include "Box2D/Dynamics/b2Fixture.h" +#include "Box2D/Dynamics/b2World.h" +#include "Box2D/Dynamics/Contacts/b2Contact.h" +#include "Box2D/Dynamics/Joints/b2Joint.h" + +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 = nullptr; + m_contactList = nullptr; + m_prev = nullptr; + m_next = nullptr; + + 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 = nullptr; + 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 = nullptr; + + // 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 nullptr; + } + + 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) +{ + if (fixture == NULL) + { + return; + } + + 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 != nullptr) + { + 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->m_body = nullptr; + fixture->m_next = nullptr; + fixture->Destroy(allocator); + 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 = nullptr; + } +} + +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/Source/3rdParty/Box2D/Dynamics/b2Body.h b/Source/3rdParty/Box2D/Dynamics/b2Body.h new file mode 100644 index 0000000..c191f6d --- /dev/null +++ b/Source/3rdParty/Box2D/Dynamics/b2Body.h @@ -0,0 +1,882 @@ +/* +* 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 "Box2D/Common/b2Math.h" +#include "Box2D/Collision/Shapes/b2Shape.h" +#include <memory> + +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 = nullptr; + 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. + /// Units are 1/time + 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. + /// Units are 1/time + 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. + /// @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 impulse to the center of mass. This immediately modifies the velocity. + /// @param impulse the world impulse vector, usually in N-seconds or kg-m/s. + /// @param wake also wake up the body + void ApplyLinearImpulseToCenter(const b2Vec2& impulse, 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) + { + 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::ApplyLinearImpulseToCenter(const b2Vec2& 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_linearVelocity += m_invMass * 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/Source/3rdParty/Box2D/Dynamics/b2ContactManager.cpp b/Source/3rdParty/Box2D/Dynamics/b2ContactManager.cpp new file mode 100644 index 0000000..051cc2f --- /dev/null +++ b/Source/3rdParty/Box2D/Dynamics/b2ContactManager.cpp @@ -0,0 +1,296 @@ +/* +* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +#include "Box2D/Dynamics/b2ContactManager.h" +#include "Box2D/Dynamics/b2Body.h" +#include "Box2D/Dynamics/b2Fixture.h" +#include "Box2D/Dynamics/b2WorldCallbacks.h" +#include "Box2D/Dynamics/Contacts/b2Contact.h" + +b2ContactFilter b2_defaultFilter; +b2ContactListener b2_defaultListener; + +b2ContactManager::b2ContactManager() +{ + m_contactList = nullptr; + m_contactCount = 0; + m_contactFilter = &b2_defaultFilter; + m_contactListener = &b2_defaultListener; + m_allocator = nullptr; +} + +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 == nullptr) + { + 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 = nullptr; + c->m_next = m_contactList; + if (m_contactList != nullptr) + { + 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 = nullptr; + c->m_nodeA.next = bodyA->m_contactList; + if (bodyA->m_contactList != nullptr) + { + 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 = nullptr; + c->m_nodeB.next = bodyB->m_contactList; + if (bodyB->m_contactList != nullptr) + { + 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/Source/3rdParty/Box2D/Dynamics/b2ContactManager.h b/Source/3rdParty/Box2D/Dynamics/b2ContactManager.h new file mode 100644 index 0000000..4c969e7 --- /dev/null +++ b/Source/3rdParty/Box2D/Dynamics/b2ContactManager.h @@ -0,0 +1,52 @@ +/* +* 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 "Box2D/Collision/b2BroadPhase.h" + +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/Source/3rdParty/Box2D/Dynamics/b2Fixture.cpp b/Source/3rdParty/Box2D/Dynamics/b2Fixture.cpp new file mode 100644 index 0000000..956b485 --- /dev/null +++ b/Source/3rdParty/Box2D/Dynamics/b2Fixture.cpp @@ -0,0 +1,303 @@ +/* +* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +#include "Box2D/Dynamics/b2Fixture.h" +#include "Box2D/Dynamics/Contacts/b2Contact.h" +#include "Box2D/Dynamics/b2World.h" +#include "Box2D/Collision/Shapes/b2CircleShape.h" +#include "Box2D/Collision/Shapes/b2EdgeShape.h" +#include "Box2D/Collision/Shapes/b2PolygonShape.h" +#include "Box2D/Collision/Shapes/b2ChainShape.h" +#include "Box2D/Collision/b2BroadPhase.h" +#include "Box2D/Collision/b2Collision.h" +#include "Box2D/Common/b2BlockAllocator.h" + +b2Fixture::b2Fixture() +{ + m_userData = nullptr; + m_body = nullptr; + m_next = nullptr; + m_proxies = nullptr; + m_proxyCount = 0; + m_shape = nullptr; + 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 = nullptr; + + 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 = nullptr; + 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 = nullptr; + + // 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 = nullptr; +} + +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 == nullptr) + { + 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 == nullptr) + { + 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/Source/3rdParty/Box2D/Dynamics/b2Fixture.h b/Source/3rdParty/Box2D/Dynamics/b2Fixture.h new file mode 100644 index 0000000..9f7a8aa --- /dev/null +++ b/Source/3rdParty/Box2D/Dynamics/b2Fixture.h @@ -0,0 +1,345 @@ +/* +* 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 "Box2D/Dynamics/b2Body.h" +#include "Box2D/Collision/b2Collision.h" +#include "Box2D/Collision/Shapes/b2Shape.h" + +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 = nullptr; + userData = nullptr; + 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 nullptr 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/Source/3rdParty/Box2D/Dynamics/b2Island.cpp b/Source/3rdParty/Box2D/Dynamics/b2Island.cpp new file mode 100644 index 0000000..dd19a8f --- /dev/null +++ b/Source/3rdParty/Box2D/Dynamics/b2Island.cpp @@ -0,0 +1,539 @@ +/* +* 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 "Box2D/Collision/b2Distance.h" +#include "Box2D/Dynamics/b2Island.h" +#include "Box2D/Dynamics/b2Body.h" +#include "Box2D/Dynamics/b2Fixture.h" +#include "Box2D/Dynamics/b2World.h" +#include "Box2D/Dynamics/Contacts/b2Contact.h" +#include "Box2D/Dynamics/Contacts/b2ContactSolver.h" +#include "Box2D/Dynamics/Joints/b2Joint.h" +#include "Box2D/Common/b2StackAllocator.h" +#include "Box2D/Common/b2Timer.h" + +/* +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 j = 0; j < m_jointCount; ++j) + { + bool jointOkay = m_joints[j]->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 == nullptr) + { + 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/Source/3rdParty/Box2D/Dynamics/b2Island.h b/Source/3rdParty/Box2D/Dynamics/b2Island.h new file mode 100644 index 0000000..68f6d4b --- /dev/null +++ b/Source/3rdParty/Box2D/Dynamics/b2Island.h @@ -0,0 +1,93 @@ +/* +* 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 "Box2D/Common/b2Math.h" +#include "Box2D/Dynamics/b2Body.h" +#include "Box2D/Dynamics/b2TimeStep.h" + +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/Source/3rdParty/Box2D/Dynamics/b2TimeStep.h b/Source/3rdParty/Box2D/Dynamics/b2TimeStep.h new file mode 100644 index 0000000..72a4838 --- /dev/null +++ b/Source/3rdParty/Box2D/Dynamics/b2TimeStep.h @@ -0,0 +1,70 @@ +/* +* 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 "Box2D/Common/b2Math.h" + +/// 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/Source/3rdParty/Box2D/Dynamics/b2World.cpp b/Source/3rdParty/Box2D/Dynamics/b2World.cpp new file mode 100644 index 0000000..f21812e --- /dev/null +++ b/Source/3rdParty/Box2D/Dynamics/b2World.cpp @@ -0,0 +1,1366 @@ +/* +* 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 "Box2D/Dynamics/b2World.h" +#include "Box2D/Dynamics/b2Body.h" +#include "Box2D/Dynamics/b2Fixture.h" +#include "Box2D/Dynamics/b2Island.h" +#include "Box2D/Dynamics/Joints/b2PulleyJoint.h" +#include "Box2D/Dynamics/Contacts/b2Contact.h" +#include "Box2D/Dynamics/Contacts/b2ContactSolver.h" +#include "Box2D/Collision/b2Collision.h" +#include "Box2D/Collision/b2BroadPhase.h" +#include "Box2D/Collision/Shapes/b2CircleShape.h" +#include "Box2D/Collision/Shapes/b2EdgeShape.h" +#include "Box2D/Collision/Shapes/b2ChainShape.h" +#include "Box2D/Collision/Shapes/b2PolygonShape.h" +#include "Box2D/Collision/b2TimeOfImpact.h" +#include "Box2D/Common/b2Draw.h" +#include "Box2D/Common/b2Timer.h" +#include <new> + +b2World::b2World(const b2Vec2& gravity) +{ + m_destructionListener = nullptr; + m_debugDraw = nullptr; + + m_bodyList = nullptr; + m_jointList = nullptr; + + 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 nullptr; + } + + void* mem = m_blockAllocator.Allocate(sizeof(b2Body)); + b2Body* b = new (mem) b2Body(def, this); + + // Add to world doubly linked list. + b->m_prev = nullptr; + 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 = nullptr; + + // 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 = nullptr; + + // 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 = nullptr; + 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 nullptr; + } + + b2Joint* j = b2Joint::Create(def, &m_blockAllocator); + + // Connect to the world list. + j->m_prev = nullptr; + 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 = nullptr; + 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 = nullptr; + 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 = nullptr; + j->m_edgeA.next = nullptr; + + // 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 = nullptr; + j->m_edgeB.next = nullptr; + + 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 (without resetting sleep timer). + b->m_flags |= b2Body::e_awakeFlag; + + // 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 = nullptr; + 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 == nullptr || 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; + + b2Color ghostColor(0.75f * color.r, 0.75f * color.g, 0.75f * color.b, color.a); + + b2Vec2 v1 = b2Mul(xf, vertices[0]); + m_debugDraw->DrawPoint(v1, 4.0f, color); + + if (chain->m_hasPrevVertex) + { + b2Vec2 vp = b2Mul(xf, chain->m_prevVertex); + m_debugDraw->DrawSegment(vp, v1, ghostColor); + m_debugDraw->DrawCircle(vp, 0.1f, ghostColor); + } + + for (int32 i = 1; i < count; ++i) + { + b2Vec2 v2 = b2Mul(xf, vertices[i]); + m_debugDraw->DrawSegment(v1, v2, color); + m_debugDraw->DrawPoint(v2, 4.0f, color); + v1 = v2; + } + + if (chain->m_hasNextVertex) + { + b2Vec2 vn = b2Mul(xf, chain->m_nextVertex); + m_debugDraw->DrawSegment(v1, vn, ghostColor); + m_debugDraw->DrawCircle(vn, 0.1f, ghostColor); + } + } + 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: + { + b2Color c; + c.Set(0.0f, 1.0f, 0.0f); + m_debugDraw->DrawPoint(p1, 4.0f, c); + m_debugDraw->DrawPoint(p2, 4.0f, c); + + c.Set(0.8f, 0.8f, 0.8f); + m_debugDraw->DrawSegment(p1, p2, c); + + } + 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 == nullptr) + { + 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(); + + //g_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 = nullptr;\n"); + b2Log("bodies = nullptr;\n"); +} diff --git a/Source/3rdParty/Box2D/Dynamics/b2World.h b/Source/3rdParty/Box2D/Dynamics/b2World.h new file mode 100644 index 0000000..d5ad20c --- /dev/null +++ b/Source/3rdParty/Box2D/Dynamics/b2World.h @@ -0,0 +1,354 @@ +/* +* 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 "Box2D/Common/b2Math.h" +#include "Box2D/Common/b2BlockAllocator.h" +#include "Box2D/Common/b2StackAllocator.h" +#include "Box2D/Dynamics/b2ContactManager.h" +#include "Box2D/Dynamics/b2WorldCallbacks.h" +#include "Box2D/Dynamics/b2TimeStep.h" + +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 nullptr 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 nullptr 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 nullptr 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/Source/3rdParty/Box2D/Dynamics/b2WorldCallbacks.cpp b/Source/3rdParty/Box2D/Dynamics/b2WorldCallbacks.cpp new file mode 100644 index 0000000..fe71073 --- /dev/null +++ b/Source/3rdParty/Box2D/Dynamics/b2WorldCallbacks.cpp @@ -0,0 +1,36 @@ +/* +* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +#include "Box2D/Dynamics/b2WorldCallbacks.h" +#include "Box2D/Dynamics/b2Fixture.h" + +// 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/Source/3rdParty/Box2D/Dynamics/b2WorldCallbacks.h b/Source/3rdParty/Box2D/Dynamics/b2WorldCallbacks.h new file mode 100644 index 0000000..3d5580a --- /dev/null +++ b/Source/3rdParty/Box2D/Dynamics/b2WorldCallbacks.h @@ -0,0 +1,155 @@ +/* +* 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 "Box2D/Common/b2Settings.h" + +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 |