From 4a87c364511cf94f4a25127f40d4a3df440d7faa Mon Sep 17 00:00:00 2001 From: oznogon Date: Sat, 21 Jan 2023 21:34:23 -0800 Subject: [PATCH] [libs] Update box2d to 2.4.1 --- CMakeLists.txt | 2 +- libs/Box2D/Box2D.h | 68 - libs/Box2D/CMakeLists.txt | 78 - libs/Box2D/Collision/Shapes/b2ChainShape.h | 105 - libs/Box2D/Collision/Shapes/b2CircleShape.h | 91 - libs/Box2D/Collision/Shapes/b2EdgeShape.h | 74 - libs/Box2D/Collision/Shapes/b2PolygonShape.h | 101 - libs/Box2D/Collision/b2CollideEdge.cpp | 698 ----- libs/Box2D/Collision/b2Collision.cpp | 252 -- libs/Box2D/Collision/b2Distance.h | 141 - libs/Box2D/Collision/b2TimeOfImpact.h | 58 - libs/Box2D/Common/b2BlockAllocator.h | 62 - libs/Box2D/Common/b2Draw.cpp | 44 - libs/Box2D/Common/b2Draw.h | 86 - libs/Box2D/Common/b2GrowableStack.h | 86 - libs/Box2D/Common/b2Math.cpp | 94 - libs/Box2D/Common/b2Settings.cpp | 44 - libs/Box2D/Common/b2StackAllocator.cpp | 83 - libs/Box2D/Common/b2StackAllocator.h | 60 - libs/Box2D/Common/b2Timer.cpp | 101 - libs/Box2D/Common/b2Timer.h | 50 - .../Contacts/b2ChainAndCircleContact.cpp | 53 - .../Contacts/b2ChainAndCircleContact.h | 39 - .../Contacts/b2ChainAndPolygonContact.cpp | 53 - .../Contacts/b2ChainAndPolygonContact.h | 39 - .../Dynamics/Contacts/b2CircleContact.cpp | 52 - .../Box2D/Dynamics/Contacts/b2CircleContact.h | 39 - .../Box2D/Dynamics/Contacts/b2ContactSolver.h | 95 - .../Contacts/b2EdgeAndCircleContact.cpp | 49 - .../Contacts/b2EdgeAndCircleContact.h | 39 - .../Contacts/b2EdgeAndPolygonContact.cpp | 49 - .../Contacts/b2EdgeAndPolygonContact.h | 39 - .../Contacts/b2PolygonAndCircleContact.cpp | 49 - .../Contacts/b2PolygonAndCircleContact.h | 38 - .../Dynamics/Contacts/b2PolygonContact.cpp | 52 - .../Dynamics/Contacts/b2PolygonContact.h | 39 - .../Box2D/Dynamics/Joints/b2DistanceJoint.cpp | 260 -- libs/Box2D/Dynamics/Joints/b2DistanceJoint.h | 169 -- libs/Box2D/Dynamics/Joints/b2FrictionJoint.h | 119 - libs/Box2D/Dynamics/Joints/b2GearJoint.h | 125 - libs/Box2D/Dynamics/Joints/b2Joint.cpp | 211 -- libs/Box2D/Dynamics/Joints/b2MotorJoint.h | 133 - libs/Box2D/Dynamics/Joints/b2MouseJoint.h | 129 - .../Dynamics/Joints/b2PrismaticJoint.cpp | 629 ---- .../Box2D/Dynamics/Joints/b2RevoluteJoint.cpp | 502 ---- libs/Box2D/Dynamics/Joints/b2RopeJoint.cpp | 241 -- libs/Box2D/Dynamics/Joints/b2RopeJoint.h | 114 - libs/Box2D/Dynamics/Joints/b2WeldJoint.h | 126 - libs/Box2D/Dynamics/Joints/b2WheelJoint.cpp | 419 --- libs/Box2D/Dynamics/Joints/b2WheelJoint.h | 211 -- libs/Box2D/Dynamics/b2ContactManager.h | 52 - libs/Box2D/Dynamics/b2TimeStep.h | 70 - libs/Box2D/Dynamics/b2WorldCallbacks.cpp | 36 - libs/Box2D/License.txt | 18 - libs/Box2D/Rope/b2Rope.cpp | 259 -- libs/Box2D/Rope/b2Rope.h | 115 - libs/box2d/CMakeLists.txt | 77 + libs/box2d/b2_api.h | 52 + libs/box2d/b2_block_allocator.h | 60 + .../Dynamics/b2Body.h => box2d/b2_body.h} | 1745 +++++------ .../b2BroadPhase.h => box2d/b2_broad_phase.h} | 495 ++- libs/box2d/b2_chain_shape.h | 101 + libs/box2d/b2_circle_shape.h | 67 + .../b2Collision.h => box2d/b2_collision.h} | 583 ++-- .../Common/b2Settings.h => box2d/b2_common.h} | 289 +- .../b2Contact.h => box2d/b2_contact.h} | 735 ++--- libs/box2d/b2_contact_manager.h | 57 + libs/box2d/b2_distance.h | 171 ++ libs/box2d/b2_distance_joint.h | 176 ++ libs/box2d/b2_draw.h | 102 + .../b2_dynamic_tree.h} | 597 ++-- libs/box2d/b2_edge_shape.h | 86 + .../b2Fixture.h => box2d/b2_fixture.h} | 716 ++--- libs/box2d/b2_friction_joint.h | 124 + libs/box2d/b2_gear_joint.h | 131 + libs/box2d/b2_growable_stack.h | 91 + .../Joints/b2Joint.h => box2d/b2_joint.h} | 459 +-- .../Common/b2Math.h => box2d/b2_math.h} | 1438 +++++---- libs/box2d/b2_motor_joint.h | 138 + libs/box2d/b2_mouse_joint.h | 134 + libs/box2d/b2_polygon_shape.h | 93 + .../b2_prismatic_joint.h} | 401 +-- .../b2_pulley_joint.h} | 309 +- .../b2_revolute_joint.h} | 415 +-- libs/box2d/b2_rope.h | 155 + libs/box2d/b2_settings.h | 127 + .../Shapes/b2Shape.h => box2d/b2_shape.h} | 211 +- libs/box2d/b2_stack_allocator.h | 65 + libs/box2d/b2_time_of_impact.h | 63 + libs/box2d/b2_time_step.h | 74 + libs/box2d/b2_timer.h | 55 + libs/box2d/b2_types.h | 33 + libs/box2d/b2_weld_joint.h | 133 + libs/box2d/b2_wheel_joint.h | 240 ++ .../Dynamics/b2World.h => box2d/b2_world.h} | 106 +- .../b2_world_callbacks.h} | 316 +- libs/box2d/box2d.h | 58 + .../src/collision/b2_broad_phase.cpp} | 250 +- .../src/collision/b2_chain_shape.cpp} | 378 ++- .../src/collision/b2_circle_shape.cpp} | 204 +- .../src/collision/b2_collide_circle.cpp} | 312 +- libs/box2d/src/collision/b2_collide_edge.cpp | 524 ++++ .../src/collision/b2_collide_polygon.cpp} | 482 +-- libs/box2d/src/collision/b2_collision.cpp | 580 ++++ .../src/collision/b2_distance.cpp} | 1347 +++++---- .../src/collision/b2_dynamic_tree.cpp} | 1579 +++++----- .../src/collision/b2_edge_shape.cpp} | 296 +- .../src/collision/b2_polygon_shape.cpp} | 836 +++--- .../src/collision/b2_time_of_impact.cpp} | 976 +++--- .../src/common/b2_block_allocator.cpp} | 445 +-- libs/box2d/src/common/b2_draw.cpp | 47 + libs/box2d/src/common/b2_math.cpp | 98 + libs/box2d/src/common/b2_settings.cpp | 74 + libs/box2d/src/common/b2_stack_allocator.cpp | 87 + libs/box2d/src/common/b2_timer.cpp | 125 + .../src/dynamics/b2_body.cpp} | 1119 +++---- .../src/dynamics/b2_chain_circle_contact.cpp | 57 + .../src/dynamics/b2_chain_circle_contact.h | 43 + .../src/dynamics/b2_chain_polygon_contact.cpp | 57 + .../src/dynamics/b2_chain_polygon_contact.h | 43 + libs/box2d/src/dynamics/b2_circle_contact.cpp | 56 + libs/box2d/src/dynamics/b2_circle_contact.h | 43 + .../src/dynamics/b2_contact.cpp} | 499 ++-- .../src/dynamics/b2_contact_manager.cpp} | 589 ++-- .../src/dynamics/b2_contact_solver.cpp} | 1681 +++++------ libs/box2d/src/dynamics/b2_contact_solver.h | 100 + libs/box2d/src/dynamics/b2_distance_joint.cpp | 421 +++ .../src/dynamics/b2_edge_circle_contact.cpp | 54 + .../src/dynamics/b2_edge_circle_contact.h | 43 + .../src/dynamics/b2_edge_polygon_contact.cpp | 54 + .../src/dynamics/b2_edge_polygon_contact.h | 43 + .../src/dynamics/b2_fixture.cpp} | 608 ++-- .../src/dynamics/b2_friction_joint.cpp} | 506 ++-- .../src/dynamics/b2_gear_joint.cpp} | 856 +++--- .../src/dynamics/b2_island.cpp} | 1083 +++---- .../src/dynamics/b2_island.h} | 190 +- libs/box2d/src/dynamics/b2_joint.cpp | 301 ++ .../src/dynamics/b2_motor_joint.cpp} | 615 ++-- .../src/dynamics/b2_mouse_joint.cpp} | 404 ++- .../dynamics/b2_polygon_circle_contact.cpp | 54 + .../src/dynamics/b2_polygon_circle_contact.h | 42 + .../box2d/src/dynamics/b2_polygon_contact.cpp | 57 + libs/box2d/src/dynamics/b2_polygon_contact.h | 43 + .../box2d/src/dynamics/b2_prismatic_joint.cpp | 643 ++++ .../src/dynamics/b2_pulley_joint.cpp} | 690 ++--- libs/box2d/src/dynamics/b2_revolute_joint.cpp | 501 ++++ .../src/dynamics/b2_weld_joint.cpp} | 688 ++--- libs/box2d/src/dynamics/b2_wheel_joint.cpp | 672 +++++ .../src/dynamics/b2_world.cpp} | 2661 ++++++++--------- .../box2d/src/dynamics/b2_world_callbacks.cpp | 40 + libs/box2d/src/rope/b2_rope.cpp | 809 +++++ src/collisionable.cpp | 4 +- 152 files changed, 22570 insertions(+), 20957 deletions(-) delete mode 100644 libs/Box2D/Box2D.h delete mode 100644 libs/Box2D/CMakeLists.txt delete mode 100644 libs/Box2D/Collision/Shapes/b2ChainShape.h delete mode 100644 libs/Box2D/Collision/Shapes/b2CircleShape.h delete mode 100644 libs/Box2D/Collision/Shapes/b2EdgeShape.h delete mode 100644 libs/Box2D/Collision/Shapes/b2PolygonShape.h delete mode 100644 libs/Box2D/Collision/b2CollideEdge.cpp delete mode 100644 libs/Box2D/Collision/b2Collision.cpp delete mode 100644 libs/Box2D/Collision/b2Distance.h delete mode 100644 libs/Box2D/Collision/b2TimeOfImpact.h delete mode 100644 libs/Box2D/Common/b2BlockAllocator.h delete mode 100644 libs/Box2D/Common/b2Draw.cpp delete mode 100644 libs/Box2D/Common/b2Draw.h delete mode 100644 libs/Box2D/Common/b2GrowableStack.h delete mode 100644 libs/Box2D/Common/b2Math.cpp delete mode 100644 libs/Box2D/Common/b2Settings.cpp delete mode 100644 libs/Box2D/Common/b2StackAllocator.cpp delete mode 100644 libs/Box2D/Common/b2StackAllocator.h delete mode 100644 libs/Box2D/Common/b2Timer.cpp delete mode 100644 libs/Box2D/Common/b2Timer.h delete mode 100644 libs/Box2D/Dynamics/Contacts/b2ChainAndCircleContact.cpp delete mode 100644 libs/Box2D/Dynamics/Contacts/b2ChainAndCircleContact.h delete mode 100644 libs/Box2D/Dynamics/Contacts/b2ChainAndPolygonContact.cpp delete mode 100644 libs/Box2D/Dynamics/Contacts/b2ChainAndPolygonContact.h delete mode 100644 libs/Box2D/Dynamics/Contacts/b2CircleContact.cpp delete mode 100644 libs/Box2D/Dynamics/Contacts/b2CircleContact.h delete mode 100644 libs/Box2D/Dynamics/Contacts/b2ContactSolver.h delete mode 100644 libs/Box2D/Dynamics/Contacts/b2EdgeAndCircleContact.cpp delete mode 100644 libs/Box2D/Dynamics/Contacts/b2EdgeAndCircleContact.h delete mode 100644 libs/Box2D/Dynamics/Contacts/b2EdgeAndPolygonContact.cpp delete mode 100644 libs/Box2D/Dynamics/Contacts/b2EdgeAndPolygonContact.h delete mode 100644 libs/Box2D/Dynamics/Contacts/b2PolygonAndCircleContact.cpp delete mode 100644 libs/Box2D/Dynamics/Contacts/b2PolygonAndCircleContact.h delete mode 100644 libs/Box2D/Dynamics/Contacts/b2PolygonContact.cpp delete mode 100644 libs/Box2D/Dynamics/Contacts/b2PolygonContact.h delete mode 100644 libs/Box2D/Dynamics/Joints/b2DistanceJoint.cpp delete mode 100644 libs/Box2D/Dynamics/Joints/b2DistanceJoint.h delete mode 100644 libs/Box2D/Dynamics/Joints/b2FrictionJoint.h delete mode 100644 libs/Box2D/Dynamics/Joints/b2GearJoint.h delete mode 100644 libs/Box2D/Dynamics/Joints/b2Joint.cpp delete mode 100644 libs/Box2D/Dynamics/Joints/b2MotorJoint.h delete mode 100644 libs/Box2D/Dynamics/Joints/b2MouseJoint.h delete mode 100644 libs/Box2D/Dynamics/Joints/b2PrismaticJoint.cpp delete mode 100644 libs/Box2D/Dynamics/Joints/b2RevoluteJoint.cpp delete mode 100644 libs/Box2D/Dynamics/Joints/b2RopeJoint.cpp delete mode 100644 libs/Box2D/Dynamics/Joints/b2RopeJoint.h delete mode 100644 libs/Box2D/Dynamics/Joints/b2WeldJoint.h delete mode 100644 libs/Box2D/Dynamics/Joints/b2WheelJoint.cpp delete mode 100644 libs/Box2D/Dynamics/Joints/b2WheelJoint.h delete mode 100644 libs/Box2D/Dynamics/b2ContactManager.h delete mode 100644 libs/Box2D/Dynamics/b2TimeStep.h delete mode 100644 libs/Box2D/Dynamics/b2WorldCallbacks.cpp delete mode 100644 libs/Box2D/License.txt delete mode 100644 libs/Box2D/Rope/b2Rope.cpp delete mode 100644 libs/Box2D/Rope/b2Rope.h create mode 100644 libs/box2d/CMakeLists.txt create mode 100644 libs/box2d/b2_api.h create mode 100644 libs/box2d/b2_block_allocator.h rename libs/{Box2D/Dynamics/b2Body.h => box2d/b2_body.h} (77%) rename libs/{Box2D/Collision/b2BroadPhase.h => box2d/b2_broad_phase.h} (69%) create mode 100644 libs/box2d/b2_chain_shape.h create mode 100644 libs/box2d/b2_circle_shape.h rename libs/{Box2D/Collision/b2Collision.h => box2d/b2_collision.h} (67%) rename libs/{Box2D/Common/b2Settings.h => box2d/b2_common.h} (57%) rename libs/{Box2D/Dynamics/Contacts/b2Contact.h => box2d/b2_contact.h} (69%) create mode 100644 libs/box2d/b2_contact_manager.h create mode 100644 libs/box2d/b2_distance.h create mode 100644 libs/box2d/b2_distance_joint.h create mode 100644 libs/box2d/b2_draw.h rename libs/{Box2D/Collision/b2DynamicTree.h => box2d/b2_dynamic_tree.h} (77%) create mode 100644 libs/box2d/b2_edge_shape.h rename libs/{Box2D/Dynamics/b2Fixture.h => box2d/b2_fixture.h} (70%) create mode 100644 libs/box2d/b2_friction_joint.h create mode 100644 libs/box2d/b2_gear_joint.h create mode 100644 libs/box2d/b2_growable_stack.h rename libs/{Box2D/Dynamics/Joints/b2Joint.h => box2d/b2_joint.h} (61%) rename libs/{Box2D/Common/b2Math.h => box2d/b2_math.h} (74%) create mode 100644 libs/box2d/b2_motor_joint.h create mode 100644 libs/box2d/b2_mouse_joint.h create mode 100644 libs/box2d/b2_polygon_shape.h rename libs/{Box2D/Dynamics/Joints/b2PrismaticJoint.h => box2d/b2_prismatic_joint.h} (56%) rename libs/{Box2D/Dynamics/Joints/b2PulleyJoint.h => box2d/b2_pulley_joint.h} (54%) rename libs/{Box2D/Dynamics/Joints/b2RevoluteJoint.h => box2d/b2_revolute_joint.h} (53%) create mode 100644 libs/box2d/b2_rope.h create mode 100644 libs/box2d/b2_settings.h rename libs/{Box2D/Collision/Shapes/b2Shape.h => box2d/b2_shape.h} (60%) create mode 100644 libs/box2d/b2_stack_allocator.h create mode 100644 libs/box2d/b2_time_of_impact.h create mode 100644 libs/box2d/b2_time_step.h create mode 100644 libs/box2d/b2_timer.h create mode 100644 libs/box2d/b2_types.h create mode 100644 libs/box2d/b2_weld_joint.h create mode 100644 libs/box2d/b2_wheel_joint.h rename libs/{Box2D/Dynamics/b2World.h => box2d/b2_world.h} (81%) rename libs/{Box2D/Dynamics/b2WorldCallbacks.h => box2d/b2_world_callbacks.h} (73%) create mode 100644 libs/box2d/box2d.h rename libs/{Box2D/Collision/b2BroadPhase.cpp => box2d/src/collision/b2_broad_phase.cpp} (60%) rename libs/{Box2D/Collision/Shapes/b2ChainShape.cpp => box2d/src/collision/b2_chain_shape.cpp} (58%) rename libs/{Box2D/Collision/Shapes/b2CircleShape.cpp => box2d/src/collision/b2_circle_shape.cpp} (56%) rename libs/{Box2D/Collision/b2CollideCircle.cpp => box2d/src/collision/b2_collide_circle.cpp} (63%) create mode 100644 libs/box2d/src/collision/b2_collide_edge.cpp rename libs/{Box2D/Collision/b2CollidePolygon.cpp => box2d/src/collision/b2_collide_polygon.cpp} (71%) create mode 100644 libs/box2d/src/collision/b2_collision.cpp rename libs/{Box2D/Collision/b2Distance.cpp => box2d/src/collision/b2_distance.cpp} (62%) rename libs/{Box2D/Collision/b2DynamicTree.cpp => box2d/src/collision/b2_dynamic_tree.cpp} (83%) rename libs/{Box2D/Collision/Shapes/b2EdgeShape.cpp => box2d/src/collision/b2_edge_shape.cpp} (52%) rename libs/{Box2D/Collision/Shapes/b2PolygonShape.cpp => box2d/src/collision/b2_polygon_shape.cpp} (51%) rename libs/{Box2D/Collision/b2TimeOfImpact.cpp => box2d/src/collision/b2_time_of_impact.cpp} (74%) rename libs/{Box2D/Common/b2BlockAllocator.cpp => box2d/src/common/b2_block_allocator.cpp} (59%) create mode 100644 libs/box2d/src/common/b2_draw.cpp create mode 100644 libs/box2d/src/common/b2_math.cpp create mode 100644 libs/box2d/src/common/b2_settings.cpp create mode 100644 libs/box2d/src/common/b2_stack_allocator.cpp create mode 100644 libs/box2d/src/common/b2_timer.cpp rename libs/{Box2D/Dynamics/b2Body.cpp => box2d/src/dynamics/b2_body.cpp} (69%) create mode 100644 libs/box2d/src/dynamics/b2_chain_circle_contact.cpp create mode 100644 libs/box2d/src/dynamics/b2_chain_circle_contact.h create mode 100644 libs/box2d/src/dynamics/b2_chain_polygon_contact.cpp create mode 100644 libs/box2d/src/dynamics/b2_chain_polygon_contact.h create mode 100644 libs/box2d/src/dynamics/b2_circle_contact.cpp create mode 100644 libs/box2d/src/dynamics/b2_circle_contact.h rename libs/{Box2D/Dynamics/Contacts/b2Contact.cpp => box2d/src/dynamics/b2_contact.cpp} (70%) rename libs/{Box2D/Dynamics/b2ContactManager.cpp => box2d/src/dynamics/b2_contact_manager.cpp} (75%) rename libs/{Box2D/Dynamics/Contacts/b2ContactSolver.cpp => box2d/src/dynamics/b2_contact_solver.cpp} (76%) create mode 100644 libs/box2d/src/dynamics/b2_contact_solver.h create mode 100644 libs/box2d/src/dynamics/b2_distance_joint.cpp create mode 100644 libs/box2d/src/dynamics/b2_edge_circle_contact.cpp create mode 100644 libs/box2d/src/dynamics/b2_edge_circle_contact.h create mode 100644 libs/box2d/src/dynamics/b2_edge_polygon_contact.cpp create mode 100644 libs/box2d/src/dynamics/b2_edge_polygon_contact.h rename libs/{Box2D/Dynamics/b2Fixture.cpp => box2d/src/dynamics/b2_fixture.cpp} (52%) rename libs/{Box2D/Dynamics/Joints/b2FrictionJoint.cpp => box2d/src/dynamics/b2_friction_joint.cpp} (61%) rename libs/{Box2D/Dynamics/Joints/b2GearJoint.cpp => box2d/src/dynamics/b2_gear_joint.cpp} (71%) rename libs/{Box2D/Dynamics/b2Island.cpp => box2d/src/dynamics/b2_island.cpp} (83%) rename libs/{Box2D/Dynamics/b2Island.h => box2d/src/dynamics/b2_island.h} (54%) create mode 100644 libs/box2d/src/dynamics/b2_joint.cpp rename libs/{Box2D/Dynamics/Joints/b2MotorJoint.cpp => box2d/src/dynamics/b2_motor_joint.cpp} (60%) rename libs/{Box2D/Dynamics/Joints/b2MouseJoint.cpp => box2d/src/dynamics/b2_mouse_joint.cpp} (54%) create mode 100644 libs/box2d/src/dynamics/b2_polygon_circle_contact.cpp create mode 100644 libs/box2d/src/dynamics/b2_polygon_circle_contact.h create mode 100644 libs/box2d/src/dynamics/b2_polygon_contact.cpp create mode 100644 libs/box2d/src/dynamics/b2_polygon_contact.h create mode 100644 libs/box2d/src/dynamics/b2_prismatic_joint.cpp rename libs/{Box2D/Dynamics/Joints/b2PulleyJoint.cpp => box2d/src/dynamics/b2_pulley_joint.cpp} (61%) create mode 100644 libs/box2d/src/dynamics/b2_revolute_joint.cpp rename libs/{Box2D/Dynamics/Joints/b2WeldJoint.cpp => box2d/src/dynamics/b2_weld_joint.cpp} (64%) create mode 100644 libs/box2d/src/dynamics/b2_wheel_joint.cpp rename libs/{Box2D/Dynamics/b2World.cpp => box2d/src/dynamics/b2_world.cpp} (78%) create mode 100644 libs/box2d/src/dynamics/b2_world_callbacks.cpp create mode 100644 libs/box2d/src/rope/b2_rope.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index ddfd9539..3308d8b2 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -74,7 +74,7 @@ else() endif() endif() -add_subdirectory(libs/Box2D) +add_subdirectory(libs/box2d) add_subdirectory(libs/glad) # See cmake/GenerateGlDebug.cmake for details. diff --git a/libs/Box2D/Box2D.h b/libs/Box2D/Box2D.h deleted file mode 100644 index f98f87a5..00000000 --- a/libs/Box2D/Box2D.h +++ /dev/null @@ -1,68 +0,0 @@ -/* -* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef BOX2D_H -#define BOX2D_H - -/** -\mainpage Box2D API Documentation - -\section intro_sec Getting Started - -For documentation please see http://box2d.org/documentation.html - -For discussion please visit http://box2d.org/forum -*/ - -// These include files constitute the main Box2D API - -#include -#include -#include - -#include -#include -#include -#include - -#include -#include -#include -#include - -#include -#include -#include -#include -#include - -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#endif diff --git a/libs/Box2D/CMakeLists.txt b/libs/Box2D/CMakeLists.txt deleted file mode 100644 index 38ef2bbc..00000000 --- a/libs/Box2D/CMakeLists.txt +++ /dev/null @@ -1,78 +0,0 @@ -cmake_minimum_required(VERSION 3.7) -project(Box2D LANGUAGES CXX) - -#---------------------------------File lists----------------------------------- -set(source_files - Collision/b2BroadPhase.cpp - Collision/b2CollideEdge.cpp - Collision/b2CollideCircle.cpp - Collision/b2CollidePolygon.cpp - Collision/b2Collision.cpp - Collision/b2Distance.cpp - Collision/b2DynamicTree.cpp - Collision/b2TimeOfImpact.cpp - Collision/Shapes/b2ChainShape.cpp - Collision/Shapes/b2CircleShape.cpp - Collision/Shapes/b2EdgeShape.cpp - Collision/Shapes/b2PolygonShape.cpp - - Common/b2BlockAllocator.cpp - Common/b2Draw.cpp - Common/b2Math.cpp - Common/b2Settings.cpp - Common/b2StackAllocator.cpp - Common/b2Timer.cpp - - Dynamics/b2Body.cpp - Dynamics/b2ContactManager.cpp - Dynamics/b2Fixture.cpp - Dynamics/b2Island.cpp - Dynamics/b2World.cpp - Dynamics/b2WorldCallbacks.cpp - Dynamics/Contacts/b2ChainAndCircleContact.cpp - Dynamics/Contacts/b2ChainAndPolygonContact.cpp - Dynamics/Contacts/b2CircleContact.cpp - Dynamics/Contacts/b2Contact.cpp - Dynamics/Contacts/b2ContactSolver.cpp - Dynamics/Contacts/b2EdgeAndCircleContact.cpp - Dynamics/Contacts/b2EdgeAndPolygonContact.cpp - Dynamics/Contacts/b2PolygonAndCircleContact.cpp - Dynamics/Contacts/b2PolygonContact.cpp - Dynamics/Joints/b2DistanceJoint.cpp - Dynamics/Joints/b2FrictionJoint.cpp - Dynamics/Joints/b2GearJoint.cpp - Dynamics/Joints/b2Joint.cpp - Dynamics/Joints/b2MotorJoint.cpp - Dynamics/Joints/b2MouseJoint.cpp - Dynamics/Joints/b2PrismaticJoint.cpp - Dynamics/Joints/b2PulleyJoint.cpp - Dynamics/Joints/b2RevoluteJoint.cpp - Dynamics/Joints/b2RopeJoint.cpp - Dynamics/Joints/b2WeldJoint.cpp - Dynamics/Joints/b2WheelJoint.cpp - - Rope/b2Rope.cpp -) - -#----------------------------------Compiling----------------------------------- -add_library(box2d STATIC ${source_files}) - -target_include_directories(box2d - PUBLIC - $) - -#--------------------------------Installation---------------------------------- -install( - TARGETS box2d - EXPORT box2d - DESTINATION lib -) -install( - DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} - DESTINATION include - FILES_MATCHING PATTERN "*.h" -) -install( - EXPORT box2d - DESTINATION share/box2d -) diff --git a/libs/Box2D/Collision/Shapes/b2ChainShape.h b/libs/Box2D/Collision/Shapes/b2ChainShape.h deleted file mode 100644 index 8b378bc8..00000000 --- a/libs/Box2D/Collision/Shapes/b2ChainShape.h +++ /dev/null @@ -1,105 +0,0 @@ -/* -* Copyright (c) 2006-2010 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef B2_CHAIN_SHAPE_H -#define B2_CHAIN_SHAPE_H - -#include - -class b2EdgeShape; - -/// A chain shape is a free form sequence of line segments. -/// The chain has two-sided collision, so you can use inside and outside collision. -/// Therefore, you may use any winding order. -/// Since there may be many vertices, they are allocated using b2Alloc. -/// Connectivity information is used to create smooth collisions. -/// WARNING: The chain will not collide properly if there are self-intersections. -class b2ChainShape : public b2Shape -{ -public: - b2ChainShape(); - - /// The destructor frees the vertices using b2Free. - ~b2ChainShape(); - - /// Clear all data. - void Clear(); - - /// Create a loop. This automatically adjusts connectivity. - /// @param vertices an array of vertices, these are copied - /// @param count the vertex count - void CreateLoop(const b2Vec2* vertices, int32 count); - - /// Create a chain with isolated end vertices. - /// @param vertices an array of vertices, these are copied - /// @param count the vertex count - void CreateChain(const b2Vec2* vertices, int32 count); - - /// Establish connectivity to a vertex that precedes the first vertex. - /// Don't call this for loops. - void SetPrevVertex(const b2Vec2& prevVertex); - - /// Establish connectivity to a vertex that follows the last vertex. - /// Don't call this for loops. - void SetNextVertex(const b2Vec2& nextVertex); - - /// Implement b2Shape. Vertices are cloned using b2Alloc. - b2Shape* Clone(b2BlockAllocator* allocator) const; - - /// @see b2Shape::GetChildCount - int32 GetChildCount() const; - - /// Get a child edge. - void GetChildEdge(b2EdgeShape* edge, int32 index) const; - - /// This always return false. - /// @see b2Shape::TestPoint - bool TestPoint(const b2Transform& transform, const b2Vec2& p) const; - - /// Implement b2Shape. - bool RayCast(b2RayCastOutput* output, const b2RayCastInput& input, - const b2Transform& transform, int32 childIndex) const; - - /// @see b2Shape::ComputeAABB - void ComputeAABB(b2AABB* aabb, const b2Transform& transform, int32 childIndex) const; - - /// Chains have zero mass. - /// @see b2Shape::ComputeMass - void ComputeMass(b2MassData* massData, float32 density) const; - - /// The vertices. Owned by this class. - b2Vec2* m_vertices; - - /// The vertex count. - int32 m_count; - - b2Vec2 m_prevVertex, m_nextVertex; - bool m_hasPrevVertex, m_hasNextVertex; -}; - -inline b2ChainShape::b2ChainShape() -{ - m_type = e_chain; - m_radius = b2_polygonRadius; - m_vertices = NULL; - m_count = 0; - m_hasPrevVertex = false; - m_hasNextVertex = false; -} - -#endif diff --git a/libs/Box2D/Collision/Shapes/b2CircleShape.h b/libs/Box2D/Collision/Shapes/b2CircleShape.h deleted file mode 100644 index 6c1fd543..00000000 --- a/libs/Box2D/Collision/Shapes/b2CircleShape.h +++ /dev/null @@ -1,91 +0,0 @@ -/* -* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef B2_CIRCLE_SHAPE_H -#define B2_CIRCLE_SHAPE_H - -#include - -/// A circle shape. -class b2CircleShape : public b2Shape -{ -public: - b2CircleShape(); - - /// Implement b2Shape. - b2Shape* Clone(b2BlockAllocator* allocator) const; - - /// @see b2Shape::GetChildCount - int32 GetChildCount() const; - - /// Implement b2Shape. - bool TestPoint(const b2Transform& transform, const b2Vec2& p) const; - - /// Implement b2Shape. - bool RayCast(b2RayCastOutput* output, const b2RayCastInput& input, - const b2Transform& transform, int32 childIndex) const; - - /// @see b2Shape::ComputeAABB - void ComputeAABB(b2AABB* aabb, const b2Transform& transform, int32 childIndex) const; - - /// @see b2Shape::ComputeMass - void ComputeMass(b2MassData* massData, float32 density) const; - - /// Get the supporting vertex index in the given direction. - int32 GetSupport(const b2Vec2& d) const; - - /// Get the supporting vertex in the given direction. - const b2Vec2& GetSupportVertex(const b2Vec2& d) const; - - /// Get the vertex count. - int32 GetVertexCount() const { return 1; } - - /// Get a vertex by index. Used by b2Distance. - const b2Vec2& GetVertex(int32 index) const; - - /// Position - b2Vec2 m_p; -}; - -inline b2CircleShape::b2CircleShape() -{ - m_type = e_circle; - m_radius = 0.0f; - m_p.SetZero(); -} - -inline int32 b2CircleShape::GetSupport(const b2Vec2 &d) const -{ - B2_NOT_USED(d); - return 0; -} - -inline const b2Vec2& b2CircleShape::GetSupportVertex(const b2Vec2 &d) const -{ - B2_NOT_USED(d); - return m_p; -} - -inline const b2Vec2& b2CircleShape::GetVertex(int32 index) const -{ - B2_NOT_USED(index); - b2Assert(index == 0); - return m_p; -} - -#endif diff --git a/libs/Box2D/Collision/Shapes/b2EdgeShape.h b/libs/Box2D/Collision/Shapes/b2EdgeShape.h deleted file mode 100644 index 99f822be..00000000 --- a/libs/Box2D/Collision/Shapes/b2EdgeShape.h +++ /dev/null @@ -1,74 +0,0 @@ -/* -* Copyright (c) 2006-2010 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef B2_EDGE_SHAPE_H -#define B2_EDGE_SHAPE_H - -#include - -/// A line segment (edge) shape. These can be connected in chains or loops -/// to other edge shapes. The connectivity information is used to ensure -/// correct contact normals. -class b2EdgeShape : public b2Shape -{ -public: - b2EdgeShape(); - - /// Set this as an isolated edge. - void Set(const b2Vec2& v1, const b2Vec2& v2); - - /// Implement b2Shape. - b2Shape* Clone(b2BlockAllocator* allocator) const; - - /// @see b2Shape::GetChildCount - int32 GetChildCount() const; - - /// @see b2Shape::TestPoint - bool TestPoint(const b2Transform& transform, const b2Vec2& p) const; - - /// Implement b2Shape. - bool RayCast(b2RayCastOutput* output, const b2RayCastInput& input, - const b2Transform& transform, int32 childIndex) const; - - /// @see b2Shape::ComputeAABB - void ComputeAABB(b2AABB* aabb, const b2Transform& transform, int32 childIndex) const; - - /// @see b2Shape::ComputeMass - void ComputeMass(b2MassData* massData, float32 density) const; - - /// These are the edge vertices - b2Vec2 m_vertex1, m_vertex2; - - /// Optional adjacent vertices. These are used for smooth collision. - b2Vec2 m_vertex0, m_vertex3; - bool m_hasVertex0, m_hasVertex3; -}; - -inline b2EdgeShape::b2EdgeShape() -{ - m_type = e_edge; - m_radius = b2_polygonRadius; - m_vertex0.x = 0.0f; - m_vertex0.y = 0.0f; - m_vertex3.x = 0.0f; - m_vertex3.y = 0.0f; - m_hasVertex0 = false; - m_hasVertex3 = false; -} - -#endif diff --git a/libs/Box2D/Collision/Shapes/b2PolygonShape.h b/libs/Box2D/Collision/Shapes/b2PolygonShape.h deleted file mode 100644 index 27b17429..00000000 --- a/libs/Box2D/Collision/Shapes/b2PolygonShape.h +++ /dev/null @@ -1,101 +0,0 @@ -/* -* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef B2_POLYGON_SHAPE_H -#define B2_POLYGON_SHAPE_H - -#include - -/// A convex polygon. It is assumed that the interior of the polygon is to -/// the left of each edge. -/// Polygons have a maximum number of vertices equal to b2_maxPolygonVertices. -/// In most cases you should not need many vertices for a convex polygon. -class b2PolygonShape : public b2Shape -{ -public: - b2PolygonShape(); - - /// Implement b2Shape. - b2Shape* Clone(b2BlockAllocator* allocator) const; - - /// @see b2Shape::GetChildCount - int32 GetChildCount() const; - - /// Create a convex hull from the given array of local points. - /// The count must be in the range [3, b2_maxPolygonVertices]. - /// @warning the points may be re-ordered, even if they form a convex polygon - /// @warning collinear points are handled but not removed. Collinear points - /// may lead to poor stacking behavior. - bool Set(const b2Vec2* points, int32 count); - - /// Build vertices to represent an axis-aligned box centered on the local origin. - /// @param hx the half-width. - /// @param hy the half-height. - void SetAsBox(float32 hx, float32 hy); - - /// Build vertices to represent an oriented box. - /// @param hx the half-width. - /// @param hy the half-height. - /// @param center the center of the box in local coordinates. - /// @param angle the rotation of the box in local coordinates. - void SetAsBox(float32 hx, float32 hy, const b2Vec2& center, float32 angle); - - /// @see b2Shape::TestPoint - bool TestPoint(const b2Transform& transform, const b2Vec2& p) const; - - /// Implement b2Shape. - bool RayCast(b2RayCastOutput* output, const b2RayCastInput& input, - const b2Transform& transform, int32 childIndex) const; - - /// @see b2Shape::ComputeAABB - void ComputeAABB(b2AABB* aabb, const b2Transform& transform, int32 childIndex) const; - - /// @see b2Shape::ComputeMass - void ComputeMass(b2MassData* massData, float32 density) const; - - /// Get the vertex count. - int32 GetVertexCount() const { return m_count; } - - /// Get a vertex by index. - const b2Vec2& GetVertex(int32 index) const; - - /// Validate convexity. This is a very time consuming operation. - /// @returns true if valid - bool Validate() const; - - b2Vec2 m_centroid; - b2Vec2 m_vertices[b2_maxPolygonVertices]; - b2Vec2 m_normals[b2_maxPolygonVertices]; - int32 m_count; -}; - -inline b2PolygonShape::b2PolygonShape() -{ - m_type = e_polygon; - m_radius = b2_polygonRadius; - m_count = 0; - m_centroid.SetZero(); -} - -inline const b2Vec2& b2PolygonShape::GetVertex(int32 index) const -{ - b2Assert(0 <= index && index < m_count); - return m_vertices[index]; -} - -#endif diff --git a/libs/Box2D/Collision/b2CollideEdge.cpp b/libs/Box2D/Collision/b2CollideEdge.cpp deleted file mode 100644 index c8276be7..00000000 --- a/libs/Box2D/Collision/b2CollideEdge.cpp +++ /dev/null @@ -1,698 +0,0 @@ -/* - * Copyright (c) 2007-2009 Erin Catto http://www.box2d.org - * - * This software is provided 'as-is', without any express or implied - * warranty. In no event will the authors be held liable for any damages - * arising from the use of this software. - * Permission is granted to anyone to use this software for any purpose, - * including commercial applications, and to alter it and redistribute it - * freely, subject to the following restrictions: - * 1. The origin of this software must not be misrepresented; you must not - * claim that you wrote the original software. If you use this software - * in a product, an acknowledgment in the product documentation would be - * appreciated but is not required. - * 2. Altered source versions must be plainly marked as such, and must not be - * misrepresented as being the original software. - * 3. This notice may not be removed or altered from any source distribution. - */ - -#include -#include -#include -#include - - -// Compute contact points for edge versus circle. -// This accounts for edge connectivity. -void b2CollideEdgeAndCircle(b2Manifold* manifold, - const b2EdgeShape* edgeA, const b2Transform& xfA, - const b2CircleShape* circleB, const b2Transform& xfB) -{ - manifold->pointCount = 0; - - // Compute circle in frame of edge - b2Vec2 Q = b2MulT(xfA, b2Mul(xfB, circleB->m_p)); - - b2Vec2 A = edgeA->m_vertex1, B = edgeA->m_vertex2; - b2Vec2 e = B - A; - - // Barycentric coordinates - float32 u = b2Dot(e, B - Q); - float32 v = b2Dot(e, Q - A); - - float32 radius = edgeA->m_radius + circleB->m_radius; - - b2ContactFeature cf; - cf.indexB = 0; - cf.typeB = b2ContactFeature::e_vertex; - - // Region A - if (v <= 0.0f) - { - b2Vec2 P = A; - b2Vec2 d = Q - P; - float32 dd = b2Dot(d, d); - if (dd > radius * radius) - { - return; - } - - // Is there an edge connected to A? - if (edgeA->m_hasVertex0) - { - b2Vec2 A1 = edgeA->m_vertex0; - b2Vec2 B1 = A; - b2Vec2 e1 = B1 - A1; - float32 u1 = b2Dot(e1, B1 - Q); - - // Is the circle in Region AB of the previous edge? - if (u1 > 0.0f) - { - return; - } - } - - cf.indexA = 0; - cf.typeA = b2ContactFeature::e_vertex; - manifold->pointCount = 1; - manifold->type = b2Manifold::e_circles; - manifold->localNormal.SetZero(); - manifold->localPoint = P; - manifold->points[0].id.key = 0; - manifold->points[0].id.cf = cf; - manifold->points[0].localPoint = circleB->m_p; - return; - } - - // Region B - if (u <= 0.0f) - { - b2Vec2 P = B; - b2Vec2 d = Q - P; - float32 dd = b2Dot(d, d); - if (dd > radius * radius) - { - return; - } - - // Is there an edge connected to B? - if (edgeA->m_hasVertex3) - { - b2Vec2 B2 = edgeA->m_vertex3; - b2Vec2 A2 = B; - b2Vec2 e2 = B2 - A2; - float32 v2 = b2Dot(e2, Q - A2); - - // Is the circle in Region AB of the next edge? - if (v2 > 0.0f) - { - return; - } - } - - cf.indexA = 1; - cf.typeA = b2ContactFeature::e_vertex; - manifold->pointCount = 1; - manifold->type = b2Manifold::e_circles; - manifold->localNormal.SetZero(); - manifold->localPoint = P; - manifold->points[0].id.key = 0; - manifold->points[0].id.cf = cf; - manifold->points[0].localPoint = circleB->m_p; - return; - } - - // Region AB - float32 den = b2Dot(e, e); - b2Assert(den > 0.0f); - b2Vec2 P = (1.0f / den) * (u * A + v * B); - b2Vec2 d = Q - P; - float32 dd = b2Dot(d, d); - if (dd > radius * radius) - { - return; - } - - b2Vec2 n(-e.y, e.x); - if (b2Dot(n, Q - A) < 0.0f) - { - n.Set(-n.x, -n.y); - } - n.Normalize(); - - cf.indexA = 0; - cf.typeA = b2ContactFeature::e_face; - manifold->pointCount = 1; - manifold->type = b2Manifold::e_faceA; - manifold->localNormal = n; - manifold->localPoint = A; - manifold->points[0].id.key = 0; - manifold->points[0].id.cf = cf; - manifold->points[0].localPoint = circleB->m_p; -} - -// This structure is used to keep track of the best separating axis. -struct b2EPAxis -{ - enum Type - { - e_unknown, - e_edgeA, - e_edgeB - }; - - Type type; - int32 index; - float32 separation; -}; - -// This holds polygon B expressed in frame A. -struct b2TempPolygon -{ - b2Vec2 vertices[b2_maxPolygonVertices]; - b2Vec2 normals[b2_maxPolygonVertices]; - int32 count; -}; - -// Reference face used for clipping -struct b2ReferenceFace -{ - int32 i1, i2; - - b2Vec2 v1, v2; - - b2Vec2 normal; - - b2Vec2 sideNormal1; - float32 sideOffset1; - - b2Vec2 sideNormal2; - float32 sideOffset2; -}; - -// This class collides and edge and a polygon, taking into account edge adjacency. -struct b2EPCollider -{ - void Collide(b2Manifold* manifold, const b2EdgeShape* edgeA, const b2Transform& xfA, - const b2PolygonShape* polygonB, const b2Transform& xfB); - b2EPAxis ComputeEdgeSeparation(); - b2EPAxis ComputePolygonSeparation(); - - enum VertexType - { - e_isolated, - e_concave, - e_convex - }; - - b2TempPolygon m_polygonB; - - b2Transform m_xf; - b2Vec2 m_centroidB; - b2Vec2 m_v0, m_v1, m_v2, m_v3; - b2Vec2 m_normal0, m_normal1, m_normal2; - b2Vec2 m_normal; - VertexType m_type1, m_type2; - b2Vec2 m_lowerLimit, m_upperLimit; - float32 m_radius; - bool m_front; -}; - -// Algorithm: -// 1. Classify v1 and v2 -// 2. Classify polygon centroid as front or back -// 3. Flip normal if necessary -// 4. Initialize normal range to [-pi, pi] about face normal -// 5. Adjust normal range according to adjacent edges -// 6. Visit each separating axes, only accept axes within the range -// 7. Return if _any_ axis indicates separation -// 8. Clip -void b2EPCollider::Collide(b2Manifold* manifold, const b2EdgeShape* edgeA, const b2Transform& xfA, - const b2PolygonShape* polygonB, const b2Transform& xfB) -{ - m_xf = b2MulT(xfA, xfB); - - m_centroidB = b2Mul(m_xf, polygonB->m_centroid); - - m_v0 = edgeA->m_vertex0; - m_v1 = edgeA->m_vertex1; - m_v2 = edgeA->m_vertex2; - m_v3 = edgeA->m_vertex3; - - bool hasVertex0 = edgeA->m_hasVertex0; - bool hasVertex3 = edgeA->m_hasVertex3; - - b2Vec2 edge1 = m_v2 - m_v1; - edge1.Normalize(); - m_normal1.Set(edge1.y, -edge1.x); - float32 offset1 = b2Dot(m_normal1, m_centroidB - m_v1); - float32 offset0 = 0.0f, offset2 = 0.0f; - bool convex1 = false, convex2 = false; - - // Is there a preceding edge? - if (hasVertex0) - { - b2Vec2 edge0 = m_v1 - m_v0; - edge0.Normalize(); - m_normal0.Set(edge0.y, -edge0.x); - convex1 = b2Cross(edge0, edge1) >= 0.0f; - offset0 = b2Dot(m_normal0, m_centroidB - m_v0); - } - - // Is there a following edge? - if (hasVertex3) - { - b2Vec2 edge2 = m_v3 - m_v2; - edge2.Normalize(); - m_normal2.Set(edge2.y, -edge2.x); - convex2 = b2Cross(edge1, edge2) > 0.0f; - offset2 = b2Dot(m_normal2, m_centroidB - m_v2); - } - - // Determine front or back collision. Determine collision normal limits. - if (hasVertex0 && hasVertex3) - { - if (convex1 && convex2) - { - m_front = offset0 >= 0.0f || offset1 >= 0.0f || offset2 >= 0.0f; - if (m_front) - { - m_normal = m_normal1; - m_lowerLimit = m_normal0; - m_upperLimit = m_normal2; - } - else - { - m_normal = -m_normal1; - m_lowerLimit = -m_normal1; - m_upperLimit = -m_normal1; - } - } - else if (convex1) - { - m_front = offset0 >= 0.0f || (offset1 >= 0.0f && offset2 >= 0.0f); - if (m_front) - { - m_normal = m_normal1; - m_lowerLimit = m_normal0; - m_upperLimit = m_normal1; - } - else - { - m_normal = -m_normal1; - m_lowerLimit = -m_normal2; - m_upperLimit = -m_normal1; - } - } - else if (convex2) - { - m_front = offset2 >= 0.0f || (offset0 >= 0.0f && offset1 >= 0.0f); - if (m_front) - { - m_normal = m_normal1; - m_lowerLimit = m_normal1; - m_upperLimit = m_normal2; - } - else - { - m_normal = -m_normal1; - m_lowerLimit = -m_normal1; - m_upperLimit = -m_normal0; - } - } - else - { - m_front = offset0 >= 0.0f && offset1 >= 0.0f && offset2 >= 0.0f; - if (m_front) - { - m_normal = m_normal1; - m_lowerLimit = m_normal1; - m_upperLimit = m_normal1; - } - else - { - m_normal = -m_normal1; - m_lowerLimit = -m_normal2; - m_upperLimit = -m_normal0; - } - } - } - else if (hasVertex0) - { - if (convex1) - { - m_front = offset0 >= 0.0f || offset1 >= 0.0f; - if (m_front) - { - m_normal = m_normal1; - m_lowerLimit = m_normal0; - m_upperLimit = -m_normal1; - } - else - { - m_normal = -m_normal1; - m_lowerLimit = m_normal1; - m_upperLimit = -m_normal1; - } - } - else - { - m_front = offset0 >= 0.0f && offset1 >= 0.0f; - if (m_front) - { - m_normal = m_normal1; - m_lowerLimit = m_normal1; - m_upperLimit = -m_normal1; - } - else - { - m_normal = -m_normal1; - m_lowerLimit = m_normal1; - m_upperLimit = -m_normal0; - } - } - } - else if (hasVertex3) - { - if (convex2) - { - m_front = offset1 >= 0.0f || offset2 >= 0.0f; - if (m_front) - { - m_normal = m_normal1; - m_lowerLimit = -m_normal1; - m_upperLimit = m_normal2; - } - else - { - m_normal = -m_normal1; - m_lowerLimit = -m_normal1; - m_upperLimit = m_normal1; - } - } - else - { - m_front = offset1 >= 0.0f && offset2 >= 0.0f; - if (m_front) - { - m_normal = m_normal1; - m_lowerLimit = -m_normal1; - m_upperLimit = m_normal1; - } - else - { - m_normal = -m_normal1; - m_lowerLimit = -m_normal2; - m_upperLimit = m_normal1; - } - } - } - else - { - m_front = offset1 >= 0.0f; - if (m_front) - { - m_normal = m_normal1; - m_lowerLimit = -m_normal1; - m_upperLimit = -m_normal1; - } - else - { - m_normal = -m_normal1; - m_lowerLimit = m_normal1; - m_upperLimit = m_normal1; - } - } - - // Get polygonB in frameA - m_polygonB.count = polygonB->m_count; - for (int32 i = 0; i < polygonB->m_count; ++i) - { - m_polygonB.vertices[i] = b2Mul(m_xf, polygonB->m_vertices[i]); - m_polygonB.normals[i] = b2Mul(m_xf.q, polygonB->m_normals[i]); - } - - m_radius = 2.0f * b2_polygonRadius; - - manifold->pointCount = 0; - - b2EPAxis edgeAxis = ComputeEdgeSeparation(); - - // If no valid normal can be found than this edge should not collide. - if (edgeAxis.type == b2EPAxis::e_unknown) - { - return; - } - - if (edgeAxis.separation > m_radius) - { - return; - } - - b2EPAxis polygonAxis = ComputePolygonSeparation(); - if (polygonAxis.type != b2EPAxis::e_unknown && polygonAxis.separation > m_radius) - { - return; - } - - // Use hysteresis for jitter reduction. - const float32 k_relativeTol = 0.98f; - const float32 k_absoluteTol = 0.001f; - - b2EPAxis primaryAxis; - if (polygonAxis.type == b2EPAxis::e_unknown) - { - primaryAxis = edgeAxis; - } - else if (polygonAxis.separation > k_relativeTol * edgeAxis.separation + k_absoluteTol) - { - primaryAxis = polygonAxis; - } - else - { - primaryAxis = edgeAxis; - } - - b2ClipVertex ie[2]; - b2ReferenceFace rf; - if (primaryAxis.type == b2EPAxis::e_edgeA) - { - manifold->type = b2Manifold::e_faceA; - - // Search for the polygon normal that is most anti-parallel to the edge normal. - int32 bestIndex = 0; - float32 bestValue = b2Dot(m_normal, m_polygonB.normals[0]); - for (int32 i = 1; i < m_polygonB.count; ++i) - { - float32 value = b2Dot(m_normal, m_polygonB.normals[i]); - if (value < bestValue) - { - bestValue = value; - bestIndex = i; - } - } - - int32 i1 = bestIndex; - int32 i2 = i1 + 1 < m_polygonB.count ? i1 + 1 : 0; - - ie[0].v = m_polygonB.vertices[i1]; - ie[0].id.cf.indexA = 0; - ie[0].id.cf.indexB = static_cast(i1); - ie[0].id.cf.typeA = b2ContactFeature::e_face; - ie[0].id.cf.typeB = b2ContactFeature::e_vertex; - - ie[1].v = m_polygonB.vertices[i2]; - ie[1].id.cf.indexA = 0; - ie[1].id.cf.indexB = static_cast(i2); - ie[1].id.cf.typeA = b2ContactFeature::e_face; - ie[1].id.cf.typeB = b2ContactFeature::e_vertex; - - if (m_front) - { - rf.i1 = 0; - rf.i2 = 1; - rf.v1 = m_v1; - rf.v2 = m_v2; - rf.normal = m_normal1; - } - else - { - rf.i1 = 1; - rf.i2 = 0; - rf.v1 = m_v2; - rf.v2 = m_v1; - rf.normal = -m_normal1; - } - } - else - { - manifold->type = b2Manifold::e_faceB; - - ie[0].v = m_v1; - ie[0].id.cf.indexA = 0; - ie[0].id.cf.indexB = static_cast(primaryAxis.index); - ie[0].id.cf.typeA = b2ContactFeature::e_vertex; - ie[0].id.cf.typeB = b2ContactFeature::e_face; - - ie[1].v = m_v2; - ie[1].id.cf.indexA = 0; - ie[1].id.cf.indexB = static_cast(primaryAxis.index); - ie[1].id.cf.typeA = b2ContactFeature::e_vertex; - ie[1].id.cf.typeB = b2ContactFeature::e_face; - - rf.i1 = primaryAxis.index; - rf.i2 = rf.i1 + 1 < m_polygonB.count ? rf.i1 + 1 : 0; - rf.v1 = m_polygonB.vertices[rf.i1]; - rf.v2 = m_polygonB.vertices[rf.i2]; - rf.normal = m_polygonB.normals[rf.i1]; - } - - rf.sideNormal1.Set(rf.normal.y, -rf.normal.x); - rf.sideNormal2 = -rf.sideNormal1; - rf.sideOffset1 = b2Dot(rf.sideNormal1, rf.v1); - rf.sideOffset2 = b2Dot(rf.sideNormal2, rf.v2); - - // Clip incident edge against extruded edge1 side edges. - b2ClipVertex clipPoints1[2]; - b2ClipVertex clipPoints2[2]; - int32 np; - - // Clip to box side 1 - np = b2ClipSegmentToLine(clipPoints1, ie, rf.sideNormal1, rf.sideOffset1, rf.i1); - - if (np < b2_maxManifoldPoints) - { - return; - } - - // Clip to negative box side 1 - np = b2ClipSegmentToLine(clipPoints2, clipPoints1, rf.sideNormal2, rf.sideOffset2, rf.i2); - - if (np < b2_maxManifoldPoints) - { - return; - } - - // Now clipPoints2 contains the clipped points. - if (primaryAxis.type == b2EPAxis::e_edgeA) - { - manifold->localNormal = rf.normal; - manifold->localPoint = rf.v1; - } - else - { - manifold->localNormal = polygonB->m_normals[rf.i1]; - manifold->localPoint = polygonB->m_vertices[rf.i1]; - } - - int32 pointCount = 0; - for (int32 i = 0; i < b2_maxManifoldPoints; ++i) - { - float32 separation; - - separation = b2Dot(rf.normal, clipPoints2[i].v - rf.v1); - - if (separation <= m_radius) - { - b2ManifoldPoint* cp = manifold->points + pointCount; - - if (primaryAxis.type == b2EPAxis::e_edgeA) - { - cp->localPoint = b2MulT(m_xf, clipPoints2[i].v); - cp->id = clipPoints2[i].id; - } - else - { - cp->localPoint = clipPoints2[i].v; - cp->id.cf.typeA = clipPoints2[i].id.cf.typeB; - cp->id.cf.typeB = clipPoints2[i].id.cf.typeA; - cp->id.cf.indexA = clipPoints2[i].id.cf.indexB; - cp->id.cf.indexB = clipPoints2[i].id.cf.indexA; - } - - ++pointCount; - } - } - - manifold->pointCount = pointCount; -} - -b2EPAxis b2EPCollider::ComputeEdgeSeparation() -{ - b2EPAxis axis; - axis.type = b2EPAxis::e_edgeA; - axis.index = m_front ? 0 : 1; - axis.separation = FLT_MAX; - - for (int32 i = 0; i < m_polygonB.count; ++i) - { - float32 s = b2Dot(m_normal, m_polygonB.vertices[i] - m_v1); - if (s < axis.separation) - { - axis.separation = s; - } - } - - return axis; -} - -b2EPAxis b2EPCollider::ComputePolygonSeparation() -{ - b2EPAxis axis; - axis.type = b2EPAxis::e_unknown; - axis.index = -1; - axis.separation = -FLT_MAX; - - b2Vec2 perp(-m_normal.y, m_normal.x); - - for (int32 i = 0; i < m_polygonB.count; ++i) - { - b2Vec2 n = -m_polygonB.normals[i]; - - float32 s1 = b2Dot(n, m_polygonB.vertices[i] - m_v1); - float32 s2 = b2Dot(n, m_polygonB.vertices[i] - m_v2); - float32 s = b2Min(s1, s2); - - if (s > m_radius) - { - // No collision - axis.type = b2EPAxis::e_edgeB; - axis.index = i; - axis.separation = s; - return axis; - } - - // Adjacency - if (b2Dot(n, perp) >= 0.0f) - { - if (b2Dot(n - m_upperLimit, m_normal) < -b2_angularSlop) - { - continue; - } - } - else - { - if (b2Dot(n - m_lowerLimit, m_normal) < -b2_angularSlop) - { - continue; - } - } - - if (s > axis.separation) - { - axis.type = b2EPAxis::e_edgeB; - axis.index = i; - axis.separation = s; - } - } - - return axis; -} - -void b2CollideEdgeAndPolygon( b2Manifold* manifold, - const b2EdgeShape* edgeA, const b2Transform& xfA, - const b2PolygonShape* polygonB, const b2Transform& xfB) -{ - b2EPCollider collider; - collider.Collide(manifold, edgeA, xfA, polygonB, xfB); -} diff --git a/libs/Box2D/Collision/b2Collision.cpp b/libs/Box2D/Collision/b2Collision.cpp deleted file mode 100644 index bd3e78b8..00000000 --- a/libs/Box2D/Collision/b2Collision.cpp +++ /dev/null @@ -1,252 +0,0 @@ -/* -* Copyright (c) 2007-2009 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ - -#include -#include - -void b2WorldManifold::Initialize(const b2Manifold* manifold, - const b2Transform& xfA, float32 radiusA, - const b2Transform& xfB, float32 radiusB) -{ - if (manifold->pointCount == 0) - { - return; - } - - switch (manifold->type) - { - case b2Manifold::e_circles: - { - normal.Set(1.0f, 0.0f); - b2Vec2 pointA = b2Mul(xfA, manifold->localPoint); - b2Vec2 pointB = b2Mul(xfB, manifold->points[0].localPoint); - if (b2DistanceSquared(pointA, pointB) > b2_epsilon * b2_epsilon) - { - normal = pointB - pointA; - normal.Normalize(); - } - - b2Vec2 cA = pointA + radiusA * normal; - b2Vec2 cB = pointB - radiusB * normal; - points[0] = 0.5f * (cA + cB); - separations[0] = b2Dot(cB - cA, normal); - } - break; - - case b2Manifold::e_faceA: - { - normal = b2Mul(xfA.q, manifold->localNormal); - b2Vec2 planePoint = b2Mul(xfA, manifold->localPoint); - - for (int32 i = 0; i < manifold->pointCount; ++i) - { - b2Vec2 clipPoint = b2Mul(xfB, manifold->points[i].localPoint); - b2Vec2 cA = clipPoint + (radiusA - b2Dot(clipPoint - planePoint, normal)) * normal; - b2Vec2 cB = clipPoint - radiusB * normal; - points[i] = 0.5f * (cA + cB); - separations[i] = b2Dot(cB - cA, normal); - } - } - break; - - case b2Manifold::e_faceB: - { - normal = b2Mul(xfB.q, manifold->localNormal); - b2Vec2 planePoint = b2Mul(xfB, manifold->localPoint); - - for (int32 i = 0; i < manifold->pointCount; ++i) - { - b2Vec2 clipPoint = b2Mul(xfA, manifold->points[i].localPoint); - b2Vec2 cB = clipPoint + (radiusB - b2Dot(clipPoint - planePoint, normal)) * normal; - b2Vec2 cA = clipPoint - radiusA * normal; - points[i] = 0.5f * (cA + cB); - separations[i] = b2Dot(cA - cB, normal); - } - - // Ensure normal points from A to B. - normal = -normal; - } - break; - } -} - -void b2GetPointStates(b2PointState state1[b2_maxManifoldPoints], b2PointState state2[b2_maxManifoldPoints], - const b2Manifold* manifold1, const b2Manifold* manifold2) -{ - for (int32 i = 0; i < b2_maxManifoldPoints; ++i) - { - state1[i] = b2_nullState; - state2[i] = b2_nullState; - } - - // Detect persists and removes. - for (int32 i = 0; i < manifold1->pointCount; ++i) - { - b2ContactID id = manifold1->points[i].id; - - state1[i] = b2_removeState; - - for (int32 j = 0; j < manifold2->pointCount; ++j) - { - if (manifold2->points[j].id.key == id.key) - { - state1[i] = b2_persistState; - break; - } - } - } - - // Detect persists and adds. - for (int32 i = 0; i < manifold2->pointCount; ++i) - { - b2ContactID id = manifold2->points[i].id; - - state2[i] = b2_addState; - - for (int32 j = 0; j < manifold1->pointCount; ++j) - { - if (manifold1->points[j].id.key == id.key) - { - state2[i] = b2_persistState; - break; - } - } - } -} - -// From Real-time Collision Detection, p179. -bool b2AABB::RayCast(b2RayCastOutput* output, const b2RayCastInput& input) const -{ - float32 tmin = -b2_maxFloat; - float32 tmax = b2_maxFloat; - - b2Vec2 p = input.p1; - b2Vec2 d = input.p2 - input.p1; - b2Vec2 absD = b2Abs(d); - - b2Vec2 normal; - - for (int32 i = 0; i < 2; ++i) - { - if (absD(i) < b2_epsilon) - { - // Parallel. - if (p(i) < lowerBound(i) || upperBound(i) < p(i)) - { - return false; - } - } - else - { - float32 inv_d = 1.0f / d(i); - float32 t1 = (lowerBound(i) - p(i)) * inv_d; - float32 t2 = (upperBound(i) - p(i)) * inv_d; - - // Sign of the normal vector. - float32 s = -1.0f; - - if (t1 > t2) - { - b2Swap(t1, t2); - s = 1.0f; - } - - // Push the min up - if (t1 > tmin) - { - normal.SetZero(); - normal(i) = s; - tmin = t1; - } - - // Pull the max down - tmax = b2Min(tmax, t2); - - if (tmin > tmax) - { - return false; - } - } - } - - // Does the ray start inside the box? - // Does the ray intersect beyond the max fraction? - if (tmin < 0.0f || input.maxFraction < tmin) - { - return false; - } - - // Intersection. - output->fraction = tmin; - output->normal = normal; - return true; -} - -// Sutherland-Hodgman clipping. -int32 b2ClipSegmentToLine(b2ClipVertex vOut[2], const b2ClipVertex vIn[2], - const b2Vec2& normal, float32 offset, int32 vertexIndexA) -{ - // Start with no output points - int32 numOut = 0; - - // Calculate the distance of end points to the line - float32 distance0 = b2Dot(normal, vIn[0].v) - offset; - float32 distance1 = b2Dot(normal, vIn[1].v) - offset; - - // If the points are behind the plane - if (distance0 <= 0.0f) vOut[numOut++] = vIn[0]; - if (distance1 <= 0.0f) vOut[numOut++] = vIn[1]; - - // If the points are on different sides of the plane - if (distance0 * distance1 < 0.0f) - { - // Find intersection point of edge and plane - float32 interp = distance0 / (distance0 - distance1); - vOut[numOut].v = vIn[0].v + interp * (vIn[1].v - vIn[0].v); - - // VertexA is hitting edgeB. - vOut[numOut].id.cf.indexA = static_cast(vertexIndexA); - vOut[numOut].id.cf.indexB = vIn[0].id.cf.indexB; - vOut[numOut].id.cf.typeA = b2ContactFeature::e_vertex; - vOut[numOut].id.cf.typeB = b2ContactFeature::e_face; - ++numOut; - } - - return numOut; -} - -bool b2TestOverlap( const b2Shape* shapeA, int32 indexA, - const b2Shape* shapeB, int32 indexB, - const b2Transform& xfA, const b2Transform& xfB) -{ - b2DistanceInput input; - input.proxyA.Set(shapeA, indexA); - input.proxyB.Set(shapeB, indexB); - input.transformA = xfA; - input.transformB = xfB; - input.useRadii = true; - - b2SimplexCache cache; - cache.count = 0; - - b2DistanceOutput output; - - b2Distance(&output, &cache, &input); - - return output.distance < 10.0f * b2_epsilon; -} diff --git a/libs/Box2D/Collision/b2Distance.h b/libs/Box2D/Collision/b2Distance.h deleted file mode 100644 index 54ed1e13..00000000 --- a/libs/Box2D/Collision/b2Distance.h +++ /dev/null @@ -1,141 +0,0 @@ - -/* -* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef B2_DISTANCE_H -#define B2_DISTANCE_H - -#include - -class b2Shape; - -/// A distance proxy is used by the GJK algorithm. -/// It encapsulates any shape. -struct b2DistanceProxy -{ - b2DistanceProxy() : m_vertices(NULL), m_count(0), m_radius(0.0f) {} - - /// Initialize the proxy using the given shape. The shape - /// must remain in scope while the proxy is in use. - void Set(const b2Shape* shape, int32 index); - - /// Get the supporting vertex index in the given direction. - int32 GetSupport(const b2Vec2& d) const; - - /// Get the supporting vertex in the given direction. - const b2Vec2& GetSupportVertex(const b2Vec2& d) const; - - /// Get the vertex count. - int32 GetVertexCount() const; - - /// Get a vertex by index. Used by b2Distance. - const b2Vec2& GetVertex(int32 index) const; - - b2Vec2 m_buffer[2]; - const b2Vec2* m_vertices; - int32 m_count; - float32 m_radius; -}; - -/// Used to warm start b2Distance. -/// Set count to zero on first call. -struct b2SimplexCache -{ - float32 metric; ///< length or area - uint16 count; - uint8 indexA[3]; ///< vertices on shape A - uint8 indexB[3]; ///< vertices on shape B -}; - -/// Input for b2Distance. -/// You have to option to use the shape radii -/// in the computation. Even -struct b2DistanceInput -{ - b2DistanceProxy proxyA; - b2DistanceProxy proxyB; - b2Transform transformA; - b2Transform transformB; - bool useRadii; -}; - -/// Output for b2Distance. -struct b2DistanceOutput -{ - b2Vec2 pointA; ///< closest point on shapeA - b2Vec2 pointB; ///< closest point on shapeB - float32 distance; - int32 iterations; ///< number of GJK iterations used -}; - -/// Compute the closest points between two shapes. Supports any combination of: -/// b2CircleShape, b2PolygonShape, b2EdgeShape. The simplex cache is input/output. -/// On the first call set b2SimplexCache.count to zero. -void b2Distance(b2DistanceOutput* output, - b2SimplexCache* cache, - const b2DistanceInput* input); - - -////////////////////////////////////////////////////////////////////////// - -inline int32 b2DistanceProxy::GetVertexCount() const -{ - return m_count; -} - -inline const b2Vec2& b2DistanceProxy::GetVertex(int32 index) const -{ - b2Assert(0 <= index && index < m_count); - return m_vertices[index]; -} - -inline int32 b2DistanceProxy::GetSupport(const b2Vec2& d) const -{ - int32 bestIndex = 0; - float32 bestValue = b2Dot(m_vertices[0], d); - for (int32 i = 1; i < m_count; ++i) - { - float32 value = b2Dot(m_vertices[i], d); - if (value > bestValue) - { - bestIndex = i; - bestValue = value; - } - } - - return bestIndex; -} - -inline const b2Vec2& b2DistanceProxy::GetSupportVertex(const b2Vec2& d) const -{ - int32 bestIndex = 0; - float32 bestValue = b2Dot(m_vertices[0], d); - for (int32 i = 1; i < m_count; ++i) - { - float32 value = b2Dot(m_vertices[i], d); - if (value > bestValue) - { - bestIndex = i; - bestValue = value; - } - } - - return m_vertices[bestIndex]; -} - -#endif diff --git a/libs/Box2D/Collision/b2TimeOfImpact.h b/libs/Box2D/Collision/b2TimeOfImpact.h deleted file mode 100644 index 179a1700..00000000 --- a/libs/Box2D/Collision/b2TimeOfImpact.h +++ /dev/null @@ -1,58 +0,0 @@ -/* -* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef B2_TIME_OF_IMPACT_H -#define B2_TIME_OF_IMPACT_H - -#include -#include - -/// Input parameters for b2TimeOfImpact -struct b2TOIInput -{ - b2DistanceProxy proxyA; - b2DistanceProxy proxyB; - b2Sweep sweepA; - b2Sweep sweepB; - float32 tMax; // defines sweep interval [0, tMax] -}; - -// Output parameters for b2TimeOfImpact. -struct b2TOIOutput -{ - enum State - { - e_unknown, - e_failed, - e_overlapped, - e_touching, - e_separated - }; - - State state; - float32 t; -}; - -/// Compute the upper bound on time before two shapes penetrate. Time is represented as -/// a fraction between [0,tMax]. This uses a swept separating axis and may miss some intermediate, -/// non-tunneling collision. If you change the time interval, you should call this function -/// again. -/// Note: use b2Distance to compute the contact point and normal at the time of impact. -void b2TimeOfImpact(b2TOIOutput* output, const b2TOIInput* input); - -#endif diff --git a/libs/Box2D/Common/b2BlockAllocator.h b/libs/Box2D/Common/b2BlockAllocator.h deleted file mode 100644 index 8ba29a5e..00000000 --- a/libs/Box2D/Common/b2BlockAllocator.h +++ /dev/null @@ -1,62 +0,0 @@ -/* -* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef B2_BLOCK_ALLOCATOR_H -#define B2_BLOCK_ALLOCATOR_H - -#include - -const int32 b2_chunkSize = 16 * 1024; -const int32 b2_maxBlockSize = 640; -const int32 b2_blockSizes = 14; -const int32 b2_chunkArrayIncrement = 128; - -struct b2Block; -struct b2Chunk; - -/// This is a small object allocator used for allocating small -/// objects that persist for more than one time step. -/// See: http://www.codeproject.com/useritems/Small_Block_Allocator.asp -class b2BlockAllocator -{ -public: - b2BlockAllocator(); - ~b2BlockAllocator(); - - /// Allocate memory. This will use b2Alloc if the size is larger than b2_maxBlockSize. - void* Allocate(int32 size); - - /// Free memory. This will use b2Free if the size is larger than b2_maxBlockSize. - void Free(void* p, int32 size); - - void Clear(); - -private: - - b2Chunk* m_chunks; - int32 m_chunkCount; - int32 m_chunkSpace; - - b2Block* m_freeLists[b2_blockSizes]; - - static int32 s_blockSizes[b2_blockSizes]; - static uint8 s_blockSizeLookup[b2_maxBlockSize + 1]; - static bool s_blockSizeLookupInitialized; -}; - -#endif diff --git a/libs/Box2D/Common/b2Draw.cpp b/libs/Box2D/Common/b2Draw.cpp deleted file mode 100644 index 327b5807..00000000 --- a/libs/Box2D/Common/b2Draw.cpp +++ /dev/null @@ -1,44 +0,0 @@ -/* -* Copyright (c) 2011 Erin Catto http://box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ - -#include - -b2Draw::b2Draw() -{ - m_drawFlags = 0; -} - -void b2Draw::SetFlags(uint32 flags) -{ - m_drawFlags = flags; -} - -uint32 b2Draw::GetFlags() const -{ - return m_drawFlags; -} - -void b2Draw::AppendFlags(uint32 flags) -{ - m_drawFlags |= flags; -} - -void b2Draw::ClearFlags(uint32 flags) -{ - m_drawFlags &= ~flags; -} diff --git a/libs/Box2D/Common/b2Draw.h b/libs/Box2D/Common/b2Draw.h deleted file mode 100644 index 16c0b323..00000000 --- a/libs/Box2D/Common/b2Draw.h +++ /dev/null @@ -1,86 +0,0 @@ -/* -* Copyright (c) 2011 Erin Catto http://box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef B2_DRAW_H -#define B2_DRAW_H - -#include - -/// Color for debug drawing. Each value has the range [0,1]. -struct b2Color -{ - b2Color() {} - b2Color(float32 r, float32 g, float32 b, float32 a = 1.0f) : r(r), g(g), b(b), a(a) {} - void Set(float32 ri, float32 gi, float32 bi, float32 ai = 1.0f) { r = ri; g = gi; b = bi; a = ai; } - float32 r, g, b, a; -}; - -/// Implement and register this class with a b2World to provide debug drawing of physics -/// entities in your game. -class b2Draw -{ -public: - b2Draw(); - - virtual ~b2Draw() {} - - enum - { - e_shapeBit = 0x0001, ///< draw shapes - e_jointBit = 0x0002, ///< draw joint connections - e_aabbBit = 0x0004, ///< draw axis aligned bounding boxes - e_pairBit = 0x0008, ///< draw broad-phase pairs - e_centerOfMassBit = 0x0010 ///< draw center of mass frame - }; - - /// Set the drawing flags. - void SetFlags(uint32 flags); - - /// Get the drawing flags. - uint32 GetFlags() const; - - /// Append flags to the current flags. - void AppendFlags(uint32 flags); - - /// Clear flags from the current flags. - void ClearFlags(uint32 flags); - - /// Draw a closed polygon provided in CCW order. - virtual void DrawPolygon(const b2Vec2* vertices, int32 vertexCount, const b2Color& color) = 0; - - /// Draw a solid closed polygon provided in CCW order. - virtual void DrawSolidPolygon(const b2Vec2* vertices, int32 vertexCount, const b2Color& color) = 0; - - /// Draw a circle. - virtual void DrawCircle(const b2Vec2& center, float32 radius, const b2Color& color) = 0; - - /// Draw a solid circle. - virtual void DrawSolidCircle(const b2Vec2& center, float32 radius, const b2Vec2& axis, const b2Color& color) = 0; - - /// Draw a line segment. - virtual void DrawSegment(const b2Vec2& p1, const b2Vec2& p2, const b2Color& color) = 0; - - /// Draw a transform. Choose your own length scale. - /// @param xf a transform. - virtual void DrawTransform(const b2Transform& xf) = 0; - -protected: - uint32 m_drawFlags; -}; - -#endif diff --git a/libs/Box2D/Common/b2GrowableStack.h b/libs/Box2D/Common/b2GrowableStack.h deleted file mode 100644 index 482097c5..00000000 --- a/libs/Box2D/Common/b2GrowableStack.h +++ /dev/null @@ -1,86 +0,0 @@ -/* -* Copyright (c) 2010 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef B2_GROWABLE_STACK_H -#define B2_GROWABLE_STACK_H -#include -#include -#include - -/// This is a growable LIFO stack with an initial capacity of N. -/// If the stack size exceeds the initial capacity, the heap is used -/// to increase the size of the stack. -template -class b2GrowableStack -{ -public: - b2GrowableStack() - { - m_stack = m_array; - m_count = 0; - m_capacity = N; - } - - ~b2GrowableStack() - { - if (m_stack != m_array) - { - b2Free(m_stack); - m_stack = NULL; - } - } - - void Push(const T& element) - { - if (m_count == m_capacity) - { - T* old = m_stack; - m_capacity *= 2; - m_stack = (T*)b2Alloc(m_capacity * sizeof(T)); - memcpy(m_stack, old, m_count * sizeof(T)); - if (old != m_array) - { - b2Free(old); - } - } - - m_stack[m_count] = element; - ++m_count; - } - - T Pop() - { - b2Assert(m_count > 0); - --m_count; - return m_stack[m_count]; - } - - int32 GetCount() - { - return m_count; - } - -private: - T* m_stack; - T m_array[N]; - int32 m_count; - int32 m_capacity; -}; - - -#endif diff --git a/libs/Box2D/Common/b2Math.cpp b/libs/Box2D/Common/b2Math.cpp deleted file mode 100644 index 4974fe18..00000000 --- a/libs/Box2D/Common/b2Math.cpp +++ /dev/null @@ -1,94 +0,0 @@ -/* -* Copyright (c) 2007-2009 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ - -#include - -const b2Vec2 b2Vec2_zero(0.0f, 0.0f); - -/// Solve A * x = b, where b is a column vector. This is more efficient -/// than computing the inverse in one-shot cases. -b2Vec3 b2Mat33::Solve33(const b2Vec3& b) const -{ - float32 det = b2Dot(ex, b2Cross(ey, ez)); - if (det != 0.0f) - { - det = 1.0f / det; - } - b2Vec3 x; - x.x = det * b2Dot(b, b2Cross(ey, ez)); - x.y = det * b2Dot(ex, b2Cross(b, ez)); - x.z = det * b2Dot(ex, b2Cross(ey, b)); - return x; -} - -/// Solve A * x = b, where b is a column vector. This is more efficient -/// than computing the inverse in one-shot cases. -b2Vec2 b2Mat33::Solve22(const b2Vec2& b) const -{ - float32 a11 = ex.x, a12 = ey.x, a21 = ex.y, a22 = ey.y; - float32 det = a11 * a22 - a12 * a21; - if (det != 0.0f) - { - det = 1.0f / det; - } - b2Vec2 x; - x.x = det * (a22 * b.x - a12 * b.y); - x.y = det * (a11 * b.y - a21 * b.x); - return x; -} - -/// -void b2Mat33::GetInverse22(b2Mat33* M) const -{ - float32 a = ex.x, b = ey.x, c = ex.y, d = ey.y; - float32 det = a * d - b * c; - if (det != 0.0f) - { - det = 1.0f / det; - } - - M->ex.x = det * d; M->ey.x = -det * b; M->ex.z = 0.0f; - M->ex.y = -det * c; M->ey.y = det * a; M->ey.z = 0.0f; - M->ez.x = 0.0f; M->ez.y = 0.0f; M->ez.z = 0.0f; -} - -/// Returns the zero matrix if singular. -void b2Mat33::GetSymInverse33(b2Mat33* M) const -{ - float32 det = b2Dot(ex, b2Cross(ey, ez)); - if (det != 0.0f) - { - det = 1.0f / det; - } - - float32 a11 = ex.x, a12 = ey.x, a13 = ez.x; - float32 a22 = ey.y, a23 = ez.y; - float32 a33 = ez.z; - - M->ex.x = det * (a22 * a33 - a23 * a23); - M->ex.y = det * (a13 * a23 - a12 * a33); - M->ex.z = det * (a12 * a23 - a13 * a22); - - M->ey.x = M->ex.y; - M->ey.y = det * (a11 * a33 - a13 * a13); - M->ey.z = det * (a13 * a12 - a11 * a23); - - M->ez.x = M->ex.z; - M->ez.y = M->ey.z; - M->ez.z = det * (a11 * a22 - a12 * a12); -} diff --git a/libs/Box2D/Common/b2Settings.cpp b/libs/Box2D/Common/b2Settings.cpp deleted file mode 100644 index 25ce9491..00000000 --- a/libs/Box2D/Common/b2Settings.cpp +++ /dev/null @@ -1,44 +0,0 @@ -/* -* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ - -#include -#include -#include -#include - -b2Version b2_version = {2, 3, 1}; - -// Memory allocators. Modify these to use your own allocator. -void* b2Alloc(int32 size) -{ - return malloc(size); -} - -void b2Free(void* mem) -{ - free(mem); -} - -// You can modify this to use your logging facility. -void b2Log(const char* string, ...) -{ - va_list args; - va_start(args, string); - vprintf(string, args); - va_end(args); -} diff --git a/libs/Box2D/Common/b2StackAllocator.cpp b/libs/Box2D/Common/b2StackAllocator.cpp deleted file mode 100644 index 4a862835..00000000 --- a/libs/Box2D/Common/b2StackAllocator.cpp +++ /dev/null @@ -1,83 +0,0 @@ -/* -* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ - -#include -#include - -b2StackAllocator::b2StackAllocator() -{ - m_index = 0; - m_allocation = 0; - m_maxAllocation = 0; - m_entryCount = 0; -} - -b2StackAllocator::~b2StackAllocator() -{ - b2Assert(m_index == 0); - b2Assert(m_entryCount == 0); -} - -void* b2StackAllocator::Allocate(int32 size) -{ - b2Assert(m_entryCount < b2_maxStackEntries); - - b2StackEntry* entry = m_entries + m_entryCount; - entry->size = size; - if (m_index + size > b2_stackSize) - { - entry->data = (char*)b2Alloc(size); - entry->usedMalloc = true; - } - else - { - entry->data = m_data + m_index; - entry->usedMalloc = false; - m_index += size; - } - - m_allocation += size; - m_maxAllocation = b2Max(m_maxAllocation, m_allocation); - ++m_entryCount; - - return entry->data; -} - -void b2StackAllocator::Free(void* p) -{ - b2Assert(m_entryCount > 0); - b2StackEntry* entry = m_entries + m_entryCount - 1; - b2Assert(p == entry->data); - if (entry->usedMalloc) - { - b2Free(p); - } - else - { - m_index -= entry->size; - } - m_allocation -= entry->size; - --m_entryCount; - - p = NULL; -} - -int32 b2StackAllocator::GetMaxAllocation() const -{ - return m_maxAllocation; -} diff --git a/libs/Box2D/Common/b2StackAllocator.h b/libs/Box2D/Common/b2StackAllocator.h deleted file mode 100644 index 796c51df..00000000 --- a/libs/Box2D/Common/b2StackAllocator.h +++ /dev/null @@ -1,60 +0,0 @@ -/* -* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef B2_STACK_ALLOCATOR_H -#define B2_STACK_ALLOCATOR_H - -#include - -const int32 b2_stackSize = 100 * 1024; // 100k -const int32 b2_maxStackEntries = 32; - -struct b2StackEntry -{ - char* data; - int32 size; - bool usedMalloc; -}; - -// This is a stack allocator used for fast per step allocations. -// You must nest allocate/free pairs. The code will assert -// if you try to interleave multiple allocate/free pairs. -class b2StackAllocator -{ -public: - b2StackAllocator(); - ~b2StackAllocator(); - - void* Allocate(int32 size); - void Free(void* p); - - int32 GetMaxAllocation() const; - -private: - - char m_data[b2_stackSize]; - int32 m_index; - - int32 m_allocation; - int32 m_maxAllocation; - - b2StackEntry m_entries[b2_maxStackEntries]; - int32 m_entryCount; -}; - -#endif diff --git a/libs/Box2D/Common/b2Timer.cpp b/libs/Box2D/Common/b2Timer.cpp deleted file mode 100644 index 44dc5cea..00000000 --- a/libs/Box2D/Common/b2Timer.cpp +++ /dev/null @@ -1,101 +0,0 @@ -/* -* Copyright (c) 2011 Erin Catto http://box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ - -#include - -#if defined(_WIN32) - -float64 b2Timer::s_invFrequency = 0.0f; - -#define WIN32_LEAN_AND_MEAN -#include - -b2Timer::b2Timer() -{ - LARGE_INTEGER largeInteger; - - if (s_invFrequency == 0.0f) - { - QueryPerformanceFrequency(&largeInteger); - s_invFrequency = float64(largeInteger.QuadPart); - if (s_invFrequency > 0.0f) - { - s_invFrequency = 1000.0f / s_invFrequency; - } - } - - QueryPerformanceCounter(&largeInteger); - m_start = float64(largeInteger.QuadPart); -} - -void b2Timer::Reset() -{ - LARGE_INTEGER largeInteger; - QueryPerformanceCounter(&largeInteger); - m_start = float64(largeInteger.QuadPart); -} - -float32 b2Timer::GetMilliseconds() const -{ - LARGE_INTEGER largeInteger; - QueryPerformanceCounter(&largeInteger); - float64 count = float64(largeInteger.QuadPart); - float32 ms = float32(s_invFrequency * (count - m_start)); - return ms; -} - -#elif defined(__linux__) || defined (__APPLE__) - -#include - -b2Timer::b2Timer() -{ - Reset(); -} - -void b2Timer::Reset() -{ - timeval t; - gettimeofday(&t, 0); - m_start_sec = t.tv_sec; - m_start_usec = t.tv_usec; -} - -float32 b2Timer::GetMilliseconds() const -{ - timeval t; - gettimeofday(&t, 0); - return 1000.0f * (t.tv_sec - m_start_sec) + 0.001f * (t.tv_usec - m_start_usec); -} - -#else - -b2Timer::b2Timer() -{ -} - -void b2Timer::Reset() -{ -} - -float32 b2Timer::GetMilliseconds() const -{ - return 0.0f; -} - -#endif diff --git a/libs/Box2D/Common/b2Timer.h b/libs/Box2D/Common/b2Timer.h deleted file mode 100644 index 823404fe..00000000 --- a/libs/Box2D/Common/b2Timer.h +++ /dev/null @@ -1,50 +0,0 @@ -/* -* Copyright (c) 2011 Erin Catto http://box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef B2_TIMER_H -#define B2_TIMER_H - -#include - -/// Timer for profiling. This has platform specific code and may -/// not work on every platform. -class b2Timer -{ -public: - - /// Constructor - b2Timer(); - - /// Reset the timer. - void Reset(); - - /// Get the time since construction or the last reset. - float32 GetMilliseconds() const; - -private: - -#if defined(_WIN32) - float64 m_start; - static float64 s_invFrequency; -#elif defined(__linux__) || defined (__APPLE__) - unsigned long m_start_sec; - unsigned long m_start_usec; -#endif -}; - -#endif diff --git a/libs/Box2D/Dynamics/Contacts/b2ChainAndCircleContact.cpp b/libs/Box2D/Dynamics/Contacts/b2ChainAndCircleContact.cpp deleted file mode 100644 index 5e2f0f05..00000000 --- a/libs/Box2D/Dynamics/Contacts/b2ChainAndCircleContact.cpp +++ /dev/null @@ -1,53 +0,0 @@ -/* -* Copyright (c) 2006-2010 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ - -#include -#include -#include -#include -#include - -#include - -b2Contact* b2ChainAndCircleContact::Create(b2Fixture* fixtureA, int32 indexA, b2Fixture* fixtureB, int32 indexB, b2BlockAllocator* allocator) -{ - void* mem = allocator->Allocate(sizeof(b2ChainAndCircleContact)); - return new (mem) b2ChainAndCircleContact(fixtureA, indexA, fixtureB, indexB); -} - -void b2ChainAndCircleContact::Destroy(b2Contact* contact, b2BlockAllocator* allocator) -{ - ((b2ChainAndCircleContact*)contact)->~b2ChainAndCircleContact(); - allocator->Free(contact, sizeof(b2ChainAndCircleContact)); -} - -b2ChainAndCircleContact::b2ChainAndCircleContact(b2Fixture* fixtureA, int32 indexA, b2Fixture* fixtureB, int32 indexB) -: b2Contact(fixtureA, indexA, fixtureB, indexB) -{ - b2Assert(m_fixtureA->GetType() == b2Shape::e_chain); - b2Assert(m_fixtureB->GetType() == b2Shape::e_circle); -} - -void b2ChainAndCircleContact::Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB) -{ - b2ChainShape* chain = (b2ChainShape*)m_fixtureA->GetShape(); - b2EdgeShape edge; - chain->GetChildEdge(&edge, m_indexA); - b2CollideEdgeAndCircle( manifold, &edge, xfA, - (b2CircleShape*)m_fixtureB->GetShape(), xfB); -} diff --git a/libs/Box2D/Dynamics/Contacts/b2ChainAndCircleContact.h b/libs/Box2D/Dynamics/Contacts/b2ChainAndCircleContact.h deleted file mode 100644 index 2dad0b64..00000000 --- a/libs/Box2D/Dynamics/Contacts/b2ChainAndCircleContact.h +++ /dev/null @@ -1,39 +0,0 @@ -/* -* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef B2_CHAIN_AND_CIRCLE_CONTACT_H -#define B2_CHAIN_AND_CIRCLE_CONTACT_H - -#include - -class b2BlockAllocator; - -class b2ChainAndCircleContact : public b2Contact -{ -public: - static b2Contact* Create( b2Fixture* fixtureA, int32 indexA, - b2Fixture* fixtureB, int32 indexB, b2BlockAllocator* allocator); - static void Destroy(b2Contact* contact, b2BlockAllocator* allocator); - - b2ChainAndCircleContact(b2Fixture* fixtureA, int32 indexA, b2Fixture* fixtureB, int32 indexB); - ~b2ChainAndCircleContact() {} - - void Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB); -}; - -#endif diff --git a/libs/Box2D/Dynamics/Contacts/b2ChainAndPolygonContact.cpp b/libs/Box2D/Dynamics/Contacts/b2ChainAndPolygonContact.cpp deleted file mode 100644 index a4a99b8c..00000000 --- a/libs/Box2D/Dynamics/Contacts/b2ChainAndPolygonContact.cpp +++ /dev/null @@ -1,53 +0,0 @@ -/* -* Copyright (c) 2006-2010 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ - -#include -#include -#include -#include -#include - -#include - -b2Contact* b2ChainAndPolygonContact::Create(b2Fixture* fixtureA, int32 indexA, b2Fixture* fixtureB, int32 indexB, b2BlockAllocator* allocator) -{ - void* mem = allocator->Allocate(sizeof(b2ChainAndPolygonContact)); - return new (mem) b2ChainAndPolygonContact(fixtureA, indexA, fixtureB, indexB); -} - -void b2ChainAndPolygonContact::Destroy(b2Contact* contact, b2BlockAllocator* allocator) -{ - ((b2ChainAndPolygonContact*)contact)->~b2ChainAndPolygonContact(); - allocator->Free(contact, sizeof(b2ChainAndPolygonContact)); -} - -b2ChainAndPolygonContact::b2ChainAndPolygonContact(b2Fixture* fixtureA, int32 indexA, b2Fixture* fixtureB, int32 indexB) -: b2Contact(fixtureA, indexA, fixtureB, indexB) -{ - b2Assert(m_fixtureA->GetType() == b2Shape::e_chain); - b2Assert(m_fixtureB->GetType() == b2Shape::e_polygon); -} - -void b2ChainAndPolygonContact::Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB) -{ - b2ChainShape* chain = (b2ChainShape*)m_fixtureA->GetShape(); - b2EdgeShape edge; - chain->GetChildEdge(&edge, m_indexA); - b2CollideEdgeAndPolygon( manifold, &edge, xfA, - (b2PolygonShape*)m_fixtureB->GetShape(), xfB); -} diff --git a/libs/Box2D/Dynamics/Contacts/b2ChainAndPolygonContact.h b/libs/Box2D/Dynamics/Contacts/b2ChainAndPolygonContact.h deleted file mode 100644 index 8f30ee86..00000000 --- a/libs/Box2D/Dynamics/Contacts/b2ChainAndPolygonContact.h +++ /dev/null @@ -1,39 +0,0 @@ -/* -* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef B2_CHAIN_AND_POLYGON_CONTACT_H -#define B2_CHAIN_AND_POLYGON_CONTACT_H - -#include - -class b2BlockAllocator; - -class b2ChainAndPolygonContact : public b2Contact -{ -public: - static b2Contact* Create( b2Fixture* fixtureA, int32 indexA, - b2Fixture* fixtureB, int32 indexB, b2BlockAllocator* allocator); - static void Destroy(b2Contact* contact, b2BlockAllocator* allocator); - - b2ChainAndPolygonContact(b2Fixture* fixtureA, int32 indexA, b2Fixture* fixtureB, int32 indexB); - ~b2ChainAndPolygonContact() {} - - void Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB); -}; - -#endif diff --git a/libs/Box2D/Dynamics/Contacts/b2CircleContact.cpp b/libs/Box2D/Dynamics/Contacts/b2CircleContact.cpp deleted file mode 100644 index 23b98a9f..00000000 --- a/libs/Box2D/Dynamics/Contacts/b2CircleContact.cpp +++ /dev/null @@ -1,52 +0,0 @@ -/* -* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ - -#include -#include -#include -#include -#include -#include - -#include - -b2Contact* b2CircleContact::Create(b2Fixture* fixtureA, int32, b2Fixture* fixtureB, int32, b2BlockAllocator* allocator) -{ - void* mem = allocator->Allocate(sizeof(b2CircleContact)); - return new (mem) b2CircleContact(fixtureA, fixtureB); -} - -void b2CircleContact::Destroy(b2Contact* contact, b2BlockAllocator* allocator) -{ - ((b2CircleContact*)contact)->~b2CircleContact(); - allocator->Free(contact, sizeof(b2CircleContact)); -} - -b2CircleContact::b2CircleContact(b2Fixture* fixtureA, b2Fixture* fixtureB) - : b2Contact(fixtureA, 0, fixtureB, 0) -{ - b2Assert(m_fixtureA->GetType() == b2Shape::e_circle); - b2Assert(m_fixtureB->GetType() == b2Shape::e_circle); -} - -void b2CircleContact::Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB) -{ - b2CollideCircles(manifold, - (b2CircleShape*)m_fixtureA->GetShape(), xfA, - (b2CircleShape*)m_fixtureB->GetShape(), xfB); -} diff --git a/libs/Box2D/Dynamics/Contacts/b2CircleContact.h b/libs/Box2D/Dynamics/Contacts/b2CircleContact.h deleted file mode 100644 index aac1f0bd..00000000 --- a/libs/Box2D/Dynamics/Contacts/b2CircleContact.h +++ /dev/null @@ -1,39 +0,0 @@ -/* -* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef B2_CIRCLE_CONTACT_H -#define B2_CIRCLE_CONTACT_H - -#include - -class b2BlockAllocator; - -class b2CircleContact : public b2Contact -{ -public: - static b2Contact* Create( b2Fixture* fixtureA, int32 indexA, - b2Fixture* fixtureB, int32 indexB, b2BlockAllocator* allocator); - static void Destroy(b2Contact* contact, b2BlockAllocator* allocator); - - b2CircleContact(b2Fixture* fixtureA, b2Fixture* fixtureB); - ~b2CircleContact() {} - - void Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB); -}; - -#endif diff --git a/libs/Box2D/Dynamics/Contacts/b2ContactSolver.h b/libs/Box2D/Dynamics/Contacts/b2ContactSolver.h deleted file mode 100644 index cb568a33..00000000 --- a/libs/Box2D/Dynamics/Contacts/b2ContactSolver.h +++ /dev/null @@ -1,95 +0,0 @@ -/* -* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef B2_CONTACT_SOLVER_H -#define B2_CONTACT_SOLVER_H - -#include -#include -#include - -class b2Contact; -class b2Body; -class b2StackAllocator; -struct b2ContactPositionConstraint; - -struct b2VelocityConstraintPoint -{ - b2Vec2 rA; - b2Vec2 rB; - float32 normalImpulse; - float32 tangentImpulse; - float32 normalMass; - float32 tangentMass; - float32 velocityBias; -}; - -struct b2ContactVelocityConstraint -{ - b2VelocityConstraintPoint points[b2_maxManifoldPoints]; - b2Vec2 normal; - b2Mat22 normalMass; - b2Mat22 K; - int32 indexA; - int32 indexB; - float32 invMassA, invMassB; - float32 invIA, invIB; - float32 friction; - float32 restitution; - float32 tangentSpeed; - int32 pointCount; - int32 contactIndex; -}; - -struct b2ContactSolverDef -{ - b2TimeStep step; - b2Contact** contacts; - int32 count; - b2Position* positions; - b2Velocity* velocities; - b2StackAllocator* allocator; -}; - -class b2ContactSolver -{ -public: - b2ContactSolver(b2ContactSolverDef* def); - ~b2ContactSolver(); - - void InitializeVelocityConstraints(); - - void WarmStart(); - void SolveVelocityConstraints(); - void StoreImpulses(); - - bool SolvePositionConstraints(); - bool SolveTOIPositionConstraints(int32 toiIndexA, int32 toiIndexB); - - b2TimeStep m_step; - b2Position* m_positions; - b2Velocity* m_velocities; - b2StackAllocator* m_allocator; - b2ContactPositionConstraint* m_positionConstraints; - b2ContactVelocityConstraint* m_velocityConstraints; - b2Contact** m_contacts; - int m_count; -}; - -#endif - diff --git a/libs/Box2D/Dynamics/Contacts/b2EdgeAndCircleContact.cpp b/libs/Box2D/Dynamics/Contacts/b2EdgeAndCircleContact.cpp deleted file mode 100644 index ee622685..00000000 --- a/libs/Box2D/Dynamics/Contacts/b2EdgeAndCircleContact.cpp +++ /dev/null @@ -1,49 +0,0 @@ -/* -* Copyright (c) 2006-2010 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ - -#include -#include -#include - -#include - -b2Contact* b2EdgeAndCircleContact::Create(b2Fixture* fixtureA, int32, b2Fixture* fixtureB, int32, b2BlockAllocator* allocator) -{ - void* mem = allocator->Allocate(sizeof(b2EdgeAndCircleContact)); - return new (mem) b2EdgeAndCircleContact(fixtureA, fixtureB); -} - -void b2EdgeAndCircleContact::Destroy(b2Contact* contact, b2BlockAllocator* allocator) -{ - ((b2EdgeAndCircleContact*)contact)->~b2EdgeAndCircleContact(); - allocator->Free(contact, sizeof(b2EdgeAndCircleContact)); -} - -b2EdgeAndCircleContact::b2EdgeAndCircleContact(b2Fixture* fixtureA, b2Fixture* fixtureB) -: b2Contact(fixtureA, 0, fixtureB, 0) -{ - b2Assert(m_fixtureA->GetType() == b2Shape::e_edge); - b2Assert(m_fixtureB->GetType() == b2Shape::e_circle); -} - -void b2EdgeAndCircleContact::Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB) -{ - b2CollideEdgeAndCircle( manifold, - (b2EdgeShape*)m_fixtureA->GetShape(), xfA, - (b2CircleShape*)m_fixtureB->GetShape(), xfB); -} diff --git a/libs/Box2D/Dynamics/Contacts/b2EdgeAndCircleContact.h b/libs/Box2D/Dynamics/Contacts/b2EdgeAndCircleContact.h deleted file mode 100644 index 11b61b88..00000000 --- a/libs/Box2D/Dynamics/Contacts/b2EdgeAndCircleContact.h +++ /dev/null @@ -1,39 +0,0 @@ -/* -* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef B2_EDGE_AND_CIRCLE_CONTACT_H -#define B2_EDGE_AND_CIRCLE_CONTACT_H - -#include - -class b2BlockAllocator; - -class b2EdgeAndCircleContact : public b2Contact -{ -public: - static b2Contact* Create( b2Fixture* fixtureA, int32 indexA, - b2Fixture* fixtureB, int32 indexB, b2BlockAllocator* allocator); - static void Destroy(b2Contact* contact, b2BlockAllocator* allocator); - - b2EdgeAndCircleContact(b2Fixture* fixtureA, b2Fixture* fixtureB); - ~b2EdgeAndCircleContact() {} - - void Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB); -}; - -#endif diff --git a/libs/Box2D/Dynamics/Contacts/b2EdgeAndPolygonContact.cpp b/libs/Box2D/Dynamics/Contacts/b2EdgeAndPolygonContact.cpp deleted file mode 100644 index 379ce243..00000000 --- a/libs/Box2D/Dynamics/Contacts/b2EdgeAndPolygonContact.cpp +++ /dev/null @@ -1,49 +0,0 @@ -/* -* Copyright (c) 2006-2010 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ - -#include -#include -#include - -#include - -b2Contact* b2EdgeAndPolygonContact::Create(b2Fixture* fixtureA, int32, b2Fixture* fixtureB, int32, b2BlockAllocator* allocator) -{ - void* mem = allocator->Allocate(sizeof(b2EdgeAndPolygonContact)); - return new (mem) b2EdgeAndPolygonContact(fixtureA, fixtureB); -} - -void b2EdgeAndPolygonContact::Destroy(b2Contact* contact, b2BlockAllocator* allocator) -{ - ((b2EdgeAndPolygonContact*)contact)->~b2EdgeAndPolygonContact(); - allocator->Free(contact, sizeof(b2EdgeAndPolygonContact)); -} - -b2EdgeAndPolygonContact::b2EdgeAndPolygonContact(b2Fixture* fixtureA, b2Fixture* fixtureB) -: b2Contact(fixtureA, 0, fixtureB, 0) -{ - b2Assert(m_fixtureA->GetType() == b2Shape::e_edge); - b2Assert(m_fixtureB->GetType() == b2Shape::e_polygon); -} - -void b2EdgeAndPolygonContact::Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB) -{ - b2CollideEdgeAndPolygon( manifold, - (b2EdgeShape*)m_fixtureA->GetShape(), xfA, - (b2PolygonShape*)m_fixtureB->GetShape(), xfB); -} diff --git a/libs/Box2D/Dynamics/Contacts/b2EdgeAndPolygonContact.h b/libs/Box2D/Dynamics/Contacts/b2EdgeAndPolygonContact.h deleted file mode 100644 index 74b27ee0..00000000 --- a/libs/Box2D/Dynamics/Contacts/b2EdgeAndPolygonContact.h +++ /dev/null @@ -1,39 +0,0 @@ -/* -* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef B2_EDGE_AND_POLYGON_CONTACT_H -#define B2_EDGE_AND_POLYGON_CONTACT_H - -#include - -class b2BlockAllocator; - -class b2EdgeAndPolygonContact : public b2Contact -{ -public: - static b2Contact* Create( b2Fixture* fixtureA, int32 indexA, - b2Fixture* fixtureB, int32 indexB, b2BlockAllocator* allocator); - static void Destroy(b2Contact* contact, b2BlockAllocator* allocator); - - b2EdgeAndPolygonContact(b2Fixture* fixtureA, b2Fixture* fixtureB); - ~b2EdgeAndPolygonContact() {} - - void Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB); -}; - -#endif diff --git a/libs/Box2D/Dynamics/Contacts/b2PolygonAndCircleContact.cpp b/libs/Box2D/Dynamics/Contacts/b2PolygonAndCircleContact.cpp deleted file mode 100644 index 41da9f19..00000000 --- a/libs/Box2D/Dynamics/Contacts/b2PolygonAndCircleContact.cpp +++ /dev/null @@ -1,49 +0,0 @@ -/* -* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ - -#include -#include -#include - -#include - -b2Contact* b2PolygonAndCircleContact::Create(b2Fixture* fixtureA, int32, b2Fixture* fixtureB, int32, b2BlockAllocator* allocator) -{ - void* mem = allocator->Allocate(sizeof(b2PolygonAndCircleContact)); - return new (mem) b2PolygonAndCircleContact(fixtureA, fixtureB); -} - -void b2PolygonAndCircleContact::Destroy(b2Contact* contact, b2BlockAllocator* allocator) -{ - ((b2PolygonAndCircleContact*)contact)->~b2PolygonAndCircleContact(); - allocator->Free(contact, sizeof(b2PolygonAndCircleContact)); -} - -b2PolygonAndCircleContact::b2PolygonAndCircleContact(b2Fixture* fixtureA, b2Fixture* fixtureB) -: b2Contact(fixtureA, 0, fixtureB, 0) -{ - b2Assert(m_fixtureA->GetType() == b2Shape::e_polygon); - b2Assert(m_fixtureB->GetType() == b2Shape::e_circle); -} - -void b2PolygonAndCircleContact::Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB) -{ - b2CollidePolygonAndCircle( manifold, - (b2PolygonShape*)m_fixtureA->GetShape(), xfA, - (b2CircleShape*)m_fixtureB->GetShape(), xfB); -} diff --git a/libs/Box2D/Dynamics/Contacts/b2PolygonAndCircleContact.h b/libs/Box2D/Dynamics/Contacts/b2PolygonAndCircleContact.h deleted file mode 100644 index 6beca16a..00000000 --- a/libs/Box2D/Dynamics/Contacts/b2PolygonAndCircleContact.h +++ /dev/null @@ -1,38 +0,0 @@ -/* -* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef B2_POLYGON_AND_CIRCLE_CONTACT_H -#define B2_POLYGON_AND_CIRCLE_CONTACT_H - -#include - -class b2BlockAllocator; - -class b2PolygonAndCircleContact : public b2Contact -{ -public: - static b2Contact* Create(b2Fixture* fixtureA, int32 indexA, b2Fixture* fixtureB, int32 indexB, b2BlockAllocator* allocator); - static void Destroy(b2Contact* contact, b2BlockAllocator* allocator); - - b2PolygonAndCircleContact(b2Fixture* fixtureA, b2Fixture* fixtureB); - ~b2PolygonAndCircleContact() {} - - void Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB); -}; - -#endif diff --git a/libs/Box2D/Dynamics/Contacts/b2PolygonContact.cpp b/libs/Box2D/Dynamics/Contacts/b2PolygonContact.cpp deleted file mode 100644 index e466190e..00000000 --- a/libs/Box2D/Dynamics/Contacts/b2PolygonContact.cpp +++ /dev/null @@ -1,52 +0,0 @@ -/* -* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ - -#include -#include -#include -#include -#include -#include - -#include - -b2Contact* b2PolygonContact::Create(b2Fixture* fixtureA, int32, b2Fixture* fixtureB, int32, b2BlockAllocator* allocator) -{ - void* mem = allocator->Allocate(sizeof(b2PolygonContact)); - return new (mem) b2PolygonContact(fixtureA, fixtureB); -} - -void b2PolygonContact::Destroy(b2Contact* contact, b2BlockAllocator* allocator) -{ - ((b2PolygonContact*)contact)->~b2PolygonContact(); - allocator->Free(contact, sizeof(b2PolygonContact)); -} - -b2PolygonContact::b2PolygonContact(b2Fixture* fixtureA, b2Fixture* fixtureB) - : b2Contact(fixtureA, 0, fixtureB, 0) -{ - b2Assert(m_fixtureA->GetType() == b2Shape::e_polygon); - b2Assert(m_fixtureB->GetType() == b2Shape::e_polygon); -} - -void b2PolygonContact::Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB) -{ - b2CollidePolygons( manifold, - (b2PolygonShape*)m_fixtureA->GetShape(), xfA, - (b2PolygonShape*)m_fixtureB->GetShape(), xfB); -} diff --git a/libs/Box2D/Dynamics/Contacts/b2PolygonContact.h b/libs/Box2D/Dynamics/Contacts/b2PolygonContact.h deleted file mode 100644 index 45932148..00000000 --- a/libs/Box2D/Dynamics/Contacts/b2PolygonContact.h +++ /dev/null @@ -1,39 +0,0 @@ -/* -* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef B2_POLYGON_CONTACT_H -#define B2_POLYGON_CONTACT_H - -#include - -class b2BlockAllocator; - -class b2PolygonContact : public b2Contact -{ -public: - static b2Contact* Create( b2Fixture* fixtureA, int32 indexA, - b2Fixture* fixtureB, int32 indexB, b2BlockAllocator* allocator); - static void Destroy(b2Contact* contact, b2BlockAllocator* allocator); - - b2PolygonContact(b2Fixture* fixtureA, b2Fixture* fixtureB); - ~b2PolygonContact() {} - - void Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB); -}; - -#endif diff --git a/libs/Box2D/Dynamics/Joints/b2DistanceJoint.cpp b/libs/Box2D/Dynamics/Joints/b2DistanceJoint.cpp deleted file mode 100644 index 1ced7d9d..00000000 --- a/libs/Box2D/Dynamics/Joints/b2DistanceJoint.cpp +++ /dev/null @@ -1,260 +0,0 @@ -/* -* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ - -#include -#include -#include - -// 1-D constrained system -// m (v2 - v1) = lambda -// v2 + (beta/h) * x1 + gamma * lambda = 0, gamma has units of inverse mass. -// x2 = x1 + h * v2 - -// 1-D mass-damper-spring system -// m (v2 - v1) + h * d * v2 + h * k * - -// C = norm(p2 - p1) - L -// u = (p2 - p1) / norm(p2 - p1) -// Cdot = dot(u, v2 + cross(w2, r2) - v1 - cross(w1, r1)) -// J = [-u -cross(r1, u) u cross(r2, u)] -// K = J * invM * JT -// = invMass1 + invI1 * cross(r1, u)^2 + invMass2 + invI2 * cross(r2, u)^2 - -void b2DistanceJointDef::Initialize(b2Body* b1, b2Body* b2, - const b2Vec2& anchor1, const b2Vec2& anchor2) -{ - bodyA = b1; - bodyB = b2; - localAnchorA = bodyA->GetLocalPoint(anchor1); - localAnchorB = bodyB->GetLocalPoint(anchor2); - b2Vec2 d = anchor2 - anchor1; - length = d.Length(); -} - -b2DistanceJoint::b2DistanceJoint(const b2DistanceJointDef* def) -: b2Joint(def) -{ - m_localAnchorA = def->localAnchorA; - m_localAnchorB = def->localAnchorB; - m_length = def->length; - m_frequencyHz = def->frequencyHz; - m_dampingRatio = def->dampingRatio; - m_impulse = 0.0f; - m_gamma = 0.0f; - m_bias = 0.0f; -} - -void b2DistanceJoint::InitVelocityConstraints(const b2SolverData& data) -{ - m_indexA = m_bodyA->m_islandIndex; - m_indexB = m_bodyB->m_islandIndex; - m_localCenterA = m_bodyA->m_sweep.localCenter; - m_localCenterB = m_bodyB->m_sweep.localCenter; - m_invMassA = m_bodyA->m_invMass; - m_invMassB = m_bodyB->m_invMass; - m_invIA = m_bodyA->m_invI; - m_invIB = m_bodyB->m_invI; - - b2Vec2 cA = data.positions[m_indexA].c; - float32 aA = data.positions[m_indexA].a; - b2Vec2 vA = data.velocities[m_indexA].v; - float32 wA = data.velocities[m_indexA].w; - - b2Vec2 cB = data.positions[m_indexB].c; - float32 aB = data.positions[m_indexB].a; - b2Vec2 vB = data.velocities[m_indexB].v; - float32 wB = data.velocities[m_indexB].w; - - b2Rot qA(aA), qB(aB); - - m_rA = b2Mul(qA, m_localAnchorA - m_localCenterA); - m_rB = b2Mul(qB, m_localAnchorB - m_localCenterB); - m_u = cB + m_rB - cA - m_rA; - - // Handle singularity. - float32 length = m_u.Length(); - if (length > b2_linearSlop) - { - m_u *= 1.0f / length; - } - else - { - m_u.Set(0.0f, 0.0f); - } - - float32 crAu = b2Cross(m_rA, m_u); - float32 crBu = b2Cross(m_rB, m_u); - float32 invMass = m_invMassA + m_invIA * crAu * crAu + m_invMassB + m_invIB * crBu * crBu; - - // Compute the effective mass matrix. - m_mass = invMass != 0.0f ? 1.0f / invMass : 0.0f; - - if (m_frequencyHz > 0.0f) - { - float32 C = length - m_length; - - // Frequency - float32 omega = 2.0f * b2_pi * m_frequencyHz; - - // Damping coefficient - float32 d = 2.0f * m_mass * m_dampingRatio * omega; - - // Spring stiffness - float32 k = m_mass * omega * omega; - - // magic formulas - float32 h = data.step.dt; - m_gamma = h * (d + h * k); - m_gamma = m_gamma != 0.0f ? 1.0f / m_gamma : 0.0f; - m_bias = C * h * k * m_gamma; - - invMass += m_gamma; - m_mass = invMass != 0.0f ? 1.0f / invMass : 0.0f; - } - else - { - m_gamma = 0.0f; - m_bias = 0.0f; - } - - if (data.step.warmStarting) - { - // Scale the impulse to support a variable time step. - m_impulse *= data.step.dtRatio; - - b2Vec2 P = m_impulse * m_u; - vA -= m_invMassA * P; - wA -= m_invIA * b2Cross(m_rA, P); - vB += m_invMassB * P; - wB += m_invIB * b2Cross(m_rB, P); - } - else - { - m_impulse = 0.0f; - } - - data.velocities[m_indexA].v = vA; - data.velocities[m_indexA].w = wA; - data.velocities[m_indexB].v = vB; - data.velocities[m_indexB].w = wB; -} - -void b2DistanceJoint::SolveVelocityConstraints(const b2SolverData& data) -{ - b2Vec2 vA = data.velocities[m_indexA].v; - float32 wA = data.velocities[m_indexA].w; - b2Vec2 vB = data.velocities[m_indexB].v; - float32 wB = data.velocities[m_indexB].w; - - // Cdot = dot(u, v + cross(w, r)) - b2Vec2 vpA = vA + b2Cross(wA, m_rA); - b2Vec2 vpB = vB + b2Cross(wB, m_rB); - float32 Cdot = b2Dot(m_u, vpB - vpA); - - float32 impulse = -m_mass * (Cdot + m_bias + m_gamma * m_impulse); - m_impulse += impulse; - - b2Vec2 P = impulse * m_u; - vA -= m_invMassA * P; - wA -= m_invIA * b2Cross(m_rA, P); - vB += m_invMassB * P; - wB += m_invIB * b2Cross(m_rB, P); - - data.velocities[m_indexA].v = vA; - data.velocities[m_indexA].w = wA; - data.velocities[m_indexB].v = vB; - data.velocities[m_indexB].w = wB; -} - -bool b2DistanceJoint::SolvePositionConstraints(const b2SolverData& data) -{ - if (m_frequencyHz > 0.0f) - { - // There is no position correction for soft distance constraints. - return true; - } - - b2Vec2 cA = data.positions[m_indexA].c; - float32 aA = data.positions[m_indexA].a; - b2Vec2 cB = data.positions[m_indexB].c; - float32 aB = data.positions[m_indexB].a; - - b2Rot qA(aA), qB(aB); - - b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_localCenterA); - b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_localCenterB); - b2Vec2 u = cB + rB - cA - rA; - - float32 length = u.Normalize(); - float32 C = length - m_length; - C = b2Clamp(C, -b2_maxLinearCorrection, b2_maxLinearCorrection); - - float32 impulse = -m_mass * C; - b2Vec2 P = impulse * u; - - cA -= m_invMassA * P; - aA -= m_invIA * b2Cross(rA, P); - cB += m_invMassB * P; - aB += m_invIB * b2Cross(rB, P); - - data.positions[m_indexA].c = cA; - data.positions[m_indexA].a = aA; - data.positions[m_indexB].c = cB; - data.positions[m_indexB].a = aB; - - return b2Abs(C) < b2_linearSlop; -} - -b2Vec2 b2DistanceJoint::GetAnchorA() const -{ - return m_bodyA->GetWorldPoint(m_localAnchorA); -} - -b2Vec2 b2DistanceJoint::GetAnchorB() const -{ - return m_bodyB->GetWorldPoint(m_localAnchorB); -} - -b2Vec2 b2DistanceJoint::GetReactionForce(float32 inv_dt) const -{ - b2Vec2 F = (inv_dt * m_impulse) * m_u; - return F; -} - -float32 b2DistanceJoint::GetReactionTorque(float32 inv_dt) const -{ - B2_NOT_USED(inv_dt); - return 0.0f; -} - -void b2DistanceJoint::Dump() -{ - int32 indexA = m_bodyA->m_islandIndex; - int32 indexB = m_bodyB->m_islandIndex; - - b2Log(" b2DistanceJointDef jd;\n"); - b2Log(" jd.bodyA = bodies[%d];\n", indexA); - b2Log(" jd.bodyB = bodies[%d];\n", indexB); - b2Log(" jd.collideConnected = bool(%d);\n", m_collideConnected); - b2Log(" jd.localAnchorA.Set(%.15lef, %.15lef);\n", m_localAnchorA.x, m_localAnchorA.y); - b2Log(" jd.localAnchorB.Set(%.15lef, %.15lef);\n", m_localAnchorB.x, m_localAnchorB.y); - b2Log(" jd.length = %.15lef;\n", m_length); - b2Log(" jd.frequencyHz = %.15lef;\n", m_frequencyHz); - b2Log(" jd.dampingRatio = %.15lef;\n", m_dampingRatio); - b2Log(" joints[%d] = m_world->CreateJoint(&jd);\n", m_index); -} diff --git a/libs/Box2D/Dynamics/Joints/b2DistanceJoint.h b/libs/Box2D/Dynamics/Joints/b2DistanceJoint.h deleted file mode 100644 index 9d0528b9..00000000 --- a/libs/Box2D/Dynamics/Joints/b2DistanceJoint.h +++ /dev/null @@ -1,169 +0,0 @@ -/* -* Copyright (c) 2006-2007 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef B2_DISTANCE_JOINT_H -#define B2_DISTANCE_JOINT_H - -#include - -/// Distance joint definition. This requires defining an -/// anchor point on both bodies and the non-zero length of the -/// distance joint. The definition uses local anchor points -/// so that the initial configuration can violate the constraint -/// slightly. This helps when saving and loading a game. -/// @warning Do not use a zero or short length. -struct b2DistanceJointDef : public b2JointDef -{ - b2DistanceJointDef() - { - type = e_distanceJoint; - localAnchorA.Set(0.0f, 0.0f); - localAnchorB.Set(0.0f, 0.0f); - length = 1.0f; - frequencyHz = 0.0f; - dampingRatio = 0.0f; - } - - /// Initialize the bodies, anchors, and length using the world - /// anchors. - void Initialize(b2Body* bodyA, b2Body* bodyB, - const b2Vec2& anchorA, const b2Vec2& anchorB); - - /// The local anchor point relative to bodyA's origin. - b2Vec2 localAnchorA; - - /// The local anchor point relative to bodyB's origin. - b2Vec2 localAnchorB; - - /// The natural length between the anchor points. - float32 length; - - /// The mass-spring-damper frequency in Hertz. A value of 0 - /// disables softness. - float32 frequencyHz; - - /// The damping ratio. 0 = no damping, 1 = critical damping. - float32 dampingRatio; -}; - -/// A distance joint constrains two points on two bodies -/// to remain at a fixed distance from each other. You can view -/// this as a massless, rigid rod. -class b2DistanceJoint : public b2Joint -{ -public: - - b2Vec2 GetAnchorA() const; - b2Vec2 GetAnchorB() const; - - /// Get the reaction force given the inverse time step. - /// Unit is N. - b2Vec2 GetReactionForce(float32 inv_dt) const; - - /// Get the reaction torque given the inverse time step. - /// Unit is N*m. This is always zero for a distance joint. - float32 GetReactionTorque(float32 inv_dt) const; - - /// The local anchor point relative to bodyA's origin. - const b2Vec2& GetLocalAnchorA() const { return m_localAnchorA; } - - /// The local anchor point relative to bodyB's origin. - const b2Vec2& GetLocalAnchorB() const { return m_localAnchorB; } - - /// Set/get the natural length. - /// Manipulating the length can lead to non-physical behavior when the frequency is zero. - void SetLength(float32 length); - float32 GetLength() const; - - /// Set/get frequency in Hz. - void SetFrequency(float32 hz); - float32 GetFrequency() const; - - /// Set/get damping ratio. - void SetDampingRatio(float32 ratio); - float32 GetDampingRatio() const; - - /// Dump joint to dmLog - void Dump(); - -protected: - - friend class b2Joint; - b2DistanceJoint(const b2DistanceJointDef* data); - - void InitVelocityConstraints(const b2SolverData& data); - void SolveVelocityConstraints(const b2SolverData& data); - bool SolvePositionConstraints(const b2SolverData& data); - - float32 m_frequencyHz; - float32 m_dampingRatio; - float32 m_bias; - - // Solver shared - b2Vec2 m_localAnchorA; - b2Vec2 m_localAnchorB; - float32 m_gamma; - float32 m_impulse; - float32 m_length; - - // Solver temp - int32 m_indexA; - int32 m_indexB; - b2Vec2 m_u; - b2Vec2 m_rA; - b2Vec2 m_rB; - b2Vec2 m_localCenterA; - b2Vec2 m_localCenterB; - float32 m_invMassA; - float32 m_invMassB; - float32 m_invIA; - float32 m_invIB; - float32 m_mass; -}; - -inline void b2DistanceJoint::SetLength(float32 length) -{ - m_length = length; -} - -inline float32 b2DistanceJoint::GetLength() const -{ - return m_length; -} - -inline void b2DistanceJoint::SetFrequency(float32 hz) -{ - m_frequencyHz = hz; -} - -inline float32 b2DistanceJoint::GetFrequency() const -{ - return m_frequencyHz; -} - -inline void b2DistanceJoint::SetDampingRatio(float32 ratio) -{ - m_dampingRatio = ratio; -} - -inline float32 b2DistanceJoint::GetDampingRatio() const -{ - return m_dampingRatio; -} - -#endif diff --git a/libs/Box2D/Dynamics/Joints/b2FrictionJoint.h b/libs/Box2D/Dynamics/Joints/b2FrictionJoint.h deleted file mode 100644 index 785d9ac1..00000000 --- a/libs/Box2D/Dynamics/Joints/b2FrictionJoint.h +++ /dev/null @@ -1,119 +0,0 @@ -/* -* Copyright (c) 2006-2007 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef B2_FRICTION_JOINT_H -#define B2_FRICTION_JOINT_H - -#include - -/// Friction joint definition. -struct b2FrictionJointDef : public b2JointDef -{ - b2FrictionJointDef() - { - type = e_frictionJoint; - localAnchorA.SetZero(); - localAnchorB.SetZero(); - maxForce = 0.0f; - maxTorque = 0.0f; - } - - /// Initialize the bodies, anchors, axis, and reference angle using the world - /// anchor and world axis. - void Initialize(b2Body* bodyA, b2Body* bodyB, const b2Vec2& anchor); - - /// The local anchor point relative to bodyA's origin. - b2Vec2 localAnchorA; - - /// The local anchor point relative to bodyB's origin. - b2Vec2 localAnchorB; - - /// The maximum friction force in N. - float32 maxForce; - - /// The maximum friction torque in N-m. - float32 maxTorque; -}; - -/// Friction joint. This is used for top-down friction. -/// It provides 2D translational friction and angular friction. -class b2FrictionJoint : public b2Joint -{ -public: - b2Vec2 GetAnchorA() const; - b2Vec2 GetAnchorB() const; - - b2Vec2 GetReactionForce(float32 inv_dt) const; - float32 GetReactionTorque(float32 inv_dt) const; - - /// The local anchor point relative to bodyA's origin. - const b2Vec2& GetLocalAnchorA() const { return m_localAnchorA; } - - /// The local anchor point relative to bodyB's origin. - const b2Vec2& GetLocalAnchorB() const { return m_localAnchorB; } - - /// Set the maximum friction force in N. - void SetMaxForce(float32 force); - - /// Get the maximum friction force in N. - float32 GetMaxForce() const; - - /// Set the maximum friction torque in N*m. - void SetMaxTorque(float32 torque); - - /// Get the maximum friction torque in N*m. - float32 GetMaxTorque() const; - - /// Dump joint to dmLog - void Dump(); - -protected: - - friend class b2Joint; - - b2FrictionJoint(const b2FrictionJointDef* def); - - void InitVelocityConstraints(const b2SolverData& data); - void SolveVelocityConstraints(const b2SolverData& data); - bool SolvePositionConstraints(const b2SolverData& data); - - b2Vec2 m_localAnchorA; - b2Vec2 m_localAnchorB; - - // Solver shared - b2Vec2 m_linearImpulse; - float32 m_angularImpulse; - float32 m_maxForce; - float32 m_maxTorque; - - // Solver temp - int32 m_indexA; - int32 m_indexB; - b2Vec2 m_rA; - b2Vec2 m_rB; - b2Vec2 m_localCenterA; - b2Vec2 m_localCenterB; - float32 m_invMassA; - float32 m_invMassB; - float32 m_invIA; - float32 m_invIB; - b2Mat22 m_linearMass; - float32 m_angularMass; -}; - -#endif diff --git a/libs/Box2D/Dynamics/Joints/b2GearJoint.h b/libs/Box2D/Dynamics/Joints/b2GearJoint.h deleted file mode 100644 index 48cdfd88..00000000 --- a/libs/Box2D/Dynamics/Joints/b2GearJoint.h +++ /dev/null @@ -1,125 +0,0 @@ -/* -* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef B2_GEAR_JOINT_H -#define B2_GEAR_JOINT_H - -#include - -/// Gear joint definition. This definition requires two existing -/// revolute or prismatic joints (any combination will work). -struct b2GearJointDef : public b2JointDef -{ - b2GearJointDef() - { - type = e_gearJoint; - joint1 = NULL; - joint2 = NULL; - ratio = 1.0f; - } - - /// The first revolute/prismatic joint attached to the gear joint. - b2Joint* joint1; - - /// The second revolute/prismatic joint attached to the gear joint. - b2Joint* joint2; - - /// The gear ratio. - /// @see b2GearJoint for explanation. - float32 ratio; -}; - -/// A gear joint is used to connect two joints together. Either joint -/// can be a revolute or prismatic joint. You specify a gear ratio -/// to bind the motions together: -/// coordinate1 + ratio * coordinate2 = constant -/// The ratio can be negative or positive. If one joint is a revolute joint -/// and the other joint is a prismatic joint, then the ratio will have units -/// of length or units of 1/length. -/// @warning You have to manually destroy the gear joint if joint1 or joint2 -/// is destroyed. -class b2GearJoint : public b2Joint -{ -public: - b2Vec2 GetAnchorA() const; - b2Vec2 GetAnchorB() const; - - b2Vec2 GetReactionForce(float32 inv_dt) const; - float32 GetReactionTorque(float32 inv_dt) const; - - /// Get the first joint. - b2Joint* GetJoint1() { return m_joint1; } - - /// Get the second joint. - b2Joint* GetJoint2() { return m_joint2; } - - /// Set/Get the gear ratio. - void SetRatio(float32 ratio); - float32 GetRatio() const; - - /// Dump joint to dmLog - void Dump(); - -protected: - - friend class b2Joint; - b2GearJoint(const b2GearJointDef* data); - - void InitVelocityConstraints(const b2SolverData& data); - void SolveVelocityConstraints(const b2SolverData& data); - bool SolvePositionConstraints(const b2SolverData& data); - - b2Joint* m_joint1; - b2Joint* m_joint2; - - b2JointType m_typeA; - b2JointType m_typeB; - - // Body A is connected to body C - // Body B is connected to body D - b2Body* m_bodyC; - b2Body* m_bodyD; - - // Solver shared - b2Vec2 m_localAnchorA; - b2Vec2 m_localAnchorB; - b2Vec2 m_localAnchorC; - b2Vec2 m_localAnchorD; - - b2Vec2 m_localAxisC; - b2Vec2 m_localAxisD; - - float32 m_referenceAngleA; - float32 m_referenceAngleB; - - float32 m_constant; - float32 m_ratio; - - float32 m_impulse; - - // Solver temp - int32 m_indexA, m_indexB, m_indexC, m_indexD; - b2Vec2 m_lcA, m_lcB, m_lcC, m_lcD; - float32 m_mA, m_mB, m_mC, m_mD; - float32 m_iA, m_iB, m_iC, m_iD; - b2Vec2 m_JvAC, m_JvBD; - float32 m_JwA, m_JwB, m_JwC, m_JwD; - float32 m_mass; -}; - -#endif diff --git a/libs/Box2D/Dynamics/Joints/b2Joint.cpp b/libs/Box2D/Dynamics/Joints/b2Joint.cpp deleted file mode 100644 index 50919495..00000000 --- a/libs/Box2D/Dynamics/Joints/b2Joint.cpp +++ /dev/null @@ -1,211 +0,0 @@ -/* -* Copyright (c) 2006-2007 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include - -b2Joint* b2Joint::Create(const b2JointDef* def, b2BlockAllocator* allocator) -{ - b2Joint* joint = NULL; - - switch (def->type) - { - case e_distanceJoint: - { - void* mem = allocator->Allocate(sizeof(b2DistanceJoint)); - joint = new (mem) b2DistanceJoint(static_cast(def)); - } - break; - - case e_mouseJoint: - { - void* mem = allocator->Allocate(sizeof(b2MouseJoint)); - joint = new (mem) b2MouseJoint(static_cast(def)); - } - break; - - case e_prismaticJoint: - { - void* mem = allocator->Allocate(sizeof(b2PrismaticJoint)); - joint = new (mem) b2PrismaticJoint(static_cast(def)); - } - break; - - case e_revoluteJoint: - { - void* mem = allocator->Allocate(sizeof(b2RevoluteJoint)); - joint = new (mem) b2RevoluteJoint(static_cast(def)); - } - break; - - case e_pulleyJoint: - { - void* mem = allocator->Allocate(sizeof(b2PulleyJoint)); - joint = new (mem) b2PulleyJoint(static_cast(def)); - } - break; - - case e_gearJoint: - { - void* mem = allocator->Allocate(sizeof(b2GearJoint)); - joint = new (mem) b2GearJoint(static_cast(def)); - } - break; - - case e_wheelJoint: - { - void* mem = allocator->Allocate(sizeof(b2WheelJoint)); - joint = new (mem) b2WheelJoint(static_cast(def)); - } - break; - - case e_weldJoint: - { - void* mem = allocator->Allocate(sizeof(b2WeldJoint)); - joint = new (mem) b2WeldJoint(static_cast(def)); - } - break; - - case e_frictionJoint: - { - void* mem = allocator->Allocate(sizeof(b2FrictionJoint)); - joint = new (mem) b2FrictionJoint(static_cast(def)); - } - break; - - case e_ropeJoint: - { - void* mem = allocator->Allocate(sizeof(b2RopeJoint)); - joint = new (mem) b2RopeJoint(static_cast(def)); - } - break; - - case e_motorJoint: - { - void* mem = allocator->Allocate(sizeof(b2MotorJoint)); - joint = new (mem) b2MotorJoint(static_cast(def)); - } - break; - - default: - b2Assert(false); - break; - } - - return joint; -} - -void b2Joint::Destroy(b2Joint* joint, b2BlockAllocator* allocator) -{ - joint->~b2Joint(); - switch (joint->m_type) - { - case e_distanceJoint: - allocator->Free(joint, sizeof(b2DistanceJoint)); - break; - - case e_mouseJoint: - allocator->Free(joint, sizeof(b2MouseJoint)); - break; - - case e_prismaticJoint: - allocator->Free(joint, sizeof(b2PrismaticJoint)); - break; - - case e_revoluteJoint: - allocator->Free(joint, sizeof(b2RevoluteJoint)); - break; - - case e_pulleyJoint: - allocator->Free(joint, sizeof(b2PulleyJoint)); - break; - - case e_gearJoint: - allocator->Free(joint, sizeof(b2GearJoint)); - break; - - case e_wheelJoint: - allocator->Free(joint, sizeof(b2WheelJoint)); - break; - - case e_weldJoint: - allocator->Free(joint, sizeof(b2WeldJoint)); - break; - - case e_frictionJoint: - allocator->Free(joint, sizeof(b2FrictionJoint)); - break; - - case e_ropeJoint: - allocator->Free(joint, sizeof(b2RopeJoint)); - break; - - case e_motorJoint: - allocator->Free(joint, sizeof(b2MotorJoint)); - break; - - default: - b2Assert(false); - break; - } -} - -b2Joint::b2Joint(const b2JointDef* def) -{ - b2Assert(def->bodyA != def->bodyB); - - m_type = def->type; - m_prev = NULL; - m_next = NULL; - m_bodyA = def->bodyA; - m_bodyB = def->bodyB; - m_index = 0; - m_collideConnected = def->collideConnected; - m_islandFlag = false; - m_userData = def->userData; - - m_edgeA.joint = NULL; - m_edgeA.other = NULL; - m_edgeA.prev = NULL; - m_edgeA.next = NULL; - - m_edgeB.joint = NULL; - m_edgeB.other = NULL; - m_edgeB.prev = NULL; - m_edgeB.next = NULL; -} - -bool b2Joint::IsActive() const -{ - return m_bodyA->IsActive() && m_bodyB->IsActive(); -} diff --git a/libs/Box2D/Dynamics/Joints/b2MotorJoint.h b/libs/Box2D/Dynamics/Joints/b2MotorJoint.h deleted file mode 100644 index 486225b9..00000000 --- a/libs/Box2D/Dynamics/Joints/b2MotorJoint.h +++ /dev/null @@ -1,133 +0,0 @@ -/* -* Copyright (c) 2006-2012 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef B2_MOTOR_JOINT_H -#define B2_MOTOR_JOINT_H - -#include - -/// Motor joint definition. -struct b2MotorJointDef : public b2JointDef -{ - b2MotorJointDef() - { - type = e_motorJoint; - linearOffset.SetZero(); - angularOffset = 0.0f; - maxForce = 1.0f; - maxTorque = 1.0f; - correctionFactor = 0.3f; - } - - /// Initialize the bodies and offsets using the current transforms. - void Initialize(b2Body* bodyA, b2Body* bodyB); - - /// Position of bodyB minus the position of bodyA, in bodyA's frame, in meters. - b2Vec2 linearOffset; - - /// The bodyB angle minus bodyA angle in radians. - float32 angularOffset; - - /// The maximum motor force in N. - float32 maxForce; - - /// The maximum motor torque in N-m. - float32 maxTorque; - - /// Position correction factor in the range [0,1]. - float32 correctionFactor; -}; - -/// A motor joint is used to control the relative motion -/// between two bodies. A typical usage is to control the movement -/// of a dynamic body with respect to the ground. -class b2MotorJoint : public b2Joint -{ -public: - b2Vec2 GetAnchorA() const; - b2Vec2 GetAnchorB() const; - - b2Vec2 GetReactionForce(float32 inv_dt) const; - float32 GetReactionTorque(float32 inv_dt) const; - - /// Set/get the target linear offset, in frame A, in meters. - void SetLinearOffset(const b2Vec2& linearOffset); - const b2Vec2& GetLinearOffset() const; - - /// Set/get the target angular offset, in radians. - void SetAngularOffset(float32 angularOffset); - float32 GetAngularOffset() const; - - /// Set the maximum friction force in N. - void SetMaxForce(float32 force); - - /// Get the maximum friction force in N. - float32 GetMaxForce() const; - - /// Set the maximum friction torque in N*m. - void SetMaxTorque(float32 torque); - - /// Get the maximum friction torque in N*m. - float32 GetMaxTorque() const; - - /// Set the position correction factor in the range [0,1]. - void SetCorrectionFactor(float32 factor); - - /// Get the position correction factor in the range [0,1]. - float32 GetCorrectionFactor() const; - - /// Dump to b2Log - void Dump(); - -protected: - - friend class b2Joint; - - b2MotorJoint(const b2MotorJointDef* def); - - void InitVelocityConstraints(const b2SolverData& data); - void SolveVelocityConstraints(const b2SolverData& data); - bool SolvePositionConstraints(const b2SolverData& data); - - // Solver shared - b2Vec2 m_linearOffset; - float32 m_angularOffset; - b2Vec2 m_linearImpulse; - float32 m_angularImpulse; - float32 m_maxForce; - float32 m_maxTorque; - float32 m_correctionFactor; - - // Solver temp - int32 m_indexA; - int32 m_indexB; - b2Vec2 m_rA; - b2Vec2 m_rB; - b2Vec2 m_localCenterA; - b2Vec2 m_localCenterB; - b2Vec2 m_linearError; - float32 m_angularError; - float32 m_invMassA; - float32 m_invMassB; - float32 m_invIA; - float32 m_invIB; - b2Mat22 m_linearMass; - float32 m_angularMass; -}; - -#endif diff --git a/libs/Box2D/Dynamics/Joints/b2MouseJoint.h b/libs/Box2D/Dynamics/Joints/b2MouseJoint.h deleted file mode 100644 index aba70ec1..00000000 --- a/libs/Box2D/Dynamics/Joints/b2MouseJoint.h +++ /dev/null @@ -1,129 +0,0 @@ -/* -* Copyright (c) 2006-2007 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef B2_MOUSE_JOINT_H -#define B2_MOUSE_JOINT_H - -#include - -/// Mouse joint definition. This requires a world target point, -/// tuning parameters, and the time step. -struct b2MouseJointDef : public b2JointDef -{ - b2MouseJointDef() - { - type = e_mouseJoint; - target.Set(0.0f, 0.0f); - maxForce = 0.0f; - frequencyHz = 5.0f; - dampingRatio = 0.7f; - } - - /// The initial world target point. This is assumed - /// to coincide with the body anchor initially. - b2Vec2 target; - - /// The maximum constraint force that can be exerted - /// to move the candidate body. Usually you will express - /// as some multiple of the weight (multiplier * mass * gravity). - float32 maxForce; - - /// The response speed. - float32 frequencyHz; - - /// The damping ratio. 0 = no damping, 1 = critical damping. - float32 dampingRatio; -}; - -/// A mouse joint is used to make a point on a body track a -/// specified world point. This a soft constraint with a maximum -/// force. This allows the constraint to stretch and without -/// applying huge forces. -/// NOTE: this joint is not documented in the manual because it was -/// developed to be used in the testbed. If you want to learn how to -/// use the mouse joint, look at the testbed. -class b2MouseJoint : public b2Joint -{ -public: - - /// Implements b2Joint. - b2Vec2 GetAnchorA() const; - - /// Implements b2Joint. - b2Vec2 GetAnchorB() const; - - /// Implements b2Joint. - b2Vec2 GetReactionForce(float32 inv_dt) const; - - /// Implements b2Joint. - float32 GetReactionTorque(float32 inv_dt) const; - - /// Use this to update the target point. - void SetTarget(const b2Vec2& target); - const b2Vec2& GetTarget() const; - - /// Set/get the maximum force in Newtons. - void SetMaxForce(float32 force); - float32 GetMaxForce() const; - - /// Set/get the frequency in Hertz. - void SetFrequency(float32 hz); - float32 GetFrequency() const; - - /// Set/get the damping ratio (dimensionless). - void SetDampingRatio(float32 ratio); - float32 GetDampingRatio() const; - - /// The mouse joint does not support dumping. - void Dump() { b2Log("Mouse joint dumping is not supported.\n"); } - - /// Implement b2Joint::ShiftOrigin - void ShiftOrigin(const b2Vec2& newOrigin); - -protected: - friend class b2Joint; - - b2MouseJoint(const b2MouseJointDef* def); - - void InitVelocityConstraints(const b2SolverData& data); - void SolveVelocityConstraints(const b2SolverData& data); - bool SolvePositionConstraints(const b2SolverData& data); - - b2Vec2 m_localAnchorB; - b2Vec2 m_targetA; - float32 m_frequencyHz; - float32 m_dampingRatio; - float32 m_beta; - - // Solver shared - b2Vec2 m_impulse; - float32 m_maxForce; - float32 m_gamma; - - // Solver temp - int32 m_indexA; - int32 m_indexB; - b2Vec2 m_rB; - b2Vec2 m_localCenterB; - float32 m_invMassB; - float32 m_invIB; - b2Mat22 m_mass; - b2Vec2 m_C; -}; - -#endif diff --git a/libs/Box2D/Dynamics/Joints/b2PrismaticJoint.cpp b/libs/Box2D/Dynamics/Joints/b2PrismaticJoint.cpp deleted file mode 100644 index bcf8ba40..00000000 --- a/libs/Box2D/Dynamics/Joints/b2PrismaticJoint.cpp +++ /dev/null @@ -1,629 +0,0 @@ -/* -* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ - -#include -#include -#include - -// Linear constraint (point-to-line) -// d = p2 - p1 = x2 + r2 - x1 - r1 -// C = dot(perp, d) -// Cdot = dot(d, cross(w1, perp)) + dot(perp, v2 + cross(w2, r2) - v1 - cross(w1, r1)) -// = -dot(perp, v1) - dot(cross(d + r1, perp), w1) + dot(perp, v2) + dot(cross(r2, perp), v2) -// J = [-perp, -cross(d + r1, perp), perp, cross(r2,perp)] -// -// Angular constraint -// C = a2 - a1 + a_initial -// Cdot = w2 - w1 -// J = [0 0 -1 0 0 1] -// -// K = J * invM * JT -// -// J = [-a -s1 a s2] -// [0 -1 0 1] -// a = perp -// s1 = cross(d + r1, a) = cross(p2 - x1, a) -// s2 = cross(r2, a) = cross(p2 - x2, a) - - -// Motor/Limit linear constraint -// C = dot(ax1, d) -// Cdot = = -dot(ax1, v1) - dot(cross(d + r1, ax1), w1) + dot(ax1, v2) + dot(cross(r2, ax1), v2) -// J = [-ax1 -cross(d+r1,ax1) ax1 cross(r2,ax1)] - -// Block Solver -// We develop a block solver that includes the joint limit. This makes the limit stiff (inelastic) even -// when the mass has poor distribution (leading to large torques about the joint anchor points). -// -// The Jacobian has 3 rows: -// J = [-uT -s1 uT s2] // linear -// [0 -1 0 1] // angular -// [-vT -a1 vT a2] // limit -// -// u = perp -// v = axis -// s1 = cross(d + r1, u), s2 = cross(r2, u) -// a1 = cross(d + r1, v), a2 = cross(r2, v) - -// M * (v2 - v1) = JT * df -// J * v2 = bias -// -// v2 = v1 + invM * JT * df -// J * (v1 + invM * JT * df) = bias -// K * df = bias - J * v1 = -Cdot -// K = J * invM * JT -// Cdot = J * v1 - bias -// -// Now solve for f2. -// df = f2 - f1 -// K * (f2 - f1) = -Cdot -// f2 = invK * (-Cdot) + f1 -// -// Clamp accumulated limit impulse. -// lower: f2(3) = max(f2(3), 0) -// upper: f2(3) = min(f2(3), 0) -// -// Solve for correct f2(1:2) -// K(1:2, 1:2) * f2(1:2) = -Cdot(1:2) - K(1:2,3) * f2(3) + K(1:2,1:3) * f1 -// = -Cdot(1:2) - K(1:2,3) * f2(3) + K(1:2,1:2) * f1(1:2) + K(1:2,3) * f1(3) -// K(1:2, 1:2) * f2(1:2) = -Cdot(1:2) - K(1:2,3) * (f2(3) - f1(3)) + K(1:2,1:2) * f1(1:2) -// f2(1:2) = invK(1:2,1:2) * (-Cdot(1:2) - K(1:2,3) * (f2(3) - f1(3))) + f1(1:2) -// -// Now compute impulse to be applied: -// df = f2 - f1 - -void b2PrismaticJointDef::Initialize(b2Body* bA, b2Body* bB, const b2Vec2& anchor, const b2Vec2& axis) -{ - bodyA = bA; - bodyB = bB; - localAnchorA = bodyA->GetLocalPoint(anchor); - localAnchorB = bodyB->GetLocalPoint(anchor); - localAxisA = bodyA->GetLocalVector(axis); - referenceAngle = bodyB->GetAngle() - bodyA->GetAngle(); -} - -b2PrismaticJoint::b2PrismaticJoint(const b2PrismaticJointDef* def) -: b2Joint(def) -{ - m_localAnchorA = def->localAnchorA; - m_localAnchorB = def->localAnchorB; - m_localXAxisA = def->localAxisA; - m_localXAxisA.Normalize(); - m_localYAxisA = b2Cross(1.0f, m_localXAxisA); - m_referenceAngle = def->referenceAngle; - - m_impulse.SetZero(); - m_motorMass = 0.0f; - m_motorImpulse = 0.0f; - - m_lowerTranslation = def->lowerTranslation; - m_upperTranslation = def->upperTranslation; - m_maxMotorForce = def->maxMotorForce; - m_motorSpeed = def->motorSpeed; - m_enableLimit = def->enableLimit; - m_enableMotor = def->enableMotor; - m_limitState = e_inactiveLimit; - - m_axis.SetZero(); - m_perp.SetZero(); -} - -void b2PrismaticJoint::InitVelocityConstraints(const b2SolverData& data) -{ - m_indexA = m_bodyA->m_islandIndex; - m_indexB = m_bodyB->m_islandIndex; - m_localCenterA = m_bodyA->m_sweep.localCenter; - m_localCenterB = m_bodyB->m_sweep.localCenter; - m_invMassA = m_bodyA->m_invMass; - m_invMassB = m_bodyB->m_invMass; - m_invIA = m_bodyA->m_invI; - m_invIB = m_bodyB->m_invI; - - b2Vec2 cA = data.positions[m_indexA].c; - float32 aA = data.positions[m_indexA].a; - b2Vec2 vA = data.velocities[m_indexA].v; - float32 wA = data.velocities[m_indexA].w; - - b2Vec2 cB = data.positions[m_indexB].c; - float32 aB = data.positions[m_indexB].a; - b2Vec2 vB = data.velocities[m_indexB].v; - float32 wB = data.velocities[m_indexB].w; - - b2Rot qA(aA), qB(aB); - - // Compute the effective masses. - b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_localCenterA); - b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_localCenterB); - b2Vec2 d = (cB - cA) + rB - rA; - - float32 mA = m_invMassA, mB = m_invMassB; - float32 iA = m_invIA, iB = m_invIB; - - // Compute motor Jacobian and effective mass. - { - m_axis = b2Mul(qA, m_localXAxisA); - m_a1 = b2Cross(d + rA, m_axis); - m_a2 = b2Cross(rB, m_axis); - - m_motorMass = mA + mB + iA * m_a1 * m_a1 + iB * m_a2 * m_a2; - if (m_motorMass > 0.0f) - { - m_motorMass = 1.0f / m_motorMass; - } - } - - // Prismatic constraint. - { - m_perp = b2Mul(qA, m_localYAxisA); - - m_s1 = b2Cross(d + rA, m_perp); - m_s2 = b2Cross(rB, m_perp); - - //float32 s1test; - //s1test = b2Cross(rA, m_perp); - - float32 k11 = mA + mB + iA * m_s1 * m_s1 + iB * m_s2 * m_s2; - float32 k12 = iA * m_s1 + iB * m_s2; - float32 k13 = iA * m_s1 * m_a1 + iB * m_s2 * m_a2; - float32 k22 = iA + iB; - if (k22 == 0.0f) - { - // For bodies with fixed rotation. - k22 = 1.0f; - } - float32 k23 = iA * m_a1 + iB * m_a2; - float32 k33 = mA + mB + iA * m_a1 * m_a1 + iB * m_a2 * m_a2; - - m_K.ex.Set(k11, k12, k13); - m_K.ey.Set(k12, k22, k23); - m_K.ez.Set(k13, k23, k33); - } - - // Compute motor and limit terms. - if (m_enableLimit) - { - float32 jointTranslation = b2Dot(m_axis, d); - if (b2Abs(m_upperTranslation - m_lowerTranslation) < 2.0f * b2_linearSlop) - { - m_limitState = e_equalLimits; - } - else if (jointTranslation <= m_lowerTranslation) - { - if (m_limitState != e_atLowerLimit) - { - m_limitState = e_atLowerLimit; - m_impulse.z = 0.0f; - } - } - else if (jointTranslation >= m_upperTranslation) - { - if (m_limitState != e_atUpperLimit) - { - m_limitState = e_atUpperLimit; - m_impulse.z = 0.0f; - } - } - else - { - m_limitState = e_inactiveLimit; - m_impulse.z = 0.0f; - } - } - else - { - m_limitState = e_inactiveLimit; - m_impulse.z = 0.0f; - } - - if (m_enableMotor == false) - { - m_motorImpulse = 0.0f; - } - - if (data.step.warmStarting) - { - // Account for variable time step. - m_impulse *= data.step.dtRatio; - m_motorImpulse *= data.step.dtRatio; - - b2Vec2 P = m_impulse.x * m_perp + (m_motorImpulse + m_impulse.z) * m_axis; - float32 LA = m_impulse.x * m_s1 + m_impulse.y + (m_motorImpulse + m_impulse.z) * m_a1; - float32 LB = m_impulse.x * m_s2 + m_impulse.y + (m_motorImpulse + m_impulse.z) * m_a2; - - vA -= mA * P; - wA -= iA * LA; - - vB += mB * P; - wB += iB * LB; - } - else - { - m_impulse.SetZero(); - m_motorImpulse = 0.0f; - } - - data.velocities[m_indexA].v = vA; - data.velocities[m_indexA].w = wA; - data.velocities[m_indexB].v = vB; - data.velocities[m_indexB].w = wB; -} - -void b2PrismaticJoint::SolveVelocityConstraints(const b2SolverData& data) -{ - b2Vec2 vA = data.velocities[m_indexA].v; - float32 wA = data.velocities[m_indexA].w; - b2Vec2 vB = data.velocities[m_indexB].v; - float32 wB = data.velocities[m_indexB].w; - - float32 mA = m_invMassA, mB = m_invMassB; - float32 iA = m_invIA, iB = m_invIB; - - // Solve linear motor constraint. - if (m_enableMotor && m_limitState != e_equalLimits) - { - float32 Cdot = b2Dot(m_axis, vB - vA) + m_a2 * wB - m_a1 * wA; - float32 impulse = m_motorMass * (m_motorSpeed - Cdot); - float32 oldImpulse = m_motorImpulse; - float32 maxImpulse = data.step.dt * m_maxMotorForce; - m_motorImpulse = b2Clamp(m_motorImpulse + impulse, -maxImpulse, maxImpulse); - impulse = m_motorImpulse - oldImpulse; - - b2Vec2 P = impulse * m_axis; - float32 LA = impulse * m_a1; - float32 LB = impulse * m_a2; - - vA -= mA * P; - wA -= iA * LA; - - vB += mB * P; - wB += iB * LB; - } - - b2Vec2 Cdot1; - Cdot1.x = b2Dot(m_perp, vB - vA) + m_s2 * wB - m_s1 * wA; - Cdot1.y = wB - wA; - - if (m_enableLimit && m_limitState != e_inactiveLimit) - { - // Solve prismatic and limit constraint in block form. - float32 Cdot2; - Cdot2 = b2Dot(m_axis, vB - vA) + m_a2 * wB - m_a1 * wA; - b2Vec3 Cdot(Cdot1.x, Cdot1.y, Cdot2); - - b2Vec3 f1 = m_impulse; - b2Vec3 df = m_K.Solve33(-Cdot); - m_impulse += df; - - if (m_limitState == e_atLowerLimit) - { - m_impulse.z = b2Max(m_impulse.z, 0.0f); - } - else if (m_limitState == e_atUpperLimit) - { - m_impulse.z = b2Min(m_impulse.z, 0.0f); - } - - // f2(1:2) = invK(1:2,1:2) * (-Cdot(1:2) - K(1:2,3) * (f2(3) - f1(3))) + f1(1:2) - b2Vec2 b = -Cdot1 - (m_impulse.z - f1.z) * b2Vec2(m_K.ez.x, m_K.ez.y); - b2Vec2 f2r = m_K.Solve22(b) + b2Vec2(f1.x, f1.y); - m_impulse.x = f2r.x; - m_impulse.y = f2r.y; - - df = m_impulse - f1; - - b2Vec2 P = df.x * m_perp + df.z * m_axis; - float32 LA = df.x * m_s1 + df.y + df.z * m_a1; - float32 LB = df.x * m_s2 + df.y + df.z * m_a2; - - vA -= mA * P; - wA -= iA * LA; - - vB += mB * P; - wB += iB * LB; - } - else - { - // Limit is inactive, just solve the prismatic constraint in block form. - b2Vec2 df = m_K.Solve22(-Cdot1); - m_impulse.x += df.x; - m_impulse.y += df.y; - - b2Vec2 P = df.x * m_perp; - float32 LA = df.x * m_s1 + df.y; - float32 LB = df.x * m_s2 + df.y; - - vA -= mA * P; - wA -= iA * LA; - - vB += mB * P; - wB += iB * LB; - } - - data.velocities[m_indexA].v = vA; - data.velocities[m_indexA].w = wA; - data.velocities[m_indexB].v = vB; - data.velocities[m_indexB].w = wB; -} - -bool b2PrismaticJoint::SolvePositionConstraints(const b2SolverData& data) -{ - b2Vec2 cA = data.positions[m_indexA].c; - float32 aA = data.positions[m_indexA].a; - b2Vec2 cB = data.positions[m_indexB].c; - float32 aB = data.positions[m_indexB].a; - - b2Rot qA(aA), qB(aB); - - float32 mA = m_invMassA, mB = m_invMassB; - float32 iA = m_invIA, iB = m_invIB; - - // Compute fresh Jacobians - b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_localCenterA); - b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_localCenterB); - b2Vec2 d = cB + rB - cA - rA; - - b2Vec2 axis = b2Mul(qA, m_localXAxisA); - float32 a1 = b2Cross(d + rA, axis); - float32 a2 = b2Cross(rB, axis); - b2Vec2 perp = b2Mul(qA, m_localYAxisA); - - float32 s1 = b2Cross(d + rA, perp); - float32 s2 = b2Cross(rB, perp); - - b2Vec3 impulse; - b2Vec2 C1; - C1.x = b2Dot(perp, d); - C1.y = aB - aA - m_referenceAngle; - - float32 linearError = b2Abs(C1.x); - float32 angularError = b2Abs(C1.y); - - bool active = false; - float32 C2 = 0.0f; - if (m_enableLimit) - { - float32 translation = b2Dot(axis, d); - if (b2Abs(m_upperTranslation - m_lowerTranslation) < 2.0f * b2_linearSlop) - { - // Prevent large angular corrections - C2 = b2Clamp(translation, -b2_maxLinearCorrection, b2_maxLinearCorrection); - linearError = b2Max(linearError, b2Abs(translation)); - active = true; - } - else if (translation <= m_lowerTranslation) - { - // Prevent large linear corrections and allow some slop. - C2 = b2Clamp(translation - m_lowerTranslation + b2_linearSlop, -b2_maxLinearCorrection, 0.0f); - linearError = b2Max(linearError, m_lowerTranslation - translation); - active = true; - } - else if (translation >= m_upperTranslation) - { - // Prevent large linear corrections and allow some slop. - C2 = b2Clamp(translation - m_upperTranslation - b2_linearSlop, 0.0f, b2_maxLinearCorrection); - linearError = b2Max(linearError, translation - m_upperTranslation); - active = true; - } - } - - if (active) - { - float32 k11 = mA + mB + iA * s1 * s1 + iB * s2 * s2; - float32 k12 = iA * s1 + iB * s2; - float32 k13 = iA * s1 * a1 + iB * s2 * a2; - float32 k22 = iA + iB; - if (k22 == 0.0f) - { - // For fixed rotation - k22 = 1.0f; - } - float32 k23 = iA * a1 + iB * a2; - float32 k33 = mA + mB + iA * a1 * a1 + iB * a2 * a2; - - b2Mat33 K; - K.ex.Set(k11, k12, k13); - K.ey.Set(k12, k22, k23); - K.ez.Set(k13, k23, k33); - - b2Vec3 C; - C.x = C1.x; - C.y = C1.y; - C.z = C2; - - impulse = K.Solve33(-C); - } - else - { - float32 k11 = mA + mB + iA * s1 * s1 + iB * s2 * s2; - float32 k12 = iA * s1 + iB * s2; - float32 k22 = iA + iB; - if (k22 == 0.0f) - { - k22 = 1.0f; - } - - b2Mat22 K; - K.ex.Set(k11, k12); - K.ey.Set(k12, k22); - - b2Vec2 impulse1 = K.Solve(-C1); - impulse.x = impulse1.x; - impulse.y = impulse1.y; - impulse.z = 0.0f; - } - - b2Vec2 P = impulse.x * perp + impulse.z * axis; - float32 LA = impulse.x * s1 + impulse.y + impulse.z * a1; - float32 LB = impulse.x * s2 + impulse.y + impulse.z * a2; - - cA -= mA * P; - aA -= iA * LA; - cB += mB * P; - aB += iB * LB; - - data.positions[m_indexA].c = cA; - data.positions[m_indexA].a = aA; - data.positions[m_indexB].c = cB; - data.positions[m_indexB].a = aB; - - return linearError <= b2_linearSlop && angularError <= b2_angularSlop; -} - -b2Vec2 b2PrismaticJoint::GetAnchorA() const -{ - return m_bodyA->GetWorldPoint(m_localAnchorA); -} - -b2Vec2 b2PrismaticJoint::GetAnchorB() const -{ - return m_bodyB->GetWorldPoint(m_localAnchorB); -} - -b2Vec2 b2PrismaticJoint::GetReactionForce(float32 inv_dt) const -{ - return inv_dt * (m_impulse.x * m_perp + (m_motorImpulse + m_impulse.z) * m_axis); -} - -float32 b2PrismaticJoint::GetReactionTorque(float32 inv_dt) const -{ - return inv_dt * m_impulse.y; -} - -float32 b2PrismaticJoint::GetJointTranslation() const -{ - b2Vec2 pA = m_bodyA->GetWorldPoint(m_localAnchorA); - b2Vec2 pB = m_bodyB->GetWorldPoint(m_localAnchorB); - b2Vec2 d = pB - pA; - b2Vec2 axis = m_bodyA->GetWorldVector(m_localXAxisA); - - float32 translation = b2Dot(d, axis); - return translation; -} - -float32 b2PrismaticJoint::GetJointSpeed() const -{ - b2Body* bA = m_bodyA; - b2Body* bB = m_bodyB; - - b2Vec2 rA = b2Mul(bA->m_xf.q, m_localAnchorA - bA->m_sweep.localCenter); - b2Vec2 rB = b2Mul(bB->m_xf.q, m_localAnchorB - bB->m_sweep.localCenter); - b2Vec2 p1 = bA->m_sweep.c + rA; - b2Vec2 p2 = bB->m_sweep.c + rB; - b2Vec2 d = p2 - p1; - b2Vec2 axis = b2Mul(bA->m_xf.q, m_localXAxisA); - - b2Vec2 vA = bA->m_linearVelocity; - b2Vec2 vB = bB->m_linearVelocity; - float32 wA = bA->m_angularVelocity; - float32 wB = bB->m_angularVelocity; - - float32 speed = b2Dot(d, b2Cross(wA, axis)) + b2Dot(axis, vB + b2Cross(wB, rB) - vA - b2Cross(wA, rA)); - return speed; -} - -bool b2PrismaticJoint::IsLimitEnabled() const -{ - return m_enableLimit; -} - -void b2PrismaticJoint::EnableLimit(bool flag) -{ - if (flag != m_enableLimit) - { - m_bodyA->SetAwake(true); - m_bodyB->SetAwake(true); - m_enableLimit = flag; - m_impulse.z = 0.0f; - } -} - -float32 b2PrismaticJoint::GetLowerLimit() const -{ - return m_lowerTranslation; -} - -float32 b2PrismaticJoint::GetUpperLimit() const -{ - return m_upperTranslation; -} - -void b2PrismaticJoint::SetLimits(float32 lower, float32 upper) -{ - b2Assert(lower <= upper); - if (lower != m_lowerTranslation || upper != m_upperTranslation) - { - m_bodyA->SetAwake(true); - m_bodyB->SetAwake(true); - m_lowerTranslation = lower; - m_upperTranslation = upper; - m_impulse.z = 0.0f; - } -} - -bool b2PrismaticJoint::IsMotorEnabled() const -{ - return m_enableMotor; -} - -void b2PrismaticJoint::EnableMotor(bool flag) -{ - m_bodyA->SetAwake(true); - m_bodyB->SetAwake(true); - m_enableMotor = flag; -} - -void b2PrismaticJoint::SetMotorSpeed(float32 speed) -{ - m_bodyA->SetAwake(true); - m_bodyB->SetAwake(true); - m_motorSpeed = speed; -} - -void b2PrismaticJoint::SetMaxMotorForce(float32 force) -{ - m_bodyA->SetAwake(true); - m_bodyB->SetAwake(true); - m_maxMotorForce = force; -} - -float32 b2PrismaticJoint::GetMotorForce(float32 inv_dt) const -{ - return inv_dt * m_motorImpulse; -} - -void b2PrismaticJoint::Dump() -{ - int32 indexA = m_bodyA->m_islandIndex; - int32 indexB = m_bodyB->m_islandIndex; - - b2Log(" b2PrismaticJointDef jd;\n"); - b2Log(" jd.bodyA = bodies[%d];\n", indexA); - b2Log(" jd.bodyB = bodies[%d];\n", indexB); - b2Log(" jd.collideConnected = bool(%d);\n", m_collideConnected); - b2Log(" jd.localAnchorA.Set(%.15lef, %.15lef);\n", m_localAnchorA.x, m_localAnchorA.y); - b2Log(" jd.localAnchorB.Set(%.15lef, %.15lef);\n", m_localAnchorB.x, m_localAnchorB.y); - b2Log(" jd.localAxisA.Set(%.15lef, %.15lef);\n", m_localXAxisA.x, m_localXAxisA.y); - b2Log(" jd.referenceAngle = %.15lef;\n", m_referenceAngle); - b2Log(" jd.enableLimit = bool(%d);\n", m_enableLimit); - b2Log(" jd.lowerTranslation = %.15lef;\n", m_lowerTranslation); - b2Log(" jd.upperTranslation = %.15lef;\n", m_upperTranslation); - b2Log(" jd.enableMotor = bool(%d);\n", m_enableMotor); - b2Log(" jd.motorSpeed = %.15lef;\n", m_motorSpeed); - b2Log(" jd.maxMotorForce = %.15lef;\n", m_maxMotorForce); - b2Log(" joints[%d] = m_world->CreateJoint(&jd);\n", m_index); -} diff --git a/libs/Box2D/Dynamics/Joints/b2RevoluteJoint.cpp b/libs/Box2D/Dynamics/Joints/b2RevoluteJoint.cpp deleted file mode 100644 index d48cae81..00000000 --- a/libs/Box2D/Dynamics/Joints/b2RevoluteJoint.cpp +++ /dev/null @@ -1,502 +0,0 @@ -/* -* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ - -#include -#include -#include - -// Point-to-point constraint -// C = p2 - p1 -// Cdot = v2 - v1 -// = v2 + cross(w2, r2) - v1 - cross(w1, r1) -// J = [-I -r1_skew I r2_skew ] -// Identity used: -// w k % (rx i + ry j) = w * (-ry i + rx j) - -// Motor constraint -// Cdot = w2 - w1 -// J = [0 0 -1 0 0 1] -// K = invI1 + invI2 - -void b2RevoluteJointDef::Initialize(b2Body* bA, b2Body* bB, const b2Vec2& anchor) -{ - bodyA = bA; - bodyB = bB; - localAnchorA = bodyA->GetLocalPoint(anchor); - localAnchorB = bodyB->GetLocalPoint(anchor); - referenceAngle = bodyB->GetAngle() - bodyA->GetAngle(); -} - -b2RevoluteJoint::b2RevoluteJoint(const b2RevoluteJointDef* def) -: b2Joint(def) -{ - m_localAnchorA = def->localAnchorA; - m_localAnchorB = def->localAnchorB; - m_referenceAngle = def->referenceAngle; - - m_impulse.SetZero(); - m_motorImpulse = 0.0f; - - m_lowerAngle = def->lowerAngle; - m_upperAngle = def->upperAngle; - m_maxMotorTorque = def->maxMotorTorque; - m_motorSpeed = def->motorSpeed; - m_enableLimit = def->enableLimit; - m_enableMotor = def->enableMotor; - m_limitState = e_inactiveLimit; -} - -void b2RevoluteJoint::InitVelocityConstraints(const b2SolverData& data) -{ - m_indexA = m_bodyA->m_islandIndex; - m_indexB = m_bodyB->m_islandIndex; - m_localCenterA = m_bodyA->m_sweep.localCenter; - m_localCenterB = m_bodyB->m_sweep.localCenter; - m_invMassA = m_bodyA->m_invMass; - m_invMassB = m_bodyB->m_invMass; - m_invIA = m_bodyA->m_invI; - m_invIB = m_bodyB->m_invI; - - float32 aA = data.positions[m_indexA].a; - b2Vec2 vA = data.velocities[m_indexA].v; - float32 wA = data.velocities[m_indexA].w; - - float32 aB = data.positions[m_indexB].a; - b2Vec2 vB = data.velocities[m_indexB].v; - float32 wB = data.velocities[m_indexB].w; - - b2Rot qA(aA), qB(aB); - - m_rA = b2Mul(qA, m_localAnchorA - m_localCenterA); - m_rB = b2Mul(qB, m_localAnchorB - m_localCenterB); - - // J = [-I -r1_skew I r2_skew] - // [ 0 -1 0 1] - // r_skew = [-ry; rx] - - // Matlab - // K = [ mA+r1y^2*iA+mB+r2y^2*iB, -r1y*iA*r1x-r2y*iB*r2x, -r1y*iA-r2y*iB] - // [ -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB, r1x*iA+r2x*iB] - // [ -r1y*iA-r2y*iB, r1x*iA+r2x*iB, iA+iB] - - float32 mA = m_invMassA, mB = m_invMassB; - float32 iA = m_invIA, iB = m_invIB; - - bool fixedRotation = (iA + iB == 0.0f); - - m_mass.ex.x = mA + mB + m_rA.y * m_rA.y * iA + m_rB.y * m_rB.y * iB; - m_mass.ey.x = -m_rA.y * m_rA.x * iA - m_rB.y * m_rB.x * iB; - m_mass.ez.x = -m_rA.y * iA - m_rB.y * iB; - m_mass.ex.y = m_mass.ey.x; - m_mass.ey.y = mA + mB + m_rA.x * m_rA.x * iA + m_rB.x * m_rB.x * iB; - m_mass.ez.y = m_rA.x * iA + m_rB.x * iB; - m_mass.ex.z = m_mass.ez.x; - m_mass.ey.z = m_mass.ez.y; - m_mass.ez.z = iA + iB; - - m_motorMass = iA + iB; - if (m_motorMass > 0.0f) - { - m_motorMass = 1.0f / m_motorMass; - } - - if (m_enableMotor == false || fixedRotation) - { - m_motorImpulse = 0.0f; - } - - if (m_enableLimit && fixedRotation == false) - { - float32 jointAngle = aB - aA - m_referenceAngle; - if (b2Abs(m_upperAngle - m_lowerAngle) < 2.0f * b2_angularSlop) - { - m_limitState = e_equalLimits; - } - else if (jointAngle <= m_lowerAngle) - { - if (m_limitState != e_atLowerLimit) - { - m_impulse.z = 0.0f; - } - m_limitState = e_atLowerLimit; - } - else if (jointAngle >= m_upperAngle) - { - if (m_limitState != e_atUpperLimit) - { - m_impulse.z = 0.0f; - } - m_limitState = e_atUpperLimit; - } - else - { - m_limitState = e_inactiveLimit; - m_impulse.z = 0.0f; - } - } - else - { - m_limitState = e_inactiveLimit; - } - - if (data.step.warmStarting) - { - // Scale impulses to support a variable time step. - m_impulse *= data.step.dtRatio; - m_motorImpulse *= data.step.dtRatio; - - b2Vec2 P(m_impulse.x, m_impulse.y); - - vA -= mA * P; - wA -= iA * (b2Cross(m_rA, P) + m_motorImpulse + m_impulse.z); - - vB += mB * P; - wB += iB * (b2Cross(m_rB, P) + m_motorImpulse + m_impulse.z); - } - else - { - m_impulse.SetZero(); - m_motorImpulse = 0.0f; - } - - data.velocities[m_indexA].v = vA; - data.velocities[m_indexA].w = wA; - data.velocities[m_indexB].v = vB; - data.velocities[m_indexB].w = wB; -} - -void b2RevoluteJoint::SolveVelocityConstraints(const b2SolverData& data) -{ - b2Vec2 vA = data.velocities[m_indexA].v; - float32 wA = data.velocities[m_indexA].w; - b2Vec2 vB = data.velocities[m_indexB].v; - float32 wB = data.velocities[m_indexB].w; - - float32 mA = m_invMassA, mB = m_invMassB; - float32 iA = m_invIA, iB = m_invIB; - - bool fixedRotation = (iA + iB == 0.0f); - - // Solve motor constraint. - if (m_enableMotor && m_limitState != e_equalLimits && fixedRotation == false) - { - float32 Cdot = wB - wA - m_motorSpeed; - float32 impulse = -m_motorMass * Cdot; - float32 oldImpulse = m_motorImpulse; - float32 maxImpulse = data.step.dt * m_maxMotorTorque; - m_motorImpulse = b2Clamp(m_motorImpulse + impulse, -maxImpulse, maxImpulse); - impulse = m_motorImpulse - oldImpulse; - - wA -= iA * impulse; - wB += iB * impulse; - } - - // Solve limit constraint. - if (m_enableLimit && m_limitState != e_inactiveLimit && fixedRotation == false) - { - b2Vec2 Cdot1 = vB + b2Cross(wB, m_rB) - vA - b2Cross(wA, m_rA); - float32 Cdot2 = wB - wA; - b2Vec3 Cdot(Cdot1.x, Cdot1.y, Cdot2); - - b2Vec3 impulse = -m_mass.Solve33(Cdot); - - if (m_limitState == e_equalLimits) - { - m_impulse += impulse; - } - else if (m_limitState == e_atLowerLimit) - { - float32 newImpulse = m_impulse.z + impulse.z; - if (newImpulse < 0.0f) - { - b2Vec2 rhs = -Cdot1 + m_impulse.z * b2Vec2(m_mass.ez.x, m_mass.ez.y); - b2Vec2 reduced = m_mass.Solve22(rhs); - impulse.x = reduced.x; - impulse.y = reduced.y; - impulse.z = -m_impulse.z; - m_impulse.x += reduced.x; - m_impulse.y += reduced.y; - m_impulse.z = 0.0f; - } - else - { - m_impulse += impulse; - } - } - else if (m_limitState == e_atUpperLimit) - { - float32 newImpulse = m_impulse.z + impulse.z; - if (newImpulse > 0.0f) - { - b2Vec2 rhs = -Cdot1 + m_impulse.z * b2Vec2(m_mass.ez.x, m_mass.ez.y); - b2Vec2 reduced = m_mass.Solve22(rhs); - impulse.x = reduced.x; - impulse.y = reduced.y; - impulse.z = -m_impulse.z; - m_impulse.x += reduced.x; - m_impulse.y += reduced.y; - m_impulse.z = 0.0f; - } - else - { - m_impulse += impulse; - } - } - - b2Vec2 P(impulse.x, impulse.y); - - vA -= mA * P; - wA -= iA * (b2Cross(m_rA, P) + impulse.z); - - vB += mB * P; - wB += iB * (b2Cross(m_rB, P) + impulse.z); - } - else - { - // Solve point-to-point constraint - b2Vec2 Cdot = vB + b2Cross(wB, m_rB) - vA - b2Cross(wA, m_rA); - b2Vec2 impulse = m_mass.Solve22(-Cdot); - - m_impulse.x += impulse.x; - m_impulse.y += impulse.y; - - vA -= mA * impulse; - wA -= iA * b2Cross(m_rA, impulse); - - vB += mB * impulse; - wB += iB * b2Cross(m_rB, impulse); - } - - data.velocities[m_indexA].v = vA; - data.velocities[m_indexA].w = wA; - data.velocities[m_indexB].v = vB; - data.velocities[m_indexB].w = wB; -} - -bool b2RevoluteJoint::SolvePositionConstraints(const b2SolverData& data) -{ - b2Vec2 cA = data.positions[m_indexA].c; - float32 aA = data.positions[m_indexA].a; - b2Vec2 cB = data.positions[m_indexB].c; - float32 aB = data.positions[m_indexB].a; - - b2Rot qA(aA), qB(aB); - - float32 angularError = 0.0f; - float32 positionError = 0.0f; - - bool fixedRotation = (m_invIA + m_invIB == 0.0f); - - // Solve angular limit constraint. - if (m_enableLimit && m_limitState != e_inactiveLimit && fixedRotation == false) - { - float32 angle = aB - aA - m_referenceAngle; - float32 limitImpulse = 0.0f; - - if (m_limitState == e_equalLimits) - { - // Prevent large angular corrections - float32 C = b2Clamp(angle - m_lowerAngle, -b2_maxAngularCorrection, b2_maxAngularCorrection); - limitImpulse = -m_motorMass * C; - angularError = b2Abs(C); - } - else if (m_limitState == e_atLowerLimit) - { - float32 C = angle - m_lowerAngle; - angularError = -C; - - // Prevent large angular corrections and allow some slop. - C = b2Clamp(C + b2_angularSlop, -b2_maxAngularCorrection, 0.0f); - limitImpulse = -m_motorMass * C; - } - else if (m_limitState == e_atUpperLimit) - { - float32 C = angle - m_upperAngle; - angularError = C; - - // Prevent large angular corrections and allow some slop. - C = b2Clamp(C - b2_angularSlop, 0.0f, b2_maxAngularCorrection); - limitImpulse = -m_motorMass * C; - } - - aA -= m_invIA * limitImpulse; - aB += m_invIB * limitImpulse; - } - - // Solve point-to-point constraint. - { - qA.Set(aA); - qB.Set(aB); - b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_localCenterA); - b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_localCenterB); - - b2Vec2 C = cB + rB - cA - rA; - positionError = C.Length(); - - float32 mA = m_invMassA, mB = m_invMassB; - float32 iA = m_invIA, iB = m_invIB; - - b2Mat22 K; - K.ex.x = mA + mB + iA * rA.y * rA.y + iB * rB.y * rB.y; - K.ex.y = -iA * rA.x * rA.y - iB * rB.x * rB.y; - K.ey.x = K.ex.y; - K.ey.y = mA + mB + iA * rA.x * rA.x + iB * rB.x * rB.x; - - b2Vec2 impulse = -K.Solve(C); - - cA -= mA * impulse; - aA -= iA * b2Cross(rA, impulse); - - cB += mB * impulse; - aB += iB * b2Cross(rB, impulse); - } - - data.positions[m_indexA].c = cA; - data.positions[m_indexA].a = aA; - data.positions[m_indexB].c = cB; - data.positions[m_indexB].a = aB; - - return positionError <= b2_linearSlop && angularError <= b2_angularSlop; -} - -b2Vec2 b2RevoluteJoint::GetAnchorA() const -{ - return m_bodyA->GetWorldPoint(m_localAnchorA); -} - -b2Vec2 b2RevoluteJoint::GetAnchorB() const -{ - return m_bodyB->GetWorldPoint(m_localAnchorB); -} - -b2Vec2 b2RevoluteJoint::GetReactionForce(float32 inv_dt) const -{ - b2Vec2 P(m_impulse.x, m_impulse.y); - return inv_dt * P; -} - -float32 b2RevoluteJoint::GetReactionTorque(float32 inv_dt) const -{ - return inv_dt * m_impulse.z; -} - -float32 b2RevoluteJoint::GetJointAngle() const -{ - b2Body* bA = m_bodyA; - b2Body* bB = m_bodyB; - return bB->m_sweep.a - bA->m_sweep.a - m_referenceAngle; -} - -float32 b2RevoluteJoint::GetJointSpeed() const -{ - b2Body* bA = m_bodyA; - b2Body* bB = m_bodyB; - return bB->m_angularVelocity - bA->m_angularVelocity; -} - -bool b2RevoluteJoint::IsMotorEnabled() const -{ - return m_enableMotor; -} - -void b2RevoluteJoint::EnableMotor(bool flag) -{ - m_bodyA->SetAwake(true); - m_bodyB->SetAwake(true); - m_enableMotor = flag; -} - -float32 b2RevoluteJoint::GetMotorTorque(float32 inv_dt) const -{ - return inv_dt * m_motorImpulse; -} - -void b2RevoluteJoint::SetMotorSpeed(float32 speed) -{ - m_bodyA->SetAwake(true); - m_bodyB->SetAwake(true); - m_motorSpeed = speed; -} - -void b2RevoluteJoint::SetMaxMotorTorque(float32 torque) -{ - m_bodyA->SetAwake(true); - m_bodyB->SetAwake(true); - m_maxMotorTorque = torque; -} - -bool b2RevoluteJoint::IsLimitEnabled() const -{ - return m_enableLimit; -} - -void b2RevoluteJoint::EnableLimit(bool flag) -{ - if (flag != m_enableLimit) - { - m_bodyA->SetAwake(true); - m_bodyB->SetAwake(true); - m_enableLimit = flag; - m_impulse.z = 0.0f; - } -} - -float32 b2RevoluteJoint::GetLowerLimit() const -{ - return m_lowerAngle; -} - -float32 b2RevoluteJoint::GetUpperLimit() const -{ - return m_upperAngle; -} - -void b2RevoluteJoint::SetLimits(float32 lower, float32 upper) -{ - b2Assert(lower <= upper); - - if (lower != m_lowerAngle || upper != m_upperAngle) - { - m_bodyA->SetAwake(true); - m_bodyB->SetAwake(true); - m_impulse.z = 0.0f; - m_lowerAngle = lower; - m_upperAngle = upper; - } -} - -void b2RevoluteJoint::Dump() -{ - int32 indexA = m_bodyA->m_islandIndex; - int32 indexB = m_bodyB->m_islandIndex; - - b2Log(" b2RevoluteJointDef jd;\n"); - b2Log(" jd.bodyA = bodies[%d];\n", indexA); - b2Log(" jd.bodyB = bodies[%d];\n", indexB); - b2Log(" jd.collideConnected = bool(%d);\n", m_collideConnected); - b2Log(" jd.localAnchorA.Set(%.15lef, %.15lef);\n", m_localAnchorA.x, m_localAnchorA.y); - b2Log(" jd.localAnchorB.Set(%.15lef, %.15lef);\n", m_localAnchorB.x, m_localAnchorB.y); - b2Log(" jd.referenceAngle = %.15lef;\n", m_referenceAngle); - b2Log(" jd.enableLimit = bool(%d);\n", m_enableLimit); - b2Log(" jd.lowerAngle = %.15lef;\n", m_lowerAngle); - b2Log(" jd.upperAngle = %.15lef;\n", m_upperAngle); - b2Log(" jd.enableMotor = bool(%d);\n", m_enableMotor); - b2Log(" jd.motorSpeed = %.15lef;\n", m_motorSpeed); - b2Log(" jd.maxMotorTorque = %.15lef;\n", m_maxMotorTorque); - b2Log(" joints[%d] = m_world->CreateJoint(&jd);\n", m_index); -} diff --git a/libs/Box2D/Dynamics/Joints/b2RopeJoint.cpp b/libs/Box2D/Dynamics/Joints/b2RopeJoint.cpp deleted file mode 100644 index 107ce1fa..00000000 --- a/libs/Box2D/Dynamics/Joints/b2RopeJoint.cpp +++ /dev/null @@ -1,241 +0,0 @@ -/* -* Copyright (c) 2007-2011 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ - -#include -#include -#include - - -// Limit: -// C = norm(pB - pA) - L -// u = (pB - pA) / norm(pB - pA) -// Cdot = dot(u, vB + cross(wB, rB) - vA - cross(wA, rA)) -// J = [-u -cross(rA, u) u cross(rB, u)] -// K = J * invM * JT -// = invMassA + invIA * cross(rA, u)^2 + invMassB + invIB * cross(rB, u)^2 - -b2RopeJoint::b2RopeJoint(const b2RopeJointDef* def) -: b2Joint(def) -{ - m_localAnchorA = def->localAnchorA; - m_localAnchorB = def->localAnchorB; - - m_maxLength = def->maxLength; - - m_mass = 0.0f; - m_impulse = 0.0f; - m_state = e_inactiveLimit; - m_length = 0.0f; -} - -void b2RopeJoint::InitVelocityConstraints(const b2SolverData& data) -{ - m_indexA = m_bodyA->m_islandIndex; - m_indexB = m_bodyB->m_islandIndex; - m_localCenterA = m_bodyA->m_sweep.localCenter; - m_localCenterB = m_bodyB->m_sweep.localCenter; - m_invMassA = m_bodyA->m_invMass; - m_invMassB = m_bodyB->m_invMass; - m_invIA = m_bodyA->m_invI; - m_invIB = m_bodyB->m_invI; - - b2Vec2 cA = data.positions[m_indexA].c; - float32 aA = data.positions[m_indexA].a; - b2Vec2 vA = data.velocities[m_indexA].v; - float32 wA = data.velocities[m_indexA].w; - - b2Vec2 cB = data.positions[m_indexB].c; - float32 aB = data.positions[m_indexB].a; - b2Vec2 vB = data.velocities[m_indexB].v; - float32 wB = data.velocities[m_indexB].w; - - b2Rot qA(aA), qB(aB); - - m_rA = b2Mul(qA, m_localAnchorA - m_localCenterA); - m_rB = b2Mul(qB, m_localAnchorB - m_localCenterB); - m_u = cB + m_rB - cA - m_rA; - - m_length = m_u.Length(); - - float32 C = m_length - m_maxLength; - if (C > 0.0f) - { - m_state = e_atUpperLimit; - } - else - { - m_state = e_inactiveLimit; - } - - if (m_length > b2_linearSlop) - { - m_u *= 1.0f / m_length; - } - else - { - m_u.SetZero(); - m_mass = 0.0f; - m_impulse = 0.0f; - return; - } - - // Compute effective mass. - float32 crA = b2Cross(m_rA, m_u); - float32 crB = b2Cross(m_rB, m_u); - float32 invMass = m_invMassA + m_invIA * crA * crA + m_invMassB + m_invIB * crB * crB; - - m_mass = invMass != 0.0f ? 1.0f / invMass : 0.0f; - - if (data.step.warmStarting) - { - // Scale the impulse to support a variable time step. - m_impulse *= data.step.dtRatio; - - b2Vec2 P = m_impulse * m_u; - vA -= m_invMassA * P; - wA -= m_invIA * b2Cross(m_rA, P); - vB += m_invMassB * P; - wB += m_invIB * b2Cross(m_rB, P); - } - else - { - m_impulse = 0.0f; - } - - data.velocities[m_indexA].v = vA; - data.velocities[m_indexA].w = wA; - data.velocities[m_indexB].v = vB; - data.velocities[m_indexB].w = wB; -} - -void b2RopeJoint::SolveVelocityConstraints(const b2SolverData& data) -{ - b2Vec2 vA = data.velocities[m_indexA].v; - float32 wA = data.velocities[m_indexA].w; - b2Vec2 vB = data.velocities[m_indexB].v; - float32 wB = data.velocities[m_indexB].w; - - // Cdot = dot(u, v + cross(w, r)) - b2Vec2 vpA = vA + b2Cross(wA, m_rA); - b2Vec2 vpB = vB + b2Cross(wB, m_rB); - float32 C = m_length - m_maxLength; - float32 Cdot = b2Dot(m_u, vpB - vpA); - - // Predictive constraint. - if (C < 0.0f) - { - Cdot += data.step.inv_dt * C; - } - - float32 impulse = -m_mass * Cdot; - float32 oldImpulse = m_impulse; - m_impulse = b2Min(0.0f, m_impulse + impulse); - impulse = m_impulse - oldImpulse; - - b2Vec2 P = impulse * m_u; - vA -= m_invMassA * P; - wA -= m_invIA * b2Cross(m_rA, P); - vB += m_invMassB * P; - wB += m_invIB * b2Cross(m_rB, P); - - data.velocities[m_indexA].v = vA; - data.velocities[m_indexA].w = wA; - data.velocities[m_indexB].v = vB; - data.velocities[m_indexB].w = wB; -} - -bool b2RopeJoint::SolvePositionConstraints(const b2SolverData& data) -{ - b2Vec2 cA = data.positions[m_indexA].c; - float32 aA = data.positions[m_indexA].a; - b2Vec2 cB = data.positions[m_indexB].c; - float32 aB = data.positions[m_indexB].a; - - b2Rot qA(aA), qB(aB); - - b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_localCenterA); - b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_localCenterB); - b2Vec2 u = cB + rB - cA - rA; - - float32 length = u.Normalize(); - float32 C = length - m_maxLength; - - C = b2Clamp(C, 0.0f, b2_maxLinearCorrection); - - float32 impulse = -m_mass * C; - b2Vec2 P = impulse * u; - - cA -= m_invMassA * P; - aA -= m_invIA * b2Cross(rA, P); - cB += m_invMassB * P; - aB += m_invIB * b2Cross(rB, P); - - data.positions[m_indexA].c = cA; - data.positions[m_indexA].a = aA; - data.positions[m_indexB].c = cB; - data.positions[m_indexB].a = aB; - - return length - m_maxLength < b2_linearSlop; -} - -b2Vec2 b2RopeJoint::GetAnchorA() const -{ - return m_bodyA->GetWorldPoint(m_localAnchorA); -} - -b2Vec2 b2RopeJoint::GetAnchorB() const -{ - return m_bodyB->GetWorldPoint(m_localAnchorB); -} - -b2Vec2 b2RopeJoint::GetReactionForce(float32 inv_dt) const -{ - b2Vec2 F = (inv_dt * m_impulse) * m_u; - return F; -} - -float32 b2RopeJoint::GetReactionTorque(float32 inv_dt) const -{ - B2_NOT_USED(inv_dt); - return 0.0f; -} - -float32 b2RopeJoint::GetMaxLength() const -{ - return m_maxLength; -} - -b2LimitState b2RopeJoint::GetLimitState() const -{ - return m_state; -} - -void b2RopeJoint::Dump() -{ - int32 indexA = m_bodyA->m_islandIndex; - int32 indexB = m_bodyB->m_islandIndex; - - b2Log(" b2RopeJointDef jd;\n"); - b2Log(" jd.bodyA = bodies[%d];\n", indexA); - b2Log(" jd.bodyB = bodies[%d];\n", indexB); - b2Log(" jd.collideConnected = bool(%d);\n", m_collideConnected); - b2Log(" jd.localAnchorA.Set(%.15lef, %.15lef);\n", m_localAnchorA.x, m_localAnchorA.y); - b2Log(" jd.localAnchorB.Set(%.15lef, %.15lef);\n", m_localAnchorB.x, m_localAnchorB.y); - b2Log(" jd.maxLength = %.15lef;\n", m_maxLength); - b2Log(" joints[%d] = m_world->CreateJoint(&jd);\n", m_index); -} diff --git a/libs/Box2D/Dynamics/Joints/b2RopeJoint.h b/libs/Box2D/Dynamics/Joints/b2RopeJoint.h deleted file mode 100644 index 30252460..00000000 --- a/libs/Box2D/Dynamics/Joints/b2RopeJoint.h +++ /dev/null @@ -1,114 +0,0 @@ -/* -* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef B2_ROPE_JOINT_H -#define B2_ROPE_JOINT_H - -#include - -/// Rope joint definition. This requires two body anchor points and -/// a maximum lengths. -/// Note: by default the connected objects will not collide. -/// see collideConnected in b2JointDef. -struct b2RopeJointDef : public b2JointDef -{ - b2RopeJointDef() - { - type = e_ropeJoint; - localAnchorA.Set(-1.0f, 0.0f); - localAnchorB.Set(1.0f, 0.0f); - maxLength = 0.0f; - } - - /// The local anchor point relative to bodyA's origin. - b2Vec2 localAnchorA; - - /// The local anchor point relative to bodyB's origin. - b2Vec2 localAnchorB; - - /// The maximum length of the rope. - /// Warning: this must be larger than b2_linearSlop or - /// the joint will have no effect. - float32 maxLength; -}; - -/// A rope joint enforces a maximum distance between two points -/// on two bodies. It has no other effect. -/// Warning: if you attempt to change the maximum length during -/// the simulation you will get some non-physical behavior. -/// A model that would allow you to dynamically modify the length -/// would have some sponginess, so I chose not to implement it -/// that way. See b2DistanceJoint if you want to dynamically -/// control length. -class b2RopeJoint : public b2Joint -{ -public: - b2Vec2 GetAnchorA() const; - b2Vec2 GetAnchorB() const; - - b2Vec2 GetReactionForce(float32 inv_dt) const; - float32 GetReactionTorque(float32 inv_dt) const; - - /// The local anchor point relative to bodyA's origin. - const b2Vec2& GetLocalAnchorA() const { return m_localAnchorA; } - - /// The local anchor point relative to bodyB's origin. - const b2Vec2& GetLocalAnchorB() const { return m_localAnchorB; } - - /// Set/Get the maximum length of the rope. - void SetMaxLength(float32 length) { m_maxLength = length; } - float32 GetMaxLength() const; - - b2LimitState GetLimitState() const; - - /// Dump joint to dmLog - void Dump(); - -protected: - - friend class b2Joint; - b2RopeJoint(const b2RopeJointDef* data); - - void InitVelocityConstraints(const b2SolverData& data); - void SolveVelocityConstraints(const b2SolverData& data); - bool SolvePositionConstraints(const b2SolverData& data); - - // Solver shared - b2Vec2 m_localAnchorA; - b2Vec2 m_localAnchorB; - float32 m_maxLength; - float32 m_length; - float32 m_impulse; - - // Solver temp - int32 m_indexA; - int32 m_indexB; - b2Vec2 m_u; - b2Vec2 m_rA; - b2Vec2 m_rB; - b2Vec2 m_localCenterA; - b2Vec2 m_localCenterB; - float32 m_invMassA; - float32 m_invMassB; - float32 m_invIA; - float32 m_invIB; - float32 m_mass; - b2LimitState m_state; -}; - -#endif diff --git a/libs/Box2D/Dynamics/Joints/b2WeldJoint.h b/libs/Box2D/Dynamics/Joints/b2WeldJoint.h deleted file mode 100644 index 2adfc26a..00000000 --- a/libs/Box2D/Dynamics/Joints/b2WeldJoint.h +++ /dev/null @@ -1,126 +0,0 @@ -/* -* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef B2_WELD_JOINT_H -#define B2_WELD_JOINT_H - -#include - -/// Weld joint definition. You need to specify local anchor points -/// where they are attached and the relative body angle. The position -/// of the anchor points is important for computing the reaction torque. -struct b2WeldJointDef : public b2JointDef -{ - b2WeldJointDef() - { - type = e_weldJoint; - localAnchorA.Set(0.0f, 0.0f); - localAnchorB.Set(0.0f, 0.0f); - referenceAngle = 0.0f; - frequencyHz = 0.0f; - dampingRatio = 0.0f; - } - - /// Initialize the bodies, anchors, and reference angle using a world - /// anchor point. - void Initialize(b2Body* bodyA, b2Body* bodyB, const b2Vec2& anchor); - - /// The local anchor point relative to bodyA's origin. - b2Vec2 localAnchorA; - - /// The local anchor point relative to bodyB's origin. - b2Vec2 localAnchorB; - - /// The bodyB angle minus bodyA angle in the reference state (radians). - float32 referenceAngle; - - /// The mass-spring-damper frequency in Hertz. Rotation only. - /// Disable softness with a value of 0. - float32 frequencyHz; - - /// The damping ratio. 0 = no damping, 1 = critical damping. - float32 dampingRatio; -}; - -/// A weld joint essentially glues two bodies together. A weld joint may -/// distort somewhat because the island constraint solver is approximate. -class b2WeldJoint : public b2Joint -{ -public: - b2Vec2 GetAnchorA() const; - b2Vec2 GetAnchorB() const; - - b2Vec2 GetReactionForce(float32 inv_dt) const; - float32 GetReactionTorque(float32 inv_dt) const; - - /// The local anchor point relative to bodyA's origin. - const b2Vec2& GetLocalAnchorA() const { return m_localAnchorA; } - - /// The local anchor point relative to bodyB's origin. - const b2Vec2& GetLocalAnchorB() const { return m_localAnchorB; } - - /// Get the reference angle. - float32 GetReferenceAngle() const { return m_referenceAngle; } - - /// Set/get frequency in Hz. - void SetFrequency(float32 hz) { m_frequencyHz = hz; } - float32 GetFrequency() const { return m_frequencyHz; } - - /// Set/get damping ratio. - void SetDampingRatio(float32 ratio) { m_dampingRatio = ratio; } - float32 GetDampingRatio() const { return m_dampingRatio; } - - /// Dump to b2Log - void Dump(); - -protected: - - friend class b2Joint; - - b2WeldJoint(const b2WeldJointDef* def); - - void InitVelocityConstraints(const b2SolverData& data); - void SolveVelocityConstraints(const b2SolverData& data); - bool SolvePositionConstraints(const b2SolverData& data); - - float32 m_frequencyHz; - float32 m_dampingRatio; - float32 m_bias; - - // Solver shared - b2Vec2 m_localAnchorA; - b2Vec2 m_localAnchorB; - float32 m_referenceAngle; - float32 m_gamma; - b2Vec3 m_impulse; - - // Solver temp - int32 m_indexA; - int32 m_indexB; - b2Vec2 m_rA; - b2Vec2 m_rB; - b2Vec2 m_localCenterA; - b2Vec2 m_localCenterB; - float32 m_invMassA; - float32 m_invMassB; - float32 m_invIA; - float32 m_invIB; - b2Mat33 m_mass; -}; - -#endif diff --git a/libs/Box2D/Dynamics/Joints/b2WheelJoint.cpp b/libs/Box2D/Dynamics/Joints/b2WheelJoint.cpp deleted file mode 100644 index 74eda681..00000000 --- a/libs/Box2D/Dynamics/Joints/b2WheelJoint.cpp +++ /dev/null @@ -1,419 +0,0 @@ -/* -* Copyright (c) 2006-2007 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ - -#include -#include -#include - -// Linear constraint (point-to-line) -// d = pB - pA = xB + rB - xA - rA -// C = dot(ay, d) -// Cdot = dot(d, cross(wA, ay)) + dot(ay, vB + cross(wB, rB) - vA - cross(wA, rA)) -// = -dot(ay, vA) - dot(cross(d + rA, ay), wA) + dot(ay, vB) + dot(cross(rB, ay), vB) -// J = [-ay, -cross(d + rA, ay), ay, cross(rB, ay)] - -// Spring linear constraint -// C = dot(ax, d) -// Cdot = = -dot(ax, vA) - dot(cross(d + rA, ax), wA) + dot(ax, vB) + dot(cross(rB, ax), vB) -// J = [-ax -cross(d+rA, ax) ax cross(rB, ax)] - -// Motor rotational constraint -// Cdot = wB - wA -// J = [0 0 -1 0 0 1] - -void b2WheelJointDef::Initialize(b2Body* bA, b2Body* bB, const b2Vec2& anchor, const b2Vec2& axis) -{ - bodyA = bA; - bodyB = bB; - localAnchorA = bodyA->GetLocalPoint(anchor); - localAnchorB = bodyB->GetLocalPoint(anchor); - localAxisA = bodyA->GetLocalVector(axis); -} - -b2WheelJoint::b2WheelJoint(const b2WheelJointDef* def) -: b2Joint(def) -{ - m_localAnchorA = def->localAnchorA; - m_localAnchorB = def->localAnchorB; - m_localXAxisA = def->localAxisA; - m_localYAxisA = b2Cross(1.0f, m_localXAxisA); - - m_mass = 0.0f; - m_impulse = 0.0f; - m_motorMass = 0.0f; - m_motorImpulse = 0.0f; - m_springMass = 0.0f; - m_springImpulse = 0.0f; - - m_maxMotorTorque = def->maxMotorTorque; - m_motorSpeed = def->motorSpeed; - m_enableMotor = def->enableMotor; - - m_frequencyHz = def->frequencyHz; - m_dampingRatio = def->dampingRatio; - - m_bias = 0.0f; - m_gamma = 0.0f; - - m_ax.SetZero(); - m_ay.SetZero(); -} - -void b2WheelJoint::InitVelocityConstraints(const b2SolverData& data) -{ - m_indexA = m_bodyA->m_islandIndex; - m_indexB = m_bodyB->m_islandIndex; - m_localCenterA = m_bodyA->m_sweep.localCenter; - m_localCenterB = m_bodyB->m_sweep.localCenter; - m_invMassA = m_bodyA->m_invMass; - m_invMassB = m_bodyB->m_invMass; - m_invIA = m_bodyA->m_invI; - m_invIB = m_bodyB->m_invI; - - float32 mA = m_invMassA, mB = m_invMassB; - float32 iA = m_invIA, iB = m_invIB; - - b2Vec2 cA = data.positions[m_indexA].c; - float32 aA = data.positions[m_indexA].a; - b2Vec2 vA = data.velocities[m_indexA].v; - float32 wA = data.velocities[m_indexA].w; - - b2Vec2 cB = data.positions[m_indexB].c; - float32 aB = data.positions[m_indexB].a; - b2Vec2 vB = data.velocities[m_indexB].v; - float32 wB = data.velocities[m_indexB].w; - - b2Rot qA(aA), qB(aB); - - // Compute the effective masses. - b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_localCenterA); - b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_localCenterB); - b2Vec2 d = cB + rB - cA - rA; - - // Point to line constraint - { - m_ay = b2Mul(qA, m_localYAxisA); - m_sAy = b2Cross(d + rA, m_ay); - m_sBy = b2Cross(rB, m_ay); - - m_mass = mA + mB + iA * m_sAy * m_sAy + iB * m_sBy * m_sBy; - - if (m_mass > 0.0f) - { - m_mass = 1.0f / m_mass; - } - } - - // Spring constraint - m_springMass = 0.0f; - m_bias = 0.0f; - m_gamma = 0.0f; - if (m_frequencyHz > 0.0f) - { - m_ax = b2Mul(qA, m_localXAxisA); - m_sAx = b2Cross(d + rA, m_ax); - m_sBx = b2Cross(rB, m_ax); - - float32 invMass = mA + mB + iA * m_sAx * m_sAx + iB * m_sBx * m_sBx; - - if (invMass > 0.0f) - { - m_springMass = 1.0f / invMass; - - float32 C = b2Dot(d, m_ax); - - // Frequency - float32 omega = 2.0f * b2_pi * m_frequencyHz; - - // Damping coefficient - float32 d = 2.0f * m_springMass * m_dampingRatio * omega; - - // Spring stiffness - float32 k = m_springMass * omega * omega; - - // magic formulas - float32 h = data.step.dt; - m_gamma = h * (d + h * k); - if (m_gamma > 0.0f) - { - m_gamma = 1.0f / m_gamma; - } - - m_bias = C * h * k * m_gamma; - - m_springMass = invMass + m_gamma; - if (m_springMass > 0.0f) - { - m_springMass = 1.0f / m_springMass; - } - } - } - else - { - m_springImpulse = 0.0f; - } - - // Rotational motor - if (m_enableMotor) - { - m_motorMass = iA + iB; - if (m_motorMass > 0.0f) - { - m_motorMass = 1.0f / m_motorMass; - } - } - else - { - m_motorMass = 0.0f; - m_motorImpulse = 0.0f; - } - - if (data.step.warmStarting) - { - // Account for variable time step. - m_impulse *= data.step.dtRatio; - m_springImpulse *= data.step.dtRatio; - m_motorImpulse *= data.step.dtRatio; - - b2Vec2 P = m_impulse * m_ay + m_springImpulse * m_ax; - float32 LA = m_impulse * m_sAy + m_springImpulse * m_sAx + m_motorImpulse; - float32 LB = m_impulse * m_sBy + m_springImpulse * m_sBx + m_motorImpulse; - - vA -= m_invMassA * P; - wA -= m_invIA * LA; - - vB += m_invMassB * P; - wB += m_invIB * LB; - } - else - { - m_impulse = 0.0f; - m_springImpulse = 0.0f; - m_motorImpulse = 0.0f; - } - - data.velocities[m_indexA].v = vA; - data.velocities[m_indexA].w = wA; - data.velocities[m_indexB].v = vB; - data.velocities[m_indexB].w = wB; -} - -void b2WheelJoint::SolveVelocityConstraints(const b2SolverData& data) -{ - float32 mA = m_invMassA, mB = m_invMassB; - float32 iA = m_invIA, iB = m_invIB; - - b2Vec2 vA = data.velocities[m_indexA].v; - float32 wA = data.velocities[m_indexA].w; - b2Vec2 vB = data.velocities[m_indexB].v; - float32 wB = data.velocities[m_indexB].w; - - // Solve spring constraint - { - float32 Cdot = b2Dot(m_ax, vB - vA) + m_sBx * wB - m_sAx * wA; - float32 impulse = -m_springMass * (Cdot + m_bias + m_gamma * m_springImpulse); - m_springImpulse += impulse; - - b2Vec2 P = impulse * m_ax; - float32 LA = impulse * m_sAx; - float32 LB = impulse * m_sBx; - - vA -= mA * P; - wA -= iA * LA; - - vB += mB * P; - wB += iB * LB; - } - - // Solve rotational motor constraint - { - float32 Cdot = wB - wA - m_motorSpeed; - float32 impulse = -m_motorMass * Cdot; - - float32 oldImpulse = m_motorImpulse; - float32 maxImpulse = data.step.dt * m_maxMotorTorque; - m_motorImpulse = b2Clamp(m_motorImpulse + impulse, -maxImpulse, maxImpulse); - impulse = m_motorImpulse - oldImpulse; - - wA -= iA * impulse; - wB += iB * impulse; - } - - // Solve point to line constraint - { - float32 Cdot = b2Dot(m_ay, vB - vA) + m_sBy * wB - m_sAy * wA; - float32 impulse = -m_mass * Cdot; - m_impulse += impulse; - - b2Vec2 P = impulse * m_ay; - float32 LA = impulse * m_sAy; - float32 LB = impulse * m_sBy; - - vA -= mA * P; - wA -= iA * LA; - - vB += mB * P; - wB += iB * LB; - } - - data.velocities[m_indexA].v = vA; - data.velocities[m_indexA].w = wA; - data.velocities[m_indexB].v = vB; - data.velocities[m_indexB].w = wB; -} - -bool b2WheelJoint::SolvePositionConstraints(const b2SolverData& data) -{ - b2Vec2 cA = data.positions[m_indexA].c; - float32 aA = data.positions[m_indexA].a; - b2Vec2 cB = data.positions[m_indexB].c; - float32 aB = data.positions[m_indexB].a; - - b2Rot qA(aA), qB(aB); - - b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_localCenterA); - b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_localCenterB); - b2Vec2 d = (cB - cA) + rB - rA; - - b2Vec2 ay = b2Mul(qA, m_localYAxisA); - - float32 sAy = b2Cross(d + rA, ay); - float32 sBy = b2Cross(rB, ay); - - float32 C = b2Dot(d, ay); - - float32 k = m_invMassA + m_invMassB + m_invIA * m_sAy * m_sAy + m_invIB * m_sBy * m_sBy; - - float32 impulse; - if (k != 0.0f) - { - impulse = - C / k; - } - else - { - impulse = 0.0f; - } - - b2Vec2 P = impulse * ay; - float32 LA = impulse * sAy; - float32 LB = impulse * sBy; - - cA -= m_invMassA * P; - aA -= m_invIA * LA; - cB += m_invMassB * P; - aB += m_invIB * LB; - - data.positions[m_indexA].c = cA; - data.positions[m_indexA].a = aA; - data.positions[m_indexB].c = cB; - data.positions[m_indexB].a = aB; - - return b2Abs(C) <= b2_linearSlop; -} - -b2Vec2 b2WheelJoint::GetAnchorA() const -{ - return m_bodyA->GetWorldPoint(m_localAnchorA); -} - -b2Vec2 b2WheelJoint::GetAnchorB() const -{ - return m_bodyB->GetWorldPoint(m_localAnchorB); -} - -b2Vec2 b2WheelJoint::GetReactionForce(float32 inv_dt) const -{ - return inv_dt * (m_impulse * m_ay + m_springImpulse * m_ax); -} - -float32 b2WheelJoint::GetReactionTorque(float32 inv_dt) const -{ - return inv_dt * m_motorImpulse; -} - -float32 b2WheelJoint::GetJointTranslation() const -{ - b2Body* bA = m_bodyA; - b2Body* bB = m_bodyB; - - b2Vec2 pA = bA->GetWorldPoint(m_localAnchorA); - b2Vec2 pB = bB->GetWorldPoint(m_localAnchorB); - b2Vec2 d = pB - pA; - b2Vec2 axis = bA->GetWorldVector(m_localXAxisA); - - float32 translation = b2Dot(d, axis); - return translation; -} - -float32 b2WheelJoint::GetJointSpeed() const -{ - float32 wA = m_bodyA->m_angularVelocity; - float32 wB = m_bodyB->m_angularVelocity; - return wB - wA; -} - -bool b2WheelJoint::IsMotorEnabled() const -{ - return m_enableMotor; -} - -void b2WheelJoint::EnableMotor(bool flag) -{ - m_bodyA->SetAwake(true); - m_bodyB->SetAwake(true); - m_enableMotor = flag; -} - -void b2WheelJoint::SetMotorSpeed(float32 speed) -{ - m_bodyA->SetAwake(true); - m_bodyB->SetAwake(true); - m_motorSpeed = speed; -} - -void b2WheelJoint::SetMaxMotorTorque(float32 torque) -{ - m_bodyA->SetAwake(true); - m_bodyB->SetAwake(true); - m_maxMotorTorque = torque; -} - -float32 b2WheelJoint::GetMotorTorque(float32 inv_dt) const -{ - return inv_dt * m_motorImpulse; -} - -void b2WheelJoint::Dump() -{ - int32 indexA = m_bodyA->m_islandIndex; - int32 indexB = m_bodyB->m_islandIndex; - - b2Log(" b2WheelJointDef jd;\n"); - b2Log(" jd.bodyA = bodies[%d];\n", indexA); - b2Log(" jd.bodyB = bodies[%d];\n", indexB); - b2Log(" jd.collideConnected = bool(%d);\n", m_collideConnected); - b2Log(" jd.localAnchorA.Set(%.15lef, %.15lef);\n", m_localAnchorA.x, m_localAnchorA.y); - b2Log(" jd.localAnchorB.Set(%.15lef, %.15lef);\n", m_localAnchorB.x, m_localAnchorB.y); - b2Log(" jd.localAxisA.Set(%.15lef, %.15lef);\n", m_localXAxisA.x, m_localXAxisA.y); - b2Log(" jd.enableMotor = bool(%d);\n", m_enableMotor); - b2Log(" jd.motorSpeed = %.15lef;\n", m_motorSpeed); - b2Log(" jd.maxMotorTorque = %.15lef;\n", m_maxMotorTorque); - b2Log(" jd.frequencyHz = %.15lef;\n", m_frequencyHz); - b2Log(" jd.dampingRatio = %.15lef;\n", m_dampingRatio); - b2Log(" joints[%d] = m_world->CreateJoint(&jd);\n", m_index); -} diff --git a/libs/Box2D/Dynamics/Joints/b2WheelJoint.h b/libs/Box2D/Dynamics/Joints/b2WheelJoint.h deleted file mode 100644 index 6e8b6454..00000000 --- a/libs/Box2D/Dynamics/Joints/b2WheelJoint.h +++ /dev/null @@ -1,211 +0,0 @@ -/* -* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef B2_WHEEL_JOINT_H -#define B2_WHEEL_JOINT_H - -#include - -/// Wheel joint definition. This requires defining a line of -/// motion using an axis and an anchor point. The definition uses local -/// anchor points and a local axis so that the initial configuration -/// can violate the constraint slightly. The joint translation is zero -/// when the local anchor points coincide in world space. Using local -/// anchors and a local axis helps when saving and loading a game. -struct b2WheelJointDef : public b2JointDef -{ - b2WheelJointDef() - { - type = e_wheelJoint; - localAnchorA.SetZero(); - localAnchorB.SetZero(); - localAxisA.Set(1.0f, 0.0f); - enableMotor = false; - maxMotorTorque = 0.0f; - motorSpeed = 0.0f; - frequencyHz = 2.0f; - dampingRatio = 0.7f; - } - - /// Initialize the bodies, anchors, axis, and reference angle using the world - /// anchor and world axis. - void Initialize(b2Body* bodyA, b2Body* bodyB, const b2Vec2& anchor, const b2Vec2& axis); - - /// The local anchor point relative to bodyA's origin. - b2Vec2 localAnchorA; - - /// The local anchor point relative to bodyB's origin. - b2Vec2 localAnchorB; - - /// The local translation axis in bodyA. - b2Vec2 localAxisA; - - /// Enable/disable the joint motor. - bool enableMotor; - - /// The maximum motor torque, usually in N-m. - float32 maxMotorTorque; - - /// The desired motor speed in radians per second. - float32 motorSpeed; - - /// Suspension frequency, zero indicates no suspension - float32 frequencyHz; - - /// Suspension damping ratio, one indicates critical damping - float32 dampingRatio; -}; - -/// A wheel joint. This joint provides two degrees of freedom: translation -/// along an axis fixed in bodyA and rotation in the plane. You can use a -/// joint limit to restrict the range of motion and a joint motor to drive -/// the rotation or to model rotational friction. -/// This joint is designed for vehicle suspensions. -class b2WheelJoint : public b2Joint -{ -public: - b2Vec2 GetAnchorA() const; - b2Vec2 GetAnchorB() const; - - b2Vec2 GetReactionForce(float32 inv_dt) const; - float32 GetReactionTorque(float32 inv_dt) const; - - /// The local anchor point relative to bodyA's origin. - const b2Vec2& GetLocalAnchorA() const { return m_localAnchorA; } - - /// The local anchor point relative to bodyB's origin. - const b2Vec2& GetLocalAnchorB() const { return m_localAnchorB; } - - /// The local joint axis relative to bodyA. - const b2Vec2& GetLocalAxisA() const { return m_localXAxisA; } - - /// Get the current joint translation, usually in meters. - float32 GetJointTranslation() const; - - /// Get the current joint translation speed, usually in meters per second. - float32 GetJointSpeed() const; - - /// Is the joint motor enabled? - bool IsMotorEnabled() const; - - /// Enable/disable the joint motor. - void EnableMotor(bool flag); - - /// Set the motor speed, usually in radians per second. - void SetMotorSpeed(float32 speed); - - /// Get the motor speed, usually in radians per second. - float32 GetMotorSpeed() const; - - /// Set/Get the maximum motor force, usually in N-m. - void SetMaxMotorTorque(float32 torque); - float32 GetMaxMotorTorque() const; - - /// Get the current motor torque given the inverse time step, usually in N-m. - float32 GetMotorTorque(float32 inv_dt) const; - - /// Set/Get the spring frequency in hertz. Setting the frequency to zero disables the spring. - void SetSpringFrequencyHz(float32 hz); - float32 GetSpringFrequencyHz() const; - - /// Set/Get the spring damping ratio - void SetSpringDampingRatio(float32 ratio); - float32 GetSpringDampingRatio() const; - - /// Dump to b2Log - void Dump(); - -protected: - - friend class b2Joint; - b2WheelJoint(const b2WheelJointDef* def); - - void InitVelocityConstraints(const b2SolverData& data); - void SolveVelocityConstraints(const b2SolverData& data); - bool SolvePositionConstraints(const b2SolverData& data); - - float32 m_frequencyHz; - float32 m_dampingRatio; - - // Solver shared - b2Vec2 m_localAnchorA; - b2Vec2 m_localAnchorB; - b2Vec2 m_localXAxisA; - b2Vec2 m_localYAxisA; - - float32 m_impulse; - float32 m_motorImpulse; - float32 m_springImpulse; - - float32 m_maxMotorTorque; - float32 m_motorSpeed; - bool m_enableMotor; - - // Solver temp - int32 m_indexA; - int32 m_indexB; - b2Vec2 m_localCenterA; - b2Vec2 m_localCenterB; - float32 m_invMassA; - float32 m_invMassB; - float32 m_invIA; - float32 m_invIB; - - b2Vec2 m_ax, m_ay; - float32 m_sAx, m_sBx; - float32 m_sAy, m_sBy; - - float32 m_mass; - float32 m_motorMass; - float32 m_springMass; - - float32 m_bias; - float32 m_gamma; -}; - -inline float32 b2WheelJoint::GetMotorSpeed() const -{ - return m_motorSpeed; -} - -inline float32 b2WheelJoint::GetMaxMotorTorque() const -{ - return m_maxMotorTorque; -} - -inline void b2WheelJoint::SetSpringFrequencyHz(float32 hz) -{ - m_frequencyHz = hz; -} - -inline float32 b2WheelJoint::GetSpringFrequencyHz() const -{ - return m_frequencyHz; -} - -inline void b2WheelJoint::SetSpringDampingRatio(float32 ratio) -{ - m_dampingRatio = ratio; -} - -inline float32 b2WheelJoint::GetSpringDampingRatio() const -{ - return m_dampingRatio; -} - -#endif diff --git a/libs/Box2D/Dynamics/b2ContactManager.h b/libs/Box2D/Dynamics/b2ContactManager.h deleted file mode 100644 index dc1f77fe..00000000 --- a/libs/Box2D/Dynamics/b2ContactManager.h +++ /dev/null @@ -1,52 +0,0 @@ -/* -* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef B2_CONTACT_MANAGER_H -#define B2_CONTACT_MANAGER_H - -#include - -class b2Contact; -class b2ContactFilter; -class b2ContactListener; -class b2BlockAllocator; - -// Delegate of b2World. -class b2ContactManager -{ -public: - b2ContactManager(); - - // Broad-phase callback. - void AddPair(void* proxyUserDataA, void* proxyUserDataB); - - void FindNewContacts(); - - void Destroy(b2Contact* c); - - void Collide(); - - b2BroadPhase m_broadPhase; - b2Contact* m_contactList; - int32 m_contactCount; - b2ContactFilter* m_contactFilter; - b2ContactListener* m_contactListener; - b2BlockAllocator* m_allocator; -}; - -#endif diff --git a/libs/Box2D/Dynamics/b2TimeStep.h b/libs/Box2D/Dynamics/b2TimeStep.h deleted file mode 100644 index abe6fb6e..00000000 --- a/libs/Box2D/Dynamics/b2TimeStep.h +++ /dev/null @@ -1,70 +0,0 @@ -/* -* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef B2_TIME_STEP_H -#define B2_TIME_STEP_H - -#include - -/// Profiling data. Times are in milliseconds. -struct b2Profile -{ - float32 step; - float32 collide; - float32 solve; - float32 solveInit; - float32 solveVelocity; - float32 solvePosition; - float32 broadphase; - float32 solveTOI; -}; - -/// This is an internal structure. -struct b2TimeStep -{ - float32 dt; // time step - float32 inv_dt; // inverse time step (0 if dt == 0). - float32 dtRatio; // dt * inv_dt0 - int32 velocityIterations; - int32 positionIterations; - bool warmStarting; -}; - -/// This is an internal structure. -struct b2Position -{ - b2Vec2 c; - float32 a; -}; - -/// This is an internal structure. -struct b2Velocity -{ - b2Vec2 v; - float32 w; -}; - -/// Solver Data -struct b2SolverData -{ - b2TimeStep step; - b2Position* positions; - b2Velocity* velocities; -}; - -#endif diff --git a/libs/Box2D/Dynamics/b2WorldCallbacks.cpp b/libs/Box2D/Dynamics/b2WorldCallbacks.cpp deleted file mode 100644 index 82b28cc0..00000000 --- a/libs/Box2D/Dynamics/b2WorldCallbacks.cpp +++ /dev/null @@ -1,36 +0,0 @@ -/* -* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ - -#include -#include - -// Return true if contact calculations should be performed between these two shapes. -// If you implement your own collision filter you may want to build from this implementation. -bool b2ContactFilter::ShouldCollide(b2Fixture* fixtureA, b2Fixture* fixtureB) -{ - const b2Filter& filterA = fixtureA->GetFilterData(); - const b2Filter& filterB = fixtureB->GetFilterData(); - - if (filterA.groupIndex == filterB.groupIndex && filterA.groupIndex != 0) - { - return filterA.groupIndex > 0; - } - - bool collide = (filterA.maskBits & filterB.categoryBits) != 0 && (filterA.categoryBits & filterB.maskBits) != 0; - return collide; -} diff --git a/libs/Box2D/License.txt b/libs/Box2D/License.txt deleted file mode 100644 index e99b7036..00000000 --- a/libs/Box2D/License.txt +++ /dev/null @@ -1,18 +0,0 @@ -Copyright (c) 2006-2013 Erin Catto http://www.gphysics.com - -This software is provided 'as-is', without any express or implied -warranty. In no event will the authors be held liable for any damages -arising from the use of this software. - -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it -freely, subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not -claim that you wrote the original software. If you use this software -in a product, an acknowledgment in the product documentation would be -appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be -misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. - diff --git a/libs/Box2D/Rope/b2Rope.cpp b/libs/Box2D/Rope/b2Rope.cpp deleted file mode 100644 index 97578b26..00000000 --- a/libs/Box2D/Rope/b2Rope.cpp +++ /dev/null @@ -1,259 +0,0 @@ -/* -* Copyright (c) 2011 Erin Catto http://box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ - -#include -#include - -b2Rope::b2Rope() -{ - m_count = 0; - m_ps = NULL; - m_p0s = NULL; - m_vs = NULL; - m_ims = NULL; - m_Ls = NULL; - m_as = NULL; - m_gravity.SetZero(); - m_k2 = 1.0f; - m_k3 = 0.1f; -} - -b2Rope::~b2Rope() -{ - b2Free(m_ps); - b2Free(m_p0s); - b2Free(m_vs); - b2Free(m_ims); - b2Free(m_Ls); - b2Free(m_as); -} - -void b2Rope::Initialize(const b2RopeDef* def) -{ - b2Assert(def->count >= 3); - m_count = def->count; - m_ps = (b2Vec2*)b2Alloc(m_count * sizeof(b2Vec2)); - m_p0s = (b2Vec2*)b2Alloc(m_count * sizeof(b2Vec2)); - m_vs = (b2Vec2*)b2Alloc(m_count * sizeof(b2Vec2)); - m_ims = (float32*)b2Alloc(m_count * sizeof(float32)); - - for (int32 i = 0; i < m_count; ++i) - { - m_ps[i] = def->vertices[i]; - m_p0s[i] = def->vertices[i]; - m_vs[i].SetZero(); - - float32 m = def->masses[i]; - if (m > 0.0f) - { - m_ims[i] = 1.0f / m; - } - else - { - m_ims[i] = 0.0f; - } - } - - int32 count2 = m_count - 1; - int32 count3 = m_count - 2; - m_Ls = (float32*)b2Alloc(count2 * sizeof(float32)); - m_as = (float32*)b2Alloc(count3 * sizeof(float32)); - - for (int32 i = 0; i < count2; ++i) - { - b2Vec2 p1 = m_ps[i]; - b2Vec2 p2 = m_ps[i+1]; - m_Ls[i] = b2Distance(p1, p2); - } - - for (int32 i = 0; i < count3; ++i) - { - b2Vec2 p1 = m_ps[i]; - b2Vec2 p2 = m_ps[i + 1]; - b2Vec2 p3 = m_ps[i + 2]; - - b2Vec2 d1 = p2 - p1; - b2Vec2 d2 = p3 - p2; - - float32 a = b2Cross(d1, d2); - float32 b = b2Dot(d1, d2); - - m_as[i] = b2Atan2(a, b); - } - - m_gravity = def->gravity; - m_damping = def->damping; - m_k2 = def->k2; - m_k3 = def->k3; -} - -void b2Rope::Step(float32 h, int32 iterations) -{ - if (h == 0.0) - { - return; - } - - float32 d = expf(- h * m_damping); - - for (int32 i = 0; i < m_count; ++i) - { - m_p0s[i] = m_ps[i]; - if (m_ims[i] > 0.0f) - { - m_vs[i] += h * m_gravity; - } - m_vs[i] *= d; - m_ps[i] += h * m_vs[i]; - - } - - for (int32 i = 0; i < iterations; ++i) - { - SolveC2(); - SolveC3(); - SolveC2(); - } - - float32 inv_h = 1.0f / h; - for (int32 i = 0; i < m_count; ++i) - { - m_vs[i] = inv_h * (m_ps[i] - m_p0s[i]); - } -} - -void b2Rope::SolveC2() -{ - int32 count2 = m_count - 1; - - for (int32 i = 0; i < count2; ++i) - { - b2Vec2 p1 = m_ps[i]; - b2Vec2 p2 = m_ps[i + 1]; - - b2Vec2 d = p2 - p1; - float32 L = d.Normalize(); - - float32 im1 = m_ims[i]; - float32 im2 = m_ims[i + 1]; - - if (im1 + im2 == 0.0f) - { - continue; - } - - float32 s1 = im1 / (im1 + im2); - float32 s2 = im2 / (im1 + im2); - - p1 -= m_k2 * s1 * (m_Ls[i] - L) * d; - p2 += m_k2 * s2 * (m_Ls[i] - L) * d; - - m_ps[i] = p1; - m_ps[i + 1] = p2; - } -} - -void b2Rope::SetAngle(float32 angle) -{ - int32 count3 = m_count - 2; - for (int32 i = 0; i < count3; ++i) - { - m_as[i] = angle; - } -} - -void b2Rope::SolveC3() -{ - int32 count3 = m_count - 2; - - for (int32 i = 0; i < count3; ++i) - { - b2Vec2 p1 = m_ps[i]; - b2Vec2 p2 = m_ps[i + 1]; - b2Vec2 p3 = m_ps[i + 2]; - - float32 m1 = m_ims[i]; - float32 m2 = m_ims[i + 1]; - float32 m3 = m_ims[i + 2]; - - b2Vec2 d1 = p2 - p1; - b2Vec2 d2 = p3 - p2; - - float32 L1sqr = d1.LengthSquared(); - float32 L2sqr = d2.LengthSquared(); - - if (L1sqr * L2sqr == 0.0f) - { - continue; - } - - float32 a = b2Cross(d1, d2); - float32 b = b2Dot(d1, d2); - - float32 angle = b2Atan2(a, b); - - b2Vec2 Jd1 = (-1.0f / L1sqr) * d1.Skew(); - b2Vec2 Jd2 = (1.0f / L2sqr) * d2.Skew(); - - b2Vec2 J1 = -Jd1; - b2Vec2 J2 = Jd1 - Jd2; - b2Vec2 J3 = Jd2; - - float32 mass = m1 * b2Dot(J1, J1) + m2 * b2Dot(J2, J2) + m3 * b2Dot(J3, J3); - if (mass == 0.0f) - { - continue; - } - - mass = 1.0f / mass; - - float32 C = angle - m_as[i]; - - while (C > b2_pi) - { - angle -= 2 * b2_pi; - C = angle - m_as[i]; - } - - while (C < -b2_pi) - { - angle += 2.0f * b2_pi; - C = angle - m_as[i]; - } - - float32 impulse = - m_k3 * mass * C; - - p1 += (m1 * impulse) * J1; - p2 += (m2 * impulse) * J2; - p3 += (m3 * impulse) * J3; - - m_ps[i] = p1; - m_ps[i + 1] = p2; - m_ps[i + 2] = p3; - } -} - -void b2Rope::Draw(b2Draw* draw) const -{ - b2Color c(0.4f, 0.5f, 0.7f); - - for (int32 i = 0; i < m_count - 1; ++i) - { - draw->DrawSegment(m_ps[i], m_ps[i+1], c); - } -} diff --git a/libs/Box2D/Rope/b2Rope.h b/libs/Box2D/Rope/b2Rope.h deleted file mode 100644 index bc5375dd..00000000 --- a/libs/Box2D/Rope/b2Rope.h +++ /dev/null @@ -1,115 +0,0 @@ -/* -* Copyright (c) 2011 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef B2_ROPE_H -#define B2_ROPE_H - -#include - -class b2Draw; - -/// -struct b2RopeDef -{ - b2RopeDef() - { - vertices = NULL; - count = 0; - masses = NULL; - gravity.SetZero(); - damping = 0.1f; - k2 = 0.9f; - k3 = 0.1f; - } - - /// - b2Vec2* vertices; - - /// - int32 count; - - /// - float32* masses; - - /// - b2Vec2 gravity; - - /// - float32 damping; - - /// Stretching stiffness - float32 k2; - - /// Bending stiffness. Values above 0.5 can make the simulation blow up. - float32 k3; -}; - -/// -class b2Rope -{ -public: - b2Rope(); - ~b2Rope(); - - /// - void Initialize(const b2RopeDef* def); - - /// - void Step(float32 timeStep, int32 iterations); - - /// - int32 GetVertexCount() const - { - return m_count; - } - - /// - const b2Vec2* GetVertices() const - { - return m_ps; - } - - /// - void Draw(b2Draw* draw) const; - - /// - void SetAngle(float32 angle); - -private: - - void SolveC2(); - void SolveC3(); - - int32 m_count; - b2Vec2* m_ps; - b2Vec2* m_p0s; - b2Vec2* m_vs; - - float32* m_ims; - - float32* m_Ls; - float32* m_as; - - b2Vec2 m_gravity; - float32 m_damping; - - float32 m_k2; - float32 m_k3; -}; - -#endif diff --git a/libs/box2d/CMakeLists.txt b/libs/box2d/CMakeLists.txt new file mode 100644 index 00000000..bae60019 --- /dev/null +++ b/libs/box2d/CMakeLists.txt @@ -0,0 +1,77 @@ +cmake_minimum_required(VERSION 3.7) +project(Box2D LANGUAGES CXX) + +#---------------------------------File lists----------------------------------- +set(source_files + src/collision/b2_broad_phase.cpp + src/collision/b2_chain_shape.cpp + src/collision/b2_circle_shape.cpp + src/collision/b2_collide_circle.cpp + src/collision/b2_collide_edge.cpp + src/collision/b2_collide_polygon.cpp + src/collision/b2_collision.cpp + src/collision/b2_distance.cpp + src/collision/b2_dynamic_tree.cpp + src/collision/b2_edge_shape.cpp + src/collision/b2_polygon_shape.cpp + src/collision/b2_time_of_impact.cpp + + src/common/b2_block_allocator.cpp + src/common/b2_draw.cpp + src/common/b2_math.cpp + src/common/b2_settings.cpp + src/common/b2_stack_allocator.cpp + src/common/b2_timer.cpp + + src/dynamics/b2_body.cpp + src/dynamics/b2_contact_manager.cpp + src/dynamics/b2_fixture.cpp + src/dynamics/b2_island.cpp + src/dynamics/b2_world.cpp + src/dynamics/b2_world_callbacks.cpp + src/dynamics/b2_chain_circle_contact.cpp + src/dynamics/b2_chain_polygon_contact.cpp + src/dynamics/b2_circle_contact.cpp + src/dynamics/b2_contact.cpp + src/dynamics/b2_contact_solver.cpp + src/dynamics/b2_edge_circle_contact.cpp + src/dynamics/b2_edge_polygon_contact.cpp + src/dynamics/b2_polygon_circle_contact.cpp + src/dynamics/b2_polygon_contact.cpp + src/dynamics/b2_distance_joint.cpp + src/dynamics/b2_friction_joint.cpp + src/dynamics/b2_gear_joint.cpp + src/dynamics/b2_joint.cpp + src/dynamics/b2_motor_joint.cpp + src/dynamics/b2_mouse_joint.cpp + src/dynamics/b2_prismatic_joint.cpp + src/dynamics/b2_pulley_joint.cpp + src/dynamics/b2_revolute_joint.cpp + src/dynamics/b2_weld_joint.cpp + src/dynamics/b2_wheel_joint.cpp + + src/rope/b2_rope.cpp +) + +#----------------------------------Compiling----------------------------------- +add_library(box2d STATIC ${source_files}) + +target_include_directories(box2d + PUBLIC + $) + +#--------------------------------Installation---------------------------------- +install( + TARGETS box2d + EXPORT box2d + DESTINATION lib +) +install( + DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} + DESTINATION include + FILES_MATCHING PATTERN "*.h" +) +install( + EXPORT box2d + DESTINATION share/box2d +) diff --git a/libs/box2d/b2_api.h b/libs/box2d/b2_api.h new file mode 100644 index 00000000..6730203c --- /dev/null +++ b/libs/box2d/b2_api.h @@ -0,0 +1,52 @@ +// MIT License + +// Copyright (c) 2019 Erin Catto + +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: + +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. + +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. + +#ifndef B2_API_H +#define B2_API_H + +#ifdef B2_SHARED + #if defined _WIN32 || defined __CYGWIN__ + #ifdef box2d_EXPORTS + #ifdef __GNUC__ + #define B2_API __attribute__ ((dllexport)) + #else + #define B2_API __declspec(dllexport) + #endif + #else + #ifdef __GNUC__ + #define B2_API __attribute__ ((dllimport)) + #else + #define B2_API __declspec(dllimport) + #endif + #endif + #else + #if __GNUC__ >= 4 + #define B2_API __attribute__ ((visibility ("default"))) + #else + #define B2_API + #endif + #endif +#else + #define B2_API +#endif + +#endif diff --git a/libs/box2d/b2_block_allocator.h b/libs/box2d/b2_block_allocator.h new file mode 100644 index 00000000..95c12de8 --- /dev/null +++ b/libs/box2d/b2_block_allocator.h @@ -0,0 +1,60 @@ +// MIT License + +// Copyright (c) 2019 Erin Catto + +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: + +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. + +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. + +#ifndef B2_BLOCK_ALLOCATOR_H +#define B2_BLOCK_ALLOCATOR_H + +#include "b2_api.h" +#include "b2_settings.h" + +const int32 b2_blockSizeCount = 14; + +struct b2Block; +struct b2Chunk; + +/// This is a small object allocator used for allocating small +/// objects that persist for more than one time step. +/// See: http://www.codeproject.com/useritems/Small_Block_Allocator.asp +class B2_API b2BlockAllocator +{ +public: + b2BlockAllocator(); + ~b2BlockAllocator(); + + /// Allocate memory. This will use b2Alloc if the size is larger than b2_maxBlockSize. + void* Allocate(int32 size); + + /// Free memory. This will use b2Free if the size is larger than b2_maxBlockSize. + void Free(void* p, int32 size); + + void Clear(); + +private: + + b2Chunk* m_chunks; + int32 m_chunkCount; + int32 m_chunkSpace; + + b2Block* m_freeLists[b2_blockSizeCount]; +}; + +#endif diff --git a/libs/Box2D/Dynamics/b2Body.h b/libs/box2d/b2_body.h similarity index 77% rename from libs/Box2D/Dynamics/b2Body.h rename to libs/box2d/b2_body.h index 5c4d96a5..16b2bb00 100644 --- a/libs/Box2D/Dynamics/b2Body.h +++ b/libs/box2d/b2_body.h @@ -1,860 +1,885 @@ -/* -* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef B2_BODY_H -#define B2_BODY_H - -#include -#include -#include - -class b2Fixture; -class b2Joint; -class b2Contact; -class b2Controller; -class b2World; -struct b2FixtureDef; -struct b2JointEdge; -struct b2ContactEdge; - -/// The body type. -/// static: zero mass, zero velocity, may be manually moved -/// kinematic: zero mass, non-zero velocity set by user, moved by solver -/// dynamic: positive mass, non-zero velocity determined by forces, moved by solver -enum b2BodyType -{ - b2_staticBody = 0, - b2_kinematicBody, - b2_dynamicBody - - // TODO_ERIN - //b2_bulletBody, -}; - -/// A body definition holds all the data needed to construct a rigid body. -/// You can safely re-use body definitions. Shapes are added to a body after construction. -struct b2BodyDef -{ - /// This constructor sets the body definition default values. - b2BodyDef() - { - userData = NULL; - position.Set(0.0f, 0.0f); - angle = 0.0f; - linearVelocity.Set(0.0f, 0.0f); - angularVelocity = 0.0f; - linearDamping = 0.0f; - angularDamping = 0.0f; - allowSleep = true; - awake = true; - fixedRotation = false; - bullet = false; - type = b2_staticBody; - active = true; - gravityScale = 1.0f; - } - - /// The body type: static, kinematic, or dynamic. - /// Note: if a dynamic body would have zero mass, the mass is set to one. - b2BodyType type; - - /// The world position of the body. Avoid creating bodies at the origin - /// since this can lead to many overlapping shapes. - b2Vec2 position; - - /// The world angle of the body in radians. - float32 angle; - - /// The linear velocity of the body's origin in world co-ordinates. - b2Vec2 linearVelocity; - - /// The angular velocity of the body. - float32 angularVelocity; - - /// Linear damping is use to reduce the linear velocity. The damping parameter - /// can be larger than 1.0f but the damping effect becomes sensitive to the - /// time step when the damping parameter is large. - float32 linearDamping; - - /// Angular damping is use to reduce the angular velocity. The damping parameter - /// can be larger than 1.0f but the damping effect becomes sensitive to the - /// time step when the damping parameter is large. - float32 angularDamping; - - /// Set this flag to false if this body should never fall asleep. Note that - /// this increases CPU usage. - bool allowSleep; - - /// Is this body initially awake or sleeping? - bool awake; - - /// Should this body be prevented from rotating? Useful for characters. - bool fixedRotation; - - /// Is this a fast moving body that should be prevented from tunneling through - /// other moving bodies? Note that all bodies are prevented from tunneling through - /// kinematic and static bodies. This setting is only considered on dynamic bodies. - /// @warning You should use this flag sparingly since it increases processing time. - bool bullet; - - /// Does this body start out active? - bool active; - - /// Use this to store application specific body data. - void* userData; - - /// Scale the gravity applied to this body. - float32 gravityScale; -}; - -/// A rigid body. These are created via b2World::CreateBody. -class b2Body -{ -public: - /// Creates a fixture and attach it to this body. Use this function if you need - /// to set some fixture parameters, like friction. Otherwise you can create the - /// fixture directly from a shape. - /// If the density is non-zero, this function automatically updates the mass of the body. - /// Contacts are not created until the next time step. - /// @param def the fixture definition. - /// @warning This function is locked during callbacks. - b2Fixture* CreateFixture(const b2FixtureDef* def); - - /// Creates a fixture from a shape and attach it to this body. - /// This is a convenience function. Use b2FixtureDef if you need to set parameters - /// like friction, restitution, user data, or filtering. - /// If the density is non-zero, this function automatically updates the mass of the body. - /// @param shape the shape to be cloned. - /// @param density the shape density (set to zero for static bodies). - /// @warning This function is locked during callbacks. - b2Fixture* CreateFixture(const b2Shape* shape, float32 density); - - /// Destroy a fixture. This removes the fixture from the broad-phase and - /// destroys all contacts associated with this fixture. This will - /// automatically adjust the mass of the body if the body is dynamic and the - /// fixture has positive density. - /// All fixtures attached to a body are implicitly destroyed when the body is destroyed. - /// @param fixture the fixture to be removed. - /// @warning This function is locked during callbacks. - void DestroyFixture(b2Fixture* fixture); - - /// Set the position of the body's origin and rotation. - /// Manipulating a body's transform may cause non-physical behavior. - /// Note: contacts are updated on the next call to b2World::Step. - /// @param position the world position of the body's local origin. - /// @param angle the world rotation in radians. - void SetTransform(const b2Vec2& position, float32 angle); - - /// Get the body transform for the body's origin. - /// @return the world transform of the body's origin. - const b2Transform& GetTransform() const; - - /// Get the world body origin position. - /// @return the world position of the body's origin. - const b2Vec2& GetPosition() const; - - /// Get the angle in radians. - /// @return the current world rotation angle in radians. - float32 GetAngle() const; - - /// Get the world position of the center of mass. - const b2Vec2& GetWorldCenter() const; - - /// Get the local position of the center of mass. - const b2Vec2& GetLocalCenter() const; - - /// Set the linear velocity of the center of mass. - /// @param v the new linear velocity of the center of mass. - void SetLinearVelocity(const b2Vec2& v); - - /// Get the linear velocity of the center of mass. - /// @return the linear velocity of the center of mass. - const b2Vec2& GetLinearVelocity() const; - - /// Set the angular velocity. - /// @param omega the new angular velocity in radians/second. - void SetAngularVelocity(float32 omega); - - /// Get the angular velocity. - /// @return the angular velocity in radians/second. - float32 GetAngularVelocity() const; - - /// Apply a force at a world point. If the force is not - /// applied at the center of mass, it will generate a torque and - /// affect the angular velocity. This wakes up the body. - /// @param force the world force vector, usually in Newtons (N). - /// @param point the world position of the point of application. - /// @param wake also wake up the body - void ApplyForce(const b2Vec2& force, const b2Vec2& point, bool wake); - - /// Apply a force to the center of mass. This wakes up the body. - /// @param force the world force vector, usually in Newtons (N). - /// @param wake also wake up the body - void ApplyForceToCenter(const b2Vec2& force, bool wake); - - /// Apply a torque. This affects the angular velocity - /// without affecting the linear velocity of the center of mass. - /// This wakes up the body. - /// @param torque about the z-axis (out of the screen), usually in N-m. - /// @param wake also wake up the body - void ApplyTorque(float32 torque, bool wake); - - /// Apply an impulse at a point. This immediately modifies the velocity. - /// It also modifies the angular velocity if the point of application - /// is not at the center of mass. This wakes up the body. - /// @param impulse the world impulse vector, usually in N-seconds or kg-m/s. - /// @param point the world position of the point of application. - /// @param wake also wake up the body - void ApplyLinearImpulse(const b2Vec2& impulse, const b2Vec2& point, bool wake); - - /// Apply an angular impulse. - /// @param impulse the angular impulse in units of kg*m*m/s - /// @param wake also wake up the body - void ApplyAngularImpulse(float32 impulse, bool wake); - - /// Get the total mass of the body. - /// @return the mass, usually in kilograms (kg). - float32 GetMass() const; - - /// Get the rotational inertia of the body about the local origin. - /// @return the rotational inertia, usually in kg-m^2. - float32 GetInertia() const; - - /// Get the mass data of the body. - /// @return a struct containing the mass, inertia and center of the body. - void GetMassData(b2MassData* data) const; - - /// Set the mass properties to override the mass properties of the fixtures. - /// Note that this changes the center of mass position. - /// Note that creating or destroying fixtures can also alter the mass. - /// This function has no effect if the body isn't dynamic. - /// @param massData the mass properties. - void SetMassData(const b2MassData* data); - - /// This resets the mass properties to the sum of the mass properties of the fixtures. - /// This normally does not need to be called unless you called SetMassData to override - /// the mass and you later want to reset the mass. - void ResetMassData(); - - /// Get the world coordinates of a point given the local coordinates. - /// @param localPoint a point on the body measured relative the the body's origin. - /// @return the same point expressed in world coordinates. - b2Vec2 GetWorldPoint(const b2Vec2& localPoint) const; - - /// Get the world coordinates of a vector given the local coordinates. - /// @param localVector a vector fixed in the body. - /// @return the same vector expressed in world coordinates. - b2Vec2 GetWorldVector(const b2Vec2& localVector) const; - - /// Gets a local point relative to the body's origin given a world point. - /// @param a point in world coordinates. - /// @return the corresponding local point relative to the body's origin. - b2Vec2 GetLocalPoint(const b2Vec2& worldPoint) const; - - /// Gets a local vector given a world vector. - /// @param a vector in world coordinates. - /// @return the corresponding local vector. - b2Vec2 GetLocalVector(const b2Vec2& worldVector) const; - - /// Get the world linear velocity of a world point attached to this body. - /// @param a point in world coordinates. - /// @return the world velocity of a point. - b2Vec2 GetLinearVelocityFromWorldPoint(const b2Vec2& worldPoint) const; - - /// Get the world velocity of a local point. - /// @param a point in local coordinates. - /// @return the world velocity of a point. - b2Vec2 GetLinearVelocityFromLocalPoint(const b2Vec2& localPoint) const; - - /// Get the linear damping of the body. - float32 GetLinearDamping() const; - - /// Set the linear damping of the body. - void SetLinearDamping(float32 linearDamping); - - /// Get the angular damping of the body. - float32 GetAngularDamping() const; - - /// Set the angular damping of the body. - void SetAngularDamping(float32 angularDamping); - - /// Get the gravity scale of the body. - float32 GetGravityScale() const; - - /// Set the gravity scale of the body. - void SetGravityScale(float32 scale); - - /// Set the type of this body. This may alter the mass and velocity. - void SetType(b2BodyType type); - - /// Get the type of this body. - b2BodyType GetType() const; - - /// Should this body be treated like a bullet for continuous collision detection? - void SetBullet(bool flag); - - /// Is this body treated like a bullet for continuous collision detection? - bool IsBullet() const; - - /// You can disable sleeping on this body. If you disable sleeping, the - /// body will be woken. - void SetSleepingAllowed(bool flag); - - /// Is this body allowed to sleep - bool IsSleepingAllowed() const; - - /// Set the sleep state of the body. A sleeping body has very - /// low CPU cost. - /// @param flag set to true to wake the body, false to put it to sleep. - void SetAwake(bool flag); - - /// Get the sleeping state of this body. - /// @return true if the body is awake. - bool IsAwake() const; - - /// Set the active state of the body. An inactive body is not - /// simulated and cannot be collided with or woken up. - /// If you pass a flag of true, all fixtures will be added to the - /// broad-phase. - /// If you pass a flag of false, all fixtures will be removed from - /// the broad-phase and all contacts will be destroyed. - /// Fixtures and joints are otherwise unaffected. You may continue - /// to create/destroy fixtures and joints on inactive bodies. - /// Fixtures on an inactive body are implicitly inactive and will - /// not participate in collisions, ray-casts, or queries. - /// Joints connected to an inactive body are implicitly inactive. - /// An inactive body is still owned by a b2World object and remains - /// in the body list. - void SetActive(bool flag); - - /// Get the active state of the body. - bool IsActive() const; - - /// Set this body to have fixed rotation. This causes the mass - /// to be reset. - void SetFixedRotation(bool flag); - - /// Does this body have fixed rotation? - bool IsFixedRotation() const; - - /// Get the list of all fixtures attached to this body. - b2Fixture* GetFixtureList(); - const b2Fixture* GetFixtureList() const; - - /// Get the list of all joints attached to this body. - b2JointEdge* GetJointList(); - const b2JointEdge* GetJointList() const; - - /// Get the list of all contacts attached to this body. - /// @warning this list changes during the time step and you may - /// miss some collisions if you don't use b2ContactListener. - b2ContactEdge* GetContactList(); - const b2ContactEdge* GetContactList() const; - - /// Get the next body in the world's body list. - b2Body* GetNext(); - const b2Body* GetNext() const; - - /// Get the user data pointer that was provided in the body definition. - void* GetUserData() const; - - /// Set the user data. Use this to store your application specific data. - void SetUserData(void* data); - - /// Get the parent world of this body. - b2World* GetWorld(); - const b2World* GetWorld() const; - - /// Dump this body to a log file - void Dump(); - -private: - - friend class b2World; - friend class b2Island; - friend class b2ContactManager; - friend class b2ContactSolver; - friend class b2Contact; - - friend class b2DistanceJoint; - friend class b2FrictionJoint; - friend class b2GearJoint; - friend class b2MotorJoint; - friend class b2MouseJoint; - friend class b2PrismaticJoint; - friend class b2PulleyJoint; - friend class b2RevoluteJoint; - friend class b2RopeJoint; - friend class b2WeldJoint; - friend class b2WheelJoint; - - // m_flags - enum - { - e_islandFlag = 0x0001, - e_awakeFlag = 0x0002, - e_autoSleepFlag = 0x0004, - e_bulletFlag = 0x0008, - e_fixedRotationFlag = 0x0010, - e_activeFlag = 0x0020, - e_toiFlag = 0x0040 - }; - - b2Body(const b2BodyDef* bd, b2World* world); - ~b2Body(); - - void SynchronizeFixtures(); - void SynchronizeTransform(); - - // This is used to prevent connected bodies from colliding. - // It may lie, depending on the collideConnected flag. - bool ShouldCollide(const b2Body* other) const; - - void Advance(float32 t); - - b2BodyType m_type; - - uint16 m_flags; - - int32 m_islandIndex; - - b2Transform m_xf; // the body origin transform - b2Sweep m_sweep; // the swept motion for CCD - - b2Vec2 m_linearVelocity; - float32 m_angularVelocity; - - b2Vec2 m_force; - float32 m_torque; - - b2World* m_world; - b2Body* m_prev; - b2Body* m_next; - - b2Fixture* m_fixtureList; - int32 m_fixtureCount; - - b2JointEdge* m_jointList; - b2ContactEdge* m_contactList; - - float32 m_mass, m_invMass; - - // Rotational inertia about the center of mass. - float32 m_I, m_invI; - - float32 m_linearDamping; - float32 m_angularDamping; - float32 m_gravityScale; - - float32 m_sleepTime; - - void* m_userData; -}; - -inline b2BodyType b2Body::GetType() const -{ - return m_type; -} - -inline const b2Transform& b2Body::GetTransform() const -{ - return m_xf; -} - -inline const b2Vec2& b2Body::GetPosition() const -{ - return m_xf.p; -} - -inline float32 b2Body::GetAngle() const -{ - return m_sweep.a; -} - -inline const b2Vec2& b2Body::GetWorldCenter() const -{ - return m_sweep.c; -} - -inline const b2Vec2& b2Body::GetLocalCenter() const -{ - return m_sweep.localCenter; -} - -inline void b2Body::SetLinearVelocity(const b2Vec2& v) -{ - if (m_type == b2_staticBody) - { - return; - } - - if (b2Dot(v,v) > 0.0f) - { - SetAwake(true); - } - - m_linearVelocity = v; -} - -inline const b2Vec2& b2Body::GetLinearVelocity() const -{ - return m_linearVelocity; -} - -inline void b2Body::SetAngularVelocity(float32 w) -{ - if (m_type == b2_staticBody) - { - return; - } - - if (w * w > 0.0f) - { - SetAwake(true); - } - - m_angularVelocity = w; -} - -inline float32 b2Body::GetAngularVelocity() const -{ - return m_angularVelocity; -} - -inline float32 b2Body::GetMass() const -{ - return m_mass; -} - -inline float32 b2Body::GetInertia() const -{ - return m_I + m_mass * b2Dot(m_sweep.localCenter, m_sweep.localCenter); -} - -inline void b2Body::GetMassData(b2MassData* data) const -{ - data->mass = m_mass; - data->I = m_I + m_mass * b2Dot(m_sweep.localCenter, m_sweep.localCenter); - data->center = m_sweep.localCenter; -} - -inline b2Vec2 b2Body::GetWorldPoint(const b2Vec2& localPoint) const -{ - return b2Mul(m_xf, localPoint); -} - -inline b2Vec2 b2Body::GetWorldVector(const b2Vec2& localVector) const -{ - return b2Mul(m_xf.q, localVector); -} - -inline b2Vec2 b2Body::GetLocalPoint(const b2Vec2& worldPoint) const -{ - return b2MulT(m_xf, worldPoint); -} - -inline b2Vec2 b2Body::GetLocalVector(const b2Vec2& worldVector) const -{ - return b2MulT(m_xf.q, worldVector); -} - -inline b2Vec2 b2Body::GetLinearVelocityFromWorldPoint(const b2Vec2& worldPoint) const -{ - return m_linearVelocity + b2Cross(m_angularVelocity, worldPoint - m_sweep.c); -} - -inline b2Vec2 b2Body::GetLinearVelocityFromLocalPoint(const b2Vec2& localPoint) const -{ - return GetLinearVelocityFromWorldPoint(GetWorldPoint(localPoint)); -} - -inline float32 b2Body::GetLinearDamping() const -{ - return m_linearDamping; -} - -inline void b2Body::SetLinearDamping(float32 linearDamping) -{ - m_linearDamping = linearDamping; -} - -inline float32 b2Body::GetAngularDamping() const -{ - return m_angularDamping; -} - -inline void b2Body::SetAngularDamping(float32 angularDamping) -{ - m_angularDamping = angularDamping; -} - -inline float32 b2Body::GetGravityScale() const -{ - return m_gravityScale; -} - -inline void b2Body::SetGravityScale(float32 scale) -{ - m_gravityScale = scale; -} - -inline void b2Body::SetBullet(bool flag) -{ - if (flag) - { - m_flags |= e_bulletFlag; - } - else - { - m_flags &= ~e_bulletFlag; - } -} - -inline bool b2Body::IsBullet() const -{ - return (m_flags & e_bulletFlag) == e_bulletFlag; -} - -inline void b2Body::SetAwake(bool flag) -{ - if (flag) - { - if ((m_flags & e_awakeFlag) == 0) - { - m_flags |= e_awakeFlag; - m_sleepTime = 0.0f; - } - } - else - { - m_flags &= ~e_awakeFlag; - m_sleepTime = 0.0f; - m_linearVelocity.SetZero(); - m_angularVelocity = 0.0f; - m_force.SetZero(); - m_torque = 0.0f; - } -} - -inline bool b2Body::IsAwake() const -{ - return (m_flags & e_awakeFlag) == e_awakeFlag; -} - -inline bool b2Body::IsActive() const -{ - return (m_flags & e_activeFlag) == e_activeFlag; -} - -inline bool b2Body::IsFixedRotation() const -{ - return (m_flags & e_fixedRotationFlag) == e_fixedRotationFlag; -} - -inline void b2Body::SetSleepingAllowed(bool flag) -{ - if (flag) - { - m_flags |= e_autoSleepFlag; - } - else - { - m_flags &= ~e_autoSleepFlag; - SetAwake(true); - } -} - -inline bool b2Body::IsSleepingAllowed() const -{ - return (m_flags & e_autoSleepFlag) == e_autoSleepFlag; -} - -inline b2Fixture* b2Body::GetFixtureList() -{ - return m_fixtureList; -} - -inline const b2Fixture* b2Body::GetFixtureList() const -{ - return m_fixtureList; -} - -inline b2JointEdge* b2Body::GetJointList() -{ - return m_jointList; -} - -inline const b2JointEdge* b2Body::GetJointList() const -{ - return m_jointList; -} - -inline b2ContactEdge* b2Body::GetContactList() -{ - return m_contactList; -} - -inline const b2ContactEdge* b2Body::GetContactList() const -{ - return m_contactList; -} - -inline b2Body* b2Body::GetNext() -{ - return m_next; -} - -inline const b2Body* b2Body::GetNext() const -{ - return m_next; -} - -inline void b2Body::SetUserData(void* data) -{ - m_userData = data; -} - -inline void* b2Body::GetUserData() const -{ - return m_userData; -} - -inline void b2Body::ApplyForce(const b2Vec2& force, const b2Vec2& point, bool wake) -{ - if (m_type != b2_dynamicBody) - { - return; - } - - if (wake && (m_flags & e_awakeFlag) == 0) - { - SetAwake(true); - } - - // Don't accumulate a force if the body is sleeping. - if (m_flags & e_awakeFlag) - { - m_force += force; - m_torque += b2Cross(point - m_sweep.c, force); - } -} - -inline void b2Body::ApplyForceToCenter(const b2Vec2& force, bool wake) -{ - if (m_type != b2_dynamicBody) - { - return; - } - - if (wake && (m_flags & e_awakeFlag) == 0) - { - SetAwake(true); - } - - // Don't accumulate a force if the body is sleeping - if (m_flags & e_awakeFlag) - { - m_force += force; - } -} - -inline void b2Body::ApplyTorque(float32 torque, bool wake) -{ - if (m_type != b2_dynamicBody) - { - return; - } - - if (wake && (m_flags & e_awakeFlag) == 0) - { - SetAwake(true); - } - - // Don't accumulate a force if the body is sleeping - if (m_flags & e_awakeFlag) - { - m_torque += torque; - } -} - -inline void b2Body::ApplyLinearImpulse(const b2Vec2& impulse, const b2Vec2& point, bool wake) -{ - if (m_type != b2_dynamicBody) - { - return; - } - - if (wake && (m_flags & e_awakeFlag) == 0) - { - SetAwake(true); - } - - // Don't accumulate velocity if the body is sleeping - if (m_flags & e_awakeFlag) - { - m_linearVelocity += m_invMass * impulse; - m_angularVelocity += m_invI * b2Cross(point - m_sweep.c, impulse); - } -} - -inline void b2Body::ApplyAngularImpulse(float32 impulse, bool wake) -{ - if (m_type != b2_dynamicBody) - { - return; - } - - if (wake && (m_flags & e_awakeFlag) == 0) - { - SetAwake(true); - } - - // Don't accumulate velocity if the body is sleeping - if (m_flags & e_awakeFlag) - { - m_angularVelocity += m_invI * impulse; - } -} - -inline void b2Body::SynchronizeTransform() -{ - m_xf.q.Set(m_sweep.a); - m_xf.p = m_sweep.c - b2Mul(m_xf.q, m_sweep.localCenter); -} - -inline void b2Body::Advance(float32 alpha) -{ - // Advance to the new safe time. This doesn't sync the broad-phase. - m_sweep.Advance(alpha); - m_sweep.c = m_sweep.c0; - m_sweep.a = m_sweep.a0; - m_xf.q.Set(m_sweep.a); - m_xf.p = m_sweep.c - b2Mul(m_xf.q, m_sweep.localCenter); -} - -inline b2World* b2Body::GetWorld() -{ - return m_world; -} - -inline const b2World* b2Body::GetWorld() const -{ - return m_world; -} - -#endif +// MIT License + +// Copyright (c) 2019 Erin Catto + +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: + +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. + +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. + +#ifndef B2_BODY_H +#define B2_BODY_H + +#include "b2_api.h" +#include "b2_math.h" +#include "b2_shape.h" + +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 +}; + +/// 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 B2_API b2BodyDef +{ + /// This constructor sets the body definition default values. + b2BodyDef() + { + 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; + enabled = 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. + float angle; + + /// The linear velocity of the body's origin in world co-ordinates. + b2Vec2 linearVelocity; + + /// The angular velocity of the body. + float 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 + float 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 + float 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 enabled? + bool enabled; + + /// Use this to store application specific body data. + b2BodyUserData userData; + + /// Scale the gravity applied to this body. + float gravityScale; +}; + +/// A rigid body. These are created via b2World::CreateBody. +class B2_API 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, float 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, float 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. + float 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(float omega); + + /// Get the angular velocity. + /// @return the angular velocity in radians/second. + float 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(float 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(float impulse, bool wake); + + /// Get the total mass of the body. + /// @return the mass, usually in kilograms (kg). + float GetMass() const; + + /// Get the rotational inertia of the body about the local origin. + /// @return the rotational inertia, usually in kg-m^2. + float GetInertia() const; + + /// Get the mass data of the body. + /// @return a struct containing the mass, inertia and center of the body. + b2MassData GetMassData() 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 data 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 worldPoint 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 worldVector 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 worldPoint 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 localPoint 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. + float GetLinearDamping() const; + + /// Set the linear damping of the body. + void SetLinearDamping(float linearDamping); + + /// Get the angular damping of the body. + float GetAngularDamping() const; + + /// Set the angular damping of the body. + void SetAngularDamping(float angularDamping); + + /// Get the gravity scale of the body. + float GetGravityScale() const; + + /// Set the gravity scale of the body. + void SetGravityScale(float 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; + + /// Allow a body to be disabled. A disabled 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 disabled bodies. + /// Fixtures on a disabled body are implicitly disabled and will + /// not participate in collisions, ray-casts, or queries. + /// Joints connected to a disabled body are implicitly disabled. + /// An diabled body is still owned by a b2World object and remains + /// in the body list. + void SetEnabled(bool flag); + + /// Get the active state of the body. + bool IsEnabled() 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. + b2BodyUserData& GetUserData(); + const b2BodyUserData& GetUserData() const; + + /// Get the parent world of this body. + b2World* GetWorld(); + const b2World* GetWorld() const; + + /// Dump this body to a 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 b2WeldJoint; + friend class b2WheelJoint; + + // m_flags + enum + { + e_islandFlag = 0x0001, + e_awakeFlag = 0x0002, + e_autoSleepFlag = 0x0004, + e_bulletFlag = 0x0008, + e_fixedRotationFlag = 0x0010, + e_enabledFlag = 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(float 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; + float m_angularVelocity; + + b2Vec2 m_force; + float m_torque; + + b2World* m_world; + b2Body* m_prev; + b2Body* m_next; + + b2Fixture* m_fixtureList; + int32 m_fixtureCount; + + b2JointEdge* m_jointList; + b2ContactEdge* m_contactList; + + float m_mass, m_invMass; + + // Rotational inertia about the center of mass. + float m_I, m_invI; + + float m_linearDamping; + float m_angularDamping; + float m_gravityScale; + + float m_sleepTime; + + b2BodyUserData 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 float 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(float w) +{ + if (m_type == b2_staticBody) + { + return; + } + + if (w * w > 0.0f) + { + SetAwake(true); + } + + m_angularVelocity = w; +} + +inline float b2Body::GetAngularVelocity() const +{ + return m_angularVelocity; +} + +inline float b2Body::GetMass() const +{ + return m_mass; +} + +inline float b2Body::GetInertia() const +{ + return m_I + m_mass * b2Dot(m_sweep.localCenter, m_sweep.localCenter); +} + +inline b2MassData b2Body::GetMassData() const +{ + b2MassData data; + data.mass = m_mass; + data.I = m_I + m_mass * b2Dot(m_sweep.localCenter, m_sweep.localCenter); + data.center = m_sweep.localCenter; + return data; +} + +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 float b2Body::GetLinearDamping() const +{ + return m_linearDamping; +} + +inline void b2Body::SetLinearDamping(float linearDamping) +{ + m_linearDamping = linearDamping; +} + +inline float b2Body::GetAngularDamping() const +{ + return m_angularDamping; +} + +inline void b2Body::SetAngularDamping(float angularDamping) +{ + m_angularDamping = angularDamping; +} + +inline float b2Body::GetGravityScale() const +{ + return m_gravityScale; +} + +inline void b2Body::SetGravityScale(float 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 (m_type == b2_staticBody) + { + return; + } + + 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::IsEnabled() const +{ + return (m_flags & e_enabledFlag) == e_enabledFlag; +} + +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 b2BodyUserData& b2Body::GetUserData() +{ + return m_userData; +} + +inline const b2BodyUserData& 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(float 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(float 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(float 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/libs/Box2D/Collision/b2BroadPhase.h b/libs/box2d/b2_broad_phase.h similarity index 69% rename from libs/Box2D/Collision/b2BroadPhase.h rename to libs/box2d/b2_broad_phase.h index 49057104..cc882ab4 100644 --- a/libs/Box2D/Collision/b2BroadPhase.h +++ b/libs/box2d/b2_broad_phase.h @@ -1,257 +1,238 @@ -/* -* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef B2_BROAD_PHASE_H -#define B2_BROAD_PHASE_H - -#include -#include -#include -#include - -struct b2Pair -{ - int32 proxyIdA; - int32 proxyIdB; -}; - -/// The broad-phase is used for computing pairs and performing volume queries and ray casts. -/// This broad-phase does not persist pairs. Instead, this reports potentially new pairs. -/// It is up to the client to consume the new pairs and to track subsequent overlap. -class b2BroadPhase -{ -public: - - enum - { - e_nullProxy = -1 - }; - - b2BroadPhase(); - ~b2BroadPhase(); - - /// Create a proxy with an initial AABB. Pairs are not reported until - /// UpdatePairs is called. - int32 CreateProxy(const b2AABB& aabb, void* userData); - - /// Destroy a proxy. It is up to the client to remove any pairs. - void DestroyProxy(int32 proxyId); - - /// Call MoveProxy as many times as you like, then when you are done - /// call UpdatePairs to finalized the proxy pairs (for your time step). - void MoveProxy(int32 proxyId, const b2AABB& aabb, const b2Vec2& displacement); - - /// Call to trigger a re-processing of it's pairs on the next call to UpdatePairs. - void TouchProxy(int32 proxyId); - - /// Get the fat AABB for a proxy. - const b2AABB& GetFatAABB(int32 proxyId) const; - - /// Get user data from a proxy. Returns NULL if the id is invalid. - void* GetUserData(int32 proxyId) const; - - /// Test overlap of fat AABBs. - bool TestOverlap(int32 proxyIdA, int32 proxyIdB) const; - - /// Get the number of proxies. - int32 GetProxyCount() const; - - /// Update the pairs. This results in pair callbacks. This can only add pairs. - template - void UpdatePairs(T* callback); - - /// Query an AABB for overlapping proxies. The callback class - /// is called for each proxy that overlaps the supplied AABB. - template - void Query(T* callback, const b2AABB& aabb) const; - - /// Ray-cast against the proxies in the tree. This relies on the callback - /// to perform a exact ray-cast in the case were the proxy contains a shape. - /// The callback also performs the any collision filtering. This has performance - /// roughly equal to k * log(n), where k is the number of collisions and n is the - /// number of proxies in the tree. - /// @param input the ray-cast input data. The ray extends from p1 to p1 + maxFraction * (p2 - p1). - /// @param callback a callback class that is called for each proxy that is hit by the ray. - template - void RayCast(T* callback, const b2RayCastInput& input) const; - - /// Get the height of the embedded tree. - int32 GetTreeHeight() const; - - /// Get the balance of the embedded tree. - int32 GetTreeBalance() const; - - /// Get the quality metric of the embedded tree. - float32 GetTreeQuality() const; - - /// Shift the world origin. Useful for large worlds. - /// The shift formula is: position -= newOrigin - /// @param newOrigin the new origin with respect to the old origin - void ShiftOrigin(const b2Vec2& newOrigin); - -private: - - friend class b2DynamicTree; - - void BufferMove(int32 proxyId); - void UnBufferMove(int32 proxyId); - - bool QueryCallback(int32 proxyId); - - b2DynamicTree m_tree; - - int32 m_proxyCount; - - int32* m_moveBuffer; - int32 m_moveCapacity; - int32 m_moveCount; - - b2Pair* m_pairBuffer; - int32 m_pairCapacity; - int32 m_pairCount; - - int32 m_queryProxyId; -}; - -/// This is used to sort pairs. -inline bool b2PairLessThan(const b2Pair& pair1, const b2Pair& pair2) -{ - if (pair1.proxyIdA < pair2.proxyIdA) - { - return true; - } - - if (pair1.proxyIdA == pair2.proxyIdA) - { - return pair1.proxyIdB < pair2.proxyIdB; - } - - return false; -} - -inline void* b2BroadPhase::GetUserData(int32 proxyId) const -{ - return m_tree.GetUserData(proxyId); -} - -inline bool b2BroadPhase::TestOverlap(int32 proxyIdA, int32 proxyIdB) const -{ - const b2AABB& aabbA = m_tree.GetFatAABB(proxyIdA); - const b2AABB& aabbB = m_tree.GetFatAABB(proxyIdB); - return b2TestOverlap(aabbA, aabbB); -} - -inline const b2AABB& b2BroadPhase::GetFatAABB(int32 proxyId) const -{ - return m_tree.GetFatAABB(proxyId); -} - -inline int32 b2BroadPhase::GetProxyCount() const -{ - return m_proxyCount; -} - -inline int32 b2BroadPhase::GetTreeHeight() const -{ - return m_tree.GetHeight(); -} - -inline int32 b2BroadPhase::GetTreeBalance() const -{ - return m_tree.GetMaxBalance(); -} - -inline float32 b2BroadPhase::GetTreeQuality() const -{ - return m_tree.GetAreaRatio(); -} - -template -void b2BroadPhase::UpdatePairs(T* callback) -{ - // Reset pair buffer - m_pairCount = 0; - - // Perform tree queries for all moving proxies. - for (int32 i = 0; i < m_moveCount; ++i) - { - m_queryProxyId = m_moveBuffer[i]; - if (m_queryProxyId == e_nullProxy) - { - continue; - } - - // We have to query the tree with the fat AABB so that - // we don't fail to create a pair that may touch later. - const b2AABB& fatAABB = m_tree.GetFatAABB(m_queryProxyId); - - // Query tree, create pairs and add them pair buffer. - m_tree.Query(this, fatAABB); - } - - // Reset move buffer - m_moveCount = 0; - - // Sort the pair buffer to expose duplicates. - std::sort(m_pairBuffer, m_pairBuffer + m_pairCount, b2PairLessThan); - - // Send the pairs back to the client. - int32 i = 0; - while (i < m_pairCount) - { - b2Pair* primaryPair = m_pairBuffer + i; - void* userDataA = m_tree.GetUserData(primaryPair->proxyIdA); - void* userDataB = m_tree.GetUserData(primaryPair->proxyIdB); - - callback->AddPair(userDataA, userDataB); - ++i; - - // Skip any duplicate pairs. - while (i < m_pairCount) - { - b2Pair* pair = m_pairBuffer + i; - if (pair->proxyIdA != primaryPair->proxyIdA || pair->proxyIdB != primaryPair->proxyIdB) - { - break; - } - ++i; - } - } - - // Try to keep the tree balanced. - //m_tree.Rebalance(4); -} - -template -inline void b2BroadPhase::Query(T* callback, const b2AABB& aabb) const -{ - m_tree.Query(callback, aabb); -} - -template -inline void b2BroadPhase::RayCast(T* callback, const b2RayCastInput& input) const -{ - m_tree.RayCast(callback, input); -} - -inline void b2BroadPhase::ShiftOrigin(const b2Vec2& newOrigin) -{ - m_tree.ShiftOrigin(newOrigin); -} - -#endif +// MIT License + +// Copyright (c) 2019 Erin Catto + +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: + +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. + +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. + +#ifndef B2_BROAD_PHASE_H +#define B2_BROAD_PHASE_H + +#include "b2_api.h" +#include "b2_settings.h" +#include "b2_collision.h" +#include "b2_dynamic_tree.h" + +struct B2_API b2Pair +{ + int32 proxyIdA; + int32 proxyIdB; +}; + +/// The broad-phase is used for computing pairs and performing volume queries and ray casts. +/// This broad-phase does not persist pairs. Instead, this reports potentially new pairs. +/// It is up to the client to consume the new pairs and to track subsequent overlap. +class B2_API b2BroadPhase +{ +public: + + enum + { + e_nullProxy = -1 + }; + + b2BroadPhase(); + ~b2BroadPhase(); + + /// Create a proxy with an initial AABB. Pairs are not reported until + /// UpdatePairs is called. + int32 CreateProxy(const b2AABB& aabb, void* userData); + + /// Destroy a proxy. It is up to the client to remove any pairs. + void DestroyProxy(int32 proxyId); + + /// Call MoveProxy as many times as you like, then when you are done + /// call UpdatePairs to finalized the proxy pairs (for your time step). + void MoveProxy(int32 proxyId, const b2AABB& aabb, const b2Vec2& displacement); + + /// Call to trigger a re-processing of it's pairs on the next call to UpdatePairs. + void TouchProxy(int32 proxyId); + + /// Get the fat AABB for a proxy. + const b2AABB& GetFatAABB(int32 proxyId) const; + + /// Get user data from a proxy. Returns nullptr if the id is invalid. + void* GetUserData(int32 proxyId) const; + + /// Test overlap of fat AABBs. + bool TestOverlap(int32 proxyIdA, int32 proxyIdB) const; + + /// Get the number of proxies. + int32 GetProxyCount() const; + + /// Update the pairs. This results in pair callbacks. This can only add pairs. + template + void UpdatePairs(T* callback); + + /// Query an AABB for overlapping proxies. The callback class + /// is called for each proxy that overlaps the supplied AABB. + template + void Query(T* callback, const b2AABB& aabb) const; + + /// Ray-cast against the proxies in the tree. This relies on the callback + /// to perform a exact ray-cast in the case were the proxy contains a shape. + /// The callback also performs the any collision filtering. This has performance + /// roughly equal to k * log(n), where k is the number of collisions and n is the + /// number of proxies in the tree. + /// @param input the ray-cast input data. The ray extends from p1 to p1 + maxFraction * (p2 - p1). + /// @param callback a callback class that is called for each proxy that is hit by the ray. + template + void RayCast(T* callback, const b2RayCastInput& input) const; + + /// Get the height of the embedded tree. + int32 GetTreeHeight() const; + + /// Get the balance of the embedded tree. + int32 GetTreeBalance() const; + + /// Get the quality metric of the embedded tree. + float GetTreeQuality() const; + + /// Shift the world origin. Useful for large worlds. + /// The shift formula is: position -= newOrigin + /// @param newOrigin the new origin with respect to the old origin + void ShiftOrigin(const b2Vec2& newOrigin); + +private: + + friend class b2DynamicTree; + + void BufferMove(int32 proxyId); + void UnBufferMove(int32 proxyId); + + bool QueryCallback(int32 proxyId); + + b2DynamicTree m_tree; + + int32 m_proxyCount; + + int32* m_moveBuffer; + int32 m_moveCapacity; + int32 m_moveCount; + + b2Pair* m_pairBuffer; + int32 m_pairCapacity; + int32 m_pairCount; + + int32 m_queryProxyId; +}; + +inline void* b2BroadPhase::GetUserData(int32 proxyId) const +{ + return m_tree.GetUserData(proxyId); +} + +inline bool b2BroadPhase::TestOverlap(int32 proxyIdA, int32 proxyIdB) const +{ + const b2AABB& aabbA = m_tree.GetFatAABB(proxyIdA); + const b2AABB& aabbB = m_tree.GetFatAABB(proxyIdB); + return b2TestOverlap(aabbA, aabbB); +} + +inline const b2AABB& b2BroadPhase::GetFatAABB(int32 proxyId) const +{ + return m_tree.GetFatAABB(proxyId); +} + +inline int32 b2BroadPhase::GetProxyCount() const +{ + return m_proxyCount; +} + +inline int32 b2BroadPhase::GetTreeHeight() const +{ + return m_tree.GetHeight(); +} + +inline int32 b2BroadPhase::GetTreeBalance() const +{ + return m_tree.GetMaxBalance(); +} + +inline float b2BroadPhase::GetTreeQuality() const +{ + return m_tree.GetAreaRatio(); +} + +template +void b2BroadPhase::UpdatePairs(T* callback) +{ + // Reset pair buffer + m_pairCount = 0; + + // Perform tree queries for all moving proxies. + for (int32 i = 0; i < m_moveCount; ++i) + { + m_queryProxyId = m_moveBuffer[i]; + if (m_queryProxyId == e_nullProxy) + { + continue; + } + + // We have to query the tree with the fat AABB so that + // we don't fail to create a pair that may touch later. + const b2AABB& fatAABB = m_tree.GetFatAABB(m_queryProxyId); + + // Query tree, create pairs and add them pair buffer. + m_tree.Query(this, fatAABB); + } + + // Send pairs to caller + for (int32 i = 0; i < m_pairCount; ++i) + { + b2Pair* primaryPair = m_pairBuffer + i; + void* userDataA = m_tree.GetUserData(primaryPair->proxyIdA); + void* userDataB = m_tree.GetUserData(primaryPair->proxyIdB); + + callback->AddPair(userDataA, userDataB); + } + + // Clear move flags + for (int32 i = 0; i < m_moveCount; ++i) + { + int32 proxyId = m_moveBuffer[i]; + if (proxyId == e_nullProxy) + { + continue; + } + + m_tree.ClearMoved(proxyId); + } + + // Reset move buffer + m_moveCount = 0; +} + +template +inline void b2BroadPhase::Query(T* callback, const b2AABB& aabb) const +{ + m_tree.Query(callback, aabb); +} + +template +inline void b2BroadPhase::RayCast(T* callback, const b2RayCastInput& input) const +{ + m_tree.RayCast(callback, input); +} + +inline void b2BroadPhase::ShiftOrigin(const b2Vec2& newOrigin) +{ + m_tree.ShiftOrigin(newOrigin); +} + +#endif diff --git a/libs/box2d/b2_chain_shape.h b/libs/box2d/b2_chain_shape.h new file mode 100644 index 00000000..da2605d6 --- /dev/null +++ b/libs/box2d/b2_chain_shape.h @@ -0,0 +1,101 @@ +// MIT License + +// Copyright (c) 2019 Erin Catto + +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: + +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. + +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. + +#ifndef B2_CHAIN_SHAPE_H +#define B2_CHAIN_SHAPE_H + +#include "b2_api.h" +#include "b2_shape.h" + +class b2EdgeShape; + +/// A chain shape is a free form sequence of line segments. +/// The chain has one-sided collision, with the surface normal pointing to the right of the edge. +/// This provides a counter-clockwise winding like the polygon shape. +/// Connectivity information is used to create smooth collisions. +/// @warning the chain will not collide properly if there are self-intersections. +class B2_API b2ChainShape : public b2Shape +{ +public: + b2ChainShape(); + + /// The destructor frees the vertices using b2Free. + ~b2ChainShape(); + + /// Clear all data. + void Clear(); + + /// Create a loop. This automatically adjusts connectivity. + /// @param vertices an array of vertices, these are copied + /// @param count the vertex count + void CreateLoop(const b2Vec2* vertices, int32 count); + + /// Create a chain with ghost vertices to connect multiple chains together. + /// @param vertices an array of vertices, these are copied + /// @param count the vertex count + /// @param prevVertex previous vertex from chain that connects to the start + /// @param nextVertex next vertex from chain that connects to the end + void CreateChain(const b2Vec2* vertices, int32 count, + const b2Vec2& prevVertex, const b2Vec2& nextVertex); + + /// Implement b2Shape. Vertices are cloned using b2Alloc. + b2Shape* Clone(b2BlockAllocator* allocator) const override; + + /// @see b2Shape::GetChildCount + int32 GetChildCount() const override; + + /// Get a child edge. + void GetChildEdge(b2EdgeShape* edge, int32 index) const; + + /// This always return false. + /// @see b2Shape::TestPoint + bool TestPoint(const b2Transform& transform, const b2Vec2& p) const override; + + /// Implement b2Shape. + bool RayCast(b2RayCastOutput* output, const b2RayCastInput& input, + const b2Transform& transform, int32 childIndex) const override; + + /// @see b2Shape::ComputeAABB + void ComputeAABB(b2AABB* aabb, const b2Transform& transform, int32 childIndex) const override; + + /// Chains have zero mass. + /// @see b2Shape::ComputeMass + void ComputeMass(b2MassData* massData, float density) const override; + + /// The vertices. Owned by this class. + b2Vec2* m_vertices; + + /// The vertex count. + int32 m_count; + + b2Vec2 m_prevVertex, m_nextVertex; +}; + +inline b2ChainShape::b2ChainShape() +{ + m_type = e_chain; + m_radius = b2_polygonRadius; + m_vertices = nullptr; + m_count = 0; +} + +#endif diff --git a/libs/box2d/b2_circle_shape.h b/libs/box2d/b2_circle_shape.h new file mode 100644 index 00000000..5e330f5a --- /dev/null +++ b/libs/box2d/b2_circle_shape.h @@ -0,0 +1,67 @@ +// MIT License + +// Copyright (c) 2019 Erin Catto + +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: + +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. + +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. + +#ifndef B2_CIRCLE_SHAPE_H +#define B2_CIRCLE_SHAPE_H + +#include "b2_api.h" +#include "b2_shape.h" + +/// A solid circle shape +class B2_API b2CircleShape : public b2Shape +{ +public: + b2CircleShape(); + + /// Implement b2Shape. + b2Shape* Clone(b2BlockAllocator* allocator) const override; + + /// @see b2Shape::GetChildCount + int32 GetChildCount() const override; + + /// Implement b2Shape. + bool TestPoint(const b2Transform& transform, const b2Vec2& p) const override; + + /// Implement b2Shape. + /// @note because the circle is solid, rays that start inside do not hit because the normal is + /// not defined. + bool RayCast(b2RayCastOutput* output, const b2RayCastInput& input, + const b2Transform& transform, int32 childIndex) const override; + + /// @see b2Shape::ComputeAABB + void ComputeAABB(b2AABB* aabb, const b2Transform& transform, int32 childIndex) const override; + + /// @see b2Shape::ComputeMass + void ComputeMass(b2MassData* massData, float density) const override; + + /// Position + b2Vec2 m_p; +}; + +inline b2CircleShape::b2CircleShape() +{ + m_type = e_circle; + m_radius = 0.0f; + m_p.SetZero(); +} + +#endif diff --git a/libs/Box2D/Collision/b2Collision.h b/libs/box2d/b2_collision.h similarity index 67% rename from libs/Box2D/Collision/b2Collision.h rename to libs/box2d/b2_collision.h index 8b0ed66a..055704e7 100644 --- a/libs/Box2D/Collision/b2Collision.h +++ b/libs/box2d/b2_collision.h @@ -1,277 +1,306 @@ -/* -* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef B2_COLLISION_H -#define B2_COLLISION_H - -#include -#include - -/// @file -/// Structures and functions used for computing contact points, distance -/// queries, and TOI queries. - -class b2Shape; -class b2CircleShape; -class b2EdgeShape; -class b2PolygonShape; - -const uint8 b2_nullFeature = UCHAR_MAX; - -/// The features that intersect to form the contact point -/// This must be 4 bytes or less. -struct b2ContactFeature -{ - enum Type - { - e_vertex = 0, - e_face = 1 - }; - - uint8 indexA; ///< Feature index on shapeA - uint8 indexB; ///< Feature index on shapeB - uint8 typeA; ///< The feature type on shapeA - uint8 typeB; ///< The feature type on shapeB -}; - -/// Contact ids to facilitate warm starting. -union b2ContactID -{ - b2ContactFeature cf; - uint32 key; ///< Used to quickly compare contact ids. -}; - -/// A manifold point is a contact point belonging to a contact -/// manifold. It holds details related to the geometry and dynamics -/// of the contact points. -/// The local point usage depends on the manifold type: -/// -e_circles: the local center of circleB -/// -e_faceA: the local center of cirlceB or the clip point of polygonB -/// -e_faceB: the clip point of polygonA -/// This structure is stored across time steps, so we keep it small. -/// Note: the impulses are used for internal caching and may not -/// provide reliable contact forces, especially for high speed collisions. -struct b2ManifoldPoint -{ - b2Vec2 localPoint; ///< usage depends on manifold type - float32 normalImpulse; ///< the non-penetration impulse - float32 tangentImpulse; ///< the friction impulse - b2ContactID id; ///< uniquely identifies a contact point between two shapes -}; - -/// A manifold for two touching convex shapes. -/// Box2D supports multiple types of contact: -/// - clip point versus plane with radius -/// - point versus point with radius (circles) -/// The local point usage depends on the manifold type: -/// -e_circles: the local center of circleA -/// -e_faceA: the center of faceA -/// -e_faceB: the center of faceB -/// Similarly the local normal usage: -/// -e_circles: not used -/// -e_faceA: the normal on polygonA -/// -e_faceB: the normal on polygonB -/// We store contacts in this way so that position correction can -/// account for movement, which is critical for continuous physics. -/// All contact scenarios must be expressed in one of these types. -/// This structure is stored across time steps, so we keep it small. -struct b2Manifold -{ - enum Type - { - e_circles, - e_faceA, - e_faceB - }; - - b2ManifoldPoint points[b2_maxManifoldPoints]; ///< the points of contact - b2Vec2 localNormal; ///< not use for Type::e_points - b2Vec2 localPoint; ///< usage depends on manifold type - Type type; - int32 pointCount; ///< the number of manifold points -}; - -/// This is used to compute the current state of a contact manifold. -struct b2WorldManifold -{ - /// Evaluate the manifold with supplied transforms. This assumes - /// modest motion from the original state. This does not change the - /// point count, impulses, etc. The radii must come from the shapes - /// that generated the manifold. - void Initialize(const b2Manifold* manifold, - const b2Transform& xfA, float32 radiusA, - const b2Transform& xfB, float32 radiusB); - - b2Vec2 normal; ///< world vector pointing from A to B - b2Vec2 points[b2_maxManifoldPoints]; ///< world contact point (point of intersection) - float32 separations[b2_maxManifoldPoints]; ///< a negative value indicates overlap, in meters -}; - -/// This is used for determining the state of contact points. -enum b2PointState -{ - b2_nullState, ///< point does not exist - b2_addState, ///< point was added in the update - b2_persistState, ///< point persisted across the update - b2_removeState ///< point was removed in the update -}; - -/// Compute the point states given two manifolds. The states pertain to the transition from manifold1 -/// to manifold2. So state1 is either persist or remove while state2 is either add or persist. -void b2GetPointStates(b2PointState state1[b2_maxManifoldPoints], b2PointState state2[b2_maxManifoldPoints], - const b2Manifold* manifold1, const b2Manifold* manifold2); - -/// Used for computing contact manifolds. -struct b2ClipVertex -{ - b2Vec2 v; - b2ContactID id; -}; - -/// Ray-cast input data. The ray extends from p1 to p1 + maxFraction * (p2 - p1). -struct b2RayCastInput -{ - b2Vec2 p1, p2; - float32 maxFraction; -}; - -/// Ray-cast output data. The ray hits at p1 + fraction * (p2 - p1), where p1 and p2 -/// come from b2RayCastInput. -struct b2RayCastOutput -{ - b2Vec2 normal; - float32 fraction; -}; - -/// An axis aligned bounding box. -struct b2AABB -{ - /// Verify that the bounds are sorted. - bool IsValid() const; - - /// Get the center of the AABB. - b2Vec2 GetCenter() const - { - return 0.5f * (lowerBound + upperBound); - } - - /// Get the extents of the AABB (half-widths). - b2Vec2 GetExtents() const - { - return 0.5f * (upperBound - lowerBound); - } - - /// Get the perimeter length - float32 GetPerimeter() const - { - float32 wx = upperBound.x - lowerBound.x; - float32 wy = upperBound.y - lowerBound.y; - return 2.0f * (wx + wy); - } - - /// Combine an AABB into this one. - void Combine(const b2AABB& aabb) - { - lowerBound = b2Min(lowerBound, aabb.lowerBound); - upperBound = b2Max(upperBound, aabb.upperBound); - } - - /// Combine two AABBs into this one. - void Combine(const b2AABB& aabb1, const b2AABB& aabb2) - { - lowerBound = b2Min(aabb1.lowerBound, aabb2.lowerBound); - upperBound = b2Max(aabb1.upperBound, aabb2.upperBound); - } - - /// Does this aabb contain the provided AABB. - bool Contains(const b2AABB& aabb) const - { - bool result = true; - result = result && lowerBound.x <= aabb.lowerBound.x; - result = result && lowerBound.y <= aabb.lowerBound.y; - result = result && aabb.upperBound.x <= upperBound.x; - result = result && aabb.upperBound.y <= upperBound.y; - return result; - } - - bool RayCast(b2RayCastOutput* output, const b2RayCastInput& input) const; - - b2Vec2 lowerBound; ///< the lower vertex - b2Vec2 upperBound; ///< the upper vertex -}; - -/// Compute the collision manifold between two circles. -void b2CollideCircles(b2Manifold* manifold, - const b2CircleShape* circleA, const b2Transform& xfA, - const b2CircleShape* circleB, const b2Transform& xfB); - -/// Compute the collision manifold between a polygon and a circle. -void b2CollidePolygonAndCircle(b2Manifold* manifold, - const b2PolygonShape* polygonA, const b2Transform& xfA, - const b2CircleShape* circleB, const b2Transform& xfB); - -/// Compute the collision manifold between two polygons. -void b2CollidePolygons(b2Manifold* manifold, - const b2PolygonShape* polygonA, const b2Transform& xfA, - const b2PolygonShape* polygonB, const b2Transform& xfB); - -/// Compute the collision manifold between an edge and a circle. -void b2CollideEdgeAndCircle(b2Manifold* manifold, - const b2EdgeShape* polygonA, const b2Transform& xfA, - const b2CircleShape* circleB, const b2Transform& xfB); - -/// Compute the collision manifold between an edge and a circle. -void b2CollideEdgeAndPolygon(b2Manifold* manifold, - const b2EdgeShape* edgeA, const b2Transform& xfA, - const b2PolygonShape* circleB, const b2Transform& xfB); - -/// Clipping for contact manifolds. -int32 b2ClipSegmentToLine(b2ClipVertex vOut[2], const b2ClipVertex vIn[2], - const b2Vec2& normal, float32 offset, int32 vertexIndexA); - -/// Determine if two generic shapes overlap. -bool b2TestOverlap( const b2Shape* shapeA, int32 indexA, - const b2Shape* shapeB, int32 indexB, - const b2Transform& xfA, const b2Transform& xfB); - -// ---------------- Inline Functions ------------------------------------------ - -inline bool b2AABB::IsValid() const -{ - b2Vec2 d = upperBound - lowerBound; - bool valid = d.x >= 0.0f && d.y >= 0.0f; - valid = valid && lowerBound.IsValid() && upperBound.IsValid(); - return valid; -} - -inline bool b2TestOverlap(const b2AABB& a, const b2AABB& b) -{ - b2Vec2 d1, d2; - d1 = b.lowerBound - a.upperBound; - d2 = a.lowerBound - b.upperBound; - - if (d1.x > 0.0f || d1.y > 0.0f) - return false; - - if (d2.x > 0.0f || d2.y > 0.0f) - return false; - - return true; -} - -#endif +// MIT License + +// Copyright (c) 2019 Erin Catto + +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: + +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. + +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. + +#ifndef B2_COLLISION_H +#define B2_COLLISION_H + +#include + +#include "b2_api.h" +#include "b2_math.h" + +/// @file +/// Structures and functions used for computing contact points, distance +/// queries, and TOI queries. + +class b2Shape; +class b2CircleShape; +class b2EdgeShape; +class b2PolygonShape; + +const uint8 b2_nullFeature = UCHAR_MAX; + +/// The features that intersect to form the contact point +/// This must be 4 bytes or less. +struct B2_API b2ContactFeature +{ + enum Type + { + e_vertex = 0, + e_face = 1 + }; + + uint8 indexA; ///< Feature index on shapeA + uint8 indexB; ///< Feature index on shapeB + uint8 typeA; ///< The feature type on shapeA + uint8 typeB; ///< The feature type on shapeB +}; + +/// Contact ids to facilitate warm starting. +union B2_API b2ContactID +{ + b2ContactFeature cf; + uint32 key; ///< Used to quickly compare contact ids. +}; + +/// A manifold point is a contact point belonging to a contact +/// manifold. It holds details related to the geometry and dynamics +/// of the contact points. +/// The local point usage depends on the manifold type: +/// -e_circles: the local center of circleB +/// -e_faceA: the local center of cirlceB or the clip point of polygonB +/// -e_faceB: the clip point of polygonA +/// This structure is stored across time steps, so we keep it small. +/// Note: the impulses are used for internal caching and may not +/// provide reliable contact forces, especially for high speed collisions. +struct B2_API b2ManifoldPoint +{ + b2Vec2 localPoint; ///< usage depends on manifold type + float normalImpulse; ///< the non-penetration impulse + float tangentImpulse; ///< the friction impulse + b2ContactID id; ///< uniquely identifies a contact point between two shapes +}; + +/// A manifold for two touching convex shapes. +/// Box2D supports multiple types of contact: +/// - clip point versus plane with radius +/// - point versus point with radius (circles) +/// The local point usage depends on the manifold type: +/// -e_circles: the local center of circleA +/// -e_faceA: the center of faceA +/// -e_faceB: the center of faceB +/// Similarly the local normal usage: +/// -e_circles: not used +/// -e_faceA: the normal on polygonA +/// -e_faceB: the normal on polygonB +/// We store contacts in this way so that position correction can +/// account for movement, which is critical for continuous physics. +/// All contact scenarios must be expressed in one of these types. +/// This structure is stored across time steps, so we keep it small. +struct B2_API b2Manifold +{ + enum Type + { + e_circles, + e_faceA, + e_faceB + }; + + b2ManifoldPoint points[b2_maxManifoldPoints]; ///< the points of contact + b2Vec2 localNormal; ///< not use for Type::e_points + b2Vec2 localPoint; ///< usage depends on manifold type + Type type; + int32 pointCount; ///< the number of manifold points +}; + +/// This is used to compute the current state of a contact manifold. +struct B2_API b2WorldManifold +{ + /// Evaluate the manifold with supplied transforms. This assumes + /// modest motion from the original state. This does not change the + /// point count, impulses, etc. The radii must come from the shapes + /// that generated the manifold. + void Initialize(const b2Manifold* manifold, + const b2Transform& xfA, float radiusA, + const b2Transform& xfB, float radiusB); + + b2Vec2 normal; ///< world vector pointing from A to B + b2Vec2 points[b2_maxManifoldPoints]; ///< world contact point (point of intersection) + float separations[b2_maxManifoldPoints]; ///< a negative value indicates overlap, in meters +}; + +/// This is used for determining the state of contact points. +enum b2PointState +{ + b2_nullState, ///< point does not exist + b2_addState, ///< point was added in the update + b2_persistState, ///< point persisted across the update + b2_removeState ///< point was removed in the update +}; + +/// Compute the point states given two manifolds. The states pertain to the transition from manifold1 +/// to manifold2. So state1 is either persist or remove while state2 is either add or persist. +B2_API void b2GetPointStates(b2PointState state1[b2_maxManifoldPoints], b2PointState state2[b2_maxManifoldPoints], + const b2Manifold* manifold1, const b2Manifold* manifold2); + +/// Used for computing contact manifolds. +struct B2_API b2ClipVertex +{ + b2Vec2 v; + b2ContactID id; +}; + +/// Ray-cast input data. The ray extends from p1 to p1 + maxFraction * (p2 - p1). +struct B2_API b2RayCastInput +{ + b2Vec2 p1, p2; + float maxFraction; +}; + +/// Ray-cast output data. The ray hits at p1 + fraction * (p2 - p1), where p1 and p2 +/// come from b2RayCastInput. +struct B2_API b2RayCastOutput +{ + b2Vec2 normal; + float fraction; +}; + +/// An axis aligned bounding box. +struct B2_API b2AABB +{ + /// Verify that the bounds are sorted. + bool IsValid() const; + + /// Get the center of the AABB. + b2Vec2 GetCenter() const + { + return 0.5f * (lowerBound + upperBound); + } + + /// Get the extents of the AABB (half-widths). + b2Vec2 GetExtents() const + { + return 0.5f * (upperBound - lowerBound); + } + + /// Get the perimeter length + float GetPerimeter() const + { + float wx = upperBound.x - lowerBound.x; + float wy = upperBound.y - lowerBound.y; + return 2.0f * (wx + wy); + } + + /// Combine an AABB into this one. + void Combine(const b2AABB& aabb) + { + lowerBound = b2Min(lowerBound, aabb.lowerBound); + upperBound = b2Max(upperBound, aabb.upperBound); + } + + /// Combine two AABBs into this one. + void Combine(const b2AABB& aabb1, const b2AABB& aabb2) + { + lowerBound = b2Min(aabb1.lowerBound, aabb2.lowerBound); + upperBound = b2Max(aabb1.upperBound, aabb2.upperBound); + } + + /// Does this aabb contain the provided AABB. + bool Contains(const b2AABB& aabb) const + { + bool result = true; + result = result && lowerBound.x <= aabb.lowerBound.x; + result = result && lowerBound.y <= aabb.lowerBound.y; + result = result && aabb.upperBound.x <= upperBound.x; + result = result && aabb.upperBound.y <= upperBound.y; + return result; + } + + bool RayCast(b2RayCastOutput* output, const b2RayCastInput& input) const; + + b2Vec2 lowerBound; ///< the lower vertex + b2Vec2 upperBound; ///< the upper vertex +}; + +/// Compute the collision manifold between two circles. +B2_API void b2CollideCircles(b2Manifold* manifold, + const b2CircleShape* circleA, const b2Transform& xfA, + const b2CircleShape* circleB, const b2Transform& xfB); + +/// Compute the collision manifold between a polygon and a circle. +B2_API void b2CollidePolygonAndCircle(b2Manifold* manifold, + const b2PolygonShape* polygonA, const b2Transform& xfA, + const b2CircleShape* circleB, const b2Transform& xfB); + +/// Compute the collision manifold between two polygons. +B2_API void b2CollidePolygons(b2Manifold* manifold, + const b2PolygonShape* polygonA, const b2Transform& xfA, + const b2PolygonShape* polygonB, const b2Transform& xfB); + +/// Compute the collision manifold between an edge and a circle. +B2_API void b2CollideEdgeAndCircle(b2Manifold* manifold, + const b2EdgeShape* polygonA, const b2Transform& xfA, + const b2CircleShape* circleB, const b2Transform& xfB); + +/// Compute the collision manifold between an edge and a polygon. +B2_API void b2CollideEdgeAndPolygon(b2Manifold* manifold, + const b2EdgeShape* edgeA, const b2Transform& xfA, + const b2PolygonShape* polygonB, const b2Transform& xfB); + +/// Clipping for contact manifolds. +B2_API int32 b2ClipSegmentToLine(b2ClipVertex vOut[2], const b2ClipVertex vIn[2], + const b2Vec2& normal, float offset, int32 vertexIndexA); + +/// Determine if two generic shapes overlap. +B2_API bool b2TestOverlap( const b2Shape* shapeA, int32 indexA, + const b2Shape* shapeB, int32 indexB, + const b2Transform& xfA, const b2Transform& xfB); + +/// Convex hull used for polygon collision +struct b2Hull +{ + b2Vec2 points[b2_maxPolygonVertices]; + int32 count; +}; + +/// Compute the convex hull of a set of points. Returns an empty hull if it fails. +/// Some failure cases: +/// - all points very close together +/// - all points on a line +/// - less than 3 points +/// - more than b2_maxPolygonVertices points +/// This welds close points and removes collinear points. +b2Hull b2ComputeHull(const b2Vec2* points, int32 count); + +/// This determines if a hull is valid. Checks for: +/// - convexity +/// - collinear points +/// This is expensive and should not be called at runtime. +bool b2ValidateHull(const b2Hull& hull); + + +// ---------------- Inline Functions ------------------------------------------ + +inline bool b2AABB::IsValid() const +{ + b2Vec2 d = upperBound - lowerBound; + bool valid = d.x >= 0.0f && d.y >= 0.0f; + valid = valid && lowerBound.IsValid() && upperBound.IsValid(); + return valid; +} + +inline bool b2TestOverlap(const b2AABB& a, const b2AABB& b) +{ + b2Vec2 d1, d2; + d1 = b.lowerBound - a.upperBound; + d2 = a.lowerBound - b.upperBound; + + if (d1.x > 0.0f || d1.y > 0.0f) + return false; + + if (d2.x > 0.0f || d2.y > 0.0f) + return false; + + return true; +} + +#endif diff --git a/libs/Box2D/Common/b2Settings.h b/libs/box2d/b2_common.h similarity index 57% rename from libs/Box2D/Common/b2Settings.h rename to libs/box2d/b2_common.h index 8d1639ff..dfca8af1 100644 --- a/libs/Box2D/Common/b2Settings.h +++ b/libs/box2d/b2_common.h @@ -1,151 +1,138 @@ -/* -* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef B2_SETTINGS_H -#define B2_SETTINGS_H - -#include -#include -#include - -#define B2_NOT_USED(x) ((void)(x)) -#define b2Assert(A) assert(A) - -typedef signed char int8; -typedef signed short int16; -typedef signed int int32; -typedef unsigned char uint8; -typedef unsigned short uint16; -typedef unsigned int uint32; -typedef float float32; -typedef double float64; - -#define b2_maxFloat FLT_MAX -#define b2_epsilon FLT_EPSILON -#define b2_pi 3.14159265359f - -/// @file -/// Global tuning constants based on meters-kilograms-seconds (MKS) units. -/// - -// Collision - -/// The maximum number of contact points between two convex shapes. Do -/// not change this value. -#define b2_maxManifoldPoints 2 - -/// The maximum number of vertices on a convex polygon. You cannot increase -/// this too much because b2BlockAllocator has a maximum object size. -#define b2_maxPolygonVertices 8 - -/// This is used to fatten AABBs in the dynamic tree. This allows proxies -/// to move by a small amount without triggering a tree adjustment. -/// This is in meters. -#define b2_aabbExtension 0.1f - -/// This is used to fatten AABBs in the dynamic tree. This is used to predict -/// the future position based on the current displacement. -/// This is a dimensionless multiplier. -#define b2_aabbMultiplier 2.0f - -/// A small length used as a collision and constraint tolerance. Usually it is -/// chosen to be numerically significant, but visually insignificant. -#define b2_linearSlop 0.005f - -/// A small angle used as a collision and constraint tolerance. Usually it is -/// chosen to be numerically significant, but visually insignificant. -#define b2_angularSlop (2.0f / 180.0f * b2_pi) - -/// The radius of the polygon/edge shape skin. This should not be modified. Making -/// this smaller means polygons will have an insufficient buffer for continuous collision. -/// Making it larger may create artifacts for vertex collision. -#define b2_polygonRadius (2.0f * b2_linearSlop) - -/// Maximum number of sub-steps per contact in continuous physics simulation. -#define b2_maxSubSteps 8 - - -// Dynamics - -/// Maximum number of contacts to be handled to solve a TOI impact. -#define b2_maxTOIContacts 32 - -/// A velocity threshold for elastic collisions. Any collision with a relative linear -/// velocity below this threshold will be treated as inelastic. -#define b2_velocityThreshold 1.0f - -/// The maximum linear position correction used when solving constraints. This helps to -/// prevent overshoot. -#define b2_maxLinearCorrection 0.2f - -/// The maximum angular position correction used when solving constraints. This helps to -/// prevent overshoot. -#define b2_maxAngularCorrection (8.0f / 180.0f * b2_pi) - -/// The maximum linear velocity of a body. This limit is very large and is used -/// to prevent numerical problems. You shouldn't need to adjust this. -#define b2_maxTranslation 20.0f -#define b2_maxTranslationSquared (b2_maxTranslation * b2_maxTranslation) - -/// The maximum angular velocity of a body. This limit is very large and is used -/// to prevent numerical problems. You shouldn't need to adjust this. -#define b2_maxRotation (0.5f * b2_pi) -#define b2_maxRotationSquared (b2_maxRotation * b2_maxRotation) - -/// This scale factor controls how fast overlap is resolved. Ideally this would be 1 so -/// that overlap is removed in one time step. However using values close to 1 often lead -/// to overshoot. -#define b2_baumgarte 0.2f -#define b2_toiBaugarte 0.75f - - -// Sleep - -/// The time that a body must be still before it will go to sleep. -#define b2_timeToSleep 0.5f - -/// A body cannot sleep if its linear velocity is above this tolerance. -#define b2_linearSleepTolerance 0.01f - -/// A body cannot sleep if its angular velocity is above this tolerance. -#define b2_angularSleepTolerance (2.0f / 180.0f * b2_pi) - -// Memory Allocation - -/// Implement this function to use your own memory allocator. -void* b2Alloc(int32 size); - -/// If you implement b2Alloc, you should also implement this function. -void b2Free(void* mem); - -/// Logging function. -void b2Log(const char* string, ...); - -/// Version numbering scheme. -/// See http://en.wikipedia.org/wiki/Software_versioning -struct b2Version -{ - int32 major; ///< significant changes - int32 minor; ///< incremental changes - int32 revision; ///< bug fixes -}; - -/// Current version. -extern b2Version b2_version; - -#endif +// MIT License + +// Copyright (c) 2019 Erin Catto + +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: + +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. + +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. + +#ifndef B2_COMMON_H +#define B2_COMMON_H + +#include "b2_settings.h" + +#include +#include +#include + +#if !defined(NDEBUG) + #define b2DEBUG +#endif + +#define B2_NOT_USED(x) ((void)(x)) +#define b2Assert(A) assert(A) + +#define b2_maxFloat FLT_MAX +#define b2_epsilon FLT_EPSILON +#define b2_pi 3.14159265359f + +/// @file +/// Global tuning constants based on meters-kilograms-seconds (MKS) units. +/// + +// Collision + +/// The maximum number of contact points between two convex shapes. Do +/// not change this value. +#define b2_maxManifoldPoints 2 + +/// This is used to fatten AABBs in the dynamic tree. This allows proxies +/// to move by a small amount without triggering a tree adjustment. +/// This is in meters. +#define b2_aabbExtension (0.1f * b2_lengthUnitsPerMeter) + +/// This is used to fatten AABBs in the dynamic tree. This is used to predict +/// the future position based on the current displacement. +/// This is a dimensionless multiplier. +#define b2_aabbMultiplier 4.0f + +/// A small length used as a collision and constraint tolerance. Usually it is +/// chosen to be numerically significant, but visually insignificant. In meters. +#define b2_linearSlop (0.005f * b2_lengthUnitsPerMeter) + +/// A small angle used as a collision and constraint tolerance. Usually it is +/// chosen to be numerically significant, but visually insignificant. +#define b2_angularSlop (2.0f / 180.0f * b2_pi) + +/// The radius of the polygon/edge shape skin. This should not be modified. Making +/// this smaller means polygons will have an insufficient buffer for continuous collision. +/// Making it larger may create artifacts for vertex collision. +#define b2_polygonRadius (2.0f * b2_linearSlop) + +/// Maximum number of sub-steps per contact in continuous physics simulation. +#define b2_maxSubSteps 8 + + +// Dynamics + +/// Maximum number of contacts to be handled to solve a TOI impact. +#define b2_maxTOIContacts 32 + +/// The maximum linear position correction used when solving constraints. This helps to +/// prevent overshoot. Meters. +#define b2_maxLinearCorrection (0.2f * b2_lengthUnitsPerMeter) + +/// The maximum angular position correction used when solving constraints. This helps to +/// prevent overshoot. +#define b2_maxAngularCorrection (8.0f / 180.0f * b2_pi) + +/// The maximum linear translation of a body per step. This limit is very large and is used +/// to prevent numerical problems. You shouldn't need to adjust this. Meters. +#define b2_maxTranslation (2.0f * b2_lengthUnitsPerMeter) +#define b2_maxTranslationSquared (b2_maxTranslation * b2_maxTranslation) + +/// The maximum angular velocity of a body. This limit is very large and is used +/// to prevent numerical problems. You shouldn't need to adjust this. +#define b2_maxRotation (0.5f * b2_pi) +#define b2_maxRotationSquared (b2_maxRotation * b2_maxRotation) + +/// This scale factor controls how fast overlap is resolved. Ideally this would be 1 so +/// that overlap is removed in one time step. However using values close to 1 often lead +/// to overshoot. +#define b2_baumgarte 0.2f +#define b2_toiBaumgarte 0.75f + + +// Sleep + +/// The time that a body must be still before it will go to sleep. +#define b2_timeToSleep 0.5f + +/// A body cannot sleep if its linear velocity is above this tolerance. +#define b2_linearSleepTolerance (0.01f * b2_lengthUnitsPerMeter) + +/// A body cannot sleep if its angular velocity is above this tolerance. +#define b2_angularSleepTolerance (2.0f / 180.0f * b2_pi) + +/// Dump to a file. Only one dump file allowed at a time. +void b2OpenDump(const char* fileName); +void b2Dump(const char* string, ...); +void b2CloseDump(); + +/// Version numbering scheme. +/// See http://en.wikipedia.org/wiki/Software_versioning +struct b2Version +{ + int32 major; ///< significant changes + int32 minor; ///< incremental changes + int32 revision; ///< bug fixes +}; + +/// Current version. +extern B2_API b2Version b2_version; + +#endif diff --git a/libs/Box2D/Dynamics/Contacts/b2Contact.h b/libs/box2d/b2_contact.h similarity index 69% rename from libs/Box2D/Dynamics/Contacts/b2Contact.h rename to libs/box2d/b2_contact.h index 347da31e..de7541f1 100644 --- a/libs/Box2D/Dynamics/Contacts/b2Contact.h +++ b/libs/box2d/b2_contact.h @@ -1,349 +1,386 @@ -/* -* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef B2_CONTACT_H -#define B2_CONTACT_H - -#include -#include -#include -#include - -class b2Body; -class b2Contact; -class b2Fixture; -class b2World; -class b2BlockAllocator; -class b2StackAllocator; -class b2ContactListener; - -/// Friction mixing law. The idea is to allow either fixture to drive the restitution to zero. -/// For example, anything slides on ice. -inline float32 b2MixFriction(float32 friction1, float32 friction2) -{ - return b2Sqrt(friction1 * friction2); -} - -/// Restitution mixing law. The idea is allow for anything to bounce off an inelastic surface. -/// For example, a superball bounces on anything. -inline float32 b2MixRestitution(float32 restitution1, float32 restitution2) -{ - return restitution1 > restitution2 ? restitution1 : restitution2; -} - -typedef b2Contact* b2ContactCreateFcn( b2Fixture* fixtureA, int32 indexA, - b2Fixture* fixtureB, int32 indexB, - b2BlockAllocator* allocator); -typedef void b2ContactDestroyFcn(b2Contact* contact, b2BlockAllocator* allocator); - -struct b2ContactRegister -{ - b2ContactCreateFcn* createFcn; - b2ContactDestroyFcn* destroyFcn; - bool primary; -}; - -/// A contact edge is used to connect bodies and contacts together -/// in a contact graph where each body is a node and each contact -/// is an edge. A contact edge belongs to a doubly linked list -/// maintained in each attached body. Each contact has two contact -/// nodes, one for each attached body. -struct b2ContactEdge -{ - b2Body* other; ///< provides quick access to the other body attached. - b2Contact* contact; ///< the contact - b2ContactEdge* prev; ///< the previous contact edge in the body's contact list - b2ContactEdge* next; ///< the next contact edge in the body's contact list -}; - -/// The class manages contact between two shapes. A contact exists for each overlapping -/// AABB in the broad-phase (except if filtered). Therefore a contact object may exist -/// that has no contact points. -class b2Contact -{ -public: - - /// Get the contact manifold. Do not modify the manifold unless you understand the - /// internals of Box2D. - b2Manifold* GetManifold(); - const b2Manifold* GetManifold() const; - - /// Get the world manifold. - void GetWorldManifold(b2WorldManifold* worldManifold) const; - - /// Is this contact touching? - bool IsTouching() const; - - /// Enable/disable this contact. This can be used inside the pre-solve - /// contact listener. The contact is only disabled for the current - /// time step (or sub-step in continuous collisions). - void SetEnabled(bool flag); - - /// Has this contact been disabled? - bool IsEnabled() const; - - /// Get the next contact in the world's contact list. - b2Contact* GetNext(); - const b2Contact* GetNext() const; - - /// Get fixture A in this contact. - b2Fixture* GetFixtureA(); - const b2Fixture* GetFixtureA() const; - - /// Get the child primitive index for fixture A. - int32 GetChildIndexA() const; - - /// Get fixture B in this contact. - b2Fixture* GetFixtureB(); - const b2Fixture* GetFixtureB() const; - - /// Get the child primitive index for fixture B. - int32 GetChildIndexB() const; - - /// Override the default friction mixture. You can call this in b2ContactListener::PreSolve. - /// This value persists until set or reset. - void SetFriction(float32 friction); - - /// Get the friction. - float32 GetFriction() const; - - /// Reset the friction mixture to the default value. - void ResetFriction(); - - /// Override the default restitution mixture. You can call this in b2ContactListener::PreSolve. - /// The value persists until you set or reset. - void SetRestitution(float32 restitution); - - /// Get the restitution. - float32 GetRestitution() const; - - /// Reset the restitution to the default value. - void ResetRestitution(); - - /// Set the desired tangent speed for a conveyor belt behavior. In meters per second. - void SetTangentSpeed(float32 speed); - - /// Get the desired tangent speed. In meters per second. - float32 GetTangentSpeed() const; - - /// Evaluate this contact with your own manifold and transforms. - virtual void Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB) = 0; - -protected: - friend class b2ContactManager; - friend class b2World; - friend class b2ContactSolver; - friend class b2Body; - friend class b2Fixture; - - // Flags stored in m_flags - enum - { - // Used when crawling contact graph when forming islands. - e_islandFlag = 0x0001, - - // Set when the shapes are touching. - e_touchingFlag = 0x0002, - - // This contact can be disabled (by user) - e_enabledFlag = 0x0004, - - // This contact needs filtering because a fixture filter was changed. - e_filterFlag = 0x0008, - - // This bullet contact had a TOI event - e_bulletHitFlag = 0x0010, - - // This contact has a valid TOI in m_toi - e_toiFlag = 0x0020 - }; - - /// Flag this contact for filtering. Filtering will occur the next time step. - void FlagForFiltering(); - - static void AddType(b2ContactCreateFcn* createFcn, b2ContactDestroyFcn* destroyFcn, - b2Shape::Type typeA, b2Shape::Type typeB); - static void InitializeRegisters(); - static b2Contact* Create(b2Fixture* fixtureA, int32 indexA, b2Fixture* fixtureB, int32 indexB, b2BlockAllocator* allocator); - static void Destroy(b2Contact* contact, b2Shape::Type typeA, b2Shape::Type typeB, b2BlockAllocator* allocator); - static void Destroy(b2Contact* contact, b2BlockAllocator* allocator); - - b2Contact() : m_fixtureA(NULL), m_fixtureB(NULL) {} - b2Contact(b2Fixture* fixtureA, int32 indexA, b2Fixture* fixtureB, int32 indexB); - virtual ~b2Contact() {} - - void Update(b2ContactListener* listener); - - static b2ContactRegister s_registers[b2Shape::e_typeCount][b2Shape::e_typeCount]; - static bool s_initialized; - - uint32 m_flags; - - // World pool and list pointers. - b2Contact* m_prev; - b2Contact* m_next; - - // Nodes for connecting bodies. - b2ContactEdge m_nodeA; - b2ContactEdge m_nodeB; - - b2Fixture* m_fixtureA; - b2Fixture* m_fixtureB; - - int32 m_indexA; - int32 m_indexB; - - b2Manifold m_manifold; - - int32 m_toiCount; - float32 m_toi; - - float32 m_friction; - float32 m_restitution; - - float32 m_tangentSpeed; -}; - -inline b2Manifold* b2Contact::GetManifold() -{ - return &m_manifold; -} - -inline const b2Manifold* b2Contact::GetManifold() const -{ - return &m_manifold; -} - -inline void b2Contact::GetWorldManifold(b2WorldManifold* worldManifold) const -{ - const b2Body* bodyA = m_fixtureA->GetBody(); - const b2Body* bodyB = m_fixtureB->GetBody(); - const b2Shape* shapeA = m_fixtureA->GetShape(); - const b2Shape* shapeB = m_fixtureB->GetShape(); - - worldManifold->Initialize(&m_manifold, bodyA->GetTransform(), shapeA->m_radius, bodyB->GetTransform(), shapeB->m_radius); -} - -inline void b2Contact::SetEnabled(bool flag) -{ - if (flag) - { - m_flags |= e_enabledFlag; - } - else - { - m_flags &= ~e_enabledFlag; - } -} - -inline bool b2Contact::IsEnabled() const -{ - return (m_flags & e_enabledFlag) == e_enabledFlag; -} - -inline bool b2Contact::IsTouching() const -{ - return (m_flags & e_touchingFlag) == e_touchingFlag; -} - -inline b2Contact* b2Contact::GetNext() -{ - return m_next; -} - -inline const b2Contact* b2Contact::GetNext() const -{ - return m_next; -} - -inline b2Fixture* b2Contact::GetFixtureA() -{ - return m_fixtureA; -} - -inline const b2Fixture* b2Contact::GetFixtureA() const -{ - return m_fixtureA; -} - -inline b2Fixture* b2Contact::GetFixtureB() -{ - return m_fixtureB; -} - -inline int32 b2Contact::GetChildIndexA() const -{ - return m_indexA; -} - -inline const b2Fixture* b2Contact::GetFixtureB() const -{ - return m_fixtureB; -} - -inline int32 b2Contact::GetChildIndexB() const -{ - return m_indexB; -} - -inline void b2Contact::FlagForFiltering() -{ - m_flags |= e_filterFlag; -} - -inline void b2Contact::SetFriction(float32 friction) -{ - m_friction = friction; -} - -inline float32 b2Contact::GetFriction() const -{ - return m_friction; -} - -inline void b2Contact::ResetFriction() -{ - m_friction = b2MixFriction(m_fixtureA->m_friction, m_fixtureB->m_friction); -} - -inline void b2Contact::SetRestitution(float32 restitution) -{ - m_restitution = restitution; -} - -inline float32 b2Contact::GetRestitution() const -{ - return m_restitution; -} - -inline void b2Contact::ResetRestitution() -{ - m_restitution = b2MixRestitution(m_fixtureA->m_restitution, m_fixtureB->m_restitution); -} - -inline void b2Contact::SetTangentSpeed(float32 speed) -{ - m_tangentSpeed = speed; -} - -inline float32 b2Contact::GetTangentSpeed() const -{ - return m_tangentSpeed; -} - -#endif +// MIT License + +// Copyright (c) 2019 Erin Catto + +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: + +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. + +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. + +#ifndef B2_CONTACT_H +#define B2_CONTACT_H + +#include "b2_api.h" +#include "b2_collision.h" +#include "b2_fixture.h" +#include "b2_math.h" +#include "b2_shape.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 float b2MixFriction(float friction1, float 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 float b2MixRestitution(float restitution1, float restitution2) +{ + return restitution1 > restitution2 ? restitution1 : restitution2; +} + +/// Restitution mixing law. This picks the lowest value. +inline float b2MixRestitutionThreshold(float threshold1, float threshold2) +{ + return threshold1 < threshold2 ? threshold1 : threshold2; +} + +typedef b2Contact* b2ContactCreateFcn( b2Fixture* fixtureA, int32 indexA, + b2Fixture* fixtureB, int32 indexB, + b2BlockAllocator* allocator); +typedef void b2ContactDestroyFcn(b2Contact* contact, b2BlockAllocator* allocator); + +struct B2_API 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 B2_API 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 B2_API 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(float friction); + + /// Get the friction. + float 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(float restitution); + + /// Get the restitution. + float GetRestitution() const; + + /// Reset the restitution to the default value. + void ResetRestitution(); + + /// Override the default restitution velocity threshold mixture. You can call this in b2ContactListener::PreSolve. + /// The value persists until you set or reset. + void SetRestitutionThreshold(float threshold); + + /// Get the restitution threshold. + float GetRestitutionThreshold() const; + + /// Reset the restitution threshold to the default value. + void ResetRestitutionThreshold(); + + /// Set the desired tangent speed for a conveyor belt behavior. In meters per second. + void SetTangentSpeed(float speed); + + /// Get the desired tangent speed. In meters per second. + float 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; + float m_toi; + + float m_friction; + float m_restitution; + float m_restitutionThreshold; + + float 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(float friction) +{ + m_friction = friction; +} + +inline float 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(float restitution) +{ + m_restitution = restitution; +} + +inline float b2Contact::GetRestitution() const +{ + return m_restitution; +} + +inline void b2Contact::ResetRestitution() +{ + m_restitution = b2MixRestitution(m_fixtureA->m_restitution, m_fixtureB->m_restitution); +} + +inline void b2Contact::SetRestitutionThreshold(float threshold) +{ + m_restitutionThreshold = threshold; +} + +inline float b2Contact::GetRestitutionThreshold() const +{ + return m_restitutionThreshold; +} + +inline void b2Contact::ResetRestitutionThreshold() +{ + m_restitutionThreshold = b2MixRestitutionThreshold(m_fixtureA->m_restitutionThreshold, m_fixtureB->m_restitutionThreshold); +} + +inline void b2Contact::SetTangentSpeed(float speed) +{ + m_tangentSpeed = speed; +} + +inline float b2Contact::GetTangentSpeed() const +{ + return m_tangentSpeed; +} + +#endif diff --git a/libs/box2d/b2_contact_manager.h b/libs/box2d/b2_contact_manager.h new file mode 100644 index 00000000..fbd3b4d4 --- /dev/null +++ b/libs/box2d/b2_contact_manager.h @@ -0,0 +1,57 @@ +// MIT License + +// Copyright (c) 2019 Erin Catto + +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: + +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. + +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. + +#ifndef B2_CONTACT_MANAGER_H +#define B2_CONTACT_MANAGER_H + +#include "b2_api.h" +#include "b2_broad_phase.h" + +class b2Contact; +class b2ContactFilter; +class b2ContactListener; +class b2BlockAllocator; + +// Delegate of b2World. +class B2_API 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/libs/box2d/b2_distance.h b/libs/box2d/b2_distance.h new file mode 100644 index 00000000..3e05773d --- /dev/null +++ b/libs/box2d/b2_distance.h @@ -0,0 +1,171 @@ +// MIT License + +// Copyright (c) 2019 Erin Catto + +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: + +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. + +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. + +#ifndef B2_DISTANCE_H +#define B2_DISTANCE_H + +#include "b2_api.h" +#include "b2_math.h" + +class b2Shape; + +/// A distance proxy is used by the GJK algorithm. +/// It encapsulates any shape. +struct B2_API b2DistanceProxy +{ + b2DistanceProxy() : m_vertices(nullptr), m_count(0), m_radius(0.0f) {} + + /// Initialize the proxy using the given shape. The shape + /// must remain in scope while the proxy is in use. + void Set(const b2Shape* shape, int32 index); + + /// Initialize the proxy using a vertex cloud and radius. The vertices + /// must remain in scope while the proxy is in use. + void Set(const b2Vec2* vertices, int32 count, float radius); + + /// Get the supporting vertex index in the given direction. + int32 GetSupport(const b2Vec2& d) const; + + /// Get the supporting vertex in the given direction. + const b2Vec2& GetSupportVertex(const b2Vec2& d) const; + + /// Get the vertex count. + int32 GetVertexCount() const; + + /// Get a vertex by index. Used by b2Distance. + const b2Vec2& GetVertex(int32 index) const; + + b2Vec2 m_buffer[2]; + const b2Vec2* m_vertices; + int32 m_count; + float m_radius; +}; + +/// Used to warm start b2Distance. +/// Set count to zero on first call. +struct B2_API b2SimplexCache +{ + float metric; ///< length or area + uint16 count; + uint8 indexA[3]; ///< vertices on shape A + uint8 indexB[3]; ///< vertices on shape B +}; + +/// Input for b2Distance. +/// You have to option to use the shape radii +/// in the computation. Even +struct B2_API b2DistanceInput +{ + b2DistanceProxy proxyA; + b2DistanceProxy proxyB; + b2Transform transformA; + b2Transform transformB; + bool useRadii; +}; + +/// Output for b2Distance. +struct B2_API b2DistanceOutput +{ + b2Vec2 pointA; ///< closest point on shapeA + b2Vec2 pointB; ///< closest point on shapeB + float distance; + int32 iterations; ///< number of GJK iterations used +}; + +/// Compute the closest points between two shapes. Supports any combination of: +/// b2CircleShape, b2PolygonShape, b2EdgeShape. The simplex cache is input/output. +/// On the first call set b2SimplexCache.count to zero. +B2_API void b2Distance(b2DistanceOutput* output, + b2SimplexCache* cache, + const b2DistanceInput* input); + +/// Input parameters for b2ShapeCast +struct B2_API b2ShapeCastInput +{ + b2DistanceProxy proxyA; + b2DistanceProxy proxyB; + b2Transform transformA; + b2Transform transformB; + b2Vec2 translationB; +}; + +/// Output results for b2ShapeCast +struct B2_API b2ShapeCastOutput +{ + b2Vec2 point; + b2Vec2 normal; + float lambda; + int32 iterations; +}; + +/// Perform a linear shape cast of shape B moving and shape A fixed. Determines the hit point, normal, and translation fraction. +/// @returns true if hit, false if there is no hit or an initial overlap +B2_API bool b2ShapeCast(b2ShapeCastOutput* output, const b2ShapeCastInput* input); + +////////////////////////////////////////////////////////////////////////// + +inline int32 b2DistanceProxy::GetVertexCount() const +{ + return m_count; +} + +inline const b2Vec2& b2DistanceProxy::GetVertex(int32 index) const +{ + b2Assert(0 <= index && index < m_count); + return m_vertices[index]; +} + +inline int32 b2DistanceProxy::GetSupport(const b2Vec2& d) const +{ + int32 bestIndex = 0; + float bestValue = b2Dot(m_vertices[0], d); + for (int32 i = 1; i < m_count; ++i) + { + float value = b2Dot(m_vertices[i], d); + if (value > bestValue) + { + bestIndex = i; + bestValue = value; + } + } + + return bestIndex; +} + +inline const b2Vec2& b2DistanceProxy::GetSupportVertex(const b2Vec2& d) const +{ + int32 bestIndex = 0; + float bestValue = b2Dot(m_vertices[0], d); + for (int32 i = 1; i < m_count; ++i) + { + float value = b2Dot(m_vertices[i], d); + if (value > bestValue) + { + bestIndex = i; + bestValue = value; + } + } + + return m_vertices[bestIndex]; +} + +#endif diff --git a/libs/box2d/b2_distance_joint.h b/libs/box2d/b2_distance_joint.h new file mode 100644 index 00000000..cfc75fef --- /dev/null +++ b/libs/box2d/b2_distance_joint.h @@ -0,0 +1,176 @@ +// MIT License + +// Copyright (c) 2019 Erin Catto + +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: + +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. + +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. + +#ifndef B2_DISTANCE_JOINT_H +#define B2_DISTANCE_JOINT_H + +#include "b2_api.h" +#include "b2_joint.h" + +/// Distance joint definition. This requires defining an anchor point on both +/// bodies and the non-zero distance 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. +struct B2_API b2DistanceJointDef : public b2JointDef +{ + b2DistanceJointDef() + { + type = e_distanceJoint; + localAnchorA.Set(0.0f, 0.0f); + localAnchorB.Set(0.0f, 0.0f); + length = 1.0f; + minLength = 0.0f; + maxLength = FLT_MAX; + stiffness = 0.0f; + damping = 0.0f; + } + + /// Initialize the bodies, anchors, and rest length using world space anchors. + /// The minimum and maximum lengths are set to the rest length. + 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 rest length of this joint. Clamped to a stable minimum value. + float length; + + /// Minimum length. Clamped to a stable minimum value. + float minLength; + + /// Maximum length. Must be greater than or equal to the minimum length. + float maxLength; + + /// The linear stiffness in N/m. + float stiffness; + + /// The linear damping in N*s/m. + float damping; +}; + +/// 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 B2_API 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(float 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. + float GetReactionTorque(float 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 rest length + float GetLength() const { return m_length; } + + /// Set the rest length + /// @returns clamped rest length + float SetLength(float length); + + /// Get the minimum length + float GetMinLength() const { return m_minLength; } + + /// Set the minimum length + /// @returns the clamped minimum length + float SetMinLength(float minLength); + + /// Get the maximum length + float GetMaxLength() const { return m_maxLength; } + + /// Set the maximum length + /// @returns the clamped maximum length + float SetMaxLength(float maxLength); + + /// Get the current length + float GetCurrentLength() const; + + /// Set/get the linear stiffness in N/m + void SetStiffness(float stiffness) { m_stiffness = stiffness; } + float GetStiffness() const { return m_stiffness; } + + /// Set/get linear damping in N*s/m + void SetDamping(float damping) { m_damping = damping; } + float GetDamping() const { return m_damping; } + + /// Dump joint to dmLog + void Dump() override; + + /// + void Draw(b2Draw* draw) const 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; + + float m_stiffness; + float m_damping; + float m_bias; + float m_length; + float m_minLength; + float m_maxLength; + + // Solver shared + b2Vec2 m_localAnchorA; + b2Vec2 m_localAnchorB; + float m_gamma; + float m_impulse; + float m_lowerImpulse; + float m_upperImpulse; + + // Solver temp + int32 m_indexA; + int32 m_indexB; + b2Vec2 m_u; + b2Vec2 m_rA; + b2Vec2 m_rB; + b2Vec2 m_localCenterA; + b2Vec2 m_localCenterB; + float m_currentLength; + float m_invMassA; + float m_invMassB; + float m_invIA; + float m_invIB; + float m_softMass; + float m_mass; +}; + +#endif diff --git a/libs/box2d/b2_draw.h b/libs/box2d/b2_draw.h new file mode 100644 index 00000000..a4d17118 --- /dev/null +++ b/libs/box2d/b2_draw.h @@ -0,0 +1,102 @@ +// MIT License + +// Copyright (c) 2019 Erin Catto + +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: + +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. + +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. + +#ifndef B2_DRAW_H +#define B2_DRAW_H + +#include "b2_api.h" +#include "b2_math.h" + +/// Color for debug drawing. Each value has the range [0,1]. +struct B2_API b2Color +{ + b2Color() {} + b2Color(float rIn, float gIn, float bIn, float aIn = 1.0f) + { + r = rIn; g = gIn; b = bIn; a = aIn; + } + + void Set(float rIn, float gIn, float bIn, float aIn = 1.0f) + { + r = rIn; g = gIn; b = bIn; a = aIn; + } + + float r, g, b, a; +}; + +/// Implement and register this class with a b2World to provide debug drawing of physics +/// entities in your game. +class B2_API b2Draw +{ +public: + b2Draw(); + + virtual ~b2Draw() {} + + enum + { + e_shapeBit = 0x0001, ///< draw shapes + e_jointBit = 0x0002, ///< draw joint connections + e_aabbBit = 0x0004, ///< draw axis aligned bounding boxes + e_pairBit = 0x0008, ///< draw broad-phase pairs + e_centerOfMassBit = 0x0010 ///< draw center of mass frame + }; + + /// Set the drawing flags. + void SetFlags(uint32 flags); + + /// Get the drawing flags. + uint32 GetFlags() const; + + /// Append flags to the current flags. + void AppendFlags(uint32 flags); + + /// Clear flags from the current flags. + void ClearFlags(uint32 flags); + + /// Draw a closed polygon provided in CCW order. + virtual void DrawPolygon(const b2Vec2* vertices, int32 vertexCount, const b2Color& color) = 0; + + /// Draw a solid closed polygon provided in CCW order. + virtual void DrawSolidPolygon(const b2Vec2* vertices, int32 vertexCount, const b2Color& color) = 0; + + /// Draw a circle. + virtual void DrawCircle(const b2Vec2& center, float radius, const b2Color& color) = 0; + + /// Draw a solid circle. + virtual void DrawSolidCircle(const b2Vec2& center, float radius, const b2Vec2& axis, const b2Color& color) = 0; + + /// Draw a line segment. + virtual void DrawSegment(const b2Vec2& p1, const b2Vec2& p2, const b2Color& color) = 0; + + /// Draw a transform. Choose your own length scale. + /// @param xf a transform. + virtual void DrawTransform(const b2Transform& xf) = 0; + + /// Draw a point. + virtual void DrawPoint(const b2Vec2& p, float size, const b2Color& color) = 0; + +protected: + uint32 m_drawFlags; +}; + +#endif diff --git a/libs/Box2D/Collision/b2DynamicTree.h b/libs/box2d/b2_dynamic_tree.h similarity index 77% rename from libs/Box2D/Collision/b2DynamicTree.h rename to libs/box2d/b2_dynamic_tree.h index 97aa461b..b8549191 100644 --- a/libs/Box2D/Collision/b2DynamicTree.h +++ b/libs/box2d/b2_dynamic_tree.h @@ -1,289 +1,308 @@ -/* -* Copyright (c) 2009 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef B2_DYNAMIC_TREE_H -#define B2_DYNAMIC_TREE_H - -#include -#include - -#define b2_nullNode (-1) - -/// A node in the dynamic tree. The client does not interact with this directly. -struct b2TreeNode -{ - bool IsLeaf() const - { - return child1 == b2_nullNode; - } - - /// Enlarged AABB - b2AABB aabb; - - void* userData; - - union - { - int32 parent; - int32 next; - }; - - int32 child1; - int32 child2; - - // leaf = 0, free node = -1 - int32 height; -}; - -/// A dynamic AABB tree broad-phase, inspired by Nathanael Presson's btDbvt. -/// A dynamic tree arranges data in a binary tree to accelerate -/// queries such as volume queries and ray casts. Leafs are proxies -/// with an AABB. In the tree we expand the proxy AABB by b2_fatAABBFactor -/// so that the proxy AABB is bigger than the client object. This allows the client -/// object to move by small amounts without triggering a tree update. -/// -/// Nodes are pooled and relocatable, so we use node indices rather than pointers. -class b2DynamicTree -{ -public: - /// Constructing the tree initializes the node pool. - b2DynamicTree(); - - /// Destroy the tree, freeing the node pool. - ~b2DynamicTree(); - - /// Create a proxy. Provide a tight fitting AABB and a userData pointer. - int32 CreateProxy(const b2AABB& aabb, void* userData); - - /// Destroy a proxy. This asserts if the id is invalid. - void DestroyProxy(int32 proxyId); - - /// Move a proxy with a swepted AABB. If the proxy has moved outside of its fattened AABB, - /// then the proxy is removed from the tree and re-inserted. Otherwise - /// the function returns immediately. - /// @return true if the proxy was re-inserted. - bool MoveProxy(int32 proxyId, const b2AABB& aabb1, const b2Vec2& displacement); - - /// Get proxy user data. - /// @return the proxy user data or 0 if the id is invalid. - void* GetUserData(int32 proxyId) const; - - /// Get the fat AABB for a proxy. - const b2AABB& GetFatAABB(int32 proxyId) const; - - /// Query an AABB for overlapping proxies. The callback class - /// is called for each proxy that overlaps the supplied AABB. - template - void Query(T* callback, const b2AABB& aabb) const; - - /// Ray-cast against the proxies in the tree. This relies on the callback - /// to perform a exact ray-cast in the case were the proxy contains a shape. - /// The callback also performs the any collision filtering. This has performance - /// roughly equal to k * log(n), where k is the number of collisions and n is the - /// number of proxies in the tree. - /// @param input the ray-cast input data. The ray extends from p1 to p1 + maxFraction * (p2 - p1). - /// @param callback a callback class that is called for each proxy that is hit by the ray. - template - void RayCast(T* callback, const b2RayCastInput& input) const; - - /// Validate this tree. For testing. - void Validate() const; - - /// Compute the height of the binary tree in O(N) time. Should not be - /// called often. - int32 GetHeight() const; - - /// Get the maximum balance of an node in the tree. The balance is the difference - /// in height of the two children of a node. - int32 GetMaxBalance() const; - - /// Get the ratio of the sum of the node areas to the root area. - float32 GetAreaRatio() const; - - /// Build an optimal tree. Very expensive. For testing. - void RebuildBottomUp(); - - /// Shift the world origin. Useful for large worlds. - /// The shift formula is: position -= newOrigin - /// @param newOrigin the new origin with respect to the old origin - void ShiftOrigin(const b2Vec2& newOrigin); - -private: - - int32 AllocateNode(); - void FreeNode(int32 node); - - void InsertLeaf(int32 node); - void RemoveLeaf(int32 node); - - int32 Balance(int32 index); - - int32 ComputeHeight() const; - int32 ComputeHeight(int32 nodeId) const; - - void ValidateStructure(int32 index) const; - void ValidateMetrics(int32 index) const; - - int32 m_root; - - b2TreeNode* m_nodes; - int32 m_nodeCount; - int32 m_nodeCapacity; - - int32 m_freeList; - - /// This is used to incrementally traverse the tree for re-balancing. - uint32 m_path; - - int32 m_insertionCount; -}; - -inline void* b2DynamicTree::GetUserData(int32 proxyId) const -{ - b2Assert(0 <= proxyId && proxyId < m_nodeCapacity); - return m_nodes[proxyId].userData; -} - -inline const b2AABB& b2DynamicTree::GetFatAABB(int32 proxyId) const -{ - b2Assert(0 <= proxyId && proxyId < m_nodeCapacity); - return m_nodes[proxyId].aabb; -} - -template -inline void b2DynamicTree::Query(T* callback, const b2AABB& aabb) const -{ - b2GrowableStack stack; - stack.Push(m_root); - - while (stack.GetCount() > 0) - { - int32 nodeId = stack.Pop(); - if (nodeId == b2_nullNode) - { - continue; - } - - const b2TreeNode* node = m_nodes + nodeId; - - if (b2TestOverlap(node->aabb, aabb)) - { - if (node->IsLeaf()) - { - bool proceed = callback->QueryCallback(nodeId); - if (proceed == false) - { - return; - } - } - else - { - stack.Push(node->child1); - stack.Push(node->child2); - } - } - } -} - -template -inline void b2DynamicTree::RayCast(T* callback, const b2RayCastInput& input) const -{ - b2Vec2 p1 = input.p1; - b2Vec2 p2 = input.p2; - b2Vec2 r = p2 - p1; - b2Assert(r.LengthSquared() > 0.0f); - r.Normalize(); - - // v is perpendicular to the segment. - b2Vec2 v = b2Cross(1.0f, r); - b2Vec2 abs_v = b2Abs(v); - - // Separating axis for segment (Gino, p80). - // |dot(v, p1 - c)| > dot(|v|, h) - - float32 maxFraction = input.maxFraction; - - // Build a bounding box for the segment. - b2AABB segmentAABB; - { - b2Vec2 t = p1 + maxFraction * (p2 - p1); - segmentAABB.lowerBound = b2Min(p1, t); - segmentAABB.upperBound = b2Max(p1, t); - } - - b2GrowableStack stack; - stack.Push(m_root); - - while (stack.GetCount() > 0) - { - int32 nodeId = stack.Pop(); - if (nodeId == b2_nullNode) - { - continue; - } - - const b2TreeNode* node = m_nodes + nodeId; - - if (b2TestOverlap(node->aabb, segmentAABB) == false) - { - continue; - } - - // Separating axis for segment (Gino, p80). - // |dot(v, p1 - c)| > dot(|v|, h) - b2Vec2 c = node->aabb.GetCenter(); - b2Vec2 h = node->aabb.GetExtents(); - float32 separation = b2Abs(b2Dot(v, p1 - c)) - b2Dot(abs_v, h); - if (separation > 0.0f) - { - continue; - } - - if (node->IsLeaf()) - { - b2RayCastInput subInput; - subInput.p1 = input.p1; - subInput.p2 = input.p2; - subInput.maxFraction = maxFraction; - - float32 value = callback->RayCastCallback(subInput, nodeId); - - if (value == 0.0f) - { - // The client has terminated the ray cast. - return; - } - - if (value > 0.0f) - { - // Update segment bounding box. - maxFraction = value; - b2Vec2 t = p1 + maxFraction * (p2 - p1); - segmentAABB.lowerBound = b2Min(p1, t); - segmentAABB.upperBound = b2Max(p1, t); - } - } - else - { - stack.Push(node->child1); - stack.Push(node->child2); - } - } -} - -#endif +// MIT License + +// Copyright (c) 2019 Erin Catto + +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: + +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. + +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. + +#ifndef B2_DYNAMIC_TREE_H +#define B2_DYNAMIC_TREE_H + +#include "b2_api.h" +#include "b2_collision.h" +#include "b2_growable_stack.h" + +#define b2_nullNode (-1) + +/// A node in the dynamic tree. The client does not interact with this directly. +struct B2_API b2TreeNode +{ + bool IsLeaf() const + { + return child1 == b2_nullNode; + } + + /// Enlarged AABB + b2AABB aabb; + + void* userData; + + union + { + int32 parent; + int32 next; + }; + + int32 child1; + int32 child2; + + // leaf = 0, free node = -1 + int32 height; + + bool moved; +}; + +/// A dynamic AABB tree broad-phase, inspired by Nathanael Presson's btDbvt. +/// A dynamic tree arranges data in a binary tree to accelerate +/// queries such as volume queries and ray casts. Leafs are proxies +/// with an AABB. In the tree we expand the proxy AABB by b2_fatAABBFactor +/// so that the proxy AABB is bigger than the client object. This allows the client +/// object to move by small amounts without triggering a tree update. +/// +/// Nodes are pooled and relocatable, so we use node indices rather than pointers. +class B2_API b2DynamicTree +{ +public: + /// Constructing the tree initializes the node pool. + b2DynamicTree(); + + /// Destroy the tree, freeing the node pool. + ~b2DynamicTree(); + + /// Create a proxy. Provide a tight fitting AABB and a userData pointer. + int32 CreateProxy(const b2AABB& aabb, void* userData); + + /// Destroy a proxy. This asserts if the id is invalid. + void DestroyProxy(int32 proxyId); + + /// Move a proxy with a swepted AABB. If the proxy has moved outside of its fattened AABB, + /// then the proxy is removed from the tree and re-inserted. Otherwise + /// the function returns immediately. + /// @return true if the proxy was re-inserted. + bool MoveProxy(int32 proxyId, const b2AABB& aabb1, const b2Vec2& displacement); + + /// Get proxy user data. + /// @return the proxy user data or 0 if the id is invalid. + void* GetUserData(int32 proxyId) const; + + bool WasMoved(int32 proxyId) const; + void ClearMoved(int32 proxyId); + + /// Get the fat AABB for a proxy. + const b2AABB& GetFatAABB(int32 proxyId) const; + + /// Query an AABB for overlapping proxies. The callback class + /// is called for each proxy that overlaps the supplied AABB. + template + void Query(T* callback, const b2AABB& aabb) const; + + /// Ray-cast against the proxies in the tree. This relies on the callback + /// to perform a exact ray-cast in the case were the proxy contains a shape. + /// The callback also performs the any collision filtering. This has performance + /// roughly equal to k * log(n), where k is the number of collisions and n is the + /// number of proxies in the tree. + /// @param input the ray-cast input data. The ray extends from p1 to p1 + maxFraction * (p2 - p1). + /// @param callback a callback class that is called for each proxy that is hit by the ray. + template + void RayCast(T* callback, const b2RayCastInput& input) const; + + /// Validate this tree. For testing. + void Validate() const; + + /// Compute the height of the binary tree in O(N) time. Should not be + /// called often. + int32 GetHeight() const; + + /// Get the maximum balance of an node in the tree. The balance is the difference + /// in height of the two children of a node. + int32 GetMaxBalance() const; + + /// Get the ratio of the sum of the node areas to the root area. + float GetAreaRatio() const; + + /// Build an optimal tree. Very expensive. For testing. + void RebuildBottomUp(); + + /// Shift the world origin. Useful for large worlds. + /// The shift formula is: position -= newOrigin + /// @param newOrigin the new origin with respect to the old origin + void ShiftOrigin(const b2Vec2& newOrigin); + +private: + + int32 AllocateNode(); + void FreeNode(int32 node); + + void InsertLeaf(int32 node); + void RemoveLeaf(int32 node); + + int32 Balance(int32 index); + + int32 ComputeHeight() const; + int32 ComputeHeight(int32 nodeId) const; + + void ValidateStructure(int32 index) const; + void ValidateMetrics(int32 index) const; + + int32 m_root; + + b2TreeNode* m_nodes; + int32 m_nodeCount; + int32 m_nodeCapacity; + + int32 m_freeList; + + int32 m_insertionCount; +}; + +inline void* b2DynamicTree::GetUserData(int32 proxyId) const +{ + b2Assert(0 <= proxyId && proxyId < m_nodeCapacity); + return m_nodes[proxyId].userData; +} + +inline bool b2DynamicTree::WasMoved(int32 proxyId) const +{ + b2Assert(0 <= proxyId && proxyId < m_nodeCapacity); + return m_nodes[proxyId].moved; +} + +inline void b2DynamicTree::ClearMoved(int32 proxyId) +{ + b2Assert(0 <= proxyId && proxyId < m_nodeCapacity); + m_nodes[proxyId].moved = false; +} + +inline const b2AABB& b2DynamicTree::GetFatAABB(int32 proxyId) const +{ + b2Assert(0 <= proxyId && proxyId < m_nodeCapacity); + return m_nodes[proxyId].aabb; +} + +template +inline void b2DynamicTree::Query(T* callback, const b2AABB& aabb) const +{ + b2GrowableStack stack; + stack.Push(m_root); + + while (stack.GetCount() > 0) + { + int32 nodeId = stack.Pop(); + if (nodeId == b2_nullNode) + { + continue; + } + + const b2TreeNode* node = m_nodes + nodeId; + + if (b2TestOverlap(node->aabb, aabb)) + { + if (node->IsLeaf()) + { + bool proceed = callback->QueryCallback(nodeId); + if (proceed == false) + { + return; + } + } + else + { + stack.Push(node->child1); + stack.Push(node->child2); + } + } + } +} + +template +inline void b2DynamicTree::RayCast(T* callback, const b2RayCastInput& input) const +{ + b2Vec2 p1 = input.p1; + b2Vec2 p2 = input.p2; + b2Vec2 r = p2 - p1; + b2Assert(r.LengthSquared() > 0.0f); + r.Normalize(); + + // v is perpendicular to the segment. + b2Vec2 v = b2Cross(1.0f, r); + b2Vec2 abs_v = b2Abs(v); + + // Separating axis for segment (Gino, p80). + // |dot(v, p1 - c)| > dot(|v|, h) + + float maxFraction = input.maxFraction; + + // Build a bounding box for the segment. + b2AABB segmentAABB; + { + b2Vec2 t = p1 + maxFraction * (p2 - p1); + segmentAABB.lowerBound = b2Min(p1, t); + segmentAABB.upperBound = b2Max(p1, t); + } + + b2GrowableStack stack; + stack.Push(m_root); + + while (stack.GetCount() > 0) + { + int32 nodeId = stack.Pop(); + if (nodeId == b2_nullNode) + { + continue; + } + + const b2TreeNode* node = m_nodes + nodeId; + + if (b2TestOverlap(node->aabb, segmentAABB) == false) + { + continue; + } + + // Separating axis for segment (Gino, p80). + // |dot(v, p1 - c)| > dot(|v|, h) + b2Vec2 c = node->aabb.GetCenter(); + b2Vec2 h = node->aabb.GetExtents(); + float separation = b2Abs(b2Dot(v, p1 - c)) - b2Dot(abs_v, h); + if (separation > 0.0f) + { + continue; + } + + if (node->IsLeaf()) + { + b2RayCastInput subInput; + subInput.p1 = input.p1; + subInput.p2 = input.p2; + subInput.maxFraction = maxFraction; + + float value = callback->RayCastCallback(subInput, nodeId); + + if (value == 0.0f) + { + // The client has terminated the ray cast. + return; + } + + if (value > 0.0f) + { + // Update segment bounding box. + maxFraction = value; + b2Vec2 t = p1 + maxFraction * (p2 - p1); + segmentAABB.lowerBound = b2Min(p1, t); + segmentAABB.upperBound = b2Max(p1, t); + } + } + else + { + stack.Push(node->child1); + stack.Push(node->child2); + } + } +} + +#endif diff --git a/libs/box2d/b2_edge_shape.h b/libs/box2d/b2_edge_shape.h new file mode 100644 index 00000000..b930ee87 --- /dev/null +++ b/libs/box2d/b2_edge_shape.h @@ -0,0 +1,86 @@ +// MIT License + +// Copyright (c) 2019 Erin Catto + +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: + +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. + +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. + +#ifndef B2_EDGE_SHAPE_H +#define B2_EDGE_SHAPE_H + +#include "b2_api.h" +#include "b2_shape.h" + +/// A line segment (edge) shape. These can be connected in chains or loops +/// to other edge shapes. Edges created independently are two-sided and do +/// no provide smooth movement across junctions. +class B2_API b2EdgeShape : public b2Shape +{ +public: + b2EdgeShape(); + + /// Set this as a part of a sequence. Vertex v0 precedes the edge and vertex v3 + /// follows. These extra vertices are used to provide smooth movement + /// across junctions. This also makes the collision one-sided. The edge + /// normal points to the right looking from v1 to v2. + void SetOneSided(const b2Vec2& v0, const b2Vec2& v1,const b2Vec2& v2, const b2Vec2& v3); + + /// Set this as an isolated edge. Collision is two-sided. + void SetTwoSided(const b2Vec2& v1, const b2Vec2& v2); + + /// Implement b2Shape. + b2Shape* Clone(b2BlockAllocator* allocator) const override; + + /// @see b2Shape::GetChildCount + int32 GetChildCount() const override; + + /// @see b2Shape::TestPoint + bool TestPoint(const b2Transform& transform, const b2Vec2& p) const override; + + /// Implement b2Shape. + bool RayCast(b2RayCastOutput* output, const b2RayCastInput& input, + const b2Transform& transform, int32 childIndex) const override; + + /// @see b2Shape::ComputeAABB + void ComputeAABB(b2AABB* aabb, const b2Transform& transform, int32 childIndex) const override; + + /// @see b2Shape::ComputeMass + void ComputeMass(b2MassData* massData, float density) const override; + + /// These are the edge vertices + b2Vec2 m_vertex1, m_vertex2; + + /// Optional adjacent vertices. These are used for smooth collision. + b2Vec2 m_vertex0, m_vertex3; + + /// Uses m_vertex0 and m_vertex3 to create smooth collision. + bool m_oneSided; +}; + +inline b2EdgeShape::b2EdgeShape() +{ + m_type = e_edge; + m_radius = b2_polygonRadius; + m_vertex0.x = 0.0f; + m_vertex0.y = 0.0f; + m_vertex3.x = 0.0f; + m_vertex3.y = 0.0f; + m_oneSided = false; +} + +#endif diff --git a/libs/Box2D/Dynamics/b2Fixture.h b/libs/box2d/b2_fixture.h similarity index 70% rename from libs/Box2D/Dynamics/b2Fixture.h rename to libs/box2d/b2_fixture.h index 5ebd0c3f..47d321df 100644 --- a/libs/Box2D/Dynamics/b2Fixture.h +++ b/libs/box2d/b2_fixture.h @@ -1,345 +1,371 @@ -/* -* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef B2_FIXTURE_H -#define B2_FIXTURE_H - -#include -#include -#include - -class b2BlockAllocator; -class b2Body; -class b2BroadPhase; -class b2Fixture; - -/// This holds contact filtering data. -struct b2Filter -{ - b2Filter() - { - categoryBits = 0x0001; - maskBits = 0xFFFF; - groupIndex = 0; - } - - /// The collision category bits. Normally you would just set one bit. - uint16 categoryBits; - - /// The collision mask bits. This states the categories that this - /// shape would accept for collision. - uint16 maskBits; - - /// Collision groups allow a certain group of objects to never collide (negative) - /// or always collide (positive). Zero means no collision group. Non-zero group - /// filtering always wins against the mask bits. - int16 groupIndex; -}; - -/// A fixture definition is used to create a fixture. This class defines an -/// abstract fixture definition. You can reuse fixture definitions safely. -struct b2FixtureDef -{ - /// The constructor sets the default fixture definition values. - b2FixtureDef() - { - shape = NULL; - userData = NULL; - friction = 0.2f; - restitution = 0.0f; - density = 0.0f; - isSensor = false; - } - - /// The shape, this must be set. The shape will be cloned, so you - /// can create the shape on the stack. - const b2Shape* shape; - - /// Use this to store application specific fixture data. - void* userData; - - /// The friction coefficient, usually in the range [0,1]. - float32 friction; - - /// The restitution (elasticity) usually in the range [0,1]. - float32 restitution; - - /// The density, usually in kg/m^2. - float32 density; - - /// A sensor shape collects contact information but never generates a collision - /// response. - bool isSensor; - - /// Contact filtering data. - b2Filter filter; -}; - -/// This proxy is used internally to connect fixtures to the broad-phase. -struct b2FixtureProxy -{ - b2AABB aabb; - b2Fixture* fixture; - int32 childIndex; - int32 proxyId; -}; - -/// A fixture is used to attach a shape to a body for collision detection. A fixture -/// inherits its transform from its parent. Fixtures hold additional non-geometric data -/// such as friction, collision filters, etc. -/// Fixtures are created via b2Body::CreateFixture. -/// @warning you cannot reuse fixtures. -class b2Fixture -{ -public: - /// Get the type of the child shape. You can use this to down cast to the concrete shape. - /// @return the shape type. - b2Shape::Type GetType() const; - - /// Get the child shape. You can modify the child shape, however you should not change the - /// number of vertices because this will crash some collision caching mechanisms. - /// Manipulating the shape may lead to non-physical behavior. - b2Shape* GetShape(); - const b2Shape* GetShape() const; - - /// Set if this fixture is a sensor. - void SetSensor(bool sensor); - - /// Is this fixture a sensor (non-solid)? - /// @return the true if the shape is a sensor. - bool IsSensor() const; - - /// Set the contact filtering data. This will not update contacts until the next time - /// step when either parent body is active and awake. - /// This automatically calls Refilter. - void SetFilterData(const b2Filter& filter); - - /// Get the contact filtering data. - const b2Filter& GetFilterData() const; - - /// Call this if you want to establish collision that was previously disabled by b2ContactFilter::ShouldCollide. - void Refilter(); - - /// Get the parent body of this fixture. This is NULL if the fixture is not attached. - /// @return the parent body. - b2Body* GetBody(); - const b2Body* GetBody() const; - - /// Get the next fixture in the parent body's fixture list. - /// @return the next shape. - b2Fixture* GetNext(); - const b2Fixture* GetNext() const; - - /// Get the user data that was assigned in the fixture definition. Use this to - /// store your application specific data. - void* GetUserData() const; - - /// Set the user data. Use this to store your application specific data. - void SetUserData(void* data); - - /// Test a point for containment in this fixture. - /// @param p a point in world coordinates. - bool TestPoint(const b2Vec2& p) const; - - /// Cast a ray against this shape. - /// @param output the ray-cast results. - /// @param input the ray-cast input parameters. - bool RayCast(b2RayCastOutput* output, const b2RayCastInput& input, int32 childIndex) const; - - /// Get the mass data for this fixture. The mass data is based on the density and - /// the shape. The rotational inertia is about the shape's origin. This operation - /// may be expensive. - void GetMassData(b2MassData* massData) const; - - /// Set the density of this fixture. This will _not_ automatically adjust the mass - /// of the body. You must call b2Body::ResetMassData to update the body's mass. - void SetDensity(float32 density); - - /// Get the density of this fixture. - float32 GetDensity() const; - - /// Get the coefficient of friction. - float32 GetFriction() const; - - /// Set the coefficient of friction. This will _not_ change the friction of - /// existing contacts. - void SetFriction(float32 friction); - - /// Get the coefficient of restitution. - float32 GetRestitution() const; - - /// Set the coefficient of restitution. This will _not_ change the restitution of - /// existing contacts. - void SetRestitution(float32 restitution); - - /// Get the fixture's AABB. This AABB may be enlarge and/or stale. - /// If you need a more accurate AABB, compute it using the shape and - /// the body transform. - const b2AABB& GetAABB(int32 childIndex) const; - - /// Dump this fixture to the log file. - void Dump(int32 bodyIndex); - -protected: - - friend class b2Body; - friend class b2World; - friend class b2Contact; - friend class b2ContactManager; - - b2Fixture(); - - // We need separation create/destroy functions from the constructor/destructor because - // the destructor cannot access the allocator (no destructor arguments allowed by C++). - void Create(b2BlockAllocator* allocator, b2Body* body, const b2FixtureDef* def); - void Destroy(b2BlockAllocator* allocator); - - // These support body activation/deactivation. - void CreateProxies(b2BroadPhase* broadPhase, const b2Transform& xf); - void DestroyProxies(b2BroadPhase* broadPhase); - - void Synchronize(b2BroadPhase* broadPhase, const b2Transform& xf1, const b2Transform& xf2); - - float32 m_density; - - b2Fixture* m_next; - b2Body* m_body; - - b2Shape* m_shape; - - float32 m_friction; - float32 m_restitution; - - b2FixtureProxy* m_proxies; - int32 m_proxyCount; - - b2Filter m_filter; - - bool m_isSensor; - - void* m_userData; -}; - -inline b2Shape::Type b2Fixture::GetType() const -{ - return m_shape->GetType(); -} - -inline b2Shape* b2Fixture::GetShape() -{ - return m_shape; -} - -inline const b2Shape* b2Fixture::GetShape() const -{ - return m_shape; -} - -inline bool b2Fixture::IsSensor() const -{ - return m_isSensor; -} - -inline const b2Filter& b2Fixture::GetFilterData() const -{ - return m_filter; -} - -inline void* b2Fixture::GetUserData() const -{ - return m_userData; -} - -inline void b2Fixture::SetUserData(void* data) -{ - m_userData = data; -} - -inline b2Body* b2Fixture::GetBody() -{ - return m_body; -} - -inline const b2Body* b2Fixture::GetBody() const -{ - return m_body; -} - -inline b2Fixture* b2Fixture::GetNext() -{ - return m_next; -} - -inline const b2Fixture* b2Fixture::GetNext() const -{ - return m_next; -} - -inline void b2Fixture::SetDensity(float32 density) -{ - b2Assert(b2IsValid(density) && density >= 0.0f); - m_density = density; -} - -inline float32 b2Fixture::GetDensity() const -{ - return m_density; -} - -inline float32 b2Fixture::GetFriction() const -{ - return m_friction; -} - -inline void b2Fixture::SetFriction(float32 friction) -{ - m_friction = friction; -} - -inline float32 b2Fixture::GetRestitution() const -{ - return m_restitution; -} - -inline void b2Fixture::SetRestitution(float32 restitution) -{ - m_restitution = restitution; -} - -inline bool b2Fixture::TestPoint(const b2Vec2& p) const -{ - return m_shape->TestPoint(m_body->GetTransform(), p); -} - -inline bool b2Fixture::RayCast(b2RayCastOutput* output, const b2RayCastInput& input, int32 childIndex) const -{ - return m_shape->RayCast(output, input, m_body->GetTransform(), childIndex); -} - -inline void b2Fixture::GetMassData(b2MassData* massData) const -{ - m_shape->ComputeMass(massData, m_density); -} - -inline const b2AABB& b2Fixture::GetAABB(int32 childIndex) const -{ - b2Assert(0 <= childIndex && childIndex < m_proxyCount); - return m_proxies[childIndex].aabb; -} - -#endif +// MIT License + +// Copyright (c) 2019 Erin Catto + +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: + +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. + +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. + +#ifndef B2_FIXTURE_H +#define B2_FIXTURE_H + +#include "b2_api.h" +#include "b2_body.h" +#include "b2_collision.h" +#include "b2_shape.h" + +class b2BlockAllocator; +class b2Body; +class b2BroadPhase; +class b2Fixture; + +/// This holds contact filtering data. +struct B2_API 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 B2_API b2FixtureDef +{ + /// The constructor sets the default fixture definition values. + b2FixtureDef() + { + shape = nullptr; + friction = 0.2f; + restitution = 0.0f; + restitutionThreshold = 1.0f * b2_lengthUnitsPerMeter; + 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. + b2FixtureUserData userData; + + /// The friction coefficient, usually in the range [0,1]. + float friction; + + /// The restitution (elasticity) usually in the range [0,1]. + float restitution; + + /// Restitution velocity threshold, usually in m/s. Collisions above this + /// speed have restitution applied (will bounce). + float restitutionThreshold; + + /// The density, usually in kg/m^2. + float 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 B2_API 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 B2_API 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. + b2FixtureUserData& GetUserData(); + const b2FixtureUserData& GetUserData() const; + + /// 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. + /// @param childIndex the child shape index (e.g. edge index) + 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(float density); + + /// Get the density of this fixture. + float GetDensity() const; + + /// Get the coefficient of friction. + float GetFriction() const; + + /// Set the coefficient of friction. This will _not_ change the friction of + /// existing contacts. + void SetFriction(float friction); + + /// Get the coefficient of restitution. + float GetRestitution() const; + + /// Set the coefficient of restitution. This will _not_ change the restitution of + /// existing contacts. + void SetRestitution(float restitution); + + /// Get the restitution velocity threshold. + float GetRestitutionThreshold() const; + + /// Set the restitution threshold. This will _not_ change the restitution threshold of + /// existing contacts. + void SetRestitutionThreshold(float threshold); + + /// 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); + + float m_density; + + b2Fixture* m_next; + b2Body* m_body; + + b2Shape* m_shape; + + float m_friction; + float m_restitution; + float m_restitutionThreshold; + + b2FixtureProxy* m_proxies; + int32 m_proxyCount; + + b2Filter m_filter; + + bool m_isSensor; + + b2FixtureUserData 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 b2FixtureUserData& b2Fixture::GetUserData() +{ + return m_userData; +} + +inline const b2FixtureUserData& b2Fixture::GetUserData() const +{ + return m_userData; +} + +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(float density) +{ + b2Assert(b2IsValid(density) && density >= 0.0f); + m_density = density; +} + +inline float b2Fixture::GetDensity() const +{ + return m_density; +} + +inline float b2Fixture::GetFriction() const +{ + return m_friction; +} + +inline void b2Fixture::SetFriction(float friction) +{ + m_friction = friction; +} + +inline float b2Fixture::GetRestitution() const +{ + return m_restitution; +} + +inline void b2Fixture::SetRestitution(float restitution) +{ + m_restitution = restitution; +} + +inline float b2Fixture::GetRestitutionThreshold() const +{ + return m_restitutionThreshold; +} + +inline void b2Fixture::SetRestitutionThreshold(float threshold) +{ + m_restitutionThreshold = threshold; +} + +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/libs/box2d/b2_friction_joint.h b/libs/box2d/b2_friction_joint.h new file mode 100644 index 00000000..99922c3a --- /dev/null +++ b/libs/box2d/b2_friction_joint.h @@ -0,0 +1,124 @@ +// MIT License + +// Copyright (c) 2019 Erin Catto + +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: + +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. + +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. + +#ifndef B2_FRICTION_JOINT_H +#define B2_FRICTION_JOINT_H + +#include "b2_api.h" +#include "b2_joint.h" + +/// Friction joint definition. +struct B2_API 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. + float maxForce; + + /// The maximum friction torque in N-m. + float maxTorque; +}; + +/// Friction joint. This is used for top-down friction. +/// It provides 2D translational friction and angular friction. +class B2_API b2FrictionJoint : public b2Joint +{ +public: + b2Vec2 GetAnchorA() const override; + b2Vec2 GetAnchorB() const override; + + b2Vec2 GetReactionForce(float inv_dt) const override; + float GetReactionTorque(float 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(float force); + + /// Get the maximum friction force in N. + float GetMaxForce() const; + + /// Set the maximum friction torque in N*m. + void SetMaxTorque(float torque); + + /// Get the maximum friction torque in N*m. + float 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; + float m_angularImpulse; + float m_maxForce; + float m_maxTorque; + + // Solver temp + int32 m_indexA; + int32 m_indexB; + b2Vec2 m_rA; + b2Vec2 m_rB; + b2Vec2 m_localCenterA; + b2Vec2 m_localCenterB; + float m_invMassA; + float m_invMassB; + float m_invIA; + float m_invIB; + b2Mat22 m_linearMass; + float m_angularMass; +}; + +#endif diff --git a/libs/box2d/b2_gear_joint.h b/libs/box2d/b2_gear_joint.h new file mode 100644 index 00000000..a7e4fa51 --- /dev/null +++ b/libs/box2d/b2_gear_joint.h @@ -0,0 +1,131 @@ +// MIT License + +// Copyright (c) 2019 Erin Catto + +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: + +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. + +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. + +#ifndef B2_GEAR_JOINT_H +#define B2_GEAR_JOINT_H + +#include "b2_joint.h" + +/// Gear joint definition. This definition requires two existing +/// revolute or prismatic joints (any combination will work). +/// @warning bodyB on the input joints must both be dynamic +struct B2_API 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. + float 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 B2_API b2GearJoint : public b2Joint +{ +public: + b2Vec2 GetAnchorA() const override; + b2Vec2 GetAnchorB() const override; + + b2Vec2 GetReactionForce(float inv_dt) const override; + float GetReactionTorque(float 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(float ratio); + float 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; + + float m_referenceAngleA; + float m_referenceAngleB; + + float m_constant; + float m_ratio; + float m_tolerance; + + float m_impulse; + + // Solver temp + int32 m_indexA, m_indexB, m_indexC, m_indexD; + b2Vec2 m_lcA, m_lcB, m_lcC, m_lcD; + float m_mA, m_mB, m_mC, m_mD; + float m_iA, m_iB, m_iC, m_iD; + b2Vec2 m_JvAC, m_JvBD; + float m_JwA, m_JwB, m_JwC, m_JwD; + float m_mass; +}; + +#endif diff --git a/libs/box2d/b2_growable_stack.h b/libs/box2d/b2_growable_stack.h new file mode 100644 index 00000000..ec42e5e9 --- /dev/null +++ b/libs/box2d/b2_growable_stack.h @@ -0,0 +1,91 @@ +// MIT License + +// Copyright (c) 2019 Erin Catto + +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: + +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. + +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. + +#ifndef B2_GROWABLE_STACK_H +#define B2_GROWABLE_STACK_H + +#include + +#include "b2_settings.h" + +/// This is a growable LIFO stack with an initial capacity of N. +/// If the stack size exceeds the initial capacity, the heap is used +/// to increase the size of the stack. +template +class b2GrowableStack +{ +public: + b2GrowableStack() + { + m_stack = m_array; + m_count = 0; + m_capacity = N; + } + + ~b2GrowableStack() + { + if (m_stack != m_array) + { + b2Free(m_stack); + m_stack = nullptr; + } + } + + void Push(const T& element) + { + if (m_count == m_capacity) + { + T* old = m_stack; + m_capacity *= 2; + m_stack = (T*)b2Alloc(m_capacity * sizeof(T)); + memcpy(m_stack, old, m_count * sizeof(T)); + if (old != m_array) + { + b2Free(old); + } + } + + m_stack[m_count] = element; + ++m_count; + } + + T Pop() + { + b2Assert(m_count > 0); + --m_count; + return m_stack[m_count]; + } + + int32 GetCount() + { + return m_count; + } + +private: + T* m_stack; + T m_array[N]; + int32 m_count; + int32 m_capacity; +}; + + +#endif diff --git a/libs/Box2D/Dynamics/Joints/b2Joint.h b/libs/box2d/b2_joint.h similarity index 61% rename from libs/Box2D/Dynamics/Joints/b2Joint.h rename to libs/box2d/b2_joint.h index f6002e50..586b13ad 100644 --- a/libs/Box2D/Dynamics/Joints/b2Joint.h +++ b/libs/box2d/b2_joint.h @@ -1,226 +1,233 @@ -/* -* Copyright (c) 2006-2007 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef B2_JOINT_H -#define B2_JOINT_H - -#include - -class b2Body; -class b2Joint; -struct b2SolverData; -class b2BlockAllocator; - -enum b2JointType -{ - e_unknownJoint, - e_revoluteJoint, - e_prismaticJoint, - e_distanceJoint, - e_pulleyJoint, - e_mouseJoint, - e_gearJoint, - e_wheelJoint, - e_weldJoint, - e_frictionJoint, - e_ropeJoint, - e_motorJoint -}; - -enum b2LimitState -{ - e_inactiveLimit, - e_atLowerLimit, - e_atUpperLimit, - e_equalLimits -}; - -struct b2Jacobian -{ - b2Vec2 linear; - float32 angularA; - float32 angularB; -}; - -/// A joint edge is used to connect bodies and joints together -/// in a joint graph where each body is a node and each joint -/// is an edge. A joint edge belongs to a doubly linked list -/// maintained in each attached body. Each joint has two joint -/// nodes, one for each attached body. -struct b2JointEdge -{ - b2Body* other; ///< provides quick access to the other body attached. - b2Joint* joint; ///< the joint - b2JointEdge* prev; ///< the previous joint edge in the body's joint list - b2JointEdge* next; ///< the next joint edge in the body's joint list -}; - -/// Joint definitions are used to construct joints. -struct b2JointDef -{ - b2JointDef() - { - type = e_unknownJoint; - userData = NULL; - bodyA = NULL; - bodyB = NULL; - collideConnected = false; - } - - /// The joint type is set automatically for concrete joint types. - b2JointType type; - - /// Use this to attach application specific data to your joints. - void* userData; - - /// The first attached body. - b2Body* bodyA; - - /// The second attached body. - b2Body* bodyB; - - /// Set this flag to true if the attached bodies should collide. - bool collideConnected; -}; - -/// The base joint class. Joints are used to constraint two bodies together in -/// various fashions. Some joints also feature limits and motors. -class b2Joint -{ -public: - - /// Get the type of the concrete joint. - b2JointType GetType() const; - - /// Get the first body attached to this joint. - b2Body* GetBodyA(); - - /// Get the second body attached to this joint. - b2Body* GetBodyB(); - - /// Get the anchor point on bodyA in world coordinates. - virtual b2Vec2 GetAnchorA() const = 0; - - /// Get the anchor point on bodyB in world coordinates. - virtual b2Vec2 GetAnchorB() const = 0; - - /// Get the reaction force on bodyB at the joint anchor in Newtons. - virtual b2Vec2 GetReactionForce(float32 inv_dt) const = 0; - - /// Get the reaction torque on bodyB in N*m. - virtual float32 GetReactionTorque(float32 inv_dt) const = 0; - - /// Get the next joint the world joint list. - b2Joint* GetNext(); - const b2Joint* GetNext() const; - - /// Get the user data pointer. - void* GetUserData() const; - - /// Set the user data pointer. - void SetUserData(void* data); - - /// Short-cut function to determine if either body is inactive. - bool IsActive() const; - - /// Get collide connected. - /// Note: modifying the collide connect flag won't work correctly because - /// the flag is only checked when fixture AABBs begin to overlap. - bool GetCollideConnected() const; - - /// Dump this joint to the log file. - virtual void Dump() { b2Log("// Dump is not supported for this joint type.\n"); } - - /// Shift the origin for any points stored in world coordinates. - virtual void ShiftOrigin(const b2Vec2& newOrigin) { B2_NOT_USED(newOrigin); } - -protected: - friend class b2World; - friend class b2Body; - friend class b2Island; - friend class b2GearJoint; - - static b2Joint* Create(const b2JointDef* def, b2BlockAllocator* allocator); - static void Destroy(b2Joint* joint, b2BlockAllocator* allocator); - - b2Joint(const b2JointDef* def); - virtual ~b2Joint() {} - - virtual void InitVelocityConstraints(const b2SolverData& data) = 0; - virtual void SolveVelocityConstraints(const b2SolverData& data) = 0; - - // This returns true if the position errors are within tolerance. - virtual bool SolvePositionConstraints(const b2SolverData& data) = 0; - - b2JointType m_type; - b2Joint* m_prev; - b2Joint* m_next; - b2JointEdge m_edgeA; - b2JointEdge m_edgeB; - b2Body* m_bodyA; - b2Body* m_bodyB; - - int32 m_index; - - bool m_islandFlag; - bool m_collideConnected; - - void* m_userData; -}; - -inline b2JointType b2Joint::GetType() const -{ - return m_type; -} - -inline b2Body* b2Joint::GetBodyA() -{ - return m_bodyA; -} - -inline b2Body* b2Joint::GetBodyB() -{ - return m_bodyB; -} - -inline b2Joint* b2Joint::GetNext() -{ - return m_next; -} - -inline const b2Joint* b2Joint::GetNext() const -{ - return m_next; -} - -inline void* b2Joint::GetUserData() const -{ - return m_userData; -} - -inline void b2Joint::SetUserData(void* data) -{ - m_userData = data; -} - -inline bool b2Joint::GetCollideConnected() const -{ - return m_collideConnected; -} - -#endif +// MIT License + +// Copyright (c) 2019 Erin Catto + +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: + +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. + +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. + +#ifndef B2_JOINT_H +#define B2_JOINT_H + +#include "b2_api.h" +#include "b2_math.h" + +class b2Body; +class b2Draw; +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_motorJoint +}; + +struct B2_API b2Jacobian +{ + b2Vec2 linear; + float angularA; + float 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 B2_API 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 B2_API b2JointDef +{ + b2JointDef() + { + type = e_unknownJoint; + 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. + b2JointUserData 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; +}; + +/// Utility to compute linear stiffness values from frequency and damping ratio +B2_API void b2LinearStiffness(float& stiffness, float& damping, + float frequencyHertz, float dampingRatio, + const b2Body* bodyA, const b2Body* bodyB); + +/// Utility to compute rotational stiffness values frequency and damping ratio +B2_API void b2AngularStiffness(float& stiffness, float& damping, + float frequencyHertz, float dampingRatio, + const b2Body* bodyA, const b2Body* bodyB); + +/// The base joint class. Joints are used to constraint two bodies together in +/// various fashions. Some joints also feature limits and motors. +class B2_API 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(float inv_dt) const = 0; + + /// Get the reaction torque on bodyB in N*m. + virtual float GetReactionTorque(float inv_dt) const = 0; + + /// Get the next joint the world joint list. + b2Joint* GetNext(); + const b2Joint* GetNext() const; + + /// Get the user data pointer. + b2JointUserData& GetUserData(); + const b2JointUserData& GetUserData() const; + + /// Short-cut function to determine if either body is enabled. + bool IsEnabled() 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() { b2Dump("// 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); } + + /// Debug draw this joint + virtual void Draw(b2Draw* draw) const; + +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; + + b2JointUserData 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 b2JointUserData& b2Joint::GetUserData() +{ + return m_userData; +} + +inline const b2JointUserData& b2Joint::GetUserData() const +{ + return m_userData; +} + +inline bool b2Joint::GetCollideConnected() const +{ + return m_collideConnected; +} + +#endif diff --git a/libs/Box2D/Common/b2Math.h b/libs/box2d/b2_math.h similarity index 74% rename from libs/Box2D/Common/b2Math.h rename to libs/box2d/b2_math.h index 4c3672b5..c98cf35d 100644 --- a/libs/Box2D/Common/b2Math.h +++ b/libs/box2d/b2_math.h @@ -1,721 +1,717 @@ -/* -* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef B2_MATH_H -#define B2_MATH_H - -#include -#include - -/// This function is used to ensure that a floating point number is not a NaN or infinity. -inline bool b2IsValid(float32 x) -{ - int32* px = reinterpret_cast(&x); - int32 ix = *px; - return (ix & 0x7f800000) != 0x7f800000; -} - -/// This is a approximate yet fast inverse square-root. -inline float32 b2InvSqrt(float32 x) -{ - union - { - float32 x; - int32 i; - } convert; - - convert.x = x; - float32 xhalf = 0.5f * x; - convert.i = 0x5f3759df - (convert.i >> 1); - x = convert.x; - x = x * (1.5f - xhalf * x * x); - return x; -} - -#define b2Sqrt(x) sqrtf(x) -#define b2Atan2(y, x) atan2f(y, x) - -/// A 2D column vector. -struct b2Vec2 -{ - /// Default constructor does nothing (for performance). - b2Vec2() {} - - /// Construct using coordinates. - b2Vec2(float32 x, float32 y) : x(x), y(y) {} - - /// Set this vector to all zeros. - void SetZero() { x = 0.0f; y = 0.0f; } - - /// Set this vector to some specified coordinates. - void Set(float32 x_, float32 y_) { x = x_; y = y_; } - - /// Negate this vector. - b2Vec2 operator -() const { b2Vec2 v; v.Set(-x, -y); return v; } - - /// Read from and indexed element. - float32 operator () (int32 i) const - { - return (&x)[i]; - } - - /// Write to an indexed element. - float32& operator () (int32 i) - { - return (&x)[i]; - } - - /// Add a vector to this vector. - void operator += (const b2Vec2& v) - { - x += v.x; y += v.y; - } - - /// Subtract a vector from this vector. - void operator -= (const b2Vec2& v) - { - x -= v.x; y -= v.y; - } - - /// Multiply this vector by a scalar. - void operator *= (float32 a) - { - x *= a; y *= a; - } - - /// Get the length of this vector (the norm). - float32 Length() const - { - return b2Sqrt(x * x + y * y); - } - - /// Get the length squared. For performance, use this instead of - /// b2Vec2::Length (if possible). - float32 LengthSquared() const - { - return x * x + y * y; - } - - /// Convert this vector into a unit vector. Returns the length. - float32 Normalize() - { - float32 length = Length(); - if (length < b2_epsilon) - { - return 0.0f; - } - float32 invLength = 1.0f / length; - x *= invLength; - y *= invLength; - - return length; - } - - /// Does this vector contain finite coordinates? - bool IsValid() const - { - return b2IsValid(x) && b2IsValid(y); - } - - /// Get the skew vector such that dot(skew_vec, other) == cross(vec, other) - b2Vec2 Skew() const - { - return b2Vec2(-y, x); - } - - float32 x, y; -}; - -/// A 2D column vector with 3 elements. -struct b2Vec3 -{ - /// Default constructor does nothing (for performance). - b2Vec3() {} - - /// Construct using coordinates. - b2Vec3(float32 x, float32 y, float32 z) : x(x), y(y), z(z) {} - - /// Set this vector to all zeros. - void SetZero() { x = 0.0f; y = 0.0f; z = 0.0f; } - - /// Set this vector to some specified coordinates. - void Set(float32 x_, float32 y_, float32 z_) { x = x_; y = y_; z = z_; } - - /// Negate this vector. - b2Vec3 operator -() const { b2Vec3 v; v.Set(-x, -y, -z); return v; } - - /// Add a vector to this vector. - void operator += (const b2Vec3& v) - { - x += v.x; y += v.y; z += v.z; - } - - /// Subtract a vector from this vector. - void operator -= (const b2Vec3& v) - { - x -= v.x; y -= v.y; z -= v.z; - } - - /// Multiply this vector by a scalar. - void operator *= (float32 s) - { - x *= s; y *= s; z *= s; - } - - float32 x, y, z; -}; - -/// A 2-by-2 matrix. Stored in column-major order. -struct b2Mat22 -{ - /// The default constructor does nothing (for performance). - b2Mat22() {} - - /// Construct this matrix using columns. - b2Mat22(const b2Vec2& c1, const b2Vec2& c2) - { - ex = c1; - ey = c2; - } - - /// Construct this matrix using scalars. - b2Mat22(float32 a11, float32 a12, float32 a21, float32 a22) - { - ex.x = a11; ex.y = a21; - ey.x = a12; ey.y = a22; - } - - /// Initialize this matrix using columns. - void Set(const b2Vec2& c1, const b2Vec2& c2) - { - ex = c1; - ey = c2; - } - - /// Set this to the identity matrix. - void SetIdentity() - { - ex.x = 1.0f; ey.x = 0.0f; - ex.y = 0.0f; ey.y = 1.0f; - } - - /// Set this matrix to all zeros. - void SetZero() - { - ex.x = 0.0f; ey.x = 0.0f; - ex.y = 0.0f; ey.y = 0.0f; - } - - b2Mat22 GetInverse() const - { - float32 a = ex.x, b = ey.x, c = ex.y, d = ey.y; - b2Mat22 B; - float32 det = a * d - b * c; - if (det != 0.0f) - { - det = 1.0f / det; - } - B.ex.x = det * d; B.ey.x = -det * b; - B.ex.y = -det * c; B.ey.y = det * a; - return B; - } - - /// Solve A * x = b, where b is a column vector. This is more efficient - /// than computing the inverse in one-shot cases. - b2Vec2 Solve(const b2Vec2& b) const - { - float32 a11 = ex.x, a12 = ey.x, a21 = ex.y, a22 = ey.y; - float32 det = a11 * a22 - a12 * a21; - if (det != 0.0f) - { - det = 1.0f / det; - } - b2Vec2 x; - x.x = det * (a22 * b.x - a12 * b.y); - x.y = det * (a11 * b.y - a21 * b.x); - return x; - } - - b2Vec2 ex, ey; -}; - -/// A 3-by-3 matrix. Stored in column-major order. -struct b2Mat33 -{ - /// The default constructor does nothing (for performance). - b2Mat33() {} - - /// Construct this matrix using columns. - b2Mat33(const b2Vec3& c1, const b2Vec3& c2, const b2Vec3& c3) - { - ex = c1; - ey = c2; - ez = c3; - } - - /// Set this matrix to all zeros. - void SetZero() - { - ex.SetZero(); - ey.SetZero(); - ez.SetZero(); - } - - /// Solve A * x = b, where b is a column vector. This is more efficient - /// than computing the inverse in one-shot cases. - b2Vec3 Solve33(const b2Vec3& b) const; - - /// Solve A * x = b, where b is a column vector. This is more efficient - /// than computing the inverse in one-shot cases. Solve only the upper - /// 2-by-2 matrix equation. - b2Vec2 Solve22(const b2Vec2& b) const; - - /// Get the inverse of this matrix as a 2-by-2. - /// Returns the zero matrix if singular. - void GetInverse22(b2Mat33* M) const; - - /// Get the symmetric inverse of this matrix as a 3-by-3. - /// Returns the zero matrix if singular. - void GetSymInverse33(b2Mat33* M) const; - - b2Vec3 ex, ey, ez; -}; - -/// Rotation -struct b2Rot -{ - b2Rot() {} - - /// Initialize from an angle in radians - explicit b2Rot(float32 angle) - { - /// TODO_ERIN optimize - s = sinf(angle); - c = cosf(angle); - } - - /// Set using an angle in radians. - void Set(float32 angle) - { - /// TODO_ERIN optimize - s = sinf(angle); - c = cosf(angle); - } - - /// Set to the identity rotation - void SetIdentity() - { - s = 0.0f; - c = 1.0f; - } - - /// Get the angle in radians - float32 GetAngle() const - { - return b2Atan2(s, c); - } - - /// Get the x-axis - b2Vec2 GetXAxis() const - { - return b2Vec2(c, s); - } - - /// Get the u-axis - b2Vec2 GetYAxis() const - { - return b2Vec2(-s, c); - } - - /// Sine and cosine - float32 s, c; -}; - -/// A transform contains translation and rotation. It is used to represent -/// the position and orientation of rigid frames. -struct b2Transform -{ - /// The default constructor does nothing. - b2Transform() {} - - /// Initialize using a position vector and a rotation. - b2Transform(const b2Vec2& position, const b2Rot& rotation) : p(position), q(rotation) {} - - /// Set this to the identity transform. - void SetIdentity() - { - p.SetZero(); - q.SetIdentity(); - } - - /// Set this based on the position and angle. - void Set(const b2Vec2& position, float32 angle) - { - p = position; - q.Set(angle); - } - - b2Vec2 p; - b2Rot q; -}; - -/// This describes the motion of a body/shape for TOI computation. -/// Shapes are defined with respect to the body origin, which may -/// no coincide with the center of mass. However, to support dynamics -/// we must interpolate the center of mass position. -struct b2Sweep -{ - /// Get the interpolated transform at a specific time. - /// @param beta is a factor in [0,1], where 0 indicates alpha0. - void GetTransform(b2Transform* xfb, float32 beta) const; - - /// Advance the sweep forward, yielding a new initial state. - /// @param alpha the new initial time. - void Advance(float32 alpha); - - /// Normalize the angles. - void Normalize(); - - b2Vec2 localCenter; ///< local center of mass position - b2Vec2 c0, c; ///< center world positions - float32 a0, a; ///< world angles - - /// Fraction of the current time step in the range [0,1] - /// c0 and a0 are the positions at alpha0. - float32 alpha0; -}; - -/// Useful constant -extern const b2Vec2 b2Vec2_zero; - -/// Perform the dot product on two vectors. -inline float32 b2Dot(const b2Vec2& a, const b2Vec2& b) -{ - return a.x * b.x + a.y * b.y; -} - -/// Perform the cross product on two vectors. In 2D this produces a scalar. -inline float32 b2Cross(const b2Vec2& a, const b2Vec2& b) -{ - return a.x * b.y - a.y * b.x; -} - -/// Perform the cross product on a vector and a scalar. In 2D this produces -/// a vector. -inline b2Vec2 b2Cross(const b2Vec2& a, float32 s) -{ - return b2Vec2(s * a.y, -s * a.x); -} - -/// Perform the cross product on a scalar and a vector. In 2D this produces -/// a vector. -inline b2Vec2 b2Cross(float32 s, const b2Vec2& a) -{ - return b2Vec2(-s * a.y, s * a.x); -} - -/// Multiply a matrix times a vector. If a rotation matrix is provided, -/// then this transforms the vector from one frame to another. -inline b2Vec2 b2Mul(const b2Mat22& A, const b2Vec2& v) -{ - return b2Vec2(A.ex.x * v.x + A.ey.x * v.y, A.ex.y * v.x + A.ey.y * v.y); -} - -/// Multiply a matrix transpose times a vector. If a rotation matrix is provided, -/// then this transforms the vector from one frame to another (inverse transform). -inline b2Vec2 b2MulT(const b2Mat22& A, const b2Vec2& v) -{ - return b2Vec2(b2Dot(v, A.ex), b2Dot(v, A.ey)); -} - -/// Add two vectors component-wise. -inline b2Vec2 operator + (const b2Vec2& a, const b2Vec2& b) -{ - return b2Vec2(a.x + b.x, a.y + b.y); -} - -/// Subtract two vectors component-wise. -inline b2Vec2 operator - (const b2Vec2& a, const b2Vec2& b) -{ - return b2Vec2(a.x - b.x, a.y - b.y); -} - -inline b2Vec2 operator * (float32 s, const b2Vec2& a) -{ - return b2Vec2(s * a.x, s * a.y); -} - -inline bool operator == (const b2Vec2& a, const b2Vec2& b) -{ - return a.x == b.x && a.y == b.y; -} - -inline float32 b2Distance(const b2Vec2& a, const b2Vec2& b) -{ - b2Vec2 c = a - b; - return c.Length(); -} - -inline float32 b2DistanceSquared(const b2Vec2& a, const b2Vec2& b) -{ - b2Vec2 c = a - b; - return b2Dot(c, c); -} - -inline b2Vec3 operator * (float32 s, const b2Vec3& a) -{ - return b2Vec3(s * a.x, s * a.y, s * a.z); -} - -/// Add two vectors component-wise. -inline b2Vec3 operator + (const b2Vec3& a, const b2Vec3& b) -{ - return b2Vec3(a.x + b.x, a.y + b.y, a.z + b.z); -} - -/// Subtract two vectors component-wise. -inline b2Vec3 operator - (const b2Vec3& a, const b2Vec3& b) -{ - return b2Vec3(a.x - b.x, a.y - b.y, a.z - b.z); -} - -/// Perform the dot product on two vectors. -inline float32 b2Dot(const b2Vec3& a, const b2Vec3& b) -{ - return a.x * b.x + a.y * b.y + a.z * b.z; -} - -/// Perform the cross product on two vectors. -inline b2Vec3 b2Cross(const b2Vec3& a, const b2Vec3& b) -{ - return b2Vec3(a.y * b.z - a.z * b.y, a.z * b.x - a.x * b.z, a.x * b.y - a.y * b.x); -} - -inline b2Mat22 operator + (const b2Mat22& A, const b2Mat22& B) -{ - return b2Mat22(A.ex + B.ex, A.ey + B.ey); -} - -// A * B -inline b2Mat22 b2Mul(const b2Mat22& A, const b2Mat22& B) -{ - return b2Mat22(b2Mul(A, B.ex), b2Mul(A, B.ey)); -} - -// A^T * B -inline b2Mat22 b2MulT(const b2Mat22& A, const b2Mat22& B) -{ - b2Vec2 c1(b2Dot(A.ex, B.ex), b2Dot(A.ey, B.ex)); - b2Vec2 c2(b2Dot(A.ex, B.ey), b2Dot(A.ey, B.ey)); - return b2Mat22(c1, c2); -} - -/// Multiply a matrix times a vector. -inline b2Vec3 b2Mul(const b2Mat33& A, const b2Vec3& v) -{ - return v.x * A.ex + v.y * A.ey + v.z * A.ez; -} - -/// Multiply a matrix times a vector. -inline b2Vec2 b2Mul22(const b2Mat33& A, const b2Vec2& v) -{ - return b2Vec2(A.ex.x * v.x + A.ey.x * v.y, A.ex.y * v.x + A.ey.y * v.y); -} - -/// Multiply two rotations: q * r -inline b2Rot b2Mul(const b2Rot& q, const b2Rot& r) -{ - // [qc -qs] * [rc -rs] = [qc*rc-qs*rs -qc*rs-qs*rc] - // [qs qc] [rs rc] [qs*rc+qc*rs -qs*rs+qc*rc] - // s = qs * rc + qc * rs - // c = qc * rc - qs * rs - b2Rot qr; - qr.s = q.s * r.c + q.c * r.s; - qr.c = q.c * r.c - q.s * r.s; - return qr; -} - -/// Transpose multiply two rotations: qT * r -inline b2Rot b2MulT(const b2Rot& q, const b2Rot& r) -{ - // [ qc qs] * [rc -rs] = [qc*rc+qs*rs -qc*rs+qs*rc] - // [-qs qc] [rs rc] [-qs*rc+qc*rs qs*rs+qc*rc] - // s = qc * rs - qs * rc - // c = qc * rc + qs * rs - b2Rot qr; - qr.s = q.c * r.s - q.s * r.c; - qr.c = q.c * r.c + q.s * r.s; - return qr; -} - -/// Rotate a vector -inline b2Vec2 b2Mul(const b2Rot& q, const b2Vec2& v) -{ - return b2Vec2(q.c * v.x - q.s * v.y, q.s * v.x + q.c * v.y); -} - -/// Inverse rotate a vector -inline b2Vec2 b2MulT(const b2Rot& q, const b2Vec2& v) -{ - return b2Vec2(q.c * v.x + q.s * v.y, -q.s * v.x + q.c * v.y); -} - -inline b2Vec2 b2Mul(const b2Transform& T, const b2Vec2& v) -{ - float32 x = (T.q.c * v.x - T.q.s * v.y) + T.p.x; - float32 y = (T.q.s * v.x + T.q.c * v.y) + T.p.y; - - return b2Vec2(x, y); -} - -inline b2Vec2 b2MulT(const b2Transform& T, const b2Vec2& v) -{ - float32 px = v.x - T.p.x; - float32 py = v.y - T.p.y; - float32 x = (T.q.c * px + T.q.s * py); - float32 y = (-T.q.s * px + T.q.c * py); - - return b2Vec2(x, y); -} - -// v2 = A.q.Rot(B.q.Rot(v1) + B.p) + A.p -// = (A.q * B.q).Rot(v1) + A.q.Rot(B.p) + A.p -inline b2Transform b2Mul(const b2Transform& A, const b2Transform& B) -{ - b2Transform C; - C.q = b2Mul(A.q, B.q); - C.p = b2Mul(A.q, B.p) + A.p; - return C; -} - -// v2 = A.q' * (B.q * v1 + B.p - A.p) -// = A.q' * B.q * v1 + A.q' * (B.p - A.p) -inline b2Transform b2MulT(const b2Transform& A, const b2Transform& B) -{ - b2Transform C; - C.q = b2MulT(A.q, B.q); - C.p = b2MulT(A.q, B.p - A.p); - return C; -} - -template -inline T b2Abs(T a) -{ - return a > T(0) ? a : -a; -} - -inline b2Vec2 b2Abs(const b2Vec2& a) -{ - return b2Vec2(b2Abs(a.x), b2Abs(a.y)); -} - -inline b2Mat22 b2Abs(const b2Mat22& A) -{ - return b2Mat22(b2Abs(A.ex), b2Abs(A.ey)); -} - -template -inline T b2Min(T a, T b) -{ - return a < b ? a : b; -} - -inline b2Vec2 b2Min(const b2Vec2& a, const b2Vec2& b) -{ - return b2Vec2(b2Min(a.x, b.x), b2Min(a.y, b.y)); -} - -template -inline T b2Max(T a, T b) -{ - return a > b ? a : b; -} - -inline b2Vec2 b2Max(const b2Vec2& a, const b2Vec2& b) -{ - return b2Vec2(b2Max(a.x, b.x), b2Max(a.y, b.y)); -} - -template -inline T b2Clamp(T a, T low, T high) -{ - return b2Max(low, b2Min(a, high)); -} - -inline b2Vec2 b2Clamp(const b2Vec2& a, const b2Vec2& low, const b2Vec2& high) -{ - return b2Max(low, b2Min(a, high)); -} - -template inline void b2Swap(T& a, T& b) -{ - T tmp = a; - a = b; - b = tmp; -} - -/// "Next Largest Power of 2 -/// Given a binary integer value x, the next largest power of 2 can be computed by a SWAR algorithm -/// that recursively "folds" the upper bits into the lower bits. This process yields a bit vector with -/// the same most significant 1 as x, but all 1's below it. Adding 1 to that value yields the next -/// largest power of 2. For a 32-bit value:" -inline uint32 b2NextPowerOfTwo(uint32 x) -{ - x |= (x >> 1); - x |= (x >> 2); - x |= (x >> 4); - x |= (x >> 8); - x |= (x >> 16); - return x + 1; -} - -inline bool b2IsPowerOfTwo(uint32 x) -{ - bool result = x > 0 && (x & (x - 1)) == 0; - return result; -} - -inline void b2Sweep::GetTransform(b2Transform* xf, float32 beta) const -{ - xf->p = (1.0f - beta) * c0 + beta * c; - float32 angle = (1.0f - beta) * a0 + beta * a; - xf->q.Set(angle); - - // Shift to origin - xf->p -= b2Mul(xf->q, localCenter); -} - -inline void b2Sweep::Advance(float32 alpha) -{ - b2Assert(alpha0 < 1.0f); - float32 beta = (alpha - alpha0) / (1.0f - alpha0); - c0 += beta * (c - c0); - a0 += beta * (a - a0); - alpha0 = alpha; -} - -/// Normalize an angle in radians to be between -pi and pi -inline void b2Sweep::Normalize() -{ - float32 twoPi = 2.0f * b2_pi; - float32 d = twoPi * floorf(a0 / twoPi); - a0 -= d; - a -= d; -} - -#endif +// MIT License + +// Copyright (c) 2019 Erin Catto + +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: + +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. + +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. + +#ifndef B2_MATH_H +#define B2_MATH_H + +#include + +#include "b2_api.h" +#include "b2_settings.h" + +/// This function is used to ensure that a floating point number is not a NaN or infinity. +inline bool b2IsValid(float x) +{ + return isfinite(x); +} + +#define b2Sqrt(x) sqrtf(x) +#define b2Atan2(y, x) atan2f(y, x) + +/// A 2D column vector. +struct B2_API b2Vec2 +{ + /// Default constructor does nothing (for performance). + b2Vec2() = default; + + /// Construct using coordinates. + b2Vec2(float xIn, float yIn) : x(xIn), y(yIn) {} + + /// Set this vector to all zeros. + void SetZero() { x = 0.0f; y = 0.0f; } + + /// Set this vector to some specified coordinates. + void Set(float x_, float y_) { x = x_; y = y_; } + + /// Negate this vector. + b2Vec2 operator -() const { b2Vec2 v; v.Set(-x, -y); return v; } + + /// Read from and indexed element. + float operator () (int32 i) const + { + return (&x)[i]; + } + + /// Write to an indexed element. + float& operator () (int32 i) + { + return (&x)[i]; + } + + /// Add a vector to this vector. + void operator += (const b2Vec2& v) + { + x += v.x; y += v.y; + } + + /// Subtract a vector from this vector. + void operator -= (const b2Vec2& v) + { + x -= v.x; y -= v.y; + } + + /// Multiply this vector by a scalar. + void operator *= (float a) + { + x *= a; y *= a; + } + + /// Get the length of this vector (the norm). + float Length() const + { + return b2Sqrt(x * x + y * y); + } + + /// Get the length squared. For performance, use this instead of + /// b2Vec2::Length (if possible). + float LengthSquared() const + { + return x * x + y * y; + } + + /// Convert this vector into a unit vector. Returns the length. + float Normalize() + { + float length = Length(); + if (length < b2_epsilon) + { + return 0.0f; + } + float invLength = 1.0f / length; + x *= invLength; + y *= invLength; + + return length; + } + + /// Does this vector contain finite coordinates? + bool IsValid() const + { + return b2IsValid(x) && b2IsValid(y); + } + + /// Get the skew vector such that dot(skew_vec, other) == cross(vec, other) + b2Vec2 Skew() const + { + return b2Vec2(-y, x); + } + + float x, y; +}; + +/// A 2D column vector with 3 elements. +struct B2_API b2Vec3 +{ + /// Default constructor does nothing (for performance). + b2Vec3() = default; + + /// Construct using coordinates. + b2Vec3(float xIn, float yIn, float zIn) : x(xIn), y(yIn), z(zIn) {} + + /// Set this vector to all zeros. + void SetZero() { x = 0.0f; y = 0.0f; z = 0.0f; } + + /// Set this vector to some specified coordinates. + void Set(float x_, float y_, float z_) { x = x_; y = y_; z = z_; } + + /// Negate this vector. + b2Vec3 operator -() const { b2Vec3 v; v.Set(-x, -y, -z); return v; } + + /// Add a vector to this vector. + void operator += (const b2Vec3& v) + { + x += v.x; y += v.y; z += v.z; + } + + /// Subtract a vector from this vector. + void operator -= (const b2Vec3& v) + { + x -= v.x; y -= v.y; z -= v.z; + } + + /// Multiply this vector by a scalar. + void operator *= (float s) + { + x *= s; y *= s; z *= s; + } + + float x, y, z; +}; + +/// A 2-by-2 matrix. Stored in column-major order. +struct B2_API b2Mat22 +{ + /// The default constructor does nothing (for performance). + b2Mat22() = default; + + /// Construct this matrix using columns. + b2Mat22(const b2Vec2& c1, const b2Vec2& c2) + { + ex = c1; + ey = c2; + } + + /// Construct this matrix using scalars. + b2Mat22(float a11, float a12, float a21, float a22) + { + ex.x = a11; ex.y = a21; + ey.x = a12; ey.y = a22; + } + + /// Initialize this matrix using columns. + void Set(const b2Vec2& c1, const b2Vec2& c2) + { + ex = c1; + ey = c2; + } + + /// Set this to the identity matrix. + void SetIdentity() + { + ex.x = 1.0f; ey.x = 0.0f; + ex.y = 0.0f; ey.y = 1.0f; + } + + /// Set this matrix to all zeros. + void SetZero() + { + ex.x = 0.0f; ey.x = 0.0f; + ex.y = 0.0f; ey.y = 0.0f; + } + + b2Mat22 GetInverse() const + { + float a = ex.x, b = ey.x, c = ex.y, d = ey.y; + b2Mat22 B; + float det = a * d - b * c; + if (det != 0.0f) + { + det = 1.0f / det; + } + B.ex.x = det * d; B.ey.x = -det * b; + B.ex.y = -det * c; B.ey.y = det * a; + return B; + } + + /// Solve A * x = b, where b is a column vector. This is more efficient + /// than computing the inverse in one-shot cases. + b2Vec2 Solve(const b2Vec2& b) const + { + float a11 = ex.x, a12 = ey.x, a21 = ex.y, a22 = ey.y; + float det = a11 * a22 - a12 * a21; + if (det != 0.0f) + { + det = 1.0f / det; + } + b2Vec2 x; + x.x = det * (a22 * b.x - a12 * b.y); + x.y = det * (a11 * b.y - a21 * b.x); + return x; + } + + b2Vec2 ex, ey; +}; + +/// A 3-by-3 matrix. Stored in column-major order. +struct B2_API b2Mat33 +{ + /// The default constructor does nothing (for performance). + b2Mat33() = default; + + /// Construct this matrix using columns. + b2Mat33(const b2Vec3& c1, const b2Vec3& c2, const b2Vec3& c3) + { + ex = c1; + ey = c2; + ez = c3; + } + + /// Set this matrix to all zeros. + void SetZero() + { + ex.SetZero(); + ey.SetZero(); + ez.SetZero(); + } + + /// Solve A * x = b, where b is a column vector. This is more efficient + /// than computing the inverse in one-shot cases. + b2Vec3 Solve33(const b2Vec3& b) const; + + /// Solve A * x = b, where b is a column vector. This is more efficient + /// than computing the inverse in one-shot cases. Solve only the upper + /// 2-by-2 matrix equation. + b2Vec2 Solve22(const b2Vec2& b) const; + + /// Get the inverse of this matrix as a 2-by-2. + /// Returns the zero matrix if singular. + void GetInverse22(b2Mat33* M) const; + + /// Get the symmetric inverse of this matrix as a 3-by-3. + /// Returns the zero matrix if singular. + void GetSymInverse33(b2Mat33* M) const; + + b2Vec3 ex, ey, ez; +}; + +/// Rotation +struct B2_API b2Rot +{ + b2Rot() = default; + + /// Initialize from an angle in radians + explicit b2Rot(float angle) + { + /// TODO_ERIN optimize + s = sinf(angle); + c = cosf(angle); + } + + /// Set using an angle in radians. + void Set(float angle) + { + /// TODO_ERIN optimize + s = sinf(angle); + c = cosf(angle); + } + + /// Set to the identity rotation + void SetIdentity() + { + s = 0.0f; + c = 1.0f; + } + + /// Get the angle in radians + float GetAngle() const + { + return b2Atan2(s, c); + } + + /// Get the x-axis + b2Vec2 GetXAxis() const + { + return b2Vec2(c, s); + } + + /// Get the u-axis + b2Vec2 GetYAxis() const + { + return b2Vec2(-s, c); + } + + /// Sine and cosine + float s, c; +}; + +/// A transform contains translation and rotation. It is used to represent +/// the position and orientation of rigid frames. +struct B2_API b2Transform +{ + /// The default constructor does nothing. + b2Transform() = default; + + /// Initialize using a position vector and a rotation. + b2Transform(const b2Vec2& position, const b2Rot& rotation) : p(position), q(rotation) {} + + /// Set this to the identity transform. + void SetIdentity() + { + p.SetZero(); + q.SetIdentity(); + } + + /// Set this based on the position and angle. + void Set(const b2Vec2& position, float angle) + { + p = position; + q.Set(angle); + } + + b2Vec2 p; + b2Rot q; +}; + +/// This describes the motion of a body/shape for TOI computation. +/// Shapes are defined with respect to the body origin, which may +/// no coincide with the center of mass. However, to support dynamics +/// we must interpolate the center of mass position. +struct B2_API b2Sweep +{ + b2Sweep() = default; + + /// Get the interpolated transform at a specific time. + /// @param transform the output transform + /// @param beta is a factor in [0,1], where 0 indicates alpha0. + void GetTransform(b2Transform* transform, float beta) const; + + /// Advance the sweep forward, yielding a new initial state. + /// @param alpha the new initial time. + void Advance(float alpha); + + /// Normalize the angles. + void Normalize(); + + b2Vec2 localCenter; ///< local center of mass position + b2Vec2 c0, c; ///< center world positions + float a0, a; ///< world angles + + /// Fraction of the current time step in the range [0,1] + /// c0 and a0 are the positions at alpha0. + float alpha0; +}; + +/// Useful constant +extern B2_API const b2Vec2 b2Vec2_zero; + +/// Perform the dot product on two vectors. +inline float b2Dot(const b2Vec2& a, const b2Vec2& b) +{ + return a.x * b.x + a.y * b.y; +} + +/// Perform the cross product on two vectors. In 2D this produces a scalar. +inline float b2Cross(const b2Vec2& a, const b2Vec2& b) +{ + return a.x * b.y - a.y * b.x; +} + +/// Perform the cross product on a vector and a scalar. In 2D this produces +/// a vector. +inline b2Vec2 b2Cross(const b2Vec2& a, float s) +{ + return b2Vec2(s * a.y, -s * a.x); +} + +/// Perform the cross product on a scalar and a vector. In 2D this produces +/// a vector. +inline b2Vec2 b2Cross(float s, const b2Vec2& a) +{ + return b2Vec2(-s * a.y, s * a.x); +} + +/// Multiply a matrix times a vector. If a rotation matrix is provided, +/// then this transforms the vector from one frame to another. +inline b2Vec2 b2Mul(const b2Mat22& A, const b2Vec2& v) +{ + return b2Vec2(A.ex.x * v.x + A.ey.x * v.y, A.ex.y * v.x + A.ey.y * v.y); +} + +/// Multiply a matrix transpose times a vector. If a rotation matrix is provided, +/// then this transforms the vector from one frame to another (inverse transform). +inline b2Vec2 b2MulT(const b2Mat22& A, const b2Vec2& v) +{ + return b2Vec2(b2Dot(v, A.ex), b2Dot(v, A.ey)); +} + +/// Add two vectors component-wise. +inline b2Vec2 operator + (const b2Vec2& a, const b2Vec2& b) +{ + return b2Vec2(a.x + b.x, a.y + b.y); +} + +/// Subtract two vectors component-wise. +inline b2Vec2 operator - (const b2Vec2& a, const b2Vec2& b) +{ + return b2Vec2(a.x - b.x, a.y - b.y); +} + +inline b2Vec2 operator * (float s, const b2Vec2& a) +{ + return b2Vec2(s * a.x, s * a.y); +} + +inline bool operator == (const b2Vec2& a, const b2Vec2& b) +{ + return a.x == b.x && a.y == b.y; +} + +inline bool operator != (const b2Vec2& a, const b2Vec2& b) +{ + return a.x != b.x || a.y != b.y; +} + +inline float b2Distance(const b2Vec2& a, const b2Vec2& b) +{ + b2Vec2 c = a - b; + return c.Length(); +} + +inline float b2DistanceSquared(const b2Vec2& a, const b2Vec2& b) +{ + b2Vec2 c = a - b; + return b2Dot(c, c); +} + +inline b2Vec3 operator * (float s, const b2Vec3& a) +{ + return b2Vec3(s * a.x, s * a.y, s * a.z); +} + +/// Add two vectors component-wise. +inline b2Vec3 operator + (const b2Vec3& a, const b2Vec3& b) +{ + return b2Vec3(a.x + b.x, a.y + b.y, a.z + b.z); +} + +/// Subtract two vectors component-wise. +inline b2Vec3 operator - (const b2Vec3& a, const b2Vec3& b) +{ + return b2Vec3(a.x - b.x, a.y - b.y, a.z - b.z); +} + +/// Perform the dot product on two vectors. +inline float b2Dot(const b2Vec3& a, const b2Vec3& b) +{ + return a.x * b.x + a.y * b.y + a.z * b.z; +} + +/// Perform the cross product on two vectors. +inline b2Vec3 b2Cross(const b2Vec3& a, const b2Vec3& b) +{ + return b2Vec3(a.y * b.z - a.z * b.y, a.z * b.x - a.x * b.z, a.x * b.y - a.y * b.x); +} + +inline b2Mat22 operator + (const b2Mat22& A, const b2Mat22& B) +{ + return b2Mat22(A.ex + B.ex, A.ey + B.ey); +} + +// A * B +inline b2Mat22 b2Mul(const b2Mat22& A, const b2Mat22& B) +{ + return b2Mat22(b2Mul(A, B.ex), b2Mul(A, B.ey)); +} + +// A^T * B +inline b2Mat22 b2MulT(const b2Mat22& A, const b2Mat22& B) +{ + b2Vec2 c1(b2Dot(A.ex, B.ex), b2Dot(A.ey, B.ex)); + b2Vec2 c2(b2Dot(A.ex, B.ey), b2Dot(A.ey, B.ey)); + return b2Mat22(c1, c2); +} + +/// Multiply a matrix times a vector. +inline b2Vec3 b2Mul(const b2Mat33& A, const b2Vec3& v) +{ + return v.x * A.ex + v.y * A.ey + v.z * A.ez; +} + +/// Multiply a matrix times a vector. +inline b2Vec2 b2Mul22(const b2Mat33& A, const b2Vec2& v) +{ + return b2Vec2(A.ex.x * v.x + A.ey.x * v.y, A.ex.y * v.x + A.ey.y * v.y); +} + +/// Multiply two rotations: q * r +inline b2Rot b2Mul(const b2Rot& q, const b2Rot& r) +{ + // [qc -qs] * [rc -rs] = [qc*rc-qs*rs -qc*rs-qs*rc] + // [qs qc] [rs rc] [qs*rc+qc*rs -qs*rs+qc*rc] + // s = qs * rc + qc * rs + // c = qc * rc - qs * rs + b2Rot qr; + qr.s = q.s * r.c + q.c * r.s; + qr.c = q.c * r.c - q.s * r.s; + return qr; +} + +/// Transpose multiply two rotations: qT * r +inline b2Rot b2MulT(const b2Rot& q, const b2Rot& r) +{ + // [ qc qs] * [rc -rs] = [qc*rc+qs*rs -qc*rs+qs*rc] + // [-qs qc] [rs rc] [-qs*rc+qc*rs qs*rs+qc*rc] + // s = qc * rs - qs * rc + // c = qc * rc + qs * rs + b2Rot qr; + qr.s = q.c * r.s - q.s * r.c; + qr.c = q.c * r.c + q.s * r.s; + return qr; +} + +/// Rotate a vector +inline b2Vec2 b2Mul(const b2Rot& q, const b2Vec2& v) +{ + return b2Vec2(q.c * v.x - q.s * v.y, q.s * v.x + q.c * v.y); +} + +/// Inverse rotate a vector +inline b2Vec2 b2MulT(const b2Rot& q, const b2Vec2& v) +{ + return b2Vec2(q.c * v.x + q.s * v.y, -q.s * v.x + q.c * v.y); +} + +inline b2Vec2 b2Mul(const b2Transform& T, const b2Vec2& v) +{ + float x = (T.q.c * v.x - T.q.s * v.y) + T.p.x; + float y = (T.q.s * v.x + T.q.c * v.y) + T.p.y; + + return b2Vec2(x, y); +} + +inline b2Vec2 b2MulT(const b2Transform& T, const b2Vec2& v) +{ + float px = v.x - T.p.x; + float py = v.y - T.p.y; + float x = (T.q.c * px + T.q.s * py); + float y = (-T.q.s * px + T.q.c * py); + + return b2Vec2(x, y); +} + +// v2 = A.q.Rot(B.q.Rot(v1) + B.p) + A.p +// = (A.q * B.q).Rot(v1) + A.q.Rot(B.p) + A.p +inline b2Transform b2Mul(const b2Transform& A, const b2Transform& B) +{ + b2Transform C; + C.q = b2Mul(A.q, B.q); + C.p = b2Mul(A.q, B.p) + A.p; + return C; +} + +// v2 = A.q' * (B.q * v1 + B.p - A.p) +// = A.q' * B.q * v1 + A.q' * (B.p - A.p) +inline b2Transform b2MulT(const b2Transform& A, const b2Transform& B) +{ + b2Transform C; + C.q = b2MulT(A.q, B.q); + C.p = b2MulT(A.q, B.p - A.p); + return C; +} + +template +inline T b2Abs(T a) +{ + return a > T(0) ? a : -a; +} + +inline b2Vec2 b2Abs(const b2Vec2& a) +{ + return b2Vec2(b2Abs(a.x), b2Abs(a.y)); +} + +inline b2Mat22 b2Abs(const b2Mat22& A) +{ + return b2Mat22(b2Abs(A.ex), b2Abs(A.ey)); +} + +template +inline T b2Min(T a, T b) +{ + return a < b ? a : b; +} + +inline b2Vec2 b2Min(const b2Vec2& a, const b2Vec2& b) +{ + return b2Vec2(b2Min(a.x, b.x), b2Min(a.y, b.y)); +} + +template +inline T b2Max(T a, T b) +{ + return a > b ? a : b; +} + +inline b2Vec2 b2Max(const b2Vec2& a, const b2Vec2& b) +{ + return b2Vec2(b2Max(a.x, b.x), b2Max(a.y, b.y)); +} + +template +inline T b2Clamp(T a, T low, T high) +{ + return b2Max(low, b2Min(a, high)); +} + +inline b2Vec2 b2Clamp(const b2Vec2& a, const b2Vec2& low, const b2Vec2& high) +{ + return b2Max(low, b2Min(a, high)); +} + +template inline void b2Swap(T& a, T& b) +{ + T tmp = a; + a = b; + b = tmp; +} + +/// "Next Largest Power of 2 +/// Given a binary integer value x, the next largest power of 2 can be computed by a SWAR algorithm +/// that recursively "folds" the upper bits into the lower bits. This process yields a bit vector with +/// the same most significant 1 as x, but all 1's below it. Adding 1 to that value yields the next +/// largest power of 2. For a 32-bit value:" +inline uint32 b2NextPowerOfTwo(uint32 x) +{ + x |= (x >> 1); + x |= (x >> 2); + x |= (x >> 4); + x |= (x >> 8); + x |= (x >> 16); + return x + 1; +} + +inline bool b2IsPowerOfTwo(uint32 x) +{ + bool result = x > 0 && (x & (x - 1)) == 0; + return result; +} + +// https://fgiesen.wordpress.com/2012/08/15/linear-interpolation-past-present-and-future/ +inline void b2Sweep::GetTransform(b2Transform* xf, float beta) const +{ + xf->p = (1.0f - beta) * c0 + beta * c; + float angle = (1.0f - beta) * a0 + beta * a; + xf->q.Set(angle); + + // Shift to origin + xf->p -= b2Mul(xf->q, localCenter); +} + +inline void b2Sweep::Advance(float alpha) +{ + b2Assert(alpha0 < 1.0f); + float beta = (alpha - alpha0) / (1.0f - alpha0); + c0 += beta * (c - c0); + a0 += beta * (a - a0); + alpha0 = alpha; +} + +/// Normalize an angle in radians to be between -pi and pi +inline void b2Sweep::Normalize() +{ + float twoPi = 2.0f * b2_pi; + float d = twoPi * floorf(a0 / twoPi); + a0 -= d; + a -= d; +} + +#endif diff --git a/libs/box2d/b2_motor_joint.h b/libs/box2d/b2_motor_joint.h new file mode 100644 index 00000000..c88115f2 --- /dev/null +++ b/libs/box2d/b2_motor_joint.h @@ -0,0 +1,138 @@ +// MIT License + +// Copyright (c) 2019 Erin Catto + +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: + +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. + +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. + +#ifndef B2_MOTOR_JOINT_H +#define B2_MOTOR_JOINT_H + +#include "b2_api.h" +#include "b2_joint.h" + +/// Motor joint definition. +struct B2_API 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. + float angularOffset; + + /// The maximum motor force in N. + float maxForce; + + /// The maximum motor torque in N-m. + float maxTorque; + + /// Position correction factor in the range [0,1]. + float 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 B2_API b2MotorJoint : public b2Joint +{ +public: + b2Vec2 GetAnchorA() const override; + b2Vec2 GetAnchorB() const override; + + b2Vec2 GetReactionForce(float inv_dt) const override; + float GetReactionTorque(float 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(float angularOffset); + float GetAngularOffset() const; + + /// Set the maximum friction force in N. + void SetMaxForce(float force); + + /// Get the maximum friction force in N. + float GetMaxForce() const; + + /// Set the maximum friction torque in N*m. + void SetMaxTorque(float torque); + + /// Get the maximum friction torque in N*m. + float GetMaxTorque() const; + + /// Set the position correction factor in the range [0,1]. + void SetCorrectionFactor(float factor); + + /// Get the position correction factor in the range [0,1]. + float 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; + float m_angularOffset; + b2Vec2 m_linearImpulse; + float m_angularImpulse; + float m_maxForce; + float m_maxTorque; + float 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; + float m_angularError; + float m_invMassA; + float m_invMassB; + float m_invIA; + float m_invIB; + b2Mat22 m_linearMass; + float m_angularMass; +}; + +#endif diff --git a/libs/box2d/b2_mouse_joint.h b/libs/box2d/b2_mouse_joint.h new file mode 100644 index 00000000..fcbc56a7 --- /dev/null +++ b/libs/box2d/b2_mouse_joint.h @@ -0,0 +1,134 @@ +// MIT License + +// Copyright (c) 2019 Erin Catto + +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: + +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. + +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. + +#ifndef B2_MOUSE_JOINT_H +#define B2_MOUSE_JOINT_H + +#include "b2_api.h" +#include "b2_joint.h" + +/// Mouse joint definition. This requires a world target point, +/// tuning parameters, and the time step. +struct B2_API b2MouseJointDef : public b2JointDef +{ + b2MouseJointDef() + { + type = e_mouseJoint; + target.Set(0.0f, 0.0f); + maxForce = 0.0f; + stiffness = 0.0f; + damping = 0.0f; + } + + /// 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). + float maxForce; + + /// The linear stiffness in N/m + float stiffness; + + /// The linear damping in N*s/m + float damping; +}; + +/// 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 B2_API b2MouseJoint : public b2Joint +{ +public: + + /// Implements b2Joint. + b2Vec2 GetAnchorA() const override; + + /// Implements b2Joint. + b2Vec2 GetAnchorB() const override; + + /// Implements b2Joint. + b2Vec2 GetReactionForce(float inv_dt) const override; + + /// Implements b2Joint. + float GetReactionTorque(float 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(float force); + float GetMaxForce() const; + + /// Set/get the linear stiffness in N/m + void SetStiffness(float stiffness) { m_stiffness = stiffness; } + float GetStiffness() const { return m_stiffness; } + + /// Set/get linear damping in N*s/m + void SetDamping(float damping) { m_damping = damping; } + float GetDamping() const { return m_damping; } + + /// 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; + float m_stiffness; + float m_damping; + float m_beta; + + // Solver shared + b2Vec2 m_impulse; + float m_maxForce; + float m_gamma; + + // Solver temp + int32 m_indexA; + int32 m_indexB; + b2Vec2 m_rB; + b2Vec2 m_localCenterB; + float m_invMassB; + float m_invIB; + b2Mat22 m_mass; + b2Vec2 m_C; +}; + +#endif diff --git a/libs/box2d/b2_polygon_shape.h b/libs/box2d/b2_polygon_shape.h new file mode 100644 index 00000000..8a208b72 --- /dev/null +++ b/libs/box2d/b2_polygon_shape.h @@ -0,0 +1,93 @@ +// MIT License + +// Copyright (c) 2019 Erin Catto + +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: + +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. + +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +#ifndef B2_POLYGON_SHAPE_H +#define B2_POLYGON_SHAPE_H + +#include "b2_api.h" +#include "b2_shape.h" + +struct b2Hull; + +/// A solid convex polygon. It is assumed that the interior of the polygon is to +/// the left of each edge. +/// Polygons have a maximum number of vertices equal to b2_maxPolygonVertices. +/// In most cases you should not need many vertices for a convex polygon. +class B2_API b2PolygonShape : public b2Shape +{ +public: + b2PolygonShape(); + + /// Implement b2Shape. + b2Shape* Clone(b2BlockAllocator* allocator) const override; + + /// @see b2Shape::GetChildCount + int32 GetChildCount() const override; + + /// Create a convex hull from the given array of local points. + /// The count must be in the range [3, b2_maxPolygonVertices]. + /// @warning the points may be re-ordered, even if they form a convex polygon + /// @warning if this fails then the polygon is invalid + /// @returns true if valid + bool Set(const b2Vec2* points, int32 count); + + /// Create a polygon from a given convex hull (see b2ComputeHull). + /// @warning the hull must be valid or this will crash or have unexpected behavior + void Set(const b2Hull& hull); + + /// Build vertices to represent an axis-aligned box centered on the local origin. + /// @param hx the half-width. + /// @param hy the half-height. + void SetAsBox(float hx, float hy); + + /// Build vertices to represent an oriented box. + /// @param hx the half-width. + /// @param hy the half-height. + /// @param center the center of the box in local coordinates. + /// @param angle the rotation of the box in local coordinates. + void SetAsBox(float hx, float hy, const b2Vec2& center, float angle); + + /// @see b2Shape::TestPoint + bool TestPoint(const b2Transform& transform, const b2Vec2& p) const override; + + /// Implement b2Shape. + /// @note because the polygon is solid, rays that start inside do not hit because the normal is + /// not defined. + bool RayCast(b2RayCastOutput* output, const b2RayCastInput& input, + const b2Transform& transform, int32 childIndex) const override; + + /// @see b2Shape::ComputeAABB + void ComputeAABB(b2AABB* aabb, const b2Transform& transform, int32 childIndex) const override; + + /// @see b2Shape::ComputeMass + void ComputeMass(b2MassData* massData, float density) const override; + + /// Validate convexity. This is a very time consuming operation. + /// @returns true if valid + bool Validate() const; + + b2Vec2 m_centroid; + b2Vec2 m_vertices[b2_maxPolygonVertices]; + b2Vec2 m_normals[b2_maxPolygonVertices]; + int32 m_count; +}; + +#endif diff --git a/libs/Box2D/Dynamics/Joints/b2PrismaticJoint.h b/libs/box2d/b2_prismatic_joint.h similarity index 56% rename from libs/Box2D/Dynamics/Joints/b2PrismaticJoint.h rename to libs/box2d/b2_prismatic_joint.h index 8d0f3422..9d12d212 100644 --- a/libs/Box2D/Dynamics/Joints/b2PrismaticJoint.h +++ b/libs/box2d/b2_prismatic_joint.h @@ -1,196 +1,205 @@ -/* -* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef B2_PRISMATIC_JOINT_H -#define B2_PRISMATIC_JOINT_H - -#include - -/// Prismatic joint definition. This requires defining a line of -/// motion using an axis and an anchor point. The definition uses local -/// anchor points and a local axis so that the initial configuration -/// can violate the constraint slightly. The joint translation is zero -/// when the local anchor points coincide in world space. Using local -/// anchors and a local axis helps when saving and loading a game. -struct b2PrismaticJointDef : public b2JointDef -{ - b2PrismaticJointDef() - { - type = e_prismaticJoint; - localAnchorA.SetZero(); - localAnchorB.SetZero(); - localAxisA.Set(1.0f, 0.0f); - referenceAngle = 0.0f; - enableLimit = false; - lowerTranslation = 0.0f; - upperTranslation = 0.0f; - enableMotor = false; - maxMotorForce = 0.0f; - motorSpeed = 0.0f; - } - - /// Initialize the bodies, anchors, axis, and reference angle using the world - /// anchor and unit world axis. - void Initialize(b2Body* bodyA, b2Body* bodyB, const b2Vec2& anchor, const b2Vec2& axis); - - /// The local anchor point relative to bodyA's origin. - b2Vec2 localAnchorA; - - /// The local anchor point relative to bodyB's origin. - b2Vec2 localAnchorB; - - /// The local translation unit axis in bodyA. - b2Vec2 localAxisA; - - /// The constrained angle between the bodies: bodyB_angle - bodyA_angle. - float32 referenceAngle; - - /// Enable/disable the joint limit. - bool enableLimit; - - /// The lower translation limit, usually in meters. - float32 lowerTranslation; - - /// The upper translation limit, usually in meters. - float32 upperTranslation; - - /// Enable/disable the joint motor. - bool enableMotor; - - /// The maximum motor torque, usually in N-m. - float32 maxMotorForce; - - /// The desired motor speed in radians per second. - float32 motorSpeed; -}; - -/// A prismatic joint. This joint provides one degree of freedom: translation -/// along an axis fixed in bodyA. Relative rotation is prevented. You can -/// use a joint limit to restrict the range of motion and a joint motor to -/// drive the motion or to model joint friction. -class b2PrismaticJoint : public b2Joint -{ -public: - b2Vec2 GetAnchorA() const; - b2Vec2 GetAnchorB() const; - - b2Vec2 GetReactionForce(float32 inv_dt) const; - float32 GetReactionTorque(float32 inv_dt) const; - - /// The local anchor point relative to bodyA's origin. - const b2Vec2& GetLocalAnchorA() const { return m_localAnchorA; } - - /// The local anchor point relative to bodyB's origin. - const b2Vec2& GetLocalAnchorB() const { return m_localAnchorB; } - - /// The local joint axis relative to bodyA. - const b2Vec2& GetLocalAxisA() const { return m_localXAxisA; } - - /// Get the reference angle. - float32 GetReferenceAngle() const { return m_referenceAngle; } - - /// Get the current joint translation, usually in meters. - float32 GetJointTranslation() const; - - /// Get the current joint translation speed, usually in meters per second. - float32 GetJointSpeed() const; - - /// Is the joint limit enabled? - bool IsLimitEnabled() const; - - /// Enable/disable the joint limit. - void EnableLimit(bool flag); - - /// Get the lower joint limit, usually in meters. - float32 GetLowerLimit() const; - - /// Get the upper joint limit, usually in meters. - float32 GetUpperLimit() const; - - /// Set the joint limits, usually in meters. - void SetLimits(float32 lower, float32 upper); - - /// Is the joint motor enabled? - bool IsMotorEnabled() const; - - /// Enable/disable the joint motor. - void EnableMotor(bool flag); - - /// Set the motor speed, usually in meters per second. - void SetMotorSpeed(float32 speed); - - /// Get the motor speed, usually in meters per second. - float32 GetMotorSpeed() const; - - /// Set the maximum motor force, usually in N. - void SetMaxMotorForce(float32 force); - float32 GetMaxMotorForce() const { return m_maxMotorForce; } - - /// Get the current motor force given the inverse time step, usually in N. - float32 GetMotorForce(float32 inv_dt) const; - - /// Dump to b2Log - void Dump(); - -protected: - friend class b2Joint; - friend class b2GearJoint; - b2PrismaticJoint(const b2PrismaticJointDef* def); - - void InitVelocityConstraints(const b2SolverData& data); - void SolveVelocityConstraints(const b2SolverData& data); - bool SolvePositionConstraints(const b2SolverData& data); - - // Solver shared - b2Vec2 m_localAnchorA; - b2Vec2 m_localAnchorB; - b2Vec2 m_localXAxisA; - b2Vec2 m_localYAxisA; - float32 m_referenceAngle; - b2Vec3 m_impulse; - float32 m_motorImpulse; - float32 m_lowerTranslation; - float32 m_upperTranslation; - float32 m_maxMotorForce; - float32 m_motorSpeed; - bool m_enableLimit; - bool m_enableMotor; - b2LimitState m_limitState; - - // Solver temp - int32 m_indexA; - int32 m_indexB; - b2Vec2 m_localCenterA; - b2Vec2 m_localCenterB; - float32 m_invMassA; - float32 m_invMassB; - float32 m_invIA; - float32 m_invIB; - b2Vec2 m_axis, m_perp; - float32 m_s1, m_s2; - float32 m_a1, m_a2; - b2Mat33 m_K; - float32 m_motorMass; -}; - -inline float32 b2PrismaticJoint::GetMotorSpeed() const -{ - return m_motorSpeed; -} - -#endif +// MIT License + +// Copyright (c) 2019 Erin Catto + +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: + +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. + +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. + +#ifndef B2_PRISMATIC_JOINT_H +#define B2_PRISMATIC_JOINT_H + +#include "b2_api.h" +#include "b2_joint.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 B2_API 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. + float referenceAngle; + + /// Enable/disable the joint limit. + bool enableLimit; + + /// The lower translation limit, usually in meters. + float lowerTranslation; + + /// The upper translation limit, usually in meters. + float upperTranslation; + + /// Enable/disable the joint motor. + bool enableMotor; + + /// The maximum motor torque, usually in N-m. + float maxMotorForce; + + /// The desired motor speed in radians per second. + float 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 B2_API b2PrismaticJoint : public b2Joint +{ +public: + b2Vec2 GetAnchorA() const override; + b2Vec2 GetAnchorB() const override; + + b2Vec2 GetReactionForce(float inv_dt) const override; + float GetReactionTorque(float 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. + float GetReferenceAngle() const { return m_referenceAngle; } + + /// Get the current joint translation, usually in meters. + float GetJointTranslation() const; + + /// Get the current joint translation speed, usually in meters per second. + float 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. + float GetLowerLimit() const; + + /// Get the upper joint limit, usually in meters. + float GetUpperLimit() const; + + /// Set the joint limits, usually in meters. + void SetLimits(float lower, float 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(float speed); + + /// Get the motor speed, usually in meters per second. + float GetMotorSpeed() const; + + /// Set the maximum motor force, usually in N. + void SetMaxMotorForce(float force); + float GetMaxMotorForce() const { return m_maxMotorForce; } + + /// Get the current motor force given the inverse time step, usually in N. + float GetMotorForce(float inv_dt) const; + + /// Dump to b2Log + void Dump() override; + + /// + void Draw(b2Draw* draw) const 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; + + b2Vec2 m_localAnchorA; + b2Vec2 m_localAnchorB; + b2Vec2 m_localXAxisA; + b2Vec2 m_localYAxisA; + float m_referenceAngle; + b2Vec2 m_impulse; + float m_motorImpulse; + float m_lowerImpulse; + float m_upperImpulse; + float m_lowerTranslation; + float m_upperTranslation; + float m_maxMotorForce; + float m_motorSpeed; + bool m_enableLimit; + bool m_enableMotor; + + // Solver temp + int32 m_indexA; + int32 m_indexB; + b2Vec2 m_localCenterA; + b2Vec2 m_localCenterB; + float m_invMassA; + float m_invMassB; + float m_invIA; + float m_invIB; + b2Vec2 m_axis, m_perp; + float m_s1, m_s2; + float m_a1, m_a2; + b2Mat22 m_K; + float m_translation; + float m_axialMass; +}; + +inline float b2PrismaticJoint::GetMotorSpeed() const +{ + return m_motorSpeed; +} + +#endif diff --git a/libs/Box2D/Dynamics/Joints/b2PulleyJoint.h b/libs/box2d/b2_pulley_joint.h similarity index 54% rename from libs/Box2D/Dynamics/Joints/b2PulleyJoint.h rename to libs/box2d/b2_pulley_joint.h index 2dcdd34a..6b144564 100644 --- a/libs/Box2D/Dynamics/Joints/b2PulleyJoint.h +++ b/libs/box2d/b2_pulley_joint.h @@ -1,152 +1,157 @@ -/* -* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef B2_PULLEY_JOINT_H -#define B2_PULLEY_JOINT_H - -#include - -const float32 b2_minPulleyLength = 2.0f; - -/// Pulley joint definition. This requires two ground anchors, -/// two dynamic body anchor points, and a pulley ratio. -struct b2PulleyJointDef : public b2JointDef -{ - b2PulleyJointDef() - { - type = e_pulleyJoint; - groundAnchorA.Set(-1.0f, 1.0f); - groundAnchorB.Set(1.0f, 1.0f); - localAnchorA.Set(-1.0f, 0.0f); - localAnchorB.Set(1.0f, 0.0f); - lengthA = 0.0f; - lengthB = 0.0f; - ratio = 1.0f; - collideConnected = true; - } - - /// Initialize the bodies, anchors, lengths, max lengths, and ratio using the world anchors. - void Initialize(b2Body* bodyA, b2Body* bodyB, - const b2Vec2& groundAnchorA, const b2Vec2& groundAnchorB, - const b2Vec2& anchorA, const b2Vec2& anchorB, - float32 ratio); - - /// The first ground anchor in world coordinates. This point never moves. - b2Vec2 groundAnchorA; - - /// The second ground anchor in world coordinates. This point never moves. - b2Vec2 groundAnchorB; - - /// The local anchor point relative to bodyA's origin. - b2Vec2 localAnchorA; - - /// The local anchor point relative to bodyB's origin. - b2Vec2 localAnchorB; - - /// The a reference length for the segment attached to bodyA. - float32 lengthA; - - /// The a reference length for the segment attached to bodyB. - float32 lengthB; - - /// The pulley ratio, used to simulate a block-and-tackle. - float32 ratio; -}; - -/// The pulley joint is connected to two bodies and two fixed ground points. -/// The pulley supports a ratio such that: -/// length1 + ratio * length2 <= constant -/// Yes, the force transmitted is scaled by the ratio. -/// Warning: the pulley joint can get a bit squirrelly by itself. They often -/// work better when combined with prismatic joints. You should also cover the -/// the anchor points with static shapes to prevent one side from going to -/// zero length. -class b2PulleyJoint : public b2Joint -{ -public: - b2Vec2 GetAnchorA() const; - b2Vec2 GetAnchorB() const; - - b2Vec2 GetReactionForce(float32 inv_dt) const; - float32 GetReactionTorque(float32 inv_dt) const; - - /// Get the first ground anchor. - b2Vec2 GetGroundAnchorA() const; - - /// Get the second ground anchor. - b2Vec2 GetGroundAnchorB() const; - - /// Get the current length of the segment attached to bodyA. - float32 GetLengthA() const; - - /// Get the current length of the segment attached to bodyB. - float32 GetLengthB() const; - - /// Get the pulley ratio. - float32 GetRatio() const; - - /// Get the current length of the segment attached to bodyA. - float32 GetCurrentLengthA() const; - - /// Get the current length of the segment attached to bodyB. - float32 GetCurrentLengthB() const; - - /// Dump joint to dmLog - void Dump(); - - /// Implement b2Joint::ShiftOrigin - void ShiftOrigin(const b2Vec2& newOrigin); - -protected: - - friend class b2Joint; - b2PulleyJoint(const b2PulleyJointDef* data); - - void InitVelocityConstraints(const b2SolverData& data); - void SolveVelocityConstraints(const b2SolverData& data); - bool SolvePositionConstraints(const b2SolverData& data); - - b2Vec2 m_groundAnchorA; - b2Vec2 m_groundAnchorB; - float32 m_lengthA; - float32 m_lengthB; - - // Solver shared - b2Vec2 m_localAnchorA; - b2Vec2 m_localAnchorB; - float32 m_constant; - float32 m_ratio; - float32 m_impulse; - - // Solver temp - int32 m_indexA; - int32 m_indexB; - b2Vec2 m_uA; - b2Vec2 m_uB; - b2Vec2 m_rA; - b2Vec2 m_rB; - b2Vec2 m_localCenterA; - b2Vec2 m_localCenterB; - float32 m_invMassA; - float32 m_invMassB; - float32 m_invIA; - float32 m_invIB; - float32 m_mass; -}; - -#endif +// MIT License + +// Copyright (c) 2019 Erin Catto + +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: + +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. + +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. + +#ifndef B2_PULLEY_JOINT_H +#define B2_PULLEY_JOINT_H + +#include "b2_api.h" +#include "b2_joint.h" + +const float b2_minPulleyLength = 2.0f; + +/// Pulley joint definition. This requires two ground anchors, +/// two dynamic body anchor points, and a pulley ratio. +struct B2_API 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, + float 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. + float lengthA; + + /// The a reference length for the segment attached to bodyB. + float lengthB; + + /// The pulley ratio, used to simulate a block-and-tackle. + float 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 B2_API b2PulleyJoint : public b2Joint +{ +public: + b2Vec2 GetAnchorA() const override; + b2Vec2 GetAnchorB() const override; + + b2Vec2 GetReactionForce(float inv_dt) const override; + float GetReactionTorque(float 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. + float GetLengthA() const; + + /// Get the current length of the segment attached to bodyB. + float GetLengthB() const; + + /// Get the pulley ratio. + float GetRatio() const; + + /// Get the current length of the segment attached to bodyA. + float GetCurrentLengthA() const; + + /// Get the current length of the segment attached to bodyB. + float 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; + float m_lengthA; + float m_lengthB; + + // Solver shared + b2Vec2 m_localAnchorA; + b2Vec2 m_localAnchorB; + float m_constant; + float m_ratio; + float 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; + float m_invMassA; + float m_invMassB; + float m_invIA; + float m_invIB; + float m_mass; +}; + +#endif diff --git a/libs/Box2D/Dynamics/Joints/b2RevoluteJoint.h b/libs/box2d/b2_revolute_joint.h similarity index 53% rename from libs/Box2D/Dynamics/Joints/b2RevoluteJoint.h rename to libs/box2d/b2_revolute_joint.h index e62cdbcc..36ad5314 100644 --- a/libs/Box2D/Dynamics/Joints/b2RevoluteJoint.h +++ b/libs/box2d/b2_revolute_joint.h @@ -1,204 +1,211 @@ -/* -* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef B2_REVOLUTE_JOINT_H -#define B2_REVOLUTE_JOINT_H - -#include - -/// Revolute joint definition. This requires defining an -/// anchor point where the bodies are joined. The definition -/// uses local anchor points so that the initial configuration -/// can violate the constraint slightly. You also need to -/// specify the initial relative angle for joint limits. This -/// helps when saving and loading a game. -/// The local anchor points are measured from the body's origin -/// rather than the center of mass because: -/// 1. you might not know where the center of mass will be. -/// 2. if you add/remove shapes from a body and recompute the mass, -/// the joints will be broken. -struct b2RevoluteJointDef : public b2JointDef -{ - b2RevoluteJointDef() - { - type = e_revoluteJoint; - localAnchorA.Set(0.0f, 0.0f); - localAnchorB.Set(0.0f, 0.0f); - referenceAngle = 0.0f; - lowerAngle = 0.0f; - upperAngle = 0.0f; - maxMotorTorque = 0.0f; - motorSpeed = 0.0f; - enableLimit = false; - enableMotor = false; - } - - /// Initialize the bodies, anchors, and reference angle using a world - /// anchor point. - void Initialize(b2Body* bodyA, b2Body* bodyB, const b2Vec2& anchor); - - /// The local anchor point relative to bodyA's origin. - b2Vec2 localAnchorA; - - /// The local anchor point relative to bodyB's origin. - b2Vec2 localAnchorB; - - /// The bodyB angle minus bodyA angle in the reference state (radians). - float32 referenceAngle; - - /// A flag to enable joint limits. - bool enableLimit; - - /// The lower angle for the joint limit (radians). - float32 lowerAngle; - - /// The upper angle for the joint limit (radians). - float32 upperAngle; - - /// A flag to enable the joint motor. - bool enableMotor; - - /// The desired motor speed. Usually in radians per second. - float32 motorSpeed; - - /// The maximum motor torque used to achieve the desired motor speed. - /// Usually in N-m. - float32 maxMotorTorque; -}; - -/// A revolute joint constrains two bodies to share a common point while they -/// are free to rotate about the point. The relative rotation about the shared -/// point is the joint angle. You can limit the relative rotation with -/// a joint limit that specifies a lower and upper angle. You can use a motor -/// to drive the relative rotation about the shared point. A maximum motor torque -/// is provided so that infinite forces are not generated. -class b2RevoluteJoint : public b2Joint -{ -public: - b2Vec2 GetAnchorA() const; - b2Vec2 GetAnchorB() const; - - /// The local anchor point relative to bodyA's origin. - const b2Vec2& GetLocalAnchorA() const { return m_localAnchorA; } - - /// The local anchor point relative to bodyB's origin. - const b2Vec2& GetLocalAnchorB() const { return m_localAnchorB; } - - /// Get the reference angle. - float32 GetReferenceAngle() const { return m_referenceAngle; } - - /// Get the current joint angle in radians. - float32 GetJointAngle() const; - - /// Get the current joint angle speed in radians per second. - float32 GetJointSpeed() const; - - /// Is the joint limit enabled? - bool IsLimitEnabled() const; - - /// Enable/disable the joint limit. - void EnableLimit(bool flag); - - /// Get the lower joint limit in radians. - float32 GetLowerLimit() const; - - /// Get the upper joint limit in radians. - float32 GetUpperLimit() const; - - /// Set the joint limits in radians. - void SetLimits(float32 lower, float32 upper); - - /// Is the joint motor enabled? - bool IsMotorEnabled() const; - - /// Enable/disable the joint motor. - void EnableMotor(bool flag); - - /// Set the motor speed in radians per second. - void SetMotorSpeed(float32 speed); - - /// Get the motor speed in radians per second. - float32 GetMotorSpeed() const; - - /// Set the maximum motor torque, usually in N-m. - void SetMaxMotorTorque(float32 torque); - float32 GetMaxMotorTorque() const { return m_maxMotorTorque; } - - /// Get the reaction force given the inverse time step. - /// Unit is N. - b2Vec2 GetReactionForce(float32 inv_dt) const; - - /// Get the reaction torque due to the joint limit given the inverse time step. - /// Unit is N*m. - float32 GetReactionTorque(float32 inv_dt) const; - - /// Get the current motor torque given the inverse time step. - /// Unit is N*m. - float32 GetMotorTorque(float32 inv_dt) const; - - /// Dump to b2Log. - void Dump(); - -protected: - - friend class b2Joint; - friend class b2GearJoint; - - b2RevoluteJoint(const b2RevoluteJointDef* def); - - void InitVelocityConstraints(const b2SolverData& data); - void SolveVelocityConstraints(const b2SolverData& data); - bool SolvePositionConstraints(const b2SolverData& data); - - // Solver shared - b2Vec2 m_localAnchorA; - b2Vec2 m_localAnchorB; - b2Vec3 m_impulse; - float32 m_motorImpulse; - - bool m_enableMotor; - float32 m_maxMotorTorque; - float32 m_motorSpeed; - - bool m_enableLimit; - float32 m_referenceAngle; - float32 m_lowerAngle; - float32 m_upperAngle; - - // Solver temp - int32 m_indexA; - int32 m_indexB; - b2Vec2 m_rA; - b2Vec2 m_rB; - b2Vec2 m_localCenterA; - b2Vec2 m_localCenterB; - float32 m_invMassA; - float32 m_invMassB; - float32 m_invIA; - float32 m_invIB; - b2Mat33 m_mass; // effective mass for point-to-point constraint. - float32 m_motorMass; // effective mass for motor/limit angular constraint. - b2LimitState m_limitState; -}; - -inline float32 b2RevoluteJoint::GetMotorSpeed() const -{ - return m_motorSpeed; -} - -#endif +// MIT License + +// Copyright (c) 2019 Erin Catto + +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: + +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. + +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. + +#ifndef B2_REVOLUTE_JOINT_H +#define B2_REVOLUTE_JOINT_H + +#include "b2_api.h" +#include "b2_joint.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 B2_API 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). + float referenceAngle; + + /// A flag to enable joint limits. + bool enableLimit; + + /// The lower angle for the joint limit (radians). + float lowerAngle; + + /// The upper angle for the joint limit (radians). + float upperAngle; + + /// A flag to enable the joint motor. + bool enableMotor; + + /// The desired motor speed. Usually in radians per second. + float motorSpeed; + + /// The maximum motor torque used to achieve the desired motor speed. + /// Usually in N-m. + float 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 B2_API 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. + float GetReferenceAngle() const { return m_referenceAngle; } + + /// Get the current joint angle in radians. + float GetJointAngle() const; + + /// Get the current joint angle speed in radians per second. + float 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. + float GetLowerLimit() const; + + /// Get the upper joint limit in radians. + float GetUpperLimit() const; + + /// Set the joint limits in radians. + void SetLimits(float lower, float 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(float speed); + + /// Get the motor speed in radians per second. + float GetMotorSpeed() const; + + /// Set the maximum motor torque, usually in N-m. + void SetMaxMotorTorque(float torque); + float GetMaxMotorTorque() const { return m_maxMotorTorque; } + + /// Get the reaction force given the inverse time step. + /// Unit is N. + b2Vec2 GetReactionForce(float inv_dt) const override; + + /// Get the reaction torque due to the joint limit given the inverse time step. + /// Unit is N*m. + float GetReactionTorque(float inv_dt) const override; + + /// Get the current motor torque given the inverse time step. + /// Unit is N*m. + float GetMotorTorque(float inv_dt) const; + + /// Dump to b2Log. + void Dump() override; + + /// + void Draw(b2Draw* draw) const 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; + b2Vec2 m_impulse; + float m_motorImpulse; + float m_lowerImpulse; + float m_upperImpulse; + bool m_enableMotor; + float m_maxMotorTorque; + float m_motorSpeed; + bool m_enableLimit; + float m_referenceAngle; + float m_lowerAngle; + float m_upperAngle; + + // Solver temp + int32 m_indexA; + int32 m_indexB; + b2Vec2 m_rA; + b2Vec2 m_rB; + b2Vec2 m_localCenterA; + b2Vec2 m_localCenterB; + float m_invMassA; + float m_invMassB; + float m_invIA; + float m_invIB; + b2Mat22 m_K; + float m_angle; + float m_axialMass; +}; + +inline float b2RevoluteJoint::GetMotorSpeed() const +{ + return m_motorSpeed; +} + +#endif diff --git a/libs/box2d/b2_rope.h b/libs/box2d/b2_rope.h new file mode 100644 index 00000000..0c4f8195 --- /dev/null +++ b/libs/box2d/b2_rope.h @@ -0,0 +1,155 @@ +// MIT License + +// Copyright (c) 2019 Erin Catto + +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: + +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. + +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. + +#ifndef B2_ROPE_H +#define B2_ROPE_H + +#include "b2_api.h" +#include "b2_math.h" + +class b2Draw; +struct b2RopeStretch; +struct b2RopeBend; + +enum b2StretchingModel +{ + b2_pbdStretchingModel, + b2_xpbdStretchingModel +}; + +enum b2BendingModel +{ + b2_springAngleBendingModel = 0, + b2_pbdAngleBendingModel, + b2_xpbdAngleBendingModel, + b2_pbdDistanceBendingModel, + b2_pbdHeightBendingModel, + b2_pbdTriangleBendingModel +}; + +/// +struct B2_API b2RopeTuning +{ + b2RopeTuning() + { + stretchingModel = b2_pbdStretchingModel; + bendingModel = b2_pbdAngleBendingModel; + damping = 0.0f; + stretchStiffness = 1.0f; + stretchHertz = 1.0f; + stretchDamping = 0.0f; + bendStiffness = 0.5f; + bendHertz = 1.0f; + bendDamping = 0.0f; + isometric = false; + fixedEffectiveMass = false; + warmStart = false; + } + + b2StretchingModel stretchingModel; + b2BendingModel bendingModel; + float damping; + float stretchStiffness; + float stretchHertz; + float stretchDamping; + float bendStiffness; + float bendHertz; + float bendDamping; + bool isometric; + bool fixedEffectiveMass; + bool warmStart; +}; + +/// +struct B2_API b2RopeDef +{ + b2RopeDef() + { + position.SetZero(); + vertices = nullptr; + count = 0; + masses = nullptr; + gravity.SetZero(); + } + + b2Vec2 position; + b2Vec2* vertices; + int32 count; + float* masses; + b2Vec2 gravity; + b2RopeTuning tuning; +}; + +/// +class B2_API b2Rope +{ +public: + b2Rope(); + ~b2Rope(); + + /// + void Create(const b2RopeDef& def); + + /// + void SetTuning(const b2RopeTuning& tuning); + + /// + void Step(float timeStep, int32 iterations, const b2Vec2& position); + + /// + void Reset(const b2Vec2& position); + + /// + void Draw(b2Draw* draw) const; + +private: + + void SolveStretch_PBD(); + void SolveStretch_XPBD(float dt); + void SolveBend_PBD_Angle(); + void SolveBend_XPBD_Angle(float dt); + void SolveBend_PBD_Distance(); + void SolveBend_PBD_Height(); + void SolveBend_PBD_Triangle(); + void ApplyBendForces(float dt); + + b2Vec2 m_position; + + int32 m_count; + int32 m_stretchCount; + int32 m_bendCount; + + b2RopeStretch* m_stretchConstraints; + b2RopeBend* m_bendConstraints; + + b2Vec2* m_bindPositions; + b2Vec2* m_ps; + b2Vec2* m_p0s; + b2Vec2* m_vs; + + float* m_invMasses; + b2Vec2 m_gravity; + + b2RopeTuning m_tuning; +}; + +#endif diff --git a/libs/box2d/b2_settings.h b/libs/box2d/b2_settings.h new file mode 100644 index 00000000..48cd95df --- /dev/null +++ b/libs/box2d/b2_settings.h @@ -0,0 +1,127 @@ +// MIT License + +// Copyright (c) 2019 Erin Catto + +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: + +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. + +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. + +#ifndef B2_SETTINGS_H +#define B2_SETTINGS_H + +#include "b2_types.h" +#include "b2_api.h" + +/// @file +/// Settings that can be overriden for your application +/// + +/// Define this macro in your build if you want to override settings +#ifdef B2_USER_SETTINGS + +/// This is a user file that includes custom definitions of the macros, structs, and functions +/// defined below. +#include "b2_user_settings.h" + +#else + +#include +#include + +// Tunable Constants + +/// You can use this to change the length scale used by your game. +/// For example for inches you could use 39.4. +#define b2_lengthUnitsPerMeter 1.0f + +/// The maximum number of vertices on a convex polygon. You cannot increase +/// this too much because b2BlockAllocator has a maximum object size. +#define b2_maxPolygonVertices 8 + +// User data + +/// You can define this to inject whatever data you want in b2Body +struct B2_API b2BodyUserData +{ + b2BodyUserData() + { + pointer = 0; + } + + /// For legacy compatibility + uintptr_t pointer; +}; + +/// You can define this to inject whatever data you want in b2Fixture +struct B2_API b2FixtureUserData +{ + b2FixtureUserData() + { + pointer = 0; + } + + /// For legacy compatibility + uintptr_t pointer; +}; + +/// You can define this to inject whatever data you want in b2Joint +struct B2_API b2JointUserData +{ + b2JointUserData() + { + pointer = 0; + } + + /// For legacy compatibility + uintptr_t pointer; +}; + +// Memory Allocation + +/// Default allocation functions +B2_API void* b2Alloc_Default(int32 size); +B2_API void b2Free_Default(void* mem); + +/// Implement this function to use your own memory allocator. +inline void* b2Alloc(int32 size) +{ + return b2Alloc_Default(size); +} + +/// If you implement b2Alloc, you should also implement this function. +inline void b2Free(void* mem) +{ + b2Free_Default(mem); +} + +/// Default logging function +B2_API void b2Log_Default(const char* string, va_list args); + +/// Implement this to use your own logging. +inline void b2Log(const char* string, ...) +{ + va_list args; + va_start(args, string); + b2Log_Default(string, args); + va_end(args); +} + +#endif // B2_USER_SETTINGS + +#include "b2_common.h" + +#endif diff --git a/libs/Box2D/Collision/Shapes/b2Shape.h b/libs/box2d/b2_shape.h similarity index 60% rename from libs/Box2D/Collision/Shapes/b2Shape.h rename to libs/box2d/b2_shape.h index fd7de262..cbed2b86 100644 --- a/libs/Box2D/Collision/Shapes/b2Shape.h +++ b/libs/box2d/b2_shape.h @@ -1,101 +1,110 @@ -/* -* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef B2_SHAPE_H -#define B2_SHAPE_H - -#include -#include -#include - -/// This holds the mass data computed for a shape. -struct b2MassData -{ - /// The mass of the shape, usually in kilograms. - float32 mass; - - /// The position of the shape's centroid relative to the shape's origin. - b2Vec2 center; - - /// The rotational inertia of the shape about the local origin. - float32 I; -}; - -/// A shape is used for collision detection. You can create a shape however you like. -/// Shapes used for simulation in b2World are created automatically when a b2Fixture -/// is created. Shapes may encapsulate a one or more child shapes. -class b2Shape -{ -public: - - enum Type - { - e_circle = 0, - e_edge = 1, - e_polygon = 2, - e_chain = 3, - e_typeCount = 4 - }; - - virtual ~b2Shape() {} - - /// Clone the concrete shape using the provided allocator. - virtual b2Shape* Clone(b2BlockAllocator* allocator) const = 0; - - /// Get the type of this shape. You can use this to down cast to the concrete shape. - /// @return the shape type. - Type GetType() const; - - /// Get the number of child primitives. - virtual int32 GetChildCount() const = 0; - - /// Test a point for containment in this shape. This only works for convex shapes. - /// @param xf the shape world transform. - /// @param p a point in world coordinates. - virtual bool TestPoint(const b2Transform& xf, const b2Vec2& p) const = 0; - - /// Cast a ray against a child shape. - /// @param output the ray-cast results. - /// @param input the ray-cast input parameters. - /// @param transform the transform to be applied to the shape. - /// @param childIndex the child shape index - virtual bool RayCast(b2RayCastOutput* output, const b2RayCastInput& input, - const b2Transform& transform, int32 childIndex) const = 0; - - /// Given a transform, compute the associated axis aligned bounding box for a child shape. - /// @param aabb returns the axis aligned box. - /// @param xf the world transform of the shape. - /// @param childIndex the child shape - virtual void ComputeAABB(b2AABB* aabb, const b2Transform& xf, int32 childIndex) const = 0; - - /// Compute the mass properties of this shape using its dimensions and density. - /// The inertia tensor is computed about the local origin. - /// @param massData returns the mass data for this shape. - /// @param density the density in kilograms per meter squared. - virtual void ComputeMass(b2MassData* massData, float32 density) const = 0; - - Type m_type; - float32 m_radius; -}; - -inline b2Shape::Type b2Shape::GetType() const -{ - return m_type; -} - -#endif +// MIT License + +// Copyright (c) 2019 Erin Catto + +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: + +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. + +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. + +#ifndef B2_SHAPE_H +#define B2_SHAPE_H + +#include "b2_api.h" +#include "b2_math.h" +#include "b2_collision.h" + +class b2BlockAllocator; + +/// This holds the mass data computed for a shape. +struct B2_API b2MassData +{ + /// The mass of the shape, usually in kilograms. + float mass; + + /// The position of the shape's centroid relative to the shape's origin. + b2Vec2 center; + + /// The rotational inertia of the shape about the local origin. + float I; +}; + +/// A shape is used for collision detection. You can create a shape however you like. +/// Shapes used for simulation in b2World are created automatically when a b2Fixture +/// is created. Shapes may encapsulate a one or more child shapes. +class B2_API b2Shape +{ +public: + + enum Type + { + e_circle = 0, + e_edge = 1, + e_polygon = 2, + e_chain = 3, + e_typeCount = 4 + }; + + virtual ~b2Shape() {} + + /// Clone the concrete shape using the provided allocator. + virtual b2Shape* Clone(b2BlockAllocator* allocator) const = 0; + + /// Get the type of this shape. You can use this to down cast to the concrete shape. + /// @return the shape type. + Type GetType() const; + + /// Get the number of child primitives. + virtual int32 GetChildCount() const = 0; + + /// Test a point for containment in this shape. This only works for convex shapes. + /// @param xf the shape world transform. + /// @param p a point in world coordinates. + virtual bool TestPoint(const b2Transform& xf, const b2Vec2& p) const = 0; + + /// Cast a ray against a child shape. + /// @param output the ray-cast results. + /// @param input the ray-cast input parameters. + /// @param transform the transform to be applied to the shape. + /// @param childIndex the child shape index + virtual bool RayCast(b2RayCastOutput* output, const b2RayCastInput& input, + const b2Transform& transform, int32 childIndex) const = 0; + + /// Given a transform, compute the associated axis aligned bounding box for a child shape. + /// @param aabb returns the axis aligned box. + /// @param xf the world transform of the shape. + /// @param childIndex the child shape + virtual void ComputeAABB(b2AABB* aabb, const b2Transform& xf, int32 childIndex) const = 0; + + /// Compute the mass properties of this shape using its dimensions and density. + /// The inertia tensor is computed about the local origin. + /// @param massData returns the mass data for this shape. + /// @param density the density in kilograms per meter squared. + virtual void ComputeMass(b2MassData* massData, float density) const = 0; + + Type m_type; + + /// Radius of a shape. For polygonal shapes this must be b2_polygonRadius. There is no support for + /// making rounded polygons. + float m_radius; +}; + +inline b2Shape::Type b2Shape::GetType() const +{ + return m_type; +} + +#endif diff --git a/libs/box2d/b2_stack_allocator.h b/libs/box2d/b2_stack_allocator.h new file mode 100644 index 00000000..1db2af5d --- /dev/null +++ b/libs/box2d/b2_stack_allocator.h @@ -0,0 +1,65 @@ +// MIT License + +// Copyright (c) 2019 Erin Catto + +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: + +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. + +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. + +#ifndef B2_STACK_ALLOCATOR_H +#define B2_STACK_ALLOCATOR_H + +#include "b2_api.h" +#include "b2_settings.h" + +const int32 b2_stackSize = 100 * 1024; // 100k +const int32 b2_maxStackEntries = 32; + +struct B2_API b2StackEntry +{ + char* data; + int32 size; + bool usedMalloc; +}; + +// This is a stack allocator used for fast per step allocations. +// You must nest allocate/free pairs. The code will assert +// if you try to interleave multiple allocate/free pairs. +class B2_API b2StackAllocator +{ +public: + b2StackAllocator(); + ~b2StackAllocator(); + + void* Allocate(int32 size); + void Free(void* p); + + int32 GetMaxAllocation() const; + +private: + + char m_data[b2_stackSize]; + int32 m_index; + + int32 m_allocation; + int32 m_maxAllocation; + + b2StackEntry m_entries[b2_maxStackEntries]; + int32 m_entryCount; +}; + +#endif diff --git a/libs/box2d/b2_time_of_impact.h b/libs/box2d/b2_time_of_impact.h new file mode 100644 index 00000000..04d46262 --- /dev/null +++ b/libs/box2d/b2_time_of_impact.h @@ -0,0 +1,63 @@ +// MIT License + +// Copyright (c) 2019 Erin Catto + +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: + +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. + +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. + +#ifndef B2_TIME_OF_IMPACT_H +#define B2_TIME_OF_IMPACT_H + +#include "b2_api.h" +#include "b2_math.h" +#include "b2_distance.h" + +/// Input parameters for b2TimeOfImpact +struct B2_API b2TOIInput +{ + b2DistanceProxy proxyA; + b2DistanceProxy proxyB; + b2Sweep sweepA; + b2Sweep sweepB; + float tMax; // defines sweep interval [0, tMax] +}; + +/// Output parameters for b2TimeOfImpact. +struct B2_API b2TOIOutput +{ + enum State + { + e_unknown, + e_failed, + e_overlapped, + e_touching, + e_separated + }; + + State state; + float t; +}; + +/// Compute the upper bound on time before two shapes penetrate. Time is represented as +/// a fraction between [0,tMax]. This uses a swept separating axis and may miss some intermediate, +/// non-tunneling collisions. If you change the time interval, you should call this function +/// again. +/// Note: use b2Distance to compute the contact point and normal at the time of impact. +B2_API void b2TimeOfImpact(b2TOIOutput* output, const b2TOIInput* input); + +#endif diff --git a/libs/box2d/b2_time_step.h b/libs/box2d/b2_time_step.h new file mode 100644 index 00000000..13d6292e --- /dev/null +++ b/libs/box2d/b2_time_step.h @@ -0,0 +1,74 @@ +// MIT License + +// Copyright (c) 2019 Erin Catto + +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: + +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. + +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +#ifndef B2_TIME_STEP_H +#define B2_TIME_STEP_H + +#include "b2_api.h" +#include "b2_math.h" + +/// Profiling data. Times are in milliseconds. +struct B2_API b2Profile +{ + float step; + float collide; + float solve; + float solveInit; + float solveVelocity; + float solvePosition; + float broadphase; + float solveTOI; +}; + +/// This is an internal structure. +struct B2_API b2TimeStep +{ + float dt; // time step + float inv_dt; // inverse time step (0 if dt == 0). + float dtRatio; // dt * inv_dt0 + int32 velocityIterations; + int32 positionIterations; + bool warmStarting; +}; + +/// This is an internal structure. +struct B2_API b2Position +{ + b2Vec2 c; + float a; +}; + +/// This is an internal structure. +struct B2_API b2Velocity +{ + b2Vec2 v; + float w; +}; + +/// Solver Data +struct B2_API b2SolverData +{ + b2TimeStep step; + b2Position* positions; + b2Velocity* velocities; +}; + +#endif diff --git a/libs/box2d/b2_timer.h b/libs/box2d/b2_timer.h new file mode 100644 index 00000000..7893c327 --- /dev/null +++ b/libs/box2d/b2_timer.h @@ -0,0 +1,55 @@ +// MIT License + +// Copyright (c) 2019 Erin Catto + +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: + +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. + +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. + +#ifndef B2_TIMER_H +#define B2_TIMER_H + +#include "b2_api.h" +#include "b2_settings.h" + +/// Timer for profiling. This has platform specific code and may +/// not work on every platform. +class B2_API b2Timer +{ +public: + + /// Constructor + b2Timer(); + + /// Reset the timer. + void Reset(); + + /// Get the time since construction or the last reset. + float GetMilliseconds() const; + +private: + +#if defined(_WIN32) + double m_start; + static double s_invFrequency; +#elif defined(__linux__) || defined (__APPLE__) + unsigned long long m_start_sec; + unsigned long long m_start_usec; +#endif +}; + +#endif diff --git a/libs/box2d/b2_types.h b/libs/box2d/b2_types.h new file mode 100644 index 00000000..e0d43775 --- /dev/null +++ b/libs/box2d/b2_types.h @@ -0,0 +1,33 @@ +// MIT License + +// Copyright (c) 2020 Erin Catto + +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: + +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. + +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. + +#ifndef B2_TYPES_H +#define B2_TYPES_H + +typedef signed char int8; +typedef signed short int16; +typedef signed int int32; +typedef unsigned char uint8; +typedef unsigned short uint16; +typedef unsigned int uint32; + +#endif diff --git a/libs/box2d/b2_weld_joint.h b/libs/box2d/b2_weld_joint.h new file mode 100644 index 00000000..38501f20 --- /dev/null +++ b/libs/box2d/b2_weld_joint.h @@ -0,0 +1,133 @@ +// MIT License + +// Copyright (c) 2019 Erin Catto + +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: + +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. + +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. + +#ifndef B2_WELD_JOINT_H +#define B2_WELD_JOINT_H + +#include "b2_api.h" +#include "b2_joint.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 B2_API b2WeldJointDef : public b2JointDef +{ + b2WeldJointDef() + { + type = e_weldJoint; + localAnchorA.Set(0.0f, 0.0f); + localAnchorB.Set(0.0f, 0.0f); + referenceAngle = 0.0f; + stiffness = 0.0f; + damping = 0.0f; + } + + /// Initialize the bodies, anchors, reference angle, stiffness, and damping. + /// @param bodyA the first body connected by this joint + /// @param bodyB the second body connected by this joint + /// @param anchor the point of connection in world coordinates + 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). + float referenceAngle; + + /// The rotational stiffness in N*m + /// Disable softness with a value of 0 + float stiffness; + + /// The rotational damping in N*m*s + float damping; +}; + +/// A weld joint essentially glues two bodies together. A weld joint may +/// distort somewhat because the island constraint solver is approximate. +class B2_API b2WeldJoint : public b2Joint +{ +public: + b2Vec2 GetAnchorA() const override; + b2Vec2 GetAnchorB() const override; + + b2Vec2 GetReactionForce(float inv_dt) const override; + float GetReactionTorque(float 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. + float GetReferenceAngle() const { return m_referenceAngle; } + + /// Set/get stiffness in N*m + void SetStiffness(float stiffness) { m_stiffness = stiffness; } + float GetStiffness() const { return m_stiffness; } + + /// Set/get damping in N*m*s + void SetDamping(float damping) { m_damping = damping; } + float GetDamping() const { return m_damping; } + + /// 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; + + float m_stiffness; + float m_damping; + float m_bias; + + // Solver shared + b2Vec2 m_localAnchorA; + b2Vec2 m_localAnchorB; + float m_referenceAngle; + float 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; + float m_invMassA; + float m_invMassB; + float m_invIA; + float m_invIB; + b2Mat33 m_mass; +}; + +#endif diff --git a/libs/box2d/b2_wheel_joint.h b/libs/box2d/b2_wheel_joint.h new file mode 100644 index 00000000..8576adbd --- /dev/null +++ b/libs/box2d/b2_wheel_joint.h @@ -0,0 +1,240 @@ +// MIT License + +// Copyright (c) 2019 Erin Catto + +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: + +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. + +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. + +#ifndef B2_WHEEL_JOINT_H +#define B2_WHEEL_JOINT_H + +#include "b2_api.h" +#include "b2_joint.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 B2_API b2WheelJointDef : public b2JointDef +{ + b2WheelJointDef() + { + type = e_wheelJoint; + localAnchorA.SetZero(); + localAnchorB.SetZero(); + localAxisA.Set(1.0f, 0.0f); + enableLimit = false; + lowerTranslation = 0.0f; + upperTranslation = 0.0f; + enableMotor = false; + maxMotorTorque = 0.0f; + motorSpeed = 0.0f; + stiffness = 0.0f; + damping = 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, 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 limit. + bool enableLimit; + + /// The lower translation limit, usually in meters. + float lowerTranslation; + + /// The upper translation limit, usually in meters. + float upperTranslation; + + /// Enable/disable the joint motor. + bool enableMotor; + + /// The maximum motor torque, usually in N-m. + float maxMotorTorque; + + /// The desired motor speed in radians per second. + float motorSpeed; + + /// Suspension stiffness. Typically in units N/m. + float stiffness; + + /// Suspension damping. Typically in units of N*s/m. + float damping; +}; + +/// 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. The spring/damper is +/// initialized upon creation. This joint is designed for vehicle suspensions. +class B2_API b2WheelJoint : public b2Joint +{ +public: + b2Vec2 GetAnchorA() const override; + b2Vec2 GetAnchorB() const override; + + b2Vec2 GetReactionForce(float inv_dt) const override; + float GetReactionTorque(float 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. + float GetJointTranslation() const; + + /// Get the current joint linear speed, usually in meters per second. + float GetJointLinearSpeed() const; + + /// Get the current joint angle in radians. + float GetJointAngle() const; + + /// Get the current joint angular speed in radians per second. + float GetJointAngularSpeed() const; + + /// Is the joint limit enabled? + bool IsLimitEnabled() const; + + /// Enable/disable the joint translation limit. + void EnableLimit(bool flag); + + /// Get the lower joint translation limit, usually in meters. + float GetLowerLimit() const; + + /// Get the upper joint translation limit, usually in meters. + float GetUpperLimit() const; + + /// Set the joint translation limits, usually in meters. + void SetLimits(float lower, float upper); + + /// 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(float speed); + + /// Get the motor speed, usually in radians per second. + float GetMotorSpeed() const; + + /// Set/Get the maximum motor force, usually in N-m. + void SetMaxMotorTorque(float torque); + float GetMaxMotorTorque() const; + + /// Get the current motor torque given the inverse time step, usually in N-m. + float GetMotorTorque(float inv_dt) const; + + /// Access spring stiffness + void SetStiffness(float stiffness); + float GetStiffness() const; + + /// Access damping + void SetDamping(float damping); + float GetDamping() const; + + /// Dump to b2Log + void Dump() override; + + /// + void Draw(b2Draw* draw) const 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; + + b2Vec2 m_localAnchorA; + b2Vec2 m_localAnchorB; + b2Vec2 m_localXAxisA; + b2Vec2 m_localYAxisA; + + float m_impulse; + float m_motorImpulse; + float m_springImpulse; + + float m_lowerImpulse; + float m_upperImpulse; + float m_translation; + float m_lowerTranslation; + float m_upperTranslation; + + float m_maxMotorTorque; + float m_motorSpeed; + + bool m_enableLimit; + bool m_enableMotor; + + float m_stiffness; + float m_damping; + + // Solver temp + int32 m_indexA; + int32 m_indexB; + b2Vec2 m_localCenterA; + b2Vec2 m_localCenterB; + float m_invMassA; + float m_invMassB; + float m_invIA; + float m_invIB; + + b2Vec2 m_ax, m_ay; + float m_sAx, m_sBx; + float m_sAy, m_sBy; + + float m_mass; + float m_motorMass; + float m_axialMass; + float m_springMass; + + float m_bias; + float m_gamma; + +}; + +inline float b2WheelJoint::GetMotorSpeed() const +{ + return m_motorSpeed; +} + +inline float b2WheelJoint::GetMaxMotorTorque() const +{ + return m_maxMotorTorque; +} + +#endif diff --git a/libs/Box2D/Dynamics/b2World.h b/libs/box2d/b2_world.h similarity index 81% rename from libs/Box2D/Dynamics/b2World.h rename to libs/box2d/b2_world.h index 3968cc78..afd73bd4 100644 --- a/libs/Box2D/Dynamics/b2World.h +++ b/libs/box2d/b2_world.h @@ -1,30 +1,35 @@ -/* -* 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. -*/ +// MIT License + +// Copyright (c) 2019 Erin Catto + +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: + +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. + +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. #ifndef B2_WORLD_H #define B2_WORLD_H -#include -#include -#include -#include -#include -#include +#include "b2_api.h" +#include "b2_block_allocator.h" +#include "b2_contact_manager.h" +#include "b2_math.h" +#include "b2_stack_allocator.h" +#include "b2_time_step.h" +#include "b2_world_callbacks.h" struct b2AABB; struct b2BodyDef; @@ -38,7 +43,7 @@ class b2Joint; /// The world class manages all physics entities, dynamic simulation, /// and asynchronous queries. The world also contains efficient memory /// management facilities. -class b2World +class B2_API b2World { public: /// Construct a world object. @@ -54,7 +59,7 @@ class b2World /// 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. + /// 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 @@ -62,7 +67,7 @@ class b2World 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 + /// inside with b2World::DebugDraw method. The debug draw object is owned /// by you and must remain in scope. void SetDebugDraw(b2Draw* debugDraw); @@ -91,7 +96,7 @@ class b2World /// @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, + void Step( float timeStep, int32 velocityIterations, int32 positionIterations); @@ -105,7 +110,7 @@ class b2World void ClearForces(); /// Call this to draw shapes and other debug draw data. This is intentionally non-const. - void DrawDebugData(); + void DebugDraw(); /// Query the world for all fixtures that potentially overlap the /// provided AABB. @@ -122,19 +127,19 @@ class b2World void RayCast(b2RayCastCallback* callback, const b2Vec2& point1, const b2Vec2& point2) const; /// Get the world body list. With the returned body, use b2Body::GetNext to get - /// the next body in the world list. A NULL body indicates the end of the list. + /// 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 NULL joint indicates the end of the list. + /// 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 NULL contact indicates the end of the list. + /// 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. @@ -177,11 +182,11 @@ class b2World /// Get the quality metric of the dynamic tree. The smaller the better. /// The minimum is 1. - float32 GetTreeQuality() const; + float GetTreeQuality() const; /// Change the global gravity vector. void SetGravity(const b2Vec2& gravity); - + /// Get the global gravity vector. b2Vec2 GetGravity() const; @@ -211,30 +216,22 @@ class b2World 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; + b2World(const b2World&) = delete; + void operator=(const b2World&) = delete; + 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; @@ -247,11 +244,15 @@ class b2World bool m_allowSleep; b2DestructionListener* m_destructionListener; - b2Draw* g_debugDraw; + b2Draw* m_debugDraw; // This is used to compute the time step ratio to // support a variable time step. - float32 m_inv_dt0; + float m_inv_dt0; + + bool m_newContacts; + bool m_locked; + bool m_clearForces; // These are for debugging the solver. bool m_warmStarting; @@ -320,25 +321,18 @@ inline b2Vec2 b2World::GetGravity() const inline bool b2World::IsLocked() const { - return (m_flags & e_locked) == e_locked; + return m_locked; } inline void b2World::SetAutoClearForces(bool flag) { - if (flag) - { - m_flags |= e_clearForces; - } - else - { - m_flags &= ~e_clearForces; - } + m_clearForces = flag; } /// 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; + return m_clearForces; } inline const b2ContactManager& b2World::GetContactManager() const diff --git a/libs/Box2D/Dynamics/b2WorldCallbacks.h b/libs/box2d/b2_world_callbacks.h similarity index 73% rename from libs/Box2D/Dynamics/b2WorldCallbacks.h rename to libs/box2d/b2_world_callbacks.h index 9b937e81..da45640e 100644 --- a/libs/Box2D/Dynamics/b2WorldCallbacks.h +++ b/libs/box2d/b2_world_callbacks.h @@ -1,155 +1,161 @@ -/* -* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef B2_WORLD_CALLBACKS_H -#define B2_WORLD_CALLBACKS_H - -#include - -struct b2Vec2; -struct b2Transform; -class b2Fixture; -class b2Body; -class b2Joint; -class b2Contact; -struct b2ContactResult; -struct b2Manifold; - -/// Joints and fixtures are destroyed when their associated -/// body is destroyed. Implement this listener so that you -/// may nullify references to these joints and shapes. -class b2DestructionListener -{ -public: - virtual ~b2DestructionListener() {} - - /// Called when any joint is about to be destroyed due - /// to the destruction of one of its attached bodies. - virtual void SayGoodbye(b2Joint* joint) = 0; - - /// Called when any fixture is about to be destroyed due - /// to the destruction of its parent body. - virtual void SayGoodbye(b2Fixture* fixture) = 0; -}; - -/// Implement this class to provide collision filtering. In other words, you can implement -/// this class if you want finer control over contact creation. -class b2ContactFilter -{ -public: - virtual ~b2ContactFilter() {} - - /// Return true if contact calculations should be performed between these two shapes. - /// @warning for performance reasons this is only called when the AABBs begin to overlap. - virtual bool ShouldCollide(b2Fixture* fixtureA, b2Fixture* fixtureB); -}; - -/// Contact impulses for reporting. Impulses are used instead of forces because -/// sub-step forces may approach infinity for rigid body collisions. These -/// match up one-to-one with the contact points in b2Manifold. -struct b2ContactImpulse -{ - float32 normalImpulses[b2_maxManifoldPoints]; - float32 tangentImpulses[b2_maxManifoldPoints]; - int32 count; -}; - -/// Implement this class to get contact information. You can use these results for -/// things like sounds and game logic. You can also get contact results by -/// traversing the contact lists after the time step. However, you might miss -/// some contacts because continuous physics leads to sub-stepping. -/// Additionally you may receive multiple callbacks for the same contact in a -/// single time step. -/// You should strive to make your callbacks efficient because there may be -/// many callbacks per time step. -/// @warning You cannot create/destroy Box2D entities inside these callbacks. -class b2ContactListener -{ -public: - virtual ~b2ContactListener() {} - - /// Called when two fixtures begin to touch. - virtual void BeginContact(b2Contact* contact) { B2_NOT_USED(contact); } - - /// Called when two fixtures cease to touch. - virtual void EndContact(b2Contact* contact) { B2_NOT_USED(contact); } - - /// This is called after a contact is updated. This allows you to inspect a - /// contact before it goes to the solver. If you are careful, you can modify the - /// contact manifold (e.g. disable contact). - /// A copy of the old manifold is provided so that you can detect changes. - /// Note: this is called only for awake bodies. - /// Note: this is called even when the number of contact points is zero. - /// Note: this is not called for sensors. - /// Note: if you set the number of contact points to zero, you will not - /// get an EndContact callback. However, you may get a BeginContact callback - /// the next step. - virtual void PreSolve(b2Contact* contact, const b2Manifold* oldManifold) - { - B2_NOT_USED(contact); - B2_NOT_USED(oldManifold); - } - - /// This lets you inspect a contact after the solver is finished. This is useful - /// for inspecting impulses. - /// Note: the contact manifold does not include time of impact impulses, which can be - /// arbitrarily large if the sub-step is small. Hence the impulse is provided explicitly - /// in a separate data structure. - /// Note: this is only called for contacts that are touching, solid, and awake. - virtual void PostSolve(b2Contact* contact, const b2ContactImpulse* impulse) - { - B2_NOT_USED(contact); - B2_NOT_USED(impulse); - } -}; - -/// Callback class for AABB queries. -/// See b2World::Query -class b2QueryCallback -{ -public: - virtual ~b2QueryCallback() {} - - /// Called for each fixture found in the query AABB. - /// @return false to terminate the query. - virtual bool ReportFixture(b2Fixture* fixture) = 0; -}; - -/// Callback class for ray casts. -/// See b2World::RayCast -class b2RayCastCallback -{ -public: - virtual ~b2RayCastCallback() {} - - /// Called for each fixture found in the query. You control how the ray cast - /// proceeds by returning a float: - /// return -1: ignore this fixture and continue - /// return 0: terminate the ray cast - /// return fraction: clip the ray to this point - /// return 1: don't clip the ray and continue - /// @param fixture the fixture hit by the ray - /// @param point the point of initial intersection - /// @param normal the normal vector at the point of intersection - /// @return -1 to filter, 0 to terminate, fraction to clip the ray for - /// closest hit, 1 to continue - virtual float32 ReportFixture( b2Fixture* fixture, const b2Vec2& point, - const b2Vec2& normal, float32 fraction) = 0; -}; - -#endif +// MIT License + +// Copyright (c) 2019 Erin Catto + +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: + +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. + +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. + +#ifndef B2_WORLD_CALLBACKS_H +#define B2_WORLD_CALLBACKS_H + +#include "b2_api.h" +#include "b2_settings.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 B2_API 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 B2_API 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 B2_API b2ContactImpulse +{ + float normalImpulses[b2_maxManifoldPoints]; + float 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 B2_API 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 B2_API 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 B2_API 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 + /// @param fraction the fraction along the ray at the point of intersection + /// @return -1 to filter, 0 to terminate, fraction to clip the ray for + /// closest hit, 1 to continue + virtual float ReportFixture( b2Fixture* fixture, const b2Vec2& point, + const b2Vec2& normal, float fraction) = 0; +}; + +#endif diff --git a/libs/box2d/box2d.h b/libs/box2d/box2d.h new file mode 100644 index 00000000..55c69582 --- /dev/null +++ b/libs/box2d/box2d.h @@ -0,0 +1,58 @@ +// MIT License + +// Copyright (c) 2019 Erin Catto + +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: + +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. + +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. + +#ifndef BOX2D_H +#define BOX2D_H + +// These include files constitute the main Box2D API + +#include "b2_settings.h" +#include "b2_draw.h" +#include "b2_timer.h" + +#include "b2_chain_shape.h" +#include "b2_circle_shape.h" +#include "b2_edge_shape.h" +#include "b2_polygon_shape.h" + +#include "b2_broad_phase.h" +#include "b2_dynamic_tree.h" + +#include "b2_body.h" +#include "b2_contact.h" +#include "b2_fixture.h" +#include "b2_time_step.h" +#include "b2_world.h" +#include "b2_world_callbacks.h" + +#include "b2_distance_joint.h" +#include "b2_friction_joint.h" +#include "b2_gear_joint.h" +#include "b2_motor_joint.h" +#include "b2_mouse_joint.h" +#include "b2_prismatic_joint.h" +#include "b2_pulley_joint.h" +#include "b2_revolute_joint.h" +#include "b2_weld_joint.h" +#include "b2_wheel_joint.h" + +#endif diff --git a/libs/Box2D/Collision/b2BroadPhase.cpp b/libs/box2d/src/collision/b2_broad_phase.cpp similarity index 60% rename from libs/Box2D/Collision/b2BroadPhase.cpp rename to libs/box2d/src/collision/b2_broad_phase.cpp index 6cddf719..d063a3aa 100644 --- a/libs/Box2D/Collision/b2BroadPhase.cpp +++ b/libs/box2d/src/collision/b2_broad_phase.cpp @@ -1,119 +1,131 @@ -/* -* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ - -#include - -b2BroadPhase::b2BroadPhase() -{ - m_proxyCount = 0; - - m_pairCapacity = 16; - m_pairCount = 0; - m_pairBuffer = (b2Pair*)b2Alloc(m_pairCapacity * sizeof(b2Pair)); - - m_moveCapacity = 16; - m_moveCount = 0; - m_moveBuffer = (int32*)b2Alloc(m_moveCapacity * sizeof(int32)); -} - -b2BroadPhase::~b2BroadPhase() -{ - b2Free(m_moveBuffer); - b2Free(m_pairBuffer); -} - -int32 b2BroadPhase::CreateProxy(const b2AABB& aabb, void* userData) -{ - int32 proxyId = m_tree.CreateProxy(aabb, userData); - ++m_proxyCount; - BufferMove(proxyId); - return proxyId; -} - -void b2BroadPhase::DestroyProxy(int32 proxyId) -{ - UnBufferMove(proxyId); - --m_proxyCount; - m_tree.DestroyProxy(proxyId); -} - -void b2BroadPhase::MoveProxy(int32 proxyId, const b2AABB& aabb, const b2Vec2& displacement) -{ - bool buffer = m_tree.MoveProxy(proxyId, aabb, displacement); - if (buffer) - { - BufferMove(proxyId); - } -} - -void b2BroadPhase::TouchProxy(int32 proxyId) -{ - BufferMove(proxyId); -} - -void b2BroadPhase::BufferMove(int32 proxyId) -{ - if (m_moveCount == m_moveCapacity) - { - int32* oldBuffer = m_moveBuffer; - m_moveCapacity *= 2; - m_moveBuffer = (int32*)b2Alloc(m_moveCapacity * sizeof(int32)); - memcpy(m_moveBuffer, oldBuffer, m_moveCount * sizeof(int32)); - b2Free(oldBuffer); - } - - m_moveBuffer[m_moveCount] = proxyId; - ++m_moveCount; -} - -void b2BroadPhase::UnBufferMove(int32 proxyId) -{ - for (int32 i = 0; i < m_moveCount; ++i) - { - if (m_moveBuffer[i] == proxyId) - { - m_moveBuffer[i] = e_nullProxy; - } - } -} - -// This is called from b2DynamicTree::Query when we are gathering pairs. -bool b2BroadPhase::QueryCallback(int32 proxyId) -{ - // A proxy cannot form a pair with itself. - if (proxyId == m_queryProxyId) - { - return true; - } - - // Grow the pair buffer as needed. - if (m_pairCount == m_pairCapacity) - { - b2Pair* oldBuffer = m_pairBuffer; - m_pairCapacity *= 2; - m_pairBuffer = (b2Pair*)b2Alloc(m_pairCapacity * sizeof(b2Pair)); - memcpy(m_pairBuffer, oldBuffer, m_pairCount * sizeof(b2Pair)); - b2Free(oldBuffer); - } - - m_pairBuffer[m_pairCount].proxyIdA = b2Min(proxyId, m_queryProxyId); - m_pairBuffer[m_pairCount].proxyIdB = b2Max(proxyId, m_queryProxyId); - ++m_pairCount; - - return true; -} +// MIT License + +// Copyright (c) 2019 Erin Catto + +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: + +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. + +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. + +#include "box2d/b2_broad_phase.h" +#include + +b2BroadPhase::b2BroadPhase() +{ + m_proxyCount = 0; + + m_pairCapacity = 16; + m_pairCount = 0; + m_pairBuffer = (b2Pair*)b2Alloc(m_pairCapacity * sizeof(b2Pair)); + + m_moveCapacity = 16; + m_moveCount = 0; + m_moveBuffer = (int32*)b2Alloc(m_moveCapacity * sizeof(int32)); +} + +b2BroadPhase::~b2BroadPhase() +{ + b2Free(m_moveBuffer); + b2Free(m_pairBuffer); +} + +int32 b2BroadPhase::CreateProxy(const b2AABB& aabb, void* userData) +{ + int32 proxyId = m_tree.CreateProxy(aabb, userData); + ++m_proxyCount; + BufferMove(proxyId); + return proxyId; +} + +void b2BroadPhase::DestroyProxy(int32 proxyId) +{ + UnBufferMove(proxyId); + --m_proxyCount; + m_tree.DestroyProxy(proxyId); +} + +void b2BroadPhase::MoveProxy(int32 proxyId, const b2AABB& aabb, const b2Vec2& displacement) +{ + bool buffer = m_tree.MoveProxy(proxyId, aabb, displacement); + if (buffer) + { + BufferMove(proxyId); + } +} + +void b2BroadPhase::TouchProxy(int32 proxyId) +{ + BufferMove(proxyId); +} + +void b2BroadPhase::BufferMove(int32 proxyId) +{ + if (m_moveCount == m_moveCapacity) + { + int32* oldBuffer = m_moveBuffer; + m_moveCapacity *= 2; + m_moveBuffer = (int32*)b2Alloc(m_moveCapacity * sizeof(int32)); + memcpy(m_moveBuffer, oldBuffer, m_moveCount * sizeof(int32)); + b2Free(oldBuffer); + } + + m_moveBuffer[m_moveCount] = proxyId; + ++m_moveCount; +} + +void b2BroadPhase::UnBufferMove(int32 proxyId) +{ + for (int32 i = 0; i < m_moveCount; ++i) + { + if (m_moveBuffer[i] == proxyId) + { + m_moveBuffer[i] = e_nullProxy; + } + } +} + +// This is called from b2DynamicTree::Query when we are gathering pairs. +bool b2BroadPhase::QueryCallback(int32 proxyId) +{ + // A proxy cannot form a pair with itself. + if (proxyId == m_queryProxyId) + { + return true; + } + + const bool moved = m_tree.WasMoved(proxyId); + if (moved && proxyId > m_queryProxyId) + { + // Both proxies are moving. Avoid duplicate pairs. + return true; + } + + // Grow the pair buffer as needed. + if (m_pairCount == m_pairCapacity) + { + b2Pair* oldBuffer = m_pairBuffer; + m_pairCapacity = m_pairCapacity + (m_pairCapacity >> 1); + m_pairBuffer = (b2Pair*)b2Alloc(m_pairCapacity * sizeof(b2Pair)); + memcpy(m_pairBuffer, oldBuffer, m_pairCount * sizeof(b2Pair)); + b2Free(oldBuffer); + } + + m_pairBuffer[m_pairCount].proxyIdA = b2Min(proxyId, m_queryProxyId); + m_pairBuffer[m_pairCount].proxyIdB = b2Max(proxyId, m_queryProxyId); + ++m_pairCount; + + return true; +} diff --git a/libs/Box2D/Collision/Shapes/b2ChainShape.cpp b/libs/box2d/src/collision/b2_chain_shape.cpp similarity index 58% rename from libs/Box2D/Collision/Shapes/b2ChainShape.cpp rename to libs/box2d/src/collision/b2_chain_shape.cpp index a9c2e1d5..b964a430 100644 --- a/libs/Box2D/Collision/Shapes/b2ChainShape.cpp +++ b/libs/box2d/src/collision/b2_chain_shape.cpp @@ -1,193 +1,185 @@ -/* -* Copyright (c) 2006-2010 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ - -#include -#include -#include -#include - -b2ChainShape::~b2ChainShape() -{ - Clear(); -} - -void b2ChainShape::Clear() -{ - b2Free(m_vertices); - m_vertices = NULL; - m_count = 0; -} - -void b2ChainShape::CreateLoop(const b2Vec2* vertices, int32 count) -{ - b2Assert(m_vertices == NULL && m_count == 0); - b2Assert(count >= 3); - for (int32 i = 1; i < count; ++i) - { - b2Vec2 v1 = vertices[i-1]; - b2Vec2 v2 = vertices[i]; - // If the code crashes here, it means your vertices are too close together. - b2Assert(b2DistanceSquared(v1, v2) > b2_linearSlop * b2_linearSlop); - } - - m_count = count + 1; - m_vertices = (b2Vec2*)b2Alloc(m_count * sizeof(b2Vec2)); - memcpy(m_vertices, vertices, count * sizeof(b2Vec2)); - m_vertices[count] = m_vertices[0]; - m_prevVertex = m_vertices[m_count - 2]; - m_nextVertex = m_vertices[1]; - m_hasPrevVertex = true; - m_hasNextVertex = true; -} - -void b2ChainShape::CreateChain(const b2Vec2* vertices, int32 count) -{ - b2Assert(m_vertices == NULL && m_count == 0); - b2Assert(count >= 2); - for (int32 i = 1; i < count; ++i) - { - // If the code crashes here, it means your vertices are too close together. - b2Assert(b2DistanceSquared(vertices[i-1], vertices[i]) > b2_linearSlop * b2_linearSlop); - } - - m_count = count; - m_vertices = (b2Vec2*)b2Alloc(count * sizeof(b2Vec2)); - memcpy(m_vertices, vertices, m_count * sizeof(b2Vec2)); - - m_hasPrevVertex = false; - m_hasNextVertex = false; - - m_prevVertex.SetZero(); - m_nextVertex.SetZero(); -} - -void b2ChainShape::SetPrevVertex(const b2Vec2& prevVertex) -{ - m_prevVertex = prevVertex; - m_hasPrevVertex = true; -} - -void b2ChainShape::SetNextVertex(const b2Vec2& nextVertex) -{ - m_nextVertex = nextVertex; - m_hasNextVertex = true; -} - -b2Shape* b2ChainShape::Clone(b2BlockAllocator* allocator) const -{ - void* mem = allocator->Allocate(sizeof(b2ChainShape)); - b2ChainShape* clone = new (mem) b2ChainShape; - clone->CreateChain(m_vertices, m_count); - clone->m_prevVertex = m_prevVertex; - clone->m_nextVertex = m_nextVertex; - clone->m_hasPrevVertex = m_hasPrevVertex; - clone->m_hasNextVertex = m_hasNextVertex; - return clone; -} - -int32 b2ChainShape::GetChildCount() const -{ - // edge count = vertex count - 1 - return m_count - 1; -} - -void b2ChainShape::GetChildEdge(b2EdgeShape* edge, int32 index) const -{ - b2Assert(0 <= index && index < m_count - 1); - edge->m_type = b2Shape::e_edge; - edge->m_radius = m_radius; - - edge->m_vertex1 = m_vertices[index + 0]; - edge->m_vertex2 = m_vertices[index + 1]; - - if (index > 0) - { - edge->m_vertex0 = m_vertices[index - 1]; - edge->m_hasVertex0 = true; - } - else - { - edge->m_vertex0 = m_prevVertex; - edge->m_hasVertex0 = m_hasPrevVertex; - } - - if (index < m_count - 2) - { - edge->m_vertex3 = m_vertices[index + 2]; - edge->m_hasVertex3 = true; - } - else - { - edge->m_vertex3 = m_nextVertex; - edge->m_hasVertex3 = m_hasNextVertex; - } -} - -bool b2ChainShape::TestPoint(const b2Transform& xf, const b2Vec2& p) const -{ - B2_NOT_USED(xf); - B2_NOT_USED(p); - return false; -} - -bool b2ChainShape::RayCast(b2RayCastOutput* output, const b2RayCastInput& input, - const b2Transform& xf, int32 childIndex) const -{ - b2Assert(childIndex < m_count); - - b2EdgeShape edgeShape; - - int32 i1 = childIndex; - int32 i2 = childIndex + 1; - if (i2 == m_count) - { - i2 = 0; - } - - edgeShape.m_vertex1 = m_vertices[i1]; - edgeShape.m_vertex2 = m_vertices[i2]; - - return edgeShape.RayCast(output, input, xf, 0); -} - -void b2ChainShape::ComputeAABB(b2AABB* aabb, const b2Transform& xf, int32 childIndex) const -{ - b2Assert(childIndex < m_count); - - int32 i1 = childIndex; - int32 i2 = childIndex + 1; - if (i2 == m_count) - { - i2 = 0; - } - - b2Vec2 v1 = b2Mul(xf, m_vertices[i1]); - b2Vec2 v2 = b2Mul(xf, m_vertices[i2]); - - aabb->lowerBound = b2Min(v1, v2); - aabb->upperBound = b2Max(v1, v2); -} - -void b2ChainShape::ComputeMass(b2MassData* massData, float32 density) const -{ - B2_NOT_USED(density); - - massData->mass = 0.0f; - massData->center.SetZero(); - massData->I = 0.0f; -} +// MIT License + +// Copyright (c) 2019 Erin Catto + +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: + +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. + +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. + +#include "box2d/b2_chain_shape.h" +#include "box2d/b2_edge_shape.h" + +#include "box2d/b2_block_allocator.h" + +#include +#include + +b2ChainShape::~b2ChainShape() +{ + Clear(); +} + +void b2ChainShape::Clear() +{ + b2Free(m_vertices); + m_vertices = nullptr; + m_count = 0; +} + +void b2ChainShape::CreateLoop(const b2Vec2* vertices, int32 count) +{ + b2Assert(m_vertices == nullptr && m_count == 0); + b2Assert(count >= 3); + if (count < 3) + { + return; + } + + for (int32 i = 1; i < count; ++i) + { + b2Vec2 v1 = vertices[i-1]; + b2Vec2 v2 = vertices[i]; + // If the code crashes here, it means your vertices are too close together. + b2Assert(b2DistanceSquared(v1, v2) > b2_linearSlop * b2_linearSlop); + } + + m_count = count + 1; + m_vertices = (b2Vec2*)b2Alloc(m_count * sizeof(b2Vec2)); + memcpy(m_vertices, vertices, count * sizeof(b2Vec2)); + m_vertices[count] = m_vertices[0]; + m_prevVertex = m_vertices[m_count - 2]; + m_nextVertex = m_vertices[1]; +} + +void b2ChainShape::CreateChain(const b2Vec2* vertices, int32 count, const b2Vec2& prevVertex, const b2Vec2& nextVertex) +{ + b2Assert(m_vertices == nullptr && m_count == 0); + b2Assert(count >= 2); + for (int32 i = 1; i < count; ++i) + { + // If the code crashes here, it means your vertices are too close together. + b2Assert(b2DistanceSquared(vertices[i-1], vertices[i]) > b2_linearSlop * b2_linearSlop); + } + + m_count = count; + m_vertices = (b2Vec2*)b2Alloc(count * sizeof(b2Vec2)); + memcpy(m_vertices, vertices, m_count * sizeof(b2Vec2)); + + m_prevVertex = prevVertex; + m_nextVertex = nextVertex; +} + +b2Shape* b2ChainShape::Clone(b2BlockAllocator* allocator) const +{ + void* mem = allocator->Allocate(sizeof(b2ChainShape)); + b2ChainShape* clone = new (mem) b2ChainShape; + clone->CreateChain(m_vertices, m_count, m_prevVertex, m_nextVertex); + return clone; +} + +int32 b2ChainShape::GetChildCount() const +{ + // edge count = vertex count - 1 + return m_count - 1; +} + +void b2ChainShape::GetChildEdge(b2EdgeShape* edge, int32 index) const +{ + b2Assert(0 <= index && index < m_count - 1); + edge->m_type = b2Shape::e_edge; + edge->m_radius = m_radius; + + edge->m_vertex1 = m_vertices[index + 0]; + edge->m_vertex2 = m_vertices[index + 1]; + edge->m_oneSided = true; + + if (index > 0) + { + edge->m_vertex0 = m_vertices[index - 1]; + } + else + { + edge->m_vertex0 = m_prevVertex; + } + + if (index < m_count - 2) + { + edge->m_vertex3 = m_vertices[index + 2]; + } + else + { + edge->m_vertex3 = m_nextVertex; + } +} + +bool b2ChainShape::TestPoint(const b2Transform& xf, const b2Vec2& p) const +{ + B2_NOT_USED(xf); + B2_NOT_USED(p); + return false; +} + +bool b2ChainShape::RayCast(b2RayCastOutput* output, const b2RayCastInput& input, + const b2Transform& xf, int32 childIndex) const +{ + b2Assert(childIndex < m_count); + + b2EdgeShape edgeShape; + + int32 i1 = childIndex; + int32 i2 = childIndex + 1; + if (i2 == m_count) + { + i2 = 0; + } + + edgeShape.m_vertex1 = m_vertices[i1]; + edgeShape.m_vertex2 = m_vertices[i2]; + + return edgeShape.RayCast(output, input, xf, 0); +} + +void b2ChainShape::ComputeAABB(b2AABB* aabb, const b2Transform& xf, int32 childIndex) const +{ + b2Assert(childIndex < m_count); + + int32 i1 = childIndex; + int32 i2 = childIndex + 1; + if (i2 == m_count) + { + i2 = 0; + } + + b2Vec2 v1 = b2Mul(xf, m_vertices[i1]); + b2Vec2 v2 = b2Mul(xf, m_vertices[i2]); + + b2Vec2 lower = b2Min(v1, v2); + b2Vec2 upper = b2Max(v1, v2); + + b2Vec2 r(m_radius, m_radius); + aabb->lowerBound = lower - r; + aabb->upperBound = upper + r; +} + +void b2ChainShape::ComputeMass(b2MassData* massData, float density) const +{ + B2_NOT_USED(density); + + massData->mass = 0.0f; + massData->center.SetZero(); + massData->I = 0.0f; +} diff --git a/libs/Box2D/Collision/Shapes/b2CircleShape.cpp b/libs/box2d/src/collision/b2_circle_shape.cpp similarity index 56% rename from libs/Box2D/Collision/Shapes/b2CircleShape.cpp rename to libs/box2d/src/collision/b2_circle_shape.cpp index 47e5b372..ecc69293 100644 --- a/libs/Box2D/Collision/Shapes/b2CircleShape.cpp +++ b/libs/box2d/src/collision/b2_circle_shape.cpp @@ -1,99 +1,105 @@ -/* -* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ - -#include -#include - -b2Shape* b2CircleShape::Clone(b2BlockAllocator* allocator) const -{ - void* mem = allocator->Allocate(sizeof(b2CircleShape)); - b2CircleShape* clone = new (mem) b2CircleShape; - *clone = *this; - return clone; -} - -int32 b2CircleShape::GetChildCount() const -{ - return 1; -} - -bool b2CircleShape::TestPoint(const b2Transform& transform, const b2Vec2& p) const -{ - b2Vec2 center = transform.p + b2Mul(transform.q, m_p); - b2Vec2 d = p - center; - return b2Dot(d, d) <= m_radius * m_radius; -} - -// Collision Detection in Interactive 3D Environments by Gino van den Bergen -// From Section 3.1.2 -// x = s + a * r -// norm(x) = radius -bool b2CircleShape::RayCast(b2RayCastOutput* output, const b2RayCastInput& input, - const b2Transform& transform, int32 childIndex) const -{ - B2_NOT_USED(childIndex); - - b2Vec2 position = transform.p + b2Mul(transform.q, m_p); - b2Vec2 s = input.p1 - position; - float32 b = b2Dot(s, s) - m_radius * m_radius; - - // Solve quadratic equation. - b2Vec2 r = input.p2 - input.p1; - float32 c = b2Dot(s, r); - float32 rr = b2Dot(r, r); - float32 sigma = c * c - rr * b; - - // Check for negative discriminant and short segment. - if (sigma < 0.0f || rr < b2_epsilon) - { - return false; - } - - // Find the point of intersection of the line with the circle. - float32 a = -(c + b2Sqrt(sigma)); - - // Is the intersection point on the segment? - if (0.0f <= a && a <= input.maxFraction * rr) - { - a /= rr; - output->fraction = a; - output->normal = s + a * r; - output->normal.Normalize(); - return true; - } - - return false; -} - -void b2CircleShape::ComputeAABB(b2AABB* aabb, const b2Transform& transform, int32 childIndex) const -{ - B2_NOT_USED(childIndex); - - b2Vec2 p = transform.p + b2Mul(transform.q, m_p); - aabb->lowerBound.Set(p.x - m_radius, p.y - m_radius); - aabb->upperBound.Set(p.x + m_radius, p.y + m_radius); -} - -void b2CircleShape::ComputeMass(b2MassData* massData, float32 density) const -{ - massData->mass = density * b2_pi * m_radius * m_radius; - massData->center = m_p; - - // inertia about the local origin - massData->I = massData->mass * (0.5f * m_radius * m_radius + b2Dot(m_p, m_p)); -} +// MIT License + +// Copyright (c) 2019 Erin Catto + +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: + +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. + +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. + +#include "box2d/b2_circle_shape.h" +#include "box2d/b2_block_allocator.h" + +#include + +b2Shape* b2CircleShape::Clone(b2BlockAllocator* allocator) const +{ + void* mem = allocator->Allocate(sizeof(b2CircleShape)); + b2CircleShape* clone = new (mem) b2CircleShape; + *clone = *this; + return clone; +} + +int32 b2CircleShape::GetChildCount() const +{ + return 1; +} + +bool b2CircleShape::TestPoint(const b2Transform& transform, const b2Vec2& p) const +{ + b2Vec2 center = transform.p + b2Mul(transform.q, m_p); + b2Vec2 d = p - center; + return b2Dot(d, d) <= m_radius * m_radius; +} + +// Collision Detection in Interactive 3D Environments by Gino van den Bergen +// From Section 3.1.2 +// x = s + a * r +// norm(x) = radius +bool b2CircleShape::RayCast(b2RayCastOutput* output, const b2RayCastInput& input, + const b2Transform& transform, int32 childIndex) const +{ + B2_NOT_USED(childIndex); + + b2Vec2 position = transform.p + b2Mul(transform.q, m_p); + b2Vec2 s = input.p1 - position; + float b = b2Dot(s, s) - m_radius * m_radius; + + // Solve quadratic equation. + b2Vec2 r = input.p2 - input.p1; + float c = b2Dot(s, r); + float rr = b2Dot(r, r); + float sigma = c * c - rr * b; + + // Check for negative discriminant and short segment. + if (sigma < 0.0f || rr < b2_epsilon) + { + return false; + } + + // Find the point of intersection of the line with the circle. + float a = -(c + b2Sqrt(sigma)); + + // Is the intersection point on the segment? + if (0.0f <= a && a <= input.maxFraction * rr) + { + a /= rr; + output->fraction = a; + output->normal = s + a * r; + output->normal.Normalize(); + return true; + } + + return false; +} + +void b2CircleShape::ComputeAABB(b2AABB* aabb, const b2Transform& transform, int32 childIndex) const +{ + B2_NOT_USED(childIndex); + + b2Vec2 p = transform.p + b2Mul(transform.q, m_p); + aabb->lowerBound.Set(p.x - m_radius, p.y - m_radius); + aabb->upperBound.Set(p.x + m_radius, p.y + m_radius); +} + +void b2CircleShape::ComputeMass(b2MassData* massData, float density) const +{ + massData->mass = density * b2_pi * m_radius * m_radius; + massData->center = m_p; + + // inertia about the local origin + massData->I = massData->mass * (0.5f * m_radius * m_radius + b2Dot(m_p, m_p)); +} diff --git a/libs/Box2D/Collision/b2CollideCircle.cpp b/libs/box2d/src/collision/b2_collide_circle.cpp similarity index 63% rename from libs/Box2D/Collision/b2CollideCircle.cpp rename to libs/box2d/src/collision/b2_collide_circle.cpp index e60b2086..469da559 100644 --- a/libs/Box2D/Collision/b2CollideCircle.cpp +++ b/libs/box2d/src/collision/b2_collide_circle.cpp @@ -1,154 +1,158 @@ -/* -* Copyright (c) 2007-2009 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ - -#include -#include -#include - -void b2CollideCircles( - b2Manifold* manifold, - const b2CircleShape* circleA, const b2Transform& xfA, - const b2CircleShape* circleB, const b2Transform& xfB) -{ - manifold->pointCount = 0; - - b2Vec2 pA = b2Mul(xfA, circleA->m_p); - b2Vec2 pB = b2Mul(xfB, circleB->m_p); - - b2Vec2 d = pB - pA; - float32 distSqr = b2Dot(d, d); - float32 rA = circleA->m_radius, rB = circleB->m_radius; - float32 radius = rA + rB; - if (distSqr > radius * radius) - { - return; - } - - manifold->type = b2Manifold::e_circles; - manifold->localPoint = circleA->m_p; - manifold->localNormal.SetZero(); - manifold->pointCount = 1; - - manifold->points[0].localPoint = circleB->m_p; - manifold->points[0].id.key = 0; -} - -void b2CollidePolygonAndCircle( - b2Manifold* manifold, - const b2PolygonShape* polygonA, const b2Transform& xfA, - const b2CircleShape* circleB, const b2Transform& xfB) -{ - manifold->pointCount = 0; - - // Compute circle position in the frame of the polygon. - b2Vec2 c = b2Mul(xfB, circleB->m_p); - b2Vec2 cLocal = b2MulT(xfA, c); - - // Find the min separating edge. - int32 normalIndex = 0; - float32 separation = -b2_maxFloat; - float32 radius = polygonA->m_radius + circleB->m_radius; - int32 vertexCount = polygonA->m_count; - const b2Vec2* vertices = polygonA->m_vertices; - const b2Vec2* normals = polygonA->m_normals; - - for (int32 i = 0; i < vertexCount; ++i) - { - float32 s = b2Dot(normals[i], cLocal - vertices[i]); - - if (s > radius) - { - // Early out. - return; - } - - if (s > separation) - { - separation = s; - normalIndex = i; - } - } - - // Vertices that subtend the incident face. - int32 vertIndex1 = normalIndex; - int32 vertIndex2 = vertIndex1 + 1 < vertexCount ? vertIndex1 + 1 : 0; - b2Vec2 v1 = vertices[vertIndex1]; - b2Vec2 v2 = vertices[vertIndex2]; - - // If the center is inside the polygon ... - if (separation < b2_epsilon) - { - manifold->pointCount = 1; - manifold->type = b2Manifold::e_faceA; - manifold->localNormal = normals[normalIndex]; - manifold->localPoint = 0.5f * (v1 + v2); - manifold->points[0].localPoint = circleB->m_p; - manifold->points[0].id.key = 0; - return; - } - - // Compute barycentric coordinates - float32 u1 = b2Dot(cLocal - v1, v2 - v1); - float32 u2 = b2Dot(cLocal - v2, v1 - v2); - if (u1 <= 0.0f) - { - if (b2DistanceSquared(cLocal, v1) > radius * radius) - { - return; - } - - manifold->pointCount = 1; - manifold->type = b2Manifold::e_faceA; - manifold->localNormal = cLocal - v1; - manifold->localNormal.Normalize(); - manifold->localPoint = v1; - manifold->points[0].localPoint = circleB->m_p; - manifold->points[0].id.key = 0; - } - else if (u2 <= 0.0f) - { - if (b2DistanceSquared(cLocal, v2) > radius * radius) - { - return; - } - - manifold->pointCount = 1; - manifold->type = b2Manifold::e_faceA; - manifold->localNormal = cLocal - v2; - manifold->localNormal.Normalize(); - manifold->localPoint = v2; - manifold->points[0].localPoint = circleB->m_p; - manifold->points[0].id.key = 0; - } - else - { - b2Vec2 faceCenter = 0.5f * (v1 + v2); - float32 separation = b2Dot(cLocal - faceCenter, normals[vertIndex1]); - if (separation > radius) - { - return; - } - - manifold->pointCount = 1; - manifold->type = b2Manifold::e_faceA; - manifold->localNormal = normals[vertIndex1]; - manifold->localPoint = faceCenter; - manifold->points[0].localPoint = circleB->m_p; - manifold->points[0].id.key = 0; - } -} +// MIT License + +// Copyright (c) 2019 Erin Catto + +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: + +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. + +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. + +#include "box2d/b2_collision.h" +#include "box2d/b2_circle_shape.h" +#include "box2d/b2_polygon_shape.h" + +void b2CollideCircles( + b2Manifold* manifold, + const b2CircleShape* circleA, const b2Transform& xfA, + const b2CircleShape* circleB, const b2Transform& xfB) +{ + manifold->pointCount = 0; + + b2Vec2 pA = b2Mul(xfA, circleA->m_p); + b2Vec2 pB = b2Mul(xfB, circleB->m_p); + + b2Vec2 d = pB - pA; + float distSqr = b2Dot(d, d); + float rA = circleA->m_radius, rB = circleB->m_radius; + float radius = rA + rB; + if (distSqr > radius * radius) + { + return; + } + + manifold->type = b2Manifold::e_circles; + manifold->localPoint = circleA->m_p; + manifold->localNormal.SetZero(); + manifold->pointCount = 1; + + manifold->points[0].localPoint = circleB->m_p; + manifold->points[0].id.key = 0; +} + +void b2CollidePolygonAndCircle( + b2Manifold* manifold, + const b2PolygonShape* polygonA, const b2Transform& xfA, + const b2CircleShape* circleB, const b2Transform& xfB) +{ + manifold->pointCount = 0; + + // Compute circle position in the frame of the polygon. + b2Vec2 c = b2Mul(xfB, circleB->m_p); + b2Vec2 cLocal = b2MulT(xfA, c); + + // Find the min separating edge. + int32 normalIndex = 0; + float separation = -b2_maxFloat; + float radius = polygonA->m_radius + circleB->m_radius; + int32 vertexCount = polygonA->m_count; + const b2Vec2* vertices = polygonA->m_vertices; + const b2Vec2* normals = polygonA->m_normals; + + for (int32 i = 0; i < vertexCount; ++i) + { + float s = b2Dot(normals[i], cLocal - vertices[i]); + + if (s > radius) + { + // Early out. + return; + } + + if (s > separation) + { + separation = s; + normalIndex = i; + } + } + + // Vertices that subtend the incident face. + int32 vertIndex1 = normalIndex; + int32 vertIndex2 = vertIndex1 + 1 < vertexCount ? vertIndex1 + 1 : 0; + b2Vec2 v1 = vertices[vertIndex1]; + b2Vec2 v2 = vertices[vertIndex2]; + + // If the center is inside the polygon ... + if (separation < b2_epsilon) + { + manifold->pointCount = 1; + manifold->type = b2Manifold::e_faceA; + manifold->localNormal = normals[normalIndex]; + manifold->localPoint = 0.5f * (v1 + v2); + manifold->points[0].localPoint = circleB->m_p; + manifold->points[0].id.key = 0; + return; + } + + // Compute barycentric coordinates + float u1 = b2Dot(cLocal - v1, v2 - v1); + float u2 = b2Dot(cLocal - v2, v1 - v2); + if (u1 <= 0.0f) + { + if (b2DistanceSquared(cLocal, v1) > radius * radius) + { + return; + } + + manifold->pointCount = 1; + manifold->type = b2Manifold::e_faceA; + manifold->localNormal = cLocal - v1; + manifold->localNormal.Normalize(); + manifold->localPoint = v1; + manifold->points[0].localPoint = circleB->m_p; + manifold->points[0].id.key = 0; + } + else if (u2 <= 0.0f) + { + if (b2DistanceSquared(cLocal, v2) > radius * radius) + { + return; + } + + manifold->pointCount = 1; + manifold->type = b2Manifold::e_faceA; + manifold->localNormal = cLocal - v2; + manifold->localNormal.Normalize(); + manifold->localPoint = v2; + manifold->points[0].localPoint = circleB->m_p; + manifold->points[0].id.key = 0; + } + else + { + b2Vec2 faceCenter = 0.5f * (v1 + v2); + float s = b2Dot(cLocal - faceCenter, normals[vertIndex1]); + if (s > radius) + { + return; + } + + manifold->pointCount = 1; + manifold->type = b2Manifold::e_faceA; + manifold->localNormal = normals[vertIndex1]; + manifold->localPoint = faceCenter; + manifold->points[0].localPoint = circleB->m_p; + manifold->points[0].id.key = 0; + } +} diff --git a/libs/box2d/src/collision/b2_collide_edge.cpp b/libs/box2d/src/collision/b2_collide_edge.cpp new file mode 100644 index 00000000..e06b9009 --- /dev/null +++ b/libs/box2d/src/collision/b2_collide_edge.cpp @@ -0,0 +1,524 @@ +// MIT License + +// Copyright (c) 2019 Erin Catto + +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: + +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. + +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. + +#include "box2d/b2_collision.h" +#include "box2d/b2_circle_shape.h" +#include "box2d/b2_edge_shape.h" +#include "box2d/b2_polygon_shape.h" + + +// Compute contact points for edge versus circle. +// This accounts for edge connectivity. +void b2CollideEdgeAndCircle(b2Manifold* manifold, + const b2EdgeShape* edgeA, const b2Transform& xfA, + const b2CircleShape* circleB, const b2Transform& xfB) +{ + manifold->pointCount = 0; + + // Compute circle in frame of edge + b2Vec2 Q = b2MulT(xfA, b2Mul(xfB, circleB->m_p)); + + b2Vec2 A = edgeA->m_vertex1, B = edgeA->m_vertex2; + b2Vec2 e = B - A; + + // Normal points to the right for a CCW winding + b2Vec2 n(e.y, -e.x); + float offset = b2Dot(n, Q - A); + + bool oneSided = edgeA->m_oneSided; + if (oneSided && offset < 0.0f) + { + return; + } + + // Barycentric coordinates + float u = b2Dot(e, B - Q); + float v = b2Dot(e, Q - A); + + float radius = edgeA->m_radius + circleB->m_radius; + + b2ContactFeature cf; + cf.indexB = 0; + cf.typeB = b2ContactFeature::e_vertex; + + // Region A + if (v <= 0.0f) + { + b2Vec2 P = A; + b2Vec2 d = Q - P; + float dd = b2Dot(d, d); + if (dd > radius * radius) + { + return; + } + + // Is there an edge connected to A? + if (edgeA->m_oneSided) + { + b2Vec2 A1 = edgeA->m_vertex0; + b2Vec2 B1 = A; + b2Vec2 e1 = B1 - A1; + float u1 = b2Dot(e1, B1 - Q); + + // Is the circle in Region AB of the previous edge? + if (u1 > 0.0f) + { + return; + } + } + + cf.indexA = 0; + cf.typeA = b2ContactFeature::e_vertex; + manifold->pointCount = 1; + manifold->type = b2Manifold::e_circles; + manifold->localNormal.SetZero(); + manifold->localPoint = P; + manifold->points[0].id.key = 0; + manifold->points[0].id.cf = cf; + manifold->points[0].localPoint = circleB->m_p; + return; + } + + // Region B + if (u <= 0.0f) + { + b2Vec2 P = B; + b2Vec2 d = Q - P; + float dd = b2Dot(d, d); + if (dd > radius * radius) + { + return; + } + + // Is there an edge connected to B? + if (edgeA->m_oneSided) + { + b2Vec2 B2 = edgeA->m_vertex3; + b2Vec2 A2 = B; + b2Vec2 e2 = B2 - A2; + float v2 = b2Dot(e2, Q - A2); + + // Is the circle in Region AB of the next edge? + if (v2 > 0.0f) + { + return; + } + } + + cf.indexA = 1; + cf.typeA = b2ContactFeature::e_vertex; + manifold->pointCount = 1; + manifold->type = b2Manifold::e_circles; + manifold->localNormal.SetZero(); + manifold->localPoint = P; + manifold->points[0].id.key = 0; + manifold->points[0].id.cf = cf; + manifold->points[0].localPoint = circleB->m_p; + return; + } + + // Region AB + float den = b2Dot(e, e); + b2Assert(den > 0.0f); + b2Vec2 P = (1.0f / den) * (u * A + v * B); + b2Vec2 d = Q - P; + float dd = b2Dot(d, d); + if (dd > radius * radius) + { + return; + } + + if (offset < 0.0f) + { + n.Set(-n.x, -n.y); + } + n.Normalize(); + + cf.indexA = 0; + cf.typeA = b2ContactFeature::e_face; + manifold->pointCount = 1; + manifold->type = b2Manifold::e_faceA; + manifold->localNormal = n; + manifold->localPoint = A; + manifold->points[0].id.key = 0; + manifold->points[0].id.cf = cf; + manifold->points[0].localPoint = circleB->m_p; +} + +// This structure is used to keep track of the best separating axis. +struct b2EPAxis +{ + enum Type + { + e_unknown, + e_edgeA, + e_edgeB + }; + + b2Vec2 normal; + Type type; + int32 index; + float separation; +}; + +// This holds polygon B expressed in frame A. +struct b2TempPolygon +{ + b2Vec2 vertices[b2_maxPolygonVertices]; + b2Vec2 normals[b2_maxPolygonVertices]; + int32 count; +}; + +// Reference face used for clipping +struct b2ReferenceFace +{ + int32 i1, i2; + b2Vec2 v1, v2; + b2Vec2 normal; + + b2Vec2 sideNormal1; + float sideOffset1; + + b2Vec2 sideNormal2; + float sideOffset2; +}; + +static b2EPAxis b2ComputeEdgeSeparation(const b2TempPolygon& polygonB, const b2Vec2& v1, const b2Vec2& normal1) +{ + b2EPAxis axis; + axis.type = b2EPAxis::e_edgeA; + axis.index = -1; + axis.separation = -FLT_MAX; + axis.normal.SetZero(); + + b2Vec2 axes[2] = { normal1, -normal1 }; + + // Find axis with least overlap (min-max problem) + for (int32 j = 0; j < 2; ++j) + { + float sj = FLT_MAX; + + // Find deepest polygon vertex along axis j + for (int32 i = 0; i < polygonB.count; ++i) + { + float si = b2Dot(axes[j], polygonB.vertices[i] - v1); + if (si < sj) + { + sj = si; + } + } + + if (sj > axis.separation) + { + axis.index = j; + axis.separation = sj; + axis.normal = axes[j]; + } + } + + return axis; +} + +static b2EPAxis b2ComputePolygonSeparation(const b2TempPolygon& polygonB, const b2Vec2& v1, const b2Vec2& v2) +{ + b2EPAxis axis; + axis.type = b2EPAxis::e_unknown; + axis.index = -1; + axis.separation = -FLT_MAX; + axis.normal.SetZero(); + + for (int32 i = 0; i < polygonB.count; ++i) + { + b2Vec2 n = -polygonB.normals[i]; + + float s1 = b2Dot(n, polygonB.vertices[i] - v1); + float s2 = b2Dot(n, polygonB.vertices[i] - v2); + float s = b2Min(s1, s2); + + if (s > axis.separation) + { + axis.type = b2EPAxis::e_edgeB; + axis.index = i; + axis.separation = s; + axis.normal = n; + } + } + + return axis; +} + +void b2CollideEdgeAndPolygon(b2Manifold* manifold, + const b2EdgeShape* edgeA, const b2Transform& xfA, + const b2PolygonShape* polygonB, const b2Transform& xfB) +{ + manifold->pointCount = 0; + + b2Transform xf = b2MulT(xfA, xfB); + + b2Vec2 centroidB = b2Mul(xf, polygonB->m_centroid); + + b2Vec2 v1 = edgeA->m_vertex1; + b2Vec2 v2 = edgeA->m_vertex2; + + b2Vec2 edge1 = v2 - v1; + edge1.Normalize(); + + // Normal points to the right for a CCW winding + b2Vec2 normal1(edge1.y, -edge1.x); + float offset1 = b2Dot(normal1, centroidB - v1); + + bool oneSided = edgeA->m_oneSided; + if (oneSided && offset1 < 0.0f) + { + return; + } + + // Get polygonB in frameA + b2TempPolygon tempPolygonB; + tempPolygonB.count = polygonB->m_count; + for (int32 i = 0; i < polygonB->m_count; ++i) + { + tempPolygonB.vertices[i] = b2Mul(xf, polygonB->m_vertices[i]); + tempPolygonB.normals[i] = b2Mul(xf.q, polygonB->m_normals[i]); + } + + float radius = polygonB->m_radius + edgeA->m_radius; + + b2EPAxis edgeAxis = b2ComputeEdgeSeparation(tempPolygonB, v1, normal1); + if (edgeAxis.separation > radius) + { + return; + } + + b2EPAxis polygonAxis = b2ComputePolygonSeparation(tempPolygonB, v1, v2); + if (polygonAxis.separation > radius) + { + return; + } + + // Use hysteresis for jitter reduction. + const float k_relativeTol = 0.98f; + const float k_absoluteTol = 0.001f; + + b2EPAxis primaryAxis; + if (polygonAxis.separation - radius > k_relativeTol * (edgeAxis.separation - radius) + k_absoluteTol) + { + primaryAxis = polygonAxis; + } + else + { + primaryAxis = edgeAxis; + } + + if (oneSided) + { + // Smooth collision + // See https://box2d.org/posts/2020/06/ghost-collisions/ + + b2Vec2 edge0 = v1 - edgeA->m_vertex0; + edge0.Normalize(); + b2Vec2 normal0(edge0.y, -edge0.x); + bool convex1 = b2Cross(edge0, edge1) >= 0.0f; + + b2Vec2 edge2 = edgeA->m_vertex3 - v2; + edge2.Normalize(); + b2Vec2 normal2(edge2.y, -edge2.x); + bool convex2 = b2Cross(edge1, edge2) >= 0.0f; + + const float sinTol = 0.1f; + bool side1 = b2Dot(primaryAxis.normal, edge1) <= 0.0f; + + // Check Gauss Map + if (side1) + { + if (convex1) + { + if (b2Cross(primaryAxis.normal, normal0) > sinTol) + { + // Skip region + return; + } + + // Admit region + } + else + { + // Snap region + primaryAxis = edgeAxis; + } + } + else + { + if (convex2) + { + if (b2Cross(normal2, primaryAxis.normal) > sinTol) + { + // Skip region + return; + } + + // Admit region + } + else + { + // Snap region + primaryAxis = edgeAxis; + } + } + } + + b2ClipVertex clipPoints[2]; + b2ReferenceFace ref; + if (primaryAxis.type == b2EPAxis::e_edgeA) + { + manifold->type = b2Manifold::e_faceA; + + // Search for the polygon normal that is most anti-parallel to the edge normal. + int32 bestIndex = 0; + float bestValue = b2Dot(primaryAxis.normal, tempPolygonB.normals[0]); + for (int32 i = 1; i < tempPolygonB.count; ++i) + { + float value = b2Dot(primaryAxis.normal, tempPolygonB.normals[i]); + if (value < bestValue) + { + bestValue = value; + bestIndex = i; + } + } + + int32 i1 = bestIndex; + int32 i2 = i1 + 1 < tempPolygonB.count ? i1 + 1 : 0; + + clipPoints[0].v = tempPolygonB.vertices[i1]; + clipPoints[0].id.cf.indexA = 0; + clipPoints[0].id.cf.indexB = static_cast(i1); + clipPoints[0].id.cf.typeA = b2ContactFeature::e_face; + clipPoints[0].id.cf.typeB = b2ContactFeature::e_vertex; + + clipPoints[1].v = tempPolygonB.vertices[i2]; + clipPoints[1].id.cf.indexA = 0; + clipPoints[1].id.cf.indexB = static_cast(i2); + clipPoints[1].id.cf.typeA = b2ContactFeature::e_face; + clipPoints[1].id.cf.typeB = b2ContactFeature::e_vertex; + + ref.i1 = 0; + ref.i2 = 1; + ref.v1 = v1; + ref.v2 = v2; + ref.normal = primaryAxis.normal; + ref.sideNormal1 = -edge1; + ref.sideNormal2 = edge1; + } + else + { + manifold->type = b2Manifold::e_faceB; + + clipPoints[0].v = v2; + clipPoints[0].id.cf.indexA = 1; + clipPoints[0].id.cf.indexB = static_cast(primaryAxis.index); + clipPoints[0].id.cf.typeA = b2ContactFeature::e_vertex; + clipPoints[0].id.cf.typeB = b2ContactFeature::e_face; + + clipPoints[1].v = v1; + clipPoints[1].id.cf.indexA = 0; + clipPoints[1].id.cf.indexB = static_cast(primaryAxis.index); + clipPoints[1].id.cf.typeA = b2ContactFeature::e_vertex; + clipPoints[1].id.cf.typeB = b2ContactFeature::e_face; + + ref.i1 = primaryAxis.index; + ref.i2 = ref.i1 + 1 < tempPolygonB.count ? ref.i1 + 1 : 0; + ref.v1 = tempPolygonB.vertices[ref.i1]; + ref.v2 = tempPolygonB.vertices[ref.i2]; + ref.normal = tempPolygonB.normals[ref.i1]; + + // CCW winding + ref.sideNormal1.Set(ref.normal.y, -ref.normal.x); + ref.sideNormal2 = -ref.sideNormal1; + } + + ref.sideOffset1 = b2Dot(ref.sideNormal1, ref.v1); + ref.sideOffset2 = b2Dot(ref.sideNormal2, ref.v2); + + // Clip incident edge against reference face side planes + b2ClipVertex clipPoints1[2]; + b2ClipVertex clipPoints2[2]; + int32 np; + + // Clip to side 1 + np = b2ClipSegmentToLine(clipPoints1, clipPoints, ref.sideNormal1, ref.sideOffset1, ref.i1); + + if (np < b2_maxManifoldPoints) + { + return; + } + + // Clip to side 2 + np = b2ClipSegmentToLine(clipPoints2, clipPoints1, ref.sideNormal2, ref.sideOffset2, ref.i2); + + if (np < b2_maxManifoldPoints) + { + return; + } + + // Now clipPoints2 contains the clipped points. + if (primaryAxis.type == b2EPAxis::e_edgeA) + { + manifold->localNormal = ref.normal; + manifold->localPoint = ref.v1; + } + else + { + manifold->localNormal = polygonB->m_normals[ref.i1]; + manifold->localPoint = polygonB->m_vertices[ref.i1]; + } + + int32 pointCount = 0; + for (int32 i = 0; i < b2_maxManifoldPoints; ++i) + { + float separation; + + separation = b2Dot(ref.normal, clipPoints2[i].v - ref.v1); + + if (separation <= radius) + { + b2ManifoldPoint* cp = manifold->points + pointCount; + + if (primaryAxis.type == b2EPAxis::e_edgeA) + { + cp->localPoint = b2MulT(xf, clipPoints2[i].v); + cp->id = clipPoints2[i].id; + } + else + { + cp->localPoint = clipPoints2[i].v; + cp->id.cf.typeA = clipPoints2[i].id.cf.typeB; + cp->id.cf.typeB = clipPoints2[i].id.cf.typeA; + cp->id.cf.indexA = clipPoints2[i].id.cf.indexB; + cp->id.cf.indexB = clipPoints2[i].id.cf.indexA; + } + + ++pointCount; + } + } + + manifold->pointCount = pointCount; +} diff --git a/libs/Box2D/Collision/b2CollidePolygon.cpp b/libs/box2d/src/collision/b2_collide_polygon.cpp similarity index 71% rename from libs/Box2D/Collision/b2CollidePolygon.cpp rename to libs/box2d/src/collision/b2_collide_polygon.cpp index b24d01e7..f3fa850b 100644 --- a/libs/Box2D/Collision/b2CollidePolygon.cpp +++ b/libs/box2d/src/collision/b2_collide_polygon.cpp @@ -1,239 +1,243 @@ -/* -* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ - -#include -#include - -// Find the max separation between poly1 and poly2 using edge normals from poly1. -static float32 b2FindMaxSeparation(int32* edgeIndex, - const b2PolygonShape* poly1, const b2Transform& xf1, - const b2PolygonShape* poly2, const b2Transform& xf2) -{ - int32 count1 = poly1->m_count; - int32 count2 = poly2->m_count; - const b2Vec2* n1s = poly1->m_normals; - const b2Vec2* v1s = poly1->m_vertices; - const b2Vec2* v2s = poly2->m_vertices; - b2Transform xf = b2MulT(xf2, xf1); - - int32 bestIndex = 0; - float32 maxSeparation = -b2_maxFloat; - for (int32 i = 0; i < count1; ++i) - { - // Get poly1 normal in frame2. - b2Vec2 n = b2Mul(xf.q, n1s[i]); - b2Vec2 v1 = b2Mul(xf, v1s[i]); - - // Find deepest point for normal i. - float32 si = b2_maxFloat; - for (int32 j = 0; j < count2; ++j) - { - float32 sij = b2Dot(n, v2s[j] - v1); - if (sij < si) - { - si = sij; - } - } - - if (si > maxSeparation) - { - maxSeparation = si; - bestIndex = i; - } - } - - *edgeIndex = bestIndex; - return maxSeparation; -} - -static void b2FindIncidentEdge(b2ClipVertex c[2], - const b2PolygonShape* poly1, const b2Transform& xf1, int32 edge1, - const b2PolygonShape* poly2, const b2Transform& xf2) -{ - const b2Vec2* normals1 = poly1->m_normals; - - int32 count2 = poly2->m_count; - const b2Vec2* vertices2 = poly2->m_vertices; - const b2Vec2* normals2 = poly2->m_normals; - - b2Assert(0 <= edge1 && edge1 < poly1->m_count); - - // Get the normal of the reference edge in poly2's frame. - b2Vec2 normal1 = b2MulT(xf2.q, b2Mul(xf1.q, normals1[edge1])); - - // Find the incident edge on poly2. - int32 index = 0; - float32 minDot = b2_maxFloat; - for (int32 i = 0; i < count2; ++i) - { - float32 dot = b2Dot(normal1, normals2[i]); - if (dot < minDot) - { - minDot = dot; - index = i; - } - } - - // Build the clip vertices for the incident edge. - int32 i1 = index; - int32 i2 = i1 + 1 < count2 ? i1 + 1 : 0; - - c[0].v = b2Mul(xf2, vertices2[i1]); - c[0].id.cf.indexA = (uint8)edge1; - c[0].id.cf.indexB = (uint8)i1; - c[0].id.cf.typeA = b2ContactFeature::e_face; - c[0].id.cf.typeB = b2ContactFeature::e_vertex; - - c[1].v = b2Mul(xf2, vertices2[i2]); - c[1].id.cf.indexA = (uint8)edge1; - c[1].id.cf.indexB = (uint8)i2; - c[1].id.cf.typeA = b2ContactFeature::e_face; - c[1].id.cf.typeB = b2ContactFeature::e_vertex; -} - -// Find edge normal of max separation on A - return if separating axis is found -// Find edge normal of max separation on B - return if separation axis is found -// Choose reference edge as min(minA, minB) -// Find incident edge -// Clip - -// The normal points from 1 to 2 -void b2CollidePolygons(b2Manifold* manifold, - const b2PolygonShape* polyA, const b2Transform& xfA, - const b2PolygonShape* polyB, const b2Transform& xfB) -{ - manifold->pointCount = 0; - float32 totalRadius = polyA->m_radius + polyB->m_radius; - - int32 edgeA = 0; - float32 separationA = b2FindMaxSeparation(&edgeA, polyA, xfA, polyB, xfB); - if (separationA > totalRadius) - return; - - int32 edgeB = 0; - float32 separationB = b2FindMaxSeparation(&edgeB, polyB, xfB, polyA, xfA); - if (separationB > totalRadius) - return; - - const b2PolygonShape* poly1; // reference polygon - const b2PolygonShape* poly2; // incident polygon - b2Transform xf1, xf2; - int32 edge1; // reference edge - uint8 flip; - const float32 k_tol = 0.1f * b2_linearSlop; - - if (separationB > separationA + k_tol) - { - poly1 = polyB; - poly2 = polyA; - xf1 = xfB; - xf2 = xfA; - edge1 = edgeB; - manifold->type = b2Manifold::e_faceB; - flip = 1; - } - else - { - poly1 = polyA; - poly2 = polyB; - xf1 = xfA; - xf2 = xfB; - edge1 = edgeA; - manifold->type = b2Manifold::e_faceA; - flip = 0; - } - - b2ClipVertex incidentEdge[2]; - b2FindIncidentEdge(incidentEdge, poly1, xf1, edge1, poly2, xf2); - - int32 count1 = poly1->m_count; - const b2Vec2* vertices1 = poly1->m_vertices; - - int32 iv1 = edge1; - int32 iv2 = edge1 + 1 < count1 ? edge1 + 1 : 0; - - b2Vec2 v11 = vertices1[iv1]; - b2Vec2 v12 = vertices1[iv2]; - - b2Vec2 localTangent = v12 - v11; - localTangent.Normalize(); - - b2Vec2 localNormal = b2Cross(localTangent, 1.0f); - b2Vec2 planePoint = 0.5f * (v11 + v12); - - b2Vec2 tangent = b2Mul(xf1.q, localTangent); - b2Vec2 normal = b2Cross(tangent, 1.0f); - - v11 = b2Mul(xf1, v11); - v12 = b2Mul(xf1, v12); - - // Face offset. - float32 frontOffset = b2Dot(normal, v11); - - // Side offsets, extended by polytope skin thickness. - float32 sideOffset1 = -b2Dot(tangent, v11) + totalRadius; - float32 sideOffset2 = b2Dot(tangent, v12) + totalRadius; - - // Clip incident edge against extruded edge1 side edges. - b2ClipVertex clipPoints1[2]; - b2ClipVertex clipPoints2[2]; - int np; - - // Clip to box side 1 - np = b2ClipSegmentToLine(clipPoints1, incidentEdge, -tangent, sideOffset1, iv1); - - if (np < 2) - return; - - // Clip to negative box side 1 - np = b2ClipSegmentToLine(clipPoints2, clipPoints1, tangent, sideOffset2, iv2); - - if (np < 2) - { - return; - } - - // Now clipPoints2 contains the clipped points. - manifold->localNormal = localNormal; - manifold->localPoint = planePoint; - - int32 pointCount = 0; - for (int32 i = 0; i < b2_maxManifoldPoints; ++i) - { - float32 separation = b2Dot(normal, clipPoints2[i].v) - frontOffset; - - if (separation <= totalRadius) - { - b2ManifoldPoint* cp = manifold->points + pointCount; - cp->localPoint = b2MulT(xf2, clipPoints2[i].v); - cp->id = clipPoints2[i].id; - if (flip) - { - // Swap features - b2ContactFeature cf = cp->id.cf; - cp->id.cf.indexA = cf.indexB; - cp->id.cf.indexB = cf.indexA; - cp->id.cf.typeA = cf.typeB; - cp->id.cf.typeB = cf.typeA; - } - ++pointCount; - } - } - - manifold->pointCount = pointCount; -} +// MIT License + +// Copyright (c) 2019 Erin Catto + +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: + +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. + +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. + +#include "box2d/b2_collision.h" +#include "box2d/b2_polygon_shape.h" + +// Find the max separation between poly1 and poly2 using edge normals from poly1. +static float b2FindMaxSeparation(int32* edgeIndex, + const b2PolygonShape* poly1, const b2Transform& xf1, + const b2PolygonShape* poly2, const b2Transform& xf2) +{ + int32 count1 = poly1->m_count; + int32 count2 = poly2->m_count; + const b2Vec2* n1s = poly1->m_normals; + const b2Vec2* v1s = poly1->m_vertices; + const b2Vec2* v2s = poly2->m_vertices; + b2Transform xf = b2MulT(xf2, xf1); + + int32 bestIndex = 0; + float maxSeparation = -b2_maxFloat; + for (int32 i = 0; i < count1; ++i) + { + // Get poly1 normal in frame2. + b2Vec2 n = b2Mul(xf.q, n1s[i]); + b2Vec2 v1 = b2Mul(xf, v1s[i]); + + // Find deepest point for normal i. + float si = b2_maxFloat; + for (int32 j = 0; j < count2; ++j) + { + float sij = b2Dot(n, v2s[j] - v1); + if (sij < si) + { + si = sij; + } + } + + if (si > maxSeparation) + { + maxSeparation = si; + bestIndex = i; + } + } + + *edgeIndex = bestIndex; + return maxSeparation; +} + +static void b2FindIncidentEdge(b2ClipVertex c[2], + const b2PolygonShape* poly1, const b2Transform& xf1, int32 edge1, + const b2PolygonShape* poly2, const b2Transform& xf2) +{ + const b2Vec2* normals1 = poly1->m_normals; + + int32 count2 = poly2->m_count; + const b2Vec2* vertices2 = poly2->m_vertices; + const b2Vec2* normals2 = poly2->m_normals; + + b2Assert(0 <= edge1 && edge1 < poly1->m_count); + + // Get the normal of the reference edge in poly2's frame. + b2Vec2 normal1 = b2MulT(xf2.q, b2Mul(xf1.q, normals1[edge1])); + + // Find the incident edge on poly2. + int32 index = 0; + float minDot = b2_maxFloat; + for (int32 i = 0; i < count2; ++i) + { + float dot = b2Dot(normal1, normals2[i]); + if (dot < minDot) + { + minDot = dot; + index = i; + } + } + + // Build the clip vertices for the incident edge. + int32 i1 = index; + int32 i2 = i1 + 1 < count2 ? i1 + 1 : 0; + + c[0].v = b2Mul(xf2, vertices2[i1]); + c[0].id.cf.indexA = (uint8)edge1; + c[0].id.cf.indexB = (uint8)i1; + c[0].id.cf.typeA = b2ContactFeature::e_face; + c[0].id.cf.typeB = b2ContactFeature::e_vertex; + + c[1].v = b2Mul(xf2, vertices2[i2]); + c[1].id.cf.indexA = (uint8)edge1; + c[1].id.cf.indexB = (uint8)i2; + c[1].id.cf.typeA = b2ContactFeature::e_face; + c[1].id.cf.typeB = b2ContactFeature::e_vertex; +} + +// Find edge normal of max separation on A - return if separating axis is found +// Find edge normal of max separation on B - return if separation axis is found +// Choose reference edge as min(minA, minB) +// Find incident edge +// Clip + +// The normal points from 1 to 2 +void b2CollidePolygons(b2Manifold* manifold, + const b2PolygonShape* polyA, const b2Transform& xfA, + const b2PolygonShape* polyB, const b2Transform& xfB) +{ + manifold->pointCount = 0; + float totalRadius = polyA->m_radius + polyB->m_radius; + + int32 edgeA = 0; + float separationA = b2FindMaxSeparation(&edgeA, polyA, xfA, polyB, xfB); + if (separationA > totalRadius) + return; + + int32 edgeB = 0; + float separationB = b2FindMaxSeparation(&edgeB, polyB, xfB, polyA, xfA); + if (separationB > totalRadius) + return; + + const b2PolygonShape* poly1; // reference polygon + const b2PolygonShape* poly2; // incident polygon + b2Transform xf1, xf2; + int32 edge1; // reference edge + uint8 flip; + const float k_tol = 0.1f * b2_linearSlop; + + if (separationB > separationA + k_tol) + { + poly1 = polyB; + poly2 = polyA; + xf1 = xfB; + xf2 = xfA; + edge1 = edgeB; + manifold->type = b2Manifold::e_faceB; + flip = 1; + } + else + { + poly1 = polyA; + poly2 = polyB; + xf1 = xfA; + xf2 = xfB; + edge1 = edgeA; + manifold->type = b2Manifold::e_faceA; + flip = 0; + } + + b2ClipVertex incidentEdge[2]; + b2FindIncidentEdge(incidentEdge, poly1, xf1, edge1, poly2, xf2); + + int32 count1 = poly1->m_count; + const b2Vec2* vertices1 = poly1->m_vertices; + + int32 iv1 = edge1; + int32 iv2 = edge1 + 1 < count1 ? edge1 + 1 : 0; + + b2Vec2 v11 = vertices1[iv1]; + b2Vec2 v12 = vertices1[iv2]; + + b2Vec2 localTangent = v12 - v11; + localTangent.Normalize(); + + b2Vec2 localNormal = b2Cross(localTangent, 1.0f); + b2Vec2 planePoint = 0.5f * (v11 + v12); + + b2Vec2 tangent = b2Mul(xf1.q, localTangent); + b2Vec2 normal = b2Cross(tangent, 1.0f); + + v11 = b2Mul(xf1, v11); + v12 = b2Mul(xf1, v12); + + // Face offset. + float frontOffset = b2Dot(normal, v11); + + // Side offsets, extended by polytope skin thickness. + float sideOffset1 = -b2Dot(tangent, v11) + totalRadius; + float sideOffset2 = b2Dot(tangent, v12) + totalRadius; + + // Clip incident edge against extruded edge1 side edges. + b2ClipVertex clipPoints1[2]; + b2ClipVertex clipPoints2[2]; + int np; + + // Clip to box side 1 + np = b2ClipSegmentToLine(clipPoints1, incidentEdge, -tangent, sideOffset1, iv1); + + if (np < 2) + return; + + // Clip to negative box side 1 + np = b2ClipSegmentToLine(clipPoints2, clipPoints1, tangent, sideOffset2, iv2); + + if (np < 2) + { + return; + } + + // Now clipPoints2 contains the clipped points. + manifold->localNormal = localNormal; + manifold->localPoint = planePoint; + + int32 pointCount = 0; + for (int32 i = 0; i < b2_maxManifoldPoints; ++i) + { + float separation = b2Dot(normal, clipPoints2[i].v) - frontOffset; + + if (separation <= totalRadius) + { + b2ManifoldPoint* cp = manifold->points + pointCount; + cp->localPoint = b2MulT(xf2, clipPoints2[i].v); + cp->id = clipPoints2[i].id; + if (flip) + { + // Swap features + b2ContactFeature cf = cp->id.cf; + cp->id.cf.indexA = cf.indexB; + cp->id.cf.indexB = cf.indexA; + cp->id.cf.typeA = cf.typeB; + cp->id.cf.typeB = cf.typeA; + } + ++pointCount; + } + } + + manifold->pointCount = pointCount; +} diff --git a/libs/box2d/src/collision/b2_collision.cpp b/libs/box2d/src/collision/b2_collision.cpp new file mode 100644 index 00000000..750bff00 --- /dev/null +++ b/libs/box2d/src/collision/b2_collision.cpp @@ -0,0 +1,580 @@ +// MIT License + +// Copyright (c) 2019 Erin Catto + +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: + +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. + +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. + +#include "box2d/b2_collision.h" +#include "box2d/b2_distance.h" + +void b2WorldManifold::Initialize(const b2Manifold* manifold, + const b2Transform& xfA, float radiusA, + const b2Transform& xfB, float radiusB) +{ + if (manifold->pointCount == 0) + { + return; + } + + switch (manifold->type) + { + case b2Manifold::e_circles: + { + normal.Set(1.0f, 0.0f); + b2Vec2 pointA = b2Mul(xfA, manifold->localPoint); + b2Vec2 pointB = b2Mul(xfB, manifold->points[0].localPoint); + if (b2DistanceSquared(pointA, pointB) > b2_epsilon * b2_epsilon) + { + normal = pointB - pointA; + normal.Normalize(); + } + + b2Vec2 cA = pointA + radiusA * normal; + b2Vec2 cB = pointB - radiusB * normal; + points[0] = 0.5f * (cA + cB); + separations[0] = b2Dot(cB - cA, normal); + } + break; + + case b2Manifold::e_faceA: + { + normal = b2Mul(xfA.q, manifold->localNormal); + b2Vec2 planePoint = b2Mul(xfA, manifold->localPoint); + + for (int32 i = 0; i < manifold->pointCount; ++i) + { + b2Vec2 clipPoint = b2Mul(xfB, manifold->points[i].localPoint); + b2Vec2 cA = clipPoint + (radiusA - b2Dot(clipPoint - planePoint, normal)) * normal; + b2Vec2 cB = clipPoint - radiusB * normal; + points[i] = 0.5f * (cA + cB); + separations[i] = b2Dot(cB - cA, normal); + } + } + break; + + case b2Manifold::e_faceB: + { + normal = b2Mul(xfB.q, manifold->localNormal); + b2Vec2 planePoint = b2Mul(xfB, manifold->localPoint); + + for (int32 i = 0; i < manifold->pointCount; ++i) + { + b2Vec2 clipPoint = b2Mul(xfA, manifold->points[i].localPoint); + b2Vec2 cB = clipPoint + (radiusB - b2Dot(clipPoint - planePoint, normal)) * normal; + b2Vec2 cA = clipPoint - radiusA * normal; + points[i] = 0.5f * (cA + cB); + separations[i] = b2Dot(cA - cB, normal); + } + + // Ensure normal points from A to B. + normal = -normal; + } + break; + } +} + +void b2GetPointStates(b2PointState state1[b2_maxManifoldPoints], b2PointState state2[b2_maxManifoldPoints], + const b2Manifold* manifold1, const b2Manifold* manifold2) +{ + for (int32 i = 0; i < b2_maxManifoldPoints; ++i) + { + state1[i] = b2_nullState; + state2[i] = b2_nullState; + } + + // Detect persists and removes. + for (int32 i = 0; i < manifold1->pointCount; ++i) + { + b2ContactID id = manifold1->points[i].id; + + state1[i] = b2_removeState; + + for (int32 j = 0; j < manifold2->pointCount; ++j) + { + if (manifold2->points[j].id.key == id.key) + { + state1[i] = b2_persistState; + break; + } + } + } + + // Detect persists and adds. + for (int32 i = 0; i < manifold2->pointCount; ++i) + { + b2ContactID id = manifold2->points[i].id; + + state2[i] = b2_addState; + + for (int32 j = 0; j < manifold1->pointCount; ++j) + { + if (manifold1->points[j].id.key == id.key) + { + state2[i] = b2_persistState; + break; + } + } + } +} + +// From Real-time Collision Detection, p179. +bool b2AABB::RayCast(b2RayCastOutput* output, const b2RayCastInput& input) const +{ + float tmin = -b2_maxFloat; + float tmax = b2_maxFloat; + + b2Vec2 p = input.p1; + b2Vec2 d = input.p2 - input.p1; + b2Vec2 absD = b2Abs(d); + + b2Vec2 normal; + + for (int32 i = 0; i < 2; ++i) + { + if (absD(i) < b2_epsilon) + { + // Parallel. + if (p(i) < lowerBound(i) || upperBound(i) < p(i)) + { + return false; + } + } + else + { + float inv_d = 1.0f / d(i); + float t1 = (lowerBound(i) - p(i)) * inv_d; + float t2 = (upperBound(i) - p(i)) * inv_d; + + // Sign of the normal vector. + float s = -1.0f; + + if (t1 > t2) + { + b2Swap(t1, t2); + s = 1.0f; + } + + // Push the min up + if (t1 > tmin) + { + normal.SetZero(); + normal(i) = s; + tmin = t1; + } + + // Pull the max down + tmax = b2Min(tmax, t2); + + if (tmin > tmax) + { + return false; + } + } + } + + // Does the ray start inside the box? + // Does the ray intersect beyond the max fraction? + if (tmin < 0.0f || input.maxFraction < tmin) + { + return false; + } + + // Intersection. + output->fraction = tmin; + output->normal = normal; + return true; +} + +// Sutherland-Hodgman clipping. +int32 b2ClipSegmentToLine(b2ClipVertex vOut[2], const b2ClipVertex vIn[2], + const b2Vec2& normal, float offset, int32 vertexIndexA) +{ + // Start with no output points + int32 count = 0; + + // Calculate the distance of end points to the line + float distance0 = b2Dot(normal, vIn[0].v) - offset; + float distance1 = b2Dot(normal, vIn[1].v) - offset; + + // If the points are behind the plane + if (distance0 <= 0.0f) vOut[count++] = vIn[0]; + if (distance1 <= 0.0f) vOut[count++] = vIn[1]; + + // If the points are on different sides of the plane + if (distance0 * distance1 < 0.0f) + { + // Find intersection point of edge and plane + float interp = distance0 / (distance0 - distance1); + vOut[count].v = vIn[0].v + interp * (vIn[1].v - vIn[0].v); + + // VertexA is hitting edgeB. + vOut[count].id.cf.indexA = static_cast(vertexIndexA); + vOut[count].id.cf.indexB = vIn[0].id.cf.indexB; + vOut[count].id.cf.typeA = b2ContactFeature::e_vertex; + vOut[count].id.cf.typeB = b2ContactFeature::e_face; + ++count; + + b2Assert(count == 2); + } + + return count; +} + +bool b2TestOverlap( const b2Shape* shapeA, int32 indexA, + const b2Shape* shapeB, int32 indexB, + const b2Transform& xfA, const b2Transform& xfB) +{ + b2DistanceInput input; + input.proxyA.Set(shapeA, indexA); + input.proxyB.Set(shapeB, indexB); + input.transformA = xfA; + input.transformB = xfB; + input.useRadii = true; + + b2SimplexCache cache; + cache.count = 0; + + b2DistanceOutput output; + + b2Distance(&output, &cache, &input); + + return output.distance < 10.0f * b2_epsilon; +} + +// quickhull recursion +static b2Hull b2RecurseHull(b2Vec2 p1, b2Vec2 p2, b2Vec2* ps, int32 count) +{ + b2Hull hull; + hull.count = 0; + + if (count == 0) + { + return hull; + } + + // create an edge vector pointing from p1 to p2 + b2Vec2 e = p2 - p1; + e.Normalize(); + + // discard points left of e and find point furthest to the right of e + b2Vec2 rightPoints[b2_maxPolygonVertices]{}; + int32 rightCount = 0; + + int32 bestIndex = 0; + float bestDistance = b2Cross(ps[bestIndex] - p1, e); + if (bestDistance > 0.0f) + { + rightPoints[rightCount++] = ps[bestIndex]; + } + + for (int32 i = 1; i < count; ++i) + { + float distance = b2Cross(ps[i] - p1, e); + if (distance > bestDistance) + { + bestIndex = i; + bestDistance = distance; + } + + if (distance > 0.0f) + { + rightPoints[rightCount++] = ps[i]; + } + } + + if (bestDistance < 2.0f * b2_linearSlop) + { + return hull; + } + + b2Vec2 bestPoint = ps[bestIndex]; + + // compute hull to the right of p1-bestPoint + b2Hull hull1 = b2RecurseHull(p1, bestPoint, rightPoints, rightCount); + + // compute hull to the right of bestPoint-p2 + b2Hull hull2 = b2RecurseHull(bestPoint, p2, rightPoints, rightCount); + + // stich together hulls + for (int32 i = 0; i < hull1.count; ++i) + { + hull.points[hull.count++] = hull1.points[i]; + } + + hull.points[hull.count++] = bestPoint; + + for (int32 i = 0; i < hull2.count; ++i) + { + hull.points[hull.count++] = hull2.points[i]; + } + + b2Assert(hull.count < b2_maxPolygonVertices); + + return hull; +} + +// quickhull algorithm +// - merges vertices based on b2_linearSlop +// - removes collinear points using b2_linearSlop +// - returns an empty hull if it fails +b2Hull b2ComputeHull(const b2Vec2* points, int32 count) +{ + b2Hull hull; + hull.count = 0; + + if (count < 3 || count > b2_maxPolygonVertices) + { + // check your data + return hull; + } + + count = b2Min(count, b2_maxPolygonVertices); + + b2AABB aabb = { {b2_maxFloat, b2_maxFloat}, {-b2_maxFloat, -b2_maxFloat} }; + + // Perform aggressive point welding. First point always remains. + // Also compute the bounding box for later. + b2Vec2 ps[b2_maxPolygonVertices]; + int32 n = 0; + const float tolSqr = 16.0f * b2_linearSlop * b2_linearSlop; + for (int32 i = 0; i < count; ++i) + { + aabb.lowerBound = b2Min(aabb.lowerBound, points[i]); + aabb.upperBound = b2Max(aabb.upperBound, points[i]); + + b2Vec2 vi = points[i]; + + bool unique = true; + for (int32 j = 0; j < i; ++j) + { + b2Vec2 vj = points[j]; + + float distSqr = b2DistanceSquared(vi, vj); + if (distSqr < tolSqr) + { + unique = false; + break; + } + } + + if (unique) + { + ps[n++] = vi; + } + } + + if (n < 3) + { + // all points very close together, check your data and check your scale + return hull; + } + + // Find an extreme point as the first point on the hull + b2Vec2 c = aabb.GetCenter(); + int32 i1 = 0; + float dsq1 = b2DistanceSquared(c, ps[i1]); + for (int32 i = 1; i < n; ++i) + { + float dsq = b2DistanceSquared(c, ps[i]); + if (dsq > dsq1) + { + i1 = i; + dsq1 = dsq; + } + } + + // remove p1 from working set + b2Vec2 p1 = ps[i1]; + ps[i1] = ps[n - 1]; + n = n - 1; + + int32 i2 = 0; + float dsq2 = b2DistanceSquared(p1, ps[i2]); + for (int32 i = 1; i < n; ++i) + { + float dsq = b2DistanceSquared(p1, ps[i]); + if (dsq > dsq2) + { + i2 = i; + dsq2 = dsq; + } + } + + // remove p2 from working set + b2Vec2 p2 = ps[i2]; + ps[i2] = ps[n - 1]; + n = n - 1; + + // split the points into points that are left and right of the line p1-p2. + b2Vec2 rightPoints[b2_maxPolygonVertices - 2]; + int32 rightCount = 0; + + b2Vec2 leftPoints[b2_maxPolygonVertices - 2]; + int32 leftCount = 0; + + b2Vec2 e = p2 - p1; + e.Normalize(); + + for (int32 i = 0; i < n; ++i) + { + float d = b2Cross(ps[i] - p1, e); + + // slop used here to skip points that are very close to the line p1-p2 + if (d >= 2.0f * b2_linearSlop) + { + rightPoints[rightCount++] = ps[i]; + } + else if (d <= -2.0f * b2_linearSlop) + { + leftPoints[leftCount++] = ps[i]; + } + } + + // compute hulls on right and left + b2Hull hull1 = b2RecurseHull(p1, p2, rightPoints, rightCount); + b2Hull hull2 = b2RecurseHull(p2, p1, leftPoints, leftCount); + + if (hull1.count == 0 && hull2.count == 0) + { + // all points collinear + return hull; + } + + // stitch hulls together, preserving CCW winding order + hull.points[hull.count++] = p1; + + for (int32 i = 0; i < hull1.count; ++i) + { + hull.points[hull.count++] = hull1.points[i]; + } + + hull.points[hull.count++] = p2; + + for (int32 i = 0; i < hull2.count; ++i) + { + hull.points[hull.count++] = hull2.points[i]; + } + + b2Assert(hull.count <= b2_maxPolygonVertices); + + // merge collinear + bool searching = true; + while (searching && hull.count > 2) + { + searching = false; + + for (int32 i = 0; i < hull.count; ++i) + { + int32 i1 = i; + int32 i2 = (i + 1) % hull.count; + int32 i3 = (i + 2) % hull.count; + + b2Vec2 p1 = hull.points[i1]; + b2Vec2 p2 = hull.points[i2]; + b2Vec2 p3 = hull.points[i3]; + + b2Vec2 e = p3 - p1; + e.Normalize(); + + b2Vec2 v = p2 - p1; + float distance = b2Cross(p2 - p1, e); + if (distance <= 2.0f * b2_linearSlop) + { + // remove midpoint from hull + for (int32 j = i2; j < hull.count - 1; ++j) + { + hull.points[j] = hull.points[j + 1]; + } + hull.count -= 1; + + // continue searching for collinear points + searching = true; + + break; + } + } + } + + if (hull.count < 3) + { + // all points collinear, shouldn't be reached since this was validated above + hull.count = 0; + } + + return hull; +} + +bool b2ValidateHull(const b2Hull& hull) +{ + if (hull.count < 3 || b2_maxPolygonVertices < hull.count) + { + return false; + } + + // test that every point is behind every edge + for (int32 i = 0; i < hull.count; ++i) + { + // create an edge vector + int32 i1 = i; + int32 i2 = i < hull.count - 1 ? i1 + 1 : 0; + b2Vec2 p = hull.points[i1]; + b2Vec2 e = hull.points[i2] - p; + e.Normalize(); + + for (int32 j = 0; j < hull.count; ++j) + { + // skip points that subtend the current edge + if (j == i1 || j == i2) + { + continue; + } + + float distance = b2Cross(hull.points[j] - p, e); + if (distance >= 0.0f) + { + return false; + } + } + } + + // test for collinear points + for (int32 i = 0; i < hull.count; ++i) + { + int32 i1 = i; + int32 i2 = (i + 1) % hull.count; + int32 i3 = (i + 2) % hull.count; + + b2Vec2 p1 = hull.points[i1]; + b2Vec2 p2 = hull.points[i2]; + b2Vec2 p3 = hull.points[i3]; + + b2Vec2 e = p3 - p1; + e.Normalize(); + + b2Vec2 v = p2 - p1; + float distance = b2Cross(p2 - p1, e); + if (distance <= b2_linearSlop) + { + // p1-p2-p3 are collinear + return false; + } + } + + return true; +} diff --git a/libs/Box2D/Collision/b2Distance.cpp b/libs/box2d/src/collision/b2_distance.cpp similarity index 62% rename from libs/Box2D/Collision/b2Distance.cpp rename to libs/box2d/src/collision/b2_distance.cpp index faa4c87a..16fa3cc3 100644 --- a/libs/Box2D/Collision/b2Distance.cpp +++ b/libs/box2d/src/collision/b2_distance.cpp @@ -1,603 +1,744 @@ -/* -* Copyright (c) 2007-2009 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ - -#include -#include -#include -#include -#include - -// GJK using Voronoi regions (Christer Ericson) and Barycentric coordinates. -int32 b2_gjkCalls, b2_gjkIters, b2_gjkMaxIters; - -void b2DistanceProxy::Set(const b2Shape* shape, int32 index) -{ - switch (shape->GetType()) - { - case b2Shape::e_circle: - { - const b2CircleShape* circle = static_cast(shape); - m_vertices = &circle->m_p; - m_count = 1; - m_radius = circle->m_radius; - } - break; - - case b2Shape::e_polygon: - { - const b2PolygonShape* polygon = static_cast(shape); - m_vertices = polygon->m_vertices; - m_count = polygon->m_count; - m_radius = polygon->m_radius; - } - break; - - case b2Shape::e_chain: - { - const b2ChainShape* chain = static_cast(shape); - b2Assert(0 <= index && index < chain->m_count); - - m_buffer[0] = chain->m_vertices[index]; - if (index + 1 < chain->m_count) - { - m_buffer[1] = chain->m_vertices[index + 1]; - } - else - { - m_buffer[1] = chain->m_vertices[0]; - } - - m_vertices = m_buffer; - m_count = 2; - m_radius = chain->m_radius; - } - break; - - case b2Shape::e_edge: - { - const b2EdgeShape* edge = static_cast(shape); - m_vertices = &edge->m_vertex1; - m_count = 2; - m_radius = edge->m_radius; - } - break; - - default: - b2Assert(false); - } -} - - -struct b2SimplexVertex -{ - b2Vec2 wA; // support point in proxyA - b2Vec2 wB; // support point in proxyB - b2Vec2 w; // wB - wA - float32 a; // barycentric coordinate for closest point - int32 indexA; // wA index - int32 indexB; // wB index -}; - -struct b2Simplex -{ - void ReadCache( const b2SimplexCache* cache, - const b2DistanceProxy* proxyA, const b2Transform& transformA, - const b2DistanceProxy* proxyB, const b2Transform& transformB) - { - b2Assert(cache->count <= 3); - - // Copy data from cache. - m_count = cache->count; - b2SimplexVertex* vertices = &m_v1; - for (int32 i = 0; i < m_count; ++i) - { - b2SimplexVertex* v = vertices + i; - v->indexA = cache->indexA[i]; - v->indexB = cache->indexB[i]; - b2Vec2 wALocal = proxyA->GetVertex(v->indexA); - b2Vec2 wBLocal = proxyB->GetVertex(v->indexB); - v->wA = b2Mul(transformA, wALocal); - v->wB = b2Mul(transformB, wBLocal); - v->w = v->wB - v->wA; - v->a = 0.0f; - } - - // Compute the new simplex metric, if it is substantially different than - // old metric then flush the simplex. - if (m_count > 1) - { - float32 metric1 = cache->metric; - float32 metric2 = GetMetric(); - if (metric2 < 0.5f * metric1 || 2.0f * metric1 < metric2 || metric2 < b2_epsilon) - { - // Reset the simplex. - m_count = 0; - } - } - - // If the cache is empty or invalid ... - if (m_count == 0) - { - b2SimplexVertex* v = vertices + 0; - v->indexA = 0; - v->indexB = 0; - b2Vec2 wALocal = proxyA->GetVertex(0); - b2Vec2 wBLocal = proxyB->GetVertex(0); - v->wA = b2Mul(transformA, wALocal); - v->wB = b2Mul(transformB, wBLocal); - v->w = v->wB - v->wA; - v->a = 1.0f; - m_count = 1; - } - } - - void WriteCache(b2SimplexCache* cache) const - { - cache->metric = GetMetric(); - cache->count = uint16(m_count); - const b2SimplexVertex* vertices = &m_v1; - for (int32 i = 0; i < m_count; ++i) - { - cache->indexA[i] = uint8(vertices[i].indexA); - cache->indexB[i] = uint8(vertices[i].indexB); - } - } - - b2Vec2 GetSearchDirection() const - { - switch (m_count) - { - case 1: - return -m_v1.w; - - case 2: - { - b2Vec2 e12 = m_v2.w - m_v1.w; - float32 sgn = b2Cross(e12, -m_v1.w); - if (sgn > 0.0f) - { - // Origin is left of e12. - return b2Cross(1.0f, e12); - } - else - { - // Origin is right of e12. - return b2Cross(e12, 1.0f); - } - } - - default: - b2Assert(false); - return b2Vec2_zero; - } - } - - b2Vec2 GetClosestPoint() const - { - switch (m_count) - { - case 0: - b2Assert(false); - return b2Vec2_zero; - - case 1: - return m_v1.w; - - case 2: - return m_v1.a * m_v1.w + m_v2.a * m_v2.w; - - case 3: - return b2Vec2_zero; - - default: - b2Assert(false); - return b2Vec2_zero; - } - } - - void GetWitnessPoints(b2Vec2* pA, b2Vec2* pB) const - { - switch (m_count) - { - case 0: - b2Assert(false); - break; - - case 1: - *pA = m_v1.wA; - *pB = m_v1.wB; - break; - - case 2: - *pA = m_v1.a * m_v1.wA + m_v2.a * m_v2.wA; - *pB = m_v1.a * m_v1.wB + m_v2.a * m_v2.wB; - break; - - case 3: - *pA = m_v1.a * m_v1.wA + m_v2.a * m_v2.wA + m_v3.a * m_v3.wA; - *pB = *pA; - break; - - default: - b2Assert(false); - break; - } - } - - float32 GetMetric() const - { - switch (m_count) - { - case 0: - b2Assert(false); - return 0.0f; - - case 1: - return 0.0f; - - case 2: - return b2Distance(m_v1.w, m_v2.w); - - case 3: - return b2Cross(m_v2.w - m_v1.w, m_v3.w - m_v1.w); - - default: - b2Assert(false); - return 0.0f; - } - } - - void Solve2(); - void Solve3(); - - b2SimplexVertex m_v1, m_v2, m_v3; - int32 m_count; -}; - - -// Solve a line segment using barycentric coordinates. -// -// p = a1 * w1 + a2 * w2 -// a1 + a2 = 1 -// -// The vector from the origin to the closest point on the line is -// perpendicular to the line. -// e12 = w2 - w1 -// dot(p, e) = 0 -// a1 * dot(w1, e) + a2 * dot(w2, e) = 0 -// -// 2-by-2 linear system -// [1 1 ][a1] = [1] -// [w1.e12 w2.e12][a2] = [0] -// -// Define -// d12_1 = dot(w2, e12) -// d12_2 = -dot(w1, e12) -// d12 = d12_1 + d12_2 -// -// Solution -// a1 = d12_1 / d12 -// a2 = d12_2 / d12 -void b2Simplex::Solve2() -{ - b2Vec2 w1 = m_v1.w; - b2Vec2 w2 = m_v2.w; - b2Vec2 e12 = w2 - w1; - - // w1 region - float32 d12_2 = -b2Dot(w1, e12); - if (d12_2 <= 0.0f) - { - // a2 <= 0, so we clamp it to 0 - m_v1.a = 1.0f; - m_count = 1; - return; - } - - // w2 region - float32 d12_1 = b2Dot(w2, e12); - if (d12_1 <= 0.0f) - { - // a1 <= 0, so we clamp it to 0 - m_v2.a = 1.0f; - m_count = 1; - m_v1 = m_v2; - return; - } - - // Must be in e12 region. - float32 inv_d12 = 1.0f / (d12_1 + d12_2); - m_v1.a = d12_1 * inv_d12; - m_v2.a = d12_2 * inv_d12; - m_count = 2; -} - -// Possible regions: -// - points[2] -// - edge points[0]-points[2] -// - edge points[1]-points[2] -// - inside the triangle -void b2Simplex::Solve3() -{ - b2Vec2 w1 = m_v1.w; - b2Vec2 w2 = m_v2.w; - b2Vec2 w3 = m_v3.w; - - // Edge12 - // [1 1 ][a1] = [1] - // [w1.e12 w2.e12][a2] = [0] - // a3 = 0 - b2Vec2 e12 = w2 - w1; - float32 w1e12 = b2Dot(w1, e12); - float32 w2e12 = b2Dot(w2, e12); - float32 d12_1 = w2e12; - float32 d12_2 = -w1e12; - - // Edge13 - // [1 1 ][a1] = [1] - // [w1.e13 w3.e13][a3] = [0] - // a2 = 0 - b2Vec2 e13 = w3 - w1; - float32 w1e13 = b2Dot(w1, e13); - float32 w3e13 = b2Dot(w3, e13); - float32 d13_1 = w3e13; - float32 d13_2 = -w1e13; - - // Edge23 - // [1 1 ][a2] = [1] - // [w2.e23 w3.e23][a3] = [0] - // a1 = 0 - b2Vec2 e23 = w3 - w2; - float32 w2e23 = b2Dot(w2, e23); - float32 w3e23 = b2Dot(w3, e23); - float32 d23_1 = w3e23; - float32 d23_2 = -w2e23; - - // Triangle123 - float32 n123 = b2Cross(e12, e13); - - float32 d123_1 = n123 * b2Cross(w2, w3); - float32 d123_2 = n123 * b2Cross(w3, w1); - float32 d123_3 = n123 * b2Cross(w1, w2); - - // w1 region - if (d12_2 <= 0.0f && d13_2 <= 0.0f) - { - m_v1.a = 1.0f; - m_count = 1; - return; - } - - // e12 - if (d12_1 > 0.0f && d12_2 > 0.0f && d123_3 <= 0.0f) - { - float32 inv_d12 = 1.0f / (d12_1 + d12_2); - m_v1.a = d12_1 * inv_d12; - m_v2.a = d12_2 * inv_d12; - m_count = 2; - return; - } - - // e13 - if (d13_1 > 0.0f && d13_2 > 0.0f && d123_2 <= 0.0f) - { - float32 inv_d13 = 1.0f / (d13_1 + d13_2); - m_v1.a = d13_1 * inv_d13; - m_v3.a = d13_2 * inv_d13; - m_count = 2; - m_v2 = m_v3; - return; - } - - // w2 region - if (d12_1 <= 0.0f && d23_2 <= 0.0f) - { - m_v2.a = 1.0f; - m_count = 1; - m_v1 = m_v2; - return; - } - - // w3 region - if (d13_1 <= 0.0f && d23_1 <= 0.0f) - { - m_v3.a = 1.0f; - m_count = 1; - m_v1 = m_v3; - return; - } - - // e23 - if (d23_1 > 0.0f && d23_2 > 0.0f && d123_1 <= 0.0f) - { - float32 inv_d23 = 1.0f / (d23_1 + d23_2); - m_v2.a = d23_1 * inv_d23; - m_v3.a = d23_2 * inv_d23; - m_count = 2; - m_v1 = m_v3; - return; - } - - // Must be in triangle123 - float32 inv_d123 = 1.0f / (d123_1 + d123_2 + d123_3); - m_v1.a = d123_1 * inv_d123; - m_v2.a = d123_2 * inv_d123; - m_v3.a = d123_3 * inv_d123; - m_count = 3; -} - -void b2Distance(b2DistanceOutput* output, - b2SimplexCache* cache, - const b2DistanceInput* input) -{ - ++b2_gjkCalls; - - const b2DistanceProxy* proxyA = &input->proxyA; - const b2DistanceProxy* proxyB = &input->proxyB; - - b2Transform transformA = input->transformA; - b2Transform transformB = input->transformB; - - // Initialize the simplex. - b2Simplex simplex; - simplex.ReadCache(cache, proxyA, transformA, proxyB, transformB); - - // Get simplex vertices as an array. - b2SimplexVertex* vertices = &simplex.m_v1; - const int32 k_maxIters = 20; - - // These store the vertices of the last simplex so that we - // can check for duplicates and prevent cycling. - int32 saveA[3], saveB[3]; - int32 saveCount = 0; - - float32 distanceSqr1 = b2_maxFloat; - float32 distanceSqr2 = distanceSqr1; - - // Main iteration loop. - int32 iter = 0; - while (iter < k_maxIters) - { - // Copy simplex so we can identify duplicates. - saveCount = simplex.m_count; - for (int32 i = 0; i < saveCount; ++i) - { - saveA[i] = vertices[i].indexA; - saveB[i] = vertices[i].indexB; - } - - switch (simplex.m_count) - { - case 1: - break; - - case 2: - simplex.Solve2(); - break; - - case 3: - simplex.Solve3(); - break; - - default: - b2Assert(false); - } - - // If we have 3 points, then the origin is in the corresponding triangle. - if (simplex.m_count == 3) - { - break; - } - - // Compute closest point. - b2Vec2 p = simplex.GetClosestPoint(); - distanceSqr2 = p.LengthSquared(); - - // Ensure progress - if (distanceSqr2 >= distanceSqr1) - { - //break; - } - distanceSqr1 = distanceSqr2; - - // Get search direction. - b2Vec2 d = simplex.GetSearchDirection(); - - // Ensure the search direction is numerically fit. - if (d.LengthSquared() < b2_epsilon * b2_epsilon) - { - // The origin is probably contained by a line segment - // or triangle. Thus the shapes are overlapped. - - // We can't return zero here even though there may be overlap. - // In case the simplex is a point, segment, or triangle it is difficult - // to determine if the origin is contained in the CSO or very close to it. - break; - } - - // Compute a tentative new simplex vertex using support points. - b2SimplexVertex* vertex = vertices + simplex.m_count; - vertex->indexA = proxyA->GetSupport(b2MulT(transformA.q, -d)); - vertex->wA = b2Mul(transformA, proxyA->GetVertex(vertex->indexA)); - b2Vec2 wBLocal; - vertex->indexB = proxyB->GetSupport(b2MulT(transformB.q, d)); - vertex->wB = b2Mul(transformB, proxyB->GetVertex(vertex->indexB)); - vertex->w = vertex->wB - vertex->wA; - - // Iteration count is equated to the number of support point calls. - ++iter; - ++b2_gjkIters; - - // Check for duplicate support points. This is the main termination criteria. - bool duplicate = false; - for (int32 i = 0; i < saveCount; ++i) - { - if (vertex->indexA == saveA[i] && vertex->indexB == saveB[i]) - { - duplicate = true; - break; - } - } - - // If we found a duplicate support point we must exit to avoid cycling. - if (duplicate) - { - break; - } - - // New vertex is ok and needed. - ++simplex.m_count; - } - - b2_gjkMaxIters = b2Max(b2_gjkMaxIters, iter); - - // Prepare output. - simplex.GetWitnessPoints(&output->pointA, &output->pointB); - output->distance = b2Distance(output->pointA, output->pointB); - output->iterations = iter; - - // Cache the simplex. - simplex.WriteCache(cache); - - // Apply radii if requested. - if (input->useRadii) - { - float32 rA = proxyA->m_radius; - float32 rB = proxyB->m_radius; - - if (output->distance > rA + rB && output->distance > b2_epsilon) - { - // Shapes are still no overlapped. - // Move the witness points to the outer surface. - output->distance -= rA + rB; - b2Vec2 normal = output->pointB - output->pointA; - normal.Normalize(); - output->pointA += rA * normal; - output->pointB -= rB * normal; - } - else - { - // Shapes are overlapped when radii are considered. - // Move the witness points to the middle. - b2Vec2 p = 0.5f * (output->pointA + output->pointB); - output->pointA = p; - output->pointB = p; - output->distance = 0.0f; - } - } -} +// MIT License + +// Copyright (c) 2019 Erin Catto + +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: + +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. + +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. + +#include "box2d/b2_circle_shape.h" +#include "box2d/b2_distance.h" +#include "box2d/b2_edge_shape.h" +#include "box2d/b2_chain_shape.h" +#include "box2d/b2_polygon_shape.h" + +// GJK using Voronoi regions (Christer Ericson) and Barycentric coordinates. +B2_API int32 b2_gjkCalls, b2_gjkIters, b2_gjkMaxIters; + +void b2DistanceProxy::Set(const b2Shape* shape, int32 index) +{ + switch (shape->GetType()) + { + case b2Shape::e_circle: + { + const b2CircleShape* circle = static_cast(shape); + m_vertices = &circle->m_p; + m_count = 1; + m_radius = circle->m_radius; + } + break; + + case b2Shape::e_polygon: + { + const b2PolygonShape* polygon = static_cast(shape); + m_vertices = polygon->m_vertices; + m_count = polygon->m_count; + m_radius = polygon->m_radius; + } + break; + + case b2Shape::e_chain: + { + const b2ChainShape* chain = static_cast(shape); + b2Assert(0 <= index && index < chain->m_count); + + m_buffer[0] = chain->m_vertices[index]; + if (index + 1 < chain->m_count) + { + m_buffer[1] = chain->m_vertices[index + 1]; + } + else + { + m_buffer[1] = chain->m_vertices[0]; + } + + m_vertices = m_buffer; + m_count = 2; + m_radius = chain->m_radius; + } + break; + + case b2Shape::e_edge: + { + const b2EdgeShape* edge = static_cast(shape); + m_vertices = &edge->m_vertex1; + m_count = 2; + m_radius = edge->m_radius; + } + break; + + default: + b2Assert(false); + } +} + +void b2DistanceProxy::Set(const b2Vec2* vertices, int32 count, float radius) +{ + m_vertices = vertices; + m_count = count; + m_radius = radius; +} + +struct b2SimplexVertex +{ + b2Vec2 wA; // support point in proxyA + b2Vec2 wB; // support point in proxyB + b2Vec2 w; // wB - wA + float a; // barycentric coordinate for closest point + int32 indexA; // wA index + int32 indexB; // wB index +}; + +struct b2Simplex +{ + void ReadCache( const b2SimplexCache* cache, + const b2DistanceProxy* proxyA, const b2Transform& transformA, + const b2DistanceProxy* proxyB, const b2Transform& transformB) + { + b2Assert(cache->count <= 3); + + // Copy data from cache. + m_count = cache->count; + b2SimplexVertex* vertices = &m_v1; + for (int32 i = 0; i < m_count; ++i) + { + b2SimplexVertex* v = vertices + i; + v->indexA = cache->indexA[i]; + v->indexB = cache->indexB[i]; + b2Vec2 wALocal = proxyA->GetVertex(v->indexA); + b2Vec2 wBLocal = proxyB->GetVertex(v->indexB); + v->wA = b2Mul(transformA, wALocal); + v->wB = b2Mul(transformB, wBLocal); + v->w = v->wB - v->wA; + v->a = 0.0f; + } + + // Compute the new simplex metric, if it is substantially different than + // old metric then flush the simplex. + if (m_count > 1) + { + float metric1 = cache->metric; + float metric2 = GetMetric(); + if (metric2 < 0.5f * metric1 || 2.0f * metric1 < metric2 || metric2 < b2_epsilon) + { + // Reset the simplex. + m_count = 0; + } + } + + // If the cache is empty or invalid ... + if (m_count == 0) + { + b2SimplexVertex* v = vertices + 0; + v->indexA = 0; + v->indexB = 0; + b2Vec2 wALocal = proxyA->GetVertex(0); + b2Vec2 wBLocal = proxyB->GetVertex(0); + v->wA = b2Mul(transformA, wALocal); + v->wB = b2Mul(transformB, wBLocal); + v->w = v->wB - v->wA; + v->a = 1.0f; + m_count = 1; + } + } + + void WriteCache(b2SimplexCache* cache) const + { + cache->metric = GetMetric(); + cache->count = uint16(m_count); + const b2SimplexVertex* vertices = &m_v1; + for (int32 i = 0; i < m_count; ++i) + { + cache->indexA[i] = uint8(vertices[i].indexA); + cache->indexB[i] = uint8(vertices[i].indexB); + } + } + + b2Vec2 GetSearchDirection() const + { + switch (m_count) + { + case 1: + return -m_v1.w; + + case 2: + { + b2Vec2 e12 = m_v2.w - m_v1.w; + float sgn = b2Cross(e12, -m_v1.w); + if (sgn > 0.0f) + { + // Origin is left of e12. + return b2Cross(1.0f, e12); + } + else + { + // Origin is right of e12. + return b2Cross(e12, 1.0f); + } + } + + default: + b2Assert(false); + return b2Vec2_zero; + } + } + + b2Vec2 GetClosestPoint() const + { + switch (m_count) + { + case 0: + b2Assert(false); + return b2Vec2_zero; + + case 1: + return m_v1.w; + + case 2: + return m_v1.a * m_v1.w + m_v2.a * m_v2.w; + + case 3: + return b2Vec2_zero; + + default: + b2Assert(false); + return b2Vec2_zero; + } + } + + void GetWitnessPoints(b2Vec2* pA, b2Vec2* pB) const + { + switch (m_count) + { + case 0: + b2Assert(false); + break; + + case 1: + *pA = m_v1.wA; + *pB = m_v1.wB; + break; + + case 2: + *pA = m_v1.a * m_v1.wA + m_v2.a * m_v2.wA; + *pB = m_v1.a * m_v1.wB + m_v2.a * m_v2.wB; + break; + + case 3: + *pA = m_v1.a * m_v1.wA + m_v2.a * m_v2.wA + m_v3.a * m_v3.wA; + *pB = *pA; + break; + + default: + b2Assert(false); + break; + } + } + + float GetMetric() const + { + switch (m_count) + { + case 0: + b2Assert(false); + return 0.0f; + + case 1: + return 0.0f; + + case 2: + return b2Distance(m_v1.w, m_v2.w); + + case 3: + return b2Cross(m_v2.w - m_v1.w, m_v3.w - m_v1.w); + + default: + b2Assert(false); + return 0.0f; + } + } + + void Solve2(); + void Solve3(); + + b2SimplexVertex m_v1, m_v2, m_v3; + int32 m_count; +}; + + +// Solve a line segment using barycentric coordinates. +// +// p = a1 * w1 + a2 * w2 +// a1 + a2 = 1 +// +// The vector from the origin to the closest point on the line is +// perpendicular to the line. +// e12 = w2 - w1 +// dot(p, e) = 0 +// a1 * dot(w1, e) + a2 * dot(w2, e) = 0 +// +// 2-by-2 linear system +// [1 1 ][a1] = [1] +// [w1.e12 w2.e12][a2] = [0] +// +// Define +// d12_1 = dot(w2, e12) +// d12_2 = -dot(w1, e12) +// d12 = d12_1 + d12_2 +// +// Solution +// a1 = d12_1 / d12 +// a2 = d12_2 / d12 +void b2Simplex::Solve2() +{ + b2Vec2 w1 = m_v1.w; + b2Vec2 w2 = m_v2.w; + b2Vec2 e12 = w2 - w1; + + // w1 region + float d12_2 = -b2Dot(w1, e12); + if (d12_2 <= 0.0f) + { + // a2 <= 0, so we clamp it to 0 + m_v1.a = 1.0f; + m_count = 1; + return; + } + + // w2 region + float d12_1 = b2Dot(w2, e12); + if (d12_1 <= 0.0f) + { + // a1 <= 0, so we clamp it to 0 + m_v2.a = 1.0f; + m_count = 1; + m_v1 = m_v2; + return; + } + + // Must be in e12 region. + float inv_d12 = 1.0f / (d12_1 + d12_2); + m_v1.a = d12_1 * inv_d12; + m_v2.a = d12_2 * inv_d12; + m_count = 2; +} + +// Possible regions: +// - points[2] +// - edge points[0]-points[2] +// - edge points[1]-points[2] +// - inside the triangle +void b2Simplex::Solve3() +{ + b2Vec2 w1 = m_v1.w; + b2Vec2 w2 = m_v2.w; + b2Vec2 w3 = m_v3.w; + + // Edge12 + // [1 1 ][a1] = [1] + // [w1.e12 w2.e12][a2] = [0] + // a3 = 0 + b2Vec2 e12 = w2 - w1; + float w1e12 = b2Dot(w1, e12); + float w2e12 = b2Dot(w2, e12); + float d12_1 = w2e12; + float d12_2 = -w1e12; + + // Edge13 + // [1 1 ][a1] = [1] + // [w1.e13 w3.e13][a3] = [0] + // a2 = 0 + b2Vec2 e13 = w3 - w1; + float w1e13 = b2Dot(w1, e13); + float w3e13 = b2Dot(w3, e13); + float d13_1 = w3e13; + float d13_2 = -w1e13; + + // Edge23 + // [1 1 ][a2] = [1] + // [w2.e23 w3.e23][a3] = [0] + // a1 = 0 + b2Vec2 e23 = w3 - w2; + float w2e23 = b2Dot(w2, e23); + float w3e23 = b2Dot(w3, e23); + float d23_1 = w3e23; + float d23_2 = -w2e23; + + // Triangle123 + float n123 = b2Cross(e12, e13); + + float d123_1 = n123 * b2Cross(w2, w3); + float d123_2 = n123 * b2Cross(w3, w1); + float d123_3 = n123 * b2Cross(w1, w2); + + // w1 region + if (d12_2 <= 0.0f && d13_2 <= 0.0f) + { + m_v1.a = 1.0f; + m_count = 1; + return; + } + + // e12 + if (d12_1 > 0.0f && d12_2 > 0.0f && d123_3 <= 0.0f) + { + float inv_d12 = 1.0f / (d12_1 + d12_2); + m_v1.a = d12_1 * inv_d12; + m_v2.a = d12_2 * inv_d12; + m_count = 2; + return; + } + + // e13 + if (d13_1 > 0.0f && d13_2 > 0.0f && d123_2 <= 0.0f) + { + float inv_d13 = 1.0f / (d13_1 + d13_2); + m_v1.a = d13_1 * inv_d13; + m_v3.a = d13_2 * inv_d13; + m_count = 2; + m_v2 = m_v3; + return; + } + + // w2 region + if (d12_1 <= 0.0f && d23_2 <= 0.0f) + { + m_v2.a = 1.0f; + m_count = 1; + m_v1 = m_v2; + return; + } + + // w3 region + if (d13_1 <= 0.0f && d23_1 <= 0.0f) + { + m_v3.a = 1.0f; + m_count = 1; + m_v1 = m_v3; + return; + } + + // e23 + if (d23_1 > 0.0f && d23_2 > 0.0f && d123_1 <= 0.0f) + { + float inv_d23 = 1.0f / (d23_1 + d23_2); + m_v2.a = d23_1 * inv_d23; + m_v3.a = d23_2 * inv_d23; + m_count = 2; + m_v1 = m_v3; + return; + } + + // Must be in triangle123 + float inv_d123 = 1.0f / (d123_1 + d123_2 + d123_3); + m_v1.a = d123_1 * inv_d123; + m_v2.a = d123_2 * inv_d123; + m_v3.a = d123_3 * inv_d123; + m_count = 3; +} + +void b2Distance(b2DistanceOutput* output, + b2SimplexCache* cache, + const b2DistanceInput* input) +{ + ++b2_gjkCalls; + + const b2DistanceProxy* proxyA = &input->proxyA; + const b2DistanceProxy* proxyB = &input->proxyB; + + b2Transform transformA = input->transformA; + b2Transform transformB = input->transformB; + + // Initialize the simplex. + b2Simplex simplex; + simplex.ReadCache(cache, proxyA, transformA, proxyB, transformB); + + // Get simplex vertices as an array. + b2SimplexVertex* vertices = &simplex.m_v1; + const int32 k_maxIters = 20; + + // These store the vertices of the last simplex so that we + // can check for duplicates and prevent cycling. + int32 saveA[3], saveB[3]; + int32 saveCount = 0; + + // Main iteration loop. + int32 iter = 0; + while (iter < k_maxIters) + { + // Copy simplex so we can identify duplicates. + saveCount = simplex.m_count; + for (int32 i = 0; i < saveCount; ++i) + { + saveA[i] = vertices[i].indexA; + saveB[i] = vertices[i].indexB; + } + + switch (simplex.m_count) + { + case 1: + break; + + case 2: + simplex.Solve2(); + break; + + case 3: + simplex.Solve3(); + break; + + default: + b2Assert(false); + } + + // If we have 3 points, then the origin is in the corresponding triangle. + if (simplex.m_count == 3) + { + break; + } + + // Get search direction. + b2Vec2 d = simplex.GetSearchDirection(); + + // Ensure the search direction is numerically fit. + if (d.LengthSquared() < b2_epsilon * b2_epsilon) + { + // The origin is probably contained by a line segment + // or triangle. Thus the shapes are overlapped. + + // We can't return zero here even though there may be overlap. + // In case the simplex is a point, segment, or triangle it is difficult + // to determine if the origin is contained in the CSO or very close to it. + break; + } + + // Compute a tentative new simplex vertex using support points. + b2SimplexVertex* vertex = vertices + simplex.m_count; + vertex->indexA = proxyA->GetSupport(b2MulT(transformA.q, -d)); + vertex->wA = b2Mul(transformA, proxyA->GetVertex(vertex->indexA)); + vertex->indexB = proxyB->GetSupport(b2MulT(transformB.q, d)); + vertex->wB = b2Mul(transformB, proxyB->GetVertex(vertex->indexB)); + vertex->w = vertex->wB - vertex->wA; + + // Iteration count is equated to the number of support point calls. + ++iter; + ++b2_gjkIters; + + // Check for duplicate support points. This is the main termination criteria. + bool duplicate = false; + for (int32 i = 0; i < saveCount; ++i) + { + if (vertex->indexA == saveA[i] && vertex->indexB == saveB[i]) + { + duplicate = true; + break; + } + } + + // If we found a duplicate support point we must exit to avoid cycling. + if (duplicate) + { + break; + } + + // New vertex is ok and needed. + ++simplex.m_count; + } + + b2_gjkMaxIters = b2Max(b2_gjkMaxIters, iter); + + // Prepare output. + simplex.GetWitnessPoints(&output->pointA, &output->pointB); + output->distance = b2Distance(output->pointA, output->pointB); + output->iterations = iter; + + // Cache the simplex. + simplex.WriteCache(cache); + + // Apply radii if requested + if (input->useRadii) + { + if (output->distance < b2_epsilon) + { + // Shapes are too close to safely compute normal + b2Vec2 p = 0.5f * (output->pointA + output->pointB); + output->pointA = p; + output->pointB = p; + output->distance = 0.0f; + } + else + { + // Keep closest points on perimeter even if overlapped, this way + // the points move smoothly. + float rA = proxyA->m_radius; + float rB = proxyB->m_radius; + b2Vec2 normal = output->pointB - output->pointA; + normal.Normalize(); + output->distance = b2Max(0.0f, output->distance - rA - rB); + output->pointA += rA * normal; + output->pointB -= rB * normal; + } + } +} + +// GJK-raycast +// Algorithm by Gino van den Bergen. +// "Smooth Mesh Contacts with GJK" in Game Physics Pearls. 2010 +bool b2ShapeCast(b2ShapeCastOutput * output, const b2ShapeCastInput * input) +{ + output->iterations = 0; + output->lambda = 1.0f; + output->normal.SetZero(); + output->point.SetZero(); + + const b2DistanceProxy* proxyA = &input->proxyA; + const b2DistanceProxy* proxyB = &input->proxyB; + + float radiusA = b2Max(proxyA->m_radius, b2_polygonRadius); + float radiusB = b2Max(proxyB->m_radius, b2_polygonRadius); + float radius = radiusA + radiusB; + + b2Transform xfA = input->transformA; + b2Transform xfB = input->transformB; + + b2Vec2 r = input->translationB; + b2Vec2 n(0.0f, 0.0f); + float lambda = 0.0f; + + // Initial simplex + b2Simplex simplex; + simplex.m_count = 0; + + // Get simplex vertices as an array. + b2SimplexVertex* vertices = &simplex.m_v1; + + // Get support point in -r direction + int32 indexA = proxyA->GetSupport(b2MulT(xfA.q, -r)); + b2Vec2 wA = b2Mul(xfA, proxyA->GetVertex(indexA)); + int32 indexB = proxyB->GetSupport(b2MulT(xfB.q, r)); + b2Vec2 wB = b2Mul(xfB, proxyB->GetVertex(indexB)); + b2Vec2 v = wA - wB; + + // Sigma is the target distance between polygons + float sigma = b2Max(b2_polygonRadius, radius - b2_polygonRadius); + const float tolerance = 0.5f * b2_linearSlop; + + // Main iteration loop. + const int32 k_maxIters = 20; + int32 iter = 0; + while (iter < k_maxIters && v.Length() - sigma > tolerance) + { + b2Assert(simplex.m_count < 3); + + output->iterations += 1; + + // Support in direction -v (A - B) + indexA = proxyA->GetSupport(b2MulT(xfA.q, -v)); + wA = b2Mul(xfA, proxyA->GetVertex(indexA)); + indexB = proxyB->GetSupport(b2MulT(xfB.q, v)); + wB = b2Mul(xfB, proxyB->GetVertex(indexB)); + b2Vec2 p = wA - wB; + + // -v is a normal at p + v.Normalize(); + + // Intersect ray with plane + float vp = b2Dot(v, p); + float vr = b2Dot(v, r); + if (vp - sigma > lambda * vr) + { + if (vr <= 0.0f) + { + return false; + } + + lambda = (vp - sigma) / vr; + if (lambda > 1.0f) + { + return false; + } + + n = -v; + simplex.m_count = 0; + } + + // Reverse simplex since it works with B - A. + // Shift by lambda * r because we want the closest point to the current clip point. + // Note that the support point p is not shifted because we want the plane equation + // to be formed in unshifted space. + b2SimplexVertex* vertex = vertices + simplex.m_count; + vertex->indexA = indexB; + vertex->wA = wB + lambda * r; + vertex->indexB = indexA; + vertex->wB = wA; + vertex->w = vertex->wB - vertex->wA; + vertex->a = 1.0f; + simplex.m_count += 1; + + switch (simplex.m_count) + { + case 1: + break; + + case 2: + simplex.Solve2(); + break; + + case 3: + simplex.Solve3(); + break; + + default: + b2Assert(false); + } + + // If we have 3 points, then the origin is in the corresponding triangle. + if (simplex.m_count == 3) + { + // Overlap + return false; + } + + // Get search direction. + v = simplex.GetClosestPoint(); + + // Iteration count is equated to the number of support point calls. + ++iter; + } + + if (iter == 0) + { + // Initial overlap + return false; + } + + // Prepare output. + b2Vec2 pointA, pointB; + simplex.GetWitnessPoints(&pointB, &pointA); + + if (v.LengthSquared() > 0.0f) + { + n = -v; + n.Normalize(); + } + + output->point = pointA + radiusA * n; + output->normal = n; + output->lambda = lambda; + output->iterations = iter; + return true; +} diff --git a/libs/Box2D/Collision/b2DynamicTree.cpp b/libs/box2d/src/collision/b2_dynamic_tree.cpp similarity index 83% rename from libs/Box2D/Collision/b2DynamicTree.cpp rename to libs/box2d/src/collision/b2_dynamic_tree.cpp index cc41b285..55a9d324 100644 --- a/libs/Box2D/Collision/b2DynamicTree.cpp +++ b/libs/box2d/src/collision/b2_dynamic_tree.cpp @@ -1,778 +1,801 @@ -/* -* Copyright (c) 2009 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ - -#include -#include - -b2DynamicTree::b2DynamicTree() -{ - m_root = b2_nullNode; - - m_nodeCapacity = 16; - m_nodeCount = 0; - m_nodes = (b2TreeNode*)b2Alloc(m_nodeCapacity * sizeof(b2TreeNode)); - memset(m_nodes, 0, m_nodeCapacity * sizeof(b2TreeNode)); - - // Build a linked list for the free list. - for (int32 i = 0; i < m_nodeCapacity - 1; ++i) - { - m_nodes[i].next = i + 1; - m_nodes[i].height = -1; - } - m_nodes[m_nodeCapacity-1].next = b2_nullNode; - m_nodes[m_nodeCapacity-1].height = -1; - m_freeList = 0; - - m_path = 0; - - m_insertionCount = 0; -} - -b2DynamicTree::~b2DynamicTree() -{ - // This frees the entire tree in one shot. - b2Free(m_nodes); -} - -// Allocate a node from the pool. Grow the pool if necessary. -int32 b2DynamicTree::AllocateNode() -{ - // Expand the node pool as needed. - if (m_freeList == b2_nullNode) - { - b2Assert(m_nodeCount == m_nodeCapacity); - - // The free list is empty. Rebuild a bigger pool. - b2TreeNode* oldNodes = m_nodes; - m_nodeCapacity *= 2; - m_nodes = (b2TreeNode*)b2Alloc(m_nodeCapacity * sizeof(b2TreeNode)); - memcpy(m_nodes, oldNodes, m_nodeCount * sizeof(b2TreeNode)); - b2Free(oldNodes); - - // Build a linked list for the free list. The parent - // pointer becomes the "next" pointer. - for (int32 i = m_nodeCount; i < m_nodeCapacity - 1; ++i) - { - m_nodes[i].next = i + 1; - m_nodes[i].height = -1; - } - m_nodes[m_nodeCapacity-1].next = b2_nullNode; - m_nodes[m_nodeCapacity-1].height = -1; - m_freeList = m_nodeCount; - } - - // Peel a node off the free list. - int32 nodeId = m_freeList; - m_freeList = m_nodes[nodeId].next; - m_nodes[nodeId].parent = b2_nullNode; - m_nodes[nodeId].child1 = b2_nullNode; - m_nodes[nodeId].child2 = b2_nullNode; - m_nodes[nodeId].height = 0; - m_nodes[nodeId].userData = NULL; - ++m_nodeCount; - return nodeId; -} - -// Return a node to the pool. -void b2DynamicTree::FreeNode(int32 nodeId) -{ - b2Assert(0 <= nodeId && nodeId < m_nodeCapacity); - b2Assert(0 < m_nodeCount); - m_nodes[nodeId].next = m_freeList; - m_nodes[nodeId].height = -1; - m_freeList = nodeId; - --m_nodeCount; -} - -// Create a proxy in the tree as a leaf node. We return the index -// of the node instead of a pointer so that we can grow -// the node pool. -int32 b2DynamicTree::CreateProxy(const b2AABB& aabb, void* userData) -{ - int32 proxyId = AllocateNode(); - - // Fatten the aabb. - b2Vec2 r(b2_aabbExtension, b2_aabbExtension); - m_nodes[proxyId].aabb.lowerBound = aabb.lowerBound - r; - m_nodes[proxyId].aabb.upperBound = aabb.upperBound + r; - m_nodes[proxyId].userData = userData; - m_nodes[proxyId].height = 0; - - InsertLeaf(proxyId); - - return proxyId; -} - -void b2DynamicTree::DestroyProxy(int32 proxyId) -{ - b2Assert(0 <= proxyId && proxyId < m_nodeCapacity); - b2Assert(m_nodes[proxyId].IsLeaf()); - - RemoveLeaf(proxyId); - FreeNode(proxyId); -} - -bool b2DynamicTree::MoveProxy(int32 proxyId, const b2AABB& aabb, const b2Vec2& displacement) -{ - b2Assert(0 <= proxyId && proxyId < m_nodeCapacity); - - b2Assert(m_nodes[proxyId].IsLeaf()); - - if (m_nodes[proxyId].aabb.Contains(aabb)) - { - return false; - } - - RemoveLeaf(proxyId); - - // Extend AABB. - b2AABB b = aabb; - b2Vec2 r(b2_aabbExtension, b2_aabbExtension); - b.lowerBound = b.lowerBound - r; - b.upperBound = b.upperBound + r; - - // Predict AABB displacement. - b2Vec2 d = b2_aabbMultiplier * displacement; - - if (d.x < 0.0f) - { - b.lowerBound.x += d.x; - } - else - { - b.upperBound.x += d.x; - } - - if (d.y < 0.0f) - { - b.lowerBound.y += d.y; - } - else - { - b.upperBound.y += d.y; - } - - m_nodes[proxyId].aabb = b; - - InsertLeaf(proxyId); - return true; -} - -void b2DynamicTree::InsertLeaf(int32 leaf) -{ - ++m_insertionCount; - - if (m_root == b2_nullNode) - { - m_root = leaf; - m_nodes[m_root].parent = b2_nullNode; - return; - } - - // Find the best sibling for this node - b2AABB leafAABB = m_nodes[leaf].aabb; - int32 index = m_root; - while (m_nodes[index].IsLeaf() == false) - { - int32 child1 = m_nodes[index].child1; - int32 child2 = m_nodes[index].child2; - - float32 area = m_nodes[index].aabb.GetPerimeter(); - - b2AABB combinedAABB; - combinedAABB.Combine(m_nodes[index].aabb, leafAABB); - float32 combinedArea = combinedAABB.GetPerimeter(); - - // Cost of creating a new parent for this node and the new leaf - float32 cost = 2.0f * combinedArea; - - // Minimum cost of pushing the leaf further down the tree - float32 inheritanceCost = 2.0f * (combinedArea - area); - - // Cost of descending into child1 - float32 cost1; - if (m_nodes[child1].IsLeaf()) - { - b2AABB aabb; - aabb.Combine(leafAABB, m_nodes[child1].aabb); - cost1 = aabb.GetPerimeter() + inheritanceCost; - } - else - { - b2AABB aabb; - aabb.Combine(leafAABB, m_nodes[child1].aabb); - float32 oldArea = m_nodes[child1].aabb.GetPerimeter(); - float32 newArea = aabb.GetPerimeter(); - cost1 = (newArea - oldArea) + inheritanceCost; - } - - // Cost of descending into child2 - float32 cost2; - if (m_nodes[child2].IsLeaf()) - { - b2AABB aabb; - aabb.Combine(leafAABB, m_nodes[child2].aabb); - cost2 = aabb.GetPerimeter() + inheritanceCost; - } - else - { - b2AABB aabb; - aabb.Combine(leafAABB, m_nodes[child2].aabb); - float32 oldArea = m_nodes[child2].aabb.GetPerimeter(); - float32 newArea = aabb.GetPerimeter(); - cost2 = newArea - oldArea + inheritanceCost; - } - - // Descend according to the minimum cost. - if (cost < cost1 && cost < cost2) - { - break; - } - - // Descend - if (cost1 < cost2) - { - index = child1; - } - else - { - index = child2; - } - } - - int32 sibling = index; - - // Create a new parent. - int32 oldParent = m_nodes[sibling].parent; - int32 newParent = AllocateNode(); - m_nodes[newParent].parent = oldParent; - m_nodes[newParent].userData = NULL; - m_nodes[newParent].aabb.Combine(leafAABB, m_nodes[sibling].aabb); - m_nodes[newParent].height = m_nodes[sibling].height + 1; - - if (oldParent != b2_nullNode) - { - // The sibling was not the root. - if (m_nodes[oldParent].child1 == sibling) - { - m_nodes[oldParent].child1 = newParent; - } - else - { - m_nodes[oldParent].child2 = newParent; - } - - m_nodes[newParent].child1 = sibling; - m_nodes[newParent].child2 = leaf; - m_nodes[sibling].parent = newParent; - m_nodes[leaf].parent = newParent; - } - else - { - // The sibling was the root. - m_nodes[newParent].child1 = sibling; - m_nodes[newParent].child2 = leaf; - m_nodes[sibling].parent = newParent; - m_nodes[leaf].parent = newParent; - m_root = newParent; - } - - // Walk back up the tree fixing heights and AABBs - index = m_nodes[leaf].parent; - while (index != b2_nullNode) - { - index = Balance(index); - - int32 child1 = m_nodes[index].child1; - int32 child2 = m_nodes[index].child2; - - b2Assert(child1 != b2_nullNode); - b2Assert(child2 != b2_nullNode); - - m_nodes[index].height = 1 + b2Max(m_nodes[child1].height, m_nodes[child2].height); - m_nodes[index].aabb.Combine(m_nodes[child1].aabb, m_nodes[child2].aabb); - - index = m_nodes[index].parent; - } - - //Validate(); -} - -void b2DynamicTree::RemoveLeaf(int32 leaf) -{ - if (leaf == m_root) - { - m_root = b2_nullNode; - return; - } - - int32 parent = m_nodes[leaf].parent; - int32 grandParent = m_nodes[parent].parent; - int32 sibling; - if (m_nodes[parent].child1 == leaf) - { - sibling = m_nodes[parent].child2; - } - else - { - sibling = m_nodes[parent].child1; - } - - if (grandParent != b2_nullNode) - { - // Destroy parent and connect sibling to grandParent. - if (m_nodes[grandParent].child1 == parent) - { - m_nodes[grandParent].child1 = sibling; - } - else - { - m_nodes[grandParent].child2 = sibling; - } - m_nodes[sibling].parent = grandParent; - FreeNode(parent); - - // Adjust ancestor bounds. - int32 index = grandParent; - while (index != b2_nullNode) - { - index = Balance(index); - - int32 child1 = m_nodes[index].child1; - int32 child2 = m_nodes[index].child2; - - m_nodes[index].aabb.Combine(m_nodes[child1].aabb, m_nodes[child2].aabb); - m_nodes[index].height = 1 + b2Max(m_nodes[child1].height, m_nodes[child2].height); - - index = m_nodes[index].parent; - } - } - else - { - m_root = sibling; - m_nodes[sibling].parent = b2_nullNode; - FreeNode(parent); - } - - //Validate(); -} - -// Perform a left or right rotation if node A is imbalanced. -// Returns the new root index. -int32 b2DynamicTree::Balance(int32 iA) -{ - b2Assert(iA != b2_nullNode); - - b2TreeNode* A = m_nodes + iA; - if (A->IsLeaf() || A->height < 2) - { - return iA; - } - - int32 iB = A->child1; - int32 iC = A->child2; - b2Assert(0 <= iB && iB < m_nodeCapacity); - b2Assert(0 <= iC && iC < m_nodeCapacity); - - b2TreeNode* B = m_nodes + iB; - b2TreeNode* C = m_nodes + iC; - - int32 balance = C->height - B->height; - - // Rotate C up - if (balance > 1) - { - int32 iF = C->child1; - int32 iG = C->child2; - b2TreeNode* F = m_nodes + iF; - b2TreeNode* G = m_nodes + iG; - b2Assert(0 <= iF && iF < m_nodeCapacity); - b2Assert(0 <= iG && iG < m_nodeCapacity); - - // Swap A and C - C->child1 = iA; - C->parent = A->parent; - A->parent = iC; - - // A's old parent should point to C - if (C->parent != b2_nullNode) - { - if (m_nodes[C->parent].child1 == iA) - { - m_nodes[C->parent].child1 = iC; - } - else - { - b2Assert(m_nodes[C->parent].child2 == iA); - m_nodes[C->parent].child2 = iC; - } - } - else - { - m_root = iC; - } - - // Rotate - if (F->height > G->height) - { - C->child2 = iF; - A->child2 = iG; - G->parent = iA; - A->aabb.Combine(B->aabb, G->aabb); - C->aabb.Combine(A->aabb, F->aabb); - - A->height = 1 + b2Max(B->height, G->height); - C->height = 1 + b2Max(A->height, F->height); - } - else - { - C->child2 = iG; - A->child2 = iF; - F->parent = iA; - A->aabb.Combine(B->aabb, F->aabb); - C->aabb.Combine(A->aabb, G->aabb); - - A->height = 1 + b2Max(B->height, F->height); - C->height = 1 + b2Max(A->height, G->height); - } - - return iC; - } - - // Rotate B up - if (balance < -1) - { - int32 iD = B->child1; - int32 iE = B->child2; - b2TreeNode* D = m_nodes + iD; - b2TreeNode* E = m_nodes + iE; - b2Assert(0 <= iD && iD < m_nodeCapacity); - b2Assert(0 <= iE && iE < m_nodeCapacity); - - // Swap A and B - B->child1 = iA; - B->parent = A->parent; - A->parent = iB; - - // A's old parent should point to B - if (B->parent != b2_nullNode) - { - if (m_nodes[B->parent].child1 == iA) - { - m_nodes[B->parent].child1 = iB; - } - else - { - b2Assert(m_nodes[B->parent].child2 == iA); - m_nodes[B->parent].child2 = iB; - } - } - else - { - m_root = iB; - } - - // Rotate - if (D->height > E->height) - { - B->child2 = iD; - A->child1 = iE; - E->parent = iA; - A->aabb.Combine(C->aabb, E->aabb); - B->aabb.Combine(A->aabb, D->aabb); - - A->height = 1 + b2Max(C->height, E->height); - B->height = 1 + b2Max(A->height, D->height); - } - else - { - B->child2 = iE; - A->child1 = iD; - D->parent = iA; - A->aabb.Combine(C->aabb, D->aabb); - B->aabb.Combine(A->aabb, E->aabb); - - A->height = 1 + b2Max(C->height, D->height); - B->height = 1 + b2Max(A->height, E->height); - } - - return iB; - } - - return iA; -} - -int32 b2DynamicTree::GetHeight() const -{ - if (m_root == b2_nullNode) - { - return 0; - } - - return m_nodes[m_root].height; -} - -// -float32 b2DynamicTree::GetAreaRatio() const -{ - if (m_root == b2_nullNode) - { - return 0.0f; - } - - const b2TreeNode* root = m_nodes + m_root; - float32 rootArea = root->aabb.GetPerimeter(); - - float32 totalArea = 0.0f; - for (int32 i = 0; i < m_nodeCapacity; ++i) - { - const b2TreeNode* node = m_nodes + i; - if (node->height < 0) - { - // Free node in pool - continue; - } - - totalArea += node->aabb.GetPerimeter(); - } - - return totalArea / rootArea; -} - -// Compute the height of a sub-tree. -int32 b2DynamicTree::ComputeHeight(int32 nodeId) const -{ - b2Assert(0 <= nodeId && nodeId < m_nodeCapacity); - b2TreeNode* node = m_nodes + nodeId; - - if (node->IsLeaf()) - { - return 0; - } - - int32 height1 = ComputeHeight(node->child1); - int32 height2 = ComputeHeight(node->child2); - return 1 + b2Max(height1, height2); -} - -int32 b2DynamicTree::ComputeHeight() const -{ - int32 height = ComputeHeight(m_root); - return height; -} - -void b2DynamicTree::ValidateStructure(int32 index) const -{ - if (index == b2_nullNode) - { - return; - } - - if (index == m_root) - { - b2Assert(m_nodes[index].parent == b2_nullNode); - } - - const b2TreeNode* node = m_nodes + index; - - int32 child1 = node->child1; - int32 child2 = node->child2; - - if (node->IsLeaf()) - { - b2Assert(child1 == b2_nullNode); - b2Assert(child2 == b2_nullNode); - b2Assert(node->height == 0); - return; - } - - b2Assert(0 <= child1 && child1 < m_nodeCapacity); - b2Assert(0 <= child2 && child2 < m_nodeCapacity); - - b2Assert(m_nodes[child1].parent == index); - b2Assert(m_nodes[child2].parent == index); - - ValidateStructure(child1); - ValidateStructure(child2); -} - -void b2DynamicTree::ValidateMetrics(int32 index) const -{ - if (index == b2_nullNode) - { - return; - } - - const b2TreeNode* node = m_nodes + index; - - int32 child1 = node->child1; - int32 child2 = node->child2; - - if (node->IsLeaf()) - { - b2Assert(child1 == b2_nullNode); - b2Assert(child2 == b2_nullNode); - b2Assert(node->height == 0); - return; - } - - b2Assert(0 <= child1 && child1 < m_nodeCapacity); - b2Assert(0 <= child2 && child2 < m_nodeCapacity); - - int32 height1 = m_nodes[child1].height; - int32 height2 = m_nodes[child2].height; - int32 height; - height = 1 + b2Max(height1, height2); - b2Assert(node->height == height); - - b2AABB aabb; - aabb.Combine(m_nodes[child1].aabb, m_nodes[child2].aabb); - - b2Assert(aabb.lowerBound == node->aabb.lowerBound); - b2Assert(aabb.upperBound == node->aabb.upperBound); - - ValidateMetrics(child1); - ValidateMetrics(child2); -} - -void b2DynamicTree::Validate() const -{ - ValidateStructure(m_root); - ValidateMetrics(m_root); - - int32 freeCount = 0; - int32 freeIndex = m_freeList; - while (freeIndex != b2_nullNode) - { - b2Assert(0 <= freeIndex && freeIndex < m_nodeCapacity); - freeIndex = m_nodes[freeIndex].next; - ++freeCount; - } - - b2Assert(GetHeight() == ComputeHeight()); - - b2Assert(m_nodeCount + freeCount == m_nodeCapacity); -} - -int32 b2DynamicTree::GetMaxBalance() const -{ - int32 maxBalance = 0; - for (int32 i = 0; i < m_nodeCapacity; ++i) - { - const b2TreeNode* node = m_nodes + i; - if (node->height <= 1) - { - continue; - } - - b2Assert(node->IsLeaf() == false); - - int32 child1 = node->child1; - int32 child2 = node->child2; - int32 balance = b2Abs(m_nodes[child2].height - m_nodes[child1].height); - maxBalance = b2Max(maxBalance, balance); - } - - return maxBalance; -} - -void b2DynamicTree::RebuildBottomUp() -{ - int32* nodes = (int32*)b2Alloc(m_nodeCount * sizeof(int32)); - int32 count = 0; - - // Build array of leaves. Free the rest. - for (int32 i = 0; i < m_nodeCapacity; ++i) - { - if (m_nodes[i].height < 0) - { - // free node in pool - continue; - } - - if (m_nodes[i].IsLeaf()) - { - m_nodes[i].parent = b2_nullNode; - nodes[count] = i; - ++count; - } - else - { - FreeNode(i); - } - } - - while (count > 1) - { - float32 minCost = b2_maxFloat; - int32 iMin = -1, jMin = -1; - for (int32 i = 0; i < count; ++i) - { - b2AABB aabbi = m_nodes[nodes[i]].aabb; - - for (int32 j = i + 1; j < count; ++j) - { - b2AABB aabbj = m_nodes[nodes[j]].aabb; - b2AABB b; - b.Combine(aabbi, aabbj); - float32 cost = b.GetPerimeter(); - if (cost < minCost) - { - iMin = i; - jMin = j; - minCost = cost; - } - } - } - - int32 index1 = nodes[iMin]; - int32 index2 = nodes[jMin]; - b2TreeNode* child1 = m_nodes + index1; - b2TreeNode* child2 = m_nodes + index2; - - int32 parentIndex = AllocateNode(); - b2TreeNode* parent = m_nodes + parentIndex; - parent->child1 = index1; - parent->child2 = index2; - parent->height = 1 + b2Max(child1->height, child2->height); - parent->aabb.Combine(child1->aabb, child2->aabb); - parent->parent = b2_nullNode; - - child1->parent = parentIndex; - child2->parent = parentIndex; - - nodes[jMin] = nodes[count-1]; - nodes[iMin] = parentIndex; - --count; - } - - m_root = nodes[0]; - b2Free(nodes); - - Validate(); -} - -void b2DynamicTree::ShiftOrigin(const b2Vec2& newOrigin) -{ - // Build array of leaves. Free the rest. - for (int32 i = 0; i < m_nodeCapacity; ++i) - { - m_nodes[i].aabb.lowerBound -= newOrigin; - m_nodes[i].aabb.upperBound -= newOrigin; - } -} +// MIT License + +// Copyright (c) 2019 Erin Catto + +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: + +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. + +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +#include "box2d/b2_dynamic_tree.h" +#include + +b2DynamicTree::b2DynamicTree() +{ + m_root = b2_nullNode; + + m_nodeCapacity = 16; + m_nodeCount = 0; + m_nodes = (b2TreeNode*)b2Alloc(m_nodeCapacity * sizeof(b2TreeNode)); + memset(m_nodes, 0, m_nodeCapacity * sizeof(b2TreeNode)); + + // Build a linked list for the free list. + for (int32 i = 0; i < m_nodeCapacity - 1; ++i) + { + m_nodes[i].next = i + 1; + m_nodes[i].height = -1; + } + m_nodes[m_nodeCapacity-1].next = b2_nullNode; + m_nodes[m_nodeCapacity-1].height = -1; + m_freeList = 0; + + m_insertionCount = 0; +} + +b2DynamicTree::~b2DynamicTree() +{ + // This frees the entire tree in one shot. + b2Free(m_nodes); +} + +// Allocate a node from the pool. Grow the pool if necessary. +int32 b2DynamicTree::AllocateNode() +{ + // Expand the node pool as needed. + if (m_freeList == b2_nullNode) + { + b2Assert(m_nodeCount == m_nodeCapacity); + + // The free list is empty. Rebuild a bigger pool. + b2TreeNode* oldNodes = m_nodes; + m_nodeCapacity *= 2; + m_nodes = (b2TreeNode*)b2Alloc(m_nodeCapacity * sizeof(b2TreeNode)); + memcpy(m_nodes, oldNodes, m_nodeCount * sizeof(b2TreeNode)); + b2Free(oldNodes); + + // Build a linked list for the free list. The parent + // pointer becomes the "next" pointer. + for (int32 i = m_nodeCount; i < m_nodeCapacity - 1; ++i) + { + m_nodes[i].next = i + 1; + m_nodes[i].height = -1; + } + m_nodes[m_nodeCapacity-1].next = b2_nullNode; + m_nodes[m_nodeCapacity-1].height = -1; + m_freeList = m_nodeCount; + } + + // Peel a node off the free list. + int32 nodeId = m_freeList; + m_freeList = m_nodes[nodeId].next; + m_nodes[nodeId].parent = b2_nullNode; + m_nodes[nodeId].child1 = b2_nullNode; + m_nodes[nodeId].child2 = b2_nullNode; + m_nodes[nodeId].height = 0; + m_nodes[nodeId].userData = nullptr; + m_nodes[nodeId].moved = false; + ++m_nodeCount; + return nodeId; +} + +// Return a node to the pool. +void b2DynamicTree::FreeNode(int32 nodeId) +{ + b2Assert(0 <= nodeId && nodeId < m_nodeCapacity); + b2Assert(0 < m_nodeCount); + m_nodes[nodeId].next = m_freeList; + m_nodes[nodeId].height = -1; + m_freeList = nodeId; + --m_nodeCount; +} + +// Create a proxy in the tree as a leaf node. We return the index +// of the node instead of a pointer so that we can grow +// the node pool. +int32 b2DynamicTree::CreateProxy(const b2AABB& aabb, void* userData) +{ + int32 proxyId = AllocateNode(); + + // Fatten the aabb. + b2Vec2 r(b2_aabbExtension, b2_aabbExtension); + m_nodes[proxyId].aabb.lowerBound = aabb.lowerBound - r; + m_nodes[proxyId].aabb.upperBound = aabb.upperBound + r; + m_nodes[proxyId].userData = userData; + m_nodes[proxyId].height = 0; + m_nodes[proxyId].moved = true; + + InsertLeaf(proxyId); + + return proxyId; +} + +void b2DynamicTree::DestroyProxy(int32 proxyId) +{ + b2Assert(0 <= proxyId && proxyId < m_nodeCapacity); + b2Assert(m_nodes[proxyId].IsLeaf()); + + RemoveLeaf(proxyId); + FreeNode(proxyId); +} + +bool b2DynamicTree::MoveProxy(int32 proxyId, const b2AABB& aabb, const b2Vec2& displacement) +{ + b2Assert(0 <= proxyId && proxyId < m_nodeCapacity); + + b2Assert(m_nodes[proxyId].IsLeaf()); + + // Extend AABB + b2AABB fatAABB; + b2Vec2 r(b2_aabbExtension, b2_aabbExtension); + fatAABB.lowerBound = aabb.lowerBound - r; + fatAABB.upperBound = aabb.upperBound + r; + + // Predict AABB movement + b2Vec2 d = b2_aabbMultiplier * displacement; + + if (d.x < 0.0f) + { + fatAABB.lowerBound.x += d.x; + } + else + { + fatAABB.upperBound.x += d.x; + } + + if (d.y < 0.0f) + { + fatAABB.lowerBound.y += d.y; + } + else + { + fatAABB.upperBound.y += d.y; + } + + const b2AABB& treeAABB = m_nodes[proxyId].aabb; + if (treeAABB.Contains(aabb)) + { + // The tree AABB still contains the object, but it might be too large. + // Perhaps the object was moving fast but has since gone to sleep. + // The huge AABB is larger than the new fat AABB. + b2AABB hugeAABB; + hugeAABB.lowerBound = fatAABB.lowerBound - 4.0f * r; + hugeAABB.upperBound = fatAABB.upperBound + 4.0f * r; + + if (hugeAABB.Contains(treeAABB)) + { + // The tree AABB contains the object AABB and the tree AABB is + // not too large. No tree update needed. + return false; + } + + // Otherwise the tree AABB is huge and needs to be shrunk + } + + RemoveLeaf(proxyId); + + m_nodes[proxyId].aabb = fatAABB; + + InsertLeaf(proxyId); + + m_nodes[proxyId].moved = true; + + return true; +} + +void b2DynamicTree::InsertLeaf(int32 leaf) +{ + ++m_insertionCount; + + if (m_root == b2_nullNode) + { + m_root = leaf; + m_nodes[m_root].parent = b2_nullNode; + return; + } + + // Find the best sibling for this node + b2AABB leafAABB = m_nodes[leaf].aabb; + int32 index = m_root; + while (m_nodes[index].IsLeaf() == false) + { + int32 child1 = m_nodes[index].child1; + int32 child2 = m_nodes[index].child2; + + float area = m_nodes[index].aabb.GetPerimeter(); + + b2AABB combinedAABB; + combinedAABB.Combine(m_nodes[index].aabb, leafAABB); + float combinedArea = combinedAABB.GetPerimeter(); + + // Cost of creating a new parent for this node and the new leaf + float cost = 2.0f * combinedArea; + + // Minimum cost of pushing the leaf further down the tree + float inheritanceCost = 2.0f * (combinedArea - area); + + // Cost of descending into child1 + float cost1; + if (m_nodes[child1].IsLeaf()) + { + b2AABB aabb; + aabb.Combine(leafAABB, m_nodes[child1].aabb); + cost1 = aabb.GetPerimeter() + inheritanceCost; + } + else + { + b2AABB aabb; + aabb.Combine(leafAABB, m_nodes[child1].aabb); + float oldArea = m_nodes[child1].aabb.GetPerimeter(); + float newArea = aabb.GetPerimeter(); + cost1 = (newArea - oldArea) + inheritanceCost; + } + + // Cost of descending into child2 + float cost2; + if (m_nodes[child2].IsLeaf()) + { + b2AABB aabb; + aabb.Combine(leafAABB, m_nodes[child2].aabb); + cost2 = aabb.GetPerimeter() + inheritanceCost; + } + else + { + b2AABB aabb; + aabb.Combine(leafAABB, m_nodes[child2].aabb); + float oldArea = m_nodes[child2].aabb.GetPerimeter(); + float newArea = aabb.GetPerimeter(); + cost2 = newArea - oldArea + inheritanceCost; + } + + // Descend according to the minimum cost. + if (cost < cost1 && cost < cost2) + { + break; + } + + // Descend + if (cost1 < cost2) + { + index = child1; + } + else + { + index = child2; + } + } + + int32 sibling = index; + + // Create a new parent. + int32 oldParent = m_nodes[sibling].parent; + int32 newParent = AllocateNode(); + m_nodes[newParent].parent = oldParent; + m_nodes[newParent].userData = nullptr; + m_nodes[newParent].aabb.Combine(leafAABB, m_nodes[sibling].aabb); + m_nodes[newParent].height = m_nodes[sibling].height + 1; + + if (oldParent != b2_nullNode) + { + // The sibling was not the root. + if (m_nodes[oldParent].child1 == sibling) + { + m_nodes[oldParent].child1 = newParent; + } + else + { + m_nodes[oldParent].child2 = newParent; + } + + m_nodes[newParent].child1 = sibling; + m_nodes[newParent].child2 = leaf; + m_nodes[sibling].parent = newParent; + m_nodes[leaf].parent = newParent; + } + else + { + // The sibling was the root. + m_nodes[newParent].child1 = sibling; + m_nodes[newParent].child2 = leaf; + m_nodes[sibling].parent = newParent; + m_nodes[leaf].parent = newParent; + m_root = newParent; + } + + // Walk back up the tree fixing heights and AABBs + index = m_nodes[leaf].parent; + while (index != b2_nullNode) + { + index = Balance(index); + + int32 child1 = m_nodes[index].child1; + int32 child2 = m_nodes[index].child2; + + b2Assert(child1 != b2_nullNode); + b2Assert(child2 != b2_nullNode); + + m_nodes[index].height = 1 + b2Max(m_nodes[child1].height, m_nodes[child2].height); + m_nodes[index].aabb.Combine(m_nodes[child1].aabb, m_nodes[child2].aabb); + + index = m_nodes[index].parent; + } + + //Validate(); +} + +void b2DynamicTree::RemoveLeaf(int32 leaf) +{ + if (leaf == m_root) + { + m_root = b2_nullNode; + return; + } + + int32 parent = m_nodes[leaf].parent; + int32 grandParent = m_nodes[parent].parent; + int32 sibling; + if (m_nodes[parent].child1 == leaf) + { + sibling = m_nodes[parent].child2; + } + else + { + sibling = m_nodes[parent].child1; + } + + if (grandParent != b2_nullNode) + { + // Destroy parent and connect sibling to grandParent. + if (m_nodes[grandParent].child1 == parent) + { + m_nodes[grandParent].child1 = sibling; + } + else + { + m_nodes[grandParent].child2 = sibling; + } + m_nodes[sibling].parent = grandParent; + FreeNode(parent); + + // Adjust ancestor bounds. + int32 index = grandParent; + while (index != b2_nullNode) + { + index = Balance(index); + + int32 child1 = m_nodes[index].child1; + int32 child2 = m_nodes[index].child2; + + m_nodes[index].aabb.Combine(m_nodes[child1].aabb, m_nodes[child2].aabb); + m_nodes[index].height = 1 + b2Max(m_nodes[child1].height, m_nodes[child2].height); + + index = m_nodes[index].parent; + } + } + else + { + m_root = sibling; + m_nodes[sibling].parent = b2_nullNode; + FreeNode(parent); + } + + //Validate(); +} + +// Perform a left or right rotation if node A is imbalanced. +// Returns the new root index. +int32 b2DynamicTree::Balance(int32 iA) +{ + b2Assert(iA != b2_nullNode); + + b2TreeNode* A = m_nodes + iA; + if (A->IsLeaf() || A->height < 2) + { + return iA; + } + + int32 iB = A->child1; + int32 iC = A->child2; + b2Assert(0 <= iB && iB < m_nodeCapacity); + b2Assert(0 <= iC && iC < m_nodeCapacity); + + b2TreeNode* B = m_nodes + iB; + b2TreeNode* C = m_nodes + iC; + + int32 balance = C->height - B->height; + + // Rotate C up + if (balance > 1) + { + int32 iF = C->child1; + int32 iG = C->child2; + b2TreeNode* F = m_nodes + iF; + b2TreeNode* G = m_nodes + iG; + b2Assert(0 <= iF && iF < m_nodeCapacity); + b2Assert(0 <= iG && iG < m_nodeCapacity); + + // Swap A and C + C->child1 = iA; + C->parent = A->parent; + A->parent = iC; + + // A's old parent should point to C + if (C->parent != b2_nullNode) + { + if (m_nodes[C->parent].child1 == iA) + { + m_nodes[C->parent].child1 = iC; + } + else + { + b2Assert(m_nodes[C->parent].child2 == iA); + m_nodes[C->parent].child2 = iC; + } + } + else + { + m_root = iC; + } + + // Rotate + if (F->height > G->height) + { + C->child2 = iF; + A->child2 = iG; + G->parent = iA; + A->aabb.Combine(B->aabb, G->aabb); + C->aabb.Combine(A->aabb, F->aabb); + + A->height = 1 + b2Max(B->height, G->height); + C->height = 1 + b2Max(A->height, F->height); + } + else + { + C->child2 = iG; + A->child2 = iF; + F->parent = iA; + A->aabb.Combine(B->aabb, F->aabb); + C->aabb.Combine(A->aabb, G->aabb); + + A->height = 1 + b2Max(B->height, F->height); + C->height = 1 + b2Max(A->height, G->height); + } + + return iC; + } + + // Rotate B up + if (balance < -1) + { + int32 iD = B->child1; + int32 iE = B->child2; + b2TreeNode* D = m_nodes + iD; + b2TreeNode* E = m_nodes + iE; + b2Assert(0 <= iD && iD < m_nodeCapacity); + b2Assert(0 <= iE && iE < m_nodeCapacity); + + // Swap A and B + B->child1 = iA; + B->parent = A->parent; + A->parent = iB; + + // A's old parent should point to B + if (B->parent != b2_nullNode) + { + if (m_nodes[B->parent].child1 == iA) + { + m_nodes[B->parent].child1 = iB; + } + else + { + b2Assert(m_nodes[B->parent].child2 == iA); + m_nodes[B->parent].child2 = iB; + } + } + else + { + m_root = iB; + } + + // Rotate + if (D->height > E->height) + { + B->child2 = iD; + A->child1 = iE; + E->parent = iA; + A->aabb.Combine(C->aabb, E->aabb); + B->aabb.Combine(A->aabb, D->aabb); + + A->height = 1 + b2Max(C->height, E->height); + B->height = 1 + b2Max(A->height, D->height); + } + else + { + B->child2 = iE; + A->child1 = iD; + D->parent = iA; + A->aabb.Combine(C->aabb, D->aabb); + B->aabb.Combine(A->aabb, E->aabb); + + A->height = 1 + b2Max(C->height, D->height); + B->height = 1 + b2Max(A->height, E->height); + } + + return iB; + } + + return iA; +} + +int32 b2DynamicTree::GetHeight() const +{ + if (m_root == b2_nullNode) + { + return 0; + } + + return m_nodes[m_root].height; +} + +// +float b2DynamicTree::GetAreaRatio() const +{ + if (m_root == b2_nullNode) + { + return 0.0f; + } + + const b2TreeNode* root = m_nodes + m_root; + float rootArea = root->aabb.GetPerimeter(); + + float totalArea = 0.0f; + for (int32 i = 0; i < m_nodeCapacity; ++i) + { + const b2TreeNode* node = m_nodes + i; + if (node->height < 0) + { + // Free node in pool + continue; + } + + totalArea += node->aabb.GetPerimeter(); + } + + return totalArea / rootArea; +} + +// Compute the height of a sub-tree. +int32 b2DynamicTree::ComputeHeight(int32 nodeId) const +{ + b2Assert(0 <= nodeId && nodeId < m_nodeCapacity); + b2TreeNode* node = m_nodes + nodeId; + + if (node->IsLeaf()) + { + return 0; + } + + int32 height1 = ComputeHeight(node->child1); + int32 height2 = ComputeHeight(node->child2); + return 1 + b2Max(height1, height2); +} + +int32 b2DynamicTree::ComputeHeight() const +{ + int32 height = ComputeHeight(m_root); + return height; +} + +void b2DynamicTree::ValidateStructure(int32 index) const +{ + if (index == b2_nullNode) + { + return; + } + + if (index == m_root) + { + b2Assert(m_nodes[index].parent == b2_nullNode); + } + + const b2TreeNode* node = m_nodes + index; + + int32 child1 = node->child1; + int32 child2 = node->child2; + + if (node->IsLeaf()) + { + b2Assert(child1 == b2_nullNode); + b2Assert(child2 == b2_nullNode); + b2Assert(node->height == 0); + return; + } + + b2Assert(0 <= child1 && child1 < m_nodeCapacity); + b2Assert(0 <= child2 && child2 < m_nodeCapacity); + + b2Assert(m_nodes[child1].parent == index); + b2Assert(m_nodes[child2].parent == index); + + ValidateStructure(child1); + ValidateStructure(child2); +} + +void b2DynamicTree::ValidateMetrics(int32 index) const +{ + if (index == b2_nullNode) + { + return; + } + + const b2TreeNode* node = m_nodes + index; + + int32 child1 = node->child1; + int32 child2 = node->child2; + + if (node->IsLeaf()) + { + b2Assert(child1 == b2_nullNode); + b2Assert(child2 == b2_nullNode); + b2Assert(node->height == 0); + return; + } + + b2Assert(0 <= child1 && child1 < m_nodeCapacity); + b2Assert(0 <= child2 && child2 < m_nodeCapacity); + + int32 height1 = m_nodes[child1].height; + int32 height2 = m_nodes[child2].height; + int32 height; + height = 1 + b2Max(height1, height2); + b2Assert(node->height == height); + + b2AABB aabb; + aabb.Combine(m_nodes[child1].aabb, m_nodes[child2].aabb); + + b2Assert(aabb.lowerBound == node->aabb.lowerBound); + b2Assert(aabb.upperBound == node->aabb.upperBound); + + ValidateMetrics(child1); + ValidateMetrics(child2); +} + +void b2DynamicTree::Validate() const +{ +#if defined(b2DEBUG) + ValidateStructure(m_root); + ValidateMetrics(m_root); + + int32 freeCount = 0; + int32 freeIndex = m_freeList; + while (freeIndex != b2_nullNode) + { + b2Assert(0 <= freeIndex && freeIndex < m_nodeCapacity); + freeIndex = m_nodes[freeIndex].next; + ++freeCount; + } + + b2Assert(GetHeight() == ComputeHeight()); + + b2Assert(m_nodeCount + freeCount == m_nodeCapacity); +#endif +} + +int32 b2DynamicTree::GetMaxBalance() const +{ + int32 maxBalance = 0; + for (int32 i = 0; i < m_nodeCapacity; ++i) + { + const b2TreeNode* node = m_nodes + i; + if (node->height <= 1) + { + continue; + } + + b2Assert(node->IsLeaf() == false); + + int32 child1 = node->child1; + int32 child2 = node->child2; + int32 balance = b2Abs(m_nodes[child2].height - m_nodes[child1].height); + maxBalance = b2Max(maxBalance, balance); + } + + return maxBalance; +} + +void b2DynamicTree::RebuildBottomUp() +{ + int32* nodes = (int32*)b2Alloc(m_nodeCount * sizeof(int32)); + int32 count = 0; + + // Build array of leaves. Free the rest. + for (int32 i = 0; i < m_nodeCapacity; ++i) + { + if (m_nodes[i].height < 0) + { + // free node in pool + continue; + } + + if (m_nodes[i].IsLeaf()) + { + m_nodes[i].parent = b2_nullNode; + nodes[count] = i; + ++count; + } + else + { + FreeNode(i); + } + } + + while (count > 1) + { + float minCost = b2_maxFloat; + int32 iMin = -1, jMin = -1; + for (int32 i = 0; i < count; ++i) + { + b2AABB aabbi = m_nodes[nodes[i]].aabb; + + for (int32 j = i + 1; j < count; ++j) + { + b2AABB aabbj = m_nodes[nodes[j]].aabb; + b2AABB b; + b.Combine(aabbi, aabbj); + float cost = b.GetPerimeter(); + if (cost < minCost) + { + iMin = i; + jMin = j; + minCost = cost; + } + } + } + + int32 index1 = nodes[iMin]; + int32 index2 = nodes[jMin]; + b2TreeNode* child1 = m_nodes + index1; + b2TreeNode* child2 = m_nodes + index2; + + int32 parentIndex = AllocateNode(); + b2TreeNode* parent = m_nodes + parentIndex; + parent->child1 = index1; + parent->child2 = index2; + parent->height = 1 + b2Max(child1->height, child2->height); + parent->aabb.Combine(child1->aabb, child2->aabb); + parent->parent = b2_nullNode; + + child1->parent = parentIndex; + child2->parent = parentIndex; + + nodes[jMin] = nodes[count-1]; + nodes[iMin] = parentIndex; + --count; + } + + m_root = nodes[0]; + b2Free(nodes); + + Validate(); +} + +void b2DynamicTree::ShiftOrigin(const b2Vec2& newOrigin) +{ + // Build array of leaves. Free the rest. + for (int32 i = 0; i < m_nodeCapacity; ++i) + { + m_nodes[i].aabb.lowerBound -= newOrigin; + m_nodes[i].aabb.upperBound -= newOrigin; + } +} diff --git a/libs/Box2D/Collision/Shapes/b2EdgeShape.cpp b/libs/box2d/src/collision/b2_edge_shape.cpp similarity index 52% rename from libs/Box2D/Collision/Shapes/b2EdgeShape.cpp rename to libs/box2d/src/collision/b2_edge_shape.cpp index 3514b2ed..65f0606a 100644 --- a/libs/Box2D/Collision/Shapes/b2EdgeShape.cpp +++ b/libs/box2d/src/collision/b2_edge_shape.cpp @@ -1,138 +1,158 @@ -/* -* Copyright (c) 2006-2010 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ - -#include -#include - -void b2EdgeShape::Set(const b2Vec2& v1, const b2Vec2& v2) -{ - m_vertex1 = v1; - m_vertex2 = v2; - m_hasVertex0 = false; - m_hasVertex3 = false; -} - -b2Shape* b2EdgeShape::Clone(b2BlockAllocator* allocator) const -{ - void* mem = allocator->Allocate(sizeof(b2EdgeShape)); - b2EdgeShape* clone = new (mem) b2EdgeShape; - *clone = *this; - return clone; -} - -int32 b2EdgeShape::GetChildCount() const -{ - return 1; -} - -bool b2EdgeShape::TestPoint(const b2Transform& xf, const b2Vec2& p) const -{ - B2_NOT_USED(xf); - B2_NOT_USED(p); - return false; -} - -// p = p1 + t * d -// v = v1 + s * e -// p1 + t * d = v1 + s * e -// s * e - t * d = p1 - v1 -bool b2EdgeShape::RayCast(b2RayCastOutput* output, const b2RayCastInput& input, - const b2Transform& xf, int32 childIndex) const -{ - B2_NOT_USED(childIndex); - - // Put the ray into the edge's frame of reference. - b2Vec2 p1 = b2MulT(xf.q, input.p1 - xf.p); - b2Vec2 p2 = b2MulT(xf.q, input.p2 - xf.p); - b2Vec2 d = p2 - p1; - - b2Vec2 v1 = m_vertex1; - b2Vec2 v2 = m_vertex2; - b2Vec2 e = v2 - v1; - b2Vec2 normal(e.y, -e.x); - normal.Normalize(); - - // q = p1 + t * d - // dot(normal, q - v1) = 0 - // dot(normal, p1 - v1) + t * dot(normal, d) = 0 - float32 numerator = b2Dot(normal, v1 - p1); - float32 denominator = b2Dot(normal, d); - - if (denominator == 0.0f) - { - return false; - } - - float32 t = numerator / denominator; - if (t < 0.0f || input.maxFraction < t) - { - return false; - } - - b2Vec2 q = p1 + t * d; - - // q = v1 + s * r - // s = dot(q - v1, r) / dot(r, r) - b2Vec2 r = v2 - v1; - float32 rr = b2Dot(r, r); - if (rr == 0.0f) - { - return false; - } - - float32 s = b2Dot(q - v1, r) / rr; - if (s < 0.0f || 1.0f < s) - { - return false; - } - - output->fraction = t; - if (numerator > 0.0f) - { - output->normal = -b2Mul(xf.q, normal); - } - else - { - output->normal = b2Mul(xf.q, normal); - } - return true; -} - -void b2EdgeShape::ComputeAABB(b2AABB* aabb, const b2Transform& xf, int32 childIndex) const -{ - B2_NOT_USED(childIndex); - - b2Vec2 v1 = b2Mul(xf, m_vertex1); - b2Vec2 v2 = b2Mul(xf, m_vertex2); - - b2Vec2 lower = b2Min(v1, v2); - b2Vec2 upper = b2Max(v1, v2); - - b2Vec2 r(m_radius, m_radius); - aabb->lowerBound = lower - r; - aabb->upperBound = upper + r; -} - -void b2EdgeShape::ComputeMass(b2MassData* massData, float32 density) const -{ - B2_NOT_USED(density); - - massData->mass = 0.0f; - massData->center = 0.5f * (m_vertex1 + m_vertex2); - massData->I = 0.0f; -} +// MIT License + +// Copyright (c) 2019 Erin Catto + +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: + +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. + +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. + +#include "box2d/b2_edge_shape.h" +#include "box2d/b2_block_allocator.h" +#include + +void b2EdgeShape::SetOneSided(const b2Vec2& v0, const b2Vec2& v1, const b2Vec2& v2, const b2Vec2& v3) +{ + m_vertex0 = v0; + m_vertex1 = v1; + m_vertex2 = v2; + m_vertex3 = v3; + m_oneSided = true; +} + +void b2EdgeShape::SetTwoSided(const b2Vec2& v1, const b2Vec2& v2) +{ + m_vertex1 = v1; + m_vertex2 = v2; + m_oneSided = false; +} + +b2Shape* b2EdgeShape::Clone(b2BlockAllocator* allocator) const +{ + void* mem = allocator->Allocate(sizeof(b2EdgeShape)); + b2EdgeShape* clone = new (mem) b2EdgeShape; + *clone = *this; + return clone; +} + +int32 b2EdgeShape::GetChildCount() const +{ + return 1; +} + +bool b2EdgeShape::TestPoint(const b2Transform& xf, const b2Vec2& p) const +{ + B2_NOT_USED(xf); + B2_NOT_USED(p); + return false; +} + +// p = p1 + t * d +// v = v1 + s * e +// p1 + t * d = v1 + s * e +// s * e - t * d = p1 - v1 +bool b2EdgeShape::RayCast(b2RayCastOutput* output, const b2RayCastInput& input, + const b2Transform& xf, int32 childIndex) const +{ + B2_NOT_USED(childIndex); + + // Put the ray into the edge's frame of reference. + b2Vec2 p1 = b2MulT(xf.q, input.p1 - xf.p); + b2Vec2 p2 = b2MulT(xf.q, input.p2 - xf.p); + b2Vec2 d = p2 - p1; + + b2Vec2 v1 = m_vertex1; + b2Vec2 v2 = m_vertex2; + b2Vec2 e = v2 - v1; + + // Normal points to the right, looking from v1 at v2 + b2Vec2 normal(e.y, -e.x); + normal.Normalize(); + + // q = p1 + t * d + // dot(normal, q - v1) = 0 + // dot(normal, p1 - v1) + t * dot(normal, d) = 0 + float numerator = b2Dot(normal, v1 - p1); + if (m_oneSided && numerator > 0.0f) + { + return false; + } + + float denominator = b2Dot(normal, d); + + if (denominator == 0.0f) + { + return false; + } + + float t = numerator / denominator; + if (t < 0.0f || input.maxFraction < t) + { + return false; + } + + b2Vec2 q = p1 + t * d; + + // q = v1 + s * r + // s = dot(q - v1, r) / dot(r, r) + b2Vec2 r = v2 - v1; + float rr = b2Dot(r, r); + if (rr == 0.0f) + { + return false; + } + + float s = b2Dot(q - v1, r) / rr; + if (s < 0.0f || 1.0f < s) + { + return false; + } + + output->fraction = t; + if (numerator > 0.0f) + { + output->normal = -b2Mul(xf.q, normal); + } + else + { + output->normal = b2Mul(xf.q, normal); + } + return true; +} + +void b2EdgeShape::ComputeAABB(b2AABB* aabb, const b2Transform& xf, int32 childIndex) const +{ + B2_NOT_USED(childIndex); + + b2Vec2 v1 = b2Mul(xf, m_vertex1); + b2Vec2 v2 = b2Mul(xf, m_vertex2); + + b2Vec2 lower = b2Min(v1, v2); + b2Vec2 upper = b2Max(v1, v2); + + b2Vec2 r(m_radius, m_radius); + aabb->lowerBound = lower - r; + aabb->upperBound = upper + r; +} + +void b2EdgeShape::ComputeMass(b2MassData* massData, float density) const +{ + B2_NOT_USED(density); + + massData->mass = 0.0f; + massData->center = 0.5f * (m_vertex1 + m_vertex2); + massData->I = 0.0f; +} diff --git a/libs/Box2D/Collision/Shapes/b2PolygonShape.cpp b/libs/box2d/src/collision/b2_polygon_shape.cpp similarity index 51% rename from libs/Box2D/Collision/Shapes/b2PolygonShape.cpp rename to libs/box2d/src/collision/b2_polygon_shape.cpp index 3708c30f..165919b4 100644 --- a/libs/Box2D/Collision/Shapes/b2PolygonShape.cpp +++ b/libs/box2d/src/collision/b2_polygon_shape.cpp @@ -1,470 +1,366 @@ -/* -* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ - -#include -#include - -b2Shape* b2PolygonShape::Clone(b2BlockAllocator* allocator) const -{ - void* mem = allocator->Allocate(sizeof(b2PolygonShape)); - b2PolygonShape* clone = new (mem) b2PolygonShape; - *clone = *this; - return clone; -} - -void b2PolygonShape::SetAsBox(float32 hx, float32 hy) -{ - m_count = 4; - m_vertices[0].Set(-hx, -hy); - m_vertices[1].Set( hx, -hy); - m_vertices[2].Set( hx, hy); - m_vertices[3].Set(-hx, hy); - m_normals[0].Set(0.0f, -1.0f); - m_normals[1].Set(1.0f, 0.0f); - m_normals[2].Set(0.0f, 1.0f); - m_normals[3].Set(-1.0f, 0.0f); - m_centroid.SetZero(); -} - -void b2PolygonShape::SetAsBox(float32 hx, float32 hy, const b2Vec2& center, float32 angle) -{ - m_count = 4; - m_vertices[0].Set(-hx, -hy); - m_vertices[1].Set( hx, -hy); - m_vertices[2].Set( hx, hy); - m_vertices[3].Set(-hx, hy); - m_normals[0].Set(0.0f, -1.0f); - m_normals[1].Set(1.0f, 0.0f); - m_normals[2].Set(0.0f, 1.0f); - m_normals[3].Set(-1.0f, 0.0f); - m_centroid = center; - - b2Transform xf; - xf.p = center; - xf.q.Set(angle); - - // Transform vertices and normals. - for (int32 i = 0; i < m_count; ++i) - { - m_vertices[i] = b2Mul(xf, m_vertices[i]); - m_normals[i] = b2Mul(xf.q, m_normals[i]); - } -} - -int32 b2PolygonShape::GetChildCount() const -{ - return 1; -} - -static b2Vec2 ComputeCentroid(const b2Vec2* vs, int32 count) -{ - b2Assert(count >= 3); - - b2Vec2 c; c.Set(0.0f, 0.0f); - float32 area = 0.0f; - - // pRef is the reference point for forming triangles. - // It's location doesn't change the result (except for rounding error). - b2Vec2 pRef(0.0f, 0.0f); -#if 0 - // This code would put the reference point inside the polygon. - for (int32 i = 0; i < count; ++i) - { - pRef += vs[i]; - } - pRef *= 1.0f / count; -#endif - - const float32 inv3 = 1.0f / 3.0f; - - for (int32 i = 0; i < count; ++i) - { - // Triangle vertices. - b2Vec2 p1 = pRef; - b2Vec2 p2 = vs[i]; - b2Vec2 p3 = i + 1 < count ? vs[i+1] : vs[0]; - - b2Vec2 e1 = p2 - p1; - b2Vec2 e2 = p3 - p1; - - float32 D = b2Cross(e1, e2); - - float32 triangleArea = 0.5f * D; - area += triangleArea; - - // Area weighted centroid - c += triangleArea * inv3 * (p1 + p2 + p3); - } - - // Centroid - //b2Assert(area > b2_epsilon); - c *= 1.0f / area; - return c; -} - -bool b2PolygonShape::Set(const b2Vec2* vertices, int32 count) -{ - //b2Assert(3 <= count && count <= b2_maxPolygonVertices); - if (count < 3 || count > b2_maxPolygonVertices) - { - SetAsBox(1.0f, 1.0f); - return false; - } - - int32 n = b2Min(count, b2_maxPolygonVertices); - - // Perform welding and copy vertices into local buffer. - b2Vec2 ps[b2_maxPolygonVertices]; - int32 tempCount = 0; - for (int32 i = 0; i < n; ++i) - { - b2Vec2 v = vertices[i]; - - bool unique = true; - for (int32 j = 0; j < tempCount; ++j) - { - if (b2DistanceSquared(v, ps[j]) < 0.5f * b2_linearSlop) - { - unique = false; - break; - } - } - - if (unique) - { - ps[tempCount++] = v; - } - } - - n = tempCount; - if (n < 3) - { - // Polygon is degenerate. - //b2Assert(false); - SetAsBox(1.0f, 1.0f); - return false; - } - - // Create the convex hull using the Gift wrapping algorithm - // http://en.wikipedia.org/wiki/Gift_wrapping_algorithm - - // Find the right most point on the hull - int32 i0 = 0; - float32 x0 = ps[0].x; - for (int32 i = 1; i < n; ++i) - { - float32 x = ps[i].x; - if (x > x0 || (x == x0 && ps[i].y < ps[i0].y)) - { - i0 = i; - x0 = x; - } - } - - int32 hull[b2_maxPolygonVertices]; - int32 m = 0; - int32 ih = i0; - - for (;;) - { - hull[m] = ih; - - int32 ie = 0; - for (int32 j = 1; j < n; ++j) - { - if (ie == ih) - { - ie = j; - continue; - } - - b2Vec2 r = ps[ie] - ps[hull[m]]; - b2Vec2 v = ps[j] - ps[hull[m]]; - float32 c = b2Cross(r, v); - if (c < 0.0f) - { - ie = j; - } - - // Collinearity check - if (c == 0.0f && v.LengthSquared() > r.LengthSquared()) - { - ie = j; - } - } - - ++m; - ih = ie; - - if (ie == i0) - { - break; - } - } - - if (m < 3) - { - // Polygon is degenerate. - //b2Assert(false); - SetAsBox(1.0f, 1.0f); - return false; - } - - m_count = m; - - // Copy vertices. - for (int32 i = 0; i < m; ++i) - { - m_vertices[i] = ps[hull[i]]; - } - - // Compute normals. Ensure the edges have non-zero length. - for (int32 i = 0; i < m; ++i) - { - int32 i1 = i; - int32 i2 = i + 1 < m ? i + 1 : 0; - b2Vec2 edge = m_vertices[i2] - m_vertices[i1]; - //b2Assert(edge.LengthSquared() > b2_epsilon * b2_epsilon); - if (edge.LengthSquared() < b2_epsilon * b2_epsilon) - return false; - m_normals[i] = b2Cross(edge, 1.0f); - m_normals[i].Normalize(); - } - - // Compute the polygon centroid. - m_centroid = ComputeCentroid(m_vertices, m); - return true; -} - -bool b2PolygonShape::TestPoint(const b2Transform& xf, const b2Vec2& p) const -{ - b2Vec2 pLocal = b2MulT(xf.q, p - xf.p); - - for (int32 i = 0; i < m_count; ++i) - { - float32 dot = b2Dot(m_normals[i], pLocal - m_vertices[i]); - if (dot > 0.0f) - { - return false; - } - } - - return true; -} - -bool b2PolygonShape::RayCast(b2RayCastOutput* output, const b2RayCastInput& input, - const b2Transform& xf, int32 childIndex) const -{ - B2_NOT_USED(childIndex); - - // Put the ray into the polygon's frame of reference. - b2Vec2 p1 = b2MulT(xf.q, input.p1 - xf.p); - b2Vec2 p2 = b2MulT(xf.q, input.p2 - xf.p); - b2Vec2 d = p2 - p1; - - float32 lower = 0.0f, upper = input.maxFraction; - - int32 index = -1; - - for (int32 i = 0; i < m_count; ++i) - { - // p = p1 + a * d - // dot(normal, p - v) = 0 - // dot(normal, p1 - v) + a * dot(normal, d) = 0 - float32 numerator = b2Dot(m_normals[i], m_vertices[i] - p1); - float32 denominator = b2Dot(m_normals[i], d); - - if (denominator == 0.0f) - { - if (numerator < 0.0f) - { - return false; - } - } - else - { - // Note: we want this predicate without division: - // lower < numerator / denominator, where denominator < 0 - // Since denominator < 0, we have to flip the inequality: - // lower < numerator / denominator <==> denominator * lower > numerator. - if (denominator < 0.0f && numerator < lower * denominator) - { - // Increase lower. - // The segment enters this half-space. - lower = numerator / denominator; - index = i; - } - else if (denominator > 0.0f && numerator < upper * denominator) - { - // Decrease upper. - // The segment exits this half-space. - upper = numerator / denominator; - } - } - - // The use of epsilon here causes the assert on lower to trip - // in some cases. Apparently the use of epsilon was to make edge - // shapes work, but now those are handled separately. - //if (upper < lower - b2_epsilon) - if (upper < lower) - { - return false; - } - } - - b2Assert(0.0f <= lower && lower <= input.maxFraction); - - if (index >= 0) - { - output->fraction = lower; - output->normal = b2Mul(xf.q, m_normals[index]); - return true; - } - - return false; -} - -void b2PolygonShape::ComputeAABB(b2AABB* aabb, const b2Transform& xf, int32 childIndex) const -{ - B2_NOT_USED(childIndex); - - b2Vec2 lower = b2Mul(xf, m_vertices[0]); - b2Vec2 upper = lower; - - for (int32 i = 1; i < m_count; ++i) - { - b2Vec2 v = b2Mul(xf, m_vertices[i]); - lower = b2Min(lower, v); - upper = b2Max(upper, v); - } - - b2Vec2 r(m_radius, m_radius); - aabb->lowerBound = lower - r; - aabb->upperBound = upper + r; -} - -void b2PolygonShape::ComputeMass(b2MassData* massData, float32 density) const -{ - // Polygon mass, centroid, and inertia. - // Let rho be the polygon density in mass per unit area. - // Then: - // mass = rho * int(dA) - // centroid.x = (1/mass) * rho * int(x * dA) - // centroid.y = (1/mass) * rho * int(y * dA) - // I = rho * int((x*x + y*y) * dA) - // - // We can compute these integrals by summing all the integrals - // for each triangle of the polygon. To evaluate the integral - // for a single triangle, we make a change of variables to - // the (u,v) coordinates of the triangle: - // x = x0 + e1x * u + e2x * v - // y = y0 + e1y * u + e2y * v - // where 0 <= u && 0 <= v && u + v <= 1. - // - // We integrate u from [0,1-v] and then v from [0,1]. - // We also need to use the Jacobian of the transformation: - // D = cross(e1, e2) - // - // Simplification: triangle centroid = (1/3) * (p1 + p2 + p3) - // - // The rest of the derivation is handled by computer algebra. - - b2Assert(m_count >= 3); - - b2Vec2 center; center.Set(0.0f, 0.0f); - float32 area = 0.0f; - float32 I = 0.0f; - - // s is the reference point for forming triangles. - // It's location doesn't change the result (except for rounding error). - b2Vec2 s(0.0f, 0.0f); - - // This code would put the reference point inside the polygon. - for (int32 i = 0; i < m_count; ++i) - { - s += m_vertices[i]; - } - s *= 1.0f / m_count; - - const float32 k_inv3 = 1.0f / 3.0f; - - for (int32 i = 0; i < m_count; ++i) - { - // Triangle vertices. - b2Vec2 e1 = m_vertices[i] - s; - b2Vec2 e2 = i + 1 < m_count ? m_vertices[i+1] - s : m_vertices[0] - s; - - float32 D = b2Cross(e1, e2); - - float32 triangleArea = 0.5f * D; - area += triangleArea; - - // Area weighted centroid - center += triangleArea * k_inv3 * (e1 + e2); - - float32 ex1 = e1.x, ey1 = e1.y; - float32 ex2 = e2.x, ey2 = e2.y; - - float32 intx2 = ex1*ex1 + ex2*ex1 + ex2*ex2; - float32 inty2 = ey1*ey1 + ey2*ey1 + ey2*ey2; - - I += (0.25f * k_inv3 * D) * (intx2 + inty2); - } - - // Total mass - massData->mass = density * area; - - // Center of mass - //b2Assert(area > b2_epsilon); - center *= 1.0f / area; - massData->center = center + s; - - // Inertia tensor relative to the local origin (point s). - massData->I = density * I; - - // Shift to center of mass then to original body origin. - massData->I += massData->mass * (b2Dot(massData->center, massData->center) - b2Dot(center, center)); -} - -bool b2PolygonShape::Validate() const -{ - for (int32 i = 0; i < m_count; ++i) - { - int32 i1 = i; - int32 i2 = i < m_count - 1 ? i1 + 1 : 0; - b2Vec2 p = m_vertices[i1]; - b2Vec2 e = m_vertices[i2] - p; - - for (int32 j = 0; j < m_count; ++j) - { - if (j == i1 || j == i2) - { - continue; - } - - b2Vec2 v = m_vertices[j] - p; - float32 c = b2Cross(e, v); - if (c < 0.0f) - { - return false; - } - } - } - - return true; -} +// MIT License + +// Copyright (c) 2019 Erin Catto + +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: + +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. + +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. + +#include "box2d/b2_polygon_shape.h" +#include "box2d/b2_block_allocator.h" + +#include + +b2PolygonShape::b2PolygonShape() +{ + m_type = e_polygon; + m_radius = b2_polygonRadius; + m_count = 0; + m_centroid.SetZero(); +} + +b2Shape* b2PolygonShape::Clone(b2BlockAllocator* allocator) const +{ + void* mem = allocator->Allocate(sizeof(b2PolygonShape)); + b2PolygonShape* clone = new (mem) b2PolygonShape; + *clone = *this; + return clone; +} + +void b2PolygonShape::SetAsBox(float hx, float hy) +{ + m_count = 4; + m_vertices[0].Set(-hx, -hy); + m_vertices[1].Set( hx, -hy); + m_vertices[2].Set( hx, hy); + m_vertices[3].Set(-hx, hy); + m_normals[0].Set(0.0f, -1.0f); + m_normals[1].Set(1.0f, 0.0f); + m_normals[2].Set(0.0f, 1.0f); + m_normals[3].Set(-1.0f, 0.0f); + m_centroid.SetZero(); +} + +void b2PolygonShape::SetAsBox(float hx, float hy, const b2Vec2& center, float angle) +{ + m_count = 4; + m_vertices[0].Set(-hx, -hy); + m_vertices[1].Set( hx, -hy); + m_vertices[2].Set( hx, hy); + m_vertices[3].Set(-hx, hy); + m_normals[0].Set(0.0f, -1.0f); + m_normals[1].Set(1.0f, 0.0f); + m_normals[2].Set(0.0f, 1.0f); + m_normals[3].Set(-1.0f, 0.0f); + m_centroid = center; + + b2Transform xf; + xf.p = center; + xf.q.Set(angle); + + // Transform vertices and normals. + for (int32 i = 0; i < m_count; ++i) + { + m_vertices[i] = b2Mul(xf, m_vertices[i]); + m_normals[i] = b2Mul(xf.q, m_normals[i]); + } +} + +int32 b2PolygonShape::GetChildCount() const +{ + return 1; +} + +static b2Vec2 ComputeCentroid(const b2Vec2* vs, int32 count) +{ + b2Assert(count >= 3); + + b2Vec2 c(0.0f, 0.0f); + float area = 0.0f; + + // Get a reference point for forming triangles. + // Use the first vertex to reduce round-off errors. + b2Vec2 s = vs[0]; + + const float inv3 = 1.0f / 3.0f; + + for (int32 i = 0; i < count; ++i) + { + // Triangle vertices. + b2Vec2 p1 = vs[0] - s; + b2Vec2 p2 = vs[i] - s; + b2Vec2 p3 = i + 1 < count ? vs[i+1] - s : vs[0] - s; + + b2Vec2 e1 = p2 - p1; + b2Vec2 e2 = p3 - p1; + + float D = b2Cross(e1, e2); + + float triangleArea = 0.5f * D; + area += triangleArea; + + // Area weighted centroid + c += triangleArea * inv3 * (p1 + p2 + p3); + } + + // Centroid + b2Assert(area > b2_epsilon); + c = (1.0f / area) * c + s; + return c; +} + +bool b2PolygonShape::Set(const b2Vec2* vertices, int32 count) +{ + b2Hull hull = b2ComputeHull(vertices, count); + + if (hull.count < 3) + { + return false; + } + + Set(hull); + + return true; +} + +void b2PolygonShape::Set(const b2Hull& hull) +{ + b2Assert(hull.count >= 3); + + m_count = hull.count; + + // Copy vertices + for (int32 i = 0; i < hull.count; ++i) + { + m_vertices[i] = hull.points[i]; + } + + // Compute normals. Ensure the edges have non-zero length. + for (int32 i = 0; i < m_count; ++i) + { + int32 i1 = i; + int32 i2 = i + 1 < m_count ? i + 1 : 0; + b2Vec2 edge = m_vertices[i2] - m_vertices[i1]; + b2Assert(edge.LengthSquared() > b2_epsilon * b2_epsilon); + m_normals[i] = b2Cross(edge, 1.0f); + m_normals[i].Normalize(); + } + + // Compute the polygon centroid. + m_centroid = ComputeCentroid(m_vertices, m_count); +} + +bool b2PolygonShape::TestPoint(const b2Transform& xf, const b2Vec2& p) const +{ + b2Vec2 pLocal = b2MulT(xf.q, p - xf.p); + + for (int32 i = 0; i < m_count; ++i) + { + float dot = b2Dot(m_normals[i], pLocal - m_vertices[i]); + if (dot > 0.0f) + { + return false; + } + } + + return true; +} + +bool b2PolygonShape::RayCast(b2RayCastOutput* output, const b2RayCastInput& input, + const b2Transform& xf, int32 childIndex) const +{ + B2_NOT_USED(childIndex); + + // Put the ray into the polygon's frame of reference. + b2Vec2 p1 = b2MulT(xf.q, input.p1 - xf.p); + b2Vec2 p2 = b2MulT(xf.q, input.p2 - xf.p); + b2Vec2 d = p2 - p1; + + float lower = 0.0f, upper = input.maxFraction; + + int32 index = -1; + + for (int32 i = 0; i < m_count; ++i) + { + // p = p1 + a * d + // dot(normal, p - v) = 0 + // dot(normal, p1 - v) + a * dot(normal, d) = 0 + float numerator = b2Dot(m_normals[i], m_vertices[i] - p1); + float denominator = b2Dot(m_normals[i], d); + + if (denominator == 0.0f) + { + if (numerator < 0.0f) + { + return false; + } + } + else + { + // Note: we want this predicate without division: + // lower < numerator / denominator, where denominator < 0 + // Since denominator < 0, we have to flip the inequality: + // lower < numerator / denominator <==> denominator * lower > numerator. + if (denominator < 0.0f && numerator < lower * denominator) + { + // Increase lower. + // The segment enters this half-space. + lower = numerator / denominator; + index = i; + } + else if (denominator > 0.0f && numerator < upper * denominator) + { + // Decrease upper. + // The segment exits this half-space. + upper = numerator / denominator; + } + } + + // The use of epsilon here causes the assert on lower to trip + // in some cases. Apparently the use of epsilon was to make edge + // shapes work, but now those are handled separately. + //if (upper < lower - b2_epsilon) + if (upper < lower) + { + return false; + } + } + + b2Assert(0.0f <= lower && lower <= input.maxFraction); + + if (index >= 0) + { + output->fraction = lower; + output->normal = b2Mul(xf.q, m_normals[index]); + return true; + } + + return false; +} + +void b2PolygonShape::ComputeAABB(b2AABB* aabb, const b2Transform& xf, int32 childIndex) const +{ + B2_NOT_USED(childIndex); + + b2Vec2 lower = b2Mul(xf, m_vertices[0]); + b2Vec2 upper = lower; + + for (int32 i = 1; i < m_count; ++i) + { + b2Vec2 v = b2Mul(xf, m_vertices[i]); + lower = b2Min(lower, v); + upper = b2Max(upper, v); + } + + b2Vec2 r(m_radius, m_radius); + aabb->lowerBound = lower - r; + aabb->upperBound = upper + r; +} + +void b2PolygonShape::ComputeMass(b2MassData* massData, float density) const +{ + // Polygon mass, centroid, and inertia. + // Let rho be the polygon density in mass per unit area. + // Then: + // mass = rho * int(dA) + // centroid.x = (1/mass) * rho * int(x * dA) + // centroid.y = (1/mass) * rho * int(y * dA) + // I = rho * int((x*x + y*y) * dA) + // + // We can compute these integrals by summing all the integrals + // for each triangle of the polygon. To evaluate the integral + // for a single triangle, we make a change of variables to + // the (u,v) coordinates of the triangle: + // x = x0 + e1x * u + e2x * v + // y = y0 + e1y * u + e2y * v + // where 0 <= u && 0 <= v && u + v <= 1. + // + // We integrate u from [0,1-v] and then v from [0,1]. + // We also need to use the Jacobian of the transformation: + // D = cross(e1, e2) + // + // Simplification: triangle centroid = (1/3) * (p1 + p2 + p3) + // + // The rest of the derivation is handled by computer algebra. + + b2Assert(m_count >= 3); + + b2Vec2 center(0.0f, 0.0f); + float area = 0.0f; + float I = 0.0f; + + // Get a reference point for forming triangles. + // Use the first vertex to reduce round-off errors. + b2Vec2 s = m_vertices[0]; + + const float k_inv3 = 1.0f / 3.0f; + + for (int32 i = 0; i < m_count; ++i) + { + // Triangle vertices. + b2Vec2 e1 = m_vertices[i] - s; + b2Vec2 e2 = i + 1 < m_count ? m_vertices[i+1] - s : m_vertices[0] - s; + + float D = b2Cross(e1, e2); + + float triangleArea = 0.5f * D; + area += triangleArea; + + // Area weighted centroid + center += triangleArea * k_inv3 * (e1 + e2); + + float ex1 = e1.x, ey1 = e1.y; + float ex2 = e2.x, ey2 = e2.y; + + float intx2 = ex1*ex1 + ex2*ex1 + ex2*ex2; + float inty2 = ey1*ey1 + ey2*ey1 + ey2*ey2; + + I += (0.25f * k_inv3 * D) * (intx2 + inty2); + } + + // Total mass + massData->mass = density * area; + + // Center of mass + b2Assert(area > b2_epsilon); + center *= 1.0f / area; + massData->center = center + s; + + // Inertia tensor relative to the local origin (point s). + massData->I = density * I; + + // Shift to center of mass then to original body origin. + massData->I += massData->mass * (b2Dot(massData->center, massData->center) - b2Dot(center, center)); +} + +bool b2PolygonShape::Validate() const +{ + if (m_count < 3 || b2_maxPolygonVertices < m_count) + { + return false; + } + + b2Hull hull; + for (int32 i = 0; i < m_count; ++i) + { + hull.points[i] = m_vertices[i]; + } + + hull.count = m_count; + + return b2ValidateHull(hull); +} diff --git a/libs/Box2D/Collision/b2TimeOfImpact.cpp b/libs/box2d/src/collision/b2_time_of_impact.cpp similarity index 74% rename from libs/Box2D/Collision/b2TimeOfImpact.cpp rename to libs/box2d/src/collision/b2_time_of_impact.cpp index fa2248c4..7a1011b7 100644 --- a/libs/Box2D/Collision/b2TimeOfImpact.cpp +++ b/libs/box2d/src/collision/b2_time_of_impact.cpp @@ -1,486 +1,490 @@ -/* -* Copyright (c) 2007-2009 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ - -#include -#include -#include -#include -#include -#include - -#include - -float32 b2_toiTime, b2_toiMaxTime; -int32 b2_toiCalls, b2_toiIters, b2_toiMaxIters; -int32 b2_toiRootIters, b2_toiMaxRootIters; - -// -struct b2SeparationFunction -{ - enum Type - { - e_points, - e_faceA, - e_faceB - }; - - // TODO_ERIN might not need to return the separation - - float32 Initialize(const b2SimplexCache* cache, - const b2DistanceProxy* proxyA, const b2Sweep& sweepA, - const b2DistanceProxy* proxyB, const b2Sweep& sweepB, - float32 t1) - { - m_proxyA = proxyA; - m_proxyB = proxyB; - int32 count = cache->count; - b2Assert(0 < count && count < 3); - - m_sweepA = sweepA; - m_sweepB = sweepB; - - b2Transform xfA, xfB; - m_sweepA.GetTransform(&xfA, t1); - m_sweepB.GetTransform(&xfB, t1); - - if (count == 1) - { - m_type = e_points; - b2Vec2 localPointA = m_proxyA->GetVertex(cache->indexA[0]); - b2Vec2 localPointB = m_proxyB->GetVertex(cache->indexB[0]); - b2Vec2 pointA = b2Mul(xfA, localPointA); - b2Vec2 pointB = b2Mul(xfB, localPointB); - m_axis = pointB - pointA; - float32 s = m_axis.Normalize(); - return s; - } - else if (cache->indexA[0] == cache->indexA[1]) - { - // Two points on B and one on A. - m_type = e_faceB; - b2Vec2 localPointB1 = proxyB->GetVertex(cache->indexB[0]); - b2Vec2 localPointB2 = proxyB->GetVertex(cache->indexB[1]); - - m_axis = b2Cross(localPointB2 - localPointB1, 1.0f); - m_axis.Normalize(); - b2Vec2 normal = b2Mul(xfB.q, m_axis); - - m_localPoint = 0.5f * (localPointB1 + localPointB2); - b2Vec2 pointB = b2Mul(xfB, m_localPoint); - - b2Vec2 localPointA = proxyA->GetVertex(cache->indexA[0]); - b2Vec2 pointA = b2Mul(xfA, localPointA); - - float32 s = b2Dot(pointA - pointB, normal); - if (s < 0.0f) - { - m_axis = -m_axis; - s = -s; - } - return s; - } - else - { - // Two points on A and one or two points on B. - m_type = e_faceA; - b2Vec2 localPointA1 = m_proxyA->GetVertex(cache->indexA[0]); - b2Vec2 localPointA2 = m_proxyA->GetVertex(cache->indexA[1]); - - m_axis = b2Cross(localPointA2 - localPointA1, 1.0f); - m_axis.Normalize(); - b2Vec2 normal = b2Mul(xfA.q, m_axis); - - m_localPoint = 0.5f * (localPointA1 + localPointA2); - b2Vec2 pointA = b2Mul(xfA, m_localPoint); - - b2Vec2 localPointB = m_proxyB->GetVertex(cache->indexB[0]); - b2Vec2 pointB = b2Mul(xfB, localPointB); - - float32 s = b2Dot(pointB - pointA, normal); - if (s < 0.0f) - { - m_axis = -m_axis; - s = -s; - } - return s; - } - } - - // - float32 FindMinSeparation(int32* indexA, int32* indexB, float32 t) const - { - b2Transform xfA, xfB; - m_sweepA.GetTransform(&xfA, t); - m_sweepB.GetTransform(&xfB, t); - - switch (m_type) - { - case e_points: - { - b2Vec2 axisA = b2MulT(xfA.q, m_axis); - b2Vec2 axisB = b2MulT(xfB.q, -m_axis); - - *indexA = m_proxyA->GetSupport(axisA); - *indexB = m_proxyB->GetSupport(axisB); - - b2Vec2 localPointA = m_proxyA->GetVertex(*indexA); - b2Vec2 localPointB = m_proxyB->GetVertex(*indexB); - - b2Vec2 pointA = b2Mul(xfA, localPointA); - b2Vec2 pointB = b2Mul(xfB, localPointB); - - float32 separation = b2Dot(pointB - pointA, m_axis); - return separation; - } - - case e_faceA: - { - b2Vec2 normal = b2Mul(xfA.q, m_axis); - b2Vec2 pointA = b2Mul(xfA, m_localPoint); - - b2Vec2 axisB = b2MulT(xfB.q, -normal); - - *indexA = -1; - *indexB = m_proxyB->GetSupport(axisB); - - b2Vec2 localPointB = m_proxyB->GetVertex(*indexB); - b2Vec2 pointB = b2Mul(xfB, localPointB); - - float32 separation = b2Dot(pointB - pointA, normal); - return separation; - } - - case e_faceB: - { - b2Vec2 normal = b2Mul(xfB.q, m_axis); - b2Vec2 pointB = b2Mul(xfB, m_localPoint); - - b2Vec2 axisA = b2MulT(xfA.q, -normal); - - *indexB = -1; - *indexA = m_proxyA->GetSupport(axisA); - - b2Vec2 localPointA = m_proxyA->GetVertex(*indexA); - b2Vec2 pointA = b2Mul(xfA, localPointA); - - float32 separation = b2Dot(pointA - pointB, normal); - return separation; - } - - default: - b2Assert(false); - *indexA = -1; - *indexB = -1; - return 0.0f; - } - } - - // - float32 Evaluate(int32 indexA, int32 indexB, float32 t) const - { - b2Transform xfA, xfB; - m_sweepA.GetTransform(&xfA, t); - m_sweepB.GetTransform(&xfB, t); - - switch (m_type) - { - case e_points: - { - b2Vec2 localPointA = m_proxyA->GetVertex(indexA); - b2Vec2 localPointB = m_proxyB->GetVertex(indexB); - - b2Vec2 pointA = b2Mul(xfA, localPointA); - b2Vec2 pointB = b2Mul(xfB, localPointB); - float32 separation = b2Dot(pointB - pointA, m_axis); - - return separation; - } - - case e_faceA: - { - b2Vec2 normal = b2Mul(xfA.q, m_axis); - b2Vec2 pointA = b2Mul(xfA, m_localPoint); - - b2Vec2 localPointB = m_proxyB->GetVertex(indexB); - b2Vec2 pointB = b2Mul(xfB, localPointB); - - float32 separation = b2Dot(pointB - pointA, normal); - return separation; - } - - case e_faceB: - { - b2Vec2 normal = b2Mul(xfB.q, m_axis); - b2Vec2 pointB = b2Mul(xfB, m_localPoint); - - b2Vec2 localPointA = m_proxyA->GetVertex(indexA); - b2Vec2 pointA = b2Mul(xfA, localPointA); - - float32 separation = b2Dot(pointA - pointB, normal); - return separation; - } - - default: - b2Assert(false); - return 0.0f; - } - } - - const b2DistanceProxy* m_proxyA; - const b2DistanceProxy* m_proxyB; - b2Sweep m_sweepA, m_sweepB; - Type m_type; - b2Vec2 m_localPoint; - b2Vec2 m_axis; -}; - -// CCD via the local separating axis method. This seeks progression -// by computing the largest time at which separation is maintained. -void b2TimeOfImpact(b2TOIOutput* output, const b2TOIInput* input) -{ - b2Timer timer; - - ++b2_toiCalls; - - output->state = b2TOIOutput::e_unknown; - output->t = input->tMax; - - const b2DistanceProxy* proxyA = &input->proxyA; - const b2DistanceProxy* proxyB = &input->proxyB; - - b2Sweep sweepA = input->sweepA; - b2Sweep sweepB = input->sweepB; - - // Large rotations can make the root finder fail, so we normalize the - // sweep angles. - sweepA.Normalize(); - sweepB.Normalize(); - - float32 tMax = input->tMax; - - float32 totalRadius = proxyA->m_radius + proxyB->m_radius; - float32 target = b2Max(b2_linearSlop, totalRadius - 3.0f * b2_linearSlop); - float32 tolerance = 0.25f * b2_linearSlop; - b2Assert(target > tolerance); - - float32 t1 = 0.0f; - const int32 k_maxIterations = 20; // TODO_ERIN b2Settings - int32 iter = 0; - - // Prepare input for distance query. - b2SimplexCache cache; - cache.count = 0; - b2DistanceInput distanceInput; - distanceInput.proxyA = input->proxyA; - distanceInput.proxyB = input->proxyB; - distanceInput.useRadii = false; - - // The outer loop progressively attempts to compute new separating axes. - // This loop terminates when an axis is repeated (no progress is made). - for(;;) - { - b2Transform xfA, xfB; - sweepA.GetTransform(&xfA, t1); - sweepB.GetTransform(&xfB, t1); - - // Get the distance between shapes. We can also use the results - // to get a separating axis. - distanceInput.transformA = xfA; - distanceInput.transformB = xfB; - b2DistanceOutput distanceOutput; - b2Distance(&distanceOutput, &cache, &distanceInput); - - // If the shapes are overlapped, we give up on continuous collision. - if (distanceOutput.distance <= 0.0f) - { - // Failure! - output->state = b2TOIOutput::e_overlapped; - output->t = 0.0f; - break; - } - - if (distanceOutput.distance < target + tolerance) - { - // Victory! - output->state = b2TOIOutput::e_touching; - output->t = t1; - break; - } - - // Initialize the separating axis. - b2SeparationFunction fcn; - fcn.Initialize(&cache, proxyA, sweepA, proxyB, sweepB, t1); -#if 0 - // Dump the curve seen by the root finder - { - const int32 N = 100; - float32 dx = 1.0f / N; - float32 xs[N+1]; - float32 fs[N+1]; - - float32 x = 0.0f; - - for (int32 i = 0; i <= N; ++i) - { - sweepA.GetTransform(&xfA, x); - sweepB.GetTransform(&xfB, x); - float32 f = fcn.Evaluate(xfA, xfB) - target; - - printf("%g %g\n", x, f); - - xs[i] = x; - fs[i] = f; - - x += dx; - } - } -#endif - - // Compute the TOI on the separating axis. We do this by successively - // resolving the deepest point. This loop is bounded by the number of vertices. - bool done = false; - float32 t2 = tMax; - int32 pushBackIter = 0; - for (;;) - { - // Find the deepest point at t2. Store the witness point indices. - int32 indexA, indexB; - float32 s2 = fcn.FindMinSeparation(&indexA, &indexB, t2); - - // Is the final configuration separated? - if (s2 > target + tolerance) - { - // Victory! - output->state = b2TOIOutput::e_separated; - output->t = tMax; - done = true; - break; - } - - // Has the separation reached tolerance? - if (s2 > target - tolerance) - { - // Advance the sweeps - t1 = t2; - break; - } - - // Compute the initial separation of the witness points. - float32 s1 = fcn.Evaluate(indexA, indexB, t1); - - // Check for initial overlap. This might happen if the root finder - // runs out of iterations. - if (s1 < target - tolerance) - { - output->state = b2TOIOutput::e_failed; - output->t = t1; - done = true; - break; - } - - // Check for touching - if (s1 <= target + tolerance) - { - // Victory! t1 should hold the TOI (could be 0.0). - output->state = b2TOIOutput::e_touching; - output->t = t1; - done = true; - break; - } - - // Compute 1D root of: f(x) - target = 0 - int32 rootIterCount = 0; - float32 a1 = t1, a2 = t2; - for (;;) - { - // Use a mix of the secant rule and bisection. - float32 t; - if (rootIterCount & 1) - { - // Secant rule to improve convergence. - t = a1 + (target - s1) * (a2 - a1) / (s2 - s1); - } - else - { - // Bisection to guarantee progress. - t = 0.5f * (a1 + a2); - } - - ++rootIterCount; - ++b2_toiRootIters; - - float32 s = fcn.Evaluate(indexA, indexB, t); - - if (b2Abs(s - target) < tolerance) - { - // t2 holds a tentative value for t1 - t2 = t; - break; - } - - // Ensure we continue to bracket the root. - if (s > target) - { - a1 = t; - s1 = s; - } - else - { - a2 = t; - s2 = s; - } - - if (rootIterCount == 50) - { - break; - } - } - - b2_toiMaxRootIters = b2Max(b2_toiMaxRootIters, rootIterCount); - - ++pushBackIter; - - if (pushBackIter == b2_maxPolygonVertices) - { - break; - } - } - - ++iter; - ++b2_toiIters; - - if (done) - { - break; - } - - if (iter == k_maxIterations) - { - // Root finder got stuck. Semi-victory. - output->state = b2TOIOutput::e_failed; - output->t = t1; - break; - } - } - - b2_toiMaxIters = b2Max(b2_toiMaxIters, iter); - - float32 time = timer.GetMilliseconds(); - b2_toiMaxTime = b2Max(b2_toiMaxTime, time); - b2_toiTime += time; -} +// MIT License + +// Copyright (c) 2019 Erin Catto + +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: + +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. + +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. + +#include "box2d/b2_collision.h" +#include "box2d/b2_distance.h" +#include "box2d/b2_circle_shape.h" +#include "box2d/b2_polygon_shape.h" +#include "box2d/b2_time_of_impact.h" +#include "box2d/b2_timer.h" + +#include + +B2_API float b2_toiTime, b2_toiMaxTime; +B2_API int32 b2_toiCalls, b2_toiIters, b2_toiMaxIters; +B2_API int32 b2_toiRootIters, b2_toiMaxRootIters; + +// +struct b2SeparationFunction +{ + enum Type + { + e_points, + e_faceA, + e_faceB + }; + + // TODO_ERIN might not need to return the separation + + float Initialize(const b2SimplexCache* cache, + const b2DistanceProxy* proxyA, const b2Sweep& sweepA, + const b2DistanceProxy* proxyB, const b2Sweep& sweepB, + float t1) + { + m_proxyA = proxyA; + m_proxyB = proxyB; + int32 count = cache->count; + b2Assert(0 < count && count < 3); + + m_sweepA = sweepA; + m_sweepB = sweepB; + + b2Transform xfA, xfB; + m_sweepA.GetTransform(&xfA, t1); + m_sweepB.GetTransform(&xfB, t1); + + if (count == 1) + { + m_type = e_points; + b2Vec2 localPointA = m_proxyA->GetVertex(cache->indexA[0]); + b2Vec2 localPointB = m_proxyB->GetVertex(cache->indexB[0]); + b2Vec2 pointA = b2Mul(xfA, localPointA); + b2Vec2 pointB = b2Mul(xfB, localPointB); + m_axis = pointB - pointA; + float s = m_axis.Normalize(); + return s; + } + else if (cache->indexA[0] == cache->indexA[1]) + { + // Two points on B and one on A. + m_type = e_faceB; + b2Vec2 localPointB1 = proxyB->GetVertex(cache->indexB[0]); + b2Vec2 localPointB2 = proxyB->GetVertex(cache->indexB[1]); + + m_axis = b2Cross(localPointB2 - localPointB1, 1.0f); + m_axis.Normalize(); + b2Vec2 normal = b2Mul(xfB.q, m_axis); + + m_localPoint = 0.5f * (localPointB1 + localPointB2); + b2Vec2 pointB = b2Mul(xfB, m_localPoint); + + b2Vec2 localPointA = proxyA->GetVertex(cache->indexA[0]); + b2Vec2 pointA = b2Mul(xfA, localPointA); + + float s = b2Dot(pointA - pointB, normal); + if (s < 0.0f) + { + m_axis = -m_axis; + s = -s; + } + return s; + } + else + { + // Two points on A and one or two points on B. + m_type = e_faceA; + b2Vec2 localPointA1 = m_proxyA->GetVertex(cache->indexA[0]); + b2Vec2 localPointA2 = m_proxyA->GetVertex(cache->indexA[1]); + + m_axis = b2Cross(localPointA2 - localPointA1, 1.0f); + m_axis.Normalize(); + b2Vec2 normal = b2Mul(xfA.q, m_axis); + + m_localPoint = 0.5f * (localPointA1 + localPointA2); + b2Vec2 pointA = b2Mul(xfA, m_localPoint); + + b2Vec2 localPointB = m_proxyB->GetVertex(cache->indexB[0]); + b2Vec2 pointB = b2Mul(xfB, localPointB); + + float s = b2Dot(pointB - pointA, normal); + if (s < 0.0f) + { + m_axis = -m_axis; + s = -s; + } + return s; + } + } + + // + float FindMinSeparation(int32* indexA, int32* indexB, float t) const + { + b2Transform xfA, xfB; + m_sweepA.GetTransform(&xfA, t); + m_sweepB.GetTransform(&xfB, t); + + switch (m_type) + { + case e_points: + { + b2Vec2 axisA = b2MulT(xfA.q, m_axis); + b2Vec2 axisB = b2MulT(xfB.q, -m_axis); + + *indexA = m_proxyA->GetSupport(axisA); + *indexB = m_proxyB->GetSupport(axisB); + + b2Vec2 localPointA = m_proxyA->GetVertex(*indexA); + b2Vec2 localPointB = m_proxyB->GetVertex(*indexB); + + b2Vec2 pointA = b2Mul(xfA, localPointA); + b2Vec2 pointB = b2Mul(xfB, localPointB); + + float separation = b2Dot(pointB - pointA, m_axis); + return separation; + } + + case e_faceA: + { + b2Vec2 normal = b2Mul(xfA.q, m_axis); + b2Vec2 pointA = b2Mul(xfA, m_localPoint); + + b2Vec2 axisB = b2MulT(xfB.q, -normal); + + *indexA = -1; + *indexB = m_proxyB->GetSupport(axisB); + + b2Vec2 localPointB = m_proxyB->GetVertex(*indexB); + b2Vec2 pointB = b2Mul(xfB, localPointB); + + float separation = b2Dot(pointB - pointA, normal); + return separation; + } + + case e_faceB: + { + b2Vec2 normal = b2Mul(xfB.q, m_axis); + b2Vec2 pointB = b2Mul(xfB, m_localPoint); + + b2Vec2 axisA = b2MulT(xfA.q, -normal); + + *indexB = -1; + *indexA = m_proxyA->GetSupport(axisA); + + b2Vec2 localPointA = m_proxyA->GetVertex(*indexA); + b2Vec2 pointA = b2Mul(xfA, localPointA); + + float separation = b2Dot(pointA - pointB, normal); + return separation; + } + + default: + b2Assert(false); + *indexA = -1; + *indexB = -1; + return 0.0f; + } + } + + // + float Evaluate(int32 indexA, int32 indexB, float t) const + { + b2Transform xfA, xfB; + m_sweepA.GetTransform(&xfA, t); + m_sweepB.GetTransform(&xfB, t); + + switch (m_type) + { + case e_points: + { + b2Vec2 localPointA = m_proxyA->GetVertex(indexA); + b2Vec2 localPointB = m_proxyB->GetVertex(indexB); + + b2Vec2 pointA = b2Mul(xfA, localPointA); + b2Vec2 pointB = b2Mul(xfB, localPointB); + float separation = b2Dot(pointB - pointA, m_axis); + + return separation; + } + + case e_faceA: + { + b2Vec2 normal = b2Mul(xfA.q, m_axis); + b2Vec2 pointA = b2Mul(xfA, m_localPoint); + + b2Vec2 localPointB = m_proxyB->GetVertex(indexB); + b2Vec2 pointB = b2Mul(xfB, localPointB); + + float separation = b2Dot(pointB - pointA, normal); + return separation; + } + + case e_faceB: + { + b2Vec2 normal = b2Mul(xfB.q, m_axis); + b2Vec2 pointB = b2Mul(xfB, m_localPoint); + + b2Vec2 localPointA = m_proxyA->GetVertex(indexA); + b2Vec2 pointA = b2Mul(xfA, localPointA); + + float separation = b2Dot(pointA - pointB, normal); + return separation; + } + + default: + b2Assert(false); + return 0.0f; + } + } + + const b2DistanceProxy* m_proxyA; + const b2DistanceProxy* m_proxyB; + b2Sweep m_sweepA, m_sweepB; + Type m_type; + b2Vec2 m_localPoint; + b2Vec2 m_axis; +}; + +// CCD via the local separating axis method. This seeks progression +// by computing the largest time at which separation is maintained. +void b2TimeOfImpact(b2TOIOutput* output, const b2TOIInput* input) +{ + b2Timer timer; + + ++b2_toiCalls; + + output->state = b2TOIOutput::e_unknown; + output->t = input->tMax; + + const b2DistanceProxy* proxyA = &input->proxyA; + const b2DistanceProxy* proxyB = &input->proxyB; + + b2Sweep sweepA = input->sweepA; + b2Sweep sweepB = input->sweepB; + + // Large rotations can make the root finder fail, so we normalize the + // sweep angles. + sweepA.Normalize(); + sweepB.Normalize(); + + float tMax = input->tMax; + + float totalRadius = proxyA->m_radius + proxyB->m_radius; + float target = b2Max(b2_linearSlop, totalRadius - 3.0f * b2_linearSlop); + float tolerance = 0.25f * b2_linearSlop; + b2Assert(target > tolerance); + + float t1 = 0.0f; + const int32 k_maxIterations = 20; // TODO_ERIN b2Settings + int32 iter = 0; + + // Prepare input for distance query. + b2SimplexCache cache; + cache.count = 0; + b2DistanceInput distanceInput; + distanceInput.proxyA = input->proxyA; + distanceInput.proxyB = input->proxyB; + distanceInput.useRadii = false; + + // The outer loop progressively attempts to compute new separating axes. + // This loop terminates when an axis is repeated (no progress is made). + for(;;) + { + b2Transform xfA, xfB; + sweepA.GetTransform(&xfA, t1); + sweepB.GetTransform(&xfB, t1); + + // Get the distance between shapes. We can also use the results + // to get a separating axis. + distanceInput.transformA = xfA; + distanceInput.transformB = xfB; + b2DistanceOutput distanceOutput; + b2Distance(&distanceOutput, &cache, &distanceInput); + + // If the shapes are overlapped, we give up on continuous collision. + if (distanceOutput.distance <= 0.0f) + { + // Failure! + output->state = b2TOIOutput::e_overlapped; + output->t = 0.0f; + break; + } + + if (distanceOutput.distance < target + tolerance) + { + // Victory! + output->state = b2TOIOutput::e_touching; + output->t = t1; + break; + } + + // Initialize the separating axis. + b2SeparationFunction fcn; + fcn.Initialize(&cache, proxyA, sweepA, proxyB, sweepB, t1); +#if 0 + // Dump the curve seen by the root finder + { + const int32 N = 100; + float dx = 1.0f / N; + float xs[N+1]; + float fs[N+1]; + + float x = 0.0f; + + for (int32 i = 0; i <= N; ++i) + { + sweepA.GetTransform(&xfA, x); + sweepB.GetTransform(&xfB, x); + float f = fcn.Evaluate(xfA, xfB) - target; + + printf("%g %g\n", x, f); + + xs[i] = x; + fs[i] = f; + + x += dx; + } + } +#endif + + // Compute the TOI on the separating axis. We do this by successively + // resolving the deepest point. This loop is bounded by the number of vertices. + bool done = false; + float t2 = tMax; + int32 pushBackIter = 0; + for (;;) + { + // Find the deepest point at t2. Store the witness point indices. + int32 indexA, indexB; + float s2 = fcn.FindMinSeparation(&indexA, &indexB, t2); + + // Is the final configuration separated? + if (s2 > target + tolerance) + { + // Victory! + output->state = b2TOIOutput::e_separated; + output->t = tMax; + done = true; + break; + } + + // Has the separation reached tolerance? + if (s2 > target - tolerance) + { + // Advance the sweeps + t1 = t2; + break; + } + + // Compute the initial separation of the witness points. + float s1 = fcn.Evaluate(indexA, indexB, t1); + + // Check for initial overlap. This might happen if the root finder + // runs out of iterations. + if (s1 < target - tolerance) + { + output->state = b2TOIOutput::e_failed; + output->t = t1; + done = true; + break; + } + + // Check for touching + if (s1 <= target + tolerance) + { + // Victory! t1 should hold the TOI (could be 0.0). + output->state = b2TOIOutput::e_touching; + output->t = t1; + done = true; + break; + } + + // Compute 1D root of: f(x) - target = 0 + int32 rootIterCount = 0; + float a1 = t1, a2 = t2; + for (;;) + { + // Use a mix of the secant rule and bisection. + float t; + if (rootIterCount & 1) + { + // Secant rule to improve convergence. + t = a1 + (target - s1) * (a2 - a1) / (s2 - s1); + } + else + { + // Bisection to guarantee progress. + t = 0.5f * (a1 + a2); + } + + ++rootIterCount; + ++b2_toiRootIters; + + float s = fcn.Evaluate(indexA, indexB, t); + + if (b2Abs(s - target) < tolerance) + { + // t2 holds a tentative value for t1 + t2 = t; + break; + } + + // Ensure we continue to bracket the root. + if (s > target) + { + a1 = t; + s1 = s; + } + else + { + a2 = t; + s2 = s; + } + + if (rootIterCount == 50) + { + break; + } + } + + b2_toiMaxRootIters = b2Max(b2_toiMaxRootIters, rootIterCount); + + ++pushBackIter; + + if (pushBackIter == b2_maxPolygonVertices) + { + break; + } + } + + ++iter; + ++b2_toiIters; + + if (done) + { + break; + } + + if (iter == k_maxIterations) + { + // Root finder got stuck. Semi-victory. + output->state = b2TOIOutput::e_failed; + output->t = t1; + break; + } + } + + b2_toiMaxIters = b2Max(b2_toiMaxIters, iter); + + float time = timer.GetMilliseconds(); + b2_toiMaxTime = b2Max(b2_toiMaxTime, time); + b2_toiTime += time; +} diff --git a/libs/Box2D/Common/b2BlockAllocator.cpp b/libs/box2d/src/common/b2_block_allocator.cpp similarity index 59% rename from libs/Box2D/Common/b2BlockAllocator.cpp rename to libs/box2d/src/common/b2_block_allocator.cpp index c9fcbfbb..595f2ad3 100644 --- a/libs/Box2D/Common/b2BlockAllocator.cpp +++ b/libs/box2d/src/common/b2_block_allocator.cpp @@ -1,215 +1,230 @@ -/* -* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ - -#include -#include -#include -#include - -int32 b2BlockAllocator::s_blockSizes[b2_blockSizes] = -{ - 16, // 0 - 32, // 1 - 64, // 2 - 96, // 3 - 128, // 4 - 160, // 5 - 192, // 6 - 224, // 7 - 256, // 8 - 320, // 9 - 384, // 10 - 448, // 11 - 512, // 12 - 640, // 13 -}; -uint8 b2BlockAllocator::s_blockSizeLookup[b2_maxBlockSize + 1]; -bool b2BlockAllocator::s_blockSizeLookupInitialized; - -struct b2Chunk -{ - int32 blockSize; - b2Block* blocks; -}; - -struct b2Block -{ - b2Block* next; -}; - -b2BlockAllocator::b2BlockAllocator() -{ - b2Assert(b2_blockSizes < UCHAR_MAX); - - m_chunkSpace = b2_chunkArrayIncrement; - m_chunkCount = 0; - m_chunks = (b2Chunk*)b2Alloc(m_chunkSpace * sizeof(b2Chunk)); - - memset(m_chunks, 0, m_chunkSpace * sizeof(b2Chunk)); - memset(m_freeLists, 0, sizeof(m_freeLists)); - - if (s_blockSizeLookupInitialized == false) - { - int32 j = 0; - for (int32 i = 1; i <= b2_maxBlockSize; ++i) - { - b2Assert(j < b2_blockSizes); - if (i <= s_blockSizes[j]) - { - s_blockSizeLookup[i] = (uint8)j; - } - else - { - ++j; - s_blockSizeLookup[i] = (uint8)j; - } - } - - s_blockSizeLookupInitialized = true; - } -} - -b2BlockAllocator::~b2BlockAllocator() -{ - for (int32 i = 0; i < m_chunkCount; ++i) - { - b2Free(m_chunks[i].blocks); - } - - b2Free(m_chunks); -} - -void* b2BlockAllocator::Allocate(int32 size) -{ - if (size == 0) - return NULL; - - b2Assert(0 < size); - - if (size > b2_maxBlockSize) - { - return b2Alloc(size); - } - - int32 index = s_blockSizeLookup[size]; - b2Assert(0 <= index && index < b2_blockSizes); - - if (m_freeLists[index]) - { - b2Block* block = m_freeLists[index]; - m_freeLists[index] = block->next; - return block; - } - else - { - if (m_chunkCount == m_chunkSpace) - { - b2Chunk* oldChunks = m_chunks; - m_chunkSpace += b2_chunkArrayIncrement; - m_chunks = (b2Chunk*)b2Alloc(m_chunkSpace * sizeof(b2Chunk)); - memcpy(m_chunks, oldChunks, m_chunkCount * sizeof(b2Chunk)); - memset(m_chunks + m_chunkCount, 0, b2_chunkArrayIncrement * sizeof(b2Chunk)); - b2Free(oldChunks); - } - - b2Chunk* chunk = m_chunks + m_chunkCount; - chunk->blocks = (b2Block*)b2Alloc(b2_chunkSize); -#if defined(_DEBUG) - memset(chunk->blocks, 0xcd, b2_chunkSize); -#endif - int32 blockSize = s_blockSizes[index]; - chunk->blockSize = blockSize; - int32 blockCount = b2_chunkSize / blockSize; - b2Assert(blockCount * blockSize <= b2_chunkSize); - for (int32 i = 0; i < blockCount - 1; ++i) - { - b2Block* block = (b2Block*)((int8*)chunk->blocks + blockSize * i); - b2Block* next = (b2Block*)((int8*)chunk->blocks + blockSize * (i + 1)); - block->next = next; - } - b2Block* last = (b2Block*)((int8*)chunk->blocks + blockSize * (blockCount - 1)); - last->next = NULL; - - m_freeLists[index] = chunk->blocks->next; - ++m_chunkCount; - - return chunk->blocks; - } -} - -void b2BlockAllocator::Free(void* p, int32 size) -{ - if (size == 0) - { - return; - } - - b2Assert(0 < size); - - if (size > b2_maxBlockSize) - { - b2Free(p); - return; - } - - int32 index = s_blockSizeLookup[size]; - b2Assert(0 <= index && index < b2_blockSizes); - -#ifdef _DEBUG - // Verify the memory address and size is valid. - int32 blockSize = s_blockSizes[index]; - bool found = false; - for (int32 i = 0; i < m_chunkCount; ++i) - { - b2Chunk* chunk = m_chunks + i; - if (chunk->blockSize != blockSize) - { - b2Assert( (int8*)p + blockSize <= (int8*)chunk->blocks || - (int8*)chunk->blocks + b2_chunkSize <= (int8*)p); - } - else - { - if ((int8*)chunk->blocks <= (int8*)p && (int8*)p + blockSize <= (int8*)chunk->blocks + b2_chunkSize) - { - found = true; - } - } - } - - b2Assert(found); - - memset(p, 0xfd, blockSize); -#endif - - b2Block* block = (b2Block*)p; - block->next = m_freeLists[index]; - m_freeLists[index] = block; -} - -void b2BlockAllocator::Clear() -{ - for (int32 i = 0; i < m_chunkCount; ++i) - { - b2Free(m_chunks[i].blocks); - } - - m_chunkCount = 0; - memset(m_chunks, 0, m_chunkSpace * sizeof(b2Chunk)); - - memset(m_freeLists, 0, sizeof(m_freeLists)); -} +// MIT License + +// Copyright (c) 2019 Erin Catto + +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: + +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. + +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. + +#include "box2d/b2_block_allocator.h" +#include +#include +#include + +static const int32 b2_chunkSize = 16 * 1024; +static const int32 b2_maxBlockSize = 640; +static const int32 b2_chunkArrayIncrement = 128; + +// These are the supported object sizes. Actual allocations are rounded up the next size. +static const int32 b2_blockSizes[b2_blockSizeCount] = +{ + 16, // 0 + 32, // 1 + 64, // 2 + 96, // 3 + 128, // 4 + 160, // 5 + 192, // 6 + 224, // 7 + 256, // 8 + 320, // 9 + 384, // 10 + 448, // 11 + 512, // 12 + 640, // 13 +}; + +// This maps an arbitrary allocation size to a suitable slot in b2_blockSizes. +struct b2SizeMap +{ + b2SizeMap() + { + int32 j = 0; + values[0] = 0; + for (int32 i = 1; i <= b2_maxBlockSize; ++i) + { + b2Assert(j < b2_blockSizeCount); + if (i <= b2_blockSizes[j]) + { + values[i] = (uint8)j; + } + else + { + ++j; + values[i] = (uint8)j; + } + } + } + + uint8 values[b2_maxBlockSize + 1]; +}; + +static const b2SizeMap b2_sizeMap; + +struct b2Chunk +{ + int32 blockSize; + b2Block* blocks; +}; + +struct b2Block +{ + b2Block* next; +}; + +b2BlockAllocator::b2BlockAllocator() +{ + b2Assert(b2_blockSizeCount < UCHAR_MAX); + + m_chunkSpace = b2_chunkArrayIncrement; + m_chunkCount = 0; + m_chunks = (b2Chunk*)b2Alloc(m_chunkSpace * sizeof(b2Chunk)); + + memset(m_chunks, 0, m_chunkSpace * sizeof(b2Chunk)); + memset(m_freeLists, 0, sizeof(m_freeLists)); +} + +b2BlockAllocator::~b2BlockAllocator() +{ + for (int32 i = 0; i < m_chunkCount; ++i) + { + b2Free(m_chunks[i].blocks); + } + + b2Free(m_chunks); +} + +void* b2BlockAllocator::Allocate(int32 size) +{ + if (size == 0) + { + return nullptr; + } + + b2Assert(0 < size); + + if (size > b2_maxBlockSize) + { + return b2Alloc(size); + } + + int32 index = b2_sizeMap.values[size]; + b2Assert(0 <= index && index < b2_blockSizeCount); + + if (m_freeLists[index]) + { + b2Block* block = m_freeLists[index]; + m_freeLists[index] = block->next; + return block; + } + else + { + if (m_chunkCount == m_chunkSpace) + { + b2Chunk* oldChunks = m_chunks; + m_chunkSpace += b2_chunkArrayIncrement; + m_chunks = (b2Chunk*)b2Alloc(m_chunkSpace * sizeof(b2Chunk)); + memcpy(m_chunks, oldChunks, m_chunkCount * sizeof(b2Chunk)); + memset(m_chunks + m_chunkCount, 0, b2_chunkArrayIncrement * sizeof(b2Chunk)); + b2Free(oldChunks); + } + + b2Chunk* chunk = m_chunks + m_chunkCount; + chunk->blocks = (b2Block*)b2Alloc(b2_chunkSize); +#if defined(_DEBUG) + memset(chunk->blocks, 0xcd, b2_chunkSize); +#endif + int32 blockSize = b2_blockSizes[index]; + chunk->blockSize = blockSize; + int32 blockCount = b2_chunkSize / blockSize; + b2Assert(blockCount * blockSize <= b2_chunkSize); + for (int32 i = 0; i < blockCount - 1; ++i) + { + b2Block* block = (b2Block*)((int8*)chunk->blocks + blockSize * i); + b2Block* next = (b2Block*)((int8*)chunk->blocks + blockSize * (i + 1)); + block->next = next; + } + b2Block* last = (b2Block*)((int8*)chunk->blocks + blockSize * (blockCount - 1)); + last->next = nullptr; + + m_freeLists[index] = chunk->blocks->next; + ++m_chunkCount; + + return chunk->blocks; + } +} + +void b2BlockAllocator::Free(void* p, int32 size) +{ + if (size == 0) + { + return; + } + + b2Assert(0 < size); + + if (size > b2_maxBlockSize) + { + b2Free(p); + return; + } + + int32 index = b2_sizeMap.values[size]; + b2Assert(0 <= index && index < b2_blockSizeCount); + +#if defined(_DEBUG) + // Verify the memory address and size is valid. + int32 blockSize = b2_blockSizes[index]; + bool found = false; + for (int32 i = 0; i < m_chunkCount; ++i) + { + b2Chunk* chunk = m_chunks + i; + if (chunk->blockSize != blockSize) + { + b2Assert( (int8*)p + blockSize <= (int8*)chunk->blocks || + (int8*)chunk->blocks + b2_chunkSize <= (int8*)p); + } + else + { + if ((int8*)chunk->blocks <= (int8*)p && (int8*)p + blockSize <= (int8*)chunk->blocks + b2_chunkSize) + { + found = true; + } + } + } + + b2Assert(found); + + memset(p, 0xfd, blockSize); +#endif + + b2Block* block = (b2Block*)p; + block->next = m_freeLists[index]; + m_freeLists[index] = block; +} + +void b2BlockAllocator::Clear() +{ + for (int32 i = 0; i < m_chunkCount; ++i) + { + b2Free(m_chunks[i].blocks); + } + + m_chunkCount = 0; + memset(m_chunks, 0, m_chunkSpace * sizeof(b2Chunk)); + memset(m_freeLists, 0, sizeof(m_freeLists)); +} diff --git a/libs/box2d/src/common/b2_draw.cpp b/libs/box2d/src/common/b2_draw.cpp new file mode 100644 index 00000000..1ec11e54 --- /dev/null +++ b/libs/box2d/src/common/b2_draw.cpp @@ -0,0 +1,47 @@ +// MIT License + +// Copyright (c) 2019 Erin Catto + +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: + +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. + +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +#include "box2d/b2_draw.h" + +b2Draw::b2Draw() +{ + m_drawFlags = 0; +} + +void b2Draw::SetFlags(uint32 flags) +{ + m_drawFlags = flags; +} + +uint32 b2Draw::GetFlags() const +{ + return m_drawFlags; +} + +void b2Draw::AppendFlags(uint32 flags) +{ + m_drawFlags |= flags; +} + +void b2Draw::ClearFlags(uint32 flags) +{ + m_drawFlags &= ~flags; +} diff --git a/libs/box2d/src/common/b2_math.cpp b/libs/box2d/src/common/b2_math.cpp new file mode 100644 index 00000000..a14460cd --- /dev/null +++ b/libs/box2d/src/common/b2_math.cpp @@ -0,0 +1,98 @@ +// MIT License + +// Copyright (c) 2019 Erin Catto + +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: + +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. + +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. + +#include "box2d/b2_math.h" + +const b2Vec2 b2Vec2_zero(0.0f, 0.0f); + +/// Solve A * x = b, where b is a column vector. This is more efficient +/// than computing the inverse in one-shot cases. +b2Vec3 b2Mat33::Solve33(const b2Vec3& b) const +{ + float det = b2Dot(ex, b2Cross(ey, ez)); + if (det != 0.0f) + { + det = 1.0f / det; + } + b2Vec3 x; + x.x = det * b2Dot(b, b2Cross(ey, ez)); + x.y = det * b2Dot(ex, b2Cross(b, ez)); + x.z = det * b2Dot(ex, b2Cross(ey, b)); + return x; +} + +/// Solve A * x = b, where b is a column vector. This is more efficient +/// than computing the inverse in one-shot cases. +b2Vec2 b2Mat33::Solve22(const b2Vec2& b) const +{ + float a11 = ex.x, a12 = ey.x, a21 = ex.y, a22 = ey.y; + float det = a11 * a22 - a12 * a21; + if (det != 0.0f) + { + det = 1.0f / det; + } + b2Vec2 x; + x.x = det * (a22 * b.x - a12 * b.y); + x.y = det * (a11 * b.y - a21 * b.x); + return x; +} + +/// +void b2Mat33::GetInverse22(b2Mat33* M) const +{ + float a = ex.x, b = ey.x, c = ex.y, d = ey.y; + float det = a * d - b * c; + if (det != 0.0f) + { + det = 1.0f / det; + } + + M->ex.x = det * d; M->ey.x = -det * b; M->ex.z = 0.0f; + M->ex.y = -det * c; M->ey.y = det * a; M->ey.z = 0.0f; + M->ez.x = 0.0f; M->ez.y = 0.0f; M->ez.z = 0.0f; +} + +/// Returns the zero matrix if singular. +void b2Mat33::GetSymInverse33(b2Mat33* M) const +{ + float det = b2Dot(ex, b2Cross(ey, ez)); + if (det != 0.0f) + { + det = 1.0f / det; + } + + float a11 = ex.x, a12 = ey.x, a13 = ez.x; + float a22 = ey.y, a23 = ez.y; + float a33 = ez.z; + + M->ex.x = det * (a22 * a33 - a23 * a23); + M->ex.y = det * (a13 * a23 - a12 * a33); + M->ex.z = det * (a12 * a23 - a13 * a22); + + M->ey.x = M->ex.y; + M->ey.y = det * (a11 * a33 - a13 * a13); + M->ey.z = det * (a13 * a12 - a11 * a23); + + M->ez.x = M->ex.z; + M->ez.y = M->ey.z; + M->ez.z = det * (a11 * a22 - a12 * a12); +} diff --git a/libs/box2d/src/common/b2_settings.cpp b/libs/box2d/src/common/b2_settings.cpp new file mode 100644 index 00000000..dde28bb3 --- /dev/null +++ b/libs/box2d/src/common/b2_settings.cpp @@ -0,0 +1,74 @@ +// MIT License + +// Copyright (c) 2019 Erin Catto + +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: + +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. + +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. + +#define _CRT_SECURE_NO_WARNINGS + +#include "box2d/b2_settings.h" +#include +#include +#include + +b2Version b2_version = {2, 4, 1}; + +// Memory allocators. Modify these to use your own allocator. +void* b2Alloc_Default(int32 size) +{ + return malloc(size); +} + +void b2Free_Default(void* mem) +{ + free(mem); +} + +// You can modify this to use your logging facility. +void b2Log_Default(const char* string, va_list args) +{ + vprintf(string, args); +} + +FILE* b2_dumpFile = nullptr; + +void b2OpenDump(const char* fileName) +{ + b2Assert(b2_dumpFile == nullptr); + b2_dumpFile = fopen(fileName, "w"); +} + +void b2Dump(const char* string, ...) +{ + if (b2_dumpFile == nullptr) + { + return; + } + + va_list args; + va_start(args, string); + vfprintf(b2_dumpFile, string, args); + va_end(args); +} + +void b2CloseDump() +{ + fclose(b2_dumpFile); + b2_dumpFile = nullptr; +} diff --git a/libs/box2d/src/common/b2_stack_allocator.cpp b/libs/box2d/src/common/b2_stack_allocator.cpp new file mode 100644 index 00000000..602db1a0 --- /dev/null +++ b/libs/box2d/src/common/b2_stack_allocator.cpp @@ -0,0 +1,87 @@ +// MIT License + +// Copyright (c) 2019 Erin Catto + +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: + +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. + +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. + +#include "box2d/b2_stack_allocator.h" +#include "box2d/b2_math.h" + +b2StackAllocator::b2StackAllocator() +{ + m_index = 0; + m_allocation = 0; + m_maxAllocation = 0; + m_entryCount = 0; +} + +b2StackAllocator::~b2StackAllocator() +{ + b2Assert(m_index == 0); + b2Assert(m_entryCount == 0); +} + +void* b2StackAllocator::Allocate(int32 size) +{ + b2Assert(m_entryCount < b2_maxStackEntries); + + b2StackEntry* entry = m_entries + m_entryCount; + entry->size = size; + if (m_index + size > b2_stackSize) + { + entry->data = (char*)b2Alloc(size); + entry->usedMalloc = true; + } + else + { + entry->data = m_data + m_index; + entry->usedMalloc = false; + m_index += size; + } + + m_allocation += size; + m_maxAllocation = b2Max(m_maxAllocation, m_allocation); + ++m_entryCount; + + return entry->data; +} + +void b2StackAllocator::Free(void* p) +{ + b2Assert(m_entryCount > 0); + b2StackEntry* entry = m_entries + m_entryCount - 1; + b2Assert(p == entry->data); + if (entry->usedMalloc) + { + b2Free(p); + } + else + { + m_index -= entry->size; + } + m_allocation -= entry->size; + --m_entryCount; + + p = nullptr; +} + +int32 b2StackAllocator::GetMaxAllocation() const +{ + return m_maxAllocation; +} diff --git a/libs/box2d/src/common/b2_timer.cpp b/libs/box2d/src/common/b2_timer.cpp new file mode 100644 index 00000000..dd7cde7f --- /dev/null +++ b/libs/box2d/src/common/b2_timer.cpp @@ -0,0 +1,125 @@ +// MIT License + +// Copyright (c) 2019 Erin Catto + +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: + +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. + +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. + +#include "box2d/b2_timer.h" + +#if defined(_WIN32) + +double b2Timer::s_invFrequency = 0.0; + +#ifndef WIN32_LEAN_AND_MEAN +#define WIN32_LEAN_AND_MEAN +#endif + +#include + +b2Timer::b2Timer() +{ + LARGE_INTEGER largeInteger; + + if (s_invFrequency == 0.0) + { + QueryPerformanceFrequency(&largeInteger); + s_invFrequency = double(largeInteger.QuadPart); + if (s_invFrequency > 0.0) + { + s_invFrequency = 1000.0 / s_invFrequency; + } + } + + QueryPerformanceCounter(&largeInteger); + m_start = double(largeInteger.QuadPart); +} + +void b2Timer::Reset() +{ + LARGE_INTEGER largeInteger; + QueryPerformanceCounter(&largeInteger); + m_start = double(largeInteger.QuadPart); +} + +float b2Timer::GetMilliseconds() const +{ + LARGE_INTEGER largeInteger; + QueryPerformanceCounter(&largeInteger); + double count = double(largeInteger.QuadPart); + float ms = float(s_invFrequency * (count - m_start)); + return ms; +} + +#elif defined(__linux__) || defined (__APPLE__) + +#include + +b2Timer::b2Timer() +{ + Reset(); +} + +void b2Timer::Reset() +{ + timeval t; + gettimeofday(&t, 0); + m_start_sec = t.tv_sec; + m_start_usec = t.tv_usec; +} + +float b2Timer::GetMilliseconds() const +{ + timeval t; + gettimeofday(&t, 0); + time_t start_sec = m_start_sec; + suseconds_t start_usec = m_start_usec; + + // http://www.gnu.org/software/libc/manual/html_node/Elapsed-Time.html + if (t.tv_usec < start_usec) + { + int nsec = (start_usec - t.tv_usec) / 1000000 + 1; + start_usec -= 1000000 * nsec; + start_sec += nsec; + } + + if (t.tv_usec - start_usec > 1000000) + { + int nsec = (t.tv_usec - start_usec) / 1000000; + start_usec += 1000000 * nsec; + start_sec -= nsec; + } + return 1000.0f * (t.tv_sec - start_sec) + 0.001f * (t.tv_usec - start_usec); +} + +#else + +b2Timer::b2Timer() +{ +} + +void b2Timer::Reset() +{ +} + +float b2Timer::GetMilliseconds() const +{ + return 0.0f; +} + +#endif diff --git a/libs/Box2D/Dynamics/b2Body.cpp b/libs/box2d/src/dynamics/b2_body.cpp similarity index 69% rename from libs/Box2D/Dynamics/b2Body.cpp rename to libs/box2d/src/dynamics/b2_body.cpp index d99fc49f..00f36856 100644 --- a/libs/Box2D/Dynamics/b2Body.cpp +++ b/libs/box2d/src/dynamics/b2_body.cpp @@ -1,549 +1,570 @@ -/* -* Copyright (c) 2006-2007 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ - -#include -#include -#include -#include -#include - -b2Body::b2Body(const b2BodyDef* bd, b2World* world) -{ - b2Assert(bd->position.IsValid()); - b2Assert(bd->linearVelocity.IsValid()); - b2Assert(b2IsValid(bd->angle)); - b2Assert(b2IsValid(bd->angularVelocity)); - b2Assert(b2IsValid(bd->angularDamping) && bd->angularDamping >= 0.0f); - b2Assert(b2IsValid(bd->linearDamping) && bd->linearDamping >= 0.0f); - - m_flags = 0; - - if (bd->bullet) - { - m_flags |= e_bulletFlag; - } - if (bd->fixedRotation) - { - m_flags |= e_fixedRotationFlag; - } - if (bd->allowSleep) - { - m_flags |= e_autoSleepFlag; - } - if (bd->awake) - { - m_flags |= e_awakeFlag; - } - if (bd->active) - { - m_flags |= e_activeFlag; - } - - m_world = world; - - m_xf.p = bd->position; - m_xf.q.Set(bd->angle); - - m_sweep.localCenter.SetZero(); - m_sweep.c0 = m_xf.p; - m_sweep.c = m_xf.p; - m_sweep.a0 = bd->angle; - m_sweep.a = bd->angle; - m_sweep.alpha0 = 0.0f; - - m_jointList = NULL; - m_contactList = NULL; - m_prev = NULL; - m_next = NULL; - - m_linearVelocity = bd->linearVelocity; - m_angularVelocity = bd->angularVelocity; - - m_linearDamping = bd->linearDamping; - m_angularDamping = bd->angularDamping; - m_gravityScale = bd->gravityScale; - - m_force.SetZero(); - m_torque = 0.0f; - - m_sleepTime = 0.0f; - - m_type = bd->type; - - if (m_type == b2_dynamicBody) - { - m_mass = 1.0f; - m_invMass = 1.0f; - } - else - { - m_mass = 0.0f; - m_invMass = 0.0f; - } - - m_I = 0.0f; - m_invI = 0.0f; - - m_userData = bd->userData; - - m_fixtureList = NULL; - m_fixtureCount = 0; -} - -b2Body::~b2Body() -{ - // shapes and joints are destroyed in b2World::Destroy -} - -void b2Body::SetType(b2BodyType type) -{ - b2Assert(m_world->IsLocked() == false); - if (m_world->IsLocked() == true) - { - return; - } - - if (m_type == type) - { - return; - } - - m_type = type; - - ResetMassData(); - - if (m_type == b2_staticBody) - { - m_linearVelocity.SetZero(); - m_angularVelocity = 0.0f; - m_sweep.a0 = m_sweep.a; - m_sweep.c0 = m_sweep.c; - SynchronizeFixtures(); - } - - SetAwake(true); - - m_force.SetZero(); - m_torque = 0.0f; - - // Delete the attached contacts. - b2ContactEdge* ce = m_contactList; - while (ce) - { - b2ContactEdge* ce0 = ce; - ce = ce->next; - m_world->m_contactManager.Destroy(ce0->contact); - } - m_contactList = NULL; - - // Touch the proxies so that new contacts will be created (when appropriate) - b2BroadPhase* broadPhase = &m_world->m_contactManager.m_broadPhase; - for (b2Fixture* f = m_fixtureList; f; f = f->m_next) - { - int32 proxyCount = f->m_proxyCount; - for (int32 i = 0; i < proxyCount; ++i) - { - broadPhase->TouchProxy(f->m_proxies[i].proxyId); - } - } -} - -b2Fixture* b2Body::CreateFixture(const b2FixtureDef* def) -{ - b2Assert(m_world->IsLocked() == false); - if (m_world->IsLocked() == true) - { - return NULL; - } - - b2BlockAllocator* allocator = &m_world->m_blockAllocator; - - void* memory = allocator->Allocate(sizeof(b2Fixture)); - b2Fixture* fixture = new (memory) b2Fixture; - fixture->Create(allocator, this, def); - - if (m_flags & e_activeFlag) - { - b2BroadPhase* broadPhase = &m_world->m_contactManager.m_broadPhase; - fixture->CreateProxies(broadPhase, m_xf); - } - - fixture->m_next = m_fixtureList; - m_fixtureList = fixture; - ++m_fixtureCount; - - fixture->m_body = this; - - // Adjust mass properties if needed. - if (fixture->m_density > 0.0f) - { - ResetMassData(); - } - - // Let the world know we have a new fixture. This will cause new contacts - // to be created at the beginning of the next time step. - m_world->m_flags |= b2World::e_newFixture; - - return fixture; -} - -b2Fixture* b2Body::CreateFixture(const b2Shape* shape, float32 density) -{ - b2FixtureDef def; - def.shape = shape; - def.density = density; - - return CreateFixture(&def); -} - -void b2Body::DestroyFixture(b2Fixture* fixture) -{ - b2Assert(m_world->IsLocked() == false); - if (m_world->IsLocked() == true) - { - return; - } - - b2Assert(fixture->m_body == this); - - // Remove the fixture from this body's singly linked list. - b2Assert(m_fixtureCount > 0); - b2Fixture** node = &m_fixtureList; - bool found = false; - while (*node != NULL) - { - if (*node == fixture) - { - *node = fixture->m_next; - found = true; - break; - } - - node = &(*node)->m_next; - } - - // You tried to remove a shape that is not attached to this body. - b2Assert(found); - - // Destroy any contacts associated with the fixture. - b2ContactEdge* edge = m_contactList; - while (edge) - { - b2Contact* c = edge->contact; - edge = edge->next; - - b2Fixture* fixtureA = c->GetFixtureA(); - b2Fixture* fixtureB = c->GetFixtureB(); - - if (fixture == fixtureA || fixture == fixtureB) - { - // This destroys the contact and removes it from - // this body's contact list. - m_world->m_contactManager.Destroy(c); - } - } - - b2BlockAllocator* allocator = &m_world->m_blockAllocator; - - if (m_flags & e_activeFlag) - { - b2BroadPhase* broadPhase = &m_world->m_contactManager.m_broadPhase; - fixture->DestroyProxies(broadPhase); - } - - fixture->Destroy(allocator); - fixture->m_body = NULL; - fixture->m_next = NULL; - fixture->~b2Fixture(); - allocator->Free(fixture, sizeof(b2Fixture)); - - --m_fixtureCount; - - // Reset the mass data. - ResetMassData(); -} - -void b2Body::ResetMassData() -{ - // Compute mass data from shapes. Each shape has its own density. - m_mass = 0.0f; - m_invMass = 0.0f; - m_I = 0.0f; - m_invI = 0.0f; - m_sweep.localCenter.SetZero(); - - // Static and kinematic bodies have zero mass. - if (m_type == b2_staticBody || m_type == b2_kinematicBody) - { - m_sweep.c0 = m_xf.p; - m_sweep.c = m_xf.p; - m_sweep.a0 = m_sweep.a; - return; - } - - b2Assert(m_type == b2_dynamicBody); - - // Accumulate mass over all fixtures. - b2Vec2 localCenter = b2Vec2_zero; - for (b2Fixture* f = m_fixtureList; f; f = f->m_next) - { - if (f->m_density == 0.0f) - { - continue; - } - - b2MassData massData; - f->GetMassData(&massData); - m_mass += massData.mass; - localCenter += massData.mass * massData.center; - m_I += massData.I; - } - - // Compute center of mass. - if (m_mass > 0.0f) - { - m_invMass = 1.0f / m_mass; - localCenter *= m_invMass; - } - else - { - // Force all dynamic bodies to have a positive mass. - m_mass = 1.0f; - m_invMass = 1.0f; - } - - if (m_I > 0.0f && (m_flags & e_fixedRotationFlag) == 0) - { - // Center the inertia about the center of mass. - m_I -= m_mass * b2Dot(localCenter, localCenter); - b2Assert(m_I > 0.0f); - m_invI = 1.0f / m_I; - - } - else - { - m_I = 0.0f; - m_invI = 0.0f; - } - - // Move center of mass. - b2Vec2 oldCenter = m_sweep.c; - m_sweep.localCenter = localCenter; - m_sweep.c0 = m_sweep.c = b2Mul(m_xf, m_sweep.localCenter); - - // Update center of mass velocity. - m_linearVelocity += b2Cross(m_angularVelocity, m_sweep.c - oldCenter); -} - -void b2Body::SetMassData(const b2MassData* massData) -{ - b2Assert(m_world->IsLocked() == false); - if (m_world->IsLocked() == true) - { - return; - } - - if (m_type != b2_dynamicBody) - { - return; - } - - m_invMass = 0.0f; - m_I = 0.0f; - m_invI = 0.0f; - - m_mass = massData->mass; - if (m_mass <= 0.0f) - { - m_mass = 1.0f; - } - - m_invMass = 1.0f / m_mass; - - if (massData->I > 0.0f && (m_flags & b2Body::e_fixedRotationFlag) == 0) - { - m_I = massData->I - m_mass * b2Dot(massData->center, massData->center); - b2Assert(m_I > 0.0f); - m_invI = 1.0f / m_I; - } - - // Move center of mass. - b2Vec2 oldCenter = m_sweep.c; - m_sweep.localCenter = massData->center; - m_sweep.c0 = m_sweep.c = b2Mul(m_xf, m_sweep.localCenter); - - // Update center of mass velocity. - m_linearVelocity += b2Cross(m_angularVelocity, m_sweep.c - oldCenter); -} - -bool b2Body::ShouldCollide(const b2Body* other) const -{ - // At least one body should be dynamic. - if (m_type != b2_dynamicBody && other->m_type != b2_dynamicBody) - { - return false; - } - - // Does a joint prevent collision? - for (b2JointEdge* jn = m_jointList; jn; jn = jn->next) - { - if (jn->other == other) - { - if (jn->joint->m_collideConnected == false) - { - return false; - } - } - } - - return true; -} - -void b2Body::SetTransform(const b2Vec2& position, float32 angle) -{ - b2Assert(m_world->IsLocked() == false); - if (m_world->IsLocked() == true) - { - return; - } - - m_xf.q.Set(angle); - m_xf.p = position; - - m_sweep.c = b2Mul(m_xf, m_sweep.localCenter); - m_sweep.a = angle; - - m_sweep.c0 = m_sweep.c; - m_sweep.a0 = angle; - - b2BroadPhase* broadPhase = &m_world->m_contactManager.m_broadPhase; - for (b2Fixture* f = m_fixtureList; f; f = f->m_next) - { - f->Synchronize(broadPhase, m_xf, m_xf); - } -} - -void b2Body::SynchronizeFixtures() -{ - b2Transform xf1; - xf1.q.Set(m_sweep.a0); - xf1.p = m_sweep.c0 - b2Mul(xf1.q, m_sweep.localCenter); - - b2BroadPhase* broadPhase = &m_world->m_contactManager.m_broadPhase; - for (b2Fixture* f = m_fixtureList; f; f = f->m_next) - { - f->Synchronize(broadPhase, xf1, m_xf); - } -} - -void b2Body::SetActive(bool flag) -{ - b2Assert(m_world->IsLocked() == false); - - if (flag == IsActive()) - { - return; - } - - if (flag) - { - m_flags |= e_activeFlag; - - // Create all proxies. - b2BroadPhase* broadPhase = &m_world->m_contactManager.m_broadPhase; - for (b2Fixture* f = m_fixtureList; f; f = f->m_next) - { - f->CreateProxies(broadPhase, m_xf); - } - - // Contacts are created the next time step. - } - else - { - m_flags &= ~e_activeFlag; - - // Destroy all proxies. - b2BroadPhase* broadPhase = &m_world->m_contactManager.m_broadPhase; - for (b2Fixture* f = m_fixtureList; f; f = f->m_next) - { - f->DestroyProxies(broadPhase); - } - - // Destroy the attached contacts. - b2ContactEdge* ce = m_contactList; - while (ce) - { - b2ContactEdge* ce0 = ce; - ce = ce->next; - m_world->m_contactManager.Destroy(ce0->contact); - } - m_contactList = NULL; - } -} - -void b2Body::SetFixedRotation(bool flag) -{ - bool status = (m_flags & e_fixedRotationFlag) == e_fixedRotationFlag; - if (status == flag) - { - return; - } - - if (flag) - { - m_flags |= e_fixedRotationFlag; - } - else - { - m_flags &= ~e_fixedRotationFlag; - } - - m_angularVelocity = 0.0f; - - ResetMassData(); -} - -void b2Body::Dump() -{ - int32 bodyIndex = m_islandIndex; - - b2Log("{\n"); - b2Log(" b2BodyDef bd;\n"); - b2Log(" bd.type = b2BodyType(%d);\n", m_type); - b2Log(" bd.position.Set(%.15lef, %.15lef);\n", m_xf.p.x, m_xf.p.y); - b2Log(" bd.angle = %.15lef;\n", m_sweep.a); - b2Log(" bd.linearVelocity.Set(%.15lef, %.15lef);\n", m_linearVelocity.x, m_linearVelocity.y); - b2Log(" bd.angularVelocity = %.15lef;\n", m_angularVelocity); - b2Log(" bd.linearDamping = %.15lef;\n", m_linearDamping); - b2Log(" bd.angularDamping = %.15lef;\n", m_angularDamping); - b2Log(" bd.allowSleep = bool(%d);\n", m_flags & e_autoSleepFlag); - b2Log(" bd.awake = bool(%d);\n", m_flags & e_awakeFlag); - b2Log(" bd.fixedRotation = bool(%d);\n", m_flags & e_fixedRotationFlag); - b2Log(" bd.bullet = bool(%d);\n", m_flags & e_bulletFlag); - b2Log(" bd.active = bool(%d);\n", m_flags & e_activeFlag); - b2Log(" bd.gravityScale = %.15lef;\n", m_gravityScale); - b2Log(" bodies[%d] = m_world->CreateBody(&bd);\n", m_islandIndex); - b2Log("\n"); - for (b2Fixture* f = m_fixtureList; f; f = f->m_next) - { - b2Log(" {\n"); - f->Dump(bodyIndex); - b2Log(" }\n"); - } - b2Log("}\n"); -} +// MIT License + +// Copyright (c) 2019 Erin Catto + +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: + +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. + +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. + +#include "box2d/b2_body.h" +#include "box2d/b2_contact.h" +#include "box2d/b2_fixture.h" +#include "box2d/b2_joint.h" +#include "box2d/b2_world.h" + +#include + +b2Body::b2Body(const b2BodyDef* bd, b2World* world) +{ + b2Assert(bd->position.IsValid()); + b2Assert(bd->linearVelocity.IsValid()); + b2Assert(b2IsValid(bd->angle)); + b2Assert(b2IsValid(bd->angularVelocity)); + b2Assert(b2IsValid(bd->angularDamping) && bd->angularDamping >= 0.0f); + b2Assert(b2IsValid(bd->linearDamping) && bd->linearDamping >= 0.0f); + + m_flags = 0; + + if (bd->bullet) + { + m_flags |= e_bulletFlag; + } + if (bd->fixedRotation) + { + m_flags |= e_fixedRotationFlag; + } + if (bd->allowSleep) + { + m_flags |= e_autoSleepFlag; + } + if (bd->awake && bd->type != b2_staticBody) + { + m_flags |= e_awakeFlag; + } + if (bd->enabled) + { + m_flags |= e_enabledFlag; + } + + 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; + + 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; + m_flags &= ~e_awakeFlag; + 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_enabledFlag) + { + 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_newContacts = true; + + return fixture; +} + +b2Fixture* b2Body::CreateFixture(const b2Shape* shape, float 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); + + const float density = fixture->m_density; + + // 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_enabledFlag) + { + 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 + if (density > 0.0f) + { + 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; + } + + 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, float 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); + } + + // Check for new contacts the next step + m_world->m_newContacts = true; +} + +void b2Body::SynchronizeFixtures() +{ + b2BroadPhase* broadPhase = &m_world->m_contactManager.m_broadPhase; + + if (m_flags & b2Body::e_awakeFlag) + { + b2Transform xf1; + xf1.q.Set(m_sweep.a0); + xf1.p = m_sweep.c0 - b2Mul(xf1.q, m_sweep.localCenter); + + for (b2Fixture* f = m_fixtureList; f; f = f->m_next) + { + f->Synchronize(broadPhase, xf1, m_xf); + } + } + else + { + for (b2Fixture* f = m_fixtureList; f; f = f->m_next) + { + f->Synchronize(broadPhase, m_xf, m_xf); + } + } +} + +void b2Body::SetEnabled(bool flag) +{ + b2Assert(m_world->IsLocked() == false); + + if (flag == IsEnabled()) + { + return; + } + + if (flag) + { + m_flags |= e_enabledFlag; + + // 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 at the beginning of the next + m_world->m_newContacts = true; + } + else + { + m_flags &= ~e_enabledFlag; + + // 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; + + // %.9g is sufficient to save and load the same value using text + // FLT_DECIMAL_DIG == 9 + + b2Dump("{\n"); + b2Dump(" b2BodyDef bd;\n"); + b2Dump(" bd.type = b2BodyType(%d);\n", m_type); + b2Dump(" bd.position.Set(%.9g, %.9g);\n", m_xf.p.x, m_xf.p.y); + b2Dump(" bd.angle = %.9g;\n", m_sweep.a); + b2Dump(" bd.linearVelocity.Set(%.9g, %.9g);\n", m_linearVelocity.x, m_linearVelocity.y); + b2Dump(" bd.angularVelocity = %.9g;\n", m_angularVelocity); + b2Dump(" bd.linearDamping = %.9g;\n", m_linearDamping); + b2Dump(" bd.angularDamping = %.9g;\n", m_angularDamping); + b2Dump(" bd.allowSleep = bool(%d);\n", m_flags & e_autoSleepFlag); + b2Dump(" bd.awake = bool(%d);\n", m_flags & e_awakeFlag); + b2Dump(" bd.fixedRotation = bool(%d);\n", m_flags & e_fixedRotationFlag); + b2Dump(" bd.bullet = bool(%d);\n", m_flags & e_bulletFlag); + b2Dump(" bd.enabled = bool(%d);\n", m_flags & e_enabledFlag); + b2Dump(" bd.gravityScale = %.9g;\n", m_gravityScale); + b2Dump(" bodies[%d] = m_world->CreateBody(&bd);\n", m_islandIndex); + b2Dump("\n"); + for (b2Fixture* f = m_fixtureList; f; f = f->m_next) + { + b2Dump(" {\n"); + f->Dump(bodyIndex); + b2Dump(" }\n"); + } + b2Dump("}\n"); +} diff --git a/libs/box2d/src/dynamics/b2_chain_circle_contact.cpp b/libs/box2d/src/dynamics/b2_chain_circle_contact.cpp new file mode 100644 index 00000000..8464fe83 --- /dev/null +++ b/libs/box2d/src/dynamics/b2_chain_circle_contact.cpp @@ -0,0 +1,57 @@ +// MIT License + +// Copyright (c) 2019 Erin Catto + +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: + +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. + +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. + +#include "b2_chain_circle_contact.h" +#include "box2d/b2_block_allocator.h" +#include "box2d/b2_fixture.h" +#include "box2d/b2_chain_shape.h" +#include "box2d/b2_edge_shape.h" + +#include + +b2Contact* b2ChainAndCircleContact::Create(b2Fixture* fixtureA, int32 indexA, b2Fixture* fixtureB, int32 indexB, b2BlockAllocator* allocator) +{ + void* mem = allocator->Allocate(sizeof(b2ChainAndCircleContact)); + return new (mem) b2ChainAndCircleContact(fixtureA, indexA, fixtureB, indexB); +} + +void b2ChainAndCircleContact::Destroy(b2Contact* contact, b2BlockAllocator* allocator) +{ + ((b2ChainAndCircleContact*)contact)->~b2ChainAndCircleContact(); + allocator->Free(contact, sizeof(b2ChainAndCircleContact)); +} + +b2ChainAndCircleContact::b2ChainAndCircleContact(b2Fixture* fixtureA, int32 indexA, b2Fixture* fixtureB, int32 indexB) +: b2Contact(fixtureA, indexA, fixtureB, indexB) +{ + b2Assert(m_fixtureA->GetType() == b2Shape::e_chain); + b2Assert(m_fixtureB->GetType() == b2Shape::e_circle); +} + +void b2ChainAndCircleContact::Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB) +{ + b2ChainShape* chain = (b2ChainShape*)m_fixtureA->GetShape(); + b2EdgeShape edge; + chain->GetChildEdge(&edge, m_indexA); + b2CollideEdgeAndCircle( manifold, &edge, xfA, + (b2CircleShape*)m_fixtureB->GetShape(), xfB); +} diff --git a/libs/box2d/src/dynamics/b2_chain_circle_contact.h b/libs/box2d/src/dynamics/b2_chain_circle_contact.h new file mode 100644 index 00000000..33ced64d --- /dev/null +++ b/libs/box2d/src/dynamics/b2_chain_circle_contact.h @@ -0,0 +1,43 @@ +// MIT License + +// Copyright (c) 2019 Erin Catto + +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: + +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. + +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. + +#ifndef B2_CHAIN_AND_CIRCLE_CONTACT_H +#define B2_CHAIN_AND_CIRCLE_CONTACT_H + +#include "box2d/b2_contact.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/libs/box2d/src/dynamics/b2_chain_polygon_contact.cpp b/libs/box2d/src/dynamics/b2_chain_polygon_contact.cpp new file mode 100644 index 00000000..b8257aab --- /dev/null +++ b/libs/box2d/src/dynamics/b2_chain_polygon_contact.cpp @@ -0,0 +1,57 @@ +// MIT License + +// Copyright (c) 2019 Erin Catto + +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: + +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. + +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. + +#include "b2_chain_polygon_contact.h" +#include "box2d/b2_block_allocator.h" +#include "box2d/b2_fixture.h" +#include "box2d/b2_chain_shape.h" +#include "box2d/b2_edge_shape.h" + +#include + +b2Contact* b2ChainAndPolygonContact::Create(b2Fixture* fixtureA, int32 indexA, b2Fixture* fixtureB, int32 indexB, b2BlockAllocator* allocator) +{ + void* mem = allocator->Allocate(sizeof(b2ChainAndPolygonContact)); + return new (mem) b2ChainAndPolygonContact(fixtureA, indexA, fixtureB, indexB); +} + +void b2ChainAndPolygonContact::Destroy(b2Contact* contact, b2BlockAllocator* allocator) +{ + ((b2ChainAndPolygonContact*)contact)->~b2ChainAndPolygonContact(); + allocator->Free(contact, sizeof(b2ChainAndPolygonContact)); +} + +b2ChainAndPolygonContact::b2ChainAndPolygonContact(b2Fixture* fixtureA, int32 indexA, b2Fixture* fixtureB, int32 indexB) +: b2Contact(fixtureA, indexA, fixtureB, indexB) +{ + b2Assert(m_fixtureA->GetType() == b2Shape::e_chain); + b2Assert(m_fixtureB->GetType() == b2Shape::e_polygon); +} + +void b2ChainAndPolygonContact::Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB) +{ + b2ChainShape* chain = (b2ChainShape*)m_fixtureA->GetShape(); + b2EdgeShape edge; + chain->GetChildEdge(&edge, m_indexA); + b2CollideEdgeAndPolygon( manifold, &edge, xfA, + (b2PolygonShape*)m_fixtureB->GetShape(), xfB); +} diff --git a/libs/box2d/src/dynamics/b2_chain_polygon_contact.h b/libs/box2d/src/dynamics/b2_chain_polygon_contact.h new file mode 100644 index 00000000..058154fb --- /dev/null +++ b/libs/box2d/src/dynamics/b2_chain_polygon_contact.h @@ -0,0 +1,43 @@ +// MIT License + +// Copyright (c) 2019 Erin Catto + +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: + +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. + +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. + +#ifndef B2_CHAIN_AND_POLYGON_CONTACT_H +#define B2_CHAIN_AND_POLYGON_CONTACT_H + +#include "box2d/b2_contact.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/libs/box2d/src/dynamics/b2_circle_contact.cpp b/libs/box2d/src/dynamics/b2_circle_contact.cpp new file mode 100644 index 00000000..738aa84c --- /dev/null +++ b/libs/box2d/src/dynamics/b2_circle_contact.cpp @@ -0,0 +1,56 @@ +// MIT License + +// Copyright (c) 2019 Erin Catto + +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: + +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. + +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. + +#include "b2_circle_contact.h" +#include "box2d/b2_block_allocator.h" +#include "box2d/b2_body.h" +#include "box2d/b2_fixture.h" +#include "box2d/b2_time_of_impact.h" +#include "box2d/b2_world_callbacks.h" + +#include + +b2Contact* b2CircleContact::Create(b2Fixture* fixtureA, int32, b2Fixture* fixtureB, int32, b2BlockAllocator* allocator) +{ + void* mem = allocator->Allocate(sizeof(b2CircleContact)); + return new (mem) b2CircleContact(fixtureA, fixtureB); +} + +void b2CircleContact::Destroy(b2Contact* contact, b2BlockAllocator* allocator) +{ + ((b2CircleContact*)contact)->~b2CircleContact(); + allocator->Free(contact, sizeof(b2CircleContact)); +} + +b2CircleContact::b2CircleContact(b2Fixture* fixtureA, b2Fixture* fixtureB) + : b2Contact(fixtureA, 0, fixtureB, 0) +{ + b2Assert(m_fixtureA->GetType() == b2Shape::e_circle); + b2Assert(m_fixtureB->GetType() == b2Shape::e_circle); +} + +void b2CircleContact::Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB) +{ + b2CollideCircles(manifold, + (b2CircleShape*)m_fixtureA->GetShape(), xfA, + (b2CircleShape*)m_fixtureB->GetShape(), xfB); +} diff --git a/libs/box2d/src/dynamics/b2_circle_contact.h b/libs/box2d/src/dynamics/b2_circle_contact.h new file mode 100644 index 00000000..98e09ea5 --- /dev/null +++ b/libs/box2d/src/dynamics/b2_circle_contact.h @@ -0,0 +1,43 @@ +// MIT License + +// Copyright (c) 2019 Erin Catto + +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: + +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. + +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. + +#ifndef B2_CIRCLE_CONTACT_H +#define B2_CIRCLE_CONTACT_H + +#include "box2d/b2_contact.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/libs/Box2D/Dynamics/Contacts/b2Contact.cpp b/libs/box2d/src/dynamics/b2_contact.cpp similarity index 70% rename from libs/Box2D/Dynamics/Contacts/b2Contact.cpp rename to libs/box2d/src/dynamics/b2_contact.cpp index 8d0104e8..1c65bc89 100644 --- a/libs/Box2D/Dynamics/Contacts/b2Contact.cpp +++ b/libs/box2d/src/dynamics/b2_contact.cpp @@ -1,247 +1,252 @@ -/* -* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include - -b2ContactRegister b2Contact::s_registers[b2Shape::e_typeCount][b2Shape::e_typeCount]; -bool b2Contact::s_initialized = false; - -void b2Contact::InitializeRegisters() -{ - AddType(b2CircleContact::Create, b2CircleContact::Destroy, b2Shape::e_circle, b2Shape::e_circle); - AddType(b2PolygonAndCircleContact::Create, b2PolygonAndCircleContact::Destroy, b2Shape::e_polygon, b2Shape::e_circle); - AddType(b2PolygonContact::Create, b2PolygonContact::Destroy, b2Shape::e_polygon, b2Shape::e_polygon); - AddType(b2EdgeAndCircleContact::Create, b2EdgeAndCircleContact::Destroy, b2Shape::e_edge, b2Shape::e_circle); - AddType(b2EdgeAndPolygonContact::Create, b2EdgeAndPolygonContact::Destroy, b2Shape::e_edge, b2Shape::e_polygon); - AddType(b2ChainAndCircleContact::Create, b2ChainAndCircleContact::Destroy, b2Shape::e_chain, b2Shape::e_circle); - AddType(b2ChainAndPolygonContact::Create, b2ChainAndPolygonContact::Destroy, b2Shape::e_chain, b2Shape::e_polygon); -} - -void b2Contact::AddType(b2ContactCreateFcn* createFcn, b2ContactDestroyFcn* destoryFcn, - b2Shape::Type type1, b2Shape::Type type2) -{ - b2Assert(0 <= type1 && type1 < b2Shape::e_typeCount); - b2Assert(0 <= type2 && type2 < b2Shape::e_typeCount); - - s_registers[type1][type2].createFcn = createFcn; - s_registers[type1][type2].destroyFcn = destoryFcn; - s_registers[type1][type2].primary = true; - - if (type1 != type2) - { - s_registers[type2][type1].createFcn = createFcn; - s_registers[type2][type1].destroyFcn = destoryFcn; - s_registers[type2][type1].primary = false; - } -} - -b2Contact* b2Contact::Create(b2Fixture* fixtureA, int32 indexA, b2Fixture* fixtureB, int32 indexB, b2BlockAllocator* allocator) -{ - if (s_initialized == false) - { - InitializeRegisters(); - s_initialized = true; - } - - b2Shape::Type type1 = fixtureA->GetType(); - b2Shape::Type type2 = fixtureB->GetType(); - - b2Assert(0 <= type1 && type1 < b2Shape::e_typeCount); - b2Assert(0 <= type2 && type2 < b2Shape::e_typeCount); - - b2ContactCreateFcn* createFcn = s_registers[type1][type2].createFcn; - if (createFcn) - { - if (s_registers[type1][type2].primary) - { - return createFcn(fixtureA, indexA, fixtureB, indexB, allocator); - } - else - { - return createFcn(fixtureB, indexB, fixtureA, indexA, allocator); - } - } - else - { - return NULL; - } -} - -void b2Contact::Destroy(b2Contact* contact, b2BlockAllocator* allocator) -{ - b2Assert(s_initialized == true); - - b2Fixture* fixtureA = contact->m_fixtureA; - b2Fixture* fixtureB = contact->m_fixtureB; - - if (contact->m_manifold.pointCount > 0 && - fixtureA->IsSensor() == false && - fixtureB->IsSensor() == false) - { - fixtureA->GetBody()->SetAwake(true); - fixtureB->GetBody()->SetAwake(true); - } - - b2Shape::Type typeA = fixtureA->GetType(); - b2Shape::Type typeB = fixtureB->GetType(); - - b2Assert(0 <= typeA && typeB < b2Shape::e_typeCount); - b2Assert(0 <= typeA && typeB < b2Shape::e_typeCount); - - b2ContactDestroyFcn* destroyFcn = s_registers[typeA][typeB].destroyFcn; - destroyFcn(contact, allocator); -} - -b2Contact::b2Contact(b2Fixture* fA, int32 indexA, b2Fixture* fB, int32 indexB) -{ - m_flags = e_enabledFlag; - - m_fixtureA = fA; - m_fixtureB = fB; - - m_indexA = indexA; - m_indexB = indexB; - - m_manifold.pointCount = 0; - - m_prev = NULL; - m_next = NULL; - - m_nodeA.contact = NULL; - m_nodeA.prev = NULL; - m_nodeA.next = NULL; - m_nodeA.other = NULL; - - m_nodeB.contact = NULL; - m_nodeB.prev = NULL; - m_nodeB.next = NULL; - m_nodeB.other = NULL; - - m_toiCount = 0; - - m_friction = b2MixFriction(m_fixtureA->m_friction, m_fixtureB->m_friction); - m_restitution = b2MixRestitution(m_fixtureA->m_restitution, m_fixtureB->m_restitution); - - m_tangentSpeed = 0.0f; -} - -// Update the contact manifold and touching status. -// Note: do not assume the fixture AABBs are overlapping or are valid. -void b2Contact::Update(b2ContactListener* listener) -{ - b2Manifold oldManifold = m_manifold; - - // Re-enable this contact. - m_flags |= e_enabledFlag; - - bool touching = false; - bool wasTouching = (m_flags & e_touchingFlag) == e_touchingFlag; - - bool sensorA = m_fixtureA->IsSensor(); - bool sensorB = m_fixtureB->IsSensor(); - bool sensor = sensorA || sensorB; - - b2Body* bodyA = m_fixtureA->GetBody(); - b2Body* bodyB = m_fixtureB->GetBody(); - const b2Transform& xfA = bodyA->GetTransform(); - const b2Transform& xfB = bodyB->GetTransform(); - - // Is this contact a sensor? - if (sensor) - { - const b2Shape* shapeA = m_fixtureA->GetShape(); - const b2Shape* shapeB = m_fixtureB->GetShape(); - touching = b2TestOverlap(shapeA, m_indexA, shapeB, m_indexB, xfA, xfB); - - // Sensors don't generate manifolds. - m_manifold.pointCount = 0; - } - else - { - Evaluate(&m_manifold, xfA, xfB); - touching = m_manifold.pointCount > 0; - - // Match old contact ids to new contact ids and copy the - // stored impulses to warm start the solver. - for (int32 i = 0; i < m_manifold.pointCount; ++i) - { - b2ManifoldPoint* mp2 = m_manifold.points + i; - mp2->normalImpulse = 0.0f; - mp2->tangentImpulse = 0.0f; - b2ContactID id2 = mp2->id; - - for (int32 j = 0; j < oldManifold.pointCount; ++j) - { - b2ManifoldPoint* mp1 = oldManifold.points + j; - - if (mp1->id.key == id2.key) - { - mp2->normalImpulse = mp1->normalImpulse; - mp2->tangentImpulse = mp1->tangentImpulse; - break; - } - } - } - - if (touching != wasTouching) - { - bodyA->SetAwake(true); - bodyB->SetAwake(true); - } - } - - if (touching) - { - m_flags |= e_touchingFlag; - } - else - { - m_flags &= ~e_touchingFlag; - } - - if (wasTouching == false && touching == true && listener) - { - listener->BeginContact(this); - } - - if (wasTouching == true && touching == false && listener) - { - listener->EndContact(this); - } - - if (sensor == false && touching && listener) - { - listener->PreSolve(this, &oldManifold); - } -} +// MIT License + +// Copyright (c) 2019 Erin Catto + +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: + +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. + +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. + +#include "b2_chain_circle_contact.h" +#include "b2_chain_polygon_contact.h" +#include "b2_circle_contact.h" +#include "b2_contact_solver.h" +#include "b2_edge_circle_contact.h" +#include "b2_edge_polygon_contact.h" +#include "b2_polygon_circle_contact.h" +#include "b2_polygon_contact.h" + +#include "box2d/b2_contact.h" +#include "box2d/b2_block_allocator.h" +#include "box2d/b2_body.h" +#include "box2d/b2_collision.h" +#include "box2d/b2_fixture.h" +#include "box2d/b2_shape.h" +#include "box2d/b2_time_of_impact.h" +#include "box2d/b2_world.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 && typeA < b2Shape::e_typeCount); + b2Assert(0 <= typeB && 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_restitutionThreshold = b2MixRestitutionThreshold(m_fixtureA->m_restitutionThreshold, m_fixtureB->m_restitutionThreshold); + + 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/libs/Box2D/Dynamics/b2ContactManager.cpp b/libs/box2d/src/dynamics/b2_contact_manager.cpp similarity index 75% rename from libs/Box2D/Dynamics/b2ContactManager.cpp rename to libs/box2d/src/dynamics/b2_contact_manager.cpp index d71439ce..56666084 100644 --- a/libs/Box2D/Dynamics/b2ContactManager.cpp +++ b/libs/box2d/src/dynamics/b2_contact_manager.cpp @@ -1,296 +1,293 @@ -/* -* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ - -#include -#include -#include -#include -#include - -b2ContactFilter b2_defaultFilter; -b2ContactListener b2_defaultListener; - -b2ContactManager::b2ContactManager() -{ - m_contactList = NULL; - m_contactCount = 0; - m_contactFilter = &b2_defaultFilter; - m_contactListener = &b2_defaultListener; - m_allocator = NULL; -} - -void b2ContactManager::Destroy(b2Contact* c) -{ - b2Fixture* fixtureA = c->GetFixtureA(); - b2Fixture* fixtureB = c->GetFixtureB(); - b2Body* bodyA = fixtureA->GetBody(); - b2Body* bodyB = fixtureB->GetBody(); - - if (m_contactListener && c->IsTouching()) - { - m_contactListener->EndContact(c); - } - - // Remove from the world. - if (c->m_prev) - { - c->m_prev->m_next = c->m_next; - } - - if (c->m_next) - { - c->m_next->m_prev = c->m_prev; - } - - if (c == m_contactList) - { - m_contactList = c->m_next; - } - - // Remove from body 1 - if (c->m_nodeA.prev) - { - c->m_nodeA.prev->next = c->m_nodeA.next; - } - - if (c->m_nodeA.next) - { - c->m_nodeA.next->prev = c->m_nodeA.prev; - } - - if (&c->m_nodeA == bodyA->m_contactList) - { - bodyA->m_contactList = c->m_nodeA.next; - } - - // Remove from body 2 - if (c->m_nodeB.prev) - { - c->m_nodeB.prev->next = c->m_nodeB.next; - } - - if (c->m_nodeB.next) - { - c->m_nodeB.next->prev = c->m_nodeB.prev; - } - - if (&c->m_nodeB == bodyB->m_contactList) - { - bodyB->m_contactList = c->m_nodeB.next; - } - - // Call the factory. - b2Contact::Destroy(c, m_allocator); - --m_contactCount; -} - -// This is the top level collision call for the time step. Here -// all the narrow phase collision is processed for the world -// contact list. -void b2ContactManager::Collide() -{ - // Update awake contacts. - b2Contact* c = m_contactList; - while (c) - { - b2Fixture* fixtureA = c->GetFixtureA(); - b2Fixture* fixtureB = c->GetFixtureB(); - int32 indexA = c->GetChildIndexA(); - int32 indexB = c->GetChildIndexB(); - b2Body* bodyA = fixtureA->GetBody(); - b2Body* bodyB = fixtureB->GetBody(); - - // Is this contact flagged for filtering? - if (c->m_flags & b2Contact::e_filterFlag) - { - // Should these bodies collide? - if (bodyB->ShouldCollide(bodyA) == false) - { - b2Contact* cNuke = c; - c = cNuke->GetNext(); - Destroy(cNuke); - continue; - } - - // Check user filtering. - if (m_contactFilter && m_contactFilter->ShouldCollide(fixtureA, fixtureB) == false) - { - b2Contact* cNuke = c; - c = cNuke->GetNext(); - Destroy(cNuke); - continue; - } - - // Clear the filtering flag. - c->m_flags &= ~b2Contact::e_filterFlag; - } - - bool activeA = bodyA->IsAwake() && bodyA->m_type != b2_staticBody; - bool activeB = bodyB->IsAwake() && bodyB->m_type != b2_staticBody; - - // At least one body must be awake and it must be dynamic or kinematic. - if (activeA == false && activeB == false) - { - c = c->GetNext(); - continue; - } - - int32 proxyIdA = fixtureA->m_proxies[indexA].proxyId; - int32 proxyIdB = fixtureB->m_proxies[indexB].proxyId; - bool overlap = m_broadPhase.TestOverlap(proxyIdA, proxyIdB); - - // Here we destroy contacts that cease to overlap in the broad-phase. - if (overlap == false) - { - b2Contact* cNuke = c; - c = cNuke->GetNext(); - Destroy(cNuke); - continue; - } - - // The contact persists. - c->Update(m_contactListener); - c = c->GetNext(); - } -} - -void b2ContactManager::FindNewContacts() -{ - m_broadPhase.UpdatePairs(this); -} - -void b2ContactManager::AddPair(void* proxyUserDataA, void* proxyUserDataB) -{ - b2FixtureProxy* proxyA = (b2FixtureProxy*)proxyUserDataA; - b2FixtureProxy* proxyB = (b2FixtureProxy*)proxyUserDataB; - - b2Fixture* fixtureA = proxyA->fixture; - b2Fixture* fixtureB = proxyB->fixture; - - int32 indexA = proxyA->childIndex; - int32 indexB = proxyB->childIndex; - - b2Body* bodyA = fixtureA->GetBody(); - b2Body* bodyB = fixtureB->GetBody(); - - // Are the fixtures on the same body? - if (bodyA == bodyB) - { - return; - } - - // TODO_ERIN use a hash table to remove a potential bottleneck when both - // bodies have a lot of contacts. - // Does a contact already exist? - b2ContactEdge* edge = bodyB->GetContactList(); - while (edge) - { - if (edge->other == bodyA) - { - b2Fixture* fA = edge->contact->GetFixtureA(); - b2Fixture* fB = edge->contact->GetFixtureB(); - int32 iA = edge->contact->GetChildIndexA(); - int32 iB = edge->contact->GetChildIndexB(); - - if (fA == fixtureA && fB == fixtureB && iA == indexA && iB == indexB) - { - // A contact already exists. - return; - } - - if (fA == fixtureB && fB == fixtureA && iA == indexB && iB == indexA) - { - // A contact already exists. - return; - } - } - - edge = edge->next; - } - - // Does a joint override collision? Is at least one body dynamic? - if (bodyB->ShouldCollide(bodyA) == false) - { - return; - } - - // Check user filtering. - if (m_contactFilter && m_contactFilter->ShouldCollide(fixtureA, fixtureB) == false) - { - return; - } - - // Call the factory. - b2Contact* c = b2Contact::Create(fixtureA, indexA, fixtureB, indexB, m_allocator); - if (c == NULL) - { - return; - } - - // Contact creation may swap fixtures. - fixtureA = c->GetFixtureA(); - fixtureB = c->GetFixtureB(); - indexA = c->GetChildIndexA(); - indexB = c->GetChildIndexB(); - bodyA = fixtureA->GetBody(); - bodyB = fixtureB->GetBody(); - - // Insert into the world. - c->m_prev = NULL; - c->m_next = m_contactList; - if (m_contactList != NULL) - { - m_contactList->m_prev = c; - } - m_contactList = c; - - // Connect to island graph. - - // Connect to body A - c->m_nodeA.contact = c; - c->m_nodeA.other = bodyB; - - c->m_nodeA.prev = NULL; - c->m_nodeA.next = bodyA->m_contactList; - if (bodyA->m_contactList != NULL) - { - bodyA->m_contactList->prev = &c->m_nodeA; - } - bodyA->m_contactList = &c->m_nodeA; - - // Connect to body B - c->m_nodeB.contact = c; - c->m_nodeB.other = bodyA; - - c->m_nodeB.prev = NULL; - c->m_nodeB.next = bodyB->m_contactList; - if (bodyB->m_contactList != NULL) - { - bodyB->m_contactList->prev = &c->m_nodeB; - } - bodyB->m_contactList = &c->m_nodeB; - - // Wake up the bodies - if (fixtureA->IsSensor() == false && fixtureB->IsSensor() == false) - { - bodyA->SetAwake(true); - bodyB->SetAwake(true); - } - - ++m_contactCount; -} +// MIT License + +// Copyright (c) 2019 Erin Catto + +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: + +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. + +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. + +#include "box2d/b2_body.h" +#include "box2d/b2_contact.h" +#include "box2d/b2_contact_manager.h" +#include "box2d/b2_fixture.h" +#include "box2d/b2_world_callbacks.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; + + ++m_contactCount; +} diff --git a/libs/Box2D/Dynamics/Contacts/b2ContactSolver.cpp b/libs/box2d/src/dynamics/b2_contact_solver.cpp similarity index 76% rename from libs/Box2D/Dynamics/Contacts/b2ContactSolver.cpp rename to libs/box2d/src/dynamics/b2_contact_solver.cpp index 06cff524..d6c08fb1 100644 --- a/libs/Box2D/Dynamics/Contacts/b2ContactSolver.cpp +++ b/libs/box2d/src/dynamics/b2_contact_solver.cpp @@ -1,838 +1,843 @@ -/* -* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ - -#include - -#include -#include -#include -#include -#include - -#define B2_DEBUG_SOLVER 0 - -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 i = 0; i < pointCount; ++i) - { - b2VelocityConstraintPoint* vcp = vc->points + i; - - // Relative velocity at contact - b2Vec2 dv = vB + b2Cross(wB, vcp->rB) - vA - b2Cross(wA, vcp->rA); - - // Compute normal impulse - float32 vn = b2Dot(dv, normal); - float32 lambda = -vcp->normalMass * (vn - vcp->velocityBias); - - // b2Clamp the accumulated impulse - float32 newImpulse = b2Max(vcp->normalImpulse + lambda, 0.0f); - lambda = newImpulse - vcp->normalImpulse; - vcp->normalImpulse = newImpulse; - - // Apply contact impulse - b2Vec2 P = lambda * normal; - vA -= mA * P; - wA -= iA * b2Cross(vcp->rA, P); - - vB += mB * P; - wB += iB * b2Cross(vcp->rB, P); - } - } - else - { - // Block solver developed in collaboration with Dirk Gregorius (back in 01/07 on Box2D_Lite). - // Build the mini LCP for this contact patch - // - // vn = A * x + b, vn >= 0, , vn >= 0, x >= 0 and vn_i * x_i = 0 with i = 1..2 - // - // A = J * W * JT and J = ( -n, -r1 x n, n, r2 x n ) - // b = vn0 - velocityBias - // - // The system is solved using the "Total enumeration method" (s. Murty). The complementary constraint vn_i * x_i - // implies that we must have in any solution either vn_i = 0 or x_i = 0. So for the 2D contact problem the cases - // vn1 = 0 and vn2 = 0, x1 = 0 and x2 = 0, x1 = 0 and vn2 = 0, x2 = 0 and vn1 = 0 need to be tested. The first valid - // solution that satisfies the problem is chosen. - // - // In order to account of the accumulated impulse 'a' (because of the iterative nature of the solver which only requires - // that the accumulated impulse is clamped and not the incremental impulse) we change the impulse variable (x_i). - // - // Substitute: - // - // x = a + d - // - // a := old total impulse - // x := new total impulse - // d := incremental impulse - // - // For the current iteration we extend the formula for the incremental impulse - // to compute the new total impulse: - // - // vn = A * d + b - // = A * (x - a) + b - // = A * x + b - A * a - // = A * x + b' - // b' = b - A * a; - - b2VelocityConstraintPoint* cp1 = vc->points + 0; - b2VelocityConstraintPoint* cp2 = vc->points + 1; - - b2Vec2 a(cp1->normalImpulse, cp2->normalImpulse); - b2Assert(a.x >= 0.0f && a.y >= 0.0f); - - // Relative velocity at contact - b2Vec2 dv1 = vB + b2Cross(wB, cp1->rB) - vA - b2Cross(wA, cp1->rA); - b2Vec2 dv2 = vB + b2Cross(wB, cp2->rB) - vA - b2Cross(wA, cp2->rA); - - // Compute normal velocity - float32 vn1 = b2Dot(dv1, normal); - float32 vn2 = b2Dot(dv2, normal); - - b2Vec2 b; - b.x = vn1 - cp1->velocityBias; - b.y = vn2 - cp2->velocityBias; - - // Compute b' - b -= b2Mul(vc->K, a); - - const float32 k_errorTol = 1e-3f; - B2_NOT_USED(k_errorTol); - - for (;;) - { - // - // Case 1: vn = 0 - // - // 0 = A * x + b' - // - // Solve for x: - // - // x = - inv(A) * b' - // - b2Vec2 x = - b2Mul(vc->normalMass, b); - - if (x.x >= 0.0f && x.y >= 0.0f) - { - // Get the incremental impulse - b2Vec2 d = x - a; - - // Apply incremental impulse - b2Vec2 P1 = d.x * normal; - b2Vec2 P2 = d.y * normal; - vA -= mA * (P1 + P2); - wA -= iA * (b2Cross(cp1->rA, P1) + b2Cross(cp2->rA, P2)); - - vB += mB * (P1 + P2); - wB += iB * (b2Cross(cp1->rB, P1) + b2Cross(cp2->rB, P2)); - - // Accumulate - cp1->normalImpulse = x.x; - cp2->normalImpulse = x.y; - -#if B2_DEBUG_SOLVER == 1 - // Postconditions - dv1 = vB + b2Cross(wB, cp1->rB) - vA - b2Cross(wA, cp1->rA); - dv2 = vB + b2Cross(wB, cp2->rB) - vA - b2Cross(wA, cp2->rA); - - // Compute normal velocity - vn1 = b2Dot(dv1, normal); - vn2 = b2Dot(dv2, normal); - - b2Assert(b2Abs(vn1 - cp1->velocityBias) < k_errorTol); - b2Assert(b2Abs(vn2 - cp2->velocityBias) < k_errorTol); -#endif - break; - } - - // - // Case 2: vn1 = 0 and x2 = 0 - // - // 0 = a11 * x1 + a12 * 0 + b1' - // vn2 = a21 * x1 + a22 * 0 + b2' - // - x.x = - cp1->normalMass * b.x; - x.y = 0.0f; - vn1 = 0.0f; - vn2 = vc->K.ex.y * x.x + b.y; - - if (x.x >= 0.0f && vn2 >= 0.0f) - { - // Get the incremental impulse - b2Vec2 d = x - a; - - // Apply incremental impulse - b2Vec2 P1 = d.x * normal; - b2Vec2 P2 = d.y * normal; - vA -= mA * (P1 + P2); - wA -= iA * (b2Cross(cp1->rA, P1) + b2Cross(cp2->rA, P2)); - - vB += mB * (P1 + P2); - wB += iB * (b2Cross(cp1->rB, P1) + b2Cross(cp2->rB, P2)); - - // Accumulate - cp1->normalImpulse = x.x; - cp2->normalImpulse = x.y; - -#if B2_DEBUG_SOLVER == 1 - // Postconditions - dv1 = vB + b2Cross(wB, cp1->rB) - vA - b2Cross(wA, cp1->rA); - - // Compute normal velocity - vn1 = b2Dot(dv1, normal); - - b2Assert(b2Abs(vn1 - cp1->velocityBias) < k_errorTol); -#endif - break; - } - - - // - // Case 3: vn2 = 0 and x1 = 0 - // - // vn1 = a11 * 0 + a12 * x2 + b1' - // 0 = a21 * 0 + a22 * x2 + b2' - // - x.x = 0.0f; - x.y = - cp2->normalMass * b.y; - vn1 = vc->K.ey.x * x.y + b.x; - vn2 = 0.0f; - - if (x.y >= 0.0f && vn1 >= 0.0f) - { - // Resubstitute for the incremental impulse - b2Vec2 d = x - a; - - // Apply incremental impulse - b2Vec2 P1 = d.x * normal; - b2Vec2 P2 = d.y * normal; - vA -= mA * (P1 + P2); - wA -= iA * (b2Cross(cp1->rA, P1) + b2Cross(cp2->rA, P2)); - - vB += mB * (P1 + P2); - wB += iB * (b2Cross(cp1->rB, P1) + b2Cross(cp2->rB, P2)); - - // Accumulate - cp1->normalImpulse = x.x; - cp2->normalImpulse = x.y; - -#if B2_DEBUG_SOLVER == 1 - // Postconditions - dv2 = vB + b2Cross(wB, cp2->rB) - vA - b2Cross(wA, cp2->rA); - - // Compute normal velocity - vn2 = b2Dot(dv2, normal); - - b2Assert(b2Abs(vn2 - cp2->velocityBias) < k_errorTol); -#endif - break; - } - - // - // Case 4: x1 = 0 and x2 = 0 - // - // vn1 = b1 - // vn2 = b2; - x.x = 0.0f; - x.y = 0.0f; - vn1 = b.x; - vn2 = b.y; - - if (vn1 >= 0.0f && vn2 >= 0.0f ) - { - // Resubstitute for the incremental impulse - b2Vec2 d = x - a; - - // Apply incremental impulse - b2Vec2 P1 = d.x * normal; - b2Vec2 P2 = d.y * normal; - vA -= mA * (P1 + P2); - wA -= iA * (b2Cross(cp1->rA, P1) + b2Cross(cp2->rA, P2)); - - vB += mB * (P1 + P2); - wB += iB * (b2Cross(cp1->rB, P1) + b2Cross(cp2->rB, P2)); - - // Accumulate - cp1->normalImpulse = x.x; - cp2->normalImpulse = x.y; - - break; - } - - // No solution, give up. This is hit sometimes, but it doesn't seem to matter. - break; - } - } - - m_velocities[indexA].v = vA; - m_velocities[indexA].w = wA; - m_velocities[indexB].v = vB; - m_velocities[indexB].w = wB; - } -} - -void b2ContactSolver::StoreImpulses() -{ - for (int32 i = 0; i < m_count; ++i) - { - b2ContactVelocityConstraint* vc = m_velocityConstraints + i; - b2Manifold* manifold = m_contacts[vc->contactIndex]->GetManifold(); - - for (int32 j = 0; j < vc->pointCount; ++j) - { - manifold->points[j].normalImpulse = vc->points[j].normalImpulse; - manifold->points[j].tangentImpulse = vc->points[j].tangentImpulse; - } - } -} - -struct b2PositionSolverManifold -{ - void Initialize(b2ContactPositionConstraint* pc, const b2Transform& xfA, const b2Transform& xfB, int32 index) - { - b2Assert(pc->pointCount > 0); - - switch (pc->type) - { - case b2Manifold::e_circles: - { - b2Vec2 pointA = b2Mul(xfA, pc->localPoint); - b2Vec2 pointB = b2Mul(xfB, pc->localPoints[0]); - normal = pointB - pointA; - normal.Normalize(); - point = 0.5f * (pointA + pointB); - separation = b2Dot(pointB - pointA, normal) - pc->radiusA - pc->radiusB; - } - break; - - case b2Manifold::e_faceA: - { - normal = b2Mul(xfA.q, pc->localNormal); - b2Vec2 planePoint = b2Mul(xfA, pc->localPoint); - - b2Vec2 clipPoint = b2Mul(xfB, pc->localPoints[index]); - separation = b2Dot(clipPoint - planePoint, normal) - pc->radiusA - pc->radiusB; - point = clipPoint; - } - break; - - case b2Manifold::e_faceB: - { - normal = b2Mul(xfB.q, pc->localNormal); - b2Vec2 planePoint = b2Mul(xfB, pc->localPoint); - - b2Vec2 clipPoint = b2Mul(xfA, pc->localPoints[index]); - separation = b2Dot(clipPoint - planePoint, normal) - pc->radiusA - pc->radiusB; - point = clipPoint; - - // Ensure normal points from A to B - normal = -normal; - } - break; - } - } - - b2Vec2 normal; - b2Vec2 point; - float32 separation; -}; - -// Sequential solver. -bool b2ContactSolver::SolvePositionConstraints() -{ - float32 minSeparation = 0.0f; - - for (int32 i = 0; i < m_count; ++i) - { - b2ContactPositionConstraint* pc = m_positionConstraints + i; - - int32 indexA = pc->indexA; - int32 indexB = pc->indexB; - b2Vec2 localCenterA = pc->localCenterA; - float32 mA = pc->invMassA; - float32 iA = pc->invIA; - b2Vec2 localCenterB = pc->localCenterB; - float32 mB = pc->invMassB; - float32 iB = pc->invIB; - int32 pointCount = pc->pointCount; - - b2Vec2 cA = m_positions[indexA].c; - float32 aA = m_positions[indexA].a; - - b2Vec2 cB = m_positions[indexB].c; - float32 aB = m_positions[indexB].a; - - // Solve normal constraints - for (int32 j = 0; j < pointCount; ++j) - { - b2Transform xfA, xfB; - xfA.q.Set(aA); - xfB.q.Set(aB); - xfA.p = cA - b2Mul(xfA.q, localCenterA); - xfB.p = cB - b2Mul(xfB.q, localCenterB); - - b2PositionSolverManifold psm; - psm.Initialize(pc, xfA, xfB, j); - b2Vec2 normal = psm.normal; - - b2Vec2 point = psm.point; - float32 separation = psm.separation; - - b2Vec2 rA = point - cA; - b2Vec2 rB = point - cB; - - // Track max constraint error. - minSeparation = b2Min(minSeparation, separation); - - // Prevent large corrections and allow slop. - float32 C = b2Clamp(b2_baumgarte * (separation + b2_linearSlop), -b2_maxLinearCorrection, 0.0f); - - // Compute the effective mass. - float32 rnA = b2Cross(rA, normal); - float32 rnB = b2Cross(rB, normal); - float32 K = mA + mB + iA * rnA * rnA + iB * rnB * rnB; - - // Compute normal impulse - float32 impulse = K > 0.0f ? - C / K : 0.0f; - - b2Vec2 P = impulse * normal; - - cA -= mA * P; - aA -= iA * b2Cross(rA, P); - - cB += mB * P; - aB += iB * b2Cross(rB, P); - } - - m_positions[indexA].c = cA; - m_positions[indexA].a = aA; - - m_positions[indexB].c = cB; - m_positions[indexB].a = aB; - } - - // We can't expect minSpeparation >= -b2_linearSlop because we don't - // push the separation above -b2_linearSlop. - return minSeparation >= -3.0f * b2_linearSlop; -} - -// Sequential position solver for position constraints. -bool b2ContactSolver::SolveTOIPositionConstraints(int32 toiIndexA, int32 toiIndexB) -{ - float32 minSeparation = 0.0f; - - for (int32 i = 0; i < m_count; ++i) - { - b2ContactPositionConstraint* pc = m_positionConstraints + i; - - int32 indexA = pc->indexA; - int32 indexB = pc->indexB; - b2Vec2 localCenterA = pc->localCenterA; - b2Vec2 localCenterB = pc->localCenterB; - int32 pointCount = pc->pointCount; - - float32 mA = 0.0f; - float32 iA = 0.0f; - if (indexA == toiIndexA || indexA == toiIndexB) - { - mA = pc->invMassA; - iA = pc->invIA; - } - - float32 mB = 0.0f; - float32 iB = 0.; - if (indexB == toiIndexA || indexB == toiIndexB) - { - mB = pc->invMassB; - iB = pc->invIB; - } - - b2Vec2 cA = m_positions[indexA].c; - float32 aA = m_positions[indexA].a; - - b2Vec2 cB = m_positions[indexB].c; - float32 aB = m_positions[indexB].a; - - // Solve normal constraints - for (int32 j = 0; j < pointCount; ++j) - { - b2Transform xfA, xfB; - xfA.q.Set(aA); - xfB.q.Set(aB); - xfA.p = cA - b2Mul(xfA.q, localCenterA); - xfB.p = cB - b2Mul(xfB.q, localCenterB); - - b2PositionSolverManifold psm; - psm.Initialize(pc, xfA, xfB, j); - b2Vec2 normal = psm.normal; - - b2Vec2 point = psm.point; - float32 separation = psm.separation; - - b2Vec2 rA = point - cA; - b2Vec2 rB = point - cB; - - // Track max constraint error. - minSeparation = b2Min(minSeparation, separation); - - // Prevent large corrections and allow slop. - float32 C = b2Clamp(b2_toiBaugarte * (separation + b2_linearSlop), -b2_maxLinearCorrection, 0.0f); - - // Compute the effective mass. - float32 rnA = b2Cross(rA, normal); - float32 rnB = b2Cross(rB, normal); - float32 K = mA + mB + iA * rnA * rnA + iB * rnB * rnB; - - // Compute normal impulse - float32 impulse = K > 0.0f ? - C / K : 0.0f; - - b2Vec2 P = impulse * normal; - - cA -= mA * P; - aA -= iA * b2Cross(rA, P); - - cB += mB * P; - aB += iB * b2Cross(rB, P); - } - - m_positions[indexA].c = cA; - m_positions[indexA].a = aA; - - m_positions[indexB].c = cB; - m_positions[indexB].a = aB; - } - - // We can't expect minSpeparation >= -b2_linearSlop because we don't - // push the separation above -b2_linearSlop. - return minSeparation >= -1.5f * b2_linearSlop; -} +// MIT License + +// Copyright (c) 2019 Erin Catto + +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: + +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. + +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. + +#include "b2_contact_solver.h" + +#include "box2d/b2_body.h" +#include "box2d/b2_contact.h" +#include "box2d/b2_fixture.h" +#include "box2d/b2_stack_allocator.h" +#include "box2d/b2_world.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 + +B2_API bool g_blockSolve = true; + +struct b2ContactPositionConstraint +{ + b2Vec2 localPoints[b2_maxManifoldPoints]; + b2Vec2 localNormal; + b2Vec2 localPoint; + int32 indexA; + int32 indexB; + float invMassA, invMassB; + b2Vec2 localCenterA, localCenterB; + float invIA, invIB; + b2Manifold::Type type; + float 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(); + float radiusA = shapeA->m_radius; + float 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->threshold = contact->m_restitutionThreshold; + 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; + + float radiusA = pc->radiusA; + float radiusB = pc->radiusB; + b2Manifold* manifold = m_contacts[vc->contactIndex]->GetManifold(); + + int32 indexA = vc->indexA; + int32 indexB = vc->indexB; + + float mA = vc->invMassA; + float mB = vc->invMassB; + float iA = vc->invIA; + float iB = vc->invIB; + b2Vec2 localCenterA = pc->localCenterA; + b2Vec2 localCenterB = pc->localCenterB; + + b2Vec2 cA = m_positions[indexA].c; + float aA = m_positions[indexA].a; + b2Vec2 vA = m_velocities[indexA].v; + float wA = m_velocities[indexA].w; + + b2Vec2 cB = m_positions[indexB].c; + float aB = m_positions[indexB].a; + b2Vec2 vB = m_velocities[indexB].v; + float 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; + + float rnA = b2Cross(vcp->rA, vc->normal); + float rnB = b2Cross(vcp->rB, vc->normal); + + float 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); + + float rtA = b2Cross(vcp->rA, tangent); + float rtB = b2Cross(vcp->rB, tangent); + + float 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; + float vRel = b2Dot(vc->normal, vB + b2Cross(wB, vcp->rB) - vA - b2Cross(wA, vcp->rA)); + if (vRel < -vc->threshold) + { + 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; + + float rn1A = b2Cross(vcp1->rA, vc->normal); + float rn1B = b2Cross(vcp1->rB, vc->normal); + float rn2A = b2Cross(vcp2->rA, vc->normal); + float rn2B = b2Cross(vcp2->rB, vc->normal); + + float k11 = mA + mB + iA * rn1A * rn1A + iB * rn1B * rn1B; + float k22 = mA + mB + iA * rn2A * rn2A + iB * rn2B * rn2B; + float k12 = mA + mB + iA * rn1A * rn2A + iB * rn1B * rn2B; + + // Ensure a reasonable condition number. + const float 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; + float mA = vc->invMassA; + float iA = vc->invIA; + float mB = vc->invMassB; + float iB = vc->invIB; + int32 pointCount = vc->pointCount; + + b2Vec2 vA = m_velocities[indexA].v; + float wA = m_velocities[indexA].w; + b2Vec2 vB = m_velocities[indexB].v; + float 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; + float mA = vc->invMassA; + float iA = vc->invIA; + float mB = vc->invMassB; + float iB = vc->invIB; + int32 pointCount = vc->pointCount; + + b2Vec2 vA = m_velocities[indexA].v; + float wA = m_velocities[indexA].w; + b2Vec2 vB = m_velocities[indexB].v; + float wB = m_velocities[indexB].w; + + b2Vec2 normal = vc->normal; + b2Vec2 tangent = b2Cross(normal, 1.0f); + float 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 + float vt = b2Dot(dv, tangent) - vc->tangentSpeed; + float lambda = vcp->tangentMass * (-vt); + + // b2Clamp the accumulated force + float maxFriction = friction * vcp->normalImpulse; + float 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 + float vn = b2Dot(dv, normal); + float lambda = -vcp->normalMass * (vn - vcp->velocityBias); + + // b2Clamp the accumulated impulse + float 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 + float vn1 = b2Dot(dv1, normal); + float vn2 = b2Dot(dv2, normal); + + b2Vec2 b; + b.x = vn1 - cp1->velocityBias; + b.y = vn2 - cp2->velocityBias; + + // Compute b' + b -= b2Mul(vc->K, a); + + const float 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; + float separation; +}; + +// Sequential solver. +bool b2ContactSolver::SolvePositionConstraints() +{ + float 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; + float mA = pc->invMassA; + float iA = pc->invIA; + b2Vec2 localCenterB = pc->localCenterB; + float mB = pc->invMassB; + float iB = pc->invIB; + int32 pointCount = pc->pointCount; + + b2Vec2 cA = m_positions[indexA].c; + float aA = m_positions[indexA].a; + + b2Vec2 cB = m_positions[indexB].c; + float 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; + float 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. + float C = b2Clamp(b2_baumgarte * (separation + b2_linearSlop), -b2_maxLinearCorrection, 0.0f); + + // Compute the effective mass. + float rnA = b2Cross(rA, normal); + float rnB = b2Cross(rB, normal); + float K = mA + mB + iA * rnA * rnA + iB * rnB * rnB; + + // Compute normal impulse + float 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) +{ + float 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; + + float mA = 0.0f; + float iA = 0.0f; + if (indexA == toiIndexA || indexA == toiIndexB) + { + mA = pc->invMassA; + iA = pc->invIA; + } + + float mB = 0.0f; + float iB = 0.0f; + if (indexB == toiIndexA || indexB == toiIndexB) + { + mB = pc->invMassB; + iB = pc->invIB; + } + + b2Vec2 cA = m_positions[indexA].c; + float aA = m_positions[indexA].a; + + b2Vec2 cB = m_positions[indexB].c; + float 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; + float 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. + float C = b2Clamp(b2_toiBaumgarte * (separation + b2_linearSlop), -b2_maxLinearCorrection, 0.0f); + + // Compute the effective mass. + float rnA = b2Cross(rA, normal); + float rnB = b2Cross(rB, normal); + float K = mA + mB + iA * rnA * rnA + iB * rnB * rnB; + + // Compute normal impulse + float 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/libs/box2d/src/dynamics/b2_contact_solver.h b/libs/box2d/src/dynamics/b2_contact_solver.h new file mode 100644 index 00000000..1064738e --- /dev/null +++ b/libs/box2d/src/dynamics/b2_contact_solver.h @@ -0,0 +1,100 @@ +// MIT License + +// Copyright (c) 2019 Erin Catto + +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: + +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. + +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. + +#ifndef B2_CONTACT_SOLVER_H +#define B2_CONTACT_SOLVER_H + +#include "box2d/b2_collision.h" +#include "box2d/b2_math.h" +#include "box2d/b2_time_step.h" + +class b2Contact; +class b2Body; +class b2StackAllocator; +struct b2ContactPositionConstraint; + +struct b2VelocityConstraintPoint +{ + b2Vec2 rA; + b2Vec2 rB; + float normalImpulse; + float tangentImpulse; + float normalMass; + float tangentMass; + float velocityBias; +}; + +struct b2ContactVelocityConstraint +{ + b2VelocityConstraintPoint points[b2_maxManifoldPoints]; + b2Vec2 normal; + b2Mat22 normalMass; + b2Mat22 K; + int32 indexA; + int32 indexB; + float invMassA, invMassB; + float invIA, invIB; + float friction; + float restitution; + float threshold; + float 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/libs/box2d/src/dynamics/b2_distance_joint.cpp b/libs/box2d/src/dynamics/b2_distance_joint.cpp new file mode 100644 index 00000000..221309ef --- /dev/null +++ b/libs/box2d/src/dynamics/b2_distance_joint.cpp @@ -0,0 +1,421 @@ +// MIT License + +// Copyright (c) 2019 Erin Catto + +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: + +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. + +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. + +#include "box2d/b2_body.h" +#include "box2d/b2_draw.h" +#include "box2d/b2_distance_joint.h" +#include "box2d/b2_time_step.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 = b2Max(d.Length(), b2_linearSlop); + minLength = length; + maxLength = length; +} + +b2DistanceJoint::b2DistanceJoint(const b2DistanceJointDef* def) +: b2Joint(def) +{ + m_localAnchorA = def->localAnchorA; + m_localAnchorB = def->localAnchorB; + m_length = b2Max(def->length, b2_linearSlop); + m_minLength = b2Max(def->minLength, b2_linearSlop); + m_maxLength = b2Max(def->maxLength, m_minLength); + m_stiffness = def->stiffness; + m_damping = def->damping; + + m_gamma = 0.0f; + m_bias = 0.0f; + m_impulse = 0.0f; + m_lowerImpulse = 0.0f; + m_upperImpulse = 0.0f; + m_currentLength = 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; + float aA = data.positions[m_indexA].a; + b2Vec2 vA = data.velocities[m_indexA].v; + float wA = data.velocities[m_indexA].w; + + b2Vec2 cB = data.positions[m_indexB].c; + float aB = data.positions[m_indexB].a; + b2Vec2 vB = data.velocities[m_indexB].v; + float 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. + m_currentLength = m_u.Length(); + if (m_currentLength > b2_linearSlop) + { + m_u *= 1.0f / m_currentLength; + } + else + { + m_u.Set(0.0f, 0.0f); + m_mass = 0.0f; + m_impulse = 0.0f; + m_lowerImpulse = 0.0f; + m_upperImpulse = 0.0f; + } + + float crAu = b2Cross(m_rA, m_u); + float crBu = b2Cross(m_rB, m_u); + float invMass = m_invMassA + m_invIA * crAu * crAu + m_invMassB + m_invIB * crBu * crBu; + m_mass = invMass != 0.0f ? 1.0f / invMass : 0.0f; + + if (m_stiffness > 0.0f && m_minLength < m_maxLength) + { + // soft + float C = m_currentLength - m_length; + + float d = m_damping; + float k = m_stiffness; + + // magic formulas + float h = data.step.dt; + + // gamma = 1 / (h * (d + h * k)) + // the extra factor of h in the denominator is since the lambda is an impulse, not a force + 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_softMass = invMass != 0.0f ? 1.0f / invMass : 0.0f; + } + else + { + // rigid + m_gamma = 0.0f; + m_bias = 0.0f; + m_softMass = m_mass; + } + + if (data.step.warmStarting) + { + // Scale the impulse to support a variable time step. + m_impulse *= data.step.dtRatio; + m_lowerImpulse *= data.step.dtRatio; + m_upperImpulse *= data.step.dtRatio; + + b2Vec2 P = (m_impulse + m_lowerImpulse - m_upperImpulse) * 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; + float wA = data.velocities[m_indexA].w; + b2Vec2 vB = data.velocities[m_indexB].v; + float wB = data.velocities[m_indexB].w; + + if (m_minLength < m_maxLength) + { + if (m_stiffness > 0.0f) + { + // Cdot = dot(u, v + cross(w, r)) + b2Vec2 vpA = vA + b2Cross(wA, m_rA); + b2Vec2 vpB = vB + b2Cross(wB, m_rB); + float Cdot = b2Dot(m_u, vpB - vpA); + + float impulse = -m_softMass * (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); + } + + // lower + { + float C = m_currentLength - m_minLength; + float bias = b2Max(0.0f, C) * data.step.inv_dt; + + b2Vec2 vpA = vA + b2Cross(wA, m_rA); + b2Vec2 vpB = vB + b2Cross(wB, m_rB); + float Cdot = b2Dot(m_u, vpB - vpA); + + float impulse = -m_mass * (Cdot + bias); + float oldImpulse = m_lowerImpulse; + m_lowerImpulse = b2Max(0.0f, m_lowerImpulse + impulse); + impulse = m_lowerImpulse - 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); + } + + // upper + { + float C = m_maxLength - m_currentLength; + float bias = b2Max(0.0f, C) * data.step.inv_dt; + + b2Vec2 vpA = vA + b2Cross(wA, m_rA); + b2Vec2 vpB = vB + b2Cross(wB, m_rB); + float Cdot = b2Dot(m_u, vpA - vpB); + + float impulse = -m_mass * (Cdot + bias); + float oldImpulse = m_upperImpulse; + m_upperImpulse = b2Max(0.0f, m_upperImpulse + impulse); + impulse = m_upperImpulse - 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); + } + } + else + { + // Equal limits + + // Cdot = dot(u, v + cross(w, r)) + b2Vec2 vpA = vA + b2Cross(wA, m_rA); + b2Vec2 vpB = vB + b2Cross(wB, m_rB); + float Cdot = b2Dot(m_u, vpB - vpA); + + float impulse = -m_mass * Cdot; + 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) +{ + b2Vec2 cA = data.positions[m_indexA].c; + float aA = data.positions[m_indexA].a; + b2Vec2 cB = data.positions[m_indexB].c; + float 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; + + float length = u.Normalize(); + float C; + if (m_minLength == m_maxLength) + { + C = length - m_minLength; + } + else if (length < m_minLength) + { + C = length - m_minLength; + } + else if (m_maxLength < length) + { + C = length - m_maxLength; + } + else + { + return true; + } + + float 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(float inv_dt) const +{ + b2Vec2 F = inv_dt * (m_impulse + m_lowerImpulse - m_upperImpulse) * m_u; + return F; +} + +float b2DistanceJoint::GetReactionTorque(float inv_dt) const +{ + B2_NOT_USED(inv_dt); + return 0.0f; +} + +float b2DistanceJoint::SetLength(float length) +{ + m_impulse = 0.0f; + m_length = b2Max(b2_linearSlop, length); + return m_length; +} + +float b2DistanceJoint::SetMinLength(float minLength) +{ + m_lowerImpulse = 0.0f; + m_minLength = b2Clamp(minLength, b2_linearSlop, m_maxLength); + return m_minLength; +} + +float b2DistanceJoint::SetMaxLength(float maxLength) +{ + m_upperImpulse = 0.0f; + m_maxLength = b2Max(maxLength, m_minLength); + return m_maxLength; +} + +float b2DistanceJoint::GetCurrentLength() const +{ + b2Vec2 pA = m_bodyA->GetWorldPoint(m_localAnchorA); + b2Vec2 pB = m_bodyB->GetWorldPoint(m_localAnchorB); + b2Vec2 d = pB - pA; + float length = d.Length(); + return length; +} + +void b2DistanceJoint::Dump() +{ + int32 indexA = m_bodyA->m_islandIndex; + int32 indexB = m_bodyB->m_islandIndex; + + b2Dump(" b2DistanceJointDef jd;\n"); + b2Dump(" jd.bodyA = bodies[%d];\n", indexA); + b2Dump(" jd.bodyB = bodies[%d];\n", indexB); + b2Dump(" jd.collideConnected = bool(%d);\n", m_collideConnected); + b2Dump(" jd.localAnchorA.Set(%.9g, %.9g);\n", m_localAnchorA.x, m_localAnchorA.y); + b2Dump(" jd.localAnchorB.Set(%.9g, %.9g);\n", m_localAnchorB.x, m_localAnchorB.y); + b2Dump(" jd.length = %.9g;\n", m_length); + b2Dump(" jd.minLength = %.9g;\n", m_minLength); + b2Dump(" jd.maxLength = %.9g;\n", m_maxLength); + b2Dump(" jd.stiffness = %.9g;\n", m_stiffness); + b2Dump(" jd.damping = %.9g;\n", m_damping); + b2Dump(" joints[%d] = m_world->CreateJoint(&jd);\n", m_index); +} + +void b2DistanceJoint::Draw(b2Draw* draw) const +{ + const b2Transform& xfA = m_bodyA->GetTransform(); + const b2Transform& xfB = m_bodyB->GetTransform(); + b2Vec2 pA = b2Mul(xfA, m_localAnchorA); + b2Vec2 pB = b2Mul(xfB, m_localAnchorB); + + b2Vec2 axis = pB - pA; + axis.Normalize(); + + b2Color c1(0.7f, 0.7f, 0.7f); + b2Color c2(0.3f, 0.9f, 0.3f); + b2Color c3(0.9f, 0.3f, 0.3f); + b2Color c4(0.4f, 0.4f, 0.4f); + + draw->DrawSegment(pA, pB, c4); + + b2Vec2 pRest = pA + m_length * axis; + draw->DrawPoint(pRest, 8.0f, c1); + + if (m_minLength != m_maxLength) + { + if (m_minLength > b2_linearSlop) + { + b2Vec2 pMin = pA + m_minLength * axis; + draw->DrawPoint(pMin, 4.0f, c2); + } + + if (m_maxLength < FLT_MAX) + { + b2Vec2 pMax = pA + m_maxLength * axis; + draw->DrawPoint(pMax, 4.0f, c3); + } + } +} diff --git a/libs/box2d/src/dynamics/b2_edge_circle_contact.cpp b/libs/box2d/src/dynamics/b2_edge_circle_contact.cpp new file mode 100644 index 00000000..8a126de6 --- /dev/null +++ b/libs/box2d/src/dynamics/b2_edge_circle_contact.cpp @@ -0,0 +1,54 @@ +// MIT License + +// Copyright (c) 2019 Erin Catto + +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: + +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. + +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. + +#include "b2_edge_circle_contact.h" + +#include "box2d/b2_block_allocator.h" +#include "box2d/b2_fixture.h" + +#include + +b2Contact* b2EdgeAndCircleContact::Create(b2Fixture* fixtureA, int32, b2Fixture* fixtureB, int32, b2BlockAllocator* allocator) +{ + void* mem = allocator->Allocate(sizeof(b2EdgeAndCircleContact)); + return new (mem) b2EdgeAndCircleContact(fixtureA, fixtureB); +} + +void b2EdgeAndCircleContact::Destroy(b2Contact* contact, b2BlockAllocator* allocator) +{ + ((b2EdgeAndCircleContact*)contact)->~b2EdgeAndCircleContact(); + allocator->Free(contact, sizeof(b2EdgeAndCircleContact)); +} + +b2EdgeAndCircleContact::b2EdgeAndCircleContact(b2Fixture* fixtureA, b2Fixture* fixtureB) +: b2Contact(fixtureA, 0, fixtureB, 0) +{ + b2Assert(m_fixtureA->GetType() == b2Shape::e_edge); + b2Assert(m_fixtureB->GetType() == b2Shape::e_circle); +} + +void b2EdgeAndCircleContact::Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB) +{ + b2CollideEdgeAndCircle( manifold, + (b2EdgeShape*)m_fixtureA->GetShape(), xfA, + (b2CircleShape*)m_fixtureB->GetShape(), xfB); +} diff --git a/libs/box2d/src/dynamics/b2_edge_circle_contact.h b/libs/box2d/src/dynamics/b2_edge_circle_contact.h new file mode 100644 index 00000000..3efc88e6 --- /dev/null +++ b/libs/box2d/src/dynamics/b2_edge_circle_contact.h @@ -0,0 +1,43 @@ +// MIT License + +// Copyright (c) 2019 Erin Catto + +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: + +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. + +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. + +#ifndef B2_EDGE_AND_CIRCLE_CONTACT_H +#define B2_EDGE_AND_CIRCLE_CONTACT_H + +#include "box2d/b2_contact.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/libs/box2d/src/dynamics/b2_edge_polygon_contact.cpp b/libs/box2d/src/dynamics/b2_edge_polygon_contact.cpp new file mode 100644 index 00000000..e617e351 --- /dev/null +++ b/libs/box2d/src/dynamics/b2_edge_polygon_contact.cpp @@ -0,0 +1,54 @@ +// MIT License + +// Copyright (c) 2019 Erin Catto + +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: + +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. + +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. + +#include "b2_edge_polygon_contact.h" + +#include "box2d/b2_block_allocator.h" +#include "box2d/b2_fixture.h" + +#include + +b2Contact* b2EdgeAndPolygonContact::Create(b2Fixture* fixtureA, int32, b2Fixture* fixtureB, int32, b2BlockAllocator* allocator) +{ + void* mem = allocator->Allocate(sizeof(b2EdgeAndPolygonContact)); + return new (mem) b2EdgeAndPolygonContact(fixtureA, fixtureB); +} + +void b2EdgeAndPolygonContact::Destroy(b2Contact* contact, b2BlockAllocator* allocator) +{ + ((b2EdgeAndPolygonContact*)contact)->~b2EdgeAndPolygonContact(); + allocator->Free(contact, sizeof(b2EdgeAndPolygonContact)); +} + +b2EdgeAndPolygonContact::b2EdgeAndPolygonContact(b2Fixture* fixtureA, b2Fixture* fixtureB) +: b2Contact(fixtureA, 0, fixtureB, 0) +{ + b2Assert(m_fixtureA->GetType() == b2Shape::e_edge); + b2Assert(m_fixtureB->GetType() == b2Shape::e_polygon); +} + +void b2EdgeAndPolygonContact::Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB) +{ + b2CollideEdgeAndPolygon( manifold, + (b2EdgeShape*)m_fixtureA->GetShape(), xfA, + (b2PolygonShape*)m_fixtureB->GetShape(), xfB); +} diff --git a/libs/box2d/src/dynamics/b2_edge_polygon_contact.h b/libs/box2d/src/dynamics/b2_edge_polygon_contact.h new file mode 100644 index 00000000..e6616b53 --- /dev/null +++ b/libs/box2d/src/dynamics/b2_edge_polygon_contact.h @@ -0,0 +1,43 @@ +// MIT License + +// Copyright (c) 2019 Erin Catto + +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: + +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. + +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. + +#ifndef B2_EDGE_AND_POLYGON_CONTACT_H +#define B2_EDGE_AND_POLYGON_CONTACT_H + +#include "box2d/b2_contact.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/libs/Box2D/Dynamics/b2Fixture.cpp b/libs/box2d/src/dynamics/b2_fixture.cpp similarity index 52% rename from libs/Box2D/Dynamics/b2Fixture.cpp rename to libs/box2d/src/dynamics/b2_fixture.cpp index d97aa448..9fd700aa 100644 --- a/libs/Box2D/Dynamics/b2Fixture.cpp +++ b/libs/box2d/src/dynamics/b2_fixture.cpp @@ -1,303 +1,305 @@ -/* -* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -b2Fixture::b2Fixture() -{ - m_userData = NULL; - m_body = NULL; - m_next = NULL; - m_proxies = NULL; - m_proxyCount = 0; - m_shape = NULL; - m_density = 0.0f; -} - -void b2Fixture::Create(b2BlockAllocator* allocator, b2Body* body, const b2FixtureDef* def) -{ - m_userData = def->userData; - m_friction = def->friction; - m_restitution = def->restitution; - - m_body = body; - m_next = NULL; - - m_filter = def->filter; - - m_isSensor = def->isSensor; - - m_shape = def->shape->Clone(allocator); - - // Reserve proxy space - int32 childCount = m_shape->GetChildCount(); - m_proxies = (b2FixtureProxy*)allocator->Allocate(childCount * sizeof(b2FixtureProxy)); - for (int32 i = 0; i < childCount; ++i) - { - m_proxies[i].fixture = NULL; - m_proxies[i].proxyId = b2BroadPhase::e_nullProxy; - } - m_proxyCount = 0; - - m_density = def->density; -} - -void b2Fixture::Destroy(b2BlockAllocator* allocator) -{ - // The proxies must be destroyed before calling this. - b2Assert(m_proxyCount == 0); - - // Free the proxy array. - int32 childCount = m_shape->GetChildCount(); - allocator->Free(m_proxies, childCount * sizeof(b2FixtureProxy)); - m_proxies = NULL; - - // Free the child shape. - switch (m_shape->m_type) - { - case b2Shape::e_circle: - { - b2CircleShape* s = (b2CircleShape*)m_shape; - s->~b2CircleShape(); - allocator->Free(s, sizeof(b2CircleShape)); - } - break; - - case b2Shape::e_edge: - { - b2EdgeShape* s = (b2EdgeShape*)m_shape; - s->~b2EdgeShape(); - allocator->Free(s, sizeof(b2EdgeShape)); - } - break; - - case b2Shape::e_polygon: - { - b2PolygonShape* s = (b2PolygonShape*)m_shape; - s->~b2PolygonShape(); - allocator->Free(s, sizeof(b2PolygonShape)); - } - break; - - case b2Shape::e_chain: - { - b2ChainShape* s = (b2ChainShape*)m_shape; - s->~b2ChainShape(); - allocator->Free(s, sizeof(b2ChainShape)); - } - break; - - default: - b2Assert(false); - break; - } - - m_shape = NULL; -} - -void b2Fixture::CreateProxies(b2BroadPhase* broadPhase, const b2Transform& xf) -{ - b2Assert(m_proxyCount == 0); - - // Create proxies in the broad-phase. - m_proxyCount = m_shape->GetChildCount(); - - for (int32 i = 0; i < m_proxyCount; ++i) - { - b2FixtureProxy* proxy = m_proxies + i; - m_shape->ComputeAABB(&proxy->aabb, xf, i); - proxy->proxyId = broadPhase->CreateProxy(proxy->aabb, proxy); - proxy->fixture = this; - proxy->childIndex = i; - } -} - -void b2Fixture::DestroyProxies(b2BroadPhase* broadPhase) -{ - // Destroy proxies in the broad-phase. - for (int32 i = 0; i < m_proxyCount; ++i) - { - b2FixtureProxy* proxy = m_proxies + i; - broadPhase->DestroyProxy(proxy->proxyId); - proxy->proxyId = b2BroadPhase::e_nullProxy; - } - - m_proxyCount = 0; -} - -void b2Fixture::Synchronize(b2BroadPhase* broadPhase, const b2Transform& transform1, const b2Transform& transform2) -{ - if (m_proxyCount == 0) - { - return; - } - - for (int32 i = 0; i < m_proxyCount; ++i) - { - b2FixtureProxy* proxy = m_proxies + i; - - // Compute an AABB that covers the swept shape (may miss some rotation effect). - b2AABB aabb1, aabb2; - m_shape->ComputeAABB(&aabb1, transform1, proxy->childIndex); - m_shape->ComputeAABB(&aabb2, transform2, proxy->childIndex); - - proxy->aabb.Combine(aabb1, aabb2); - - b2Vec2 displacement = transform2.p - transform1.p; - - broadPhase->MoveProxy(proxy->proxyId, proxy->aabb, displacement); - } -} - -void b2Fixture::SetFilterData(const b2Filter& filter) -{ - m_filter = filter; - - Refilter(); -} - -void b2Fixture::Refilter() -{ - if (m_body == NULL) - { - return; - } - - // Flag associated contacts for filtering. - b2ContactEdge* edge = m_body->GetContactList(); - while (edge) - { - b2Contact* contact = edge->contact; - b2Fixture* fixtureA = contact->GetFixtureA(); - b2Fixture* fixtureB = contact->GetFixtureB(); - if (fixtureA == this || fixtureB == this) - { - contact->FlagForFiltering(); - } - - edge = edge->next; - } - - b2World* world = m_body->GetWorld(); - - if (world == NULL) - { - return; - } - - // Touch each proxy so that new pairs may be created - b2BroadPhase* broadPhase = &world->m_contactManager.m_broadPhase; - for (int32 i = 0; i < m_proxyCount; ++i) - { - broadPhase->TouchProxy(m_proxies[i].proxyId); - } -} - -void b2Fixture::SetSensor(bool sensor) -{ - if (sensor != m_isSensor) - { - m_body->SetAwake(true); - m_isSensor = sensor; - } -} - -void b2Fixture::Dump(int32 bodyIndex) -{ - b2Log(" b2FixtureDef fd;\n"); - b2Log(" fd.friction = %.15lef;\n", m_friction); - b2Log(" fd.restitution = %.15lef;\n", m_restitution); - b2Log(" fd.density = %.15lef;\n", m_density); - b2Log(" fd.isSensor = bool(%d);\n", m_isSensor); - b2Log(" fd.filter.categoryBits = uint16(%d);\n", m_filter.categoryBits); - b2Log(" fd.filter.maskBits = uint16(%d);\n", m_filter.maskBits); - b2Log(" fd.filter.groupIndex = int16(%d);\n", m_filter.groupIndex); - - switch (m_shape->m_type) - { - case b2Shape::e_circle: - { - b2CircleShape* s = (b2CircleShape*)m_shape; - b2Log(" b2CircleShape shape;\n"); - b2Log(" shape.m_radius = %.15lef;\n", s->m_radius); - b2Log(" shape.m_p.Set(%.15lef, %.15lef);\n", s->m_p.x, s->m_p.y); - } - break; - - case b2Shape::e_edge: - { - b2EdgeShape* s = (b2EdgeShape*)m_shape; - b2Log(" b2EdgeShape shape;\n"); - b2Log(" shape.m_radius = %.15lef;\n", s->m_radius); - b2Log(" shape.m_vertex0.Set(%.15lef, %.15lef);\n", s->m_vertex0.x, s->m_vertex0.y); - b2Log(" shape.m_vertex1.Set(%.15lef, %.15lef);\n", s->m_vertex1.x, s->m_vertex1.y); - b2Log(" shape.m_vertex2.Set(%.15lef, %.15lef);\n", s->m_vertex2.x, s->m_vertex2.y); - b2Log(" shape.m_vertex3.Set(%.15lef, %.15lef);\n", s->m_vertex3.x, s->m_vertex3.y); - b2Log(" shape.m_hasVertex0 = bool(%d);\n", s->m_hasVertex0); - b2Log(" shape.m_hasVertex3 = bool(%d);\n", s->m_hasVertex3); - } - break; - - case b2Shape::e_polygon: - { - b2PolygonShape* s = (b2PolygonShape*)m_shape; - b2Log(" b2PolygonShape shape;\n"); - b2Log(" b2Vec2 vs[%d];\n", b2_maxPolygonVertices); - for (int32 i = 0; i < s->m_count; ++i) - { - b2Log(" vs[%d].Set(%.15lef, %.15lef);\n", i, s->m_vertices[i].x, s->m_vertices[i].y); - } - b2Log(" shape.Set(vs, %d);\n", s->m_count); - } - break; - - case b2Shape::e_chain: - { - b2ChainShape* s = (b2ChainShape*)m_shape; - b2Log(" b2ChainShape shape;\n"); - b2Log(" b2Vec2 vs[%d];\n", s->m_count); - for (int32 i = 0; i < s->m_count; ++i) - { - b2Log(" vs[%d].Set(%.15lef, %.15lef);\n", i, s->m_vertices[i].x, s->m_vertices[i].y); - } - b2Log(" shape.CreateChain(vs, %d);\n", s->m_count); - b2Log(" shape.m_prevVertex.Set(%.15lef, %.15lef);\n", s->m_prevVertex.x, s->m_prevVertex.y); - b2Log(" shape.m_nextVertex.Set(%.15lef, %.15lef);\n", s->m_nextVertex.x, s->m_nextVertex.y); - b2Log(" shape.m_hasPrevVertex = bool(%d);\n", s->m_hasPrevVertex); - b2Log(" shape.m_hasNextVertex = bool(%d);\n", s->m_hasNextVertex); - } - break; - - default: - return; - } - - b2Log("\n"); - b2Log(" fd.shape = &shape;\n"); - b2Log("\n"); - b2Log(" bodies[%d]->CreateFixture(&fd);\n", bodyIndex); -} +// MIT License + +// Copyright (c) 2019 Erin Catto + +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: + +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. + +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. + +#include "box2d/b2_fixture.h" +#include "box2d/b2_block_allocator.h" +#include "box2d/b2_broad_phase.h" +#include "box2d/b2_chain_shape.h" +#include "box2d/b2_circle_shape.h" +#include "box2d/b2_collision.h" +#include "box2d/b2_contact.h" +#include "box2d/b2_edge_shape.h" +#include "box2d/b2_polygon_shape.h" +#include "box2d/b2_world.h" + +b2Fixture::b2Fixture() +{ + 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_restitutionThreshold = def->restitutionThreshold; + + 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 = aabb2.GetCenter() - aabb1.GetCenter(); + + 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) +{ + b2Dump(" b2FixtureDef fd;\n"); + b2Dump(" fd.friction = %.9g;\n", m_friction); + b2Dump(" fd.restitution = %.9g;\n", m_restitution); + b2Dump(" fd.restitutionThreshold = %.9g;\n", m_restitutionThreshold); + b2Dump(" fd.density = %.9g;\n", m_density); + b2Dump(" fd.isSensor = bool(%d);\n", m_isSensor); + b2Dump(" fd.filter.categoryBits = uint16(%d);\n", m_filter.categoryBits); + b2Dump(" fd.filter.maskBits = uint16(%d);\n", m_filter.maskBits); + b2Dump(" fd.filter.groupIndex = int16(%d);\n", m_filter.groupIndex); + + switch (m_shape->m_type) + { + case b2Shape::e_circle: + { + b2CircleShape* s = (b2CircleShape*)m_shape; + b2Dump(" b2CircleShape shape;\n"); + b2Dump(" shape.m_radius = %.9g;\n", s->m_radius); + b2Dump(" shape.m_p.Set(%.9g, %.9g);\n", s->m_p.x, s->m_p.y); + } + break; + + case b2Shape::e_edge: + { + b2EdgeShape* s = (b2EdgeShape*)m_shape; + b2Dump(" b2EdgeShape shape;\n"); + b2Dump(" shape.m_radius = %.9g;\n", s->m_radius); + b2Dump(" shape.m_vertex0.Set(%.9g, %.9g);\n", s->m_vertex0.x, s->m_vertex0.y); + b2Dump(" shape.m_vertex1.Set(%.9g, %.9g);\n", s->m_vertex1.x, s->m_vertex1.y); + b2Dump(" shape.m_vertex2.Set(%.9g, %.9g);\n", s->m_vertex2.x, s->m_vertex2.y); + b2Dump(" shape.m_vertex3.Set(%.9g, %.9g);\n", s->m_vertex3.x, s->m_vertex3.y); + b2Dump(" shape.m_oneSided = bool(%d);\n", s->m_oneSided); + } + break; + + case b2Shape::e_polygon: + { + b2PolygonShape* s = (b2PolygonShape*)m_shape; + b2Dump(" b2PolygonShape shape;\n"); + b2Dump(" b2Vec2 vs[%d];\n", b2_maxPolygonVertices); + for (int32 i = 0; i < s->m_count; ++i) + { + b2Dump(" vs[%d].Set(%.9g, %.9g);\n", i, s->m_vertices[i].x, s->m_vertices[i].y); + } + b2Dump(" shape.Set(vs, %d);\n", s->m_count); + } + break; + + case b2Shape::e_chain: + { + b2ChainShape* s = (b2ChainShape*)m_shape; + b2Dump(" b2ChainShape shape;\n"); + b2Dump(" b2Vec2 vs[%d];\n", s->m_count); + for (int32 i = 0; i < s->m_count; ++i) + { + b2Dump(" vs[%d].Set(%.9g, %.9g);\n", i, s->m_vertices[i].x, s->m_vertices[i].y); + } + b2Dump(" shape.CreateChain(vs, %d);\n", s->m_count); + b2Dump(" shape.m_prevVertex.Set(%.9g, %.9g);\n", s->m_prevVertex.x, s->m_prevVertex.y); + b2Dump(" shape.m_nextVertex.Set(%.9g, %.9g);\n", s->m_nextVertex.x, s->m_nextVertex.y); + } + break; + + default: + return; + } + + b2Dump("\n"); + b2Dump(" fd.shape = &shape;\n"); + b2Dump("\n"); + b2Dump(" bodies[%d]->CreateFixture(&fd);\n", bodyIndex); +} diff --git a/libs/Box2D/Dynamics/Joints/b2FrictionJoint.cpp b/libs/box2d/src/dynamics/b2_friction_joint.cpp similarity index 61% rename from libs/Box2D/Dynamics/Joints/b2FrictionJoint.cpp rename to libs/box2d/src/dynamics/b2_friction_joint.cpp index 075e72bb..d9d893ad 100644 --- a/libs/Box2D/Dynamics/Joints/b2FrictionJoint.cpp +++ b/libs/box2d/src/dynamics/b2_friction_joint.cpp @@ -1,251 +1,255 @@ -/* -* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ - -#include -#include -#include - -// Point-to-point constraint -// Cdot = v2 - v1 -// = v2 + cross(w2, r2) - v1 - cross(w1, r1) -// J = [-I -r1_skew I r2_skew ] -// Identity used: -// w k % (rx i + ry j) = w * (-ry i + rx j) - -// Angle constraint -// Cdot = w2 - w1 -// J = [0 0 -1 0 0 1] -// K = invI1 + invI2 - -void b2FrictionJointDef::Initialize(b2Body* bA, b2Body* bB, const b2Vec2& anchor) -{ - bodyA = bA; - bodyB = bB; - localAnchorA = bodyA->GetLocalPoint(anchor); - localAnchorB = bodyB->GetLocalPoint(anchor); -} - -b2FrictionJoint::b2FrictionJoint(const b2FrictionJointDef* def) -: b2Joint(def) -{ - m_localAnchorA = def->localAnchorA; - m_localAnchorB = def->localAnchorB; - - m_linearImpulse.SetZero(); - m_angularImpulse = 0.0f; - - m_maxForce = def->maxForce; - m_maxTorque = def->maxTorque; -} - -void b2FrictionJoint::InitVelocityConstraints(const b2SolverData& data) -{ - m_indexA = m_bodyA->m_islandIndex; - m_indexB = m_bodyB->m_islandIndex; - m_localCenterA = m_bodyA->m_sweep.localCenter; - m_localCenterB = m_bodyB->m_sweep.localCenter; - m_invMassA = m_bodyA->m_invMass; - m_invMassB = m_bodyB->m_invMass; - m_invIA = m_bodyA->m_invI; - m_invIB = m_bodyB->m_invI; - - float32 aA = data.positions[m_indexA].a; - b2Vec2 vA = data.velocities[m_indexA].v; - float32 wA = data.velocities[m_indexA].w; - - float32 aB = data.positions[m_indexB].a; - b2Vec2 vB = data.velocities[m_indexB].v; - float32 wB = data.velocities[m_indexB].w; - - b2Rot qA(aA), qB(aB); - - // Compute the effective mass matrix. - m_rA = b2Mul(qA, m_localAnchorA - m_localCenterA); - m_rB = b2Mul(qB, m_localAnchorB - m_localCenterB); - - // J = [-I -r1_skew I r2_skew] - // [ 0 -1 0 1] - // r_skew = [-ry; rx] - - // Matlab - // K = [ mA+r1y^2*iA+mB+r2y^2*iB, -r1y*iA*r1x-r2y*iB*r2x, -r1y*iA-r2y*iB] - // [ -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB, r1x*iA+r2x*iB] - // [ -r1y*iA-r2y*iB, r1x*iA+r2x*iB, iA+iB] - - float32 mA = m_invMassA, mB = m_invMassB; - float32 iA = m_invIA, iB = m_invIB; - - b2Mat22 K; - K.ex.x = mA + mB + iA * m_rA.y * m_rA.y + iB * m_rB.y * m_rB.y; - K.ex.y = -iA * m_rA.x * m_rA.y - iB * m_rB.x * m_rB.y; - K.ey.x = K.ex.y; - K.ey.y = mA + mB + iA * m_rA.x * m_rA.x + iB * m_rB.x * m_rB.x; - - m_linearMass = K.GetInverse(); - - m_angularMass = iA + iB; - if (m_angularMass > 0.0f) - { - m_angularMass = 1.0f / m_angularMass; - } - - if (data.step.warmStarting) - { - // Scale impulses to support a variable time step. - m_linearImpulse *= data.step.dtRatio; - m_angularImpulse *= data.step.dtRatio; - - b2Vec2 P(m_linearImpulse.x, m_linearImpulse.y); - vA -= mA * P; - wA -= iA * (b2Cross(m_rA, P) + m_angularImpulse); - vB += mB * P; - wB += iB * (b2Cross(m_rB, P) + m_angularImpulse); - } - else - { - m_linearImpulse.SetZero(); - m_angularImpulse = 0.0f; - } - - data.velocities[m_indexA].v = vA; - data.velocities[m_indexA].w = wA; - data.velocities[m_indexB].v = vB; - data.velocities[m_indexB].w = wB; -} - -void b2FrictionJoint::SolveVelocityConstraints(const b2SolverData& data) -{ - b2Vec2 vA = data.velocities[m_indexA].v; - float32 wA = data.velocities[m_indexA].w; - b2Vec2 vB = data.velocities[m_indexB].v; - float32 wB = data.velocities[m_indexB].w; - - float32 mA = m_invMassA, mB = m_invMassB; - float32 iA = m_invIA, iB = m_invIB; - - float32 h = data.step.dt; - - // Solve angular friction - { - float32 Cdot = wB - wA; - float32 impulse = -m_angularMass * Cdot; - - float32 oldImpulse = m_angularImpulse; - float32 maxImpulse = h * m_maxTorque; - m_angularImpulse = b2Clamp(m_angularImpulse + impulse, -maxImpulse, maxImpulse); - impulse = m_angularImpulse - oldImpulse; - - wA -= iA * impulse; - wB += iB * impulse; - } - - // Solve linear friction - { - b2Vec2 Cdot = vB + b2Cross(wB, m_rB) - vA - b2Cross(wA, m_rA); - - b2Vec2 impulse = -b2Mul(m_linearMass, Cdot); - b2Vec2 oldImpulse = m_linearImpulse; - m_linearImpulse += impulse; - - float32 maxImpulse = h * m_maxForce; - - if (m_linearImpulse.LengthSquared() > maxImpulse * maxImpulse) - { - m_linearImpulse.Normalize(); - m_linearImpulse *= maxImpulse; - } - - impulse = m_linearImpulse - oldImpulse; - - vA -= mA * impulse; - wA -= iA * b2Cross(m_rA, impulse); - - vB += mB * impulse; - wB += iB * b2Cross(m_rB, impulse); - } - - data.velocities[m_indexA].v = vA; - data.velocities[m_indexA].w = wA; - data.velocities[m_indexB].v = vB; - data.velocities[m_indexB].w = wB; -} - -bool b2FrictionJoint::SolvePositionConstraints(const b2SolverData& data) -{ - B2_NOT_USED(data); - - return true; -} - -b2Vec2 b2FrictionJoint::GetAnchorA() const -{ - return m_bodyA->GetWorldPoint(m_localAnchorA); -} - -b2Vec2 b2FrictionJoint::GetAnchorB() const -{ - return m_bodyB->GetWorldPoint(m_localAnchorB); -} - -b2Vec2 b2FrictionJoint::GetReactionForce(float32 inv_dt) const -{ - return inv_dt * m_linearImpulse; -} - -float32 b2FrictionJoint::GetReactionTorque(float32 inv_dt) const -{ - return inv_dt * m_angularImpulse; -} - -void b2FrictionJoint::SetMaxForce(float32 force) -{ - b2Assert(b2IsValid(force) && force >= 0.0f); - m_maxForce = force; -} - -float32 b2FrictionJoint::GetMaxForce() const -{ - return m_maxForce; -} - -void b2FrictionJoint::SetMaxTorque(float32 torque) -{ - b2Assert(b2IsValid(torque) && torque >= 0.0f); - m_maxTorque = torque; -} - -float32 b2FrictionJoint::GetMaxTorque() const -{ - return m_maxTorque; -} - -void b2FrictionJoint::Dump() -{ - int32 indexA = m_bodyA->m_islandIndex; - int32 indexB = m_bodyB->m_islandIndex; - - b2Log(" b2FrictionJointDef jd;\n"); - b2Log(" jd.bodyA = bodies[%d];\n", indexA); - b2Log(" jd.bodyB = bodies[%d];\n", indexB); - b2Log(" jd.collideConnected = bool(%d);\n", m_collideConnected); - b2Log(" jd.localAnchorA.Set(%.15lef, %.15lef);\n", m_localAnchorA.x, m_localAnchorA.y); - b2Log(" jd.localAnchorB.Set(%.15lef, %.15lef);\n", m_localAnchorB.x, m_localAnchorB.y); - b2Log(" jd.maxForce = %.15lef;\n", m_maxForce); - b2Log(" jd.maxTorque = %.15lef;\n", m_maxTorque); - b2Log(" joints[%d] = m_world->CreateJoint(&jd);\n", m_index); -} +// MIT License + +// Copyright (c) 2019 Erin Catto + +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: + +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. + +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. + +#include "box2d/b2_friction_joint.h" +#include "box2d/b2_body.h" +#include "box2d/b2_time_step.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; + + float aA = data.positions[m_indexA].a; + b2Vec2 vA = data.velocities[m_indexA].v; + float wA = data.velocities[m_indexA].w; + + float aB = data.positions[m_indexB].a; + b2Vec2 vB = data.velocities[m_indexB].v; + float 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] + + float mA = m_invMassA, mB = m_invMassB; + float 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; + float wA = data.velocities[m_indexA].w; + b2Vec2 vB = data.velocities[m_indexB].v; + float wB = data.velocities[m_indexB].w; + + float mA = m_invMassA, mB = m_invMassB; + float iA = m_invIA, iB = m_invIB; + + float h = data.step.dt; + + // Solve angular friction + { + float Cdot = wB - wA; + float impulse = -m_angularMass * Cdot; + + float oldImpulse = m_angularImpulse; + float 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; + + float 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(float inv_dt) const +{ + return inv_dt * m_linearImpulse; +} + +float b2FrictionJoint::GetReactionTorque(float inv_dt) const +{ + return inv_dt * m_angularImpulse; +} + +void b2FrictionJoint::SetMaxForce(float force) +{ + b2Assert(b2IsValid(force) && force >= 0.0f); + m_maxForce = force; +} + +float b2FrictionJoint::GetMaxForce() const +{ + return m_maxForce; +} + +void b2FrictionJoint::SetMaxTorque(float torque) +{ + b2Assert(b2IsValid(torque) && torque >= 0.0f); + m_maxTorque = torque; +} + +float b2FrictionJoint::GetMaxTorque() const +{ + return m_maxTorque; +} + +void b2FrictionJoint::Dump() +{ + int32 indexA = m_bodyA->m_islandIndex; + int32 indexB = m_bodyB->m_islandIndex; + + b2Dump(" b2FrictionJointDef jd;\n"); + b2Dump(" jd.bodyA = bodies[%d];\n", indexA); + b2Dump(" jd.bodyB = bodies[%d];\n", indexB); + b2Dump(" jd.collideConnected = bool(%d);\n", m_collideConnected); + b2Dump(" jd.localAnchorA.Set(%.9g, %.9g);\n", m_localAnchorA.x, m_localAnchorA.y); + b2Dump(" jd.localAnchorB.Set(%.9g, %.9g);\n", m_localAnchorB.x, m_localAnchorB.y); + b2Dump(" jd.maxForce = %.9g;\n", m_maxForce); + b2Dump(" jd.maxTorque = %.9g;\n", m_maxTorque); + b2Dump(" joints[%d] = m_world->CreateJoint(&jd);\n", m_index); +} diff --git a/libs/Box2D/Dynamics/Joints/b2GearJoint.cpp b/libs/box2d/src/dynamics/b2_gear_joint.cpp similarity index 71% rename from libs/Box2D/Dynamics/Joints/b2GearJoint.cpp rename to libs/box2d/src/dynamics/b2_gear_joint.cpp index 233d972a..5fb54724 100644 --- a/libs/Box2D/Dynamics/Joints/b2GearJoint.cpp +++ b/libs/box2d/src/dynamics/b2_gear_joint.cpp @@ -1,419 +1,437 @@ -/* -* Copyright (c) 2007-2011 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ - -#include -#include -#include -#include -#include - -// Gear Joint: -// C0 = (coordinate1 + ratio * coordinate2)_initial -// C = (coordinate1 + ratio * coordinate2) - C0 = 0 -// J = [J1 ratio * J2] -// K = J * invM * JT -// = J1 * invM1 * J1T + ratio * ratio * J2 * invM2 * J2T -// -// Revolute: -// coordinate = rotation -// Cdot = angularVelocity -// J = [0 0 1] -// K = J * invM * JT = invI -// -// Prismatic: -// coordinate = dot(p - pg, ug) -// Cdot = dot(v + cross(w, r), ug) -// J = [ug cross(r, ug)] -// K = J * invM * JT = invMass + invI * cross(r, ug)^2 - -b2GearJoint::b2GearJoint(const b2GearJointDef* def) -: b2Joint(def) -{ - m_joint1 = def->joint1; - m_joint2 = def->joint2; - - m_typeA = m_joint1->GetType(); - m_typeB = m_joint2->GetType(); - - b2Assert(m_typeA == e_revoluteJoint || m_typeA == e_prismaticJoint); - b2Assert(m_typeB == e_revoluteJoint || m_typeB == e_prismaticJoint); - - float32 coordinateA, coordinateB; - - // TODO_ERIN there might be some problem with the joint edges in b2Joint. - - m_bodyC = m_joint1->GetBodyA(); - m_bodyA = m_joint1->GetBodyB(); - - // Get geometry of joint1 - b2Transform xfA = m_bodyA->m_xf; - float32 aA = m_bodyA->m_sweep.a; - b2Transform xfC = m_bodyC->m_xf; - float32 aC = m_bodyC->m_sweep.a; - - if (m_typeA == e_revoluteJoint) - { - b2RevoluteJoint* revolute = (b2RevoluteJoint*)def->joint1; - m_localAnchorC = revolute->m_localAnchorA; - m_localAnchorA = revolute->m_localAnchorB; - m_referenceAngleA = revolute->m_referenceAngle; - m_localAxisC.SetZero(); - - coordinateA = aA - aC - m_referenceAngleA; - } - else - { - b2PrismaticJoint* prismatic = (b2PrismaticJoint*)def->joint1; - m_localAnchorC = prismatic->m_localAnchorA; - m_localAnchorA = prismatic->m_localAnchorB; - m_referenceAngleA = prismatic->m_referenceAngle; - m_localAxisC = prismatic->m_localXAxisA; - - b2Vec2 pC = m_localAnchorC; - b2Vec2 pA = b2MulT(xfC.q, b2Mul(xfA.q, m_localAnchorA) + (xfA.p - xfC.p)); - coordinateA = b2Dot(pA - pC, m_localAxisC); - } - - m_bodyD = m_joint2->GetBodyA(); - m_bodyB = m_joint2->GetBodyB(); - - // Get geometry of joint2 - b2Transform xfB = m_bodyB->m_xf; - float32 aB = m_bodyB->m_sweep.a; - b2Transform xfD = m_bodyD->m_xf; - float32 aD = m_bodyD->m_sweep.a; - - if (m_typeB == e_revoluteJoint) - { - b2RevoluteJoint* revolute = (b2RevoluteJoint*)def->joint2; - m_localAnchorD = revolute->m_localAnchorA; - m_localAnchorB = revolute->m_localAnchorB; - m_referenceAngleB = revolute->m_referenceAngle; - m_localAxisD.SetZero(); - - coordinateB = aB - aD - m_referenceAngleB; - } - else - { - b2PrismaticJoint* prismatic = (b2PrismaticJoint*)def->joint2; - m_localAnchorD = prismatic->m_localAnchorA; - m_localAnchorB = prismatic->m_localAnchorB; - m_referenceAngleB = prismatic->m_referenceAngle; - m_localAxisD = prismatic->m_localXAxisA; - - b2Vec2 pD = m_localAnchorD; - b2Vec2 pB = b2MulT(xfD.q, b2Mul(xfB.q, m_localAnchorB) + (xfB.p - xfD.p)); - coordinateB = b2Dot(pB - pD, m_localAxisD); - } - - m_ratio = def->ratio; - - m_constant = coordinateA + m_ratio * coordinateB; - - m_impulse = 0.0f; -} - -void b2GearJoint::InitVelocityConstraints(const b2SolverData& data) -{ - m_indexA = m_bodyA->m_islandIndex; - m_indexB = m_bodyB->m_islandIndex; - m_indexC = m_bodyC->m_islandIndex; - m_indexD = m_bodyD->m_islandIndex; - m_lcA = m_bodyA->m_sweep.localCenter; - m_lcB = m_bodyB->m_sweep.localCenter; - m_lcC = m_bodyC->m_sweep.localCenter; - m_lcD = m_bodyD->m_sweep.localCenter; - m_mA = m_bodyA->m_invMass; - m_mB = m_bodyB->m_invMass; - m_mC = m_bodyC->m_invMass; - m_mD = m_bodyD->m_invMass; - m_iA = m_bodyA->m_invI; - m_iB = m_bodyB->m_invI; - m_iC = m_bodyC->m_invI; - m_iD = m_bodyD->m_invI; - - float32 aA = data.positions[m_indexA].a; - b2Vec2 vA = data.velocities[m_indexA].v; - float32 wA = data.velocities[m_indexA].w; - - float32 aB = data.positions[m_indexB].a; - b2Vec2 vB = data.velocities[m_indexB].v; - float32 wB = data.velocities[m_indexB].w; - - float32 aC = data.positions[m_indexC].a; - b2Vec2 vC = data.velocities[m_indexC].v; - float32 wC = data.velocities[m_indexC].w; - - float32 aD = data.positions[m_indexD].a; - b2Vec2 vD = data.velocities[m_indexD].v; - float32 wD = data.velocities[m_indexD].w; - - b2Rot qA(aA), qB(aB), qC(aC), qD(aD); - - m_mass = 0.0f; - - if (m_typeA == e_revoluteJoint) - { - m_JvAC.SetZero(); - m_JwA = 1.0f; - m_JwC = 1.0f; - m_mass += m_iA + m_iC; - } - else - { - b2Vec2 u = b2Mul(qC, m_localAxisC); - b2Vec2 rC = b2Mul(qC, m_localAnchorC - m_lcC); - b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_lcA); - m_JvAC = u; - m_JwC = b2Cross(rC, u); - m_JwA = b2Cross(rA, u); - m_mass += m_mC + m_mA + m_iC * m_JwC * m_JwC + m_iA * m_JwA * m_JwA; - } - - if (m_typeB == e_revoluteJoint) - { - m_JvBD.SetZero(); - m_JwB = m_ratio; - m_JwD = m_ratio; - m_mass += m_ratio * m_ratio * (m_iB + m_iD); - } - else - { - b2Vec2 u = b2Mul(qD, m_localAxisD); - b2Vec2 rD = b2Mul(qD, m_localAnchorD - m_lcD); - b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_lcB); - m_JvBD = m_ratio * u; - m_JwD = m_ratio * b2Cross(rD, u); - m_JwB = m_ratio * b2Cross(rB, u); - m_mass += m_ratio * m_ratio * (m_mD + m_mB) + m_iD * m_JwD * m_JwD + m_iB * m_JwB * m_JwB; - } - - // Compute effective mass. - m_mass = m_mass > 0.0f ? 1.0f / m_mass : 0.0f; - - if (data.step.warmStarting) - { - vA += (m_mA * m_impulse) * m_JvAC; - wA += m_iA * m_impulse * m_JwA; - vB += (m_mB * m_impulse) * m_JvBD; - wB += m_iB * m_impulse * m_JwB; - vC -= (m_mC * m_impulse) * m_JvAC; - wC -= m_iC * m_impulse * m_JwC; - vD -= (m_mD * m_impulse) * m_JvBD; - wD -= m_iD * m_impulse * m_JwD; - } - else - { - m_impulse = 0.0f; - } - - data.velocities[m_indexA].v = vA; - data.velocities[m_indexA].w = wA; - data.velocities[m_indexB].v = vB; - data.velocities[m_indexB].w = wB; - data.velocities[m_indexC].v = vC; - data.velocities[m_indexC].w = wC; - data.velocities[m_indexD].v = vD; - data.velocities[m_indexD].w = wD; -} - -void b2GearJoint::SolveVelocityConstraints(const b2SolverData& data) -{ - b2Vec2 vA = data.velocities[m_indexA].v; - float32 wA = data.velocities[m_indexA].w; - b2Vec2 vB = data.velocities[m_indexB].v; - float32 wB = data.velocities[m_indexB].w; - b2Vec2 vC = data.velocities[m_indexC].v; - float32 wC = data.velocities[m_indexC].w; - b2Vec2 vD = data.velocities[m_indexD].v; - float32 wD = data.velocities[m_indexD].w; - - float32 Cdot = b2Dot(m_JvAC, vA - vC) + b2Dot(m_JvBD, vB - vD); - Cdot += (m_JwA * wA - m_JwC * wC) + (m_JwB * wB - m_JwD * wD); - - float32 impulse = -m_mass * Cdot; - m_impulse += impulse; - - vA += (m_mA * impulse) * m_JvAC; - wA += m_iA * impulse * m_JwA; - vB += (m_mB * impulse) * m_JvBD; - wB += m_iB * impulse * m_JwB; - vC -= (m_mC * impulse) * m_JvAC; - wC -= m_iC * impulse * m_JwC; - vD -= (m_mD * impulse) * m_JvBD; - wD -= m_iD * impulse * m_JwD; - - data.velocities[m_indexA].v = vA; - data.velocities[m_indexA].w = wA; - data.velocities[m_indexB].v = vB; - data.velocities[m_indexB].w = wB; - data.velocities[m_indexC].v = vC; - data.velocities[m_indexC].w = wC; - data.velocities[m_indexD].v = vD; - data.velocities[m_indexD].w = wD; -} - -bool b2GearJoint::SolvePositionConstraints(const b2SolverData& data) -{ - b2Vec2 cA = data.positions[m_indexA].c; - float32 aA = data.positions[m_indexA].a; - b2Vec2 cB = data.positions[m_indexB].c; - float32 aB = data.positions[m_indexB].a; - b2Vec2 cC = data.positions[m_indexC].c; - float32 aC = data.positions[m_indexC].a; - b2Vec2 cD = data.positions[m_indexD].c; - float32 aD = data.positions[m_indexD].a; - - b2Rot qA(aA), qB(aB), qC(aC), qD(aD); - - float32 linearError = 0.0f; - - float32 coordinateA, coordinateB; - - b2Vec2 JvAC, JvBD; - float32 JwA, JwB, JwC, JwD; - float32 mass = 0.0f; - - if (m_typeA == e_revoluteJoint) - { - JvAC.SetZero(); - JwA = 1.0f; - JwC = 1.0f; - mass += m_iA + m_iC; - - coordinateA = aA - aC - m_referenceAngleA; - } - else - { - b2Vec2 u = b2Mul(qC, m_localAxisC); - b2Vec2 rC = b2Mul(qC, m_localAnchorC - m_lcC); - b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_lcA); - JvAC = u; - JwC = b2Cross(rC, u); - JwA = b2Cross(rA, u); - mass += m_mC + m_mA + m_iC * JwC * JwC + m_iA * JwA * JwA; - - b2Vec2 pC = m_localAnchorC - m_lcC; - b2Vec2 pA = b2MulT(qC, rA + (cA - cC)); - coordinateA = b2Dot(pA - pC, m_localAxisC); - } - - if (m_typeB == e_revoluteJoint) - { - JvBD.SetZero(); - JwB = m_ratio; - JwD = m_ratio; - mass += m_ratio * m_ratio * (m_iB + m_iD); - - coordinateB = aB - aD - m_referenceAngleB; - } - else - { - b2Vec2 u = b2Mul(qD, m_localAxisD); - b2Vec2 rD = b2Mul(qD, m_localAnchorD - m_lcD); - b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_lcB); - JvBD = m_ratio * u; - JwD = m_ratio * b2Cross(rD, u); - JwB = m_ratio * b2Cross(rB, u); - mass += m_ratio * m_ratio * (m_mD + m_mB) + m_iD * JwD * JwD + m_iB * JwB * JwB; - - b2Vec2 pD = m_localAnchorD - m_lcD; - b2Vec2 pB = b2MulT(qD, rB + (cB - cD)); - coordinateB = b2Dot(pB - pD, m_localAxisD); - } - - float32 C = (coordinateA + m_ratio * coordinateB) - m_constant; - - float32 impulse = 0.0f; - if (mass > 0.0f) - { - impulse = -C / mass; - } - - cA += m_mA * impulse * JvAC; - aA += m_iA * impulse * JwA; - cB += m_mB * impulse * JvBD; - aB += m_iB * impulse * JwB; - cC -= m_mC * impulse * JvAC; - aC -= m_iC * impulse * JwC; - cD -= m_mD * impulse * JvBD; - aD -= m_iD * impulse * JwD; - - data.positions[m_indexA].c = cA; - data.positions[m_indexA].a = aA; - data.positions[m_indexB].c = cB; - data.positions[m_indexB].a = aB; - data.positions[m_indexC].c = cC; - data.positions[m_indexC].a = aC; - data.positions[m_indexD].c = cD; - data.positions[m_indexD].a = aD; - - // TODO_ERIN not implemented - return linearError < b2_linearSlop; -} - -b2Vec2 b2GearJoint::GetAnchorA() const -{ - return m_bodyA->GetWorldPoint(m_localAnchorA); -} - -b2Vec2 b2GearJoint::GetAnchorB() const -{ - return m_bodyB->GetWorldPoint(m_localAnchorB); -} - -b2Vec2 b2GearJoint::GetReactionForce(float32 inv_dt) const -{ - b2Vec2 P = m_impulse * m_JvAC; - return inv_dt * P; -} - -float32 b2GearJoint::GetReactionTorque(float32 inv_dt) const -{ - float32 L = m_impulse * m_JwA; - return inv_dt * L; -} - -void b2GearJoint::SetRatio(float32 ratio) -{ - b2Assert(b2IsValid(ratio)); - m_ratio = ratio; -} - -float32 b2GearJoint::GetRatio() const -{ - return m_ratio; -} - -void b2GearJoint::Dump() -{ - int32 indexA = m_bodyA->m_islandIndex; - int32 indexB = m_bodyB->m_islandIndex; - - int32 index1 = m_joint1->m_index; - int32 index2 = m_joint2->m_index; - - b2Log(" b2GearJointDef jd;\n"); - b2Log(" jd.bodyA = bodies[%d];\n", indexA); - b2Log(" jd.bodyB = bodies[%d];\n", indexB); - b2Log(" jd.collideConnected = bool(%d);\n", m_collideConnected); - b2Log(" jd.joint1 = joints[%d];\n", index1); - b2Log(" jd.joint2 = joints[%d];\n", index2); - b2Log(" jd.ratio = %.15lef;\n", m_ratio); - b2Log(" joints[%d] = m_world->CreateJoint(&jd);\n", m_index); -} +// MIT License + +// Copyright (c) 2019 Erin Catto + +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: + +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. + +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. + +#include "box2d/b2_gear_joint.h" +#include "box2d/b2_revolute_joint.h" +#include "box2d/b2_prismatic_joint.h" +#include "box2d/b2_body.h" +#include "box2d/b2_time_step.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); + + float 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(); + + // Body B on joint1 must be dynamic + b2Assert(m_bodyA->m_type == b2_dynamicBody); + + // Get geometry of joint1 + b2Transform xfA = m_bodyA->m_xf; + float aA = m_bodyA->m_sweep.a; + b2Transform xfC = m_bodyC->m_xf; + float 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; + + // position error is measured in radians + m_tolerance = b2_angularSlop; + } + 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); + + // position error is measured in meters + m_tolerance = b2_linearSlop; + } + + m_bodyD = m_joint2->GetBodyA(); + m_bodyB = m_joint2->GetBodyB(); + + // Body B on joint2 must be dynamic + b2Assert(m_bodyB->m_type == b2_dynamicBody); + + // Get geometry of joint2 + b2Transform xfB = m_bodyB->m_xf; + float aB = m_bodyB->m_sweep.a; + b2Transform xfD = m_bodyD->m_xf; + float 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; + + float aA = data.positions[m_indexA].a; + b2Vec2 vA = data.velocities[m_indexA].v; + float wA = data.velocities[m_indexA].w; + + float aB = data.positions[m_indexB].a; + b2Vec2 vB = data.velocities[m_indexB].v; + float wB = data.velocities[m_indexB].w; + + float aC = data.positions[m_indexC].a; + b2Vec2 vC = data.velocities[m_indexC].v; + float wC = data.velocities[m_indexC].w; + + float aD = data.positions[m_indexD].a; + b2Vec2 vD = data.velocities[m_indexD].v; + float 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; + float wA = data.velocities[m_indexA].w; + b2Vec2 vB = data.velocities[m_indexB].v; + float wB = data.velocities[m_indexB].w; + b2Vec2 vC = data.velocities[m_indexC].v; + float wC = data.velocities[m_indexC].w; + b2Vec2 vD = data.velocities[m_indexD].v; + float wD = data.velocities[m_indexD].w; + + float Cdot = b2Dot(m_JvAC, vA - vC) + b2Dot(m_JvBD, vB - vD); + Cdot += (m_JwA * wA - m_JwC * wC) + (m_JwB * wB - m_JwD * wD); + + float 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; + float aA = data.positions[m_indexA].a; + b2Vec2 cB = data.positions[m_indexB].c; + float aB = data.positions[m_indexB].a; + b2Vec2 cC = data.positions[m_indexC].c; + float aC = data.positions[m_indexC].a; + b2Vec2 cD = data.positions[m_indexD].c; + float aD = data.positions[m_indexD].a; + + b2Rot qA(aA), qB(aB), qC(aC), qD(aD); + + float coordinateA, coordinateB; + + b2Vec2 JvAC, JvBD; + float JwA, JwB, JwC, JwD; + float 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); + } + + float C = (coordinateA + m_ratio * coordinateB) - m_constant; + + float 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; + + if (b2Abs(C) < m_tolerance) + { + return true; + } + + return false; +} + +b2Vec2 b2GearJoint::GetAnchorA() const +{ + return m_bodyA->GetWorldPoint(m_localAnchorA); +} + +b2Vec2 b2GearJoint::GetAnchorB() const +{ + return m_bodyB->GetWorldPoint(m_localAnchorB); +} + +b2Vec2 b2GearJoint::GetReactionForce(float inv_dt) const +{ + b2Vec2 P = m_impulse * m_JvAC; + return inv_dt * P; +} + +float b2GearJoint::GetReactionTorque(float inv_dt) const +{ + float L = m_impulse * m_JwA; + return inv_dt * L; +} + +void b2GearJoint::SetRatio(float ratio) +{ + b2Assert(b2IsValid(ratio)); + m_ratio = ratio; +} + +float 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; + + b2Dump(" b2GearJointDef jd;\n"); + b2Dump(" jd.bodyA = bodies[%d];\n", indexA); + b2Dump(" jd.bodyB = bodies[%d];\n", indexB); + b2Dump(" jd.collideConnected = bool(%d);\n", m_collideConnected); + b2Dump(" jd.joint1 = joints[%d];\n", index1); + b2Dump(" jd.joint2 = joints[%d];\n", index2); + b2Dump(" jd.ratio = %.9g;\n", m_ratio); + b2Dump(" joints[%d] = m_world->CreateJoint(&jd);\n", m_index); +} diff --git a/libs/Box2D/Dynamics/b2Island.cpp b/libs/box2d/src/dynamics/b2_island.cpp similarity index 83% rename from libs/Box2D/Dynamics/b2Island.cpp rename to libs/box2d/src/dynamics/b2_island.cpp index b0cb19aa..34413056 100644 --- a/libs/Box2D/Dynamics/b2Island.cpp +++ b/libs/box2d/src/dynamics/b2_island.cpp @@ -1,539 +1,544 @@ -/* -* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -/* -Position Correction Notes -========================= -I tried the several algorithms for position correction of the 2D revolute joint. -I looked at these systems: -- simple pendulum (1m diameter sphere on massless 5m stick) with initial angular velocity of 100 rad/s. -- suspension bridge with 30 1m long planks of length 1m. -- multi-link chain with 30 1m long links. - -Here are the algorithms: - -Baumgarte - A fraction of the position error is added to the velocity error. There is no -separate position solver. - -Pseudo Velocities - After the velocity solver and position integration, -the position error, Jacobian, and effective mass are recomputed. Then -the velocity constraints are solved with pseudo velocities and a fraction -of the position error is added to the pseudo velocity error. The pseudo -velocities are initialized to zero and there is no warm-starting. After -the position solver, the pseudo velocities are added to the positions. -This is also called the First Order World method or the Position LCP method. - -Modified Nonlinear Gauss-Seidel (NGS) - Like Pseudo Velocities except the -position error is re-computed for each constraint and the positions are updated -after the constraint is solved. The radius vectors (aka Jacobians) are -re-computed too (otherwise the algorithm has horrible instability). The pseudo -velocity states are not needed because they are effectively zero at the beginning -of each iteration. Since we have the current position error, we allow the -iterations to terminate early if the error becomes smaller than b2_linearSlop. - -Full NGS or just NGS - Like Modified NGS except the effective mass are re-computed -each time a constraint is solved. - -Here are the results: -Baumgarte - this is the cheapest algorithm but it has some stability problems, -especially with the bridge. The chain links separate easily close to the root -and they jitter as they struggle to pull together. This is one of the most common -methods in the field. The big drawback is that the position correction artificially -affects the momentum, thus leading to instabilities and false bounce. I used a -bias factor of 0.2. A larger bias factor makes the bridge less stable, a smaller -factor makes joints and contacts more spongy. - -Pseudo Velocities - the is more stable than the Baumgarte method. The bridge is -stable. However, joints still separate with large angular velocities. Drag the -simple pendulum in a circle quickly and the joint will separate. The chain separates -easily and does not recover. I used a bias factor of 0.2. A larger value lead to -the bridge collapsing when a heavy cube drops on it. - -Modified NGS - this algorithm is better in some ways than Baumgarte and Pseudo -Velocities, but in other ways it is worse. The bridge and chain are much more -stable, but the simple pendulum goes unstable at high angular velocities. - -Full NGS - stable in all tests. The joints display good stiffness. The bridge -still sags, but this is better than infinite forces. - -Recommendations -Pseudo Velocities are not really worthwhile because the bridge and chain cannot -recover from joint separation. In other cases the benefit over Baumgarte is small. - -Modified NGS is not a robust method for the revolute joint due to the violent -instability seen in the simple pendulum. Perhaps it is viable with other constraint -types, especially scalar constraints where the effective mass is a scalar. - -This leaves Baumgarte and Full NGS. Baumgarte has small, but manageable instabilities -and is very fast. I don't think we can escape Baumgarte, especially in highly -demanding cases where high constraint fidelity is not needed. - -Full NGS is robust and easy on the eyes. I recommend this as an option for -higher fidelity simulation and certainly for suspension bridges and long chains. -Full NGS might be a good choice for ragdolls, especially motorized ragdolls where -joint separation can be problematic. The number of NGS iterations can be reduced -for better performance without harming robustness much. - -Each joint in a can be handled differently in the position solver. So I recommend -a system where the user can select the algorithm on a per joint basis. I would -probably default to the slower Full NGS and let the user select the faster -Baumgarte method in performance critical scenarios. -*/ - -/* -Cache Performance - -The Box2D solvers are dominated by cache misses. Data structures are designed -to increase the number of cache hits. Much of misses are due to random access -to body data. The constraint structures are iterated over linearly, which leads -to few cache misses. - -The bodies are not accessed during iteration. Instead read only data, such as -the mass values are stored with the constraints. The mutable data are the constraint -impulses and the bodies velocities/positions. The impulses are held inside the -constraint structures. The body velocities/positions are held in compact, temporary -arrays to increase the number of cache hits. Linear and angular velocity are -stored in a single array since multiple arrays lead to multiple misses. -*/ - -/* -2D Rotation - -R = [cos(theta) -sin(theta)] - [sin(theta) cos(theta) ] - -thetaDot = omega - -Let q1 = cos(theta), q2 = sin(theta). -R = [q1 -q2] - [q2 q1] - -q1Dot = -thetaDot * q2 -q2Dot = thetaDot * q1 - -q1_new = q1_old - dt * w * q2 -q2_new = q2_old + dt * w * q1 -then normalize. - -This might be faster than computing sin+cos. -However, we can compute sin+cos of the same angle fast. -*/ - -b2Island::b2Island( - int32 bodyCapacity, - int32 contactCapacity, - int32 jointCapacity, - b2StackAllocator* allocator, - b2ContactListener* listener) -{ - m_bodyCapacity = bodyCapacity; - m_contactCapacity = contactCapacity; - m_jointCapacity = jointCapacity; - m_bodyCount = 0; - m_contactCount = 0; - m_jointCount = 0; - - m_allocator = allocator; - m_listener = listener; - - m_bodies = (b2Body**)m_allocator->Allocate(bodyCapacity * sizeof(b2Body*)); - m_contacts = (b2Contact**)m_allocator->Allocate(contactCapacity * sizeof(b2Contact*)); - m_joints = (b2Joint**)m_allocator->Allocate(jointCapacity * sizeof(b2Joint*)); - - m_velocities = (b2Velocity*)m_allocator->Allocate(m_bodyCapacity * sizeof(b2Velocity)); - m_positions = (b2Position*)m_allocator->Allocate(m_bodyCapacity * sizeof(b2Position)); -} - -b2Island::~b2Island() -{ - // Warning: the order should reverse the constructor order. - m_allocator->Free(m_positions); - m_allocator->Free(m_velocities); - m_allocator->Free(m_joints); - m_allocator->Free(m_contacts); - m_allocator->Free(m_bodies); -} - -void b2Island::Solve(b2Profile* profile, const b2TimeStep& step, const b2Vec2& gravity, bool allowSleep) -{ - b2Timer timer; - - float32 h = step.dt; - - // Integrate velocities and apply damping. Initialize the body state. - for (int32 i = 0; i < m_bodyCount; ++i) - { - b2Body* b = m_bodies[i]; - - b2Vec2 c = b->m_sweep.c; - float32 a = b->m_sweep.a; - b2Vec2 v = b->m_linearVelocity; - float32 w = b->m_angularVelocity; - - // Store positions for continuous collision. - b->m_sweep.c0 = b->m_sweep.c; - b->m_sweep.a0 = b->m_sweep.a; - - if (b->m_type == b2_dynamicBody) - { - // Integrate velocities. - v += h * (b->m_gravityScale * gravity + b->m_invMass * b->m_force); - w += h * b->m_invI * b->m_torque; - - // Apply damping. - // ODE: dv/dt + c * v = 0 - // Solution: v(t) = v0 * exp(-c * t) - // Time step: v(t + dt) = v0 * exp(-c * (t + dt)) = v0 * exp(-c * t) * exp(-c * dt) = v * exp(-c * dt) - // v2 = exp(-c * dt) * v1 - // Pade approximation: - // v2 = v1 * 1 / (1 + c * dt) - v *= 1.0f / (1.0f + h * b->m_linearDamping); - w *= 1.0f / (1.0f + h * b->m_angularDamping); - } - - m_positions[i].c = c; - m_positions[i].a = a; - m_velocities[i].v = v; - m_velocities[i].w = w; - } - - timer.Reset(); - - // Solver data - b2SolverData solverData; - solverData.step = step; - solverData.positions = m_positions; - solverData.velocities = m_velocities; - - // Initialize velocity constraints. - b2ContactSolverDef contactSolverDef; - contactSolverDef.step = step; - contactSolverDef.contacts = m_contacts; - contactSolverDef.count = m_contactCount; - contactSolverDef.positions = m_positions; - contactSolverDef.velocities = m_velocities; - contactSolverDef.allocator = m_allocator; - - b2ContactSolver contactSolver(&contactSolverDef); - contactSolver.InitializeVelocityConstraints(); - - if (step.warmStarting) - { - contactSolver.WarmStart(); - } - - for (int32 i = 0; i < m_jointCount; ++i) - { - m_joints[i]->InitVelocityConstraints(solverData); - } - - profile->solveInit = timer.GetMilliseconds(); - - // Solve velocity constraints - timer.Reset(); - for (int32 i = 0; i < step.velocityIterations; ++i) - { - for (int32 j = 0; j < m_jointCount; ++j) - { - m_joints[j]->SolveVelocityConstraints(solverData); - } - - contactSolver.SolveVelocityConstraints(); - } - - // Store impulses for warm starting - contactSolver.StoreImpulses(); - profile->solveVelocity = timer.GetMilliseconds(); - - // Integrate positions - for (int32 i = 0; i < m_bodyCount; ++i) - { - b2Vec2 c = m_positions[i].c; - float32 a = m_positions[i].a; - b2Vec2 v = m_velocities[i].v; - float32 w = m_velocities[i].w; - - // Check for large velocities - b2Vec2 translation = h * v; - if (b2Dot(translation, translation) > b2_maxTranslationSquared) - { - float32 ratio = b2_maxTranslation / translation.Length(); - v *= ratio; - } - - float32 rotation = h * w; - if (rotation * rotation > b2_maxRotationSquared) - { - float32 ratio = b2_maxRotation / b2Abs(rotation); - w *= ratio; - } - - // Integrate - c += h * v; - a += h * w; - - m_positions[i].c = c; - m_positions[i].a = a; - m_velocities[i].v = v; - m_velocities[i].w = w; - } - - // Solve position constraints - timer.Reset(); - bool positionSolved = false; - for (int32 i = 0; i < step.positionIterations; ++i) - { - bool contactsOkay = contactSolver.SolvePositionConstraints(); - - bool jointsOkay = true; - for (int32 i = 0; i < m_jointCount; ++i) - { - bool jointOkay = m_joints[i]->SolvePositionConstraints(solverData); - jointsOkay = jointsOkay && jointOkay; - } - - if (contactsOkay && jointsOkay) - { - // Exit early if the position errors are small. - positionSolved = true; - break; - } - } - - // Copy state buffers back to the bodies - for (int32 i = 0; i < m_bodyCount; ++i) - { - b2Body* body = m_bodies[i]; - body->m_sweep.c = m_positions[i].c; - body->m_sweep.a = m_positions[i].a; - body->m_linearVelocity = m_velocities[i].v; - body->m_angularVelocity = m_velocities[i].w; - body->SynchronizeTransform(); - } - - profile->solvePosition = timer.GetMilliseconds(); - - Report(contactSolver.m_velocityConstraints); - - if (allowSleep) - { - float32 minSleepTime = b2_maxFloat; - - const float32 linTolSqr = b2_linearSleepTolerance * b2_linearSleepTolerance; - const float32 angTolSqr = b2_angularSleepTolerance * b2_angularSleepTolerance; - - for (int32 i = 0; i < m_bodyCount; ++i) - { - b2Body* b = m_bodies[i]; - if (b->GetType() == b2_staticBody) - { - continue; - } - - if ((b->m_flags & b2Body::e_autoSleepFlag) == 0 || - b->m_angularVelocity * b->m_angularVelocity > angTolSqr || - b2Dot(b->m_linearVelocity, b->m_linearVelocity) > linTolSqr) - { - b->m_sleepTime = 0.0f; - minSleepTime = 0.0f; - } - else - { - b->m_sleepTime += h; - minSleepTime = b2Min(minSleepTime, b->m_sleepTime); - } - } - - if (minSleepTime >= b2_timeToSleep && positionSolved) - { - for (int32 i = 0; i < m_bodyCount; ++i) - { - b2Body* b = m_bodies[i]; - b->SetAwake(false); - } - } - } -} - -void b2Island::SolveTOI(const b2TimeStep& subStep, int32 toiIndexA, int32 toiIndexB) -{ - b2Assert(toiIndexA < m_bodyCount); - b2Assert(toiIndexB < m_bodyCount); - - // Initialize the body state. - for (int32 i = 0; i < m_bodyCount; ++i) - { - b2Body* b = m_bodies[i]; - m_positions[i].c = b->m_sweep.c; - m_positions[i].a = b->m_sweep.a; - m_velocities[i].v = b->m_linearVelocity; - m_velocities[i].w = b->m_angularVelocity; - } - - b2ContactSolverDef contactSolverDef; - contactSolverDef.contacts = m_contacts; - contactSolverDef.count = m_contactCount; - contactSolverDef.allocator = m_allocator; - contactSolverDef.step = subStep; - contactSolverDef.positions = m_positions; - contactSolverDef.velocities = m_velocities; - b2ContactSolver contactSolver(&contactSolverDef); - - // Solve position constraints. - for (int32 i = 0; i < subStep.positionIterations; ++i) - { - bool contactsOkay = contactSolver.SolveTOIPositionConstraints(toiIndexA, toiIndexB); - if (contactsOkay) - { - break; - } - } - -#if 0 - // Is the new position really safe? - for (int32 i = 0; i < m_contactCount; ++i) - { - b2Contact* c = m_contacts[i]; - b2Fixture* fA = c->GetFixtureA(); - b2Fixture* fB = c->GetFixtureB(); - - b2Body* bA = fA->GetBody(); - b2Body* bB = fB->GetBody(); - - int32 indexA = c->GetChildIndexA(); - int32 indexB = c->GetChildIndexB(); - - b2DistanceInput input; - input.proxyA.Set(fA->GetShape(), indexA); - input.proxyB.Set(fB->GetShape(), indexB); - input.transformA = bA->GetTransform(); - input.transformB = bB->GetTransform(); - input.useRadii = false; - - b2DistanceOutput output; - b2SimplexCache cache; - cache.count = 0; - b2Distance(&output, &cache, &input); - - if (output.distance == 0 || cache.count == 3) - { - cache.count += 0; - } - } -#endif - - // Leap of faith to new safe state. - m_bodies[toiIndexA]->m_sweep.c0 = m_positions[toiIndexA].c; - m_bodies[toiIndexA]->m_sweep.a0 = m_positions[toiIndexA].a; - m_bodies[toiIndexB]->m_sweep.c0 = m_positions[toiIndexB].c; - m_bodies[toiIndexB]->m_sweep.a0 = m_positions[toiIndexB].a; - - // No warm starting is needed for TOI events because warm - // starting impulses were applied in the discrete solver. - contactSolver.InitializeVelocityConstraints(); - - // Solve velocity constraints. - for (int32 i = 0; i < subStep.velocityIterations; ++i) - { - contactSolver.SolveVelocityConstraints(); - } - - // Don't store the TOI contact forces for warm starting - // because they can be quite large. - - float32 h = subStep.dt; - - // Integrate positions - for (int32 i = 0; i < m_bodyCount; ++i) - { - b2Vec2 c = m_positions[i].c; - float32 a = m_positions[i].a; - b2Vec2 v = m_velocities[i].v; - float32 w = m_velocities[i].w; - - // Check for large velocities - b2Vec2 translation = h * v; - if (b2Dot(translation, translation) > b2_maxTranslationSquared) - { - float32 ratio = b2_maxTranslation / translation.Length(); - v *= ratio; - } - - float32 rotation = h * w; - if (rotation * rotation > b2_maxRotationSquared) - { - float32 ratio = b2_maxRotation / b2Abs(rotation); - w *= ratio; - } - - // Integrate - c += h * v; - a += h * w; - - m_positions[i].c = c; - m_positions[i].a = a; - m_velocities[i].v = v; - m_velocities[i].w = w; - - // Sync bodies - b2Body* body = m_bodies[i]; - body->m_sweep.c = c; - body->m_sweep.a = a; - body->m_linearVelocity = v; - body->m_angularVelocity = w; - body->SynchronizeTransform(); - } - - Report(contactSolver.m_velocityConstraints); -} - -void b2Island::Report(const b2ContactVelocityConstraint* constraints) -{ - if (m_listener == NULL) - { - return; - } - - for (int32 i = 0; i < m_contactCount; ++i) - { - b2Contact* c = m_contacts[i]; - - const b2ContactVelocityConstraint* vc = constraints + i; - - b2ContactImpulse impulse; - impulse.count = vc->pointCount; - for (int32 j = 0; j < vc->pointCount; ++j) - { - impulse.normalImpulses[j] = vc->points[j].normalImpulse; - impulse.tangentImpulses[j] = vc->points[j].tangentImpulse; - } - - m_listener->PostSolve(c, &impulse); - } -} +// MIT License + +// Copyright (c) 2019 Erin Catto + +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: + +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. + +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. + +#include "box2d/b2_body.h" +#include "box2d/b2_contact.h" +#include "box2d/b2_distance.h" +#include "box2d/b2_fixture.h" +#include "box2d/b2_joint.h" +#include "box2d/b2_stack_allocator.h" +#include "box2d/b2_timer.h" +#include "box2d/b2_world.h" + +#include "b2_contact_solver.h" +#include "b2_island.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; + + float 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; + float a = b->m_sweep.a; + b2Vec2 v = b->m_linearVelocity; + float 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_invMass * (b->m_gravityScale * b->m_mass * gravity + 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; + float a = m_positions[i].a; + b2Vec2 v = m_velocities[i].v; + float w = m_velocities[i].w; + + // Check for large velocities + b2Vec2 translation = h * v; + if (b2Dot(translation, translation) > b2_maxTranslationSquared) + { + float ratio = b2_maxTranslation / translation.Length(); + v *= ratio; + } + + float rotation = h * w; + if (rotation * rotation > b2_maxRotationSquared) + { + float 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) + { + float minSleepTime = b2_maxFloat; + + const float linTolSqr = b2_linearSleepTolerance * b2_linearSleepTolerance; + const float 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. + + float h = subStep.dt; + + // Integrate positions + for (int32 i = 0; i < m_bodyCount; ++i) + { + b2Vec2 c = m_positions[i].c; + float a = m_positions[i].a; + b2Vec2 v = m_velocities[i].v; + float w = m_velocities[i].w; + + // Check for large velocities + b2Vec2 translation = h * v; + if (b2Dot(translation, translation) > b2_maxTranslationSquared) + { + float ratio = b2_maxTranslation / translation.Length(); + v *= ratio; + } + + float rotation = h * w; + if (rotation * rotation > b2_maxRotationSquared) + { + float 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/libs/Box2D/Dynamics/b2Island.h b/libs/box2d/src/dynamics/b2_island.h similarity index 54% rename from libs/Box2D/Dynamics/b2Island.h rename to libs/box2d/src/dynamics/b2_island.h index ef3d9b1d..2e28a357 100644 --- a/libs/Box2D/Dynamics/b2Island.h +++ b/libs/box2d/src/dynamics/b2_island.h @@ -1,93 +1,97 @@ -/* -* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef B2_ISLAND_H -#define B2_ISLAND_H - -#include -#include -#include - -class b2Contact; -class b2Joint; -class b2StackAllocator; -class b2ContactListener; -struct b2ContactVelocityConstraint; -struct b2Profile; - -/// This is an internal class. -class b2Island -{ -public: - b2Island(int32 bodyCapacity, int32 contactCapacity, int32 jointCapacity, - b2StackAllocator* allocator, b2ContactListener* listener); - ~b2Island(); - - void Clear() - { - m_bodyCount = 0; - m_contactCount = 0; - m_jointCount = 0; - } - - void Solve(b2Profile* profile, const b2TimeStep& step, const b2Vec2& gravity, bool allowSleep); - - void SolveTOI(const b2TimeStep& subStep, int32 toiIndexA, int32 toiIndexB); - - void Add(b2Body* body) - { - b2Assert(m_bodyCount < m_bodyCapacity); - body->m_islandIndex = m_bodyCount; - m_bodies[m_bodyCount] = body; - ++m_bodyCount; - } - - void Add(b2Contact* contact) - { - b2Assert(m_contactCount < m_contactCapacity); - m_contacts[m_contactCount++] = contact; - } - - void Add(b2Joint* joint) - { - b2Assert(m_jointCount < m_jointCapacity); - m_joints[m_jointCount++] = joint; - } - - void Report(const b2ContactVelocityConstraint* constraints); - - b2StackAllocator* m_allocator; - b2ContactListener* m_listener; - - b2Body** m_bodies; - b2Contact** m_contacts; - b2Joint** m_joints; - - b2Position* m_positions; - b2Velocity* m_velocities; - - int32 m_bodyCount; - int32 m_jointCount; - int32 m_contactCount; - - int32 m_bodyCapacity; - int32 m_contactCapacity; - int32 m_jointCapacity; -}; - -#endif +// MIT License + +// Copyright (c) 2019 Erin Catto + +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: + +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. + +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. + +#ifndef B2_ISLAND_H +#define B2_ISLAND_H + +#include "box2d/b2_body.h" +#include "box2d/b2_math.h" +#include "box2d/b2_time_step.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/libs/box2d/src/dynamics/b2_joint.cpp b/libs/box2d/src/dynamics/b2_joint.cpp new file mode 100644 index 00000000..41addbbb --- /dev/null +++ b/libs/box2d/src/dynamics/b2_joint.cpp @@ -0,0 +1,301 @@ +// MIT License + +// Copyright (c) 2019 Erin Catto + +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: + +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. + +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. + +#include "box2d/b2_block_allocator.h" +#include "box2d/b2_body.h" +#include "box2d/b2_distance_joint.h" +#include "box2d/b2_draw.h" +#include "box2d/b2_friction_joint.h" +#include "box2d/b2_gear_joint.h" +#include "box2d/b2_motor_joint.h" +#include "box2d/b2_mouse_joint.h" +#include "box2d/b2_prismatic_joint.h" +#include "box2d/b2_pulley_joint.h" +#include "box2d/b2_revolute_joint.h" +#include "box2d/b2_weld_joint.h" +#include "box2d/b2_wheel_joint.h" +#include "box2d/b2_world.h" + +#include + +void b2LinearStiffness(float& stiffness, float& damping, + float frequencyHertz, float dampingRatio, + const b2Body* bodyA, const b2Body* bodyB) +{ + float massA = bodyA->GetMass(); + float massB = bodyB->GetMass(); + float mass; + if (massA > 0.0f && massB > 0.0f) + { + mass = massA * massB / (massA + massB); + } + else if (massA > 0.0f) + { + mass = massA; + } + else + { + mass = massB; + } + + float omega = 2.0f * b2_pi * frequencyHertz; + stiffness = mass * omega * omega; + damping = 2.0f * mass * dampingRatio * omega; +} + +void b2AngularStiffness(float& stiffness, float& damping, + float frequencyHertz, float dampingRatio, + const b2Body* bodyA, const b2Body* bodyB) +{ + float IA = bodyA->GetInertia(); + float IB = bodyB->GetInertia(); + float I; + if (IA > 0.0f && IB > 0.0f) + { + I = IA * IB / (IA + IB); + } + else if (IA > 0.0f) + { + I = IA; + } + else + { + I = IB; + } + + float omega = 2.0f * b2_pi * frequencyHertz; + stiffness = I * omega * omega; + damping = 2.0f * I * dampingRatio * omega; +} + +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(def)); + } + break; + + case e_mouseJoint: + { + void* mem = allocator->Allocate(sizeof(b2MouseJoint)); + joint = new (mem) b2MouseJoint(static_cast(def)); + } + break; + + case e_prismaticJoint: + { + void* mem = allocator->Allocate(sizeof(b2PrismaticJoint)); + joint = new (mem) b2PrismaticJoint(static_cast(def)); + } + break; + + case e_revoluteJoint: + { + void* mem = allocator->Allocate(sizeof(b2RevoluteJoint)); + joint = new (mem) b2RevoluteJoint(static_cast(def)); + } + break; + + case e_pulleyJoint: + { + void* mem = allocator->Allocate(sizeof(b2PulleyJoint)); + joint = new (mem) b2PulleyJoint(static_cast(def)); + } + break; + + case e_gearJoint: + { + void* mem = allocator->Allocate(sizeof(b2GearJoint)); + joint = new (mem) b2GearJoint(static_cast(def)); + } + break; + + case e_wheelJoint: + { + void* mem = allocator->Allocate(sizeof(b2WheelJoint)); + joint = new (mem) b2WheelJoint(static_cast(def)); + } + break; + + case e_weldJoint: + { + void* mem = allocator->Allocate(sizeof(b2WeldJoint)); + joint = new (mem) b2WeldJoint(static_cast(def)); + } + break; + + case e_frictionJoint: + { + void* mem = allocator->Allocate(sizeof(b2FrictionJoint)); + joint = new (mem) b2FrictionJoint(static_cast(def)); + } + break; + + case e_motorJoint: + { + void* mem = allocator->Allocate(sizeof(b2MotorJoint)); + joint = new (mem) b2MotorJoint(static_cast(def)); + } + break; + + default: + b2Assert(false); + break; + } + + return joint; +} + +void b2Joint::Destroy(b2Joint* joint, b2BlockAllocator* allocator) +{ + joint->~b2Joint(); + switch (joint->m_type) + { + case e_distanceJoint: + allocator->Free(joint, sizeof(b2DistanceJoint)); + break; + + case e_mouseJoint: + allocator->Free(joint, sizeof(b2MouseJoint)); + break; + + case e_prismaticJoint: + allocator->Free(joint, sizeof(b2PrismaticJoint)); + break; + + case e_revoluteJoint: + allocator->Free(joint, sizeof(b2RevoluteJoint)); + break; + + case e_pulleyJoint: + allocator->Free(joint, sizeof(b2PulleyJoint)); + break; + + case e_gearJoint: + allocator->Free(joint, sizeof(b2GearJoint)); + break; + + case e_wheelJoint: + allocator->Free(joint, sizeof(b2WheelJoint)); + break; + + case e_weldJoint: + allocator->Free(joint, sizeof(b2WeldJoint)); + break; + + case e_frictionJoint: + allocator->Free(joint, sizeof(b2FrictionJoint)); + break; + + case e_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::IsEnabled() const +{ + return m_bodyA->IsEnabled() && m_bodyB->IsEnabled(); +} + +void b2Joint::Draw(b2Draw* draw) const +{ + const b2Transform& xf1 = m_bodyA->GetTransform(); + const b2Transform& xf2 = m_bodyB->GetTransform(); + b2Vec2 x1 = xf1.p; + b2Vec2 x2 = xf2.p; + b2Vec2 p1 = GetAnchorA(); + b2Vec2 p2 = GetAnchorB(); + + b2Color color(0.5f, 0.8f, 0.8f); + + switch (m_type) + { + case e_distanceJoint: + draw->DrawSegment(p1, p2, color); + break; + + case e_pulleyJoint: + { + b2PulleyJoint* pulley = (b2PulleyJoint*)this; + b2Vec2 s1 = pulley->GetGroundAnchorA(); + b2Vec2 s2 = pulley->GetGroundAnchorB(); + draw->DrawSegment(s1, p1, color); + draw->DrawSegment(s2, p2, color); + draw->DrawSegment(s1, s2, color); + } + break; + + case e_mouseJoint: + { + b2Color c; + c.Set(0.0f, 1.0f, 0.0f); + draw->DrawPoint(p1, 4.0f, c); + draw->DrawPoint(p2, 4.0f, c); + + c.Set(0.8f, 0.8f, 0.8f); + draw->DrawSegment(p1, p2, c); + + } + break; + + default: + draw->DrawSegment(x1, p1, color); + draw->DrawSegment(p1, p2, color); + draw->DrawSegment(x2, p2, color); + } +} diff --git a/libs/Box2D/Dynamics/Joints/b2MotorJoint.cpp b/libs/box2d/src/dynamics/b2_motor_joint.cpp similarity index 60% rename from libs/Box2D/Dynamics/Joints/b2MotorJoint.cpp rename to libs/box2d/src/dynamics/b2_motor_joint.cpp index a07a6401..6e0b0753 100644 --- a/libs/Box2D/Dynamics/Joints/b2MotorJoint.cpp +++ b/libs/box2d/src/dynamics/b2_motor_joint.cpp @@ -1,304 +1,311 @@ -/* -* Copyright (c) 2006-2012 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ - -#include -#include -#include - -// Point-to-point constraint -// Cdot = v2 - v1 -// = v2 + cross(w2, r2) - v1 - cross(w1, r1) -// J = [-I -r1_skew I r2_skew ] -// Identity used: -// w k % (rx i + ry j) = w * (-ry i + rx j) - -// Angle constraint -// Cdot = w2 - w1 -// J = [0 0 -1 0 0 1] -// K = invI1 + invI2 - -void b2MotorJointDef::Initialize(b2Body* bA, b2Body* bB) -{ - bodyA = bA; - bodyB = bB; - b2Vec2 xB = bodyB->GetPosition(); - linearOffset = bodyA->GetLocalPoint(xB); - - float32 angleA = bodyA->GetAngle(); - float32 angleB = bodyB->GetAngle(); - angularOffset = angleB - angleA; -} - -b2MotorJoint::b2MotorJoint(const b2MotorJointDef* def) -: b2Joint(def) -{ - m_linearOffset = def->linearOffset; - m_angularOffset = def->angularOffset; - - m_linearImpulse.SetZero(); - m_angularImpulse = 0.0f; - - m_maxForce = def->maxForce; - m_maxTorque = def->maxTorque; - m_correctionFactor = def->correctionFactor; -} - -void b2MotorJoint::InitVelocityConstraints(const b2SolverData& data) -{ - m_indexA = m_bodyA->m_islandIndex; - m_indexB = m_bodyB->m_islandIndex; - m_localCenterA = m_bodyA->m_sweep.localCenter; - m_localCenterB = m_bodyB->m_sweep.localCenter; - m_invMassA = m_bodyA->m_invMass; - m_invMassB = m_bodyB->m_invMass; - m_invIA = m_bodyA->m_invI; - m_invIB = m_bodyB->m_invI; - - b2Vec2 cA = data.positions[m_indexA].c; - float32 aA = data.positions[m_indexA].a; - b2Vec2 vA = data.velocities[m_indexA].v; - float32 wA = data.velocities[m_indexA].w; - - b2Vec2 cB = data.positions[m_indexB].c; - float32 aB = data.positions[m_indexB].a; - b2Vec2 vB = data.velocities[m_indexB].v; - float32 wB = data.velocities[m_indexB].w; - - b2Rot qA(aA), qB(aB); - - // Compute the effective mass matrix. - m_rA = b2Mul(qA, -m_localCenterA); - m_rB = b2Mul(qB, -m_localCenterB); - - // J = [-I -r1_skew I r2_skew] - // [ 0 -1 0 1] - // r_skew = [-ry; rx] - - // Matlab - // K = [ mA+r1y^2*iA+mB+r2y^2*iB, -r1y*iA*r1x-r2y*iB*r2x, -r1y*iA-r2y*iB] - // [ -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB, r1x*iA+r2x*iB] - // [ -r1y*iA-r2y*iB, r1x*iA+r2x*iB, iA+iB] - - float32 mA = m_invMassA, mB = m_invMassB; - float32 iA = m_invIA, iB = m_invIB; - - b2Mat22 K; - K.ex.x = mA + mB + iA * m_rA.y * m_rA.y + iB * m_rB.y * m_rB.y; - K.ex.y = -iA * m_rA.x * m_rA.y - iB * m_rB.x * m_rB.y; - K.ey.x = K.ex.y; - K.ey.y = mA + mB + iA * m_rA.x * m_rA.x + iB * m_rB.x * m_rB.x; - - m_linearMass = K.GetInverse(); - - m_angularMass = iA + iB; - if (m_angularMass > 0.0f) - { - m_angularMass = 1.0f / m_angularMass; - } - - m_linearError = cB + m_rB - cA - m_rA - b2Mul(qA, m_linearOffset); - m_angularError = aB - aA - m_angularOffset; - - if (data.step.warmStarting) - { - // Scale impulses to support a variable time step. - m_linearImpulse *= data.step.dtRatio; - m_angularImpulse *= data.step.dtRatio; - - b2Vec2 P(m_linearImpulse.x, m_linearImpulse.y); - vA -= mA * P; - wA -= iA * (b2Cross(m_rA, P) + m_angularImpulse); - vB += mB * P; - wB += iB * (b2Cross(m_rB, P) + m_angularImpulse); - } - else - { - m_linearImpulse.SetZero(); - m_angularImpulse = 0.0f; - } - - data.velocities[m_indexA].v = vA; - data.velocities[m_indexA].w = wA; - data.velocities[m_indexB].v = vB; - data.velocities[m_indexB].w = wB; -} - -void b2MotorJoint::SolveVelocityConstraints(const b2SolverData& data) -{ - b2Vec2 vA = data.velocities[m_indexA].v; - float32 wA = data.velocities[m_indexA].w; - b2Vec2 vB = data.velocities[m_indexB].v; - float32 wB = data.velocities[m_indexB].w; - - float32 mA = m_invMassA, mB = m_invMassB; - float32 iA = m_invIA, iB = m_invIB; - - float32 h = data.step.dt; - float32 inv_h = data.step.inv_dt; - - // Solve angular friction - { - float32 Cdot = wB - wA + inv_h * m_correctionFactor * m_angularError; - float32 impulse = -m_angularMass * Cdot; - - float32 oldImpulse = m_angularImpulse; - float32 maxImpulse = h * m_maxTorque; - m_angularImpulse = b2Clamp(m_angularImpulse + impulse, -maxImpulse, maxImpulse); - impulse = m_angularImpulse - oldImpulse; - - wA -= iA * impulse; - wB += iB * impulse; - } - - // Solve linear friction - { - b2Vec2 Cdot = vB + b2Cross(wB, m_rB) - vA - b2Cross(wA, m_rA) + inv_h * m_correctionFactor * m_linearError; - - b2Vec2 impulse = -b2Mul(m_linearMass, Cdot); - b2Vec2 oldImpulse = m_linearImpulse; - m_linearImpulse += impulse; - - float32 maxImpulse = h * m_maxForce; - - if (m_linearImpulse.LengthSquared() > maxImpulse * maxImpulse) - { - m_linearImpulse.Normalize(); - m_linearImpulse *= maxImpulse; - } - - impulse = m_linearImpulse - oldImpulse; - - vA -= mA * impulse; - wA -= iA * b2Cross(m_rA, impulse); - - vB += mB * impulse; - wB += iB * b2Cross(m_rB, impulse); - } - - data.velocities[m_indexA].v = vA; - data.velocities[m_indexA].w = wA; - data.velocities[m_indexB].v = vB; - data.velocities[m_indexB].w = wB; -} - -bool b2MotorJoint::SolvePositionConstraints(const b2SolverData& data) -{ - B2_NOT_USED(data); - - return true; -} - -b2Vec2 b2MotorJoint::GetAnchorA() const -{ - return m_bodyA->GetPosition(); -} - -b2Vec2 b2MotorJoint::GetAnchorB() const -{ - return m_bodyB->GetPosition(); -} - -b2Vec2 b2MotorJoint::GetReactionForce(float32 inv_dt) const -{ - return inv_dt * m_linearImpulse; -} - -float32 b2MotorJoint::GetReactionTorque(float32 inv_dt) const -{ - return inv_dt * m_angularImpulse; -} - -void b2MotorJoint::SetMaxForce(float32 force) -{ - b2Assert(b2IsValid(force) && force >= 0.0f); - m_maxForce = force; -} - -float32 b2MotorJoint::GetMaxForce() const -{ - return m_maxForce; -} - -void b2MotorJoint::SetMaxTorque(float32 torque) -{ - b2Assert(b2IsValid(torque) && torque >= 0.0f); - m_maxTorque = torque; -} - -float32 b2MotorJoint::GetMaxTorque() const -{ - return m_maxTorque; -} - -void b2MotorJoint::SetCorrectionFactor(float32 factor) -{ - b2Assert(b2IsValid(factor) && 0.0f <= factor && factor <= 1.0f); - m_correctionFactor = factor; -} - -float32 b2MotorJoint::GetCorrectionFactor() const -{ - return m_correctionFactor; -} - -void b2MotorJoint::SetLinearOffset(const b2Vec2& linearOffset) -{ - if (linearOffset.x != m_linearOffset.x || linearOffset.y != m_linearOffset.y) - { - m_bodyA->SetAwake(true); - m_bodyB->SetAwake(true); - m_linearOffset = linearOffset; - } -} - -const b2Vec2& b2MotorJoint::GetLinearOffset() const -{ - return m_linearOffset; -} - -void b2MotorJoint::SetAngularOffset(float32 angularOffset) -{ - if (angularOffset != m_angularOffset) - { - m_bodyA->SetAwake(true); - m_bodyB->SetAwake(true); - m_angularOffset = angularOffset; - } -} - -float32 b2MotorJoint::GetAngularOffset() const -{ - return m_angularOffset; -} - -void b2MotorJoint::Dump() -{ - int32 indexA = m_bodyA->m_islandIndex; - int32 indexB = m_bodyB->m_islandIndex; - - b2Log(" b2MotorJointDef jd;\n"); - b2Log(" jd.bodyA = bodies[%d];\n", indexA); - b2Log(" jd.bodyB = bodies[%d];\n", indexB); - b2Log(" jd.collideConnected = bool(%d);\n", m_collideConnected); - b2Log(" jd.linearOffset.Set(%.15lef, %.15lef);\n", m_linearOffset.x, m_linearOffset.y); - b2Log(" jd.angularOffset = %.15lef;\n", m_angularOffset); - b2Log(" jd.maxForce = %.15lef;\n", m_maxForce); - b2Log(" jd.maxTorque = %.15lef;\n", m_maxTorque); - b2Log(" jd.correctionFactor = %.15lef;\n", m_correctionFactor); - b2Log(" joints[%d] = m_world->CreateJoint(&jd);\n", m_index); -} +// MIT License + +// Copyright (c) 2019 Erin Catto + +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: + +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. + +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. + +#include "box2d/b2_body.h" +#include "box2d/b2_motor_joint.h" +#include "box2d/b2_time_step.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); + + float angleA = bodyA->GetAngle(); + float 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; + float aA = data.positions[m_indexA].a; + b2Vec2 vA = data.velocities[m_indexA].v; + float wA = data.velocities[m_indexA].w; + + b2Vec2 cB = data.positions[m_indexB].c; + float aB = data.positions[m_indexB].a; + b2Vec2 vB = data.velocities[m_indexB].v; + float 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] + + float mA = m_invMassA, mB = m_invMassB; + float 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; + float wA = data.velocities[m_indexA].w; + b2Vec2 vB = data.velocities[m_indexB].v; + float wB = data.velocities[m_indexB].w; + + float mA = m_invMassA, mB = m_invMassB; + float iA = m_invIA, iB = m_invIB; + + float h = data.step.dt; + float inv_h = data.step.inv_dt; + + // Solve angular friction + { + float Cdot = wB - wA + inv_h * m_correctionFactor * m_angularError; + float impulse = -m_angularMass * Cdot; + + float oldImpulse = m_angularImpulse; + float 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; + + float 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(float inv_dt) const +{ + return inv_dt * m_linearImpulse; +} + +float b2MotorJoint::GetReactionTorque(float inv_dt) const +{ + return inv_dt * m_angularImpulse; +} + +void b2MotorJoint::SetMaxForce(float force) +{ + b2Assert(b2IsValid(force) && force >= 0.0f); + m_maxForce = force; +} + +float b2MotorJoint::GetMaxForce() const +{ + return m_maxForce; +} + +void b2MotorJoint::SetMaxTorque(float torque) +{ + b2Assert(b2IsValid(torque) && torque >= 0.0f); + m_maxTorque = torque; +} + +float b2MotorJoint::GetMaxTorque() const +{ + return m_maxTorque; +} + +void b2MotorJoint::SetCorrectionFactor(float factor) +{ + b2Assert(b2IsValid(factor) && 0.0f <= factor && factor <= 1.0f); + m_correctionFactor = factor; +} + +float 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(float angularOffset) +{ + if (angularOffset != m_angularOffset) + { + m_bodyA->SetAwake(true); + m_bodyB->SetAwake(true); + m_angularOffset = angularOffset; + } +} + +float b2MotorJoint::GetAngularOffset() const +{ + return m_angularOffset; +} + +void b2MotorJoint::Dump() +{ + int32 indexA = m_bodyA->m_islandIndex; + int32 indexB = m_bodyB->m_islandIndex; + + b2Dump(" b2MotorJointDef jd;\n"); + b2Dump(" jd.bodyA = bodies[%d];\n", indexA); + b2Dump(" jd.bodyB = bodies[%d];\n", indexB); + b2Dump(" jd.collideConnected = bool(%d);\n", m_collideConnected); + b2Dump(" jd.linearOffset.Set(%.9g, %.9g);\n", m_linearOffset.x, m_linearOffset.y); + b2Dump(" jd.angularOffset = %.9g;\n", m_angularOffset); + b2Dump(" jd.maxForce = %.9g;\n", m_maxForce); + b2Dump(" jd.maxTorque = %.9g;\n", m_maxTorque); + b2Dump(" jd.correctionFactor = %.9g;\n", m_correctionFactor); + b2Dump(" joints[%d] = m_world->CreateJoint(&jd);\n", m_index); +} diff --git a/libs/Box2D/Dynamics/Joints/b2MouseJoint.cpp b/libs/box2d/src/dynamics/b2_mouse_joint.cpp similarity index 54% rename from libs/Box2D/Dynamics/Joints/b2MouseJoint.cpp rename to libs/box2d/src/dynamics/b2_mouse_joint.cpp index eb1de9d3..dbc214fa 100644 --- a/libs/Box2D/Dynamics/Joints/b2MouseJoint.cpp +++ b/libs/box2d/src/dynamics/b2_mouse_joint.cpp @@ -1,221 +1,189 @@ -/* -* Copyright (c) 2006-2007 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ - -#include -#include -#include - -// p = attached point, m = mouse point -// C = p - m -// Cdot = v -// = v + cross(w, r) -// J = [I r_skew] -// Identity used: -// w k % (rx i + ry j) = w * (-ry i + rx j) - -b2MouseJoint::b2MouseJoint(const b2MouseJointDef* def) -: b2Joint(def) -{ - b2Assert(def->target.IsValid()); - b2Assert(b2IsValid(def->maxForce) && def->maxForce >= 0.0f); - b2Assert(b2IsValid(def->frequencyHz) && def->frequencyHz >= 0.0f); - b2Assert(b2IsValid(def->dampingRatio) && def->dampingRatio >= 0.0f); - - m_targetA = def->target; - m_localAnchorB = b2MulT(m_bodyB->GetTransform(), m_targetA); - - m_maxForce = def->maxForce; - m_impulse.SetZero(); - - m_frequencyHz = def->frequencyHz; - m_dampingRatio = def->dampingRatio; - - m_beta = 0.0f; - m_gamma = 0.0f; -} - -void b2MouseJoint::SetTarget(const b2Vec2& target) -{ - if (m_bodyB->IsAwake() == false) - { - m_bodyB->SetAwake(true); - } - m_targetA = target; -} - -const b2Vec2& b2MouseJoint::GetTarget() const -{ - return m_targetA; -} - -void b2MouseJoint::SetMaxForce(float32 force) -{ - m_maxForce = force; -} - -float32 b2MouseJoint::GetMaxForce() const -{ - return m_maxForce; -} - -void b2MouseJoint::SetFrequency(float32 hz) -{ - m_frequencyHz = hz; -} - -float32 b2MouseJoint::GetFrequency() const -{ - return m_frequencyHz; -} - -void b2MouseJoint::SetDampingRatio(float32 ratio) -{ - m_dampingRatio = ratio; -} - -float32 b2MouseJoint::GetDampingRatio() const -{ - return m_dampingRatio; -} - -void b2MouseJoint::InitVelocityConstraints(const b2SolverData& data) -{ - m_indexB = m_bodyB->m_islandIndex; - m_localCenterB = m_bodyB->m_sweep.localCenter; - m_invMassB = m_bodyB->m_invMass; - m_invIB = m_bodyB->m_invI; - - b2Vec2 cB = data.positions[m_indexB].c; - float32 aB = data.positions[m_indexB].a; - b2Vec2 vB = data.velocities[m_indexB].v; - float32 wB = data.velocities[m_indexB].w; - - b2Rot qB(aB); - - float32 mass = m_bodyB->GetMass(); - - // Frequency - float32 omega = 2.0f * b2_pi * m_frequencyHz; - - // Damping coefficient - float32 d = 2.0f * mass * m_dampingRatio * omega; - - // Spring stiffness - float32 k = mass * (omega * omega); - - // magic formulas - // gamma has units of inverse mass. - // beta has units of inverse time. - float32 h = data.step.dt; - b2Assert(d + h * k > b2_epsilon); - m_gamma = h * (d + h * k); - if (m_gamma != 0.0f) - { - m_gamma = 1.0f / m_gamma; - } - m_beta = h * k * m_gamma; - - // Compute the effective mass matrix. - m_rB = b2Mul(qB, m_localAnchorB - m_localCenterB); - - // K = [(1/m1 + 1/m2) * eye(2) - skew(r1) * invI1 * skew(r1) - skew(r2) * invI2 * skew(r2)] - // = [1/m1+1/m2 0 ] + invI1 * [r1.y*r1.y -r1.x*r1.y] + invI2 * [r1.y*r1.y -r1.x*r1.y] - // [ 0 1/m1+1/m2] [-r1.x*r1.y r1.x*r1.x] [-r1.x*r1.y r1.x*r1.x] - b2Mat22 K; - K.ex.x = m_invMassB + m_invIB * m_rB.y * m_rB.y + m_gamma; - K.ex.y = -m_invIB * m_rB.x * m_rB.y; - K.ey.x = K.ex.y; - K.ey.y = m_invMassB + m_invIB * m_rB.x * m_rB.x + m_gamma; - - m_mass = K.GetInverse(); - - m_C = cB + m_rB - m_targetA; - m_C *= m_beta; - - // Cheat with some damping - wB *= 0.98f; - - if (data.step.warmStarting) - { - m_impulse *= data.step.dtRatio; - vB += m_invMassB * m_impulse; - wB += m_invIB * b2Cross(m_rB, m_impulse); - } - else - { - m_impulse.SetZero(); - } - - data.velocities[m_indexB].v = vB; - data.velocities[m_indexB].w = wB; -} - -void b2MouseJoint::SolveVelocityConstraints(const b2SolverData& data) -{ - b2Vec2 vB = data.velocities[m_indexB].v; - float32 wB = data.velocities[m_indexB].w; - - // Cdot = v + cross(w, r) - b2Vec2 Cdot = vB + b2Cross(wB, m_rB); - b2Vec2 impulse = b2Mul(m_mass, -(Cdot + m_C + m_gamma * m_impulse)); - - b2Vec2 oldImpulse = m_impulse; - m_impulse += impulse; - float32 maxImpulse = data.step.dt * m_maxForce; - if (m_impulse.LengthSquared() > maxImpulse * maxImpulse) - { - m_impulse *= maxImpulse / m_impulse.Length(); - } - impulse = m_impulse - oldImpulse; - - vB += m_invMassB * impulse; - wB += m_invIB * b2Cross(m_rB, impulse); - - data.velocities[m_indexB].v = vB; - data.velocities[m_indexB].w = wB; -} - -bool b2MouseJoint::SolvePositionConstraints(const b2SolverData& data) -{ - B2_NOT_USED(data); - return true; -} - -b2Vec2 b2MouseJoint::GetAnchorA() const -{ - return m_targetA; -} - -b2Vec2 b2MouseJoint::GetAnchorB() const -{ - return m_bodyB->GetWorldPoint(m_localAnchorB); -} - -b2Vec2 b2MouseJoint::GetReactionForce(float32 inv_dt) const -{ - return inv_dt * m_impulse; -} - -float32 b2MouseJoint::GetReactionTorque(float32 inv_dt) const -{ - return inv_dt * 0.0f; -} - +// MIT License + +// Copyright (c) 2019 Erin Catto + +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: + +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. + +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. + +#include "box2d/b2_body.h" +#include "box2d/b2_mouse_joint.h" +#include "box2d/b2_time_step.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) +{ + m_targetA = def->target; + m_localAnchorB = b2MulT(m_bodyB->GetTransform(), m_targetA); + m_maxForce = def->maxForce; + m_stiffness = def->stiffness; + m_damping = def->damping; + + m_impulse.SetZero(); + 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(float force) +{ + m_maxForce = force; +} + +float b2MouseJoint::GetMaxForce() const +{ + return m_maxForce; +} + +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; + float aB = data.positions[m_indexB].a; + b2Vec2 vB = data.velocities[m_indexB].v; + float wB = data.velocities[m_indexB].w; + + b2Rot qB(aB); + + float d = m_damping; + float k = m_stiffness; + + // magic formulas + // gamma has units of inverse mass. + // beta has units of inverse time. + float h = data.step.dt; + 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 *= b2Max(0.0f, 1.0f - 0.02f * (60.0f * data.step.dt)); + + 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; + float 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; + float 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(float inv_dt) const +{ + return inv_dt * m_impulse; +} + +float b2MouseJoint::GetReactionTorque(float inv_dt) const +{ + return inv_dt * 0.0f; +} + void b2MouseJoint::ShiftOrigin(const b2Vec2& newOrigin) { m_targetA -= newOrigin; diff --git a/libs/box2d/src/dynamics/b2_polygon_circle_contact.cpp b/libs/box2d/src/dynamics/b2_polygon_circle_contact.cpp new file mode 100644 index 00000000..e4f34f58 --- /dev/null +++ b/libs/box2d/src/dynamics/b2_polygon_circle_contact.cpp @@ -0,0 +1,54 @@ +// MIT License + +// Copyright (c) 2019 Erin Catto + +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: + +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. + +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. + +#include "b2_polygon_circle_contact.h" + +#include "box2d/b2_block_allocator.h" +#include "box2d/b2_fixture.h" + +#include + +b2Contact* b2PolygonAndCircleContact::Create(b2Fixture* fixtureA, int32, b2Fixture* fixtureB, int32, b2BlockAllocator* allocator) +{ + void* mem = allocator->Allocate(sizeof(b2PolygonAndCircleContact)); + return new (mem) b2PolygonAndCircleContact(fixtureA, fixtureB); +} + +void b2PolygonAndCircleContact::Destroy(b2Contact* contact, b2BlockAllocator* allocator) +{ + ((b2PolygonAndCircleContact*)contact)->~b2PolygonAndCircleContact(); + allocator->Free(contact, sizeof(b2PolygonAndCircleContact)); +} + +b2PolygonAndCircleContact::b2PolygonAndCircleContact(b2Fixture* fixtureA, b2Fixture* fixtureB) +: b2Contact(fixtureA, 0, fixtureB, 0) +{ + b2Assert(m_fixtureA->GetType() == b2Shape::e_polygon); + b2Assert(m_fixtureB->GetType() == b2Shape::e_circle); +} + +void b2PolygonAndCircleContact::Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB) +{ + b2CollidePolygonAndCircle( manifold, + (b2PolygonShape*)m_fixtureA->GetShape(), xfA, + (b2CircleShape*)m_fixtureB->GetShape(), xfB); +} diff --git a/libs/box2d/src/dynamics/b2_polygon_circle_contact.h b/libs/box2d/src/dynamics/b2_polygon_circle_contact.h new file mode 100644 index 00000000..6ae54251 --- /dev/null +++ b/libs/box2d/src/dynamics/b2_polygon_circle_contact.h @@ -0,0 +1,42 @@ +// MIT License + +// Copyright (c) 2019 Erin Catto + +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: + +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. + +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. + +#ifndef B2_POLYGON_AND_CIRCLE_CONTACT_H +#define B2_POLYGON_AND_CIRCLE_CONTACT_H + +#include "box2d/b2_contact.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/libs/box2d/src/dynamics/b2_polygon_contact.cpp b/libs/box2d/src/dynamics/b2_polygon_contact.cpp new file mode 100644 index 00000000..e92a9e80 --- /dev/null +++ b/libs/box2d/src/dynamics/b2_polygon_contact.cpp @@ -0,0 +1,57 @@ +// MIT License + +// Copyright (c) 2019 Erin Catto + +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: + +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. + +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. + +#include "b2_polygon_contact.h" + +#include "box2d/b2_block_allocator.h" +#include "box2d/b2_body.h" +#include "box2d/b2_fixture.h" +#include "box2d/b2_time_of_impact.h" +#include "box2d/b2_world_callbacks.h" + +#include + +b2Contact* b2PolygonContact::Create(b2Fixture* fixtureA, int32, b2Fixture* fixtureB, int32, b2BlockAllocator* allocator) +{ + void* mem = allocator->Allocate(sizeof(b2PolygonContact)); + return new (mem) b2PolygonContact(fixtureA, fixtureB); +} + +void b2PolygonContact::Destroy(b2Contact* contact, b2BlockAllocator* allocator) +{ + ((b2PolygonContact*)contact)->~b2PolygonContact(); + allocator->Free(contact, sizeof(b2PolygonContact)); +} + +b2PolygonContact::b2PolygonContact(b2Fixture* fixtureA, b2Fixture* fixtureB) + : b2Contact(fixtureA, 0, fixtureB, 0) +{ + b2Assert(m_fixtureA->GetType() == b2Shape::e_polygon); + b2Assert(m_fixtureB->GetType() == b2Shape::e_polygon); +} + +void b2PolygonContact::Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB) +{ + b2CollidePolygons( manifold, + (b2PolygonShape*)m_fixtureA->GetShape(), xfA, + (b2PolygonShape*)m_fixtureB->GetShape(), xfB); +} diff --git a/libs/box2d/src/dynamics/b2_polygon_contact.h b/libs/box2d/src/dynamics/b2_polygon_contact.h new file mode 100644 index 00000000..0516cb0e --- /dev/null +++ b/libs/box2d/src/dynamics/b2_polygon_contact.h @@ -0,0 +1,43 @@ +// MIT License + +// Copyright (c) 2019 Erin Catto + +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: + +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. + +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. + +#ifndef B2_POLYGON_CONTACT_H +#define B2_POLYGON_CONTACT_H + +#include "box2d/b2_contact.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/libs/box2d/src/dynamics/b2_prismatic_joint.cpp b/libs/box2d/src/dynamics/b2_prismatic_joint.cpp new file mode 100644 index 00000000..00e77690 --- /dev/null +++ b/libs/box2d/src/dynamics/b2_prismatic_joint.cpp @@ -0,0 +1,643 @@ +// MIT License + +// Copyright (c) 2019 Erin Catto + +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: + +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. + +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. + +#include "box2d/b2_body.h" +#include "box2d/b2_draw.h" +#include "box2d/b2_prismatic_joint.h" +#include "box2d/b2_time_step.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)] + +// Predictive limit is applied even when the limit is not active. +// Prevents a constraint speed that can lead to a constraint error in one time step. +// Want C2 = C1 + h * Cdot >= 0 +// Or: +// Cdot + C1/h >= 0 +// I do not apply a negative constraint error because that is handled in position correction. +// So: +// Cdot + max(C1, 0)/h >= 0 + +// Block Solver +// We develop a block solver that includes the angular and linear constraints. This makes the limit stiffer. +// +// The Jacobian has 2 rows: +// J = [-uT -s1 uT s2] // linear +// [0 -1 0 1] // angular +// +// u = perp +// s1 = cross(d + r1, u), s2 = cross(r2, u) +// a1 = cross(d + r1, v), a2 = cross(r2, v) + +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_axialMass = 0.0f; + m_motorImpulse = 0.0f; + m_lowerImpulse = 0.0f; + m_upperImpulse = 0.0f; + + m_lowerTranslation = def->lowerTranslation; + m_upperTranslation = def->upperTranslation; + + b2Assert(m_lowerTranslation <= m_upperTranslation); + + m_maxMotorForce = def->maxMotorForce; + m_motorSpeed = def->motorSpeed; + m_enableLimit = def->enableLimit; + m_enableMotor = def->enableMotor; + + m_translation = 0.0f; + 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; + float aA = data.positions[m_indexA].a; + b2Vec2 vA = data.velocities[m_indexA].v; + float wA = data.velocities[m_indexA].w; + + b2Vec2 cB = data.positions[m_indexB].c; + float aB = data.positions[m_indexB].a; + b2Vec2 vB = data.velocities[m_indexB].v; + float 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; + + float mA = m_invMassA, mB = m_invMassB; + float 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_axialMass = mA + mB + iA * m_a1 * m_a1 + iB * m_a2 * m_a2; + if (m_axialMass > 0.0f) + { + m_axialMass = 1.0f / m_axialMass; + } + } + + // Prismatic constraint. + { + m_perp = b2Mul(qA, m_localYAxisA); + + m_s1 = b2Cross(d + rA, m_perp); + m_s2 = b2Cross(rB, m_perp); + + float k11 = mA + mB + iA * m_s1 * m_s1 + iB * m_s2 * m_s2; + float k12 = iA * m_s1 + iB * m_s2; + float k22 = iA + iB; + if (k22 == 0.0f) + { + // For bodies with fixed rotation. + k22 = 1.0f; + } + + m_K.ex.Set(k11, k12); + m_K.ey.Set(k12, k22); + } + + if (m_enableLimit) + { + m_translation = b2Dot(m_axis, d); + } + else + { + m_lowerImpulse = 0.0f; + m_upperImpulse = 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; + m_lowerImpulse *= data.step.dtRatio; + m_upperImpulse *= data.step.dtRatio; + + float axialImpulse = m_motorImpulse + m_lowerImpulse - m_upperImpulse; + b2Vec2 P = m_impulse.x * m_perp + axialImpulse * m_axis; + float LA = m_impulse.x * m_s1 + m_impulse.y + axialImpulse * m_a1; + float LB = m_impulse.x * m_s2 + m_impulse.y + axialImpulse * m_a2; + + vA -= mA * P; + wA -= iA * LA; + + vB += mB * P; + wB += iB * LB; + } + else + { + m_impulse.SetZero(); + m_motorImpulse = 0.0f; + m_lowerImpulse = 0.0f; + m_upperImpulse = 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; + float wA = data.velocities[m_indexA].w; + b2Vec2 vB = data.velocities[m_indexB].v; + float wB = data.velocities[m_indexB].w; + + float mA = m_invMassA, mB = m_invMassB; + float iA = m_invIA, iB = m_invIB; + + // Solve linear motor constraint + if (m_enableMotor) + { + float Cdot = b2Dot(m_axis, vB - vA) + m_a2 * wB - m_a1 * wA; + float impulse = m_axialMass * (m_motorSpeed - Cdot); + float oldImpulse = m_motorImpulse; + float maxImpulse = data.step.dt * m_maxMotorForce; + m_motorImpulse = b2Clamp(m_motorImpulse + impulse, -maxImpulse, maxImpulse); + impulse = m_motorImpulse - oldImpulse; + + b2Vec2 P = impulse * m_axis; + float LA = impulse * m_a1; + float LB = impulse * m_a2; + + vA -= mA * P; + wA -= iA * LA; + vB += mB * P; + wB += iB * LB; + } + + if (m_enableLimit) + { + // Lower limit + { + float C = m_translation - m_lowerTranslation; + float Cdot = b2Dot(m_axis, vB - vA) + m_a2 * wB - m_a1 * wA; + float impulse = -m_axialMass * (Cdot + b2Max(C, 0.0f) * data.step.inv_dt); + float oldImpulse = m_lowerImpulse; + m_lowerImpulse = b2Max(m_lowerImpulse + impulse, 0.0f); + impulse = m_lowerImpulse - oldImpulse; + + b2Vec2 P = impulse * m_axis; + float LA = impulse * m_a1; + float LB = impulse * m_a2; + + vA -= mA * P; + wA -= iA * LA; + vB += mB * P; + wB += iB * LB; + } + + // Upper limit + // Note: signs are flipped to keep C positive when the constraint is satisfied. + // This also keeps the impulse positive when the limit is active. + { + float C = m_upperTranslation - m_translation; + float Cdot = b2Dot(m_axis, vA - vB) + m_a1 * wA - m_a2 * wB; + float impulse = -m_axialMass * (Cdot + b2Max(C, 0.0f) * data.step.inv_dt); + float oldImpulse = m_upperImpulse; + m_upperImpulse = b2Max(m_upperImpulse + impulse, 0.0f); + impulse = m_upperImpulse - oldImpulse; + + b2Vec2 P = impulse * m_axis; + float LA = impulse * m_a1; + float LB = impulse * m_a2; + + vA += mA * P; + wA += iA * LA; + vB -= mB * P; + wB -= iB * LB; + } + } + + // Solve the prismatic constraint in block form. + { + b2Vec2 Cdot; + Cdot.x = b2Dot(m_perp, vB - vA) + m_s2 * wB - m_s1 * wA; + Cdot.y = wB - wA; + + b2Vec2 df = m_K.Solve(-Cdot); + m_impulse += df; + + b2Vec2 P = df.x * m_perp; + float LA = df.x * m_s1 + df.y; + float 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; + float aA = data.positions[m_indexA].a; + b2Vec2 cB = data.positions[m_indexB].c; + float aB = data.positions[m_indexB].a; + + b2Rot qA(aA), qB(aB); + + float mA = m_invMassA, mB = m_invMassB; + float 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); + float a1 = b2Cross(d + rA, axis); + float a2 = b2Cross(rB, axis); + b2Vec2 perp = b2Mul(qA, m_localYAxisA); + + float s1 = b2Cross(d + rA, perp); + float s2 = b2Cross(rB, perp); + + b2Vec3 impulse; + b2Vec2 C1; + C1.x = b2Dot(perp, d); + C1.y = aB - aA - m_referenceAngle; + + float linearError = b2Abs(C1.x); + float angularError = b2Abs(C1.y); + + bool active = false; + float C2 = 0.0f; + if (m_enableLimit) + { + float translation = b2Dot(axis, d); + if (b2Abs(m_upperTranslation - m_lowerTranslation) < 2.0f * b2_linearSlop) + { + C2 = translation; + linearError = b2Max(linearError, b2Abs(translation)); + active = true; + } + else if (translation <= m_lowerTranslation) + { + C2 = b2Min(translation - m_lowerTranslation, 0.0f); + linearError = b2Max(linearError, m_lowerTranslation - translation); + active = true; + } + else if (translation >= m_upperTranslation) + { + C2 = b2Max(translation - m_upperTranslation, 0.0f); + linearError = b2Max(linearError, translation - m_upperTranslation); + active = true; + } + } + + if (active) + { + float k11 = mA + mB + iA * s1 * s1 + iB * s2 * s2; + float k12 = iA * s1 + iB * s2; + float k13 = iA * s1 * a1 + iB * s2 * a2; + float k22 = iA + iB; + if (k22 == 0.0f) + { + // For fixed rotation + k22 = 1.0f; + } + float k23 = iA * a1 + iB * a2; + float 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 + { + float k11 = mA + mB + iA * s1 * s1 + iB * s2 * s2; + float k12 = iA * s1 + iB * s2; + float 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; + float LA = impulse.x * s1 + impulse.y + impulse.z * a1; + float 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(float inv_dt) const +{ + return inv_dt * (m_impulse.x * m_perp + (m_motorImpulse + m_lowerImpulse - m_upperImpulse) * m_axis); +} + +float b2PrismaticJoint::GetReactionTorque(float inv_dt) const +{ + return inv_dt * m_impulse.y; +} + +float 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); + + float translation = b2Dot(d, axis); + return translation; +} + +float 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; + float wA = bA->m_angularVelocity; + float wB = bB->m_angularVelocity; + + float 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_lowerImpulse = 0.0f; + m_upperImpulse = 0.0f; + } +} + +float b2PrismaticJoint::GetLowerLimit() const +{ + return m_lowerTranslation; +} + +float b2PrismaticJoint::GetUpperLimit() const +{ + return m_upperTranslation; +} + +void b2PrismaticJoint::SetLimits(float lower, float 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_lowerImpulse = 0.0f; + m_upperImpulse = 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(float speed) +{ + if (speed != m_motorSpeed) + { + m_bodyA->SetAwake(true); + m_bodyB->SetAwake(true); + m_motorSpeed = speed; + } +} + +void b2PrismaticJoint::SetMaxMotorForce(float force) +{ + if (force != m_maxMotorForce) + { + m_bodyA->SetAwake(true); + m_bodyB->SetAwake(true); + m_maxMotorForce = force; + } +} + +float b2PrismaticJoint::GetMotorForce(float inv_dt) const +{ + return inv_dt * m_motorImpulse; +} + +void b2PrismaticJoint::Dump() +{ + // FLT_DECIMAL_DIG == 9 + + int32 indexA = m_bodyA->m_islandIndex; + int32 indexB = m_bodyB->m_islandIndex; + + b2Dump(" b2PrismaticJointDef jd;\n"); + b2Dump(" jd.bodyA = bodies[%d];\n", indexA); + b2Dump(" jd.bodyB = bodies[%d];\n", indexB); + b2Dump(" jd.collideConnected = bool(%d);\n", m_collideConnected); + b2Dump(" jd.localAnchorA.Set(%.9g, %.9g);\n", m_localAnchorA.x, m_localAnchorA.y); + b2Dump(" jd.localAnchorB.Set(%.9g, %.9g);\n", m_localAnchorB.x, m_localAnchorB.y); + b2Dump(" jd.localAxisA.Set(%.9g, %.9g);\n", m_localXAxisA.x, m_localXAxisA.y); + b2Dump(" jd.referenceAngle = %.9g;\n", m_referenceAngle); + b2Dump(" jd.enableLimit = bool(%d);\n", m_enableLimit); + b2Dump(" jd.lowerTranslation = %.9g;\n", m_lowerTranslation); + b2Dump(" jd.upperTranslation = %.9g;\n", m_upperTranslation); + b2Dump(" jd.enableMotor = bool(%d);\n", m_enableMotor); + b2Dump(" jd.motorSpeed = %.9g;\n", m_motorSpeed); + b2Dump(" jd.maxMotorForce = %.9g;\n", m_maxMotorForce); + b2Dump(" joints[%d] = m_world->CreateJoint(&jd);\n", m_index); +} + +void b2PrismaticJoint::Draw(b2Draw* draw) const +{ + const b2Transform& xfA = m_bodyA->GetTransform(); + const b2Transform& xfB = m_bodyB->GetTransform(); + b2Vec2 pA = b2Mul(xfA, m_localAnchorA); + b2Vec2 pB = b2Mul(xfB, m_localAnchorB); + + b2Vec2 axis = b2Mul(xfA.q, m_localXAxisA); + + b2Color c1(0.7f, 0.7f, 0.7f); + b2Color c2(0.3f, 0.9f, 0.3f); + b2Color c3(0.9f, 0.3f, 0.3f); + b2Color c4(0.3f, 0.3f, 0.9f); + b2Color c5(0.4f, 0.4f, 0.4f); + + draw->DrawSegment(pA, pB, c5); + + if (m_enableLimit) + { + b2Vec2 lower = pA + m_lowerTranslation * axis; + b2Vec2 upper = pA + m_upperTranslation * axis; + b2Vec2 perp = b2Mul(xfA.q, m_localYAxisA); + draw->DrawSegment(lower, upper, c1); + draw->DrawSegment(lower - 0.5f * perp, lower + 0.5f * perp, c2); + draw->DrawSegment(upper - 0.5f * perp, upper + 0.5f * perp, c3); + } + else + { + draw->DrawSegment(pA - 1.0f * axis, pA + 1.0f * axis, c1); + } + + draw->DrawPoint(pA, 5.0f, c1); + draw->DrawPoint(pB, 5.0f, c4); +} diff --git a/libs/Box2D/Dynamics/Joints/b2PulleyJoint.cpp b/libs/box2d/src/dynamics/b2_pulley_joint.cpp similarity index 61% rename from libs/Box2D/Dynamics/Joints/b2PulleyJoint.cpp rename to libs/box2d/src/dynamics/b2_pulley_joint.cpp index f8b22853..099e57e4 100644 --- a/libs/Box2D/Dynamics/Joints/b2PulleyJoint.cpp +++ b/libs/box2d/src/dynamics/b2_pulley_joint.cpp @@ -1,346 +1,350 @@ -/* -* Copyright (c) 2007 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ - -#include -#include -#include - -// Pulley: -// length1 = norm(p1 - s1) -// length2 = norm(p2 - s2) -// C0 = (length1 + ratio * length2)_initial -// C = C0 - (length1 + ratio * length2) -// u1 = (p1 - s1) / norm(p1 - s1) -// u2 = (p2 - s2) / norm(p2 - s2) -// Cdot = -dot(u1, v1 + cross(w1, r1)) - ratio * dot(u2, v2 + cross(w2, r2)) -// J = -[u1 cross(r1, u1) ratio * u2 ratio * cross(r2, u2)] -// K = J * invM * JT -// = invMass1 + invI1 * cross(r1, u1)^2 + ratio^2 * (invMass2 + invI2 * cross(r2, u2)^2) - -void b2PulleyJointDef::Initialize(b2Body* bA, b2Body* bB, - const b2Vec2& groundA, const b2Vec2& groundB, - const b2Vec2& anchorA, const b2Vec2& anchorB, - float32 r) -{ - bodyA = bA; - bodyB = bB; - groundAnchorA = groundA; - groundAnchorB = groundB; - localAnchorA = bodyA->GetLocalPoint(anchorA); - localAnchorB = bodyB->GetLocalPoint(anchorB); - b2Vec2 dA = anchorA - groundA; - lengthA = dA.Length(); - b2Vec2 dB = anchorB - groundB; - lengthB = dB.Length(); - ratio = r; - b2Assert(ratio > b2_epsilon); -} - -b2PulleyJoint::b2PulleyJoint(const b2PulleyJointDef* def) -: b2Joint(def) -{ - m_groundAnchorA = def->groundAnchorA; - m_groundAnchorB = def->groundAnchorB; - m_localAnchorA = def->localAnchorA; - m_localAnchorB = def->localAnchorB; - - m_lengthA = def->lengthA; - m_lengthB = def->lengthB; - - b2Assert(def->ratio != 0.0f); - m_ratio = def->ratio; - - m_constant = def->lengthA + m_ratio * def->lengthB; - - m_impulse = 0.0f; -} - -void b2PulleyJoint::InitVelocityConstraints(const b2SolverData& data) -{ - m_indexA = m_bodyA->m_islandIndex; - m_indexB = m_bodyB->m_islandIndex; - m_localCenterA = m_bodyA->m_sweep.localCenter; - m_localCenterB = m_bodyB->m_sweep.localCenter; - m_invMassA = m_bodyA->m_invMass; - m_invMassB = m_bodyB->m_invMass; - m_invIA = m_bodyA->m_invI; - m_invIB = m_bodyB->m_invI; - - b2Vec2 cA = data.positions[m_indexA].c; - float32 aA = data.positions[m_indexA].a; - b2Vec2 vA = data.velocities[m_indexA].v; - float32 wA = data.velocities[m_indexA].w; - - b2Vec2 cB = data.positions[m_indexB].c; - float32 aB = data.positions[m_indexB].a; - b2Vec2 vB = data.velocities[m_indexB].v; - float32 wB = data.velocities[m_indexB].w; - - b2Rot qA(aA), qB(aB); - - m_rA = b2Mul(qA, m_localAnchorA - m_localCenterA); - m_rB = b2Mul(qB, m_localAnchorB - m_localCenterB); - - // Get the pulley axes. - m_uA = cA + m_rA - m_groundAnchorA; - m_uB = cB + m_rB - m_groundAnchorB; - - float32 lengthA = m_uA.Length(); - float32 lengthB = m_uB.Length(); - - if (lengthA > 10.0f * b2_linearSlop) - { - m_uA *= 1.0f / lengthA; - } - else - { - m_uA.SetZero(); - } - - if (lengthB > 10.0f * b2_linearSlop) - { - m_uB *= 1.0f / lengthB; - } - else - { - m_uB.SetZero(); - } - - // Compute effective mass. - float32 ruA = b2Cross(m_rA, m_uA); - float32 ruB = b2Cross(m_rB, m_uB); - - float32 mA = m_invMassA + m_invIA * ruA * ruA; - float32 mB = m_invMassB + m_invIB * ruB * ruB; - - m_mass = mA + m_ratio * m_ratio * mB; - - if (m_mass > 0.0f) - { - m_mass = 1.0f / m_mass; - } - - if (data.step.warmStarting) - { - // Scale impulses to support variable time steps. - m_impulse *= data.step.dtRatio; - - // Warm starting. - b2Vec2 PA = -(m_impulse) * m_uA; - b2Vec2 PB = (-m_ratio * m_impulse) * m_uB; - - vA += m_invMassA * PA; - wA += m_invIA * b2Cross(m_rA, PA); - vB += m_invMassB * PB; - wB += m_invIB * b2Cross(m_rB, PB); - } - else - { - m_impulse = 0.0f; - } - - data.velocities[m_indexA].v = vA; - data.velocities[m_indexA].w = wA; - data.velocities[m_indexB].v = vB; - data.velocities[m_indexB].w = wB; -} - -void b2PulleyJoint::SolveVelocityConstraints(const b2SolverData& data) -{ - b2Vec2 vA = data.velocities[m_indexA].v; - float32 wA = data.velocities[m_indexA].w; - b2Vec2 vB = data.velocities[m_indexB].v; - float32 wB = data.velocities[m_indexB].w; - - b2Vec2 vpA = vA + b2Cross(wA, m_rA); - b2Vec2 vpB = vB + b2Cross(wB, m_rB); - - float32 Cdot = -b2Dot(m_uA, vpA) - m_ratio * b2Dot(m_uB, vpB); - float32 impulse = -m_mass * Cdot; - m_impulse += impulse; - - b2Vec2 PA = -impulse * m_uA; - b2Vec2 PB = -m_ratio * impulse * m_uB; - vA += m_invMassA * PA; - wA += m_invIA * b2Cross(m_rA, PA); - vB += m_invMassB * PB; - wB += m_invIB * b2Cross(m_rB, PB); - - data.velocities[m_indexA].v = vA; - data.velocities[m_indexA].w = wA; - data.velocities[m_indexB].v = vB; - data.velocities[m_indexB].w = wB; -} - -bool b2PulleyJoint::SolvePositionConstraints(const b2SolverData& data) -{ - b2Vec2 cA = data.positions[m_indexA].c; - float32 aA = data.positions[m_indexA].a; - b2Vec2 cB = data.positions[m_indexB].c; - float32 aB = data.positions[m_indexB].a; - - b2Rot qA(aA), qB(aB); - - b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_localCenterA); - b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_localCenterB); - - // Get the pulley axes. - b2Vec2 uA = cA + rA - m_groundAnchorA; - b2Vec2 uB = cB + rB - m_groundAnchorB; - - float32 lengthA = uA.Length(); - float32 lengthB = uB.Length(); - - if (lengthA > 10.0f * b2_linearSlop) - { - uA *= 1.0f / lengthA; - } - else - { - uA.SetZero(); - } - - if (lengthB > 10.0f * b2_linearSlop) - { - uB *= 1.0f / lengthB; - } - else - { - uB.SetZero(); - } - - // Compute effective mass. - float32 ruA = b2Cross(rA, uA); - float32 ruB = b2Cross(rB, uB); - - float32 mA = m_invMassA + m_invIA * ruA * ruA; - float32 mB = m_invMassB + m_invIB * ruB * ruB; - - float32 mass = mA + m_ratio * m_ratio * mB; - - if (mass > 0.0f) - { - mass = 1.0f / mass; - } - - float32 C = m_constant - lengthA - m_ratio * lengthB; - float32 linearError = b2Abs(C); - - float32 impulse = -mass * C; - - b2Vec2 PA = -impulse * uA; - b2Vec2 PB = -m_ratio * impulse * uB; - - cA += m_invMassA * PA; - aA += m_invIA * b2Cross(rA, PA); - cB += m_invMassB * PB; - aB += m_invIB * b2Cross(rB, PB); - - data.positions[m_indexA].c = cA; - data.positions[m_indexA].a = aA; - data.positions[m_indexB].c = cB; - data.positions[m_indexB].a = aB; - - return linearError < b2_linearSlop; -} - -b2Vec2 b2PulleyJoint::GetAnchorA() const -{ - return m_bodyA->GetWorldPoint(m_localAnchorA); -} - -b2Vec2 b2PulleyJoint::GetAnchorB() const -{ - return m_bodyB->GetWorldPoint(m_localAnchorB); -} - -b2Vec2 b2PulleyJoint::GetReactionForce(float32 inv_dt) const -{ - b2Vec2 P = m_impulse * m_uB; - return inv_dt * P; -} - -float32 b2PulleyJoint::GetReactionTorque(float32 inv_dt) const -{ - B2_NOT_USED(inv_dt); - return 0.0f; -} - -b2Vec2 b2PulleyJoint::GetGroundAnchorA() const -{ - return m_groundAnchorA; -} - -b2Vec2 b2PulleyJoint::GetGroundAnchorB() const -{ - return m_groundAnchorB; -} - -float32 b2PulleyJoint::GetLengthA() const -{ - return m_lengthA; -} - -float32 b2PulleyJoint::GetLengthB() const -{ - return m_lengthB; -} - -float32 b2PulleyJoint::GetRatio() const -{ - return m_ratio; -} - -float32 b2PulleyJoint::GetCurrentLengthA() const -{ - b2Vec2 p = m_bodyA->GetWorldPoint(m_localAnchorA); - b2Vec2 s = m_groundAnchorA; - b2Vec2 d = p - s; - return d.Length(); -} - -float32 b2PulleyJoint::GetCurrentLengthB() const -{ - b2Vec2 p = m_bodyB->GetWorldPoint(m_localAnchorB); - b2Vec2 s = m_groundAnchorB; - b2Vec2 d = p - s; - return d.Length(); -} - -void b2PulleyJoint::Dump() -{ - int32 indexA = m_bodyA->m_islandIndex; - int32 indexB = m_bodyB->m_islandIndex; - - b2Log(" b2PulleyJointDef jd;\n"); - b2Log(" jd.bodyA = bodies[%d];\n", indexA); - b2Log(" jd.bodyB = bodies[%d];\n", indexB); - b2Log(" jd.collideConnected = bool(%d);\n", m_collideConnected); - b2Log(" jd.groundAnchorA.Set(%.15lef, %.15lef);\n", m_groundAnchorA.x, m_groundAnchorA.y); - b2Log(" jd.groundAnchorB.Set(%.15lef, %.15lef);\n", m_groundAnchorB.x, m_groundAnchorB.y); - b2Log(" jd.localAnchorA.Set(%.15lef, %.15lef);\n", m_localAnchorA.x, m_localAnchorA.y); - b2Log(" jd.localAnchorB.Set(%.15lef, %.15lef);\n", m_localAnchorB.x, m_localAnchorB.y); - b2Log(" jd.lengthA = %.15lef;\n", m_lengthA); - b2Log(" jd.lengthB = %.15lef;\n", m_lengthB); - b2Log(" jd.ratio = %.15lef;\n", m_ratio); - b2Log(" joints[%d] = m_world->CreateJoint(&jd);\n", m_index); -} - +// MIT License + +// Copyright (c) 2019 Erin Catto + +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: + +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. + +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. + +#include "box2d/b2_body.h" +#include "box2d/b2_pulley_joint.h" +#include "box2d/b2_time_step.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, + float 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; + float aA = data.positions[m_indexA].a; + b2Vec2 vA = data.velocities[m_indexA].v; + float wA = data.velocities[m_indexA].w; + + b2Vec2 cB = data.positions[m_indexB].c; + float aB = data.positions[m_indexB].a; + b2Vec2 vB = data.velocities[m_indexB].v; + float 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; + + float lengthA = m_uA.Length(); + float 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. + float ruA = b2Cross(m_rA, m_uA); + float ruB = b2Cross(m_rB, m_uB); + + float mA = m_invMassA + m_invIA * ruA * ruA; + float 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; + float wA = data.velocities[m_indexA].w; + b2Vec2 vB = data.velocities[m_indexB].v; + float wB = data.velocities[m_indexB].w; + + b2Vec2 vpA = vA + b2Cross(wA, m_rA); + b2Vec2 vpB = vB + b2Cross(wB, m_rB); + + float Cdot = -b2Dot(m_uA, vpA) - m_ratio * b2Dot(m_uB, vpB); + float 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; + float aA = data.positions[m_indexA].a; + b2Vec2 cB = data.positions[m_indexB].c; + float 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; + + float lengthA = uA.Length(); + float 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. + float ruA = b2Cross(rA, uA); + float ruB = b2Cross(rB, uB); + + float mA = m_invMassA + m_invIA * ruA * ruA; + float mB = m_invMassB + m_invIB * ruB * ruB; + + float mass = mA + m_ratio * m_ratio * mB; + + if (mass > 0.0f) + { + mass = 1.0f / mass; + } + + float C = m_constant - lengthA - m_ratio * lengthB; + float linearError = b2Abs(C); + + float 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(float inv_dt) const +{ + b2Vec2 P = m_impulse * m_uB; + return inv_dt * P; +} + +float b2PulleyJoint::GetReactionTorque(float 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; +} + +float b2PulleyJoint::GetLengthA() const +{ + return m_lengthA; +} + +float b2PulleyJoint::GetLengthB() const +{ + return m_lengthB; +} + +float b2PulleyJoint::GetRatio() const +{ + return m_ratio; +} + +float b2PulleyJoint::GetCurrentLengthA() const +{ + b2Vec2 p = m_bodyA->GetWorldPoint(m_localAnchorA); + b2Vec2 s = m_groundAnchorA; + b2Vec2 d = p - s; + return d.Length(); +} + +float 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; + + b2Dump(" b2PulleyJointDef jd;\n"); + b2Dump(" jd.bodyA = bodies[%d];\n", indexA); + b2Dump(" jd.bodyB = bodies[%d];\n", indexB); + b2Dump(" jd.collideConnected = bool(%d);\n", m_collideConnected); + b2Dump(" jd.groundAnchorA.Set(%.9g, %.9g);\n", m_groundAnchorA.x, m_groundAnchorA.y); + b2Dump(" jd.groundAnchorB.Set(%.9g, %.9g);\n", m_groundAnchorB.x, m_groundAnchorB.y); + b2Dump(" jd.localAnchorA.Set(%.9g, %.9g);\n", m_localAnchorA.x, m_localAnchorA.y); + b2Dump(" jd.localAnchorB.Set(%.9g, %.9g);\n", m_localAnchorB.x, m_localAnchorB.y); + b2Dump(" jd.lengthA = %.9g;\n", m_lengthA); + b2Dump(" jd.lengthB = %.9g;\n", m_lengthB); + b2Dump(" jd.ratio = %.9g;\n", m_ratio); + b2Dump(" joints[%d] = m_world->CreateJoint(&jd);\n", m_index); +} + void b2PulleyJoint::ShiftOrigin(const b2Vec2& newOrigin) { m_groundAnchorA -= newOrigin; diff --git a/libs/box2d/src/dynamics/b2_revolute_joint.cpp b/libs/box2d/src/dynamics/b2_revolute_joint.cpp new file mode 100644 index 00000000..f7cc4cc2 --- /dev/null +++ b/libs/box2d/src/dynamics/b2_revolute_joint.cpp @@ -0,0 +1,501 @@ +// MIT License + +// Copyright (c) 2019 Erin Catto + +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: + +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. + +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. + +#include "box2d/b2_body.h" +#include "box2d/b2_draw.h" +#include "box2d/b2_revolute_joint.h" +#include "box2d/b2_time_step.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_axialMass = 0.0f; + m_motorImpulse = 0.0f; + m_lowerImpulse = 0.0f; + m_upperImpulse = 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_angle = 0.0f; +} + +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; + + float aA = data.positions[m_indexA].a; + b2Vec2 vA = data.velocities[m_indexA].v; + float wA = data.velocities[m_indexA].w; + + float aB = data.positions[m_indexB].a; + b2Vec2 vB = data.velocities[m_indexB].v; + float 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] + // r_skew = [-ry; rx] + + // Matlab + // K = [ mA+r1y^2*iA+mB+r2y^2*iB, -r1y*iA*r1x-r2y*iB*r2x] + // [ -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB] + + float mA = m_invMassA, mB = m_invMassB; + float iA = m_invIA, iB = m_invIB; + + m_K.ex.x = mA + mB + m_rA.y * m_rA.y * iA + m_rB.y * m_rB.y * iB; + m_K.ey.x = -m_rA.y * m_rA.x * iA - m_rB.y * m_rB.x * iB; + m_K.ex.y = m_K.ey.x; + m_K.ey.y = mA + mB + m_rA.x * m_rA.x * iA + m_rB.x * m_rB.x * iB; + + m_axialMass = iA + iB; + bool fixedRotation; + if (m_axialMass > 0.0f) + { + m_axialMass = 1.0f / m_axialMass; + fixedRotation = false; + } + else + { + fixedRotation = true; + } + + m_angle = aB - aA - m_referenceAngle; + if (m_enableLimit == false || fixedRotation) + { + m_lowerImpulse = 0.0f; + m_upperImpulse = 0.0f; + } + + if (m_enableMotor == false || fixedRotation) + { + m_motorImpulse = 0.0f; + } + + if (data.step.warmStarting) + { + // Scale impulses to support a variable time step. + m_impulse *= data.step.dtRatio; + m_motorImpulse *= data.step.dtRatio; + m_lowerImpulse *= data.step.dtRatio; + m_upperImpulse *= data.step.dtRatio; + + float axialImpulse = m_motorImpulse + m_lowerImpulse - m_upperImpulse; + b2Vec2 P(m_impulse.x, m_impulse.y); + + vA -= mA * P; + wA -= iA * (b2Cross(m_rA, P) + axialImpulse); + + vB += mB * P; + wB += iB * (b2Cross(m_rB, P) + axialImpulse); + } + else + { + m_impulse.SetZero(); + m_motorImpulse = 0.0f; + m_lowerImpulse = 0.0f; + m_upperImpulse = 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; + float wA = data.velocities[m_indexA].w; + b2Vec2 vB = data.velocities[m_indexB].v; + float wB = data.velocities[m_indexB].w; + + float mA = m_invMassA, mB = m_invMassB; + float iA = m_invIA, iB = m_invIB; + + bool fixedRotation = (iA + iB == 0.0f); + + // Solve motor constraint. + if (m_enableMotor && fixedRotation == false) + { + float Cdot = wB - wA - m_motorSpeed; + float impulse = -m_axialMass * Cdot; + float oldImpulse = m_motorImpulse; + float maxImpulse = data.step.dt * m_maxMotorTorque; + m_motorImpulse = b2Clamp(m_motorImpulse + impulse, -maxImpulse, maxImpulse); + impulse = m_motorImpulse - oldImpulse; + + wA -= iA * impulse; + wB += iB * impulse; + } + + if (m_enableLimit && fixedRotation == false) + { + // Lower limit + { + float C = m_angle - m_lowerAngle; + float Cdot = wB - wA; + float impulse = -m_axialMass * (Cdot + b2Max(C, 0.0f) * data.step.inv_dt); + float oldImpulse = m_lowerImpulse; + m_lowerImpulse = b2Max(m_lowerImpulse + impulse, 0.0f); + impulse = m_lowerImpulse - oldImpulse; + + wA -= iA * impulse; + wB += iB * impulse; + } + + // Upper limit + // Note: signs are flipped to keep C positive when the constraint is satisfied. + // This also keeps the impulse positive when the limit is active. + { + float C = m_upperAngle - m_angle; + float Cdot = wA - wB; + float impulse = -m_axialMass * (Cdot + b2Max(C, 0.0f) * data.step.inv_dt); + float oldImpulse = m_upperImpulse; + m_upperImpulse = b2Max(m_upperImpulse + impulse, 0.0f); + impulse = m_upperImpulse - oldImpulse; + + wA += iA * impulse; + wB -= iB * impulse; + } + } + + // Solve point-to-point constraint + { + b2Vec2 Cdot = vB + b2Cross(wB, m_rB) - vA - b2Cross(wA, m_rA); + b2Vec2 impulse = m_K.Solve(-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; + float aA = data.positions[m_indexA].a; + b2Vec2 cB = data.positions[m_indexB].c; + float aB = data.positions[m_indexB].a; + + b2Rot qA(aA), qB(aB); + + float angularError = 0.0f; + float positionError = 0.0f; + + bool fixedRotation = (m_invIA + m_invIB == 0.0f); + + // Solve angular limit constraint + if (m_enableLimit && fixedRotation == false) + { + float angle = aB - aA - m_referenceAngle; + float C = 0.0f; + + if (b2Abs(m_upperAngle - m_lowerAngle) < 2.0f * b2_angularSlop) + { + // Prevent large angular corrections + C = b2Clamp(angle - m_lowerAngle, -b2_maxAngularCorrection, b2_maxAngularCorrection); + } + else if (angle <= m_lowerAngle) + { + // Prevent large angular corrections and allow some slop. + C = b2Clamp(angle - m_lowerAngle + b2_angularSlop, -b2_maxAngularCorrection, 0.0f); + } + else if (angle >= m_upperAngle) + { + // Prevent large angular corrections and allow some slop. + C = b2Clamp(angle - m_upperAngle - b2_angularSlop, 0.0f, b2_maxAngularCorrection); + } + + float limitImpulse = -m_axialMass * C; + aA -= m_invIA * limitImpulse; + aB += m_invIB * limitImpulse; + angularError = b2Abs(C); + } + + // 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(); + + float mA = m_invMassA, mB = m_invMassB; + float 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(float inv_dt) const +{ + b2Vec2 P(m_impulse.x, m_impulse.y); + return inv_dt * P; +} + +float b2RevoluteJoint::GetReactionTorque(float inv_dt) const +{ + return inv_dt * (m_motorImpulse + m_lowerImpulse - m_upperImpulse); +} + +float b2RevoluteJoint::GetJointAngle() const +{ + b2Body* bA = m_bodyA; + b2Body* bB = m_bodyB; + return bB->m_sweep.a - bA->m_sweep.a - m_referenceAngle; +} + +float 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; + } +} + +float b2RevoluteJoint::GetMotorTorque(float inv_dt) const +{ + return inv_dt * m_motorImpulse; +} + +void b2RevoluteJoint::SetMotorSpeed(float speed) +{ + if (speed != m_motorSpeed) + { + m_bodyA->SetAwake(true); + m_bodyB->SetAwake(true); + m_motorSpeed = speed; + } +} + +void b2RevoluteJoint::SetMaxMotorTorque(float 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_lowerImpulse = 0.0f; + m_upperImpulse = 0.0f; + } +} + +float b2RevoluteJoint::GetLowerLimit() const +{ + return m_lowerAngle; +} + +float b2RevoluteJoint::GetUpperLimit() const +{ + return m_upperAngle; +} + +void b2RevoluteJoint::SetLimits(float lower, float upper) +{ + b2Assert(lower <= upper); + + if (lower != m_lowerAngle || upper != m_upperAngle) + { + m_bodyA->SetAwake(true); + m_bodyB->SetAwake(true); + m_lowerImpulse = 0.0f; + m_upperImpulse = 0.0f; + m_lowerAngle = lower; + m_upperAngle = upper; + } +} + +void b2RevoluteJoint::Dump() +{ + int32 indexA = m_bodyA->m_islandIndex; + int32 indexB = m_bodyB->m_islandIndex; + + b2Dump(" b2RevoluteJointDef jd;\n"); + b2Dump(" jd.bodyA = bodies[%d];\n", indexA); + b2Dump(" jd.bodyB = bodies[%d];\n", indexB); + b2Dump(" jd.collideConnected = bool(%d);\n", m_collideConnected); + b2Dump(" jd.localAnchorA.Set(%.9g, %.9g);\n", m_localAnchorA.x, m_localAnchorA.y); + b2Dump(" jd.localAnchorB.Set(%.9g, %.9g);\n", m_localAnchorB.x, m_localAnchorB.y); + b2Dump(" jd.referenceAngle = %.9g;\n", m_referenceAngle); + b2Dump(" jd.enableLimit = bool(%d);\n", m_enableLimit); + b2Dump(" jd.lowerAngle = %.9g;\n", m_lowerAngle); + b2Dump(" jd.upperAngle = %.9g;\n", m_upperAngle); + b2Dump(" jd.enableMotor = bool(%d);\n", m_enableMotor); + b2Dump(" jd.motorSpeed = %.9g;\n", m_motorSpeed); + b2Dump(" jd.maxMotorTorque = %.9g;\n", m_maxMotorTorque); + b2Dump(" joints[%d] = m_world->CreateJoint(&jd);\n", m_index); +} + +/// +void b2RevoluteJoint::Draw(b2Draw* draw) const +{ + const b2Transform& xfA = m_bodyA->GetTransform(); + const b2Transform& xfB = m_bodyB->GetTransform(); + b2Vec2 pA = b2Mul(xfA, m_localAnchorA); + b2Vec2 pB = b2Mul(xfB, m_localAnchorB); + + b2Color c1(0.7f, 0.7f, 0.7f); + b2Color c2(0.3f, 0.9f, 0.3f); + b2Color c3(0.9f, 0.3f, 0.3f); + b2Color c4(0.3f, 0.3f, 0.9f); + b2Color c5(0.4f, 0.4f, 0.4f); + + draw->DrawPoint(pA, 5.0f, c4); + draw->DrawPoint(pB, 5.0f, c5); + + float aA = m_bodyA->GetAngle(); + float aB = m_bodyB->GetAngle(); + float angle = aB - aA - m_referenceAngle; + + const float L = 0.5f; + + b2Vec2 r = L * b2Vec2(cosf(angle), sinf(angle)); + draw->DrawSegment(pB, pB + r, c1); + draw->DrawCircle(pB, L, c1); + + if (m_enableLimit) + { + b2Vec2 rlo = L * b2Vec2(cosf(m_lowerAngle), sinf(m_lowerAngle)); + b2Vec2 rhi = L * b2Vec2(cosf(m_upperAngle), sinf(m_upperAngle)); + + draw->DrawSegment(pB, pB + rlo, c2); + draw->DrawSegment(pB, pB + rhi, c3); + } + + b2Color color(0.5f, 0.8f, 0.8f); + draw->DrawSegment(xfA.p, pA, color); + draw->DrawSegment(pA, pB, color); + draw->DrawSegment(xfB.p, pB, color); +} diff --git a/libs/Box2D/Dynamics/Joints/b2WeldJoint.cpp b/libs/box2d/src/dynamics/b2_weld_joint.cpp similarity index 64% rename from libs/Box2D/Dynamics/Joints/b2WeldJoint.cpp rename to libs/box2d/src/dynamics/b2_weld_joint.cpp index 5c5614d8..df3ee0a3 100644 --- a/libs/Box2D/Dynamics/Joints/b2WeldJoint.cpp +++ b/libs/box2d/src/dynamics/b2_weld_joint.cpp @@ -1,344 +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 -#include -#include - -// Point-to-point constraint -// C = p2 - p1 -// Cdot = v2 - v1 -// = v2 + cross(w2, r2) - v1 - cross(w1, r1) -// J = [-I -r1_skew I r2_skew ] -// Identity used: -// w k % (rx i + ry j) = w * (-ry i + rx j) - -// Angle constraint -// C = angle2 - angle1 - referenceAngle -// Cdot = w2 - w1 -// J = [0 0 -1 0 0 1] -// K = invI1 + invI2 - -void b2WeldJointDef::Initialize(b2Body* bA, b2Body* bB, const b2Vec2& anchor) -{ - bodyA = bA; - bodyB = bB; - localAnchorA = bodyA->GetLocalPoint(anchor); - localAnchorB = bodyB->GetLocalPoint(anchor); - referenceAngle = bodyB->GetAngle() - bodyA->GetAngle(); -} - -b2WeldJoint::b2WeldJoint(const b2WeldJointDef* def) -: b2Joint(def) -{ - m_localAnchorA = def->localAnchorA; - m_localAnchorB = def->localAnchorB; - m_referenceAngle = def->referenceAngle; - m_frequencyHz = def->frequencyHz; - m_dampingRatio = def->dampingRatio; - - m_impulse.SetZero(); -} - -void b2WeldJoint::InitVelocityConstraints(const b2SolverData& data) -{ - m_indexA = m_bodyA->m_islandIndex; - m_indexB = m_bodyB->m_islandIndex; - m_localCenterA = m_bodyA->m_sweep.localCenter; - m_localCenterB = m_bodyB->m_sweep.localCenter; - m_invMassA = m_bodyA->m_invMass; - m_invMassB = m_bodyB->m_invMass; - m_invIA = m_bodyA->m_invI; - m_invIB = m_bodyB->m_invI; - - float32 aA = data.positions[m_indexA].a; - b2Vec2 vA = data.velocities[m_indexA].v; - float32 wA = data.velocities[m_indexA].w; - - float32 aB = data.positions[m_indexB].a; - b2Vec2 vB = data.velocities[m_indexB].v; - float32 wB = data.velocities[m_indexB].w; - - b2Rot qA(aA), qB(aB); - - m_rA = b2Mul(qA, m_localAnchorA - m_localCenterA); - m_rB = b2Mul(qB, m_localAnchorB - m_localCenterB); - - // J = [-I -r1_skew I r2_skew] - // [ 0 -1 0 1] - // r_skew = [-ry; rx] - - // Matlab - // K = [ mA+r1y^2*iA+mB+r2y^2*iB, -r1y*iA*r1x-r2y*iB*r2x, -r1y*iA-r2y*iB] - // [ -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB, r1x*iA+r2x*iB] - // [ -r1y*iA-r2y*iB, r1x*iA+r2x*iB, iA+iB] - - float32 mA = m_invMassA, mB = m_invMassB; - float32 iA = m_invIA, iB = m_invIB; - - b2Mat33 K; - K.ex.x = mA + mB + m_rA.y * m_rA.y * iA + m_rB.y * m_rB.y * iB; - K.ey.x = -m_rA.y * m_rA.x * iA - m_rB.y * m_rB.x * iB; - K.ez.x = -m_rA.y * iA - m_rB.y * iB; - K.ex.y = K.ey.x; - K.ey.y = mA + mB + m_rA.x * m_rA.x * iA + m_rB.x * m_rB.x * iB; - K.ez.y = m_rA.x * iA + m_rB.x * iB; - K.ex.z = K.ez.x; - K.ey.z = K.ez.y; - K.ez.z = iA + iB; - - if (m_frequencyHz > 0.0f) - { - K.GetInverse22(&m_mass); - - float32 invM = iA + iB; - float32 m = invM > 0.0f ? 1.0f / invM : 0.0f; - - float32 C = aB - aA - m_referenceAngle; - - // Frequency - float32 omega = 2.0f * b2_pi * m_frequencyHz; - - // Damping coefficient - float32 d = 2.0f * m * m_dampingRatio * omega; - - // Spring stiffness - float32 k = m * omega * omega; - - // magic formulas - float32 h = data.step.dt; - m_gamma = h * (d + h * k); - m_gamma = m_gamma != 0.0f ? 1.0f / m_gamma : 0.0f; - m_bias = C * h * k * m_gamma; - - invM += m_gamma; - m_mass.ez.z = invM != 0.0f ? 1.0f / invM : 0.0f; - } - else 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); -} +// MIT License + +// Copyright (c) 2019 Erin Catto + +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: + +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. + +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. + +#include "box2d/b2_body.h" +#include "box2d/b2_time_step.h" +#include "box2d/b2_weld_joint.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_stiffness = def->stiffness; + m_damping = def->damping; + + 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; + + float aA = data.positions[m_indexA].a; + b2Vec2 vA = data.velocities[m_indexA].v; + float wA = data.velocities[m_indexA].w; + + float aB = data.positions[m_indexB].a; + b2Vec2 vB = data.velocities[m_indexB].v; + float 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] + + float mA = m_invMassA, mB = m_invMassB; + float 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_stiffness > 0.0f) + { + K.GetInverse22(&m_mass); + + float invM = iA + iB; + + float C = aB - aA - m_referenceAngle; + + // Damping coefficient + float d = m_damping; + + // Spring stiffness + float k = m_stiffness; + + // magic formulas + float 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; + float wA = data.velocities[m_indexA].w; + b2Vec2 vB = data.velocities[m_indexB].v; + float wB = data.velocities[m_indexB].w; + + float mA = m_invMassA, mB = m_invMassB; + float iA = m_invIA, iB = m_invIB; + + if (m_stiffness > 0.0f) + { + float Cdot2 = wB - wA; + + float 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); + float 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; + float aA = data.positions[m_indexA].a; + b2Vec2 cB = data.positions[m_indexB].c; + float aB = data.positions[m_indexB].a; + + b2Rot qA(aA), qB(aB); + + float mA = m_invMassA, mB = m_invMassB; + float iA = m_invIA, iB = m_invIB; + + b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_localCenterA); + b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_localCenterB); + + float 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_stiffness > 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; + float 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(float inv_dt) const +{ + b2Vec2 P(m_impulse.x, m_impulse.y); + return inv_dt * P; +} + +float b2WeldJoint::GetReactionTorque(float inv_dt) const +{ + return inv_dt * m_impulse.z; +} + +void b2WeldJoint::Dump() +{ + int32 indexA = m_bodyA->m_islandIndex; + int32 indexB = m_bodyB->m_islandIndex; + + b2Dump(" b2WeldJointDef jd;\n"); + b2Dump(" jd.bodyA = bodies[%d];\n", indexA); + b2Dump(" jd.bodyB = bodies[%d];\n", indexB); + b2Dump(" jd.collideConnected = bool(%d);\n", m_collideConnected); + b2Dump(" jd.localAnchorA.Set(%.9g, %.9g);\n", m_localAnchorA.x, m_localAnchorA.y); + b2Dump(" jd.localAnchorB.Set(%.9g, %.9g);\n", m_localAnchorB.x, m_localAnchorB.y); + b2Dump(" jd.referenceAngle = %.9g;\n", m_referenceAngle); + b2Dump(" jd.stiffness = %.9g;\n", m_stiffness); + b2Dump(" jd.damping = %.9g;\n", m_damping); + b2Dump(" joints[%d] = m_world->CreateJoint(&jd);\n", m_index); +} diff --git a/libs/box2d/src/dynamics/b2_wheel_joint.cpp b/libs/box2d/src/dynamics/b2_wheel_joint.cpp new file mode 100644 index 00000000..c23b9846 --- /dev/null +++ b/libs/box2d/src/dynamics/b2_wheel_joint.cpp @@ -0,0 +1,672 @@ +// MIT License + +// Copyright (c) 2019 Erin Catto + +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: + +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. + +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. + +#include "box2d/b2_body.h" +#include "box2d/b2_draw.h" +#include "box2d/b2_wheel_joint.h" +#include "box2d/b2_time_step.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_axialMass = 0.0f; + m_lowerImpulse = 0.0f; + m_upperImpulse = 0.0f; + m_lowerTranslation = def->lowerTranslation; + m_upperTranslation = def->upperTranslation; + m_enableLimit = def->enableLimit; + + m_maxMotorTorque = def->maxMotorTorque; + m_motorSpeed = def->motorSpeed; + m_enableMotor = def->enableMotor; + + m_bias = 0.0f; + m_gamma = 0.0f; + + m_ax.SetZero(); + m_ay.SetZero(); + + m_stiffness = def->stiffness; + m_damping = def->damping; +} + +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; + + float mA = m_invMassA, mB = m_invMassB; + float iA = m_invIA, iB = m_invIB; + + b2Vec2 cA = data.positions[m_indexA].c; + float aA = data.positions[m_indexA].a; + b2Vec2 vA = data.velocities[m_indexA].v; + float wA = data.velocities[m_indexA].w; + + b2Vec2 cB = data.positions[m_indexB].c; + float aB = data.positions[m_indexB].a; + b2Vec2 vB = data.velocities[m_indexB].v; + float 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_ax = b2Mul(qA, m_localXAxisA); + m_sAx = b2Cross(d + rA, m_ax); + m_sBx = b2Cross(rB, m_ax); + + const float invMass = mA + mB + iA * m_sAx * m_sAx + iB * m_sBx * m_sBx; + if (invMass > 0.0f) + { + m_axialMass = 1.0f / invMass; + } + else + { + m_axialMass = 0.0f; + } + + m_springMass = 0.0f; + m_bias = 0.0f; + m_gamma = 0.0f; + + if (m_stiffness > 0.0f && invMass > 0.0f) + { + m_springMass = 1.0f / invMass; + + float C = b2Dot(d, m_ax); + + // magic formulas + float h = data.step.dt; + m_gamma = h * (m_damping + h * m_stiffness); + if (m_gamma > 0.0f) + { + m_gamma = 1.0f / m_gamma; + } + + m_bias = C * h * m_stiffness * m_gamma; + + m_springMass = invMass + m_gamma; + if (m_springMass > 0.0f) + { + m_springMass = 1.0f / m_springMass; + } + } + else + { + m_springImpulse = 0.0f; + } + + if (m_enableLimit) + { + m_translation = b2Dot(m_ax, d); + } + else + { + m_lowerImpulse = 0.0f; + m_upperImpulse = 0.0f; + } + + 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; + + float axialImpulse = m_springImpulse + m_lowerImpulse - m_upperImpulse; + b2Vec2 P = m_impulse * m_ay + axialImpulse * m_ax; + float LA = m_impulse * m_sAy + axialImpulse * m_sAx + m_motorImpulse; + float LB = m_impulse * m_sBy + axialImpulse * 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; + m_lowerImpulse = 0.0f; + m_upperImpulse = 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) +{ + float mA = m_invMassA, mB = m_invMassB; + float iA = m_invIA, iB = m_invIB; + + b2Vec2 vA = data.velocities[m_indexA].v; + float wA = data.velocities[m_indexA].w; + b2Vec2 vB = data.velocities[m_indexB].v; + float wB = data.velocities[m_indexB].w; + + // Solve spring constraint + { + float Cdot = b2Dot(m_ax, vB - vA) + m_sBx * wB - m_sAx * wA; + float impulse = -m_springMass * (Cdot + m_bias + m_gamma * m_springImpulse); + m_springImpulse += impulse; + + b2Vec2 P = impulse * m_ax; + float LA = impulse * m_sAx; + float LB = impulse * m_sBx; + + vA -= mA * P; + wA -= iA * LA; + + vB += mB * P; + wB += iB * LB; + } + + // Solve rotational motor constraint + { + float Cdot = wB - wA - m_motorSpeed; + float impulse = -m_motorMass * Cdot; + + float oldImpulse = m_motorImpulse; + float maxImpulse = data.step.dt * m_maxMotorTorque; + m_motorImpulse = b2Clamp(m_motorImpulse + impulse, -maxImpulse, maxImpulse); + impulse = m_motorImpulse - oldImpulse; + + wA -= iA * impulse; + wB += iB * impulse; + } + + if (m_enableLimit) + { + // Lower limit + { + float C = m_translation - m_lowerTranslation; + float Cdot = b2Dot(m_ax, vB - vA) + m_sBx * wB - m_sAx * wA; + float impulse = -m_axialMass * (Cdot + b2Max(C, 0.0f) * data.step.inv_dt); + float oldImpulse = m_lowerImpulse; + m_lowerImpulse = b2Max(m_lowerImpulse + impulse, 0.0f); + impulse = m_lowerImpulse - oldImpulse; + + b2Vec2 P = impulse * m_ax; + float LA = impulse * m_sAx; + float LB = impulse * m_sBx; + + vA -= mA * P; + wA -= iA * LA; + vB += mB * P; + wB += iB * LB; + } + + // Upper limit + // Note: signs are flipped to keep C positive when the constraint is satisfied. + // This also keeps the impulse positive when the limit is active. + { + float C = m_upperTranslation - m_translation; + float Cdot = b2Dot(m_ax, vA - vB) + m_sAx * wA - m_sBx * wB; + float impulse = -m_axialMass * (Cdot + b2Max(C, 0.0f) * data.step.inv_dt); + float oldImpulse = m_upperImpulse; + m_upperImpulse = b2Max(m_upperImpulse + impulse, 0.0f); + impulse = m_upperImpulse - oldImpulse; + + b2Vec2 P = impulse * m_ax; + float LA = impulse * m_sAx; + float LB = impulse * m_sBx; + + vA += mA * P; + wA += iA * LA; + vB -= mB * P; + wB -= iB * LB; + } + } + + // Solve point to line constraint + { + float Cdot = b2Dot(m_ay, vB - vA) + m_sBy * wB - m_sAy * wA; + float impulse = -m_mass * Cdot; + m_impulse += impulse; + + b2Vec2 P = impulse * m_ay; + float LA = impulse * m_sAy; + float 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; + float aA = data.positions[m_indexA].a; + b2Vec2 cB = data.positions[m_indexB].c; + float aB = data.positions[m_indexB].a; + + float linearError = 0.0f; + + if (m_enableLimit) + { + 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 ax = b2Mul(qA, m_localXAxisA); + float sAx = b2Cross(d + rA, m_ax); + float sBx = b2Cross(rB, m_ax); + + float C = 0.0f; + float translation = b2Dot(ax, d); + if (b2Abs(m_upperTranslation - m_lowerTranslation) < 2.0f * b2_linearSlop) + { + C = translation; + } + else if (translation <= m_lowerTranslation) + { + C = b2Min(translation - m_lowerTranslation, 0.0f); + } + else if (translation >= m_upperTranslation) + { + C = b2Max(translation - m_upperTranslation, 0.0f); + } + + if (C != 0.0f) + { + + float invMass = m_invMassA + m_invMassB + m_invIA * sAx * sAx + m_invIB * sBx * sBx; + float impulse = 0.0f; + if (invMass != 0.0f) + { + impulse = -C / invMass; + } + + b2Vec2 P = impulse * ax; + float LA = impulse * sAx; + float LB = impulse * sBx; + + cA -= m_invMassA * P; + aA -= m_invIA * LA; + cB += m_invMassB * P; + aB += m_invIB * LB; + + linearError = b2Abs(C); + } + } + + // Solve perpendicular constraint + { + 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); + + float sAy = b2Cross(d + rA, ay); + float sBy = b2Cross(rB, ay); + + float C = b2Dot(d, ay); + + float invMass = m_invMassA + m_invMassB + m_invIA * m_sAy * m_sAy + m_invIB * m_sBy * m_sBy; + + float impulse = 0.0f; + if (invMass != 0.0f) + { + impulse = - C / invMass; + } + + b2Vec2 P = impulse * ay; + float LA = impulse * sAy; + float LB = impulse * sBy; + + cA -= m_invMassA * P; + aA -= m_invIA * LA; + cB += m_invMassB * P; + aB += m_invIB * LB; + + linearError = b2Max(linearError, b2Abs(C)); + } + + 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 b2WheelJoint::GetAnchorA() const +{ + return m_bodyA->GetWorldPoint(m_localAnchorA); +} + +b2Vec2 b2WheelJoint::GetAnchorB() const +{ + return m_bodyB->GetWorldPoint(m_localAnchorB); +} + +b2Vec2 b2WheelJoint::GetReactionForce(float inv_dt) const +{ + return inv_dt * (m_impulse * m_ay + (m_springImpulse + m_lowerImpulse - m_upperImpulse) * m_ax); +} + +float b2WheelJoint::GetReactionTorque(float inv_dt) const +{ + return inv_dt * m_motorImpulse; +} + +float 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); + + float translation = b2Dot(d, axis); + return translation; +} + +float 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; + float wA = bA->m_angularVelocity; + float wB = bB->m_angularVelocity; + + float speed = b2Dot(d, b2Cross(wA, axis)) + b2Dot(axis, vB + b2Cross(wB, rB) - vA - b2Cross(wA, rA)); + return speed; +} + +float b2WheelJoint::GetJointAngle() const +{ + b2Body* bA = m_bodyA; + b2Body* bB = m_bodyB; + return bB->m_sweep.a - bA->m_sweep.a; +} + +float b2WheelJoint::GetJointAngularSpeed() const +{ + float wA = m_bodyA->m_angularVelocity; + float wB = m_bodyB->m_angularVelocity; + return wB - wA; +} + +bool b2WheelJoint::IsLimitEnabled() const +{ + return m_enableLimit; +} + +void b2WheelJoint::EnableLimit(bool flag) +{ + if (flag != m_enableLimit) + { + m_bodyA->SetAwake(true); + m_bodyB->SetAwake(true); + m_enableLimit = flag; + m_lowerImpulse = 0.0f; + m_upperImpulse = 0.0f; + } +} + +float b2WheelJoint::GetLowerLimit() const +{ + return m_lowerTranslation; +} + +float b2WheelJoint::GetUpperLimit() const +{ + return m_upperTranslation; +} + +void b2WheelJoint::SetLimits(float lower, float 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_lowerImpulse = 0.0f; + m_upperImpulse = 0.0f; + } +} + +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(float speed) +{ + if (speed != m_motorSpeed) + { + m_bodyA->SetAwake(true); + m_bodyB->SetAwake(true); + m_motorSpeed = speed; + } +} + +void b2WheelJoint::SetMaxMotorTorque(float torque) +{ + if (torque != m_maxMotorTorque) + { + m_bodyA->SetAwake(true); + m_bodyB->SetAwake(true); + m_maxMotorTorque = torque; + } +} + +float b2WheelJoint::GetMotorTorque(float inv_dt) const +{ + return inv_dt * m_motorImpulse; +} + +void b2WheelJoint::SetStiffness(float stiffness) +{ + m_stiffness = stiffness; +} + +float b2WheelJoint::GetStiffness() const +{ + return m_stiffness; +} + +void b2WheelJoint::SetDamping(float damping) +{ + m_damping = damping; +} + +float b2WheelJoint::GetDamping() const +{ + return m_damping; +} + +void b2WheelJoint::Dump() +{ + // FLT_DECIMAL_DIG == 9 + + int32 indexA = m_bodyA->m_islandIndex; + int32 indexB = m_bodyB->m_islandIndex; + + b2Dump(" b2WheelJointDef jd;\n"); + b2Dump(" jd.bodyA = bodies[%d];\n", indexA); + b2Dump(" jd.bodyB = bodies[%d];\n", indexB); + b2Dump(" jd.collideConnected = bool(%d);\n", m_collideConnected); + b2Dump(" jd.localAnchorA.Set(%.9g, %.9g);\n", m_localAnchorA.x, m_localAnchorA.y); + b2Dump(" jd.localAnchorB.Set(%.9g, %.9g);\n", m_localAnchorB.x, m_localAnchorB.y); + b2Dump(" jd.localAxisA.Set(%.9g, %.9g);\n", m_localXAxisA.x, m_localXAxisA.y); + b2Dump(" jd.enableMotor = bool(%d);\n", m_enableMotor); + b2Dump(" jd.motorSpeed = %.9g;\n", m_motorSpeed); + b2Dump(" jd.maxMotorTorque = %.9g;\n", m_maxMotorTorque); + b2Dump(" jd.stiffness = %.9g;\n", m_stiffness); + b2Dump(" jd.damping = %.9g;\n", m_damping); + b2Dump(" joints[%d] = m_world->CreateJoint(&jd);\n", m_index); +} + +/// +void b2WheelJoint::Draw(b2Draw* draw) const +{ + const b2Transform& xfA = m_bodyA->GetTransform(); + const b2Transform& xfB = m_bodyB->GetTransform(); + b2Vec2 pA = b2Mul(xfA, m_localAnchorA); + b2Vec2 pB = b2Mul(xfB, m_localAnchorB); + + b2Vec2 axis = b2Mul(xfA.q, m_localXAxisA); + + b2Color c1(0.7f, 0.7f, 0.7f); + b2Color c2(0.3f, 0.9f, 0.3f); + b2Color c3(0.9f, 0.3f, 0.3f); + b2Color c4(0.3f, 0.3f, 0.9f); + b2Color c5(0.4f, 0.4f, 0.4f); + + draw->DrawSegment(pA, pB, c5); + + if (m_enableLimit) + { + b2Vec2 lower = pA + m_lowerTranslation * axis; + b2Vec2 upper = pA + m_upperTranslation * axis; + b2Vec2 perp = b2Mul(xfA.q, m_localYAxisA); + draw->DrawSegment(lower, upper, c1); + draw->DrawSegment(lower - 0.5f * perp, lower + 0.5f * perp, c2); + draw->DrawSegment(upper - 0.5f * perp, upper + 0.5f * perp, c3); + } + else + { + draw->DrawSegment(pA - 1.0f * axis, pA + 1.0f * axis, c1); + } + + draw->DrawPoint(pA, 5.0f, c1); + draw->DrawPoint(pB, 5.0f, c4); +} diff --git a/libs/Box2D/Dynamics/b2World.cpp b/libs/box2d/src/dynamics/b2_world.cpp similarity index 78% rename from libs/Box2D/Dynamics/b2World.cpp rename to libs/box2d/src/dynamics/b2_world.cpp index b23042c2..78ec084c 100644 --- a/libs/Box2D/Dynamics/b2World.cpp +++ b/libs/box2d/src/dynamics/b2_world.cpp @@ -1,1339 +1,1322 @@ -/* -* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -b2World::b2World(const b2Vec2& gravity) -{ - m_destructionListener = NULL; - g_debugDraw = NULL; - - m_bodyList = NULL; - m_jointList = NULL; - - m_bodyCount = 0; - m_jointCount = 0; - - m_warmStarting = true; - m_continuousPhysics = true; - m_subStepping = false; - - m_stepComplete = true; - - m_allowSleep = true; - m_gravity = gravity; - - m_flags = e_clearForces; - - m_inv_dt0 = 0.0f; - - m_contactManager.m_allocator = &m_blockAllocator; - - memset(&m_profile, 0, sizeof(b2Profile)); -} - -b2World::~b2World() -{ - // Some shapes allocate using b2Alloc. - b2Body* b = m_bodyList; - while (b) - { - b2Body* bNext = b->m_next; - - b2Fixture* f = b->m_fixtureList; - while (f) - { - b2Fixture* fNext = f->m_next; - f->m_proxyCount = 0; - f->Destroy(&m_blockAllocator); - f = fNext; - } - - b = bNext; - } -} - -void b2World::SetDestructionListener(b2DestructionListener* listener) -{ - m_destructionListener = listener; -} - -void b2World::SetContactFilter(b2ContactFilter* filter) -{ - m_contactManager.m_contactFilter = filter; -} - -void b2World::SetContactListener(b2ContactListener* listener) -{ - m_contactManager.m_contactListener = listener; -} - -void b2World::SetDebugDraw(b2Draw* debugDraw) -{ - g_debugDraw = debugDraw; -} - -b2Body* b2World::CreateBody(const b2BodyDef* def) -{ - b2Assert(IsLocked() == false); - if (IsLocked()) - { - return NULL; - } - - void* mem = m_blockAllocator.Allocate(sizeof(b2Body)); - b2Body* b = new (mem) b2Body(def, this); - - // Add to world doubly linked list. - b->m_prev = NULL; - b->m_next = m_bodyList; - if (m_bodyList) - { - m_bodyList->m_prev = b; - } - m_bodyList = b; - ++m_bodyCount; - - return b; -} - -void b2World::DestroyBody(b2Body* b) -{ - b2Assert(m_bodyCount > 0); - b2Assert(IsLocked() == false); - if (IsLocked()) - { - return; - } - - // Delete the attached joints. - b2JointEdge* je = b->m_jointList; - while (je) - { - b2JointEdge* je0 = je; - je = je->next; - - if (m_destructionListener) - { - m_destructionListener->SayGoodbye(je0->joint); - } - - DestroyJoint(je0->joint); - - b->m_jointList = je; - } - b->m_jointList = NULL; - - // Delete the attached contacts. - b2ContactEdge* ce = b->m_contactList; - while (ce) - { - b2ContactEdge* ce0 = ce; - ce = ce->next; - m_contactManager.Destroy(ce0->contact); - } - b->m_contactList = NULL; - - // Delete the attached fixtures. This destroys broad-phase proxies. - b2Fixture* f = b->m_fixtureList; - while (f) - { - b2Fixture* f0 = f; - f = f->m_next; - - if (m_destructionListener) - { - m_destructionListener->SayGoodbye(f0); - } - - f0->DestroyProxies(&m_contactManager.m_broadPhase); - f0->Destroy(&m_blockAllocator); - f0->~b2Fixture(); - m_blockAllocator.Free(f0, sizeof(b2Fixture)); - - b->m_fixtureList = f; - b->m_fixtureCount -= 1; - } - b->m_fixtureList = NULL; - b->m_fixtureCount = 0; - - // Remove world body list. - if (b->m_prev) - { - b->m_prev->m_next = b->m_next; - } - - if (b->m_next) - { - b->m_next->m_prev = b->m_prev; - } - - if (b == m_bodyList) - { - m_bodyList = b->m_next; - } - - --m_bodyCount; - b->~b2Body(); - m_blockAllocator.Free(b, sizeof(b2Body)); -} - -b2Joint* b2World::CreateJoint(const b2JointDef* def) -{ - b2Assert(IsLocked() == false); - if (IsLocked()) - { - return NULL; - } - - b2Joint* j = b2Joint::Create(def, &m_blockAllocator); - - // Connect to the world list. - j->m_prev = NULL; - j->m_next = m_jointList; - if (m_jointList) - { - m_jointList->m_prev = j; - } - m_jointList = j; - ++m_jointCount; - - // Connect to the bodies' doubly linked lists. - j->m_edgeA.joint = j; - j->m_edgeA.other = j->m_bodyB; - j->m_edgeA.prev = NULL; - j->m_edgeA.next = j->m_bodyA->m_jointList; - if (j->m_bodyA->m_jointList) j->m_bodyA->m_jointList->prev = &j->m_edgeA; - j->m_bodyA->m_jointList = &j->m_edgeA; - - j->m_edgeB.joint = j; - j->m_edgeB.other = j->m_bodyA; - j->m_edgeB.prev = NULL; - j->m_edgeB.next = j->m_bodyB->m_jointList; - if (j->m_bodyB->m_jointList) j->m_bodyB->m_jointList->prev = &j->m_edgeB; - j->m_bodyB->m_jointList = &j->m_edgeB; - - b2Body* bodyA = def->bodyA; - b2Body* bodyB = def->bodyB; - - // If the joint prevents collisions, then flag any contacts for filtering. - if (def->collideConnected == false) - { - b2ContactEdge* edge = bodyB->GetContactList(); - while (edge) - { - if (edge->other == bodyA) - { - // Flag the contact for filtering at the next time step (where either - // body is awake). - edge->contact->FlagForFiltering(); - } - - edge = edge->next; - } - } - - // Note: creating a joint doesn't wake the bodies. - - return j; -} - -void b2World::DestroyJoint(b2Joint* j) -{ - b2Assert(IsLocked() == false); - if (IsLocked()) - { - return; - } - - bool collideConnected = j->m_collideConnected; - - // Remove from the doubly linked list. - if (j->m_prev) - { - j->m_prev->m_next = j->m_next; - } - - if (j->m_next) - { - j->m_next->m_prev = j->m_prev; - } - - if (j == m_jointList) - { - m_jointList = j->m_next; - } - - // Disconnect from island graph. - b2Body* bodyA = j->m_bodyA; - b2Body* bodyB = j->m_bodyB; - - // Wake up connected bodies. - bodyA->SetAwake(true); - bodyB->SetAwake(true); - - // Remove from body 1. - if (j->m_edgeA.prev) - { - j->m_edgeA.prev->next = j->m_edgeA.next; - } - - if (j->m_edgeA.next) - { - j->m_edgeA.next->prev = j->m_edgeA.prev; - } - - if (&j->m_edgeA == bodyA->m_jointList) - { - bodyA->m_jointList = j->m_edgeA.next; - } - - j->m_edgeA.prev = NULL; - j->m_edgeA.next = NULL; - - // Remove from body 2 - if (j->m_edgeB.prev) - { - j->m_edgeB.prev->next = j->m_edgeB.next; - } - - if (j->m_edgeB.next) - { - j->m_edgeB.next->prev = j->m_edgeB.prev; - } - - if (&j->m_edgeB == bodyB->m_jointList) - { - bodyB->m_jointList = j->m_edgeB.next; - } - - j->m_edgeB.prev = NULL; - j->m_edgeB.next = NULL; - - b2Joint::Destroy(j, &m_blockAllocator); - - b2Assert(m_jointCount > 0); - --m_jointCount; - - // If the joint prevents collisions, then flag any contacts for filtering. - if (collideConnected == false) - { - b2ContactEdge* edge = bodyB->GetContactList(); - while (edge) - { - if (edge->other == bodyA) - { - // Flag the contact for filtering at the next time step (where either - // body is awake). - edge->contact->FlagForFiltering(); - } - - edge = edge->next; - } - } -} - -// -void b2World::SetAllowSleeping(bool flag) -{ - if (flag == m_allowSleep) - { - return; - } - - m_allowSleep = flag; - if (m_allowSleep == false) - { - for (b2Body* b = m_bodyList; b; b = b->m_next) - { - b->SetAwake(true); - } - } -} - -// Find islands, integrate and solve constraints, solve position constraints -void b2World::Solve(const b2TimeStep& step) -{ - m_profile.solveInit = 0.0f; - m_profile.solveVelocity = 0.0f; - m_profile.solvePosition = 0.0f; - - // Size the island for the worst case. - b2Island island(m_bodyCount, - m_contactManager.m_contactCount, - m_jointCount, - &m_stackAllocator, - m_contactManager.m_contactListener); - - // Clear all the island flags. - for (b2Body* b = m_bodyList; b; b = b->m_next) - { - b->m_flags &= ~b2Body::e_islandFlag; - } - for (b2Contact* c = m_contactManager.m_contactList; c; c = c->m_next) - { - c->m_flags &= ~b2Contact::e_islandFlag; - } - for (b2Joint* j = m_jointList; j; j = j->m_next) - { - j->m_islandFlag = false; - } - - // Build and simulate all awake islands. - int32 stackSize = m_bodyCount; - b2Body** stack = (b2Body**)m_stackAllocator.Allocate(stackSize * sizeof(b2Body*)); - for (b2Body* seed = m_bodyList; seed; seed = seed->m_next) - { - if (seed->m_flags & b2Body::e_islandFlag) - { - continue; - } - - if (seed->IsAwake() == false || seed->IsActive() == false) - { - continue; - } - - // The seed can be dynamic or kinematic. - if (seed->GetType() == b2_staticBody) - { - continue; - } - - // Reset island and stack. - island.Clear(); - int32 stackCount = 0; - stack[stackCount++] = seed; - seed->m_flags |= b2Body::e_islandFlag; - - // Perform a depth first search (DFS) on the constraint graph. - while (stackCount > 0) - { - // Grab the next body off the stack and add it to the island. - b2Body* b = stack[--stackCount]; - b2Assert(b->IsActive() == true); - island.Add(b); - - // Make sure the body is awake. - b->SetAwake(true); - - // To keep islands as small as possible, we don't - // propagate islands across static bodies. - if (b->GetType() == b2_staticBody) - { - continue; - } - - // Search all contacts connected to this body. - for (b2ContactEdge* ce = b->m_contactList; ce; ce = ce->next) - { - b2Contact* contact = ce->contact; - - // Has this contact already been added to an island? - if (contact->m_flags & b2Contact::e_islandFlag) - { - continue; - } - - // Is this contact solid and touching? - if (contact->IsEnabled() == false || - contact->IsTouching() == false) - { - continue; - } - - // Skip sensors. - bool sensorA = contact->m_fixtureA->m_isSensor; - bool sensorB = contact->m_fixtureB->m_isSensor; - if (sensorA || sensorB) - { - continue; - } - - island.Add(contact); - contact->m_flags |= b2Contact::e_islandFlag; - - b2Body* other = ce->other; - - // Was the other body already added to this island? - if (other->m_flags & b2Body::e_islandFlag) - { - continue; - } - - b2Assert(stackCount < stackSize); - stack[stackCount++] = other; - other->m_flags |= b2Body::e_islandFlag; - } - - // Search all joints connect to this body. - for (b2JointEdge* je = b->m_jointList; je; je = je->next) - { - if (je->joint->m_islandFlag == true) - { - continue; - } - - b2Body* other = je->other; - - // Don't simulate joints connected to inactive bodies. - if (other->IsActive() == false) - { - continue; - } - - island.Add(je->joint); - je->joint->m_islandFlag = true; - - if (other->m_flags & b2Body::e_islandFlag) - { - continue; - } - - b2Assert(stackCount < stackSize); - stack[stackCount++] = other; - other->m_flags |= b2Body::e_islandFlag; - } - } - - b2Profile profile; - island.Solve(&profile, step, m_gravity, m_allowSleep); - m_profile.solveInit += profile.solveInit; - m_profile.solveVelocity += profile.solveVelocity; - m_profile.solvePosition += profile.solvePosition; - - // Post solve cleanup. - for (int32 i = 0; i < island.m_bodyCount; ++i) - { - // Allow static bodies to participate in other islands. - b2Body* b = island.m_bodies[i]; - if (b->GetType() == b2_staticBody) - { - b->m_flags &= ~b2Body::e_islandFlag; - } - } - } - - m_stackAllocator.Free(stack); - - { - b2Timer timer; - // Synchronize fixtures, check for out of range bodies. - for (b2Body* b = m_bodyList; b; b = b->GetNext()) - { - // If a body was not in an island then it did not move. - if ((b->m_flags & b2Body::e_islandFlag) == 0) - { - continue; - } - - if (b->GetType() == b2_staticBody) - { - continue; - } - - // Update fixtures (for broad-phase). - b->SynchronizeFixtures(); - } - - // Look for new contacts. - m_contactManager.FindNewContacts(); - m_profile.broadphase = timer.GetMilliseconds(); - } -} - -// Find TOI contacts and solve them. -void b2World::SolveTOI(const b2TimeStep& step) -{ - b2Island island(2 * b2_maxTOIContacts, b2_maxTOIContacts, 0, &m_stackAllocator, m_contactManager.m_contactListener); - - if (m_stepComplete) - { - for (b2Body* b = m_bodyList; b; b = b->m_next) - { - b->m_flags &= ~b2Body::e_islandFlag; - b->m_sweep.alpha0 = 0.0f; - } - - for (b2Contact* c = m_contactManager.m_contactList; c; c = c->m_next) - { - // Invalidate TOI - c->m_flags &= ~(b2Contact::e_toiFlag | b2Contact::e_islandFlag); - c->m_toiCount = 0; - c->m_toi = 1.0f; - } - } - - // Find TOI events and solve them. - for (;;) - { - // Find the first TOI. - b2Contact* minContact = NULL; - float32 minAlpha = 1.0f; - - for (b2Contact* c = m_contactManager.m_contactList; c; c = c->m_next) - { - // Is this contact disabled? - if (c->IsEnabled() == false) - { - continue; - } - - // Prevent excessive sub-stepping. - if (c->m_toiCount > b2_maxSubSteps) - { - continue; - } - - float32 alpha = 1.0f; - if (c->m_flags & b2Contact::e_toiFlag) - { - // This contact has a valid cached TOI. - alpha = c->m_toi; - } - else - { - b2Fixture* fA = c->GetFixtureA(); - b2Fixture* fB = c->GetFixtureB(); - - // Is there a sensor? - if (fA->IsSensor() || fB->IsSensor()) - { - continue; - } - - b2Body* bA = fA->GetBody(); - b2Body* bB = fB->GetBody(); - - b2BodyType typeA = bA->m_type; - b2BodyType typeB = bB->m_type; - b2Assert(typeA == b2_dynamicBody || typeB == b2_dynamicBody); - - bool activeA = bA->IsAwake() && typeA != b2_staticBody; - bool activeB = bB->IsAwake() && typeB != b2_staticBody; - - // Is at least one body active (awake and dynamic or kinematic)? - if (activeA == false && activeB == false) - { - continue; - } - - bool collideA = bA->IsBullet() || typeA != b2_dynamicBody; - bool collideB = bB->IsBullet() || typeB != b2_dynamicBody; - - // Are these two non-bullet dynamic bodies? - if (collideA == false && collideB == false) - { - continue; - } - - // Compute the TOI for this contact. - // Put the sweeps onto the same time interval. - float32 alpha0 = bA->m_sweep.alpha0; - - if (bA->m_sweep.alpha0 < bB->m_sweep.alpha0) - { - alpha0 = bB->m_sweep.alpha0; - bA->m_sweep.Advance(alpha0); - } - else if (bB->m_sweep.alpha0 < bA->m_sweep.alpha0) - { - alpha0 = bA->m_sweep.alpha0; - bB->m_sweep.Advance(alpha0); - } - - b2Assert(alpha0 < 1.0f); - - int32 indexA = c->GetChildIndexA(); - int32 indexB = c->GetChildIndexB(); - - // Compute the time of impact in interval [0, minTOI] - b2TOIInput input; - input.proxyA.Set(fA->GetShape(), indexA); - input.proxyB.Set(fB->GetShape(), indexB); - input.sweepA = bA->m_sweep; - input.sweepB = bB->m_sweep; - input.tMax = 1.0f; - - b2TOIOutput output; - b2TimeOfImpact(&output, &input); - - // Beta is the fraction of the remaining portion of the . - float32 beta = output.t; - if (output.state == b2TOIOutput::e_touching) - { - alpha = b2Min(alpha0 + (1.0f - alpha0) * beta, 1.0f); - } - else - { - alpha = 1.0f; - } - - c->m_toi = alpha; - c->m_flags |= b2Contact::e_toiFlag; - } - - if (alpha < minAlpha) - { - // This is the minimum TOI found so far. - minContact = c; - minAlpha = alpha; - } - } - - if (minContact == NULL || 1.0f - 10.0f * b2_epsilon < minAlpha) - { - // No more TOI events. Done! - m_stepComplete = true; - break; - } - - // Advance the bodies to the TOI. - b2Fixture* fA = minContact->GetFixtureA(); - b2Fixture* fB = minContact->GetFixtureB(); - b2Body* bA = fA->GetBody(); - b2Body* bB = fB->GetBody(); - - b2Sweep backup1 = bA->m_sweep; - b2Sweep backup2 = bB->m_sweep; - - bA->Advance(minAlpha); - bB->Advance(minAlpha); - - // The TOI contact likely has some new contact points. - minContact->Update(m_contactManager.m_contactListener); - minContact->m_flags &= ~b2Contact::e_toiFlag; - ++minContact->m_toiCount; - - // Is the contact solid? - if (minContact->IsEnabled() == false || minContact->IsTouching() == false) - { - // Restore the sweeps. - minContact->SetEnabled(false); - bA->m_sweep = backup1; - bB->m_sweep = backup2; - bA->SynchronizeTransform(); - bB->SynchronizeTransform(); - continue; - } - - bA->SetAwake(true); - bB->SetAwake(true); - - // Build the island - island.Clear(); - island.Add(bA); - island.Add(bB); - island.Add(minContact); - - bA->m_flags |= b2Body::e_islandFlag; - bB->m_flags |= b2Body::e_islandFlag; - minContact->m_flags |= b2Contact::e_islandFlag; - - // Get contacts on bodyA and bodyB. - b2Body* bodies[2] = {bA, bB}; - for (int32 i = 0; i < 2; ++i) - { - b2Body* body = bodies[i]; - if (body->m_type == b2_dynamicBody) - { - for (b2ContactEdge* ce = body->m_contactList; ce; ce = ce->next) - { - if (island.m_bodyCount == island.m_bodyCapacity) - { - break; - } - - if (island.m_contactCount == island.m_contactCapacity) - { - break; - } - - b2Contact* contact = ce->contact; - - // Has this contact already been added to the island? - if (contact->m_flags & b2Contact::e_islandFlag) - { - continue; - } - - // Only add static, kinematic, or bullet bodies. - b2Body* other = ce->other; - if (other->m_type == b2_dynamicBody && - body->IsBullet() == false && other->IsBullet() == false) - { - continue; - } - - // Skip sensors. - bool sensorA = contact->m_fixtureA->m_isSensor; - bool sensorB = contact->m_fixtureB->m_isSensor; - if (sensorA || sensorB) - { - continue; - } - - // Tentatively advance the body to the TOI. - b2Sweep backup = other->m_sweep; - if ((other->m_flags & b2Body::e_islandFlag) == 0) - { - other->Advance(minAlpha); - } - - // Update the contact points - contact->Update(m_contactManager.m_contactListener); - - // Was the contact disabled by the user? - if (contact->IsEnabled() == false) - { - other->m_sweep = backup; - other->SynchronizeTransform(); - continue; - } - - // Are there contact points? - if (contact->IsTouching() == false) - { - other->m_sweep = backup; - other->SynchronizeTransform(); - continue; - } - - // Add the contact to the island - contact->m_flags |= b2Contact::e_islandFlag; - island.Add(contact); - - // Has the other body already been added to the island? - if (other->m_flags & b2Body::e_islandFlag) - { - continue; - } - - // Add the other body to the island. - other->m_flags |= b2Body::e_islandFlag; - - if (other->m_type != b2_staticBody) - { - other->SetAwake(true); - } - - island.Add(other); - } - } - } - - b2TimeStep subStep; - subStep.dt = (1.0f - minAlpha) * step.dt; - subStep.inv_dt = 1.0f / subStep.dt; - subStep.dtRatio = 1.0f; - subStep.positionIterations = 20; - subStep.velocityIterations = step.velocityIterations; - subStep.warmStarting = false; - island.SolveTOI(subStep, bA->m_islandIndex, bB->m_islandIndex); - - // Reset island flags and synchronize broad-phase proxies. - for (int32 i = 0; i < island.m_bodyCount; ++i) - { - b2Body* body = island.m_bodies[i]; - body->m_flags &= ~b2Body::e_islandFlag; - - if (body->m_type != b2_dynamicBody) - { - continue; - } - - body->SynchronizeFixtures(); - - // Invalidate all contact TOIs on this displaced body. - for (b2ContactEdge* ce = body->m_contactList; ce; ce = ce->next) - { - ce->contact->m_flags &= ~(b2Contact::e_toiFlag | b2Contact::e_islandFlag); - } - } - - // Commit fixture proxy movements to the broad-phase so that new contacts are created. - // Also, some contacts can be destroyed. - m_contactManager.FindNewContacts(); - - if (m_subStepping) - { - m_stepComplete = false; - break; - } - } -} - -void b2World::Step(float32 dt, int32 velocityIterations, int32 positionIterations) -{ - b2Timer stepTimer; - - // If new fixtures were added, we need to find the new contacts. - if (m_flags & e_newFixture) - { - m_contactManager.FindNewContacts(); - m_flags &= ~e_newFixture; - } - - m_flags |= e_locked; - - b2TimeStep step; - step.dt = dt; - step.velocityIterations = velocityIterations; - step.positionIterations = positionIterations; - if (dt > 0.0f) - { - step.inv_dt = 1.0f / dt; - } - else - { - step.inv_dt = 0.0f; - } - - step.dtRatio = m_inv_dt0 * dt; - - step.warmStarting = m_warmStarting; - - // Update contacts. This is where some contacts are destroyed. - { - b2Timer timer; - m_contactManager.Collide(); - m_profile.collide = timer.GetMilliseconds(); - } - - // Integrate velocities, solve velocity constraints, and integrate positions. - if (m_stepComplete && step.dt > 0.0f) - { - b2Timer timer; - Solve(step); - m_profile.solve = timer.GetMilliseconds(); - } - - // Handle TOI events. - if (m_continuousPhysics && step.dt > 0.0f) - { - b2Timer timer; - SolveTOI(step); - m_profile.solveTOI = timer.GetMilliseconds(); - } - - if (step.dt > 0.0f) - { - m_inv_dt0 = step.inv_dt; - } - - if (m_flags & e_clearForces) - { - ClearForces(); - } - - m_flags &= ~e_locked; - - m_profile.step = stepTimer.GetMilliseconds(); -} - -void b2World::ClearForces() -{ - for (b2Body* body = m_bodyList; body; body = body->GetNext()) - { - body->m_force.SetZero(); - body->m_torque = 0.0f; - } -} - -struct b2WorldQueryWrapper -{ - bool QueryCallback(int32 proxyId) - { - b2FixtureProxy* proxy = (b2FixtureProxy*)broadPhase->GetUserData(proxyId); - return callback->ReportFixture(proxy->fixture); - } - - const b2BroadPhase* broadPhase; - b2QueryCallback* callback; -}; - -void b2World::QueryAABB(b2QueryCallback* callback, const b2AABB& aabb) const -{ - b2WorldQueryWrapper wrapper; - wrapper.broadPhase = &m_contactManager.m_broadPhase; - wrapper.callback = callback; - m_contactManager.m_broadPhase.Query(&wrapper, aabb); -} - -struct b2WorldRayCastWrapper -{ - float32 RayCastCallback(const b2RayCastInput& input, int32 proxyId) - { - void* userData = broadPhase->GetUserData(proxyId); - b2FixtureProxy* proxy = (b2FixtureProxy*)userData; - b2Fixture* fixture = proxy->fixture; - int32 index = proxy->childIndex; - b2RayCastOutput output; - bool hit = fixture->RayCast(&output, input, index); - - if (hit) - { - float32 fraction = output.fraction; - b2Vec2 point = (1.0f - fraction) * input.p1 + fraction * input.p2; - return callback->ReportFixture(fixture, point, output.normal, fraction); - } - - return input.maxFraction; - } - - const b2BroadPhase* broadPhase; - b2RayCastCallback* callback; -}; - -void b2World::RayCast(b2RayCastCallback* callback, const b2Vec2& point1, const b2Vec2& point2) const -{ - b2WorldRayCastWrapper wrapper; - wrapper.broadPhase = &m_contactManager.m_broadPhase; - wrapper.callback = callback; - b2RayCastInput input; - input.maxFraction = 1.0f; - input.p1 = point1; - input.p2 = point2; - m_contactManager.m_broadPhase.RayCast(&wrapper, input); -} - -void b2World::DrawShape(b2Fixture* fixture, const b2Transform& xf, const b2Color& color) -{ - switch (fixture->GetType()) - { - case b2Shape::e_circle: - { - b2CircleShape* circle = (b2CircleShape*)fixture->GetShape(); - - b2Vec2 center = b2Mul(xf, circle->m_p); - float32 radius = circle->m_radius; - b2Vec2 axis = b2Mul(xf.q, b2Vec2(1.0f, 0.0f)); - - g_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); - g_debugDraw->DrawSegment(v1, v2, color); - } - break; - - case b2Shape::e_chain: - { - b2ChainShape* chain = (b2ChainShape*)fixture->GetShape(); - int32 count = chain->m_count; - const b2Vec2* vertices = chain->m_vertices; - - b2Vec2 v1 = b2Mul(xf, vertices[0]); - for (int32 i = 1; i < count; ++i) - { - b2Vec2 v2 = b2Mul(xf, vertices[i]); - g_debugDraw->DrawSegment(v1, v2, color); - g_debugDraw->DrawCircle(v1, 0.05f, color); - v1 = v2; - } - } - break; - - case b2Shape::e_polygon: - { - b2PolygonShape* poly = (b2PolygonShape*)fixture->GetShape(); - int32 vertexCount = poly->m_count; - b2Assert(vertexCount <= b2_maxPolygonVertices); - b2Vec2 vertices[b2_maxPolygonVertices]; - - for (int32 i = 0; i < vertexCount; ++i) - { - vertices[i] = b2Mul(xf, poly->m_vertices[i]); - } - - g_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: - g_debugDraw->DrawSegment(p1, p2, color); - break; - - case e_pulleyJoint: - { - b2PulleyJoint* pulley = (b2PulleyJoint*)joint; - b2Vec2 s1 = pulley->GetGroundAnchorA(); - b2Vec2 s2 = pulley->GetGroundAnchorB(); - g_debugDraw->DrawSegment(s1, p1, color); - g_debugDraw->DrawSegment(s2, p2, color); - g_debugDraw->DrawSegment(s1, s2, color); - } - break; - - case e_mouseJoint: - // don't draw this - break; - - default: - g_debugDraw->DrawSegment(x1, p1, color); - g_debugDraw->DrawSegment(p1, p2, color); - g_debugDraw->DrawSegment(x2, p2, color); - } -} - -void b2World::DrawDebugData() -{ - if (g_debugDraw == NULL) - { - return; - } - - uint32 flags = g_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); - - g_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(); - g_debugDraw->DrawTransform(xf); - } - } -} - -int32 b2World::GetProxyCount() const -{ - return m_contactManager.m_broadPhase.GetProxyCount(); -} - -int32 b2World::GetTreeHeight() const -{ - return m_contactManager.m_broadPhase.GetTreeHeight(); -} - -int32 b2World::GetTreeBalance() const -{ - return m_contactManager.m_broadPhase.GetTreeBalance(); -} - -float32 b2World::GetTreeQuality() const -{ - return m_contactManager.m_broadPhase.GetTreeQuality(); -} - -void b2World::ShiftOrigin(const b2Vec2& newOrigin) -{ - b2Assert((m_flags & e_locked) == 0); - if ((m_flags & e_locked) == e_locked) - { - return; - } - - for (b2Body* b = m_bodyList; b; b = b->m_next) - { - b->m_xf.p -= newOrigin; - b->m_sweep.c0 -= newOrigin; - b->m_sweep.c -= newOrigin; - } - - for (b2Joint* j = m_jointList; j; j = j->m_next) - { - j->ShiftOrigin(newOrigin); - } - - m_contactManager.m_broadPhase.ShiftOrigin(newOrigin); -} - -void b2World::Dump() -{ - if ((m_flags & e_locked) == e_locked) - { - return; - } - - b2Log("b2Vec2 g(%.15lef, %.15lef);\n", m_gravity.x, m_gravity.y); - b2Log("m_world->SetGravity(g);\n"); - - b2Log("b2Body** bodies = (b2Body**)b2Alloc(%d * sizeof(b2Body*));\n", m_bodyCount); - b2Log("b2Joint** joints = (b2Joint**)b2Alloc(%d * sizeof(b2Joint*));\n", m_jointCount); - int32 i = 0; - for (b2Body* b = m_bodyList; b; b = b->m_next) - { - b->m_islandIndex = i; - b->Dump(); - ++i; - } - - i = 0; - for (b2Joint* j = m_jointList; j; j = j->m_next) - { - j->m_index = i; - ++i; - } - - // First pass on joints, skip gear joints. - for (b2Joint* j = m_jointList; j; j = j->m_next) - { - if (j->m_type == e_gearJoint) - { - continue; - } - - b2Log("{\n"); - j->Dump(); - b2Log("}\n"); - } - - // Second pass on joints, only gear joints. - for (b2Joint* j = m_jointList; j; j = j->m_next) - { - if (j->m_type != e_gearJoint) - { - continue; - } - - b2Log("{\n"); - j->Dump(); - b2Log("}\n"); - } - - b2Log("b2Free(joints);\n"); - b2Log("b2Free(bodies);\n"); - b2Log("joints = NULL;\n"); - b2Log("bodies = NULL;\n"); -} +// MIT License + +// Copyright (c) 2019 Erin Catto + +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: + +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. + +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. + +#include "b2_contact_solver.h" +#include "b2_island.h" + +#include "box2d/b2_body.h" +#include "box2d/b2_broad_phase.h" +#include "box2d/b2_chain_shape.h" +#include "box2d/b2_circle_shape.h" +#include "box2d/b2_collision.h" +#include "box2d/b2_contact.h" +#include "box2d/b2_draw.h" +#include "box2d/b2_edge_shape.h" +#include "box2d/b2_fixture.h" +#include "box2d/b2_polygon_shape.h" +#include "box2d/b2_pulley_joint.h" +#include "box2d/b2_time_of_impact.h" +#include "box2d/b2_timer.h" +#include "box2d/b2_world.h" + +#include + +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_newContacts = false; + m_locked = false; + m_clearForces = true; + + 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->IsEnabled() == 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->IsEnabled() == true); + island.Add(b); + + // To keep islands as small as possible, we don't + // propagate islands across static bodies. + if (b->GetType() == b2_staticBody) + { + continue; + } + + // Make sure the body is awake (without resetting sleep timer). + b->m_flags |= b2Body::e_awakeFlag; + + // 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 disabled bodies. + if (other->IsEnabled() == 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; + float 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; + } + + float 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. + float 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 . + float 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(float dt, int32 velocityIterations, int32 positionIterations) +{ + b2Timer stepTimer; + + // If new fixtures were added, we need to find the new contacts. + if (m_newContacts) + { + m_contactManager.FindNewContacts(); + m_newContacts = false; + } + + m_locked = true; + + 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_clearForces) + { + ClearForces(); + } + + m_locked = false; + + 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 +{ + float 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) + { + float 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); + float 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); + + if (edge->m_oneSided == false) + { + m_debugDraw->DrawPoint(v1, 4.0f, color); + m_debugDraw->DrawPoint(v2, 4.0f, color); + } + } + break; + + case b2Shape::e_chain: + { + b2ChainShape* chain = (b2ChainShape*)fixture->GetShape(); + int32 count = chain->m_count; + const b2Vec2* vertices = chain->m_vertices; + + b2Vec2 v1 = b2Mul(xf, vertices[0]); + for (int32 i = 1; i < count; ++i) + { + b2Vec2 v2 = b2Mul(xf, vertices[i]); + m_debugDraw->DrawSegment(v1, v2, color); + v1 = v2; + } + } + break; + + case b2Shape::e_polygon: + { + b2PolygonShape* poly = (b2PolygonShape*)fixture->GetShape(); + int32 vertexCount = poly->m_count; + b2Assert(vertexCount <= b2_maxPolygonVertices); + b2Vec2 vertices[b2_maxPolygonVertices]; + + for (int32 i = 0; i < vertexCount; ++i) + { + vertices[i] = b2Mul(xf, poly->m_vertices[i]); + } + + m_debugDraw->DrawSolidPolygon(vertices, vertexCount, color); + } + break; + + default: + break; + } +} + +void b2World::DebugDraw() +{ + 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->GetType() == b2_dynamicBody && b->m_mass == 0.0f) + { + // Bad body + DrawShape(f, xf, b2Color(1.0f, 0.0f, 0.0f)); + } + else if (b->IsEnabled() == 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()) + { + j->Draw(m_debugDraw); + } + } + + 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(); + int32 indexA = c->GetChildIndexA(); + int32 indexB = c->GetChildIndexB(); + b2Vec2 cA = fixtureA->GetAABB(indexA).GetCenter(); + b2Vec2 cB = fixtureB->GetAABB(indexB).GetCenter(); + + m_debugDraw->DrawSegment(cA, cB, color); + } + } + + if (flags & b2Draw::e_aabbBit) + { + b2Color color(0.9f, 0.3f, 0.9f); + b2BroadPhase* bp = &m_contactManager.m_broadPhase; + + for (b2Body* b = m_bodyList; b; b = b->GetNext()) + { + if (b->IsEnabled() == 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(); +} + +float b2World::GetTreeQuality() const +{ + return m_contactManager.m_broadPhase.GetTreeQuality(); +} + +void b2World::ShiftOrigin(const b2Vec2& newOrigin) +{ + b2Assert(m_locked == false); + if (m_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_locked) + { + return; + } + + b2OpenDump("box2d_dump.inl"); + + b2Dump("b2Vec2 g(%.9g, %.9g);\n", m_gravity.x, m_gravity.y); + b2Dump("m_world->SetGravity(g);\n"); + + b2Dump("b2Body** bodies = (b2Body**)b2Alloc(%d * sizeof(b2Body*));\n", m_bodyCount); + b2Dump("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; + } + + b2Dump("{\n"); + j->Dump(); + b2Dump("}\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; + } + + b2Dump("{\n"); + j->Dump(); + b2Dump("}\n"); + } + + b2Dump("b2Free(joints);\n"); + b2Dump("b2Free(bodies);\n"); + b2Dump("joints = nullptr;\n"); + b2Dump("bodies = nullptr;\n"); + + b2CloseDump(); +} diff --git a/libs/box2d/src/dynamics/b2_world_callbacks.cpp b/libs/box2d/src/dynamics/b2_world_callbacks.cpp new file mode 100644 index 00000000..e1583e22 --- /dev/null +++ b/libs/box2d/src/dynamics/b2_world_callbacks.cpp @@ -0,0 +1,40 @@ +// MIT License + +// Copyright (c) 2019 Erin Catto + +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: + +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. + +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. + +#include "box2d/b2_fixture.h" +#include "box2d/b2_world_callbacks.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/libs/box2d/src/rope/b2_rope.cpp b/libs/box2d/src/rope/b2_rope.cpp new file mode 100644 index 00000000..71fbe79e --- /dev/null +++ b/libs/box2d/src/rope/b2_rope.cpp @@ -0,0 +1,809 @@ +// MIT License + +// Copyright (c) 2019 Erin Catto + +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: + +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. + +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. + +#include "box2d/b2_draw.h" +#include "box2d/b2_rope.h" + +#include + +struct b2RopeStretch +{ + int32 i1, i2; + float invMass1, invMass2; + float L; + float lambda; + float spring; + float damper; +}; + +struct b2RopeBend +{ + int32 i1, i2, i3; + float invMass1, invMass2, invMass3; + float invEffectiveMass; + float lambda; + float L1, L2; + float alpha1, alpha2; + float spring; + float damper; +}; + +b2Rope::b2Rope() +{ + m_position.SetZero(); + m_count = 0; + m_stretchCount = 0; + m_bendCount = 0; + m_stretchConstraints = nullptr; + m_bendConstraints = nullptr; + m_bindPositions = nullptr; + m_ps = nullptr; + m_p0s = nullptr; + m_vs = nullptr; + m_invMasses = nullptr; + m_gravity.SetZero(); +} + +b2Rope::~b2Rope() +{ + b2Free(m_stretchConstraints); + b2Free(m_bendConstraints); + b2Free(m_bindPositions); + b2Free(m_ps); + b2Free(m_p0s); + b2Free(m_vs); + b2Free(m_invMasses); +} + +void b2Rope::Create(const b2RopeDef& def) +{ + b2Assert(def.count >= 3); + m_position = def.position; + m_count = def.count; + m_bindPositions = (b2Vec2*)b2Alloc(m_count * sizeof(b2Vec2)); + m_ps = (b2Vec2*)b2Alloc(m_count * sizeof(b2Vec2)); + m_p0s = (b2Vec2*)b2Alloc(m_count * sizeof(b2Vec2)); + m_vs = (b2Vec2*)b2Alloc(m_count * sizeof(b2Vec2)); + m_invMasses = (float*)b2Alloc(m_count * sizeof(float)); + + for (int32 i = 0; i < m_count; ++i) + { + m_bindPositions[i] = def.vertices[i]; + m_ps[i] = def.vertices[i] + m_position; + m_p0s[i] = def.vertices[i] + m_position; + m_vs[i].SetZero(); + + float m = def.masses[i]; + if (m > 0.0f) + { + m_invMasses[i] = 1.0f / m; + } + else + { + m_invMasses[i] = 0.0f; + } + } + + m_stretchCount = m_count - 1; + m_bendCount = m_count - 2; + + m_stretchConstraints = (b2RopeStretch*)b2Alloc(m_stretchCount * sizeof(b2RopeStretch)); + m_bendConstraints = (b2RopeBend*)b2Alloc(m_bendCount * sizeof(b2RopeBend)); + + for (int32 i = 0; i < m_stretchCount; ++i) + { + b2RopeStretch& c = m_stretchConstraints[i]; + + b2Vec2 p1 = m_ps[i]; + b2Vec2 p2 = m_ps[i+1]; + + c.i1 = i; + c.i2 = i + 1; + c.L = b2Distance(p1, p2); + c.invMass1 = m_invMasses[i]; + c.invMass2 = m_invMasses[i + 1]; + c.lambda = 0.0f; + c.damper = 0.0f; + c.spring = 0.0f; + } + + for (int32 i = 0; i < m_bendCount; ++i) + { + b2RopeBend& c = m_bendConstraints[i]; + + b2Vec2 p1 = m_ps[i]; + b2Vec2 p2 = m_ps[i + 1]; + b2Vec2 p3 = m_ps[i + 2]; + + c.i1 = i; + c.i2 = i + 1; + c.i3 = i + 2; + c.invMass1 = m_invMasses[i]; + c.invMass2 = m_invMasses[i + 1]; + c.invMass3 = m_invMasses[i + 2]; + c.invEffectiveMass = 0.0f; + c.L1 = b2Distance(p1, p2); + c.L2 = b2Distance(p2, p3); + c.lambda = 0.0f; + + // Pre-compute effective mass (TODO use flattened config) + b2Vec2 e1 = p2 - p1; + b2Vec2 e2 = p3 - p2; + float L1sqr = e1.LengthSquared(); + float L2sqr = e2.LengthSquared(); + + if (L1sqr * L2sqr == 0.0f) + { + continue; + } + + b2Vec2 Jd1 = (-1.0f / L1sqr) * e1.Skew(); + b2Vec2 Jd2 = (1.0f / L2sqr) * e2.Skew(); + + b2Vec2 J1 = -Jd1; + b2Vec2 J2 = Jd1 - Jd2; + b2Vec2 J3 = Jd2; + + c.invEffectiveMass = c.invMass1 * b2Dot(J1, J1) + c.invMass2 * b2Dot(J2, J2) + c.invMass3 * b2Dot(J3, J3); + + b2Vec2 r = p3 - p1; + + float rr = r.LengthSquared(); + if (rr == 0.0f) + { + continue; + } + + // a1 = h2 / (h1 + h2) + // a2 = h1 / (h1 + h2) + c.alpha1 = b2Dot(e2, r) / rr; + c.alpha2 = b2Dot(e1, r) / rr; + } + + m_gravity = def.gravity; + + SetTuning(def.tuning); +} + +void b2Rope::SetTuning(const b2RopeTuning& tuning) +{ + m_tuning = tuning; + + // Pre-compute spring and damper values based on tuning + + const float bendOmega = 2.0f * b2_pi * m_tuning.bendHertz; + + for (int32 i = 0; i < m_bendCount; ++i) + { + b2RopeBend& c = m_bendConstraints[i]; + + float L1sqr = c.L1 * c.L1; + float L2sqr = c.L2 * c.L2; + + if (L1sqr * L2sqr == 0.0f) + { + c.spring = 0.0f; + c.damper = 0.0f; + continue; + } + + // Flatten the triangle formed by the two edges + float J2 = 1.0f / c.L1 + 1.0f / c.L2; + float sum = c.invMass1 / L1sqr + c.invMass2 * J2 * J2 + c.invMass3 / L2sqr; + if (sum == 0.0f) + { + c.spring = 0.0f; + c.damper = 0.0f; + continue; + } + + float mass = 1.0f / sum; + + c.spring = mass * bendOmega * bendOmega; + c.damper = 2.0f * mass * m_tuning.bendDamping * bendOmega; + } + + const float stretchOmega = 2.0f * b2_pi * m_tuning.stretchHertz; + + for (int32 i = 0; i < m_stretchCount; ++i) + { + b2RopeStretch& c = m_stretchConstraints[i]; + + float sum = c.invMass1 + c.invMass2; + if (sum == 0.0f) + { + continue; + } + + float mass = 1.0f / sum; + + c.spring = mass * stretchOmega * stretchOmega; + c.damper = 2.0f * mass * m_tuning.stretchDamping * stretchOmega; + } +} + +void b2Rope::Step(float dt, int32 iterations, const b2Vec2& position) +{ + if (dt == 0.0f) + { + return; + } + + const float inv_dt = 1.0f / dt; + float d = expf(- dt * m_tuning.damping); + + // Apply gravity and damping + for (int32 i = 0; i < m_count; ++i) + { + if (m_invMasses[i] > 0.0f) + { + m_vs[i] *= d; + m_vs[i] += dt * m_gravity; + } + else + { + m_vs[i] = inv_dt * (m_bindPositions[i] + position - m_p0s[i]); + } + } + + // Apply bending spring + if (m_tuning.bendingModel == b2_springAngleBendingModel) + { + ApplyBendForces(dt); + } + + for (int32 i = 0; i < m_bendCount; ++i) + { + m_bendConstraints[i].lambda = 0.0f; + } + + for (int32 i = 0; i < m_stretchCount; ++i) + { + m_stretchConstraints[i].lambda = 0.0f; + } + + // Update position + for (int32 i = 0; i < m_count; ++i) + { + m_ps[i] += dt * m_vs[i]; + } + + // Solve constraints + for (int32 i = 0; i < iterations; ++i) + { + if (m_tuning.bendingModel == b2_pbdAngleBendingModel) + { + SolveBend_PBD_Angle(); + } + else if (m_tuning.bendingModel == b2_xpbdAngleBendingModel) + { + SolveBend_XPBD_Angle(dt); + } + else if (m_tuning.bendingModel == b2_pbdDistanceBendingModel) + { + SolveBend_PBD_Distance(); + } + else if (m_tuning.bendingModel == b2_pbdHeightBendingModel) + { + SolveBend_PBD_Height(); + } + else if (m_tuning.bendingModel == b2_pbdTriangleBendingModel) + { + SolveBend_PBD_Triangle(); + } + + if (m_tuning.stretchingModel == b2_pbdStretchingModel) + { + SolveStretch_PBD(); + } + else if (m_tuning.stretchingModel == b2_xpbdStretchingModel) + { + SolveStretch_XPBD(dt); + } + } + + // Constrain velocity + for (int32 i = 0; i < m_count; ++i) + { + m_vs[i] = inv_dt * (m_ps[i] - m_p0s[i]); + m_p0s[i] = m_ps[i]; + } +} + +void b2Rope::Reset(const b2Vec2& position) +{ + m_position = position; + + for (int32 i = 0; i < m_count; ++i) + { + m_ps[i] = m_bindPositions[i] + m_position; + m_p0s[i] = m_bindPositions[i] + m_position; + m_vs[i].SetZero(); + } + + for (int32 i = 0; i < m_bendCount; ++i) + { + m_bendConstraints[i].lambda = 0.0f; + } + + for (int32 i = 0; i < m_stretchCount; ++i) + { + m_stretchConstraints[i].lambda = 0.0f; + } +} + +void b2Rope::SolveStretch_PBD() +{ + const float stiffness = m_tuning.stretchStiffness; + + for (int32 i = 0; i < m_stretchCount; ++i) + { + const b2RopeStretch& c = m_stretchConstraints[i]; + + b2Vec2 p1 = m_ps[c.i1]; + b2Vec2 p2 = m_ps[c.i2]; + + b2Vec2 d = p2 - p1; + float L = d.Normalize(); + + float sum = c.invMass1 + c.invMass2; + if (sum == 0.0f) + { + continue; + } + + float s1 = c.invMass1 / sum; + float s2 = c.invMass2 / sum; + + p1 -= stiffness * s1 * (c.L - L) * d; + p2 += stiffness * s2 * (c.L - L) * d; + + m_ps[c.i1] = p1; + m_ps[c.i2] = p2; + } +} + +void b2Rope::SolveStretch_XPBD(float dt) +{ + b2Assert(dt > 0.0f); + + for (int32 i = 0; i < m_stretchCount; ++i) + { + b2RopeStretch& c = m_stretchConstraints[i]; + + b2Vec2 p1 = m_ps[c.i1]; + b2Vec2 p2 = m_ps[c.i2]; + + b2Vec2 dp1 = p1 - m_p0s[c.i1]; + b2Vec2 dp2 = p2 - m_p0s[c.i2]; + + b2Vec2 u = p2 - p1; + float L = u.Normalize(); + + b2Vec2 J1 = -u; + b2Vec2 J2 = u; + + float sum = c.invMass1 + c.invMass2; + if (sum == 0.0f) + { + continue; + } + + const float alpha = 1.0f / (c.spring * dt * dt); // 1 / kg + const float beta = dt * dt * c.damper; // kg * s + const float sigma = alpha * beta / dt; // non-dimensional + float C = L - c.L; + + // This is using the initial velocities + float Cdot = b2Dot(J1, dp1) + b2Dot(J2, dp2); + + float B = C + alpha * c.lambda + sigma * Cdot; + float sum2 = (1.0f + sigma) * sum + alpha; + + float impulse = -B / sum2; + + p1 += (c.invMass1 * impulse) * J1; + p2 += (c.invMass2 * impulse) * J2; + + m_ps[c.i1] = p1; + m_ps[c.i2] = p2; + c.lambda += impulse; + } +} + +void b2Rope::SolveBend_PBD_Angle() +{ + const float stiffness = m_tuning.bendStiffness; + + for (int32 i = 0; i < m_bendCount; ++i) + { + const b2RopeBend& c = m_bendConstraints[i]; + + b2Vec2 p1 = m_ps[c.i1]; + b2Vec2 p2 = m_ps[c.i2]; + b2Vec2 p3 = m_ps[c.i3]; + + b2Vec2 d1 = p2 - p1; + b2Vec2 d2 = p3 - p2; + float a = b2Cross(d1, d2); + float b = b2Dot(d1, d2); + + float angle = b2Atan2(a, b); + + float L1sqr, L2sqr; + + if (m_tuning.isometric) + { + L1sqr = c.L1 * c.L1; + L2sqr = c.L2 * c.L2; + } + else + { + L1sqr = d1.LengthSquared(); + L2sqr = d2.LengthSquared(); + } + + if (L1sqr * L2sqr == 0.0f) + { + continue; + } + + b2Vec2 Jd1 = (-1.0f / L1sqr) * d1.Skew(); + b2Vec2 Jd2 = (1.0f / L2sqr) * d2.Skew(); + + b2Vec2 J1 = -Jd1; + b2Vec2 J2 = Jd1 - Jd2; + b2Vec2 J3 = Jd2; + + float sum; + if (m_tuning.fixedEffectiveMass) + { + sum = c.invEffectiveMass; + } + else + { + sum = c.invMass1 * b2Dot(J1, J1) + c.invMass2 * b2Dot(J2, J2) + c.invMass3 * b2Dot(J3, J3); + } + + if (sum == 0.0f) + { + sum = c.invEffectiveMass; + } + + float impulse = -stiffness * angle / sum; + + p1 += (c.invMass1 * impulse) * J1; + p2 += (c.invMass2 * impulse) * J2; + p3 += (c.invMass3 * impulse) * J3; + + m_ps[c.i1] = p1; + m_ps[c.i2] = p2; + m_ps[c.i3] = p3; + } +} + +void b2Rope::SolveBend_XPBD_Angle(float dt) +{ + b2Assert(dt > 0.0f); + + for (int32 i = 0; i < m_bendCount; ++i) + { + b2RopeBend& c = m_bendConstraints[i]; + + b2Vec2 p1 = m_ps[c.i1]; + b2Vec2 p2 = m_ps[c.i2]; + b2Vec2 p3 = m_ps[c.i3]; + + b2Vec2 dp1 = p1 - m_p0s[c.i1]; + b2Vec2 dp2 = p2 - m_p0s[c.i2]; + b2Vec2 dp3 = p3 - m_p0s[c.i3]; + + b2Vec2 d1 = p2 - p1; + b2Vec2 d2 = p3 - p2; + + float L1sqr, L2sqr; + + if (m_tuning.isometric) + { + L1sqr = c.L1 * c.L1; + L2sqr = c.L2 * c.L2; + } + else + { + L1sqr = d1.LengthSquared(); + L2sqr = d2.LengthSquared(); + } + + if (L1sqr * L2sqr == 0.0f) + { + continue; + } + + float a = b2Cross(d1, d2); + float b = b2Dot(d1, d2); + + float angle = b2Atan2(a, b); + + b2Vec2 Jd1 = (-1.0f / L1sqr) * d1.Skew(); + b2Vec2 Jd2 = (1.0f / L2sqr) * d2.Skew(); + + b2Vec2 J1 = -Jd1; + b2Vec2 J2 = Jd1 - Jd2; + b2Vec2 J3 = Jd2; + + float sum; + if (m_tuning.fixedEffectiveMass) + { + sum = c.invEffectiveMass; + } + else + { + sum = c.invMass1 * b2Dot(J1, J1) + c.invMass2 * b2Dot(J2, J2) + c.invMass3 * b2Dot(J3, J3); + } + + if (sum == 0.0f) + { + continue; + } + + const float alpha = 1.0f / (c.spring * dt * dt); + const float beta = dt * dt * c.damper; + const float sigma = alpha * beta / dt; + float C = angle; + + // This is using the initial velocities + float Cdot = b2Dot(J1, dp1) + b2Dot(J2, dp2) + b2Dot(J3, dp3); + + float B = C + alpha * c.lambda + sigma * Cdot; + float sum2 = (1.0f + sigma) * sum + alpha; + + float impulse = -B / sum2; + + p1 += (c.invMass1 * impulse) * J1; + p2 += (c.invMass2 * impulse) * J2; + p3 += (c.invMass3 * impulse) * J3; + + m_ps[c.i1] = p1; + m_ps[c.i2] = p2; + m_ps[c.i3] = p3; + c.lambda += impulse; + } +} + +void b2Rope::ApplyBendForces(float dt) +{ + // omega = 2 * pi * hz + const float omega = 2.0f * b2_pi * m_tuning.bendHertz; + + for (int32 i = 0; i < m_bendCount; ++i) + { + const b2RopeBend& c = m_bendConstraints[i]; + + b2Vec2 p1 = m_ps[c.i1]; + b2Vec2 p2 = m_ps[c.i2]; + b2Vec2 p3 = m_ps[c.i3]; + + b2Vec2 v1 = m_vs[c.i1]; + b2Vec2 v2 = m_vs[c.i2]; + b2Vec2 v3 = m_vs[c.i3]; + + b2Vec2 d1 = p2 - p1; + b2Vec2 d2 = p3 - p2; + + float L1sqr, L2sqr; + + if (m_tuning.isometric) + { + L1sqr = c.L1 * c.L1; + L2sqr = c.L2 * c.L2; + } + else + { + L1sqr = d1.LengthSquared(); + L2sqr = d2.LengthSquared(); + } + + if (L1sqr * L2sqr == 0.0f) + { + continue; + } + + float a = b2Cross(d1, d2); + float b = b2Dot(d1, d2); + + float angle = b2Atan2(a, b); + + b2Vec2 Jd1 = (-1.0f / L1sqr) * d1.Skew(); + b2Vec2 Jd2 = (1.0f / L2sqr) * d2.Skew(); + + b2Vec2 J1 = -Jd1; + b2Vec2 J2 = Jd1 - Jd2; + b2Vec2 J3 = Jd2; + + float sum; + if (m_tuning.fixedEffectiveMass) + { + sum = c.invEffectiveMass; + } + else + { + sum = c.invMass1 * b2Dot(J1, J1) + c.invMass2 * b2Dot(J2, J2) + c.invMass3 * b2Dot(J3, J3); + } + + if (sum == 0.0f) + { + continue; + } + + float mass = 1.0f / sum; + + const float spring = mass * omega * omega; + const float damper = 2.0f * mass * m_tuning.bendDamping * omega; + + float C = angle; + float Cdot = b2Dot(J1, v1) + b2Dot(J2, v2) + b2Dot(J3, v3); + + float impulse = -dt * (spring * C + damper * Cdot); + + m_vs[c.i1] += (c.invMass1 * impulse) * J1; + m_vs[c.i2] += (c.invMass2 * impulse) * J2; + m_vs[c.i3] += (c.invMass3 * impulse) * J3; + } +} + +void b2Rope::SolveBend_PBD_Distance() +{ + const float stiffness = m_tuning.bendStiffness; + + for (int32 i = 0; i < m_bendCount; ++i) + { + const b2RopeBend& c = m_bendConstraints[i]; + + int32 i1 = c.i1; + int32 i2 = c.i3; + + b2Vec2 p1 = m_ps[i1]; + b2Vec2 p2 = m_ps[i2]; + + b2Vec2 d = p2 - p1; + float L = d.Normalize(); + + float sum = c.invMass1 + c.invMass3; + if (sum == 0.0f) + { + continue; + } + + float s1 = c.invMass1 / sum; + float s2 = c.invMass3 / sum; + + p1 -= stiffness * s1 * (c.L1 + c.L2 - L) * d; + p2 += stiffness * s2 * (c.L1 + c.L2 - L) * d; + + m_ps[i1] = p1; + m_ps[i2] = p2; + } +} + +// Constraint based implementation of: +// P. Volino: Simple Linear Bending Stiffness in Particle Systems +void b2Rope::SolveBend_PBD_Height() +{ + const float stiffness = m_tuning.bendStiffness; + + for (int32 i = 0; i < m_bendCount; ++i) + { + const b2RopeBend& c = m_bendConstraints[i]; + + b2Vec2 p1 = m_ps[c.i1]; + b2Vec2 p2 = m_ps[c.i2]; + b2Vec2 p3 = m_ps[c.i3]; + + // Barycentric coordinates are held constant + b2Vec2 d = c.alpha1 * p1 + c.alpha2 * p3 - p2; + float dLen = d.Length(); + + if (dLen == 0.0f) + { + continue; + } + + b2Vec2 dHat = (1.0f / dLen) * d; + + b2Vec2 J1 = c.alpha1 * dHat; + b2Vec2 J2 = -dHat; + b2Vec2 J3 = c.alpha2 * dHat; + + float sum = c.invMass1 * c.alpha1 * c.alpha1 + c.invMass2 + c.invMass3 * c.alpha2 * c.alpha2; + + if (sum == 0.0f) + { + continue; + } + + float C = dLen; + float mass = 1.0f / sum; + float impulse = -stiffness * mass * C; + + p1 += (c.invMass1 * impulse) * J1; + p2 += (c.invMass2 * impulse) * J2; + p3 += (c.invMass3 * impulse) * J3; + + m_ps[c.i1] = p1; + m_ps[c.i2] = p2; + m_ps[c.i3] = p3; + } +} + +// M. Kelager: A Triangle Bending Constraint Model for PBD +void b2Rope::SolveBend_PBD_Triangle() +{ + const float stiffness = m_tuning.bendStiffness; + + for (int32 i = 0; i < m_bendCount; ++i) + { + const b2RopeBend& c = m_bendConstraints[i]; + + b2Vec2 b0 = m_ps[c.i1]; + b2Vec2 v = m_ps[c.i2]; + b2Vec2 b1 = m_ps[c.i3]; + + float wb0 = c.invMass1; + float wv = c.invMass2; + float wb1 = c.invMass3; + + float W = wb0 + wb1 + 2.0f * wv; + float invW = stiffness / W; + + b2Vec2 d = v - (1.0f / 3.0f) * (b0 + v + b1); + + b2Vec2 db0 = 2.0f * wb0 * invW * d; + b2Vec2 dv = -4.0f * wv * invW * d; + b2Vec2 db1 = 2.0f * wb1 * invW * d; + + b0 += db0; + v += dv; + b1 += db1; + + m_ps[c.i1] = b0; + m_ps[c.i2] = v; + m_ps[c.i3] = b1; + } +} + +void b2Rope::Draw(b2Draw* draw) const +{ + b2Color c(0.4f, 0.5f, 0.7f); + b2Color pg(0.1f, 0.8f, 0.1f); + b2Color pd(0.7f, 0.2f, 0.4f); + + for (int32 i = 0; i < m_count - 1; ++i) + { + draw->DrawSegment(m_ps[i], m_ps[i+1], c); + + const b2Color& pc = m_invMasses[i] > 0.0f ? pd : pg; + draw->DrawPoint(m_ps[i], 5.0f, pc); + } + + const b2Color& pc = m_invMasses[m_count - 1] > 0.0f ? pd : pg; + draw->DrawPoint(m_ps[m_count - 1], 5.0f, pc); +} diff --git a/src/collisionable.cpp b/src/collisionable.cpp index 1b87f868..1afe3a57 100644 --- a/src/collisionable.cpp +++ b/src/collisionable.cpp @@ -7,7 +7,7 @@ #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wsuggest-override" #endif//__GNUC__ -#include "Box2D/Box2D.h" +#include "box2d/box2d.h" #if defined(__GNUC__) && !defined(__clang__) #pragma GCC diagnostic pop #endif//__GNUC__ @@ -238,7 +238,7 @@ void Collisionable::setCollisionChain(const std::vector& points, bool } else { - shape.CreateChain(b_points.data(), static_cast(b_points.size())); + shape.CreateChain(b_points.data(), static_cast(b_points.size()), b2Vec2_zero, b2Vec2_zero); } createBody(&shape);