From 74c6021e8de1853099260f9de301c6ca63592a7b Mon Sep 17 00:00:00 2001 From: Garrett Brown Date: Sun, 16 Aug 2020 06:34:37 -0400 Subject: [PATCH] Starting box2d update --- CMakeLists.txt | 160 ++-- src/libraries/Box2D/Box2D.h | 103 ++- .../Box2D/Collision/Shapes/b2ChainShape.h | 105 --- .../Box2D/Collision/Shapes/b2CircleShape.h | 91 --- .../Box2D/Collision/Shapes/b2EdgeShape.h | 74 -- .../Box2D/Collision/b2CollideEdge.cpp | 698 ---------------- .../Box2D/Collision/b2TimeOfImpact.h | 58 -- .../{b2BroadPhase.cpp => b2_broad_phase.cpp} | 49 +- .../b2ChainShape.cpp => b2_chain_shape.cpp} | 96 +-- .../b2CircleShape.cpp => b2_circle_shape.cpp} | 54 +- ...ollideCircle.cpp => b2_collide_circle.cpp} | 64 +- .../Box2D/Collision/b2_collide_edge.cpp | 524 ++++++++++++ ...lidePolygon.cpp => b2_collide_polygon.cpp} | 70 +- .../{b2Collision.cpp => b2_collision.cpp} | 88 +- .../{b2Distance.cpp => b2_distance.cpp} | 277 +++++-- ...{b2DynamicTree.cpp => b2_dynamic_tree.cpp} | 129 +-- .../b2EdgeShape.cpp => b2_edge_shape.cpp} | 74 +- ...2PolygonShape.cpp => b2_polygon_shape.cpp} | 130 ++- ...TimeOfImpact.cpp => b2_time_of_impact.cpp} | 112 +-- src/libraries/Box2D/Common/b2BlockAllocator.h | 62 -- src/libraries/Box2D/Common/b2Draw.cpp | 44 - src/libraries/Box2D/Common/b2Draw.h | 86 -- src/libraries/Box2D/Common/b2GrowableStack.h | 85 -- src/libraries/Box2D/Common/b2Math.cpp | 94 --- src/libraries/Box2D/Common/b2Settings.cpp | 52 -- .../Box2D/Common/b2StackAllocator.cpp | 83 -- src/libraries/Box2D/Common/b2StackAllocator.h | 60 -- src/libraries/Box2D/Common/b2Timer.cpp | 101 --- src/libraries/Box2D/Common/b2Timer.h | 50 -- ...ckAllocator.cpp => b2_block_allocator.cpp} | 120 +-- src/libraries/Box2D/Common/b2_draw.cpp | 47 ++ src/libraries/Box2D/Common/b2_math.cpp | 98 +++ src/libraries/Box2D/Common/b2_settings.cpp | 77 ++ .../Box2D/Common/b2_stack_allocator.cpp | 87 ++ src/libraries/Box2D/Common/b2_timer.cpp | 125 +++ .../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.h | 169 ---- .../Box2D/Dynamics/Joints/b2FrictionJoint.h | 119 --- .../Box2D/Dynamics/Joints/b2GearJoint.h | 125 --- .../Box2D/Dynamics/Joints/b2Joint.cpp | 211 ----- .../Box2D/Dynamics/Joints/b2MotorJoint.h | 133 --- .../Box2D/Dynamics/Joints/b2MouseJoint.h | 129 --- .../Dynamics/Joints/b2PrismaticJoint.cpp | 629 -------------- .../Box2D/Dynamics/Joints/b2RevoluteJoint.cpp | 502 ------------ .../Box2D/Dynamics/Joints/b2RopeJoint.h | 114 --- .../Box2D/Dynamics/Joints/b2WeldJoint.h | 126 --- .../Box2D/Dynamics/Joints/b2WheelJoint.cpp | 419 ---------- .../Box2D/Dynamics/Joints/b2WheelJoint.h | 210 ----- .../Box2D/Dynamics/b2ContactManager.h | 52 -- src/libraries/Box2D/Dynamics/b2TimeStep.h | 70 -- .../Box2D/Dynamics/b2WorldCallbacks.cpp | 36 - .../Dynamics/{b2Body.cpp => b2_body.cpp} | 189 +++-- .../Dynamics/b2_chain_circle_contact.cpp | 57 ++ .../Box2D/Dynamics/b2_chain_circle_contact.h | 43 + .../Dynamics/b2_chain_polygon_contact.cpp | 57 ++ .../Box2D/Dynamics/b2_chain_polygon_contact.h | 43 + .../Box2D/Dynamics/b2_circle_contact.cpp | 56 ++ .../Box2D/Dynamics/b2_circle_contact.h | 43 + .../b2Contact.cpp => b2_contact.cpp} | 96 +-- ...tactManager.cpp => b2_contact_manager.cpp} | 73 +- ...ontactSolver.cpp => b2_contact_solver.cpp} | 218 ++--- .../Box2D/Dynamics/b2_contact_solver.h | 99 +++ ...istanceJoint.cpp => b2_distance_joint.cpp} | 132 +-- .../Box2D/Dynamics/b2_edge_circle_contact.cpp | 54 ++ .../Box2D/Dynamics/b2_edge_circle_contact.h | 43 + .../Dynamics/b2_edge_polygon_contact.cpp | 54 ++ .../Box2D/Dynamics/b2_edge_polygon_contact.h | 43 + .../{b2Fixture.cpp => b2_fixture.cpp} | 149 ++-- ...rictionJoint.cpp => b2_friction_joint.cpp} | 106 +-- .../b2GearJoint.cpp => b2_gear_joint.cpp} | 132 +-- .../Dynamics/{b2Island.cpp => b2_island.cpp} | 101 +-- .../Dynamics/{b2Island.h => b2_island.h} | 44 +- src/libraries/Box2D/Dynamics/b2_joint.cpp | 313 +++++++ .../b2MotorJoint.cpp => b2_motor_joint.cpp} | 131 +-- .../b2MouseJoint.cpp => b2_mouse_joint.cpp} | 112 +-- .../Dynamics/b2_polygon_circle_contact.cpp | 54 ++ .../Dynamics/b2_polygon_circle_contact.h | 42 + .../Box2D/Dynamics/b2_polygon_contact.cpp | 57 ++ .../Box2D/Dynamics/b2_polygon_contact.h | 43 + .../Box2D/Dynamics/b2_prismatic_joint.cpp | 644 +++++++++++++++ .../b2PulleyJoint.cpp => b2_pulley_joint.cpp} | 136 ++-- .../Box2D/Dynamics/b2_revolute_joint.cpp | 501 ++++++++++++ .../b2RopeJoint.cpp => b2_rope_joint.cpp} | 119 ++- .../b2WeldJoint.cpp => b2_weld_joint.cpp} | 130 +-- .../Box2D/Dynamics/b2_wheel_joint.cpp | 672 +++++++++++++++ .../Dynamics/{b2World.cpp => b2_world.cpp} | 285 +++---- .../Box2D/Dynamics/b2_world_callbacks.cpp | 40 + src/libraries/Box2D/Rope/b2Rope.cpp | 259 ------ src/libraries/Box2D/Rope/b2Rope.h | 115 --- src/libraries/Box2D/Rope/b2_rope.cpp | 769 ++++++++++++++++++ src/libraries/Box2D/b2_block_allocator.h | 59 ++ .../Box2D/{Dynamics/b2Body.h => b2_body.h} | 217 ++--- .../b2BroadPhase.h => b2_broad_phase.h} | 106 +-- src/libraries/Box2D/b2_chain_shape.h | 100 +++ src/libraries/Box2D/b2_circle_shape.h | 66 ++ .../b2Collision.h => b2_collision.h} | 64 +- .../Contacts/b2Contact.h => b2_contact.h} | 88 +- src/libraries/Box2D/b2_contact_manager.h | 56 ++ .../{Collision/b2Distance.h => b2_distance.h} | 81 +- src/libraries/Box2D/b2_distance_joint.h | 140 ++++ src/libraries/Box2D/b2_draw.h | 101 +++ .../b2DynamicTree.h => b2_dynamic_tree.h} | 67 +- src/libraries/Box2D/b2_edge_shape.h | 85 ++ .../{Dynamics/b2Fixture.h => b2_fixture.h} | 87 +- src/libraries/Box2D/b2_friction_joint.h | 123 +++ src/libraries/Box2D/b2_gear_joint.h | 129 +++ src/libraries/Box2D/b2_growable_stack.h | 90 ++ .../{Dynamics/Joints/b2Joint.h => b2_joint.h} | 82 +- .../Box2D/{Common/b2Math.h => b2_math.h} | 170 ++-- src/libraries/Box2D/b2_motor_joint.h | 137 ++++ src/libraries/Box2D/b2_mouse_joint.h | 133 +++ .../b2PolygonShape.h => b2_polygon_shape.h} | 71 +- ...2PrismaticJoint.h => b2_prismatic_joint.h} | 128 +-- .../b2PulleyJoint.h => b2_pulley_joint.h} | 98 +-- .../b2RevoluteJoint.h => b2_revolute_joint.h} | 136 ++-- src/libraries/Box2D/b2_rope.h | 150 ++++ src/libraries/Box2D/b2_rope_joint.h | 118 +++ .../{Common/b2Settings.h => b2_settings.h} | 56 +- .../Shapes/b2Shape.h => b2_shape.h} | 56 +- src/libraries/Box2D/b2_stack_allocator.h | 64 ++ src/libraries/Box2D/b2_time_of_impact.h | 62 ++ src/libraries/Box2D/b2_time_step.h | 73 ++ src/libraries/Box2D/b2_timer.h | 54 ++ src/libraries/Box2D/b2_weld_joint.h | 132 +++ src/libraries/Box2D/b2_wheel_joint.h | 239 ++++++ .../Box2D/{Dynamics/b2World.h => b2_world.h} | 96 +-- ...2WorldCallbacks.h => b2_world_callbacks.h} | 49 +- src/modules/physics/box2d/Body.cpp | 4 +- src/modules/physics/box2d/ChainShape.cpp | 32 +- src/modules/physics/box2d/DistanceJoint.cpp | 8 +- src/modules/physics/box2d/EdgeShape.cpp | 30 +- src/modules/physics/box2d/Joint.cpp | 2 +- src/modules/physics/box2d/MouseJoint.cpp | 8 +- src/modules/physics/box2d/Physics.cpp | 28 +- src/modules/physics/box2d/PolygonShape.cpp | 4 +- src/modules/physics/box2d/WeldJoint.cpp | 8 +- src/modules/physics/box2d/WheelJoint.cpp | 10 +- src/modules/physics/box2d/World.cpp | 4 +- src/modules/physics/box2d/World.h | 2 +- 153 files changed, 9634 insertions(+), 8518 deletions(-) delete mode 100644 src/libraries/Box2D/Collision/Shapes/b2ChainShape.h delete mode 100644 src/libraries/Box2D/Collision/Shapes/b2CircleShape.h delete mode 100644 src/libraries/Box2D/Collision/Shapes/b2EdgeShape.h delete mode 100644 src/libraries/Box2D/Collision/b2CollideEdge.cpp delete mode 100644 src/libraries/Box2D/Collision/b2TimeOfImpact.h rename src/libraries/Box2D/Collision/{b2BroadPhase.cpp => b2_broad_phase.cpp} (60%) rename src/libraries/Box2D/Collision/{Shapes/b2ChainShape.cpp => b2_chain_shape.cpp} (60%) rename src/libraries/Box2D/Collision/{Shapes/b2CircleShape.cpp => b2_circle_shape.cpp} (56%) rename src/libraries/Box2D/Collision/{b2CollideCircle.cpp => b2_collide_circle.cpp} (63%) create mode 100644 src/libraries/Box2D/Collision/b2_collide_edge.cpp rename src/libraries/Box2D/Collision/{b2CollidePolygon.cpp => b2_collide_polygon.cpp} (72%) rename src/libraries/Box2D/Collision/{b2Collision.cpp => b2_collision.cpp} (67%) rename src/libraries/Box2D/Collision/{b2Distance.cpp => b2_distance.cpp} (64%) rename src/libraries/Box2D/Collision/{b2DynamicTree.cpp => b2_dynamic_tree.cpp} (83%) rename src/libraries/Box2D/Collision/{Shapes/b2EdgeShape.cpp => b2_edge_shape.cpp} (52%) rename src/libraries/Box2D/Collision/{Shapes/b2PolygonShape.cpp => b2_polygon_shape.cpp} (74%) rename src/libraries/Box2D/Collision/{b2TimeOfImpact.cpp => b2_time_of_impact.cpp} (78%) delete mode 100644 src/libraries/Box2D/Common/b2BlockAllocator.h delete mode 100644 src/libraries/Box2D/Common/b2Draw.cpp delete mode 100644 src/libraries/Box2D/Common/b2Draw.h delete mode 100644 src/libraries/Box2D/Common/b2GrowableStack.h delete mode 100644 src/libraries/Box2D/Common/b2Math.cpp delete mode 100644 src/libraries/Box2D/Common/b2Settings.cpp delete mode 100644 src/libraries/Box2D/Common/b2StackAllocator.cpp delete mode 100644 src/libraries/Box2D/Common/b2StackAllocator.h delete mode 100644 src/libraries/Box2D/Common/b2Timer.cpp delete mode 100644 src/libraries/Box2D/Common/b2Timer.h rename src/libraries/Box2D/Common/{b2BlockAllocator.cpp => b2_block_allocator.cpp} (59%) create mode 100644 src/libraries/Box2D/Common/b2_draw.cpp create mode 100644 src/libraries/Box2D/Common/b2_math.cpp create mode 100644 src/libraries/Box2D/Common/b2_settings.cpp create mode 100644 src/libraries/Box2D/Common/b2_stack_allocator.cpp create mode 100644 src/libraries/Box2D/Common/b2_timer.cpp delete mode 100644 src/libraries/Box2D/Dynamics/Contacts/b2ChainAndCircleContact.cpp delete mode 100644 src/libraries/Box2D/Dynamics/Contacts/b2ChainAndCircleContact.h delete mode 100644 src/libraries/Box2D/Dynamics/Contacts/b2ChainAndPolygonContact.cpp delete mode 100644 src/libraries/Box2D/Dynamics/Contacts/b2ChainAndPolygonContact.h delete mode 100644 src/libraries/Box2D/Dynamics/Contacts/b2CircleContact.cpp delete mode 100644 src/libraries/Box2D/Dynamics/Contacts/b2CircleContact.h delete mode 100644 src/libraries/Box2D/Dynamics/Contacts/b2ContactSolver.h delete mode 100644 src/libraries/Box2D/Dynamics/Contacts/b2EdgeAndCircleContact.cpp delete mode 100644 src/libraries/Box2D/Dynamics/Contacts/b2EdgeAndCircleContact.h delete mode 100644 src/libraries/Box2D/Dynamics/Contacts/b2EdgeAndPolygonContact.cpp delete mode 100644 src/libraries/Box2D/Dynamics/Contacts/b2EdgeAndPolygonContact.h delete mode 100644 src/libraries/Box2D/Dynamics/Contacts/b2PolygonAndCircleContact.cpp delete mode 100644 src/libraries/Box2D/Dynamics/Contacts/b2PolygonAndCircleContact.h delete mode 100644 src/libraries/Box2D/Dynamics/Contacts/b2PolygonContact.cpp delete mode 100644 src/libraries/Box2D/Dynamics/Contacts/b2PolygonContact.h delete mode 100644 src/libraries/Box2D/Dynamics/Joints/b2DistanceJoint.h delete mode 100644 src/libraries/Box2D/Dynamics/Joints/b2FrictionJoint.h delete mode 100644 src/libraries/Box2D/Dynamics/Joints/b2GearJoint.h delete mode 100644 src/libraries/Box2D/Dynamics/Joints/b2Joint.cpp delete mode 100644 src/libraries/Box2D/Dynamics/Joints/b2MotorJoint.h delete mode 100644 src/libraries/Box2D/Dynamics/Joints/b2MouseJoint.h delete mode 100644 src/libraries/Box2D/Dynamics/Joints/b2PrismaticJoint.cpp delete mode 100644 src/libraries/Box2D/Dynamics/Joints/b2RevoluteJoint.cpp delete mode 100644 src/libraries/Box2D/Dynamics/Joints/b2RopeJoint.h delete mode 100644 src/libraries/Box2D/Dynamics/Joints/b2WeldJoint.h delete mode 100644 src/libraries/Box2D/Dynamics/Joints/b2WheelJoint.cpp delete mode 100644 src/libraries/Box2D/Dynamics/Joints/b2WheelJoint.h delete mode 100644 src/libraries/Box2D/Dynamics/b2ContactManager.h delete mode 100644 src/libraries/Box2D/Dynamics/b2TimeStep.h delete mode 100644 src/libraries/Box2D/Dynamics/b2WorldCallbacks.cpp rename src/libraries/Box2D/Dynamics/{b2Body.cpp => b2_body.cpp} (70%) create mode 100644 src/libraries/Box2D/Dynamics/b2_chain_circle_contact.cpp create mode 100644 src/libraries/Box2D/Dynamics/b2_chain_circle_contact.h create mode 100644 src/libraries/Box2D/Dynamics/b2_chain_polygon_contact.cpp create mode 100644 src/libraries/Box2D/Dynamics/b2_chain_polygon_contact.h create mode 100644 src/libraries/Box2D/Dynamics/b2_circle_contact.cpp create mode 100644 src/libraries/Box2D/Dynamics/b2_circle_contact.h rename src/libraries/Box2D/Dynamics/{Contacts/b2Contact.cpp => b2_contact.cpp} (72%) rename src/libraries/Box2D/Dynamics/{b2ContactManager.cpp => b2_contact_manager.cpp} (78%) rename src/libraries/Box2D/Dynamics/{Contacts/b2ContactSolver.cpp => b2_contact_solver.cpp} (79%) create mode 100644 src/libraries/Box2D/Dynamics/b2_contact_solver.h rename src/libraries/Box2D/Dynamics/{Joints/b2DistanceJoint.cpp => b2_distance_joint.cpp} (59%) create mode 100644 src/libraries/Box2D/Dynamics/b2_edge_circle_contact.cpp create mode 100644 src/libraries/Box2D/Dynamics/b2_edge_circle_contact.h create mode 100644 src/libraries/Box2D/Dynamics/b2_edge_polygon_contact.cpp create mode 100644 src/libraries/Box2D/Dynamics/b2_edge_polygon_contact.h rename src/libraries/Box2D/Dynamics/{b2Fixture.cpp => b2_fixture.cpp} (54%) rename src/libraries/Box2D/Dynamics/{Joints/b2FrictionJoint.cpp => b2_friction_joint.cpp} (62%) rename src/libraries/Box2D/Dynamics/{Joints/b2GearJoint.cpp => b2_gear_joint.cpp} (74%) rename src/libraries/Box2D/Dynamics/{b2Island.cpp => b2_island.cpp} (86%) rename src/libraries/Box2D/Dynamics/{b2Island.h => b2_island.h} (54%) create mode 100644 src/libraries/Box2D/Dynamics/b2_joint.cpp rename src/libraries/Box2D/Dynamics/{Joints/b2MotorJoint.cpp => b2_motor_joint.cpp} (61%) rename src/libraries/Box2D/Dynamics/{Joints/b2MouseJoint.cpp => b2_mouse_joint.cpp} (56%) create mode 100644 src/libraries/Box2D/Dynamics/b2_polygon_circle_contact.cpp create mode 100644 src/libraries/Box2D/Dynamics/b2_polygon_circle_contact.h create mode 100644 src/libraries/Box2D/Dynamics/b2_polygon_contact.cpp create mode 100644 src/libraries/Box2D/Dynamics/b2_polygon_contact.h create mode 100644 src/libraries/Box2D/Dynamics/b2_prismatic_joint.cpp rename src/libraries/Box2D/Dynamics/{Joints/b2PulleyJoint.cpp => b2_pulley_joint.cpp} (62%) create mode 100644 src/libraries/Box2D/Dynamics/b2_revolute_joint.cpp rename src/libraries/Box2D/Dynamics/{Joints/b2RopeJoint.cpp => b2_rope_joint.cpp} (57%) rename src/libraries/Box2D/Dynamics/{Joints/b2WeldJoint.cpp => b2_weld_joint.cpp} (66%) create mode 100644 src/libraries/Box2D/Dynamics/b2_wheel_joint.cpp rename src/libraries/Box2D/Dynamics/{b2World.cpp => b2_world.cpp} (82%) create mode 100644 src/libraries/Box2D/Dynamics/b2_world_callbacks.cpp delete mode 100644 src/libraries/Box2D/Rope/b2Rope.cpp delete mode 100644 src/libraries/Box2D/Rope/b2Rope.h create mode 100644 src/libraries/Box2D/Rope/b2_rope.cpp create mode 100644 src/libraries/Box2D/b2_block_allocator.h rename src/libraries/Box2D/{Dynamics/b2Body.h => b2_body.h} (80%) rename src/libraries/Box2D/{Collision/b2BroadPhase.h => b2_broad_phase.h} (72%) create mode 100644 src/libraries/Box2D/b2_chain_shape.h create mode 100644 src/libraries/Box2D/b2_circle_shape.h rename src/libraries/Box2D/{Collision/b2Collision.h => b2_collision.h} (81%) rename src/libraries/Box2D/{Dynamics/Contacts/b2Contact.h => b2_contact.h} (78%) create mode 100644 src/libraries/Box2D/b2_contact_manager.h rename src/libraries/Box2D/{Collision/b2Distance.h => b2_distance.h} (52%) create mode 100644 src/libraries/Box2D/b2_distance_joint.h create mode 100644 src/libraries/Box2D/b2_draw.h rename src/libraries/Box2D/{Collision/b2DynamicTree.h => b2_dynamic_tree.h} (78%) create mode 100644 src/libraries/Box2D/b2_edge_shape.h rename src/libraries/Box2D/{Dynamics/b2Fixture.h => b2_fixture.h} (79%) create mode 100644 src/libraries/Box2D/b2_friction_joint.h create mode 100644 src/libraries/Box2D/b2_gear_joint.h create mode 100644 src/libraries/Box2D/b2_growable_stack.h rename src/libraries/Box2D/{Dynamics/Joints/b2Joint.h => b2_joint.h} (66%) rename src/libraries/Box2D/{Common/b2Math.h => b2_math.h} (78%) create mode 100644 src/libraries/Box2D/b2_motor_joint.h create mode 100644 src/libraries/Box2D/b2_mouse_joint.h rename src/libraries/Box2D/{Collision/Shapes/b2PolygonShape.h => b2_polygon_shape.h} (51%) rename src/libraries/Box2D/{Dynamics/Joints/b2PrismaticJoint.h => b2_prismatic_joint.h} (58%) rename src/libraries/Box2D/{Dynamics/Joints/b2PulleyJoint.h => b2_pulley_joint.h} (56%) rename src/libraries/Box2D/{Dynamics/Joints/b2RevoluteJoint.h => b2_revolute_joint.h} (55%) create mode 100644 src/libraries/Box2D/b2_rope.h create mode 100644 src/libraries/Box2D/b2_rope_joint.h rename src/libraries/Box2D/{Common/b2Settings.h => b2_settings.h} (74%) rename src/libraries/Box2D/{Collision/Shapes/b2Shape.h => b2_shape.h} (62%) create mode 100644 src/libraries/Box2D/b2_stack_allocator.h create mode 100644 src/libraries/Box2D/b2_time_of_impact.h create mode 100644 src/libraries/Box2D/b2_time_step.h create mode 100644 src/libraries/Box2D/b2_timer.h create mode 100644 src/libraries/Box2D/b2_weld_joint.h create mode 100644 src/libraries/Box2D/b2_wheel_joint.h rename src/libraries/Box2D/{Dynamics/b2World.h => b2_world.h} (82%) rename src/libraries/Box2D/{Dynamics/b2WorldCallbacks.h => b2_world_callbacks.h} (76%) diff --git a/CMakeLists.txt b/CMakeLists.txt index 541b0ea14..d1290e393 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1068,126 +1068,77 @@ set(LOVE_SRC_3P_BOX2D_ROOT ) set(LOVE_SRC_3P_BOX2D_COLLISION - src/libraries/Box2D/Collision/b2BroadPhase.cpp - src/libraries/Box2D/Collision/b2BroadPhase.h - src/libraries/Box2D/Collision/b2CollideCircle.cpp - src/libraries/Box2D/Collision/b2CollideEdge.cpp - src/libraries/Box2D/Collision/b2CollidePolygon.cpp - src/libraries/Box2D/Collision/b2Collision.cpp - src/libraries/Box2D/Collision/b2Collision.h - src/libraries/Box2D/Collision/b2Distance.cpp - src/libraries/Box2D/Collision/b2Distance.h - src/libraries/Box2D/Collision/b2DynamicTree.cpp - src/libraries/Box2D/Collision/b2DynamicTree.h - src/libraries/Box2D/Collision/b2TimeOfImpact.cpp - src/libraries/Box2D/Collision/b2TimeOfImpact.h -) - -set(LOVE_SRC_3P_BOX2D_COLLISION_SHAPES - src/libraries/Box2D/Collision/Shapes/b2ChainShape.cpp - src/libraries/Box2D/Collision/Shapes/b2ChainShape.h - src/libraries/Box2D/Collision/Shapes/b2CircleShape.cpp - src/libraries/Box2D/Collision/Shapes/b2CircleShape.h - src/libraries/Box2D/Collision/Shapes/b2EdgeShape.cpp - src/libraries/Box2D/Collision/Shapes/b2EdgeShape.h - src/libraries/Box2D/Collision/Shapes/b2PolygonShape.cpp - src/libraries/Box2D/Collision/Shapes/b2PolygonShape.h - src/libraries/Box2D/Collision/Shapes/b2Shape.h + src/libraries/Box2D/collision/b2_broad_phase.cpp + src/libraries/Box2D/collision/b2_chain_shape.cpp + src/libraries/Box2D/collision/b2_circle_shape.cpp + src/libraries/Box2D/collision/b2_collide_circle.cpp + src/libraries/Box2D/collision/b2_collide_edge.cpp + src/libraries/Box2D/collision/b2_collide_polygon.cpp + src/libraries/Box2D/collision/b2_collision.cpp + src/libraries/Box2D/collision/b2_distance.cpp + src/libraries/Box2D/collision/b2_dynamic_tree.cpp + src/libraries/Box2D/collision/b2_edge_shape.cpp + src/libraries/Box2D/collision/b2_polygon_shape.cpp + src/libraries/Box2D/collision/b2_time_of_impact.cpp ) set(LOVE_SRC_3P_BOX2D_COMMON - src/libraries/Box2D/Common/b2BlockAllocator.cpp - src/libraries/Box2D/Common/b2BlockAllocator.h - src/libraries/Box2D/Common/b2Draw.cpp - src/libraries/Box2D/Common/b2Draw.h - src/libraries/Box2D/Common/b2GrowableStack.h - src/libraries/Box2D/Common/b2Math.cpp - src/libraries/Box2D/Common/b2Math.h - src/libraries/Box2D/Common/b2Settings.cpp - src/libraries/Box2D/Common/b2Settings.h - src/libraries/Box2D/Common/b2StackAllocator.cpp - src/libraries/Box2D/Common/b2StackAllocator.h - src/libraries/Box2D/Common/b2Timer.cpp - src/libraries/Box2D/Common/b2Timer.h + src/libraries/Box2D/common/b2_block_allocator.cpp + src/libraries/Box2D/common/b2_draw.cpp + src/libraries/Box2D/common/b2_math.cpp + src/libraries/Box2D/common/b2_settings.cpp + src/libraries/Box2D/common/b2_stack_allocator.cpp + src/libraries/Box2D/common/b2_timer.cpp ) set(LOVE_SRC_3P_BOX2D_DYNAMICS - src/libraries/Box2D/Dynamics/b2Body.cpp - src/libraries/Box2D/Dynamics/b2Body.h - src/libraries/Box2D/Dynamics/b2ContactManager.cpp - src/libraries/Box2D/Dynamics/b2ContactManager.h - src/libraries/Box2D/Dynamics/b2Fixture.cpp - src/libraries/Box2D/Dynamics/b2Fixture.h - src/libraries/Box2D/Dynamics/b2Island.cpp - src/libraries/Box2D/Dynamics/b2Island.h - src/libraries/Box2D/Dynamics/b2TimeStep.h - src/libraries/Box2D/Dynamics/b2World.cpp - src/libraries/Box2D/Dynamics/b2World.h - src/libraries/Box2D/Dynamics/b2WorldCallbacks.cpp - src/libraries/Box2D/Dynamics/b2WorldCallbacks.h -) - -set(LOVE_SRC_3P_BOX2D_DYNAMICS_CONTACTS - src/libraries/Box2D/Dynamics/Contacts/b2ChainAndCircleContact.cpp - src/libraries/Box2D/Dynamics/Contacts/b2ChainAndCircleContact.h - src/libraries/Box2D/Dynamics/Contacts/b2ChainAndPolygonContact.cpp - src/libraries/Box2D/Dynamics/Contacts/b2ChainAndPolygonContact.h - src/libraries/Box2D/Dynamics/Contacts/b2CircleContact.cpp - src/libraries/Box2D/Dynamics/Contacts/b2CircleContact.h - src/libraries/Box2D/Dynamics/Contacts/b2Contact.cpp - src/libraries/Box2D/Dynamics/Contacts/b2Contact.h - src/libraries/Box2D/Dynamics/Contacts/b2ContactSolver.cpp - src/libraries/Box2D/Dynamics/Contacts/b2ContactSolver.h - src/libraries/Box2D/Dynamics/Contacts/b2EdgeAndCircleContact.cpp - src/libraries/Box2D/Dynamics/Contacts/b2EdgeAndCircleContact.h - src/libraries/Box2D/Dynamics/Contacts/b2EdgeAndPolygonContact.cpp - src/libraries/Box2D/Dynamics/Contacts/b2EdgeAndPolygonContact.h - src/libraries/Box2D/Dynamics/Contacts/b2PolygonAndCircleContact.cpp - src/libraries/Box2D/Dynamics/Contacts/b2PolygonAndCircleContact.h - src/libraries/Box2D/Dynamics/Contacts/b2PolygonContact.cpp - src/libraries/Box2D/Dynamics/Contacts/b2PolygonContact.h -) - -set(LOVE_SRC_3P_BOX2D_DYNAMICS_JOINTS - src/libraries/Box2D/Dynamics/Joints/b2DistanceJoint.cpp - src/libraries/Box2D/Dynamics/Joints/b2DistanceJoint.h - src/libraries/Box2D/Dynamics/Joints/b2FrictionJoint.cpp - src/libraries/Box2D/Dynamics/Joints/b2FrictionJoint.h - src/libraries/Box2D/Dynamics/Joints/b2GearJoint.cpp - src/libraries/Box2D/Dynamics/Joints/b2GearJoint.h - src/libraries/Box2D/Dynamics/Joints/b2Joint.cpp - src/libraries/Box2D/Dynamics/Joints/b2Joint.h - src/libraries/Box2D/Dynamics/Joints/b2MotorJoint.cpp - src/libraries/Box2D/Dynamics/Joints/b2MotorJoint.h - src/libraries/Box2D/Dynamics/Joints/b2MouseJoint.cpp - src/libraries/Box2D/Dynamics/Joints/b2MouseJoint.h - src/libraries/Box2D/Dynamics/Joints/b2PrismaticJoint.cpp - src/libraries/Box2D/Dynamics/Joints/b2PrismaticJoint.h - src/libraries/Box2D/Dynamics/Joints/b2PulleyJoint.cpp - src/libraries/Box2D/Dynamics/Joints/b2PulleyJoint.h - src/libraries/Box2D/Dynamics/Joints/b2RevoluteJoint.cpp - src/libraries/Box2D/Dynamics/Joints/b2RevoluteJoint.h - src/libraries/Box2D/Dynamics/Joints/b2RopeJoint.cpp - src/libraries/Box2D/Dynamics/Joints/b2RopeJoint.h - src/libraries/Box2D/Dynamics/Joints/b2WeldJoint.cpp - src/libraries/Box2D/Dynamics/Joints/b2WeldJoint.h - src/libraries/Box2D/Dynamics/Joints/b2WheelJoint.cpp - src/libraries/Box2D/Dynamics/Joints/b2WheelJoint.h + src/libraries/Box2D/dynamics/b2_body.cpp + src/libraries/Box2D/dynamics/b2_chain_circle_contact.cpp + src/libraries/Box2D/dynamics/b2_chain_circle_contact.h + src/libraries/Box2D/dynamics/b2_chain_polygon_contact.cpp + src/libraries/Box2D/dynamics/b2_chain_polygon_contact.h + src/libraries/Box2D/dynamics/b2_circle_contact.cpp + src/libraries/Box2D/dynamics/b2_circle_contact.h + src/libraries/Box2D/dynamics/b2_contact.cpp + src/libraries/Box2D/dynamics/b2_contact_manager.cpp + src/libraries/Box2D/dynamics/b2_contact_solver.cpp + src/libraries/Box2D/dynamics/b2_contact_solver.h + src/libraries/Box2D/dynamics/b2_distance_joint.cpp + src/libraries/Box2D/dynamics/b2_edge_circle_contact.cpp + src/libraries/Box2D/dynamics/b2_edge_circle_contact.h + src/libraries/Box2D/dynamics/b2_edge_polygon_contact.cpp + src/libraries/Box2D/dynamics/b2_edge_polygon_contact.h + src/libraries/Box2D/dynamics/b2_fixture.cpp + src/libraries/Box2D/dynamics/b2_friction_joint.cpp + src/libraries/Box2D/dynamics/b2_gear_joint.cpp + src/libraries/Box2D/dynamics/b2_island.cpp + src/libraries/Box2D/dynamics/b2_island.h + src/libraries/Box2D/dynamics/b2_joint.cpp + src/libraries/Box2D/dynamics/b2_motor_joint.cpp + src/libraries/Box2D/dynamics/b2_mouse_joint.cpp + src/libraries/Box2D/dynamics/b2_polygon_circle_contact.cpp + src/libraries/Box2D/dynamics/b2_polygon_circle_contact.h + src/libraries/Box2D/dynamics/b2_polygon_contact.cpp + src/libraries/Box2D/dynamics/b2_polygon_contact.h + src/libraries/Box2D/dynamics/b2_prismatic_joint.cpp + src/libraries/Box2D/dynamics/b2_pulley_joint.cpp + src/libraries/Box2D/dynamics/b2_revolute_joint.cpp + src/libraries/Box2D/dynamics/b2_rope_joint.cpp + src/libraries/Box2D/dynamics/b2_weld_joint.cpp + src/libraries/Box2D/dynamics/b2_wheel_joint.cpp + src/libraries/Box2D/dynamics/b2_world.cpp + src/libraries/Box2D/dynamics/b2_world_callbacks.cpp ) set(LOVE_SRC_3P_BOX2D_ROPE - src/libraries/Box2D/Rope/b2Rope.cpp - src/libraries/Box2D/Rope/b2Rope.h + src/libraries/Box2D/rope/b2_rope.cpp ) set(LOVE_SRC_3P_BOX2D ${LOVE_SRC_3P_BOX2D_ROOT} ${LOVE_SRC_3P_BOX2D_COLLISION} - ${LOVE_SRC_3P_BOX2D_COLLISION_SHAPES} ${LOVE_SRC_3P_BOX2D_COMMON} ${LOVE_SRC_3P_BOX2D_DYNAMICS} - ${LOVE_SRC_3P_BOX2D_DYNAMICS_CONTACTS} - ${LOVE_SRC_3P_BOX2D_DYNAMICS_JOINTS} ${LOVE_SRC_3P_BOX2D_ROPE} ) @@ -1726,6 +1677,7 @@ set(LOVE_LIB_SRC include_directories( src src/libraries + src/libraries/Box2D src/modules ${LOVE_INCLUDE_DIRS} ) diff --git a/src/libraries/Box2D/Box2D.h b/src/libraries/Box2D/Box2D.h index 187046131..71db86c58 100644 --- a/src/libraries/Box2D/Box2D.h +++ b/src/libraries/Box2D/Box2D.h @@ -1,68 +1,59 @@ -/* -* 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. -*/ +// 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 -/** -\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 "b2_settings.h" +#include "b2_draw.h" +#include "b2_timer.h" -#include -#include -#include -#include +#include "b2_chain_shape.h" +#include "b2_circle_shape.h" +#include "b2_edge_shape.h" +#include "b2_polygon_shape.h" -#include -#include -#include -#include +#include "b2_broad_phase.h" +#include "b2_dynamic_tree.h" -#include -#include -#include -#include -#include +#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 - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include +#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_rope_joint.h" +#include "b2_weld_joint.h" +#include "b2_wheel_joint.h" #endif diff --git a/src/libraries/Box2D/Collision/Shapes/b2ChainShape.h b/src/libraries/Box2D/Collision/Shapes/b2ChainShape.h deleted file mode 100644 index 44493c43d..000000000 --- a/src/libraries/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/src/libraries/Box2D/Collision/Shapes/b2CircleShape.h b/src/libraries/Box2D/Collision/Shapes/b2CircleShape.h deleted file mode 100644 index 3209f924a..000000000 --- a/src/libraries/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/src/libraries/Box2D/Collision/Shapes/b2EdgeShape.h b/src/libraries/Box2D/Collision/Shapes/b2EdgeShape.h deleted file mode 100644 index 0acef8518..000000000 --- a/src/libraries/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/src/libraries/Box2D/Collision/b2CollideEdge.cpp b/src/libraries/Box2D/Collision/b2CollideEdge.cpp deleted file mode 100644 index 88e311c66..000000000 --- a/src/libraries/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/src/libraries/Box2D/Collision/b2TimeOfImpact.h b/src/libraries/Box2D/Collision/b2TimeOfImpact.h deleted file mode 100644 index 20620c1ee..000000000 --- a/src/libraries/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/src/libraries/Box2D/Collision/b2BroadPhase.cpp b/src/libraries/Box2D/Collision/b2_broad_phase.cpp similarity index 60% rename from src/libraries/Box2D/Collision/b2BroadPhase.cpp rename to src/libraries/Box2D/Collision/b2_broad_phase.cpp index f46e4d090..d063a3aa9 100644 --- a/src/libraries/Box2D/Collision/b2BroadPhase.cpp +++ b/src/libraries/Box2D/Collision/b2_broad_phase.cpp @@ -1,22 +1,26 @@ -/* -* 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. -*/ +// MIT License -#include +// 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() @@ -102,11 +106,18 @@ bool b2BroadPhase::QueryCallback(int32 proxyId) 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 *= 2; + 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); diff --git a/src/libraries/Box2D/Collision/Shapes/b2ChainShape.cpp b/src/libraries/Box2D/Collision/b2_chain_shape.cpp similarity index 60% rename from src/libraries/Box2D/Collision/Shapes/b2ChainShape.cpp rename to src/libraries/Box2D/Collision/b2_chain_shape.cpp index 0c7282cc1..b964a4303 100644 --- a/src/libraries/Box2D/Collision/Shapes/b2ChainShape.cpp +++ b/src/libraries/Box2D/Collision/b2_chain_shape.cpp @@ -1,23 +1,30 @@ -/* -* 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. -*/ +// 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 #include #include @@ -29,14 +36,19 @@ b2ChainShape::~b2ChainShape() void b2ChainShape::Clear() { b2Free(m_vertices); - m_vertices = NULL; + m_vertices = nullptr; m_count = 0; } void b2ChainShape::CreateLoop(const b2Vec2* vertices, int32 count) { - b2Assert(m_vertices == NULL && m_count == 0); + 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]; @@ -51,13 +63,11 @@ void b2ChainShape::CreateLoop(const b2Vec2* vertices, int32 count) 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) +void b2ChainShape::CreateChain(const b2Vec2* vertices, int32 count, const b2Vec2& prevVertex, const b2Vec2& nextVertex) { - b2Assert(m_vertices == NULL && m_count == 0); + b2Assert(m_vertices == nullptr && m_count == 0); b2Assert(count >= 2); for (int32 i = 1; i < count; ++i) { @@ -69,34 +79,15 @@ void b2ChainShape::CreateChain(const b2Vec2* vertices, int32 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; + clone->CreateChain(m_vertices, m_count, m_prevVertex, m_nextVertex); return clone; } @@ -114,27 +105,24 @@ void b2ChainShape::GetChildEdge(b2EdgeShape* edge, int32 index) const 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]; - 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; } } @@ -179,11 +167,15 @@ void b2ChainShape::ComputeAABB(b2AABB* aabb, const b2Transform& xf, int32 childI b2Vec2 v1 = b2Mul(xf, m_vertices[i1]); b2Vec2 v2 = b2Mul(xf, m_vertices[i2]); - aabb->lowerBound = b2Min(v1, v2); - aabb->upperBound = b2Max(v1, v2); + 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, float32 density) const +void b2ChainShape::ComputeMass(b2MassData* massData, float density) const { B2_NOT_USED(density); diff --git a/src/libraries/Box2D/Collision/Shapes/b2CircleShape.cpp b/src/libraries/Box2D/Collision/b2_circle_shape.cpp similarity index 56% rename from src/libraries/Box2D/Collision/Shapes/b2CircleShape.cpp rename to src/libraries/Box2D/Collision/b2_circle_shape.cpp index 39d4ae999..ecc69293b 100644 --- a/src/libraries/Box2D/Collision/Shapes/b2CircleShape.cpp +++ b/src/libraries/Box2D/Collision/b2_circle_shape.cpp @@ -1,22 +1,28 @@ -/* -* 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. -*/ +// 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 #include b2Shape* b2CircleShape::Clone(b2BlockAllocator* allocator) const @@ -50,13 +56,13 @@ bool b2CircleShape::RayCast(b2RayCastOutput* output, const b2RayCastInput& input b2Vec2 position = transform.p + b2Mul(transform.q, m_p); b2Vec2 s = input.p1 - position; - float32 b = b2Dot(s, s) - m_radius * m_radius; + float 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; + 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) @@ -65,7 +71,7 @@ bool b2CircleShape::RayCast(b2RayCastOutput* output, const b2RayCastInput& input } // Find the point of intersection of the line with the circle. - float32 a = -(c + b2Sqrt(sigma)); + float a = -(c + b2Sqrt(sigma)); // Is the intersection point on the segment? if (0.0f <= a && a <= input.maxFraction * rr) @@ -89,7 +95,7 @@ void b2CircleShape::ComputeAABB(b2AABB* aabb, const b2Transform& transform, int3 aabb->upperBound.Set(p.x + m_radius, p.y + m_radius); } -void b2CircleShape::ComputeMass(b2MassData* massData, float32 density) const +void b2CircleShape::ComputeMass(b2MassData* massData, float density) const { massData->mass = density * b2_pi * m_radius * m_radius; massData->center = m_p; diff --git a/src/libraries/Box2D/Collision/b2CollideCircle.cpp b/src/libraries/Box2D/Collision/b2_collide_circle.cpp similarity index 63% rename from src/libraries/Box2D/Collision/b2CollideCircle.cpp rename to src/libraries/Box2D/Collision/b2_collide_circle.cpp index 4181fd47c..469da5590 100644 --- a/src/libraries/Box2D/Collision/b2CollideCircle.cpp +++ b/src/libraries/Box2D/Collision/b2_collide_circle.cpp @@ -1,24 +1,28 @@ -/* -* 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. -*/ +// MIT License -#include -#include -#include +// 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, @@ -31,9 +35,9 @@ void b2CollideCircles( 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; + float distSqr = b2Dot(d, d); + float rA = circleA->m_radius, rB = circleB->m_radius; + float radius = rA + rB; if (distSqr > radius * radius) { return; @@ -61,15 +65,15 @@ void b2CollidePolygonAndCircle( // Find the min separating edge. int32 normalIndex = 0; - float32 separation = -b2_maxFloat; - float32 radius = polygonA->m_radius + circleB->m_radius; + 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) { - float32 s = b2Dot(normals[i], cLocal - vertices[i]); + float s = b2Dot(normals[i], cLocal - vertices[i]); if (s > radius) { @@ -103,8 +107,8 @@ void b2CollidePolygonAndCircle( } // Compute barycentric coordinates - float32 u1 = b2Dot(cLocal - v1, v2 - v1); - float32 u2 = b2Dot(cLocal - v2, v1 - v2); + float u1 = b2Dot(cLocal - v1, v2 - v1); + float u2 = b2Dot(cLocal - v2, v1 - v2); if (u1 <= 0.0f) { if (b2DistanceSquared(cLocal, v1) > radius * radius) @@ -138,8 +142,8 @@ void b2CollidePolygonAndCircle( else { b2Vec2 faceCenter = 0.5f * (v1 + v2); - float32 separation = b2Dot(cLocal - faceCenter, normals[vertIndex1]); - if (separation > radius) + float s = b2Dot(cLocal - faceCenter, normals[vertIndex1]); + if (s > radius) { return; } diff --git a/src/libraries/Box2D/Collision/b2_collide_edge.cpp b/src/libraries/Box2D/Collision/b2_collide_edge.cpp new file mode 100644 index 000000000..e06b90097 --- /dev/null +++ b/src/libraries/Box2D/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/src/libraries/Box2D/Collision/b2CollidePolygon.cpp b/src/libraries/Box2D/Collision/b2_collide_polygon.cpp similarity index 72% rename from src/libraries/Box2D/Collision/b2CollidePolygon.cpp rename to src/libraries/Box2D/Collision/b2_collide_polygon.cpp index 0aac38b2a..f3fa850bc 100644 --- a/src/libraries/Box2D/Collision/b2CollidePolygon.cpp +++ b/src/libraries/Box2D/Collision/b2_collide_polygon.cpp @@ -1,26 +1,30 @@ -/* -* 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. -*/ +// MIT License -#include -#include +// 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 float32 b2FindMaxSeparation(int32* edgeIndex, +static float b2FindMaxSeparation(int32* edgeIndex, const b2PolygonShape* poly1, const b2Transform& xf1, const b2PolygonShape* poly2, const b2Transform& xf2) { @@ -32,7 +36,7 @@ static float32 b2FindMaxSeparation(int32* edgeIndex, b2Transform xf = b2MulT(xf2, xf1); int32 bestIndex = 0; - float32 maxSeparation = -b2_maxFloat; + float maxSeparation = -b2_maxFloat; for (int32 i = 0; i < count1; ++i) { // Get poly1 normal in frame2. @@ -40,10 +44,10 @@ static float32 b2FindMaxSeparation(int32* edgeIndex, b2Vec2 v1 = b2Mul(xf, v1s[i]); // Find deepest point for normal i. - float32 si = b2_maxFloat; + float si = b2_maxFloat; for (int32 j = 0; j < count2; ++j) { - float32 sij = b2Dot(n, v2s[j] - v1); + float sij = b2Dot(n, v2s[j] - v1); if (sij < si) { si = sij; @@ -78,10 +82,10 @@ static void b2FindIncidentEdge(b2ClipVertex c[2], // Find the incident edge on poly2. int32 index = 0; - float32 minDot = b2_maxFloat; + float minDot = b2_maxFloat; for (int32 i = 0; i < count2; ++i) { - float32 dot = b2Dot(normal1, normals2[i]); + float dot = b2Dot(normal1, normals2[i]); if (dot < minDot) { minDot = dot; @@ -118,15 +122,15 @@ void b2CollidePolygons(b2Manifold* manifold, const b2PolygonShape* polyB, const b2Transform& xfB) { manifold->pointCount = 0; - float32 totalRadius = polyA->m_radius + polyB->m_radius; + float totalRadius = polyA->m_radius + polyB->m_radius; int32 edgeA = 0; - float32 separationA = b2FindMaxSeparation(&edgeA, polyA, xfA, polyB, xfB); + float separationA = b2FindMaxSeparation(&edgeA, polyA, xfA, polyB, xfB); if (separationA > totalRadius) return; int32 edgeB = 0; - float32 separationB = b2FindMaxSeparation(&edgeB, polyB, xfB, polyA, xfA); + float separationB = b2FindMaxSeparation(&edgeB, polyB, xfB, polyA, xfA); if (separationB > totalRadius) return; @@ -135,7 +139,7 @@ void b2CollidePolygons(b2Manifold* manifold, b2Transform xf1, xf2; int32 edge1; // reference edge uint8 flip; - const float32 k_tol = 0.1f * b2_linearSlop; + const float k_tol = 0.1f * b2_linearSlop; if (separationB > separationA + k_tol) { @@ -183,11 +187,11 @@ void b2CollidePolygons(b2Manifold* manifold, v12 = b2Mul(xf1, v12); // Face offset. - float32 frontOffset = b2Dot(normal, v11); + float frontOffset = b2Dot(normal, v11); // Side offsets, extended by polytope skin thickness. - float32 sideOffset1 = -b2Dot(tangent, v11) + totalRadius; - float32 sideOffset2 = b2Dot(tangent, v12) + totalRadius; + float sideOffset1 = -b2Dot(tangent, v11) + totalRadius; + float sideOffset2 = b2Dot(tangent, v12) + totalRadius; // Clip incident edge against extruded edge1 side edges. b2ClipVertex clipPoints1[2]; @@ -215,7 +219,7 @@ void b2CollidePolygons(b2Manifold* manifold, int32 pointCount = 0; for (int32 i = 0; i < b2_maxManifoldPoints; ++i) { - float32 separation = b2Dot(normal, clipPoints2[i].v) - frontOffset; + float separation = b2Dot(normal, clipPoints2[i].v) - frontOffset; if (separation <= totalRadius) { diff --git a/src/libraries/Box2D/Collision/b2Collision.cpp b/src/libraries/Box2D/Collision/b2_collision.cpp similarity index 67% rename from src/libraries/Box2D/Collision/b2Collision.cpp rename to src/libraries/Box2D/Collision/b2_collision.cpp index 4ebf35565..8a0501be3 100644 --- a/src/libraries/Box2D/Collision/b2Collision.cpp +++ b/src/libraries/Box2D/Collision/b2_collision.cpp @@ -1,27 +1,31 @@ -/* -* 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. -*/ +// MIT License -#include -#include +// 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, float32 radiusA, - const b2Transform& xfB, float32 radiusB) + const b2Transform& xfA, float radiusA, + const b2Transform& xfB, float radiusB) { if (manifold->pointCount == 0) { @@ -132,8 +136,8 @@ void b2GetPointStates(b2PointState state1[b2_maxManifoldPoints], b2PointState st // From Real-time Collision Detection, p179. bool b2AABB::RayCast(b2RayCastOutput* output, const b2RayCastInput& input) const { - float32 tmin = -b2_maxFloat; - float32 tmax = b2_maxFloat; + float tmin = -b2_maxFloat; + float tmax = b2_maxFloat; b2Vec2 p = input.p1; b2Vec2 d = input.p2 - input.p1; @@ -153,12 +157,12 @@ bool b2AABB::RayCast(b2RayCastOutput* output, const b2RayCastInput& input) const } else { - float32 inv_d = 1.0f / d(i); - float32 t1 = (lowerBound(i) - p(i)) * inv_d; - float32 t2 = (upperBound(i) - p(i)) * inv_d; + 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. - float32 s = -1.0f; + float s = -1.0f; if (t1 > t2) { @@ -199,35 +203,37 @@ bool b2AABB::RayCast(b2RayCastOutput* output, const b2RayCastInput& input) const // Sutherland-Hodgman clipping. int32 b2ClipSegmentToLine(b2ClipVertex vOut[2], const b2ClipVertex vIn[2], - const b2Vec2& normal, float32 offset, int32 vertexIndexA) + const b2Vec2& normal, float offset, int32 vertexIndexA) { // Start with no output points - int32 numOut = 0; + int32 count = 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; + 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[numOut++] = vIn[0]; - if (distance1 <= 0.0f) vOut[numOut++] = vIn[1]; + 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 - float32 interp = distance0 / (distance0 - distance1); - vOut[numOut].v = vIn[0].v + interp * (vIn[1].v - vIn[0].v); + float interp = distance0 / (distance0 - distance1); + vOut[count].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; + 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 numOut; + return count; } bool b2TestOverlap( const b2Shape* shapeA, int32 indexA, diff --git a/src/libraries/Box2D/Collision/b2Distance.cpp b/src/libraries/Box2D/Collision/b2_distance.cpp similarity index 64% rename from src/libraries/Box2D/Collision/b2Distance.cpp rename to src/libraries/Box2D/Collision/b2_distance.cpp index 78daab3e2..e2a27abd0 100644 --- a/src/libraries/Box2D/Collision/b2Distance.cpp +++ b/src/libraries/Box2D/Collision/b2_distance.cpp @@ -1,26 +1,30 @@ -/* -* 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. -*/ +// MIT License -#include -#include -#include -#include -#include +// 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. int32 b2_gjkCalls, b2_gjkIters, b2_gjkMaxIters; @@ -82,13 +86,19 @@ void b2DistanceProxy::Set(const b2Shape* shape, int32 index) } } +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 - float32 a; // barycentric coordinate for closest point + float a; // barycentric coordinate for closest point int32 indexA; // wA index int32 indexB; // wB index }; @@ -121,8 +131,8 @@ struct b2Simplex // old metric then flush the simplex. if (m_count > 1) { - float32 metric1 = cache->metric; - float32 metric2 = GetMetric(); + float metric1 = cache->metric; + float metric2 = GetMetric(); if (metric2 < 0.5f * metric1 || 2.0f * metric1 < metric2 || metric2 < b2_epsilon) { // Reset the simplex. @@ -168,7 +178,7 @@ struct b2Simplex case 2: { b2Vec2 e12 = m_v2.w - m_v1.w; - float32 sgn = b2Cross(e12, -m_v1.w); + float sgn = b2Cross(e12, -m_v1.w); if (sgn > 0.0f) { // Origin is left of e12. @@ -239,7 +249,7 @@ struct b2Simplex } } - float32 GetMetric() const + float GetMetric() const { switch (m_count) { @@ -300,7 +310,7 @@ void b2Simplex::Solve2() b2Vec2 e12 = w2 - w1; // w1 region - float32 d12_2 = -b2Dot(w1, e12); + float d12_2 = -b2Dot(w1, e12); if (d12_2 <= 0.0f) { // a2 <= 0, so we clamp it to 0 @@ -310,7 +320,7 @@ void b2Simplex::Solve2() } // w2 region - float32 d12_1 = b2Dot(w2, e12); + float d12_1 = b2Dot(w2, e12); if (d12_1 <= 0.0f) { // a1 <= 0, so we clamp it to 0 @@ -321,7 +331,7 @@ void b2Simplex::Solve2() } // Must be in e12 region. - float32 inv_d12 = 1.0f / (d12_1 + d12_2); + 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; @@ -343,37 +353,37 @@ void b2Simplex::Solve3() // [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; + 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; - float32 w1e13 = b2Dot(w1, e13); - float32 w3e13 = b2Dot(w3, e13); - float32 d13_1 = w3e13; - float32 d13_2 = -w1e13; + 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; - float32 w2e23 = b2Dot(w2, e23); - float32 w3e23 = b2Dot(w3, e23); - float32 d23_1 = w3e23; - float32 d23_2 = -w2e23; + float w2e23 = b2Dot(w2, e23); + float w3e23 = b2Dot(w3, e23); + float d23_1 = w3e23; + float d23_2 = -w2e23; // Triangle123 - float32 n123 = b2Cross(e12, e13); + float 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); + 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) @@ -386,7 +396,7 @@ void b2Simplex::Solve3() // e12 if (d12_1 > 0.0f && d12_2 > 0.0f && d123_3 <= 0.0f) { - float32 inv_d12 = 1.0f / (d12_1 + d12_2); + 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; @@ -396,7 +406,7 @@ void b2Simplex::Solve3() // e13 if (d13_1 > 0.0f && d13_2 > 0.0f && d123_2 <= 0.0f) { - float32 inv_d13 = 1.0f / (d13_1 + d13_2); + 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; @@ -425,7 +435,7 @@ void b2Simplex::Solve3() // e23 if (d23_1 > 0.0f && d23_2 > 0.0f && d123_1 <= 0.0f) { - float32 inv_d23 = 1.0f / (d23_1 + d23_2); + 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; @@ -434,7 +444,7 @@ void b2Simplex::Solve3() } // Must be in triangle123 - float32 inv_d123 = 1.0f / (d123_1 + d123_2 + d123_3); + 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; @@ -466,9 +476,6 @@ void b2Distance(b2DistanceOutput* output, 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) @@ -504,17 +511,6 @@ void b2Distance(b2DistanceOutput* output, 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(); @@ -534,7 +530,6 @@ void b2Distance(b2DistanceOutput* output, 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; @@ -577,8 +572,8 @@ void b2Distance(b2DistanceOutput* output, // Apply radii if requested. if (input->useRadii) { - float32 rA = proxyA->m_radius; - float32 rB = proxyB->m_radius; + float rA = proxyA->m_radius; + float rB = proxyB->m_radius; if (output->distance > rA + rB && output->distance > b2_epsilon) { @@ -601,3 +596,151 @@ void b2Distance(b2DistanceOutput* output, } } } + +// 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/src/libraries/Box2D/Collision/b2DynamicTree.cpp b/src/libraries/Box2D/Collision/b2_dynamic_tree.cpp similarity index 83% rename from src/libraries/Box2D/Collision/b2DynamicTree.cpp rename to src/libraries/Box2D/Collision/b2_dynamic_tree.cpp index ef57b4220..45e093320 100644 --- a/src/libraries/Box2D/Collision/b2DynamicTree.cpp +++ b/src/libraries/Box2D/Collision/b2_dynamic_tree.cpp @@ -1,22 +1,25 @@ -/* -* 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. -*/ +// MIT License -#include +// 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() @@ -83,7 +86,8 @@ int32 b2DynamicTree::AllocateNode() m_nodes[nodeId].child1 = b2_nullNode; m_nodes[nodeId].child2 = b2_nullNode; m_nodes[nodeId].height = 0; - m_nodes[nodeId].userData = NULL; + m_nodes[nodeId].userData = nullptr; + m_nodes[nodeId].moved = false; ++m_nodeCount; return nodeId; } @@ -112,6 +116,7 @@ int32 b2DynamicTree::CreateProxy(const b2AABB& aabb, void* userData) 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); @@ -133,43 +138,61 @@ bool b2DynamicTree::MoveProxy(int32 proxyId, const b2AABB& aabb, const b2Vec2& d b2Assert(m_nodes[proxyId].IsLeaf()); - if (m_nodes[proxyId].aabb.Contains(aabb)) - { - return false; - } - - RemoveLeaf(proxyId); - - // Extend AABB. - b2AABB b = aabb; + // Extend AABB + b2AABB fatAABB; b2Vec2 r(b2_aabbExtension, b2_aabbExtension); - b.lowerBound = b.lowerBound - r; - b.upperBound = b.upperBound + r; + fatAABB.lowerBound = aabb.lowerBound - r; + fatAABB.upperBound = aabb.upperBound + r; - // Predict AABB displacement. + // Predict AABB movement b2Vec2 d = b2_aabbMultiplier * displacement; if (d.x < 0.0f) { - b.lowerBound.x += d.x; + fatAABB.lowerBound.x += d.x; } else { - b.upperBound.x += d.x; + fatAABB.upperBound.x += d.x; } if (d.y < 0.0f) { - b.lowerBound.y += d.y; + fatAABB.lowerBound.y += d.y; } else { - b.upperBound.y += d.y; + fatAABB.upperBound.y += d.y; } - m_nodes[proxyId].aabb = b; + 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; } @@ -192,20 +215,20 @@ void b2DynamicTree::InsertLeaf(int32 leaf) int32 child1 = m_nodes[index].child1; int32 child2 = m_nodes[index].child2; - float32 area = m_nodes[index].aabb.GetPerimeter(); + float area = m_nodes[index].aabb.GetPerimeter(); b2AABB combinedAABB; combinedAABB.Combine(m_nodes[index].aabb, leafAABB); - float32 combinedArea = combinedAABB.GetPerimeter(); + float combinedArea = combinedAABB.GetPerimeter(); // Cost of creating a new parent for this node and the new leaf - float32 cost = 2.0f * combinedArea; + float cost = 2.0f * combinedArea; // Minimum cost of pushing the leaf further down the tree - float32 inheritanceCost = 2.0f * (combinedArea - area); + float inheritanceCost = 2.0f * (combinedArea - area); // Cost of descending into child1 - float32 cost1; + float cost1; if (m_nodes[child1].IsLeaf()) { b2AABB aabb; @@ -216,13 +239,13 @@ void b2DynamicTree::InsertLeaf(int32 leaf) { b2AABB aabb; aabb.Combine(leafAABB, m_nodes[child1].aabb); - float32 oldArea = m_nodes[child1].aabb.GetPerimeter(); - float32 newArea = aabb.GetPerimeter(); + float oldArea = m_nodes[child1].aabb.GetPerimeter(); + float newArea = aabb.GetPerimeter(); cost1 = (newArea - oldArea) + inheritanceCost; } // Cost of descending into child2 - float32 cost2; + float cost2; if (m_nodes[child2].IsLeaf()) { b2AABB aabb; @@ -233,8 +256,8 @@ void b2DynamicTree::InsertLeaf(int32 leaf) { b2AABB aabb; aabb.Combine(leafAABB, m_nodes[child2].aabb); - float32 oldArea = m_nodes[child2].aabb.GetPerimeter(); - float32 newArea = aabb.GetPerimeter(); + float oldArea = m_nodes[child2].aabb.GetPerimeter(); + float newArea = aabb.GetPerimeter(); cost2 = newArea - oldArea + inheritanceCost; } @@ -261,7 +284,7 @@ void b2DynamicTree::InsertLeaf(int32 leaf) int32 oldParent = m_nodes[sibling].parent; int32 newParent = AllocateNode(); m_nodes[newParent].parent = oldParent; - m_nodes[newParent].userData = NULL; + m_nodes[newParent].userData = nullptr; m_nodes[newParent].aabb.Combine(leafAABB, m_nodes[sibling].aabb); m_nodes[newParent].height = m_nodes[sibling].height + 1; @@ -528,7 +551,7 @@ int32 b2DynamicTree::GetHeight() const } // -float32 b2DynamicTree::GetAreaRatio() const +float b2DynamicTree::GetAreaRatio() const { if (m_root == b2_nullNode) { @@ -536,9 +559,9 @@ float32 b2DynamicTree::GetAreaRatio() const } const b2TreeNode* root = m_nodes + m_root; - float32 rootArea = root->aabb.GetPerimeter(); + float rootArea = root->aabb.GetPerimeter(); - float32 totalArea = 0.0f; + float totalArea = 0.0f; for (int32 i = 0; i < m_nodeCapacity; ++i) { const b2TreeNode* node = m_nodes + i; @@ -652,6 +675,7 @@ void b2DynamicTree::ValidateMetrics(int32 index) const void b2DynamicTree::Validate() const { +#if defined(b2DEBUG) ValidateStructure(m_root); ValidateMetrics(m_root); @@ -667,6 +691,7 @@ void b2DynamicTree::Validate() const b2Assert(GetHeight() == ComputeHeight()); b2Assert(m_nodeCount + freeCount == m_nodeCapacity); +#endif } int32 b2DynamicTree::GetMaxBalance() const @@ -719,7 +744,7 @@ void b2DynamicTree::RebuildBottomUp() while (count > 1) { - float32 minCost = b2_maxFloat; + float minCost = b2_maxFloat; int32 iMin = -1, jMin = -1; for (int32 i = 0; i < count; ++i) { @@ -730,7 +755,7 @@ void b2DynamicTree::RebuildBottomUp() b2AABB aabbj = m_nodes[nodes[j]].aabb; b2AABB b; b.Combine(aabbi, aabbj); - float32 cost = b.GetPerimeter(); + float cost = b.GetPerimeter(); if (cost < minCost) { iMin = i; diff --git a/src/libraries/Box2D/Collision/Shapes/b2EdgeShape.cpp b/src/libraries/Box2D/Collision/b2_edge_shape.cpp similarity index 52% rename from src/libraries/Box2D/Collision/Shapes/b2EdgeShape.cpp rename to src/libraries/Box2D/Collision/b2_edge_shape.cpp index 8bcd12f7a..65f0606a6 100644 --- a/src/libraries/Box2D/Collision/Shapes/b2EdgeShape.cpp +++ b/src/libraries/Box2D/Collision/b2_edge_shape.cpp @@ -1,30 +1,43 @@ -/* -* 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. -*/ +// MIT License -#include +// 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::Set(const b2Vec2& v1, const b2Vec2& v2) +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_hasVertex0 = false; - m_hasVertex3 = false; + m_oneSided = false; } b2Shape* b2EdgeShape::Clone(b2BlockAllocator* allocator) const @@ -64,21 +77,28 @@ bool b2EdgeShape::RayCast(b2RayCastOutput* output, const b2RayCastInput& input, 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 - float32 numerator = b2Dot(normal, v1 - p1); - float32 denominator = b2Dot(normal, d); + 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; } - float32 t = numerator / denominator; + float t = numerator / denominator; if (t < 0.0f || input.maxFraction < t) { return false; @@ -89,13 +109,13 @@ bool b2EdgeShape::RayCast(b2RayCastOutput* output, const b2RayCastInput& input, // q = v1 + s * r // s = dot(q - v1, r) / dot(r, r) b2Vec2 r = v2 - v1; - float32 rr = b2Dot(r, r); + float rr = b2Dot(r, r); if (rr == 0.0f) { return false; } - float32 s = b2Dot(q - v1, r) / rr; + float s = b2Dot(q - v1, r) / rr; if (s < 0.0f || 1.0f < s) { return false; @@ -128,7 +148,7 @@ void b2EdgeShape::ComputeAABB(b2AABB* aabb, const b2Transform& xf, int32 childIn aabb->upperBound = upper + r; } -void b2EdgeShape::ComputeMass(b2MassData* massData, float32 density) const +void b2EdgeShape::ComputeMass(b2MassData* massData, float density) const { B2_NOT_USED(density); diff --git a/src/libraries/Box2D/Collision/Shapes/b2PolygonShape.cpp b/src/libraries/Box2D/Collision/b2_polygon_shape.cpp similarity index 74% rename from src/libraries/Box2D/Collision/Shapes/b2PolygonShape.cpp rename to src/libraries/Box2D/Collision/b2_polygon_shape.cpp index 26ba7b3a7..368fb801f 100644 --- a/src/libraries/Box2D/Collision/Shapes/b2PolygonShape.cpp +++ b/src/libraries/Box2D/Collision/b2_polygon_shape.cpp @@ -1,22 +1,28 @@ -/* -* 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. -*/ +// 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 #include b2Shape* b2PolygonShape::Clone(b2BlockAllocator* allocator) const @@ -27,7 +33,7 @@ b2Shape* b2PolygonShape::Clone(b2BlockAllocator* allocator) const return clone; } -void b2PolygonShape::SetAsBox(float32 hx, float32 hy) +void b2PolygonShape::SetAsBox(float hx, float hy) { m_count = 4; m_vertices[0].Set(-hx, -hy); @@ -41,7 +47,7 @@ void b2PolygonShape::SetAsBox(float32 hx, float32 hy) m_centroid.SetZero(); } -void b2PolygonShape::SetAsBox(float32 hx, float32 hy, const b2Vec2& center, float32 angle) +void b2PolygonShape::SetAsBox(float hx, float hy, const b2Vec2& center, float angle) { m_count = 4; m_vertices[0].Set(-hx, -hy); @@ -75,36 +81,28 @@ static b2Vec2 ComputeCentroid(const b2Vec2* vs, int32 count) { b2Assert(count >= 3); - b2Vec2 c; c.Set(0.0f, 0.0f); - float32 area = 0.0f; + b2Vec2 c(0.0f, 0.0f); + float 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 + // Get a reference point for forming triangles. + // Use the first vertex to reduce round-off errors. + b2Vec2 s = vs[0]; - const float32 inv3 = 1.0f / 3.0f; + const float 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 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; - float32 D = b2Cross(e1, e2); + float D = b2Cross(e1, e2); - float32 triangleArea = 0.5f * D; + float triangleArea = 0.5f * D; area += triangleArea; // Area weighted centroid @@ -113,7 +111,7 @@ static b2Vec2 ComputeCentroid(const b2Vec2* vs, int32 count) // Centroid b2Assert(area > b2_epsilon); - c *= 1.0f / area; + c = (1.0f / area) * c + s; return c; } @@ -165,10 +163,10 @@ void b2PolygonShape::Set(const b2Vec2* vertices, int32 count) // Find the right most point on the hull int32 i0 = 0; - float32 x0 = ps[0].x; + float x0 = ps[0].x; for (int32 i = 1; i < n; ++i) { - float32 x = ps[i].x; + float x = ps[i].x; if (x > x0 || (x == x0 && ps[i].y < ps[i0].y)) { i0 = i; @@ -182,6 +180,7 @@ void b2PolygonShape::Set(const b2Vec2* vertices, int32 count) for (;;) { + b2Assert(m < b2_maxPolygonVertices); hull[m] = ih; int32 ie = 0; @@ -195,7 +194,7 @@ void b2PolygonShape::Set(const b2Vec2* vertices, int32 count) b2Vec2 r = ps[ie] - ps[hull[m]]; b2Vec2 v = ps[j] - ps[hull[m]]; - float32 c = b2Cross(r, v); + float c = b2Cross(r, v); if (c < 0.0f) { ie = j; @@ -254,7 +253,7 @@ bool b2PolygonShape::TestPoint(const b2Transform& xf, const b2Vec2& p) const for (int32 i = 0; i < m_count; ++i) { - float32 dot = b2Dot(m_normals[i], pLocal - m_vertices[i]); + float dot = b2Dot(m_normals[i], pLocal - m_vertices[i]); if (dot > 0.0f) { return false; @@ -274,7 +273,7 @@ bool b2PolygonShape::RayCast(b2RayCastOutput* output, const b2RayCastInput& inpu b2Vec2 p2 = b2MulT(xf.q, input.p2 - xf.p); b2Vec2 d = p2 - p1; - float32 lower = 0.0f, upper = input.maxFraction; + float lower = 0.0f, upper = input.maxFraction; int32 index = -1; @@ -283,8 +282,8 @@ bool b2PolygonShape::RayCast(b2RayCastOutput* output, const b2RayCastInput& inpu // 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); + float numerator = b2Dot(m_normals[i], m_vertices[i] - p1); + float denominator = b2Dot(m_normals[i], d); if (denominator == 0.0f) { @@ -355,7 +354,7 @@ void b2PolygonShape::ComputeAABB(b2AABB* aabb, const b2Transform& xf, int32 chil aabb->upperBound = upper + r; } -void b2PolygonShape::ComputeMass(b2MassData* massData, float32 density) const +void b2PolygonShape::ComputeMass(b2MassData* massData, float density) const { // Polygon mass, centroid, and inertia. // Let rho be the polygon density in mass per unit area. @@ -383,22 +382,15 @@ void b2PolygonShape::ComputeMass(b2MassData* massData, float32 density) const b2Assert(m_count >= 3); - b2Vec2 center; center.Set(0.0f, 0.0f); - float32 area = 0.0f; - float32 I = 0.0f; + b2Vec2 center(0.0f, 0.0f); + float area = 0.0f; + float 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); + // Get a reference point for forming triangles. + // Use the first vertex to reduce round-off errors. + b2Vec2 s = m_vertices[0]; - // 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; + const float k_inv3 = 1.0f / 3.0f; for (int32 i = 0; i < m_count; ++i) { @@ -406,19 +398,19 @@ void b2PolygonShape::ComputeMass(b2MassData* massData, float32 density) const 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); + float D = b2Cross(e1, e2); - float32 triangleArea = 0.5f * D; + float 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; + float ex1 = e1.x, ey1 = e1.y; + float ex2 = e2.x, ey2 = e2.y; - float32 intx2 = ex1*ex1 + ex2*ex1 + ex2*ex2; - float32 inty2 = ey1*ey1 + ey2*ey1 + ey2*ey2; + float intx2 = ex1*ex1 + ex2*ex1 + ex2*ex2; + float inty2 = ey1*ey1 + ey2*ey1 + ey2*ey2; I += (0.25f * k_inv3 * D) * (intx2 + inty2); } @@ -455,7 +447,7 @@ bool b2PolygonShape::Validate() const } b2Vec2 v = m_vertices[j] - p; - float32 c = b2Cross(e, v); + float c = b2Cross(e, v); if (c < 0.0f) { return false; diff --git a/src/libraries/Box2D/Collision/b2TimeOfImpact.cpp b/src/libraries/Box2D/Collision/b2_time_of_impact.cpp similarity index 78% rename from src/libraries/Box2D/Collision/b2TimeOfImpact.cpp rename to src/libraries/Box2D/Collision/b2_time_of_impact.cpp index 60da95c38..ca61a31f7 100644 --- a/src/libraries/Box2D/Collision/b2TimeOfImpact.cpp +++ b/src/libraries/Box2D/Collision/b2_time_of_impact.cpp @@ -1,31 +1,35 @@ -/* -* 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. -*/ +// MIT License -#include -#include -#include -#include -#include -#include +// 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 -float32 b2_toiTime, b2_toiMaxTime; +float b2_toiTime, b2_toiMaxTime; int32 b2_toiCalls, b2_toiIters, b2_toiMaxIters; int32 b2_toiRootIters, b2_toiMaxRootIters; @@ -41,10 +45,10 @@ struct b2SeparationFunction // TODO_ERIN might not need to return the separation - float32 Initialize(const b2SimplexCache* cache, + float Initialize(const b2SimplexCache* cache, const b2DistanceProxy* proxyA, const b2Sweep& sweepA, const b2DistanceProxy* proxyB, const b2Sweep& sweepB, - float32 t1) + float t1) { m_proxyA = proxyA; m_proxyB = proxyB; @@ -66,7 +70,7 @@ struct b2SeparationFunction b2Vec2 pointA = b2Mul(xfA, localPointA); b2Vec2 pointB = b2Mul(xfB, localPointB); m_axis = pointB - pointA; - float32 s = m_axis.Normalize(); + float s = m_axis.Normalize(); return s; } else if (cache->indexA[0] == cache->indexA[1]) @@ -86,7 +90,7 @@ struct b2SeparationFunction b2Vec2 localPointA = proxyA->GetVertex(cache->indexA[0]); b2Vec2 pointA = b2Mul(xfA, localPointA); - float32 s = b2Dot(pointA - pointB, normal); + float s = b2Dot(pointA - pointB, normal); if (s < 0.0f) { m_axis = -m_axis; @@ -111,7 +115,7 @@ struct b2SeparationFunction b2Vec2 localPointB = m_proxyB->GetVertex(cache->indexB[0]); b2Vec2 pointB = b2Mul(xfB, localPointB); - float32 s = b2Dot(pointB - pointA, normal); + float s = b2Dot(pointB - pointA, normal); if (s < 0.0f) { m_axis = -m_axis; @@ -122,7 +126,7 @@ struct b2SeparationFunction } // - float32 FindMinSeparation(int32* indexA, int32* indexB, float32 t) const + float FindMinSeparation(int32* indexA, int32* indexB, float t) const { b2Transform xfA, xfB; m_sweepA.GetTransform(&xfA, t); @@ -144,7 +148,7 @@ struct b2SeparationFunction b2Vec2 pointA = b2Mul(xfA, localPointA); b2Vec2 pointB = b2Mul(xfB, localPointB); - float32 separation = b2Dot(pointB - pointA, m_axis); + float separation = b2Dot(pointB - pointA, m_axis); return separation; } @@ -161,7 +165,7 @@ struct b2SeparationFunction b2Vec2 localPointB = m_proxyB->GetVertex(*indexB); b2Vec2 pointB = b2Mul(xfB, localPointB); - float32 separation = b2Dot(pointB - pointA, normal); + float separation = b2Dot(pointB - pointA, normal); return separation; } @@ -178,7 +182,7 @@ struct b2SeparationFunction b2Vec2 localPointA = m_proxyA->GetVertex(*indexA); b2Vec2 pointA = b2Mul(xfA, localPointA); - float32 separation = b2Dot(pointA - pointB, normal); + float separation = b2Dot(pointA - pointB, normal); return separation; } @@ -191,7 +195,7 @@ struct b2SeparationFunction } // - float32 Evaluate(int32 indexA, int32 indexB, float32 t) const + float Evaluate(int32 indexA, int32 indexB, float t) const { b2Transform xfA, xfB; m_sweepA.GetTransform(&xfA, t); @@ -206,7 +210,7 @@ struct b2SeparationFunction b2Vec2 pointA = b2Mul(xfA, localPointA); b2Vec2 pointB = b2Mul(xfB, localPointB); - float32 separation = b2Dot(pointB - pointA, m_axis); + float separation = b2Dot(pointB - pointA, m_axis); return separation; } @@ -219,7 +223,7 @@ struct b2SeparationFunction b2Vec2 localPointB = m_proxyB->GetVertex(indexB); b2Vec2 pointB = b2Mul(xfB, localPointB); - float32 separation = b2Dot(pointB - pointA, normal); + float separation = b2Dot(pointB - pointA, normal); return separation; } @@ -231,7 +235,7 @@ struct b2SeparationFunction b2Vec2 localPointA = m_proxyA->GetVertex(indexA); b2Vec2 pointA = b2Mul(xfA, localPointA); - float32 separation = b2Dot(pointA - pointB, normal); + float separation = b2Dot(pointA - pointB, normal); return separation; } @@ -271,14 +275,14 @@ void b2TimeOfImpact(b2TOIOutput* output, const b2TOIInput* input) sweepA.Normalize(); sweepB.Normalize(); - float32 tMax = input->tMax; + float 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; + 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); - float32 t1 = 0.0f; + float t1 = 0.0f; const int32 k_maxIterations = 20; // TODO_ERIN b2Settings int32 iter = 0; @@ -329,17 +333,17 @@ void b2TimeOfImpact(b2TOIOutput* output, const b2TOIInput* input) // 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]; + float dx = 1.0f / N; + float xs[N+1]; + float fs[N+1]; - float32 x = 0.0f; + float 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; + float f = fcn.Evaluate(xfA, xfB) - target; printf("%g %g\n", x, f); @@ -354,13 +358,13 @@ void b2TimeOfImpact(b2TOIOutput* output, const b2TOIInput* input) // 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; + float 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); + float s2 = fcn.FindMinSeparation(&indexA, &indexB, t2); // Is the final configuration separated? if (s2 > target + tolerance) @@ -381,7 +385,7 @@ void b2TimeOfImpact(b2TOIOutput* output, const b2TOIInput* input) } // Compute the initial separation of the witness points. - float32 s1 = fcn.Evaluate(indexA, indexB, t1); + float s1 = fcn.Evaluate(indexA, indexB, t1); // Check for initial overlap. This might happen if the root finder // runs out of iterations. @@ -405,11 +409,11 @@ void b2TimeOfImpact(b2TOIOutput* output, const b2TOIInput* input) // Compute 1D root of: f(x) - target = 0 int32 rootIterCount = 0; - float32 a1 = t1, a2 = t2; + float a1 = t1, a2 = t2; for (;;) { // Use a mix of the secant rule and bisection. - float32 t; + float t; if (rootIterCount & 1) { // Secant rule to improve convergence. @@ -424,7 +428,7 @@ void b2TimeOfImpact(b2TOIOutput* output, const b2TOIInput* input) ++rootIterCount; ++b2_toiRootIters; - float32 s = fcn.Evaluate(indexA, indexB, t); + float s = fcn.Evaluate(indexA, indexB, t); if (b2Abs(s - target) < tolerance) { @@ -480,7 +484,7 @@ void b2TimeOfImpact(b2TOIOutput* output, const b2TOIInput* input) b2_toiMaxIters = b2Max(b2_toiMaxIters, iter); - float32 time = timer.GetMilliseconds(); + float time = timer.GetMilliseconds(); b2_toiMaxTime = b2Max(b2_toiMaxTime, time); b2_toiTime += time; } diff --git a/src/libraries/Box2D/Common/b2BlockAllocator.h b/src/libraries/Box2D/Common/b2BlockAllocator.h deleted file mode 100644 index cea3cef96..000000000 --- a/src/libraries/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/src/libraries/Box2D/Common/b2Draw.cpp b/src/libraries/Box2D/Common/b2Draw.cpp deleted file mode 100644 index 2cd9b1a56..000000000 --- a/src/libraries/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/src/libraries/Box2D/Common/b2Draw.h b/src/libraries/Box2D/Common/b2Draw.h deleted file mode 100644 index be65fb0ff..000000000 --- a/src/libraries/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/src/libraries/Box2D/Common/b2GrowableStack.h b/src/libraries/Box2D/Common/b2GrowableStack.h deleted file mode 100644 index 989247710..000000000 --- a/src/libraries/Box2D/Common/b2GrowableStack.h +++ /dev/null @@ -1,85 +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 - -/// 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/src/libraries/Box2D/Common/b2Math.cpp b/src/libraries/Box2D/Common/b2Math.cpp deleted file mode 100644 index d6027be90..000000000 --- a/src/libraries/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/src/libraries/Box2D/Common/b2Settings.cpp b/src/libraries/Box2D/Common/b2Settings.cpp deleted file mode 100644 index ed6d83e0e..000000000 --- a/src/libraries/Box2D/Common/b2Settings.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 "common/Exception.h" - -b2Version b2_version = {2, 3, 2}; - -// 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); -} - -void loveAssert(bool test, const char *teststr) -{ - if (!test) - throw love::Exception("Box2D assertion failed: %s", teststr); -} diff --git a/src/libraries/Box2D/Common/b2StackAllocator.cpp b/src/libraries/Box2D/Common/b2StackAllocator.cpp deleted file mode 100644 index 321cabb23..000000000 --- a/src/libraries/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/src/libraries/Box2D/Common/b2StackAllocator.h b/src/libraries/Box2D/Common/b2StackAllocator.h deleted file mode 100644 index bb87a7edc..000000000 --- a/src/libraries/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/src/libraries/Box2D/Common/b2Timer.cpp b/src/libraries/Box2D/Common/b2Timer.cpp deleted file mode 100644 index cbea5309e..000000000 --- a/src/libraries/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/src/libraries/Box2D/Common/b2Timer.h b/src/libraries/Box2D/Common/b2Timer.h deleted file mode 100644 index edf1ca459..000000000 --- a/src/libraries/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/src/libraries/Box2D/Common/b2BlockAllocator.cpp b/src/libraries/Box2D/Common/b2_block_allocator.cpp similarity index 59% rename from src/libraries/Box2D/Common/b2BlockAllocator.cpp rename to src/libraries/Box2D/Common/b2_block_allocator.cpp index f16db7028..595f2ad39 100644 --- a/src/libraries/Box2D/Common/b2BlockAllocator.cpp +++ b/src/libraries/Box2D/Common/b2_block_allocator.cpp @@ -1,28 +1,36 @@ -/* -* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ +// MIT License -#include +// 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 -#include -int32 b2BlockAllocator::s_blockSizes[b2_blockSizes] = +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 @@ -39,8 +47,33 @@ int32 b2BlockAllocator::s_blockSizes[b2_blockSizes] = 512, // 12 640, // 13 }; -uint8 b2BlockAllocator::s_blockSizeLookup[b2_maxBlockSize + 1]; -bool b2BlockAllocator::s_blockSizeLookupInitialized; + +// 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 { @@ -55,7 +88,7 @@ struct b2Block b2BlockAllocator::b2BlockAllocator() { - b2Assert(b2_blockSizes < UCHAR_MAX); + b2Assert(b2_blockSizeCount < UCHAR_MAX); m_chunkSpace = b2_chunkArrayIncrement; m_chunkCount = 0; @@ -63,26 +96,6 @@ b2BlockAllocator::b2BlockAllocator() 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() @@ -98,7 +111,9 @@ b2BlockAllocator::~b2BlockAllocator() void* b2BlockAllocator::Allocate(int32 size) { if (size == 0) - return NULL; + { + return nullptr; + } b2Assert(0 < size); @@ -107,8 +122,8 @@ void* b2BlockAllocator::Allocate(int32 size) return b2Alloc(size); } - int32 index = s_blockSizeLookup[size]; - b2Assert(0 <= index && index < b2_blockSizes); + int32 index = b2_sizeMap.values[size]; + b2Assert(0 <= index && index < b2_blockSizeCount); if (m_freeLists[index]) { @@ -133,7 +148,7 @@ void* b2BlockAllocator::Allocate(int32 size) #if defined(_DEBUG) memset(chunk->blocks, 0xcd, b2_chunkSize); #endif - int32 blockSize = s_blockSizes[index]; + int32 blockSize = b2_blockSizes[index]; chunk->blockSize = blockSize; int32 blockCount = b2_chunkSize / blockSize; b2Assert(blockCount * blockSize <= b2_chunkSize); @@ -144,7 +159,7 @@ void* b2BlockAllocator::Allocate(int32 size) block->next = next; } b2Block* last = (b2Block*)((int8*)chunk->blocks + blockSize * (blockCount - 1)); - last->next = NULL; + last->next = nullptr; m_freeLists[index] = chunk->blocks->next; ++m_chunkCount; @@ -168,12 +183,12 @@ void b2BlockAllocator::Free(void* p, int32 size) return; } - int32 index = s_blockSizeLookup[size]; - b2Assert(0 <= index && index < b2_blockSizes); + int32 index = b2_sizeMap.values[size]; + b2Assert(0 <= index && index < b2_blockSizeCount); -#ifdef _DEBUG +#if defined(_DEBUG) // Verify the memory address and size is valid. - int32 blockSize = s_blockSizes[index]; + int32 blockSize = b2_blockSizes[index]; bool found = false; for (int32 i = 0; i < m_chunkCount; ++i) { @@ -211,6 +226,5 @@ void b2BlockAllocator::Clear() m_chunkCount = 0; memset(m_chunks, 0, m_chunkSpace * sizeof(b2Chunk)); - memset(m_freeLists, 0, sizeof(m_freeLists)); } diff --git a/src/libraries/Box2D/Common/b2_draw.cpp b/src/libraries/Box2D/Common/b2_draw.cpp new file mode 100644 index 000000000..1ec11e54a --- /dev/null +++ b/src/libraries/Box2D/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/src/libraries/Box2D/Common/b2_math.cpp b/src/libraries/Box2D/Common/b2_math.cpp new file mode 100644 index 000000000..a14460cd5 --- /dev/null +++ b/src/libraries/Box2D/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/src/libraries/Box2D/Common/b2_settings.cpp b/src/libraries/Box2D/Common/b2_settings.cpp new file mode 100644 index 000000000..11436f7de --- /dev/null +++ b/src/libraries/Box2D/Common/b2_settings.cpp @@ -0,0 +1,77 @@ +// 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, 0}; + +// Memory allocators. Modify these to use your own allocator. +void* b2Alloc(int32 size) +{ + return malloc(size); +} + +void b2Free(void* mem) +{ + free(mem); +} + +// You can modify this to use your logging facility. +void b2Log(const char* string, ...) +{ + va_list args; + va_start(args, string); + vprintf(string, args); + va_end(args); +} + +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/src/libraries/Box2D/Common/b2_stack_allocator.cpp b/src/libraries/Box2D/Common/b2_stack_allocator.cpp new file mode 100644 index 000000000..602db1a00 --- /dev/null +++ b/src/libraries/Box2D/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/src/libraries/Box2D/Common/b2_timer.cpp b/src/libraries/Box2D/Common/b2_timer.cpp new file mode 100644 index 000000000..dd7cde7f4 --- /dev/null +++ b/src/libraries/Box2D/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/src/libraries/Box2D/Dynamics/Contacts/b2ChainAndCircleContact.cpp b/src/libraries/Box2D/Dynamics/Contacts/b2ChainAndCircleContact.cpp deleted file mode 100644 index 01c921f3d..000000000 --- a/src/libraries/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/src/libraries/Box2D/Dynamics/Contacts/b2ChainAndCircleContact.h b/src/libraries/Box2D/Dynamics/Contacts/b2ChainAndCircleContact.h deleted file mode 100644 index b93c30edc..000000000 --- a/src/libraries/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/src/libraries/Box2D/Dynamics/Contacts/b2ChainAndPolygonContact.cpp b/src/libraries/Box2D/Dynamics/Contacts/b2ChainAndPolygonContact.cpp deleted file mode 100644 index 4642fa46a..000000000 --- a/src/libraries/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/src/libraries/Box2D/Dynamics/Contacts/b2ChainAndPolygonContact.h b/src/libraries/Box2D/Dynamics/Contacts/b2ChainAndPolygonContact.h deleted file mode 100644 index 0c09af68a..000000000 --- a/src/libraries/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/src/libraries/Box2D/Dynamics/Contacts/b2CircleContact.cpp b/src/libraries/Box2D/Dynamics/Contacts/b2CircleContact.cpp deleted file mode 100644 index b47f35aed..000000000 --- a/src/libraries/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/src/libraries/Box2D/Dynamics/Contacts/b2CircleContact.h b/src/libraries/Box2D/Dynamics/Contacts/b2CircleContact.h deleted file mode 100644 index 079d3abcd..000000000 --- a/src/libraries/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/src/libraries/Box2D/Dynamics/Contacts/b2ContactSolver.h b/src/libraries/Box2D/Dynamics/Contacts/b2ContactSolver.h deleted file mode 100644 index a622e30d8..000000000 --- a/src/libraries/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/src/libraries/Box2D/Dynamics/Contacts/b2EdgeAndCircleContact.cpp b/src/libraries/Box2D/Dynamics/Contacts/b2EdgeAndCircleContact.cpp deleted file mode 100644 index d1fd3295c..000000000 --- a/src/libraries/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/src/libraries/Box2D/Dynamics/Contacts/b2EdgeAndCircleContact.h b/src/libraries/Box2D/Dynamics/Contacts/b2EdgeAndCircleContact.h deleted file mode 100644 index e7a34553c..000000000 --- a/src/libraries/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/src/libraries/Box2D/Dynamics/Contacts/b2EdgeAndPolygonContact.cpp b/src/libraries/Box2D/Dynamics/Contacts/b2EdgeAndPolygonContact.cpp deleted file mode 100644 index ba536bc39..000000000 --- a/src/libraries/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/src/libraries/Box2D/Dynamics/Contacts/b2EdgeAndPolygonContact.h b/src/libraries/Box2D/Dynamics/Contacts/b2EdgeAndPolygonContact.h deleted file mode 100644 index 3b40f1216..000000000 --- a/src/libraries/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/src/libraries/Box2D/Dynamics/Contacts/b2PolygonAndCircleContact.cpp b/src/libraries/Box2D/Dynamics/Contacts/b2PolygonAndCircleContact.cpp deleted file mode 100644 index 7c9cbcc92..000000000 --- a/src/libraries/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/src/libraries/Box2D/Dynamics/Contacts/b2PolygonAndCircleContact.h b/src/libraries/Box2D/Dynamics/Contacts/b2PolygonAndCircleContact.h deleted file mode 100644 index fe0645e08..000000000 --- a/src/libraries/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/src/libraries/Box2D/Dynamics/Contacts/b2PolygonContact.cpp b/src/libraries/Box2D/Dynamics/Contacts/b2PolygonContact.cpp deleted file mode 100644 index b5706ca10..000000000 --- a/src/libraries/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/src/libraries/Box2D/Dynamics/Contacts/b2PolygonContact.h b/src/libraries/Box2D/Dynamics/Contacts/b2PolygonContact.h deleted file mode 100644 index 434e6c926..000000000 --- a/src/libraries/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/src/libraries/Box2D/Dynamics/Joints/b2DistanceJoint.h b/src/libraries/Box2D/Dynamics/Joints/b2DistanceJoint.h deleted file mode 100644 index 2faba9d47..000000000 --- a/src/libraries/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/src/libraries/Box2D/Dynamics/Joints/b2FrictionJoint.h b/src/libraries/Box2D/Dynamics/Joints/b2FrictionJoint.h deleted file mode 100644 index d57abe80d..000000000 --- a/src/libraries/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/src/libraries/Box2D/Dynamics/Joints/b2GearJoint.h b/src/libraries/Box2D/Dynamics/Joints/b2GearJoint.h deleted file mode 100644 index fd3d7b2fa..000000000 --- a/src/libraries/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/src/libraries/Box2D/Dynamics/Joints/b2Joint.cpp b/src/libraries/Box2D/Dynamics/Joints/b2Joint.cpp deleted file mode 100644 index dd55be94f..000000000 --- a/src/libraries/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/src/libraries/Box2D/Dynamics/Joints/b2MotorJoint.h b/src/libraries/Box2D/Dynamics/Joints/b2MotorJoint.h deleted file mode 100644 index 0b76d6fe7..000000000 --- a/src/libraries/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/src/libraries/Box2D/Dynamics/Joints/b2MouseJoint.h b/src/libraries/Box2D/Dynamics/Joints/b2MouseJoint.h deleted file mode 100644 index 8b50a0e14..000000000 --- a/src/libraries/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/src/libraries/Box2D/Dynamics/Joints/b2PrismaticJoint.cpp b/src/libraries/Box2D/Dynamics/Joints/b2PrismaticJoint.cpp deleted file mode 100644 index 217e14502..000000000 --- a/src/libraries/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/src/libraries/Box2D/Dynamics/Joints/b2RevoluteJoint.cpp b/src/libraries/Box2D/Dynamics/Joints/b2RevoluteJoint.cpp deleted file mode 100644 index 64bd19070..000000000 --- a/src/libraries/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/src/libraries/Box2D/Dynamics/Joints/b2RopeJoint.h b/src/libraries/Box2D/Dynamics/Joints/b2RopeJoint.h deleted file mode 100644 index eaca575d0..000000000 --- a/src/libraries/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/src/libraries/Box2D/Dynamics/Joints/b2WeldJoint.h b/src/libraries/Box2D/Dynamics/Joints/b2WeldJoint.h deleted file mode 100644 index eed9bc103..000000000 --- a/src/libraries/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/src/libraries/Box2D/Dynamics/Joints/b2WheelJoint.cpp b/src/libraries/Box2D/Dynamics/Joints/b2WheelJoint.cpp deleted file mode 100644 index c9f6de600..000000000 --- a/src/libraries/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/src/libraries/Box2D/Dynamics/Joints/b2WheelJoint.h b/src/libraries/Box2D/Dynamics/Joints/b2WheelJoint.h deleted file mode 100644 index a33c77f4b..000000000 --- a/src/libraries/Box2D/Dynamics/Joints/b2WheelJoint.h +++ /dev/null @@ -1,210 +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. In other words, it is a point to -/// line constraint with a rotational motor and a linear spring/damper. -/// This joint is designed for vehicle suspensions. -class b2WheelJoint : public b2Joint -{ -public: - b2Vec2 GetAnchorA() const; - 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/src/libraries/Box2D/Dynamics/b2ContactManager.h b/src/libraries/Box2D/Dynamics/b2ContactManager.h deleted file mode 100644 index 6a1d072af..000000000 --- a/src/libraries/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/src/libraries/Box2D/Dynamics/b2TimeStep.h b/src/libraries/Box2D/Dynamics/b2TimeStep.h deleted file mode 100644 index a3727624c..000000000 --- a/src/libraries/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/src/libraries/Box2D/Dynamics/b2WorldCallbacks.cpp b/src/libraries/Box2D/Dynamics/b2WorldCallbacks.cpp deleted file mode 100644 index 0f4b9c53c..000000000 --- a/src/libraries/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/src/libraries/Box2D/Dynamics/b2Body.cpp b/src/libraries/Box2D/Dynamics/b2_body.cpp similarity index 70% rename from src/libraries/Box2D/Dynamics/b2Body.cpp rename to src/libraries/Box2D/Dynamics/b2_body.cpp index b6550531f..39851c4cb 100644 --- a/src/libraries/Box2D/Dynamics/b2Body.cpp +++ b/src/libraries/Box2D/Dynamics/b2_body.cpp @@ -1,26 +1,32 @@ -/* -* 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. -*/ +// MIT License -#include -#include -#include -#include -#include +// 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) { @@ -45,13 +51,13 @@ b2Body::b2Body(const b2BodyDef* bd, b2World* world) { m_flags |= e_autoSleepFlag; } - if (bd->awake) + if (bd->awake && bd->type != b2_staticBody) { m_flags |= e_awakeFlag; } - if (bd->active) + if (bd->enabled) { - m_flags |= e_activeFlag; + m_flags |= e_enabledFlag; } m_world = world; @@ -66,10 +72,10 @@ b2Body::b2Body(const b2BodyDef* bd, b2World* world) m_sweep.a = bd->angle; m_sweep.alpha0 = 0.0f; - m_jointList = NULL; - m_contactList = NULL; - m_prev = NULL; - m_next = NULL; + m_jointList = nullptr; + m_contactList = nullptr; + m_prev = nullptr; + m_next = nullptr; m_linearVelocity = bd->linearVelocity; m_angularVelocity = bd->angularVelocity; @@ -85,23 +91,15 @@ b2Body::b2Body(const b2BodyDef* bd, b2World* world) 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_mass = 0.0f; + m_invMass = 0.0f; m_I = 0.0f; m_invI = 0.0f; m_userData = bd->userData; - m_fixtureList = NULL; + m_fixtureList = nullptr; m_fixtureCount = 0; } @@ -133,6 +131,7 @@ void b2Body::SetType(b2BodyType type) m_angularVelocity = 0.0f; m_sweep.a0 = m_sweep.a; m_sweep.c0 = m_sweep.c; + m_flags &= ~e_awakeFlag; SynchronizeFixtures(); } @@ -149,7 +148,7 @@ void b2Body::SetType(b2BodyType type) ce = ce->next; m_world->m_contactManager.Destroy(ce0->contact); } - m_contactList = NULL; + m_contactList = nullptr; // Touch the proxies so that new contacts will be created (when appropriate) b2BroadPhase* broadPhase = &m_world->m_contactManager.m_broadPhase; @@ -168,7 +167,7 @@ b2Fixture* b2Body::CreateFixture(const b2FixtureDef* def) b2Assert(m_world->IsLocked() == false); if (m_world->IsLocked() == true) { - return NULL; + return nullptr; } b2BlockAllocator* allocator = &m_world->m_blockAllocator; @@ -177,7 +176,7 @@ b2Fixture* b2Body::CreateFixture(const b2FixtureDef* def) b2Fixture* fixture = new (memory) b2Fixture; fixture->Create(allocator, this, def); - if (m_flags & e_activeFlag) + if (m_flags & e_enabledFlag) { b2BroadPhase* broadPhase = &m_world->m_contactManager.m_broadPhase; fixture->CreateProxies(broadPhase, m_xf); @@ -197,12 +196,12 @@ b2Fixture* b2Body::CreateFixture(const b2FixtureDef* def) // 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; + m_world->m_newContacts = true; return fixture; } -b2Fixture* b2Body::CreateFixture(const b2Shape* shape, float32 density) +b2Fixture* b2Body::CreateFixture(const b2Shape* shape, float density) { b2FixtureDef def; def.shape = shape; @@ -213,6 +212,11 @@ b2Fixture* b2Body::CreateFixture(const b2Shape* shape, float32 density) void b2Body::DestroyFixture(b2Fixture* fixture) { + if (fixture == NULL) + { + return; + } + b2Assert(m_world->IsLocked() == false); if (m_world->IsLocked() == true) { @@ -225,7 +229,7 @@ void b2Body::DestroyFixture(b2Fixture* fixture) b2Assert(m_fixtureCount > 0); b2Fixture** node = &m_fixtureList; bool found = false; - while (*node != NULL) + while (*node != nullptr) { if (*node == fixture) { @@ -260,15 +264,15 @@ void b2Body::DestroyFixture(b2Fixture* fixture) b2BlockAllocator* allocator = &m_world->m_blockAllocator; - if (m_flags & e_activeFlag) + 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->m_body = NULL; - fixture->m_next = NULL; fixture->~b2Fixture(); allocator->Free(fixture, sizeof(b2Fixture)); @@ -320,12 +324,6 @@ void b2Body::ResetMassData() 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) { @@ -414,7 +412,7 @@ bool b2Body::ShouldCollide(const b2Body* other) const return true; } -void b2Body::SetTransform(const b2Vec2& position, float32 angle) +void b2Body::SetTransform(const b2Vec2& position, float angle) { b2Assert(m_world->IsLocked() == false); if (m_world->IsLocked() == true) @@ -440,29 +438,40 @@ void b2Body::SetTransform(const b2Vec2& position, float32 angle) 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) + + if (m_flags & b2Body::e_awakeFlag) { - f->Synchronize(broadPhase, xf1, m_xf); + 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::SetActive(bool flag) +void b2Body::SetEnabled(bool flag) { b2Assert(m_world->IsLocked() == false); - if (flag == IsActive()) + if (flag == IsEnabled()) { return; } if (flag) { - m_flags |= e_activeFlag; + m_flags |= e_enabledFlag; // Create all proxies. b2BroadPhase* broadPhase = &m_world->m_contactManager.m_broadPhase; @@ -471,11 +480,12 @@ void b2Body::SetActive(bool flag) f->CreateProxies(broadPhase, m_xf); } - // Contacts are created the next time step. + // Contacts are created at the beginning of the next + m_world->m_newContacts = true; } else { - m_flags &= ~e_activeFlag; + m_flags &= ~e_enabledFlag; // Destroy all proxies. b2BroadPhase* broadPhase = &m_world->m_contactManager.m_broadPhase; @@ -492,7 +502,7 @@ void b2Body::SetActive(bool flag) ce = ce->next; m_world->m_contactManager.Destroy(ce0->contact); } - m_contactList = NULL; + m_contactList = nullptr; } } @@ -522,28 +532,31 @@ 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"); + // %.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) { - b2Log(" {\n"); + b2Dump(" {\n"); f->Dump(bodyIndex); - b2Log(" }\n"); + b2Dump(" }\n"); } - b2Log("}\n"); + b2Dump("}\n"); } diff --git a/src/libraries/Box2D/Dynamics/b2_chain_circle_contact.cpp b/src/libraries/Box2D/Dynamics/b2_chain_circle_contact.cpp new file mode 100644 index 000000000..8464fe836 --- /dev/null +++ b/src/libraries/Box2D/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/src/libraries/Box2D/Dynamics/b2_chain_circle_contact.h b/src/libraries/Box2D/Dynamics/b2_chain_circle_contact.h new file mode 100644 index 000000000..33ced64d8 --- /dev/null +++ b/src/libraries/Box2D/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/src/libraries/Box2D/Dynamics/b2_chain_polygon_contact.cpp b/src/libraries/Box2D/Dynamics/b2_chain_polygon_contact.cpp new file mode 100644 index 000000000..b8257aab9 --- /dev/null +++ b/src/libraries/Box2D/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/src/libraries/Box2D/Dynamics/b2_chain_polygon_contact.h b/src/libraries/Box2D/Dynamics/b2_chain_polygon_contact.h new file mode 100644 index 000000000..058154fb1 --- /dev/null +++ b/src/libraries/Box2D/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/src/libraries/Box2D/Dynamics/b2_circle_contact.cpp b/src/libraries/Box2D/Dynamics/b2_circle_contact.cpp new file mode 100644 index 000000000..738aa84ca --- /dev/null +++ b/src/libraries/Box2D/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/src/libraries/Box2D/Dynamics/b2_circle_contact.h b/src/libraries/Box2D/Dynamics/b2_circle_contact.h new file mode 100644 index 000000000..98e09ea5b --- /dev/null +++ b/src/libraries/Box2D/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/src/libraries/Box2D/Dynamics/Contacts/b2Contact.cpp b/src/libraries/Box2D/Dynamics/b2_contact.cpp similarity index 72% rename from src/libraries/Box2D/Dynamics/Contacts/b2Contact.cpp rename to src/libraries/Box2D/Dynamics/b2_contact.cpp index d9f1021e4..7c95649d0 100644 --- a/src/libraries/Box2D/Dynamics/Contacts/b2Contact.cpp +++ b/src/libraries/Box2D/Dynamics/b2_contact.cpp @@ -1,38 +1,42 @@ -/* -* 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. -*/ +// MIT License -#include -#include -#include -#include -#include -#include -#include -#include -#include +// Copyright (c) 2019 Erin Catto -#include -#include -#include -#include -#include -#include -#include +// 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; @@ -94,7 +98,7 @@ b2Contact* b2Contact::Create(b2Fixture* fixtureA, int32 indexA, b2Fixture* fixtu } else { - return NULL; + return nullptr; } } @@ -116,8 +120,8 @@ void b2Contact::Destroy(b2Contact* contact, b2BlockAllocator* allocator) b2Shape::Type typeA = fixtureA->GetType(); b2Shape::Type typeB = fixtureB->GetType(); - b2Assert(0 <= typeA && typeB < b2Shape::e_typeCount); - b2Assert(0 <= typeA && typeB < b2Shape::e_typeCount); + b2Assert(0 <= typeA && typeA < b2Shape::e_typeCount); + b2Assert(0 <= typeB && typeB < b2Shape::e_typeCount); b2ContactDestroyFcn* destroyFcn = s_registers[typeA][typeB].destroyFcn; destroyFcn(contact, allocator); @@ -135,18 +139,18 @@ b2Contact::b2Contact(b2Fixture* fA, int32 indexA, b2Fixture* fB, int32 indexB) m_manifold.pointCount = 0; - m_prev = NULL; - m_next = NULL; + m_prev = nullptr; + m_next = nullptr; - m_nodeA.contact = NULL; - m_nodeA.prev = NULL; - m_nodeA.next = NULL; - m_nodeA.other = NULL; + m_nodeA.contact = nullptr; + m_nodeA.prev = nullptr; + m_nodeA.next = nullptr; + m_nodeA.other = nullptr; - m_nodeB.contact = NULL; - m_nodeB.prev = NULL; - m_nodeB.next = NULL; - m_nodeB.other = NULL; + m_nodeB.contact = nullptr; + m_nodeB.prev = nullptr; + m_nodeB.next = nullptr; + m_nodeB.other = nullptr; m_toiCount = 0; diff --git a/src/libraries/Box2D/Dynamics/b2ContactManager.cpp b/src/libraries/Box2D/Dynamics/b2_contact_manager.cpp similarity index 78% rename from src/libraries/Box2D/Dynamics/b2ContactManager.cpp rename to src/libraries/Box2D/Dynamics/b2_contact_manager.cpp index d8a189b74..56666084c 100644 --- a/src/libraries/Box2D/Dynamics/b2ContactManager.cpp +++ b/src/libraries/Box2D/Dynamics/b2_contact_manager.cpp @@ -1,37 +1,41 @@ -/* -* 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. -*/ +// MIT License -#include -#include -#include -#include -#include +// 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 = NULL; + m_contactList = nullptr; m_contactCount = 0; m_contactFilter = &b2_defaultFilter; m_contactListener = &b2_defaultListener; - m_allocator = NULL; + m_allocator = nullptr; } void b2ContactManager::Destroy(b2Contact* c) @@ -237,7 +241,7 @@ void b2ContactManager::AddPair(void* proxyUserDataA, void* proxyUserDataB) // Call the factory. b2Contact* c = b2Contact::Create(fixtureA, indexA, fixtureB, indexB, m_allocator); - if (c == NULL) + if (c == nullptr) { return; } @@ -251,9 +255,9 @@ void b2ContactManager::AddPair(void* proxyUserDataA, void* proxyUserDataB) bodyB = fixtureB->GetBody(); // Insert into the world. - c->m_prev = NULL; + c->m_prev = nullptr; c->m_next = m_contactList; - if (m_contactList != NULL) + if (m_contactList != nullptr) { m_contactList->m_prev = c; } @@ -265,9 +269,9 @@ void b2ContactManager::AddPair(void* proxyUserDataA, void* proxyUserDataB) c->m_nodeA.contact = c; c->m_nodeA.other = bodyB; - c->m_nodeA.prev = NULL; + c->m_nodeA.prev = nullptr; c->m_nodeA.next = bodyA->m_contactList; - if (bodyA->m_contactList != NULL) + if (bodyA->m_contactList != nullptr) { bodyA->m_contactList->prev = &c->m_nodeA; } @@ -277,20 +281,13 @@ void b2ContactManager::AddPair(void* proxyUserDataA, void* proxyUserDataB) c->m_nodeB.contact = c; c->m_nodeB.other = bodyA; - c->m_nodeB.prev = NULL; + c->m_nodeB.prev = nullptr; c->m_nodeB.next = bodyB->m_contactList; - if (bodyB->m_contactList != NULL) + if (bodyB->m_contactList != nullptr) { bodyB->m_contactList->prev = &c->m_nodeB; } bodyB->m_contactList = &c->m_nodeB; - // Wake up the bodies - if (fixtureA->IsSensor() == false && fixtureB->IsSensor() == false) - { - bodyA->SetAwake(true); - bodyB->SetAwake(true); - } - ++m_contactCount; } diff --git a/src/libraries/Box2D/Dynamics/Contacts/b2ContactSolver.cpp b/src/libraries/Box2D/Dynamics/b2_contact_solver.cpp similarity index 79% rename from src/libraries/Box2D/Dynamics/Contacts/b2ContactSolver.cpp rename to src/libraries/Box2D/Dynamics/b2_contact_solver.cpp index e31b6c236..f60a2086e 100644 --- a/src/libraries/Box2D/Dynamics/Contacts/b2ContactSolver.cpp +++ b/src/libraries/Box2D/Dynamics/b2_contact_solver.cpp @@ -1,29 +1,34 @@ -/* -* 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 -#include +// Copyright (c) 2019 Erin Catto -#include -#include -#include -#include -#include +// 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 bool g_blockSolve = true; @@ -35,11 +40,11 @@ struct b2ContactPositionConstraint b2Vec2 localPoint; int32 indexA; int32 indexB; - float32 invMassA, invMassB; + float invMassA, invMassB; b2Vec2 localCenterA, localCenterB; - float32 invIA, invIB; + float invIA, invIB; b2Manifold::Type type; - float32 radiusA, radiusB; + float radiusA, radiusB; int32 pointCount; }; @@ -63,8 +68,8 @@ b2ContactSolver::b2ContactSolver(b2ContactSolverDef* def) b2Fixture* fixtureB = contact->m_fixtureB; b2Shape* shapeA = fixtureA->GetShape(); b2Shape* shapeB = fixtureB->GetShape(); - float32 radiusA = shapeA->m_radius; - float32 radiusB = shapeB->m_radius; + float radiusA = shapeA->m_radius; + float radiusB = shapeB->m_radius; b2Body* bodyA = fixtureA->GetBody(); b2Body* bodyB = fixtureB->GetBody(); b2Manifold* manifold = contact->GetManifold(); @@ -144,29 +149,29 @@ void b2ContactSolver::InitializeVelocityConstraints() b2ContactVelocityConstraint* vc = m_velocityConstraints + i; b2ContactPositionConstraint* pc = m_positionConstraints + i; - float32 radiusA = pc->radiusA; - float32 radiusB = pc->radiusB; + float radiusA = pc->radiusA; + float 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; + 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; - float32 aA = m_positions[indexA].a; + float aA = m_positions[indexA].a; b2Vec2 vA = m_velocities[indexA].v; - float32 wA = m_velocities[indexA].w; + float wA = m_velocities[indexA].w; b2Vec2 cB = m_positions[indexB].c; - float32 aB = m_positions[indexB].a; + float aB = m_positions[indexB].a; b2Vec2 vB = m_velocities[indexB].v; - float32 wB = m_velocities[indexB].w; + float wB = m_velocities[indexB].w; b2Assert(manifold->pointCount > 0); @@ -189,25 +194,25 @@ void b2ContactSolver::InitializeVelocityConstraints() 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); + float rnA = b2Cross(vcp->rA, vc->normal); + float rnB = b2Cross(vcp->rB, vc->normal); - float32 kNormal = mA + mB + iA * rnA * rnA + iB * rnB * rnB; + 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); - float32 rtA = b2Cross(vcp->rA, tangent); - float32 rtB = b2Cross(vcp->rB, tangent); + float rtA = b2Cross(vcp->rA, tangent); + float rtB = b2Cross(vcp->rB, tangent); - float32 kTangent = mA + mB + iA * rtA * rtA + iB * rtB * rtB; + 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; - float32 vRel = b2Dot(vc->normal, vB + b2Cross(wB, vcp->rB) - vA - b2Cross(wA, vcp->rA)); + float vRel = b2Dot(vc->normal, vB + b2Cross(wB, vcp->rB) - vA - b2Cross(wA, vcp->rA)); if (vRel < -b2_velocityThreshold) { vcp->velocityBias = -vc->restitution * vRel; @@ -220,17 +225,17 @@ void b2ContactSolver::InitializeVelocityConstraints() 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); + 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); - 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; + 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 float32 k_maxConditionNumber = 1000.0f; + const float k_maxConditionNumber = 1000.0f; if (k11 * k11 < k_maxConditionNumber * (k11 * k22 - k12 * k12)) { // K is safe to invert. @@ -257,16 +262,16 @@ void b2ContactSolver::WarmStart() int32 indexA = vc->indexA; int32 indexB = vc->indexB; - float32 mA = vc->invMassA; - float32 iA = vc->invIA; - float32 mB = vc->invMassB; - float32 iB = vc->invIB; + 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; - float32 wA = m_velocities[indexA].w; + float wA = m_velocities[indexA].w; b2Vec2 vB = m_velocities[indexB].v; - float32 wB = m_velocities[indexB].w; + float wB = m_velocities[indexB].w; b2Vec2 normal = vc->normal; b2Vec2 tangent = b2Cross(normal, 1.0f); @@ -296,20 +301,20 @@ void b2ContactSolver::SolveVelocityConstraints() int32 indexA = vc->indexA; int32 indexB = vc->indexB; - float32 mA = vc->invMassA; - float32 iA = vc->invIA; - float32 mB = vc->invMassB; - float32 iB = vc->invIB; + 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; - float32 wA = m_velocities[indexA].w; + float wA = m_velocities[indexA].w; b2Vec2 vB = m_velocities[indexB].v; - float32 wB = m_velocities[indexB].w; + float wB = m_velocities[indexB].w; b2Vec2 normal = vc->normal; b2Vec2 tangent = b2Cross(normal, 1.0f); - float32 friction = vc->friction; + float friction = vc->friction; b2Assert(pointCount == 1 || pointCount == 2); @@ -323,12 +328,12 @@ void b2ContactSolver::SolveVelocityConstraints() 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); + float vt = b2Dot(dv, tangent) - vc->tangentSpeed; + float lambda = vcp->tangentMass * (-vt); // b2Clamp the accumulated force - float32 maxFriction = friction * vcp->normalImpulse; - float32 newImpulse = b2Clamp(vcp->tangentImpulse + lambda, -maxFriction, maxFriction); + float maxFriction = friction * vcp->normalImpulse; + float newImpulse = b2Clamp(vcp->tangentImpulse + lambda, -maxFriction, maxFriction); lambda = newImpulse - vcp->tangentImpulse; vcp->tangentImpulse = newImpulse; @@ -345,19 +350,19 @@ void b2ContactSolver::SolveVelocityConstraints() // Solve normal constraints if (pointCount == 1 || g_blockSolve == false) { - for (int32 i = 0; i < pointCount; ++i) + for (int32 j = 0; j < pointCount; ++j) { - b2VelocityConstraintPoint* vcp = vc->points + i; + b2VelocityConstraintPoint* vcp = vc->points + j; // Relative velocity at contact b2Vec2 dv = vB + b2Cross(wB, vcp->rB) - vA - b2Cross(wA, vcp->rA); // Compute normal impulse - float32 vn = b2Dot(dv, normal); - float32 lambda = -vcp->normalMass * (vn - vcp->velocityBias); + float vn = b2Dot(dv, normal); + float lambda = -vcp->normalMass * (vn - vcp->velocityBias); // b2Clamp the accumulated impulse - float32 newImpulse = b2Max(vcp->normalImpulse + lambda, 0.0f); + float newImpulse = b2Max(vcp->normalImpulse + lambda, 0.0f); lambda = newImpulse - vcp->normalImpulse; vcp->normalImpulse = newImpulse; @@ -375,7 +380,7 @@ void b2ContactSolver::SolveVelocityConstraints() // 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 + // 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 @@ -416,8 +421,8 @@ void b2ContactSolver::SolveVelocityConstraints() 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); + float vn1 = b2Dot(dv1, normal); + float vn2 = b2Dot(dv2, normal); b2Vec2 b; b.x = vn1 - cp1->velocityBias; @@ -426,7 +431,7 @@ void b2ContactSolver::SolveVelocityConstraints() // Compute b' b -= b2Mul(vc->K, a); - const float32 k_errorTol = 1e-3f; + const float k_errorTol = 1e-3f; B2_NOT_USED(k_errorTol); for (;;) @@ -485,7 +490,6 @@ void b2ContactSolver::SolveVelocityConstraints() 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 @@ -664,13 +668,13 @@ struct b2PositionSolverManifold b2Vec2 normal; b2Vec2 point; - float32 separation; + float separation; }; // Sequential solver. bool b2ContactSolver::SolvePositionConstraints() { - float32 minSeparation = 0.0f; + float minSeparation = 0.0f; for (int32 i = 0; i < m_count; ++i) { @@ -679,18 +683,18 @@ bool b2ContactSolver::SolvePositionConstraints() int32 indexA = pc->indexA; int32 indexB = pc->indexB; b2Vec2 localCenterA = pc->localCenterA; - float32 mA = pc->invMassA; - float32 iA = pc->invIA; + float mA = pc->invMassA; + float iA = pc->invIA; b2Vec2 localCenterB = pc->localCenterB; - float32 mB = pc->invMassB; - float32 iB = pc->invIB; + float mB = pc->invMassB; + float iB = pc->invIB; int32 pointCount = pc->pointCount; b2Vec2 cA = m_positions[indexA].c; - float32 aA = m_positions[indexA].a; + float aA = m_positions[indexA].a; b2Vec2 cB = m_positions[indexB].c; - float32 aB = m_positions[indexB].a; + float aB = m_positions[indexB].a; // Solve normal constraints for (int32 j = 0; j < pointCount; ++j) @@ -706,7 +710,7 @@ bool b2ContactSolver::SolvePositionConstraints() b2Vec2 normal = psm.normal; b2Vec2 point = psm.point; - float32 separation = psm.separation; + float separation = psm.separation; b2Vec2 rA = point - cA; b2Vec2 rB = point - cB; @@ -715,15 +719,15 @@ bool b2ContactSolver::SolvePositionConstraints() minSeparation = b2Min(minSeparation, separation); // Prevent large corrections and allow slop. - float32 C = b2Clamp(b2_baumgarte * (separation + b2_linearSlop), -b2_maxLinearCorrection, 0.0f); + float 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; + float rnA = b2Cross(rA, normal); + float rnB = b2Cross(rB, normal); + float K = mA + mB + iA * rnA * rnA + iB * rnB * rnB; // Compute normal impulse - float32 impulse = K > 0.0f ? - C / K : 0.0f; + float impulse = K > 0.0f ? - C / K : 0.0f; b2Vec2 P = impulse * normal; @@ -749,7 +753,7 @@ bool b2ContactSolver::SolvePositionConstraints() // Sequential position solver for position constraints. bool b2ContactSolver::SolveTOIPositionConstraints(int32 toiIndexA, int32 toiIndexB) { - float32 minSeparation = 0.0f; + float minSeparation = 0.0f; for (int32 i = 0; i < m_count; ++i) { @@ -761,16 +765,16 @@ bool b2ContactSolver::SolveTOIPositionConstraints(int32 toiIndexA, int32 toiInde b2Vec2 localCenterB = pc->localCenterB; int32 pointCount = pc->pointCount; - float32 mA = 0.0f; - float32 iA = 0.0f; + float mA = 0.0f; + float iA = 0.0f; if (indexA == toiIndexA || indexA == toiIndexB) { mA = pc->invMassA; iA = pc->invIA; } - float32 mB = 0.0f; - float32 iB = 0.; + float mB = 0.0f; + float iB = 0.; if (indexB == toiIndexA || indexB == toiIndexB) { mB = pc->invMassB; @@ -778,10 +782,10 @@ bool b2ContactSolver::SolveTOIPositionConstraints(int32 toiIndexA, int32 toiInde } b2Vec2 cA = m_positions[indexA].c; - float32 aA = m_positions[indexA].a; + float aA = m_positions[indexA].a; b2Vec2 cB = m_positions[indexB].c; - float32 aB = m_positions[indexB].a; + float aB = m_positions[indexB].a; // Solve normal constraints for (int32 j = 0; j < pointCount; ++j) @@ -797,7 +801,7 @@ bool b2ContactSolver::SolveTOIPositionConstraints(int32 toiIndexA, int32 toiInde b2Vec2 normal = psm.normal; b2Vec2 point = psm.point; - float32 separation = psm.separation; + float separation = psm.separation; b2Vec2 rA = point - cA; b2Vec2 rB = point - cB; @@ -806,15 +810,15 @@ bool b2ContactSolver::SolveTOIPositionConstraints(int32 toiIndexA, int32 toiInde minSeparation = b2Min(minSeparation, separation); // Prevent large corrections and allow slop. - float32 C = b2Clamp(b2_toiBaugarte * (separation + b2_linearSlop), -b2_maxLinearCorrection, 0.0f); + float C = b2Clamp(b2_toiBaumgarte * (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; + float rnA = b2Cross(rA, normal); + float rnB = b2Cross(rB, normal); + float K = mA + mB + iA * rnA * rnA + iB * rnB * rnB; // Compute normal impulse - float32 impulse = K > 0.0f ? - C / K : 0.0f; + float impulse = K > 0.0f ? - C / K : 0.0f; b2Vec2 P = impulse * normal; diff --git a/src/libraries/Box2D/Dynamics/b2_contact_solver.h b/src/libraries/Box2D/Dynamics/b2_contact_solver.h new file mode 100644 index 000000000..bef5e819c --- /dev/null +++ b/src/libraries/Box2D/Dynamics/b2_contact_solver.h @@ -0,0 +1,99 @@ +// 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 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/src/libraries/Box2D/Dynamics/Joints/b2DistanceJoint.cpp b/src/libraries/Box2D/Dynamics/b2_distance_joint.cpp similarity index 59% rename from src/libraries/Box2D/Dynamics/Joints/b2DistanceJoint.cpp rename to src/libraries/Box2D/Dynamics/b2_distance_joint.cpp index ba2b6c009..9eea08559 100644 --- a/src/libraries/Box2D/Dynamics/Joints/b2DistanceJoint.cpp +++ b/src/libraries/Box2D/Dynamics/b2_distance_joint.cpp @@ -1,24 +1,28 @@ -/* -* 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 -#include -#include -#include +// 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_distance_joint.h" +#include "box2d/b2_time_step.h" // 1-D constrained system // m (v2 - v1) = lambda @@ -35,6 +39,7 @@ // 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) { @@ -52,8 +57,9 @@ b2DistanceJoint::b2DistanceJoint(const b2DistanceJointDef* def) m_localAnchorA = def->localAnchorA; m_localAnchorB = def->localAnchorB; m_length = def->length; - m_frequencyHz = def->frequencyHz; - m_dampingRatio = def->dampingRatio; + m_stiffness = def->stiffness; + m_damping = def->damping; + m_impulse = 0.0f; m_gamma = 0.0f; m_bias = 0.0f; @@ -71,14 +77,14 @@ void b2DistanceJoint::InitVelocityConstraints(const b2SolverData& data) m_invIB = m_bodyB->m_invI; b2Vec2 cA = data.positions[m_indexA].c; - float32 aA = data.positions[m_indexA].a; + float aA = data.positions[m_indexA].a; b2Vec2 vA = data.velocities[m_indexA].v; - float32 wA = data.velocities[m_indexA].w; + float wA = data.velocities[m_indexA].w; b2Vec2 cB = data.positions[m_indexB].c; - float32 aB = data.positions[m_indexB].a; + float aB = data.positions[m_indexB].a; b2Vec2 vB = data.velocities[m_indexB].v; - float32 wB = data.velocities[m_indexB].w; + float wB = data.velocities[m_indexB].w; b2Rot qA(aA), qB(aB); @@ -87,7 +93,7 @@ void b2DistanceJoint::InitVelocityConstraints(const b2SolverData& data) m_u = cB + m_rB - cA - m_rA; // Handle singularity. - float32 length = m_u.Length(); + float length = m_u.Length(); if (length > b2_linearSlop) { m_u *= 1.0f / length; @@ -97,28 +103,21 @@ void b2DistanceJoint::InitVelocityConstraints(const b2SolverData& data) 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; + 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; - // Compute the effective mass matrix. - m_mass = invMass != 0.0f ? 1.0f / invMass : 0.0f; - - if (m_frequencyHz > 0.0f) + if (m_stiffness > 0.0f) { - float32 C = length - m_length; + float 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; + float d = m_damping; + float k = m_stiffness; // magic formulas - float32 h = data.step.dt; + 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; @@ -130,6 +129,7 @@ void b2DistanceJoint::InitVelocityConstraints(const b2SolverData& data) { m_gamma = 0.0f; m_bias = 0.0f; + m_mass = invMass != 0.0f ? 1.0f / invMass : 0.0f; } if (data.step.warmStarting) @@ -157,16 +157,16 @@ void b2DistanceJoint::InitVelocityConstraints(const b2SolverData& data) void b2DistanceJoint::SolveVelocityConstraints(const b2SolverData& data) { b2Vec2 vA = data.velocities[m_indexA].v; - float32 wA = data.velocities[m_indexA].w; + float wA = data.velocities[m_indexA].w; b2Vec2 vB = data.velocities[m_indexB].v; - float32 wB = data.velocities[m_indexB].w; + float 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); + float Cdot = b2Dot(m_u, vpB - vpA); - float32 impulse = -m_mass * (Cdot + m_bias + m_gamma * m_impulse); + float impulse = -m_mass * (Cdot + m_bias + m_gamma * m_impulse); m_impulse += impulse; b2Vec2 P = impulse * m_u; @@ -183,16 +183,16 @@ void b2DistanceJoint::SolveVelocityConstraints(const b2SolverData& data) bool b2DistanceJoint::SolvePositionConstraints(const b2SolverData& data) { - if (m_frequencyHz > 0.0f) + if (m_stiffness > 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; + float aA = data.positions[m_indexA].a; b2Vec2 cB = data.positions[m_indexB].c; - float32 aB = data.positions[m_indexB].a; + float aB = data.positions[m_indexB].a; b2Rot qA(aA), qB(aB); @@ -200,11 +200,11 @@ bool b2DistanceJoint::SolvePositionConstraints(const b2SolverData& data) b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_localCenterB); b2Vec2 u = cB + rB - cA - rA; - float32 length = u.Normalize(); - float32 C = length - m_length; + float length = u.Normalize(); + float C = length - m_length; C = b2Clamp(C, -b2_maxLinearCorrection, b2_maxLinearCorrection); - float32 impulse = -m_mass * C; + float impulse = -m_mass * C; b2Vec2 P = impulse * u; cA -= m_invMassA * P; @@ -230,13 +230,13 @@ b2Vec2 b2DistanceJoint::GetAnchorB() const return m_bodyB->GetWorldPoint(m_localAnchorB); } -b2Vec2 b2DistanceJoint::GetReactionForce(float32 inv_dt) const +b2Vec2 b2DistanceJoint::GetReactionForce(float inv_dt) const { b2Vec2 F = (inv_dt * m_impulse) * m_u; return F; } -float32 b2DistanceJoint::GetReactionTorque(float32 inv_dt) const +float b2DistanceJoint::GetReactionTorque(float inv_dt) const { B2_NOT_USED(inv_dt); return 0.0f; @@ -247,14 +247,14 @@ 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); + 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.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/src/libraries/Box2D/Dynamics/b2_edge_circle_contact.cpp b/src/libraries/Box2D/Dynamics/b2_edge_circle_contact.cpp new file mode 100644 index 000000000..8a126de6c --- /dev/null +++ b/src/libraries/Box2D/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/src/libraries/Box2D/Dynamics/b2_edge_circle_contact.h b/src/libraries/Box2D/Dynamics/b2_edge_circle_contact.h new file mode 100644 index 000000000..3efc88e60 --- /dev/null +++ b/src/libraries/Box2D/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/src/libraries/Box2D/Dynamics/b2_edge_polygon_contact.cpp b/src/libraries/Box2D/Dynamics/b2_edge_polygon_contact.cpp new file mode 100644 index 000000000..e617e3516 --- /dev/null +++ b/src/libraries/Box2D/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/src/libraries/Box2D/Dynamics/b2_edge_polygon_contact.h b/src/libraries/Box2D/Dynamics/b2_edge_polygon_contact.h new file mode 100644 index 000000000..e6616b534 --- /dev/null +++ b/src/libraries/Box2D/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/src/libraries/Box2D/Dynamics/b2Fixture.cpp b/src/libraries/Box2D/Dynamics/b2_fixture.cpp similarity index 54% rename from src/libraries/Box2D/Dynamics/b2Fixture.cpp rename to src/libraries/Box2D/Dynamics/b2_fixture.cpp index fb17f0a3e..420cfdcfa 100644 --- a/src/libraries/Box2D/Dynamics/b2Fixture.cpp +++ b/src/libraries/Box2D/Dynamics/b2_fixture.cpp @@ -1,40 +1,44 @@ -/* -* 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. -*/ +// MIT License -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include +// 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_userData = NULL; - m_body = NULL; - m_next = NULL; - m_proxies = NULL; + m_userData = nullptr; + m_body = nullptr; + m_next = nullptr; + m_proxies = nullptr; m_proxyCount = 0; - m_shape = NULL; + m_shape = nullptr; m_density = 0.0f; } @@ -45,7 +49,7 @@ void b2Fixture::Create(b2BlockAllocator* allocator, b2Body* body, const b2Fixtur m_restitution = def->restitution; m_body = body; - m_next = NULL; + m_next = nullptr; m_filter = def->filter; @@ -58,7 +62,7 @@ void b2Fixture::Create(b2BlockAllocator* allocator, b2Body* body, const b2Fixtur m_proxies = (b2FixtureProxy*)allocator->Allocate(childCount * sizeof(b2FixtureProxy)); for (int32 i = 0; i < childCount; ++i) { - m_proxies[i].fixture = NULL; + m_proxies[i].fixture = nullptr; m_proxies[i].proxyId = b2BroadPhase::e_nullProxy; } m_proxyCount = 0; @@ -74,7 +78,7 @@ void b2Fixture::Destroy(b2BlockAllocator* allocator) // Free the proxy array. int32 childCount = m_shape->GetChildCount(); allocator->Free(m_proxies, childCount * sizeof(b2FixtureProxy)); - m_proxies = NULL; + m_proxies = nullptr; // Free the child shape. switch (m_shape->m_type) @@ -116,7 +120,7 @@ void b2Fixture::Destroy(b2BlockAllocator* allocator) break; } - m_shape = NULL; + m_shape = nullptr; } void b2Fixture::CreateProxies(b2BroadPhase* broadPhase, const b2Transform& xf) @@ -167,7 +171,7 @@ void b2Fixture::Synchronize(b2BroadPhase* broadPhase, const b2Transform& transfo proxy->aabb.Combine(aabb1, aabb2); - b2Vec2 displacement = transform2.p - transform1.p; + b2Vec2 displacement = aabb2.GetCenter() - aabb1.GetCenter(); broadPhase->MoveProxy(proxy->proxyId, proxy->aabb, displacement); } @@ -182,7 +186,7 @@ void b2Fixture::SetFilterData(const b2Filter& filter) void b2Fixture::Refilter() { - if (m_body == NULL) + if (m_body == nullptr) { return; } @@ -204,7 +208,7 @@ void b2Fixture::Refilter() b2World* world = m_body->GetWorld(); - if (world == NULL) + if (world == nullptr) { return; } @@ -228,67 +232,64 @@ void b2Fixture::SetSensor(bool 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); + b2Dump(" b2FixtureDef fd;\n"); + b2Dump(" fd.friction = %.9g;\n", m_friction); + b2Dump(" fd.restitution = %.9g;\n", m_restitution); + 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; - 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); + 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; - 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); + 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; - b2Log(" b2PolygonShape shape;\n"); - b2Log(" b2Vec2 vs[%d];\n", b2_maxPolygonVertices); + b2Dump(" b2PolygonShape shape;\n"); + b2Dump(" 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); + b2Dump(" vs[%d].Set(%.9g, %.9g);\n", i, s->m_vertices[i].x, s->m_vertices[i].y); } - b2Log(" shape.Set(vs, %d);\n", s->m_count); + b2Dump(" 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); + b2Dump(" b2ChainShape shape;\n"); + b2Dump(" 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); + b2Dump(" vs[%d].Set(%.9g, %.9g);\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); + 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; @@ -296,8 +297,8 @@ void b2Fixture::Dump(int32 bodyIndex) return; } - b2Log("\n"); - b2Log(" fd.shape = &shape;\n"); - b2Log("\n"); - b2Log(" bodies[%d]->CreateFixture(&fd);\n", bodyIndex); + b2Dump("\n"); + b2Dump(" fd.shape = &shape;\n"); + b2Dump("\n"); + b2Dump(" bodies[%d]->CreateFixture(&fd);\n", bodyIndex); } diff --git a/src/libraries/Box2D/Dynamics/Joints/b2FrictionJoint.cpp b/src/libraries/Box2D/Dynamics/b2_friction_joint.cpp similarity index 62% rename from src/libraries/Box2D/Dynamics/Joints/b2FrictionJoint.cpp rename to src/libraries/Box2D/Dynamics/b2_friction_joint.cpp index 412d257a7..d9d893ad3 100644 --- a/src/libraries/Box2D/Dynamics/Joints/b2FrictionJoint.cpp +++ b/src/libraries/Box2D/Dynamics/b2_friction_joint.cpp @@ -1,24 +1,28 @@ -/* -* 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 -#include -#include -#include +// 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 @@ -64,13 +68,13 @@ void b2FrictionJoint::InitVelocityConstraints(const b2SolverData& data) m_invIA = m_bodyA->m_invI; m_invIB = m_bodyB->m_invI; - float32 aA = data.positions[m_indexA].a; + float aA = data.positions[m_indexA].a; b2Vec2 vA = data.velocities[m_indexA].v; - float32 wA = data.velocities[m_indexA].w; + float wA = data.velocities[m_indexA].w; - float32 aB = data.positions[m_indexB].a; + float aB = data.positions[m_indexB].a; b2Vec2 vB = data.velocities[m_indexB].v; - float32 wB = data.velocities[m_indexB].w; + float wB = data.velocities[m_indexB].w; b2Rot qA(aA), qB(aB); @@ -87,8 +91,8 @@ void b2FrictionJoint::InitVelocityConstraints(const b2SolverData& data) // [ -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; + 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; @@ -131,22 +135,22 @@ void b2FrictionJoint::InitVelocityConstraints(const b2SolverData& data) void b2FrictionJoint::SolveVelocityConstraints(const b2SolverData& data) { b2Vec2 vA = data.velocities[m_indexA].v; - float32 wA = data.velocities[m_indexA].w; + float wA = data.velocities[m_indexA].w; b2Vec2 vB = data.velocities[m_indexB].v; - float32 wB = data.velocities[m_indexB].w; + float wB = data.velocities[m_indexB].w; - float32 mA = m_invMassA, mB = m_invMassB; - float32 iA = m_invIA, iB = m_invIB; + float mA = m_invMassA, mB = m_invMassB; + float iA = m_invIA, iB = m_invIB; - float32 h = data.step.dt; + float h = data.step.dt; // Solve angular friction { - float32 Cdot = wB - wA; - float32 impulse = -m_angularMass * Cdot; + float Cdot = wB - wA; + float impulse = -m_angularMass * Cdot; - float32 oldImpulse = m_angularImpulse; - float32 maxImpulse = h * m_maxTorque; + float oldImpulse = m_angularImpulse; + float maxImpulse = h * m_maxTorque; m_angularImpulse = b2Clamp(m_angularImpulse + impulse, -maxImpulse, maxImpulse); impulse = m_angularImpulse - oldImpulse; @@ -162,7 +166,7 @@ void b2FrictionJoint::SolveVelocityConstraints(const b2SolverData& data) b2Vec2 oldImpulse = m_linearImpulse; m_linearImpulse += impulse; - float32 maxImpulse = h * m_maxForce; + float maxImpulse = h * m_maxForce; if (m_linearImpulse.LengthSquared() > maxImpulse * maxImpulse) { @@ -202,34 +206,34 @@ b2Vec2 b2FrictionJoint::GetAnchorB() const return m_bodyB->GetWorldPoint(m_localAnchorB); } -b2Vec2 b2FrictionJoint::GetReactionForce(float32 inv_dt) const +b2Vec2 b2FrictionJoint::GetReactionForce(float inv_dt) const { return inv_dt * m_linearImpulse; } -float32 b2FrictionJoint::GetReactionTorque(float32 inv_dt) const +float b2FrictionJoint::GetReactionTorque(float inv_dt) const { return inv_dt * m_angularImpulse; } -void b2FrictionJoint::SetMaxForce(float32 force) +void b2FrictionJoint::SetMaxForce(float force) { b2Assert(b2IsValid(force) && force >= 0.0f); m_maxForce = force; } -float32 b2FrictionJoint::GetMaxForce() const +float b2FrictionJoint::GetMaxForce() const { return m_maxForce; } -void b2FrictionJoint::SetMaxTorque(float32 torque) +void b2FrictionJoint::SetMaxTorque(float torque) { b2Assert(b2IsValid(torque) && torque >= 0.0f); m_maxTorque = torque; } -float32 b2FrictionJoint::GetMaxTorque() const +float b2FrictionJoint::GetMaxTorque() const { return m_maxTorque; } @@ -239,13 +243,13 @@ 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); + 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/src/libraries/Box2D/Dynamics/Joints/b2GearJoint.cpp b/src/libraries/Box2D/Dynamics/b2_gear_joint.cpp similarity index 74% rename from src/libraries/Box2D/Dynamics/Joints/b2GearJoint.cpp rename to src/libraries/Box2D/Dynamics/b2_gear_joint.cpp index 155eb0c1d..414291093 100644 --- a/src/libraries/Box2D/Dynamics/Joints/b2GearJoint.cpp +++ b/src/libraries/Box2D/Dynamics/b2_gear_joint.cpp @@ -1,26 +1,30 @@ -/* -* 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. -*/ +// MIT License -#include -#include -#include -#include -#include +// 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 @@ -53,7 +57,7 @@ b2GearJoint::b2GearJoint(const b2GearJointDef* def) b2Assert(m_typeA == e_revoluteJoint || m_typeA == e_prismaticJoint); b2Assert(m_typeB == e_revoluteJoint || m_typeB == e_prismaticJoint); - float32 coordinateA, coordinateB; + float coordinateA, coordinateB; // TODO_ERIN there might be some problem with the joint edges in b2Joint. @@ -62,9 +66,9 @@ b2GearJoint::b2GearJoint(const b2GearJointDef* def) // Get geometry of joint1 b2Transform xfA = m_bodyA->m_xf; - float32 aA = m_bodyA->m_sweep.a; + float aA = m_bodyA->m_sweep.a; b2Transform xfC = m_bodyC->m_xf; - float32 aC = m_bodyC->m_sweep.a; + float aC = m_bodyC->m_sweep.a; if (m_typeA == e_revoluteJoint) { @@ -94,9 +98,9 @@ b2GearJoint::b2GearJoint(const b2GearJointDef* def) // Get geometry of joint2 b2Transform xfB = m_bodyB->m_xf; - float32 aB = m_bodyB->m_sweep.a; + float aB = m_bodyB->m_sweep.a; b2Transform xfD = m_bodyD->m_xf; - float32 aD = m_bodyD->m_sweep.a; + float aD = m_bodyD->m_sweep.a; if (m_typeB == e_revoluteJoint) { @@ -147,21 +151,21 @@ void b2GearJoint::InitVelocityConstraints(const b2SolverData& data) m_iC = m_bodyC->m_invI; m_iD = m_bodyD->m_invI; - float32 aA = data.positions[m_indexA].a; + float aA = data.positions[m_indexA].a; b2Vec2 vA = data.velocities[m_indexA].v; - float32 wA = data.velocities[m_indexA].w; + float wA = data.velocities[m_indexA].w; - float32 aB = data.positions[m_indexB].a; + float aB = data.positions[m_indexB].a; b2Vec2 vB = data.velocities[m_indexB].v; - float32 wB = data.velocities[m_indexB].w; + float wB = data.velocities[m_indexB].w; - float32 aC = data.positions[m_indexC].a; + float aC = data.positions[m_indexC].a; b2Vec2 vC = data.velocities[m_indexC].v; - float32 wC = data.velocities[m_indexC].w; + float wC = data.velocities[m_indexC].w; - float32 aD = data.positions[m_indexD].a; + float aD = data.positions[m_indexD].a; b2Vec2 vD = data.velocities[m_indexD].v; - float32 wD = data.velocities[m_indexD].w; + float wD = data.velocities[m_indexD].w; b2Rot qA(aA), qB(aB), qC(aC), qD(aD); @@ -235,18 +239,18 @@ void b2GearJoint::InitVelocityConstraints(const b2SolverData& data) void b2GearJoint::SolveVelocityConstraints(const b2SolverData& data) { b2Vec2 vA = data.velocities[m_indexA].v; - float32 wA = data.velocities[m_indexA].w; + float wA = data.velocities[m_indexA].w; b2Vec2 vB = data.velocities[m_indexB].v; - float32 wB = data.velocities[m_indexB].w; + float wB = data.velocities[m_indexB].w; b2Vec2 vC = data.velocities[m_indexC].v; - float32 wC = data.velocities[m_indexC].w; + float wC = data.velocities[m_indexC].w; b2Vec2 vD = data.velocities[m_indexD].v; - float32 wD = data.velocities[m_indexD].w; + float wD = data.velocities[m_indexD].w; - float32 Cdot = b2Dot(m_JvAC, vA - vC) + b2Dot(m_JvBD, vB - vD); + 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); - float32 impulse = -m_mass * Cdot; + float impulse = -m_mass * Cdot; m_impulse += impulse; vA += (m_mA * impulse) * m_JvAC; @@ -271,23 +275,23 @@ void b2GearJoint::SolveVelocityConstraints(const b2SolverData& data) bool b2GearJoint::SolvePositionConstraints(const b2SolverData& data) { b2Vec2 cA = data.positions[m_indexA].c; - float32 aA = data.positions[m_indexA].a; + float aA = data.positions[m_indexA].a; b2Vec2 cB = data.positions[m_indexB].c; - float32 aB = data.positions[m_indexB].a; + float aB = data.positions[m_indexB].a; b2Vec2 cC = data.positions[m_indexC].c; - float32 aC = data.positions[m_indexC].a; + float aC = data.positions[m_indexC].a; b2Vec2 cD = data.positions[m_indexD].c; - float32 aD = data.positions[m_indexD].a; + float aD = data.positions[m_indexD].a; b2Rot qA(aA), qB(aB), qC(aC), qD(aD); - float32 linearError = 0.0f; + float linearError = 0.0f; - float32 coordinateA, coordinateB; + float coordinateA, coordinateB; b2Vec2 JvAC, JvBD; - float32 JwA, JwB, JwC, JwD; - float32 mass = 0.0f; + float JwA, JwB, JwC, JwD; + float mass = 0.0f; if (m_typeA == e_revoluteJoint) { @@ -337,9 +341,9 @@ bool b2GearJoint::SolvePositionConstraints(const b2SolverData& data) coordinateB = b2Dot(pB - pD, m_localAxisD); } - float32 C = (coordinateA + m_ratio * coordinateB) - m_constant; + float C = (coordinateA + m_ratio * coordinateB) - m_constant; - float32 impulse = 0.0f; + float impulse = 0.0f; if (mass > 0.0f) { impulse = -C / mass; @@ -377,25 +381,25 @@ b2Vec2 b2GearJoint::GetAnchorB() const return m_bodyB->GetWorldPoint(m_localAnchorB); } -b2Vec2 b2GearJoint::GetReactionForce(float32 inv_dt) const +b2Vec2 b2GearJoint::GetReactionForce(float inv_dt) const { b2Vec2 P = m_impulse * m_JvAC; return inv_dt * P; } -float32 b2GearJoint::GetReactionTorque(float32 inv_dt) const +float b2GearJoint::GetReactionTorque(float inv_dt) const { - float32 L = m_impulse * m_JwA; + float L = m_impulse * m_JwA; return inv_dt * L; } -void b2GearJoint::SetRatio(float32 ratio) +void b2GearJoint::SetRatio(float ratio) { b2Assert(b2IsValid(ratio)); m_ratio = ratio; } -float32 b2GearJoint::GetRatio() const +float b2GearJoint::GetRatio() const { return m_ratio; } @@ -408,12 +412,12 @@ void b2GearJoint::Dump() 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); + 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/src/libraries/Box2D/Dynamics/b2Island.cpp b/src/libraries/Box2D/Dynamics/b2_island.cpp similarity index 86% rename from src/libraries/Box2D/Dynamics/b2Island.cpp rename to src/libraries/Box2D/Dynamics/b2_island.cpp index d46589dee..48d89a850 100644 --- a/src/libraries/Box2D/Dynamics/b2Island.cpp +++ b/src/libraries/Box2D/Dynamics/b2_island.cpp @@ -1,31 +1,36 @@ -/* -* 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 -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include +// 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_island.h" +#include "dynamics/b2_contact_solver.h" /* Position Correction Notes @@ -184,7 +189,7 @@ void b2Island::Solve(b2Profile* profile, const b2TimeStep& step, const b2Vec2& g { b2Timer timer; - float32 h = step.dt; + float h = step.dt; // Integrate velocities and apply damping. Initialize the body state. for (int32 i = 0; i < m_bodyCount; ++i) @@ -192,9 +197,9 @@ void b2Island::Solve(b2Profile* profile, const b2TimeStep& step, const b2Vec2& g b2Body* b = m_bodies[i]; b2Vec2 c = b->m_sweep.c; - float32 a = b->m_sweep.a; + float a = b->m_sweep.a; b2Vec2 v = b->m_linearVelocity; - float32 w = b->m_angularVelocity; + float w = b->m_angularVelocity; // Store positions for continuous collision. b->m_sweep.c0 = b->m_sweep.c; @@ -203,7 +208,7 @@ void b2Island::Solve(b2Profile* profile, const b2TimeStep& step, const b2Vec2& g if (b->m_type == b2_dynamicBody) { // Integrate velocities. - v += h * (b->m_gravityScale * gravity + b->m_invMass * b->m_force); + 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. @@ -275,22 +280,22 @@ void b2Island::Solve(b2Profile* profile, const b2TimeStep& step, const b2Vec2& g for (int32 i = 0; i < m_bodyCount; ++i) { b2Vec2 c = m_positions[i].c; - float32 a = m_positions[i].a; + float a = m_positions[i].a; b2Vec2 v = m_velocities[i].v; - float32 w = m_velocities[i].w; + float 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(); + float ratio = b2_maxTranslation / translation.Length(); v *= ratio; } - float32 rotation = h * w; + float rotation = h * w; if (rotation * rotation > b2_maxRotationSquared) { - float32 ratio = b2_maxRotation / b2Abs(rotation); + float ratio = b2_maxRotation / b2Abs(rotation); w *= ratio; } @@ -312,9 +317,9 @@ void b2Island::Solve(b2Profile* profile, const b2TimeStep& step, const b2Vec2& g bool contactsOkay = contactSolver.SolvePositionConstraints(); bool jointsOkay = true; - for (int32 i = 0; i < m_jointCount; ++i) + for (int32 j = 0; j < m_jointCount; ++j) { - bool jointOkay = m_joints[i]->SolvePositionConstraints(solverData); + bool jointOkay = m_joints[j]->SolvePositionConstraints(solverData); jointsOkay = jointsOkay && jointOkay; } @@ -343,10 +348,10 @@ void b2Island::Solve(b2Profile* profile, const b2TimeStep& step, const b2Vec2& g if (allowSleep) { - float32 minSleepTime = b2_maxFloat; + float minSleepTime = b2_maxFloat; - const float32 linTolSqr = b2_linearSleepTolerance * b2_linearSleepTolerance; - const float32 angTolSqr = b2_angularSleepTolerance * b2_angularSleepTolerance; + const float linTolSqr = b2_linearSleepTolerance * b2_linearSleepTolerance; + const float angTolSqr = b2_angularSleepTolerance * b2_angularSleepTolerance; for (int32 i = 0; i < m_bodyCount; ++i) { @@ -467,28 +472,28 @@ void b2Island::SolveTOI(const b2TimeStep& subStep, int32 toiIndexA, int32 toiInd // Don't store the TOI contact forces for warm starting // because they can be quite large. - float32 h = subStep.dt; + float 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; + float a = m_positions[i].a; b2Vec2 v = m_velocities[i].v; - float32 w = m_velocities[i].w; + float 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(); + float ratio = b2_maxTranslation / translation.Length(); v *= ratio; } - float32 rotation = h * w; + float rotation = h * w; if (rotation * rotation > b2_maxRotationSquared) { - float32 ratio = b2_maxRotation / b2Abs(rotation); + float ratio = b2_maxRotation / b2Abs(rotation); w *= ratio; } @@ -515,7 +520,7 @@ void b2Island::SolveTOI(const b2TimeStep& subStep, int32 toiIndexA, int32 toiInd void b2Island::Report(const b2ContactVelocityConstraint* constraints) { - if (m_listener == NULL) + if (m_listener == nullptr) { return; } diff --git a/src/libraries/Box2D/Dynamics/b2Island.h b/src/libraries/Box2D/Dynamics/b2_island.h similarity index 54% rename from src/libraries/Box2D/Dynamics/b2Island.h rename to src/libraries/Box2D/Dynamics/b2_island.h index 4a814c463..2e28a357b 100644 --- a/src/libraries/Box2D/Dynamics/b2Island.h +++ b/src/libraries/Box2D/Dynamics/b2_island.h @@ -1,27 +1,31 @@ -/* -* 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. -*/ +// 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 -#include -#include +#include "box2d/b2_body.h" +#include "box2d/b2_math.h" +#include "box2d/b2_time_step.h" class b2Contact; class b2Joint; diff --git a/src/libraries/Box2D/Dynamics/b2_joint.cpp b/src/libraries/Box2D/Dynamics/b2_joint.cpp new file mode 100644 index 000000000..1f8ac2644 --- /dev/null +++ b/src/libraries/Box2D/Dynamics/b2_joint.cpp @@ -0,0 +1,313 @@ +// 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_rope_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_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 = 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/src/libraries/Box2D/Dynamics/Joints/b2MotorJoint.cpp b/src/libraries/Box2D/Dynamics/b2_motor_joint.cpp similarity index 61% rename from src/libraries/Box2D/Dynamics/Joints/b2MotorJoint.cpp rename to src/libraries/Box2D/Dynamics/b2_motor_joint.cpp index e5bbcb165..6e0b07538 100644 --- a/src/libraries/Box2D/Dynamics/Joints/b2MotorJoint.cpp +++ b/src/libraries/Box2D/Dynamics/b2_motor_joint.cpp @@ -1,24 +1,28 @@ -/* -* 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. -*/ +// MIT License -#include -#include -#include +// 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 @@ -26,6 +30,9 @@ // 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 @@ -39,8 +46,8 @@ void b2MotorJointDef::Initialize(b2Body* bA, b2Body* bB) b2Vec2 xB = bodyB->GetPosition(); linearOffset = bodyA->GetLocalPoint(xB); - float32 angleA = bodyA->GetAngle(); - float32 angleB = bodyB->GetAngle(); + float angleA = bodyA->GetAngle(); + float angleB = bodyB->GetAngle(); angularOffset = angleB - angleA; } @@ -70,23 +77,22 @@ void b2MotorJoint::InitVelocityConstraints(const b2SolverData& data) m_invIB = m_bodyB->m_invI; b2Vec2 cA = data.positions[m_indexA].c; - float32 aA = data.positions[m_indexA].a; + float aA = data.positions[m_indexA].a; b2Vec2 vA = data.velocities[m_indexA].v; - float32 wA = data.velocities[m_indexA].w; + float wA = data.velocities[m_indexA].w; b2Vec2 cB = data.positions[m_indexB].c; - float32 aB = data.positions[m_indexB].a; + float aB = data.positions[m_indexB].a; b2Vec2 vB = data.velocities[m_indexB].v; - float32 wB = data.velocities[m_indexB].w; + float wB = data.velocities[m_indexB].w; b2Rot qA(aA), qB(aB); // Compute the effective mass matrix. - m_rA = b2Mul(qA, -m_localCenterA); + m_rA = b2Mul(qA, m_linearOffset - m_localCenterA); m_rB = b2Mul(qB, -m_localCenterB); // J = [-I -r1_skew I r2_skew] - // [ 0 -1 0 1] // r_skew = [-ry; rx] // Matlab @@ -94,9 +100,10 @@ void b2MotorJoint::InitVelocityConstraints(const b2SolverData& data) // [ -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; + 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; @@ -111,7 +118,7 @@ void b2MotorJoint::InitVelocityConstraints(const b2SolverData& data) m_angularMass = 1.0f / m_angularMass; } - m_linearError = cB + m_rB - cA - m_rA - b2Mul(qA, m_linearOffset); + m_linearError = cB + m_rB - cA - m_rA; m_angularError = aB - aA - m_angularOffset; if (data.step.warmStarting) @@ -141,23 +148,23 @@ void b2MotorJoint::InitVelocityConstraints(const b2SolverData& data) void b2MotorJoint::SolveVelocityConstraints(const b2SolverData& data) { b2Vec2 vA = data.velocities[m_indexA].v; - float32 wA = data.velocities[m_indexA].w; + float wA = data.velocities[m_indexA].w; b2Vec2 vB = data.velocities[m_indexB].v; - float32 wB = data.velocities[m_indexB].w; + float wB = data.velocities[m_indexB].w; - float32 mA = m_invMassA, mB = m_invMassB; - float32 iA = m_invIA, iB = m_invIB; + float mA = m_invMassA, mB = m_invMassB; + float iA = m_invIA, iB = m_invIB; - float32 h = data.step.dt; - float32 inv_h = data.step.inv_dt; + float h = data.step.dt; + float inv_h = data.step.inv_dt; // Solve angular friction { - float32 Cdot = wB - wA + inv_h * m_correctionFactor * m_angularError; - float32 impulse = -m_angularMass * Cdot; + float Cdot = wB - wA + inv_h * m_correctionFactor * m_angularError; + float impulse = -m_angularMass * Cdot; - float32 oldImpulse = m_angularImpulse; - float32 maxImpulse = h * m_maxTorque; + float oldImpulse = m_angularImpulse; + float maxImpulse = h * m_maxTorque; m_angularImpulse = b2Clamp(m_angularImpulse + impulse, -maxImpulse, maxImpulse); impulse = m_angularImpulse - oldImpulse; @@ -173,7 +180,7 @@ void b2MotorJoint::SolveVelocityConstraints(const b2SolverData& data) b2Vec2 oldImpulse = m_linearImpulse; m_linearImpulse += impulse; - float32 maxImpulse = h * m_maxForce; + float maxImpulse = h * m_maxForce; if (m_linearImpulse.LengthSquared() > maxImpulse * maxImpulse) { @@ -213,45 +220,45 @@ b2Vec2 b2MotorJoint::GetAnchorB() const return m_bodyB->GetPosition(); } -b2Vec2 b2MotorJoint::GetReactionForce(float32 inv_dt) const +b2Vec2 b2MotorJoint::GetReactionForce(float inv_dt) const { return inv_dt * m_linearImpulse; } -float32 b2MotorJoint::GetReactionTorque(float32 inv_dt) const +float b2MotorJoint::GetReactionTorque(float inv_dt) const { return inv_dt * m_angularImpulse; } -void b2MotorJoint::SetMaxForce(float32 force) +void b2MotorJoint::SetMaxForce(float force) { b2Assert(b2IsValid(force) && force >= 0.0f); m_maxForce = force; } -float32 b2MotorJoint::GetMaxForce() const +float b2MotorJoint::GetMaxForce() const { return m_maxForce; } -void b2MotorJoint::SetMaxTorque(float32 torque) +void b2MotorJoint::SetMaxTorque(float torque) { b2Assert(b2IsValid(torque) && torque >= 0.0f); m_maxTorque = torque; } -float32 b2MotorJoint::GetMaxTorque() const +float b2MotorJoint::GetMaxTorque() const { return m_maxTorque; } -void b2MotorJoint::SetCorrectionFactor(float32 factor) +void b2MotorJoint::SetCorrectionFactor(float factor) { b2Assert(b2IsValid(factor) && 0.0f <= factor && factor <= 1.0f); m_correctionFactor = factor; } -float32 b2MotorJoint::GetCorrectionFactor() const +float b2MotorJoint::GetCorrectionFactor() const { return m_correctionFactor; } @@ -271,7 +278,7 @@ const b2Vec2& b2MotorJoint::GetLinearOffset() const return m_linearOffset; } -void b2MotorJoint::SetAngularOffset(float32 angularOffset) +void b2MotorJoint::SetAngularOffset(float angularOffset) { if (angularOffset != m_angularOffset) { @@ -281,7 +288,7 @@ void b2MotorJoint::SetAngularOffset(float32 angularOffset) } } -float32 b2MotorJoint::GetAngularOffset() const +float b2MotorJoint::GetAngularOffset() const { return m_angularOffset; } @@ -291,14 +298,14 @@ 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); + 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/src/libraries/Box2D/Dynamics/Joints/b2MouseJoint.cpp b/src/libraries/Box2D/Dynamics/b2_mouse_joint.cpp similarity index 56% rename from src/libraries/Box2D/Dynamics/Joints/b2MouseJoint.cpp rename to src/libraries/Box2D/Dynamics/b2_mouse_joint.cpp index ea96edde9..1ea6dd420 100644 --- a/src/libraries/Box2D/Dynamics/Joints/b2MouseJoint.cpp +++ b/src/libraries/Box2D/Dynamics/b2_mouse_joint.cpp @@ -1,24 +1,28 @@ -/* -* 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. -*/ +// MIT License -#include -#include -#include +// 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 @@ -31,31 +35,24 @@ 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_stiffness = def->stiffness; + m_damping = def->damping; + 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) + if (target != m_targetA) { m_bodyB->SetAwake(true); + m_targetA = target; } - m_targetA = target; } const b2Vec2& b2MouseJoint::GetTarget() const @@ -63,36 +60,16 @@ const b2Vec2& b2MouseJoint::GetTarget() const return m_targetA; } -void b2MouseJoint::SetMaxForce(float32 force) +void b2MouseJoint::SetMaxForce(float force) { m_maxForce = force; } -float32 b2MouseJoint::GetMaxForce() const +float 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; @@ -101,28 +78,21 @@ void b2MouseJoint::InitVelocityConstraints(const b2SolverData& data) m_invIB = m_bodyB->m_invI; b2Vec2 cB = data.positions[m_indexB].c; - float32 aB = data.positions[m_indexB].a; + float aB = data.positions[m_indexB].a; b2Vec2 vB = data.velocities[m_indexB].v; - float32 wB = data.velocities[m_indexB].w; + float wB = data.velocities[m_indexB].w; b2Rot qB(aB); - float32 mass = m_bodyB->GetMass(); + float 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); + float d = m_damping; + float k = m_stiffness; // 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); + float h = data.step.dt; m_gamma = h * (d + h * k); if (m_gamma != 0.0f) { @@ -168,7 +138,7 @@ void b2MouseJoint::InitVelocityConstraints(const b2SolverData& data) void b2MouseJoint::SolveVelocityConstraints(const b2SolverData& data) { b2Vec2 vB = data.velocities[m_indexB].v; - float32 wB = data.velocities[m_indexB].w; + float wB = data.velocities[m_indexB].w; // Cdot = v + cross(w, r) b2Vec2 Cdot = vB + b2Cross(wB, m_rB); @@ -176,7 +146,7 @@ void b2MouseJoint::SolveVelocityConstraints(const b2SolverData& data) b2Vec2 oldImpulse = m_impulse; m_impulse += impulse; - float32 maxImpulse = data.step.dt * m_maxForce; + float maxImpulse = data.step.dt * m_maxForce; if (m_impulse.LengthSquared() > maxImpulse * maxImpulse) { m_impulse *= maxImpulse / m_impulse.Length(); @@ -206,12 +176,12 @@ b2Vec2 b2MouseJoint::GetAnchorB() const return m_bodyB->GetWorldPoint(m_localAnchorB); } -b2Vec2 b2MouseJoint::GetReactionForce(float32 inv_dt) const +b2Vec2 b2MouseJoint::GetReactionForce(float inv_dt) const { return inv_dt * m_impulse; } -float32 b2MouseJoint::GetReactionTorque(float32 inv_dt) const +float b2MouseJoint::GetReactionTorque(float inv_dt) const { return inv_dt * 0.0f; } diff --git a/src/libraries/Box2D/Dynamics/b2_polygon_circle_contact.cpp b/src/libraries/Box2D/Dynamics/b2_polygon_circle_contact.cpp new file mode 100644 index 000000000..e4f34f582 --- /dev/null +++ b/src/libraries/Box2D/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/src/libraries/Box2D/Dynamics/b2_polygon_circle_contact.h b/src/libraries/Box2D/Dynamics/b2_polygon_circle_contact.h new file mode 100644 index 000000000..6ae542510 --- /dev/null +++ b/src/libraries/Box2D/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/src/libraries/Box2D/Dynamics/b2_polygon_contact.cpp b/src/libraries/Box2D/Dynamics/b2_polygon_contact.cpp new file mode 100644 index 000000000..e92a9e806 --- /dev/null +++ b/src/libraries/Box2D/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/src/libraries/Box2D/Dynamics/b2_polygon_contact.h b/src/libraries/Box2D/Dynamics/b2_polygon_contact.h new file mode 100644 index 000000000..0516cb0ec --- /dev/null +++ b/src/libraries/Box2D/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/src/libraries/Box2D/Dynamics/b2_prismatic_joint.cpp b/src/libraries/Box2D/Dynamics/b2_prismatic_joint.cpp new file mode 100644 index 000000000..cb550170f --- /dev/null +++ b/src/libraries/Box2D/Dynamics/b2_prismatic_joint.cpp @@ -0,0 +1,644 @@ +// 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/src/libraries/Box2D/Dynamics/Joints/b2PulleyJoint.cpp b/src/libraries/Box2D/Dynamics/b2_pulley_joint.cpp similarity index 62% rename from src/libraries/Box2D/Dynamics/Joints/b2PulleyJoint.cpp rename to src/libraries/Box2D/Dynamics/b2_pulley_joint.cpp index 9cd340f1b..099e57e49 100644 --- a/src/libraries/Box2D/Dynamics/Joints/b2PulleyJoint.cpp +++ b/src/libraries/Box2D/Dynamics/b2_pulley_joint.cpp @@ -1,24 +1,28 @@ -/* -* 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. -*/ +// MIT License -#include -#include -#include +// 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) @@ -35,7 +39,7 @@ void b2PulleyJointDef::Initialize(b2Body* bA, b2Body* bB, const b2Vec2& groundA, const b2Vec2& groundB, const b2Vec2& anchorA, const b2Vec2& anchorB, - float32 r) + float r) { bodyA = bA; bodyB = bB; @@ -82,14 +86,14 @@ void b2PulleyJoint::InitVelocityConstraints(const b2SolverData& data) m_invIB = m_bodyB->m_invI; b2Vec2 cA = data.positions[m_indexA].c; - float32 aA = data.positions[m_indexA].a; + float aA = data.positions[m_indexA].a; b2Vec2 vA = data.velocities[m_indexA].v; - float32 wA = data.velocities[m_indexA].w; + float wA = data.velocities[m_indexA].w; b2Vec2 cB = data.positions[m_indexB].c; - float32 aB = data.positions[m_indexB].a; + float aB = data.positions[m_indexB].a; b2Vec2 vB = data.velocities[m_indexB].v; - float32 wB = data.velocities[m_indexB].w; + float wB = data.velocities[m_indexB].w; b2Rot qA(aA), qB(aB); @@ -100,8 +104,8 @@ void b2PulleyJoint::InitVelocityConstraints(const b2SolverData& data) m_uA = cA + m_rA - m_groundAnchorA; m_uB = cB + m_rB - m_groundAnchorB; - float32 lengthA = m_uA.Length(); - float32 lengthB = m_uB.Length(); + float lengthA = m_uA.Length(); + float lengthB = m_uB.Length(); if (lengthA > 10.0f * b2_linearSlop) { @@ -122,11 +126,11 @@ void b2PulleyJoint::InitVelocityConstraints(const b2SolverData& data) } // Compute effective mass. - float32 ruA = b2Cross(m_rA, m_uA); - float32 ruB = b2Cross(m_rB, m_uB); + float ruA = b2Cross(m_rA, m_uA); + float ruB = b2Cross(m_rB, m_uB); - float32 mA = m_invMassA + m_invIA * ruA * ruA; - float32 mB = m_invMassB + m_invIB * ruB * ruB; + float mA = m_invMassA + m_invIA * ruA * ruA; + float mB = m_invMassB + m_invIB * ruB * ruB; m_mass = mA + m_ratio * m_ratio * mB; @@ -163,15 +167,15 @@ void b2PulleyJoint::InitVelocityConstraints(const b2SolverData& data) void b2PulleyJoint::SolveVelocityConstraints(const b2SolverData& data) { b2Vec2 vA = data.velocities[m_indexA].v; - float32 wA = data.velocities[m_indexA].w; + float wA = data.velocities[m_indexA].w; b2Vec2 vB = data.velocities[m_indexB].v; - float32 wB = data.velocities[m_indexB].w; + float 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; + 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; @@ -190,9 +194,9 @@ void b2PulleyJoint::SolveVelocityConstraints(const b2SolverData& data) bool b2PulleyJoint::SolvePositionConstraints(const b2SolverData& data) { b2Vec2 cA = data.positions[m_indexA].c; - float32 aA = data.positions[m_indexA].a; + float aA = data.positions[m_indexA].a; b2Vec2 cB = data.positions[m_indexB].c; - float32 aB = data.positions[m_indexB].a; + float aB = data.positions[m_indexB].a; b2Rot qA(aA), qB(aB); @@ -203,8 +207,8 @@ bool b2PulleyJoint::SolvePositionConstraints(const b2SolverData& data) b2Vec2 uA = cA + rA - m_groundAnchorA; b2Vec2 uB = cB + rB - m_groundAnchorB; - float32 lengthA = uA.Length(); - float32 lengthB = uB.Length(); + float lengthA = uA.Length(); + float lengthB = uB.Length(); if (lengthA > 10.0f * b2_linearSlop) { @@ -225,23 +229,23 @@ bool b2PulleyJoint::SolvePositionConstraints(const b2SolverData& data) } // Compute effective mass. - float32 ruA = b2Cross(rA, uA); - float32 ruB = b2Cross(rB, uB); + float ruA = b2Cross(rA, uA); + float ruB = b2Cross(rB, uB); - float32 mA = m_invMassA + m_invIA * ruA * ruA; - float32 mB = m_invMassB + m_invIB * ruB * ruB; + float mA = m_invMassA + m_invIA * ruA * ruA; + float mB = m_invMassB + m_invIB * ruB * ruB; - float32 mass = mA + m_ratio * m_ratio * mB; + float 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); + float C = m_constant - lengthA - m_ratio * lengthB; + float linearError = b2Abs(C); - float32 impulse = -mass * C; + float impulse = -mass * C; b2Vec2 PA = -impulse * uA; b2Vec2 PB = -m_ratio * impulse * uB; @@ -269,13 +273,13 @@ b2Vec2 b2PulleyJoint::GetAnchorB() const return m_bodyB->GetWorldPoint(m_localAnchorB); } -b2Vec2 b2PulleyJoint::GetReactionForce(float32 inv_dt) const +b2Vec2 b2PulleyJoint::GetReactionForce(float inv_dt) const { b2Vec2 P = m_impulse * m_uB; return inv_dt * P; } -float32 b2PulleyJoint::GetReactionTorque(float32 inv_dt) const +float b2PulleyJoint::GetReactionTorque(float inv_dt) const { B2_NOT_USED(inv_dt); return 0.0f; @@ -291,22 +295,22 @@ b2Vec2 b2PulleyJoint::GetGroundAnchorB() const return m_groundAnchorB; } -float32 b2PulleyJoint::GetLengthA() const +float b2PulleyJoint::GetLengthA() const { return m_lengthA; } -float32 b2PulleyJoint::GetLengthB() const +float b2PulleyJoint::GetLengthB() const { return m_lengthB; } -float32 b2PulleyJoint::GetRatio() const +float b2PulleyJoint::GetRatio() const { return m_ratio; } -float32 b2PulleyJoint::GetCurrentLengthA() const +float b2PulleyJoint::GetCurrentLengthA() const { b2Vec2 p = m_bodyA->GetWorldPoint(m_localAnchorA); b2Vec2 s = m_groundAnchorA; @@ -314,7 +318,7 @@ float32 b2PulleyJoint::GetCurrentLengthA() const return d.Length(); } -float32 b2PulleyJoint::GetCurrentLengthB() const +float b2PulleyJoint::GetCurrentLengthB() const { b2Vec2 p = m_bodyB->GetWorldPoint(m_localAnchorB); b2Vec2 s = m_groundAnchorB; @@ -327,18 +331,18 @@ 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); + 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) diff --git a/src/libraries/Box2D/Dynamics/b2_revolute_joint.cpp b/src/libraries/Box2D/Dynamics/b2_revolute_joint.cpp new file mode 100644 index 000000000..35162b084 --- /dev/null +++ b/src/libraries/Box2D/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_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/src/libraries/Box2D/Dynamics/Joints/b2RopeJoint.cpp b/src/libraries/Box2D/Dynamics/b2_rope_joint.cpp similarity index 57% rename from src/libraries/Box2D/Dynamics/Joints/b2RopeJoint.cpp rename to src/libraries/Box2D/Dynamics/b2_rope_joint.cpp index 952ee7a3e..ebe800f82 100644 --- a/src/libraries/Box2D/Dynamics/Joints/b2RopeJoint.cpp +++ b/src/libraries/Box2D/Dynamics/b2_rope_joint.cpp @@ -1,24 +1,28 @@ -/* -* 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. -*/ +// MIT License -#include -#include -#include +// 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_rope_joint.h" +#include "box2d/b2_time_step.h" // Limit: @@ -39,7 +43,6 @@ b2RopeJoint::b2RopeJoint(const b2RopeJointDef* def) m_mass = 0.0f; m_impulse = 0.0f; - m_state = e_inactiveLimit; m_length = 0.0f; } @@ -55,14 +58,14 @@ void b2RopeJoint::InitVelocityConstraints(const b2SolverData& data) m_invIB = m_bodyB->m_invI; b2Vec2 cA = data.positions[m_indexA].c; - float32 aA = data.positions[m_indexA].a; + float aA = data.positions[m_indexA].a; b2Vec2 vA = data.velocities[m_indexA].v; - float32 wA = data.velocities[m_indexA].w; + float wA = data.velocities[m_indexA].w; b2Vec2 cB = data.positions[m_indexB].c; - float32 aB = data.positions[m_indexB].a; + float aB = data.positions[m_indexB].a; b2Vec2 vB = data.velocities[m_indexB].v; - float32 wB = data.velocities[m_indexB].w; + float wB = data.velocities[m_indexB].w; b2Rot qA(aA), qB(aB); @@ -72,16 +75,6 @@ void b2RopeJoint::InitVelocityConstraints(const b2SolverData& data) 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; @@ -95,9 +88,9 @@ void b2RopeJoint::InitVelocityConstraints(const b2SolverData& data) } // 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; + float crA = b2Cross(m_rA, m_u); + float crB = b2Cross(m_rB, m_u); + float invMass = m_invMassA + m_invIA * crA * crA + m_invMassB + m_invIB * crB * crB; m_mass = invMass != 0.0f ? 1.0f / invMass : 0.0f; @@ -126,15 +119,15 @@ void b2RopeJoint::InitVelocityConstraints(const b2SolverData& data) void b2RopeJoint::SolveVelocityConstraints(const b2SolverData& data) { b2Vec2 vA = data.velocities[m_indexA].v; - float32 wA = data.velocities[m_indexA].w; + float wA = data.velocities[m_indexA].w; b2Vec2 vB = data.velocities[m_indexB].v; - float32 wB = data.velocities[m_indexB].w; + float 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); + float C = m_length - m_maxLength; + float Cdot = b2Dot(m_u, vpB - vpA); // Predictive constraint. if (C < 0.0f) @@ -142,8 +135,8 @@ void b2RopeJoint::SolveVelocityConstraints(const b2SolverData& data) Cdot += data.step.inv_dt * C; } - float32 impulse = -m_mass * Cdot; - float32 oldImpulse = m_impulse; + float impulse = -m_mass * Cdot; + float oldImpulse = m_impulse; m_impulse = b2Min(0.0f, m_impulse + impulse); impulse = m_impulse - oldImpulse; @@ -162,9 +155,9 @@ void b2RopeJoint::SolveVelocityConstraints(const b2SolverData& data) bool b2RopeJoint::SolvePositionConstraints(const b2SolverData& data) { b2Vec2 cA = data.positions[m_indexA].c; - float32 aA = data.positions[m_indexA].a; + float aA = data.positions[m_indexA].a; b2Vec2 cB = data.positions[m_indexB].c; - float32 aB = data.positions[m_indexB].a; + float aB = data.positions[m_indexB].a; b2Rot qA(aA), qB(aB); @@ -172,12 +165,12 @@ bool b2RopeJoint::SolvePositionConstraints(const b2SolverData& data) b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_localCenterB); b2Vec2 u = cB + rB - cA - rA; - float32 length = u.Normalize(); - float32 C = length - m_maxLength; + m_length = u.Normalize(); + float C = m_length - m_maxLength; C = b2Clamp(C, 0.0f, b2_maxLinearCorrection); - float32 impulse = -m_mass * C; + float impulse = -m_mass * C; b2Vec2 P = impulse * u; cA -= m_invMassA * P; @@ -190,7 +183,7 @@ bool b2RopeJoint::SolvePositionConstraints(const b2SolverData& data) data.positions[m_indexB].c = cB; data.positions[m_indexB].a = aB; - return length - m_maxLength < b2_linearSlop; + return m_length - m_maxLength < b2_linearSlop; } b2Vec2 b2RopeJoint::GetAnchorA() const @@ -203,26 +196,26 @@ b2Vec2 b2RopeJoint::GetAnchorB() const return m_bodyB->GetWorldPoint(m_localAnchorB); } -b2Vec2 b2RopeJoint::GetReactionForce(float32 inv_dt) const +b2Vec2 b2RopeJoint::GetReactionForce(float inv_dt) const { b2Vec2 F = (inv_dt * m_impulse) * m_u; return F; } -float32 b2RopeJoint::GetReactionTorque(float32 inv_dt) const +float b2RopeJoint::GetReactionTorque(float inv_dt) const { B2_NOT_USED(inv_dt); return 0.0f; } -float32 b2RopeJoint::GetMaxLength() const +float b2RopeJoint::GetMaxLength() const { return m_maxLength; } -b2LimitState b2RopeJoint::GetLimitState() const +float b2RopeJoint::GetLength() const { - return m_state; + return m_length; } void b2RopeJoint::Dump() @@ -230,12 +223,12 @@ 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); + b2Dump(" b2RopeJointDef 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.maxLength = %.9g;\n", m_maxLength); + b2Dump(" joints[%d] = m_world->CreateJoint(&jd);\n", m_index); } diff --git a/src/libraries/Box2D/Dynamics/Joints/b2WeldJoint.cpp b/src/libraries/Box2D/Dynamics/b2_weld_joint.cpp similarity index 66% rename from src/libraries/Box2D/Dynamics/Joints/b2WeldJoint.cpp rename to src/libraries/Box2D/Dynamics/b2_weld_joint.cpp index ffdd386b1..df3ee0a3e 100644 --- a/src/libraries/Box2D/Dynamics/Joints/b2WeldJoint.cpp +++ b/src/libraries/Box2D/Dynamics/b2_weld_joint.cpp @@ -1,24 +1,28 @@ -/* -* 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 -#include -#include -#include +// 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 @@ -49,8 +53,8 @@ b2WeldJoint::b2WeldJoint(const b2WeldJointDef* def) m_localAnchorA = def->localAnchorA; m_localAnchorB = def->localAnchorB; m_referenceAngle = def->referenceAngle; - m_frequencyHz = def->frequencyHz; - m_dampingRatio = def->dampingRatio; + m_stiffness = def->stiffness; + m_damping = def->damping; m_impulse.SetZero(); } @@ -66,13 +70,13 @@ void b2WeldJoint::InitVelocityConstraints(const b2SolverData& data) m_invIA = m_bodyA->m_invI; m_invIB = m_bodyB->m_invI; - float32 aA = data.positions[m_indexA].a; + float aA = data.positions[m_indexA].a; b2Vec2 vA = data.velocities[m_indexA].v; - float32 wA = data.velocities[m_indexA].w; + float wA = data.velocities[m_indexA].w; - float32 aB = data.positions[m_indexB].a; + float aB = data.positions[m_indexB].a; b2Vec2 vB = data.velocities[m_indexB].v; - float32 wB = data.velocities[m_indexB].w; + float wB = data.velocities[m_indexB].w; b2Rot qA(aA), qB(aB); @@ -88,8 +92,8 @@ void b2WeldJoint::InitVelocityConstraints(const b2SolverData& data) // [ -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; + 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; @@ -102,26 +106,22 @@ void b2WeldJoint::InitVelocityConstraints(const b2SolverData& data) K.ey.z = K.ez.y; K.ez.z = iA + iB; - if (m_frequencyHz > 0.0f) + if (m_stiffness > 0.0f) { K.GetInverse22(&m_mass); - float32 invM = iA + iB; - float32 m = invM > 0.0f ? 1.0f / invM : 0.0f; + float invM = iA + iB; - float32 C = aB - aA - m_referenceAngle; - - // Frequency - float32 omega = 2.0f * b2_pi * m_frequencyHz; + float C = aB - aA - m_referenceAngle; // Damping coefficient - float32 d = 2.0f * m * m_dampingRatio * omega; + float d = m_damping; // Spring stiffness - float32 k = m * omega * omega; + float k = m_stiffness; // magic formulas - float32 h = data.step.dt; + 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; @@ -169,18 +169,18 @@ void b2WeldJoint::InitVelocityConstraints(const b2SolverData& data) void b2WeldJoint::SolveVelocityConstraints(const b2SolverData& data) { b2Vec2 vA = data.velocities[m_indexA].v; - float32 wA = data.velocities[m_indexA].w; + float wA = data.velocities[m_indexA].w; b2Vec2 vB = data.velocities[m_indexB].v; - float32 wB = data.velocities[m_indexB].w; + float wB = data.velocities[m_indexB].w; - float32 mA = m_invMassA, mB = m_invMassB; - float32 iA = m_invIA, iB = m_invIB; + float mA = m_invMassA, mB = m_invMassB; + float iA = m_invIA, iB = m_invIB; - if (m_frequencyHz > 0.0f) + if (m_stiffness > 0.0f) { - float32 Cdot2 = wB - wA; + float Cdot2 = wB - wA; - float32 impulse2 = -m_mass.ez.z * (Cdot2 + m_bias + m_gamma * m_impulse.z); + float impulse2 = -m_mass.ez.z * (Cdot2 + m_bias + m_gamma * m_impulse.z); m_impulse.z += impulse2; wA -= iA * impulse2; @@ -203,7 +203,7 @@ void b2WeldJoint::SolveVelocityConstraints(const b2SolverData& data) else { b2Vec2 Cdot1 = vB + b2Cross(wB, m_rB) - vA - b2Cross(wA, m_rA); - float32 Cdot2 = wB - wA; + float Cdot2 = wB - wA; b2Vec3 Cdot(Cdot1.x, Cdot1.y, Cdot2); b2Vec3 impulse = -b2Mul(m_mass, Cdot); @@ -227,19 +227,19 @@ void b2WeldJoint::SolveVelocityConstraints(const b2SolverData& data) bool b2WeldJoint::SolvePositionConstraints(const b2SolverData& data) { b2Vec2 cA = data.positions[m_indexA].c; - float32 aA = data.positions[m_indexA].a; + float aA = data.positions[m_indexA].a; b2Vec2 cB = data.positions[m_indexB].c; - float32 aB = data.positions[m_indexB].a; + float 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; + 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); - float32 positionError, angularError; + float positionError, angularError; b2Mat33 K; K.ex.x = mA + mB + rA.y * rA.y * iA + rB.y * rB.y * iB; @@ -252,7 +252,7 @@ bool b2WeldJoint::SolvePositionConstraints(const b2SolverData& data) K.ey.z = K.ez.y; K.ez.z = iA + iB; - if (m_frequencyHz > 0.0f) + if (m_stiffness > 0.0f) { b2Vec2 C1 = cB + rB - cA - rA; @@ -270,7 +270,7 @@ bool b2WeldJoint::SolvePositionConstraints(const b2SolverData& data) else { b2Vec2 C1 = cB + rB - cA - rA; - float32 C2 = aB - aA - m_referenceAngle; + float C2 = aB - aA - m_referenceAngle; positionError = C1.Length(); angularError = b2Abs(C2); @@ -315,13 +315,13 @@ b2Vec2 b2WeldJoint::GetAnchorB() const return m_bodyB->GetWorldPoint(m_localAnchorB); } -b2Vec2 b2WeldJoint::GetReactionForce(float32 inv_dt) const +b2Vec2 b2WeldJoint::GetReactionForce(float inv_dt) const { b2Vec2 P(m_impulse.x, m_impulse.y); return inv_dt * P; } -float32 b2WeldJoint::GetReactionTorque(float32 inv_dt) const +float b2WeldJoint::GetReactionTorque(float inv_dt) const { return inv_dt * m_impulse.z; } @@ -331,14 +331,14 @@ 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); + 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/src/libraries/Box2D/Dynamics/b2_wheel_joint.cpp b/src/libraries/Box2D/Dynamics/b2_wheel_joint.cpp new file mode 100644 index 000000000..3e76678bf --- /dev/null +++ b/src/libraries/Box2D/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_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/src/libraries/Box2D/Dynamics/b2World.cpp b/src/libraries/Box2D/Dynamics/b2_world.cpp similarity index 82% rename from src/libraries/Box2D/Dynamics/b2World.cpp rename to src/libraries/Box2D/Dynamics/b2_world.cpp index c6cf2957b..1a0f79131 100644 --- a/src/libraries/Box2D/Dynamics/b2World.cpp +++ b/src/libraries/Box2D/Dynamics/b2_world.cpp @@ -1,46 +1,52 @@ -/* -* 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. + +#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 -#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_destructionListener = nullptr; + m_debugDraw = nullptr; - m_bodyList = NULL; - m_jointList = NULL; + m_bodyList = nullptr; + m_jointList = nullptr; m_bodyCount = 0; m_jointCount = 0; @@ -54,7 +60,9 @@ b2World::b2World(const b2Vec2& gravity) m_allowSleep = true; m_gravity = gravity; - m_flags = e_clearForces; + m_newContacts = false; + m_locked = false; + m_clearForces = true; m_inv_dt0 = 0.0f; @@ -101,7 +109,7 @@ void b2World::SetContactListener(b2ContactListener* listener) void b2World::SetDebugDraw(b2Draw* debugDraw) { - g_debugDraw = debugDraw; + m_debugDraw = debugDraw; } b2Body* b2World::CreateBody(const b2BodyDef* def) @@ -109,14 +117,14 @@ b2Body* b2World::CreateBody(const b2BodyDef* def) b2Assert(IsLocked() == false); if (IsLocked()) { - return NULL; + 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 = NULL; + b->m_prev = nullptr; b->m_next = m_bodyList; if (m_bodyList) { @@ -153,7 +161,7 @@ void b2World::DestroyBody(b2Body* b) b->m_jointList = je; } - b->m_jointList = NULL; + b->m_jointList = nullptr; // Delete the attached contacts. b2ContactEdge* ce = b->m_contactList; @@ -163,7 +171,7 @@ void b2World::DestroyBody(b2Body* b) ce = ce->next; m_contactManager.Destroy(ce0->contact); } - b->m_contactList = NULL; + b->m_contactList = nullptr; // Delete the attached fixtures. This destroys broad-phase proxies. b2Fixture* f = b->m_fixtureList; @@ -185,7 +193,7 @@ void b2World::DestroyBody(b2Body* b) b->m_fixtureList = f; b->m_fixtureCount -= 1; } - b->m_fixtureList = NULL; + b->m_fixtureList = nullptr; b->m_fixtureCount = 0; // Remove world body list. @@ -214,13 +222,13 @@ b2Joint* b2World::CreateJoint(const b2JointDef* def) b2Assert(IsLocked() == false); if (IsLocked()) { - return NULL; + return nullptr; } b2Joint* j = b2Joint::Create(def, &m_blockAllocator); // Connect to the world list. - j->m_prev = NULL; + j->m_prev = nullptr; j->m_next = m_jointList; if (m_jointList) { @@ -232,14 +240,14 @@ b2Joint* b2World::CreateJoint(const b2JointDef* def) // 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.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 = NULL; + 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; @@ -319,8 +327,8 @@ void b2World::DestroyJoint(b2Joint* j) bodyA->m_jointList = j->m_edgeA.next; } - j->m_edgeA.prev = NULL; - j->m_edgeA.next = NULL; + j->m_edgeA.prev = nullptr; + j->m_edgeA.next = nullptr; // Remove from body 2 if (j->m_edgeB.prev) @@ -338,8 +346,8 @@ void b2World::DestroyJoint(b2Joint* j) bodyB->m_jointList = j->m_edgeB.next; } - j->m_edgeB.prev = NULL; - j->m_edgeB.next = NULL; + j->m_edgeB.prev = nullptr; + j->m_edgeB.next = nullptr; b2Joint::Destroy(j, &m_blockAllocator); @@ -420,7 +428,7 @@ void b2World::Solve(const b2TimeStep& step) continue; } - if (seed->IsAwake() == false || seed->IsActive() == false) + if (seed->IsAwake() == false || seed->IsEnabled() == false) { continue; } @@ -442,12 +450,9 @@ void b2World::Solve(const b2TimeStep& step) { // Grab the next body off the stack and add it to the island. b2Body* b = stack[--stackCount]; - b2Assert(b->IsActive() == true); + b2Assert(b->IsEnabled() == 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) @@ -455,6 +460,9 @@ void b2World::Solve(const b2TimeStep& step) 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) { @@ -507,8 +515,8 @@ void b2World::Solve(const b2TimeStep& step) b2Body* other = je->other; - // Don't simulate joints connected to inactive bodies. - if (other->IsActive() == false) + // Don't simulate joints connected to diabled bodies. + if (other->IsEnabled() == false) { continue; } @@ -599,8 +607,8 @@ void b2World::SolveTOI(const b2TimeStep& step) for (;;) { // Find the first TOI. - b2Contact* minContact = NULL; - float32 minAlpha = 1.0f; + b2Contact* minContact = nullptr; + float minAlpha = 1.0f; for (b2Contact* c = m_contactManager.m_contactList; c; c = c->m_next) { @@ -616,7 +624,7 @@ void b2World::SolveTOI(const b2TimeStep& step) continue; } - float32 alpha = 1.0f; + float alpha = 1.0f; if (c->m_flags & b2Contact::e_toiFlag) { // This contact has a valid cached TOI. @@ -660,7 +668,7 @@ void b2World::SolveTOI(const b2TimeStep& step) // Compute the TOI for this contact. // Put the sweeps onto the same time interval. - float32 alpha0 = bA->m_sweep.alpha0; + float alpha0 = bA->m_sweep.alpha0; if (bA->m_sweep.alpha0 < bB->m_sweep.alpha0) { @@ -690,7 +698,7 @@ void b2World::SolveTOI(const b2TimeStep& step) b2TimeOfImpact(&output, &input); // Beta is the fraction of the remaining portion of the . - float32 beta = output.t; + float beta = output.t; if (output.state == b2TOIOutput::e_touching) { alpha = b2Min(alpha0 + (1.0f - alpha0) * beta, 1.0f); @@ -712,7 +720,7 @@ void b2World::SolveTOI(const b2TimeStep& step) } } - if (minContact == NULL || 1.0f - 10.0f * b2_epsilon < minAlpha) + if (minContact == nullptr || 1.0f - 10.0f * b2_epsilon < minAlpha) { // No more TOI events. Done! m_stepComplete = true; @@ -894,18 +902,18 @@ void b2World::SolveTOI(const b2TimeStep& step) } } -void b2World::Step(float32 dt, int32 velocityIterations, int32 positionIterations) +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_flags & e_newFixture) + if (m_newContacts) { m_contactManager.FindNewContacts(); - m_flags &= ~e_newFixture; + m_newContacts = false; } - m_flags |= e_locked; + m_locked = true; b2TimeStep step; step.dt = dt; @@ -952,12 +960,12 @@ void b2World::Step(float32 dt, int32 velocityIterations, int32 positionIteration m_inv_dt0 = step.inv_dt; } - if (m_flags & e_clearForces) + if (m_clearForces) { ClearForces(); } - m_flags &= ~e_locked; + m_locked = false; m_profile.step = stepTimer.GetMilliseconds(); } @@ -993,7 +1001,7 @@ void b2World::QueryAABB(b2QueryCallback* callback, const b2AABB& aabb) const struct b2WorldRayCastWrapper { - float32 RayCastCallback(const b2RayCastInput& input, int32 proxyId) + float RayCastCallback(const b2RayCastInput& input, int32 proxyId) { void* userData = broadPhase->GetUserData(proxyId); b2FixtureProxy* proxy = (b2FixtureProxy*)userData; @@ -1004,7 +1012,7 @@ struct b2WorldRayCastWrapper if (hit) { - float32 fraction = output.fraction; + float fraction = output.fraction; b2Vec2 point = (1.0f - fraction) * input.p1 + fraction * input.p2; return callback->ReportFixture(fixture, point, output.normal, fraction); } @@ -1037,10 +1045,10 @@ void b2World::DrawShape(b2Fixture* fixture, const b2Transform& xf, const b2Color b2CircleShape* circle = (b2CircleShape*)fixture->GetShape(); b2Vec2 center = b2Mul(xf, circle->m_p); - float32 radius = circle->m_radius; + float radius = circle->m_radius; b2Vec2 axis = b2Mul(xf.q, b2Vec2(1.0f, 0.0f)); - g_debugDraw->DrawSolidCircle(center, radius, axis, color); + m_debugDraw->DrawSolidCircle(center, radius, axis, color); } break; @@ -1049,7 +1057,13 @@ void b2World::DrawShape(b2Fixture* fixture, const b2Transform& xf, const b2Color 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); + 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; @@ -1063,8 +1077,7 @@ void b2World::DrawShape(b2Fixture* fixture, const b2Transform& xf, const b2Color 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); + m_debugDraw->DrawSegment(v1, v2, color); v1 = v2; } } @@ -1082,64 +1095,23 @@ void b2World::DrawShape(b2Fixture* fixture, const b2Transform& xf, const b2Color vertices[i] = b2Mul(xf, poly->m_vertices[i]); } - g_debugDraw->DrawSolidPolygon(vertices, vertexCount, color); + m_debugDraw->DrawSolidPolygon(vertices, vertexCount, color); } break; - - default: - break; - } -} - -void b2World::DrawJoint(b2Joint* joint) -{ - b2Body* bodyA = joint->GetBodyA(); - b2Body* bodyB = joint->GetBodyB(); - const b2Transform& xf1 = bodyA->GetTransform(); - const b2Transform& xf2 = bodyB->GetTransform(); - b2Vec2 x1 = xf1.p; - b2Vec2 x2 = xf2.p; - b2Vec2 p1 = joint->GetAnchorA(); - b2Vec2 p2 = joint->GetAnchorB(); - - b2Color color(0.5f, 0.8f, 0.8f); - - switch (joint->GetType()) - { - case e_distanceJoint: - 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); + break; } } -void b2World::DrawDebugData() +void b2World::DebugDraw() { - if (g_debugDraw == NULL) + if (m_debugDraw == nullptr) { return; } - uint32 flags = g_debugDraw->GetFlags(); + uint32 flags = m_debugDraw->GetFlags(); if (flags & b2Draw::e_shapeBit) { @@ -1148,7 +1120,12 @@ void b2World::DrawDebugData() const b2Transform& xf = b->GetTransform(); for (b2Fixture* f = b->GetFixtureList(); f; f = f->GetNext()) { - if (b->IsActive() == false) + 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)); } @@ -1176,7 +1153,7 @@ void b2World::DrawDebugData() { for (b2Joint* j = m_jointList; j; j = j->GetNext()) { - DrawJoint(j); + j->Draw(m_debugDraw); } } @@ -1185,13 +1162,14 @@ void b2World::DrawDebugData() 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(); + 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(); - //b2Vec2 cA = fixtureA->GetAABB().GetCenter(); - //b2Vec2 cB = fixtureB->GetAABB().GetCenter(); - - //g_debugDraw->DrawSegment(cA, cB, color); + m_debugDraw->DrawSegment(cA, cB, color); } } @@ -1202,7 +1180,7 @@ void b2World::DrawDebugData() for (b2Body* b = m_bodyList; b; b = b->GetNext()) { - if (b->IsActive() == false) + if (b->IsEnabled() == false) { continue; } @@ -1219,7 +1197,7 @@ void b2World::DrawDebugData() vs[2].Set(aabb.upperBound.x, aabb.upperBound.y); vs[3].Set(aabb.lowerBound.x, aabb.upperBound.y); - g_debugDraw->DrawPolygon(vs, 4, color); + m_debugDraw->DrawPolygon(vs, 4, color); } } } @@ -1231,7 +1209,7 @@ void b2World::DrawDebugData() { b2Transform xf = b->GetTransform(); xf.p = b->GetWorldCenter(); - g_debugDraw->DrawTransform(xf); + m_debugDraw->DrawTransform(xf); } } } @@ -1251,15 +1229,15 @@ int32 b2World::GetTreeBalance() const return m_contactManager.m_broadPhase.GetTreeBalance(); } -float32 b2World::GetTreeQuality() const +float 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) + b2Assert(m_locked == false); + if (m_locked) { return; } @@ -1281,16 +1259,19 @@ void b2World::ShiftOrigin(const b2Vec2& newOrigin) void b2World::Dump() { - if ((m_flags & e_locked) == e_locked) + if (m_locked) { return; } - b2Log("b2Vec2 g(%.15lef, %.15lef);\n", m_gravity.x, m_gravity.y); - b2Log("m_world->SetGravity(g);\n"); + 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); - 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) { @@ -1314,9 +1295,9 @@ void b2World::Dump() continue; } - b2Log("{\n"); + b2Dump("{\n"); j->Dump(); - b2Log("}\n"); + b2Dump("}\n"); } // Second pass on joints, only gear joints. @@ -1327,13 +1308,15 @@ void b2World::Dump() continue; } - b2Log("{\n"); + b2Dump("{\n"); j->Dump(); - b2Log("}\n"); + b2Dump("}\n"); } - b2Log("b2Free(joints);\n"); - b2Log("b2Free(bodies);\n"); - b2Log("joints = NULL;\n"); - b2Log("bodies = NULL;\n"); + b2Dump("b2Free(joints);\n"); + b2Dump("b2Free(bodies);\n"); + b2Dump("joints = nullptr;\n"); + b2Dump("bodies = nullptr;\n"); + + b2CloseDump(); } diff --git a/src/libraries/Box2D/Dynamics/b2_world_callbacks.cpp b/src/libraries/Box2D/Dynamics/b2_world_callbacks.cpp new file mode 100644 index 000000000..e1583e221 --- /dev/null +++ b/src/libraries/Box2D/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/src/libraries/Box2D/Rope/b2Rope.cpp b/src/libraries/Box2D/Rope/b2Rope.cpp deleted file mode 100644 index 7af3a6cf2..000000000 --- a/src/libraries/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/src/libraries/Box2D/Rope/b2Rope.h b/src/libraries/Box2D/Rope/b2Rope.h deleted file mode 100644 index 246cf923f..000000000 --- a/src/libraries/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/src/libraries/Box2D/Rope/b2_rope.cpp b/src/libraries/Box2D/Rope/b2_rope.cpp new file mode 100644 index 000000000..511944814 --- /dev/null +++ b/src/libraries/Box2D/Rope/b2_rope.cpp @@ -0,0 +1,769 @@ +// 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.0) + { + 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(); + } + + 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; + } +} + +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/libraries/Box2D/b2_block_allocator.h b/src/libraries/Box2D/b2_block_allocator.h new file mode 100644 index 000000000..48957343e --- /dev/null +++ b/src/libraries/Box2D/b2_block_allocator.h @@ -0,0 +1,59 @@ +// 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 "box2d/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 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/src/libraries/Box2D/Dynamics/b2Body.h b/src/libraries/Box2D/b2_body.h similarity index 80% rename from src/libraries/Box2D/Dynamics/b2Body.h rename to src/libraries/Box2D/b2_body.h index 24bae6403..6ba8574de 100644 --- a/src/libraries/Box2D/Dynamics/b2Body.h +++ b/src/libraries/Box2D/b2_body.h @@ -1,27 +1,30 @@ -/* -* 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_BODY_H #define B2_BODY_H -#include -#include -#include +#include "b2_math.h" +#include "b2_shape.h" class b2Fixture; class b2Joint; @@ -53,7 +56,7 @@ struct b2BodyDef /// This constructor sets the body definition default values. b2BodyDef() { - userData = NULL; + userData = nullptr; position.Set(0.0f, 0.0f); angle = 0.0f; linearVelocity.Set(0.0f, 0.0f); @@ -65,7 +68,7 @@ struct b2BodyDef fixedRotation = false; bullet = false; type = b2_staticBody; - active = true; + enabled = true; gravityScale = 1.0f; } @@ -78,23 +81,25 @@ struct b2BodyDef b2Vec2 position; /// The world angle of the body in radians. - float32 angle; + float angle; /// The linear velocity of the body's origin in world co-ordinates. b2Vec2 linearVelocity; /// The angular velocity of the body. - float32 angularVelocity; + 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. - float32 linearDamping; + /// 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. - float32 angularDamping; + /// Units are 1/time + float angularDamping; /// Set this flag to false if this body should never fall asleep. Note that /// this increases CPU usage. @@ -112,14 +117,14 @@ struct b2BodyDef /// @warning You should use this flag sparingly since it increases processing time. bool bullet; - /// Does this body start out active? - bool active; + /// Does this body start out enabled? + bool enabled; /// Use this to store application specific body data. void* userData; /// Scale the gravity applied to this body. - float32 gravityScale; + float gravityScale; }; /// A rigid body. These are created via b2World::CreateBody. @@ -142,7 +147,7 @@ public: /// @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); + 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 @@ -158,7 +163,7 @@ public: /// 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); + 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. @@ -170,7 +175,7 @@ public: /// Get the angle in radians. /// @return the current world rotation angle in radians. - float32 GetAngle() const; + float GetAngle() const; /// Get the world position of the center of mass. const b2Vec2& GetWorldCenter() const; @@ -188,11 +193,11 @@ public: /// Set the angular velocity. /// @param omega the new angular velocity in radians/second. - void SetAngularVelocity(float32 omega); + void SetAngularVelocity(float omega); /// Get the angular velocity. /// @return the angular velocity in radians/second. - float32 GetAngularVelocity() const; + 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 @@ -209,10 +214,9 @@ public: /// 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); + 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 @@ -222,18 +226,23 @@ public: /// @param wake also wake up the body void ApplyLinearImpulse(const b2Vec2& impulse, const b2Vec2& point, bool wake); + /// Apply an impulse to the center of mass. This immediately modifies the velocity. + /// @param impulse the world impulse vector, usually in N-seconds or kg-m/s. + /// @param wake also wake up the body + void ApplyLinearImpulseToCenter(const b2Vec2& impulse, bool wake); + /// Apply an angular impulse. /// @param impulse the angular impulse in units of kg*m*m/s /// @param wake also wake up the body - void ApplyAngularImpulse(float32 impulse, bool wake); + void ApplyAngularImpulse(float impulse, bool wake); /// Get the total mass of the body. /// @return the mass, usually in kilograms (kg). - float32 GetMass() const; + float 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; + float GetInertia() const; /// Get the mass data of the body. /// @return a struct containing the mass, inertia and center of the body. @@ -243,7 +252,7 @@ public: /// 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. + /// @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. @@ -262,42 +271,42 @@ public: 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. + /// @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 a vector in world coordinates. + /// @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 a point in world coordinates. + /// @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 a point in local coordinates. + /// @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. - float32 GetLinearDamping() const; + float GetLinearDamping() const; /// Set the linear damping of the body. - void SetLinearDamping(float32 linearDamping); + void SetLinearDamping(float linearDamping); /// Get the angular damping of the body. - float32 GetAngularDamping() const; + float GetAngularDamping() const; /// Set the angular damping of the body. - void SetAngularDamping(float32 angularDamping); + void SetAngularDamping(float angularDamping); /// Get the gravity scale of the body. - float32 GetGravityScale() const; + float GetGravityScale() const; /// Set the gravity scale of the body. - void SetGravityScale(float32 scale); + void SetGravityScale(float scale); /// Set the type of this body. This may alter the mass and velocity. void SetType(b2BodyType type); @@ -327,23 +336,22 @@ public: /// @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. + /// 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 inactive bodies. - /// Fixtures on an inactive body are implicitly inactive and will + /// 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 an inactive body are implicitly inactive. - /// An inactive body is still owned by a b2World object and remains + /// 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 SetActive(bool flag); + void SetEnabled(bool flag); /// Get the active state of the body. - bool IsActive() const; + bool IsEnabled() const; /// Set this body to have fixed rotation. This causes the mass /// to be reset. @@ -380,7 +388,7 @@ public: b2World* GetWorld(); const b2World* GetWorld() const; - /// Dump this body to a log file + /// Dump this body to a file void Dump(); private: @@ -411,7 +419,7 @@ private: e_autoSleepFlag = 0x0004, e_bulletFlag = 0x0008, e_fixedRotationFlag = 0x0010, - e_activeFlag = 0x0020, + e_enabledFlag = 0x0020, e_toiFlag = 0x0040 }; @@ -425,7 +433,7 @@ private: // It may lie, depending on the collideConnected flag. bool ShouldCollide(const b2Body* other) const; - void Advance(float32 t); + void Advance(float t); b2BodyType m_type; @@ -437,10 +445,10 @@ private: b2Sweep m_sweep; // the swept motion for CCD b2Vec2 m_linearVelocity; - float32 m_angularVelocity; + float m_angularVelocity; b2Vec2 m_force; - float32 m_torque; + float m_torque; b2World* m_world; b2Body* m_prev; @@ -452,16 +460,16 @@ private: b2JointEdge* m_jointList; b2ContactEdge* m_contactList; - float32 m_mass, m_invMass; + float m_mass, m_invMass; // Rotational inertia about the center of mass. - float32 m_I, m_invI; + float m_I, m_invI; - float32 m_linearDamping; - float32 m_angularDamping; - float32 m_gravityScale; + float m_linearDamping; + float m_angularDamping; + float m_gravityScale; - float32 m_sleepTime; + float m_sleepTime; void* m_userData; }; @@ -481,7 +489,7 @@ inline const b2Vec2& b2Body::GetPosition() const return m_xf.p; } -inline float32 b2Body::GetAngle() const +inline float b2Body::GetAngle() const { return m_sweep.a; } @@ -516,7 +524,7 @@ inline const b2Vec2& b2Body::GetLinearVelocity() const return m_linearVelocity; } -inline void b2Body::SetAngularVelocity(float32 w) +inline void b2Body::SetAngularVelocity(float w) { if (m_type == b2_staticBody) { @@ -531,17 +539,17 @@ inline void b2Body::SetAngularVelocity(float32 w) m_angularVelocity = w; } -inline float32 b2Body::GetAngularVelocity() const +inline float b2Body::GetAngularVelocity() const { return m_angularVelocity; } -inline float32 b2Body::GetMass() const +inline float b2Body::GetMass() const { return m_mass; } -inline float32 b2Body::GetInertia() const +inline float b2Body::GetInertia() const { return m_I + m_mass * b2Dot(m_sweep.localCenter, m_sweep.localCenter); } @@ -583,32 +591,32 @@ inline b2Vec2 b2Body::GetLinearVelocityFromLocalPoint(const b2Vec2& localPoint) return GetLinearVelocityFromWorldPoint(GetWorldPoint(localPoint)); } -inline float32 b2Body::GetLinearDamping() const +inline float b2Body::GetLinearDamping() const { return m_linearDamping; } -inline void b2Body::SetLinearDamping(float32 linearDamping) +inline void b2Body::SetLinearDamping(float linearDamping) { m_linearDamping = linearDamping; } -inline float32 b2Body::GetAngularDamping() const +inline float b2Body::GetAngularDamping() const { return m_angularDamping; } -inline void b2Body::SetAngularDamping(float32 angularDamping) +inline void b2Body::SetAngularDamping(float angularDamping) { m_angularDamping = angularDamping; } -inline float32 b2Body::GetGravityScale() const +inline float b2Body::GetGravityScale() const { return m_gravityScale; } -inline void b2Body::SetGravityScale(float32 scale) +inline void b2Body::SetGravityScale(float scale) { m_gravityScale = scale; } @@ -632,13 +640,15 @@ inline bool b2Body::IsBullet() const inline void b2Body::SetAwake(bool flag) { + if (m_type == b2_staticBody) + { + return; + } + if (flag) { - if ((m_flags & e_awakeFlag) == 0) - { - m_flags |= e_awakeFlag; - m_sleepTime = 0.0f; - } + m_flags |= e_awakeFlag; + m_sleepTime = 0.0f; } else { @@ -656,9 +666,9 @@ inline bool b2Body::IsAwake() const return (m_flags & e_awakeFlag) == e_awakeFlag; } -inline bool b2Body::IsActive() const +inline bool b2Body::IsEnabled() const { - return (m_flags & e_activeFlag) == e_activeFlag; + return (m_flags & e_enabledFlag) == e_enabledFlag; } inline bool b2Body::IsFixedRotation() const @@ -773,7 +783,7 @@ inline void b2Body::ApplyForceToCenter(const b2Vec2& force, bool wake) } } -inline void b2Body::ApplyTorque(float32 torque, bool wake) +inline void b2Body::ApplyTorque(float torque, bool wake) { if (m_type != b2_dynamicBody) { @@ -812,7 +822,26 @@ inline void b2Body::ApplyLinearImpulse(const b2Vec2& impulse, const b2Vec2& poin } } -inline void b2Body::ApplyAngularImpulse(float32 impulse, bool wake) +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) { @@ -837,7 +866,7 @@ inline void b2Body::SynchronizeTransform() m_xf.p = m_sweep.c - b2Mul(m_xf.q, m_sweep.localCenter); } -inline void b2Body::Advance(float32 alpha) +inline void b2Body::Advance(float alpha) { // Advance to the new safe time. This doesn't sync the broad-phase. m_sweep.Advance(alpha); diff --git a/src/libraries/Box2D/Collision/b2BroadPhase.h b/src/libraries/Box2D/b2_broad_phase.h similarity index 72% rename from src/libraries/Box2D/Collision/b2BroadPhase.h rename to src/libraries/Box2D/b2_broad_phase.h index 0c460cabb..cf1cf6773 100644 --- a/src/libraries/Box2D/Collision/b2BroadPhase.h +++ b/src/libraries/Box2D/b2_broad_phase.h @@ -1,28 +1,31 @@ -/* -* 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. -*/ +// 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 -#include -#include -#include +#include "b2_settings.h" +#include "b2_collision.h" +#include "b2_dynamic_tree.h" struct b2Pair { @@ -62,7 +65,7 @@ public: /// 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. + /// Get user data from a proxy. Returns nullptr if the id is invalid. void* GetUserData(int32 proxyId) const; /// Test overlap of fat AABBs. @@ -97,7 +100,7 @@ public: int32 GetTreeBalance() const; /// Get the quality metric of the embedded tree. - float32 GetTreeQuality() const; + float GetTreeQuality() const; /// Shift the world origin. Useful for large worlds. /// The shift formula is: position -= newOrigin @@ -128,22 +131,6 @@ private: 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); @@ -176,7 +163,7 @@ inline int32 b2BroadPhase::GetTreeBalance() const return m_tree.GetMaxBalance(); } -inline float32 b2BroadPhase::GetTreeQuality() const +inline float b2BroadPhase::GetTreeQuality() const { return m_tree.GetAreaRatio(); } @@ -204,37 +191,30 @@ void b2BroadPhase::UpdatePairs(T* callback) 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) + // 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); - ++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); + // 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 diff --git a/src/libraries/Box2D/b2_chain_shape.h b/src/libraries/Box2D/b2_chain_shape.h new file mode 100644 index 000000000..9ba0b8b1c --- /dev/null +++ b/src/libraries/Box2D/b2_chain_shape.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_CHAIN_SHAPE_H +#define B2_CHAIN_SHAPE_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 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/src/libraries/Box2D/b2_circle_shape.h b/src/libraries/Box2D/b2_circle_shape.h new file mode 100644 index 000000000..009d77db5 --- /dev/null +++ b/src/libraries/Box2D/b2_circle_shape.h @@ -0,0 +1,66 @@ +// 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_shape.h" + +/// A solid circle shape +class 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/src/libraries/Box2D/Collision/b2Collision.h b/src/libraries/Box2D/b2_collision.h similarity index 81% rename from src/libraries/Box2D/Collision/b2Collision.h rename to src/libraries/Box2D/b2_collision.h index ad023b3ca..aaa207b35 100644 --- a/src/libraries/Box2D/Collision/b2Collision.h +++ b/src/libraries/Box2D/b2_collision.h @@ -1,25 +1,29 @@ -/* -* 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. -*/ +// 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_math.h" #include /// @file @@ -69,8 +73,8 @@ union b2ContactID struct b2ManifoldPoint { b2Vec2 localPoint; ///< usage depends on manifold type - float32 normalImpulse; ///< the non-penetration impulse - float32 tangentImpulse; ///< the friction impulse + float normalImpulse; ///< the non-penetration impulse + float tangentImpulse; ///< the friction impulse b2ContactID id; ///< uniquely identifies a contact point between two shapes }; @@ -114,12 +118,12 @@ struct b2WorldManifold /// 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); + 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) - float32 separations[b2_maxManifoldPoints]; ///< a negative value indicates overlap, in meters + float separations[b2_maxManifoldPoints]; ///< a negative value indicates overlap, in meters }; /// This is used for determining the state of contact points. @@ -147,7 +151,7 @@ struct b2ClipVertex struct b2RayCastInput { b2Vec2 p1, p2; - float32 maxFraction; + float maxFraction; }; /// Ray-cast output data. The ray hits at p1 + fraction * (p2 - p1), where p1 and p2 @@ -155,7 +159,7 @@ struct b2RayCastInput struct b2RayCastOutput { b2Vec2 normal; - float32 fraction; + float fraction; }; /// An axis aligned bounding box. @@ -177,10 +181,10 @@ struct b2AABB } /// Get the perimeter length - float32 GetPerimeter() const + float GetPerimeter() const { - float32 wx = upperBound.x - lowerBound.x; - float32 wy = upperBound.y - lowerBound.y; + float wx = upperBound.x - lowerBound.x; + float wy = upperBound.y - lowerBound.y; return 2.0f * (wx + wy); } @@ -235,14 +239,14 @@ 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. +/// Compute the collision manifold between an edge and a polygon. 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); + const b2Vec2& normal, float offset, int32 vertexIndexA); /// Determine if two generic shapes overlap. bool b2TestOverlap( const b2Shape* shapeA, int32 indexA, diff --git a/src/libraries/Box2D/Dynamics/Contacts/b2Contact.h b/src/libraries/Box2D/b2_contact.h similarity index 78% rename from src/libraries/Box2D/Dynamics/Contacts/b2Contact.h rename to src/libraries/Box2D/b2_contact.h index 36e31e256..d4c59c620 100644 --- a/src/libraries/Box2D/Dynamics/Contacts/b2Contact.h +++ b/src/libraries/Box2D/b2_contact.h @@ -1,28 +1,32 @@ -/* -* 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. -*/ +// 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 -#include -#include -#include +#include "b2_collision.h" +#include "b2_fixture.h" +#include "b2_math.h" +#include "b2_shape.h" class b2Body; class b2Contact; @@ -32,16 +36,16 @@ class b2BlockAllocator; class b2StackAllocator; class b2ContactListener; -/// Friction mixing law. The idea is to allow either fixture to drive the restitution to zero. +/// Friction mixing law. The idea is to allow either fixture to drive the friction to zero. /// For example, anything slides on ice. -inline float32 b2MixFriction(float32 friction1, float32 friction2) +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 float32 b2MixRestitution(float32 restitution1, float32 restitution2) +inline float b2MixRestitution(float restitution1, float restitution2) { return restitution1 > restitution2 ? restitution1 : restitution2; } @@ -117,29 +121,29 @@ public: /// Override the default friction mixture. You can call this in b2ContactListener::PreSolve. /// This value persists until set or reset. - void SetFriction(float32 friction); + void SetFriction(float friction); /// Get the friction. - float32 GetFriction() const; + 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(float32 restitution); + void SetRestitution(float restitution); /// Get the restitution. - float32 GetRestitution() const; + float 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); + void SetTangentSpeed(float speed); /// Get the desired tangent speed. In meters per second. - float32 GetTangentSpeed() const; + float GetTangentSpeed() const; /// Evaluate this contact with your own manifold and transforms. virtual void Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB) = 0; @@ -157,7 +161,7 @@ protected: // Used when crawling contact graph when forming islands. e_islandFlag = 0x0001, - // Set when the shapes are touching. + // Set when the shapes are touching. e_touchingFlag = 0x0002, // This contact can be disabled (by user) @@ -183,7 +187,7 @@ protected: 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() : m_fixtureA(nullptr), m_fixtureB(nullptr) {} b2Contact(b2Fixture* fixtureA, int32 indexA, b2Fixture* fixtureB, int32 indexB); virtual ~b2Contact() {} @@ -211,12 +215,12 @@ protected: b2Manifold m_manifold; int32 m_toiCount; - float32 m_toi; + float m_toi; - float32 m_friction; - float32 m_restitution; + float m_friction; + float m_restitution; - float32 m_tangentSpeed; + float m_tangentSpeed; }; inline b2Manifold* b2Contact::GetManifold() @@ -306,12 +310,12 @@ inline void b2Contact::FlagForFiltering() m_flags |= e_filterFlag; } -inline void b2Contact::SetFriction(float32 friction) +inline void b2Contact::SetFriction(float friction) { m_friction = friction; } -inline float32 b2Contact::GetFriction() const +inline float b2Contact::GetFriction() const { return m_friction; } @@ -321,12 +325,12 @@ inline void b2Contact::ResetFriction() m_friction = b2MixFriction(m_fixtureA->m_friction, m_fixtureB->m_friction); } -inline void b2Contact::SetRestitution(float32 restitution) +inline void b2Contact::SetRestitution(float restitution) { m_restitution = restitution; } -inline float32 b2Contact::GetRestitution() const +inline float b2Contact::GetRestitution() const { return m_restitution; } @@ -336,12 +340,12 @@ inline void b2Contact::ResetRestitution() m_restitution = b2MixRestitution(m_fixtureA->m_restitution, m_fixtureB->m_restitution); } -inline void b2Contact::SetTangentSpeed(float32 speed) +inline void b2Contact::SetTangentSpeed(float speed) { m_tangentSpeed = speed; } -inline float32 b2Contact::GetTangentSpeed() const +inline float b2Contact::GetTangentSpeed() const { return m_tangentSpeed; } diff --git a/src/libraries/Box2D/b2_contact_manager.h b/src/libraries/Box2D/b2_contact_manager.h new file mode 100644 index 000000000..efa156847 --- /dev/null +++ b/src/libraries/Box2D/b2_contact_manager.h @@ -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. + +#ifndef B2_CONTACT_MANAGER_H +#define B2_CONTACT_MANAGER_H + +#include "b2_broad_phase.h" + +class b2Contact; +class b2ContactFilter; +class b2ContactListener; +class b2BlockAllocator; + +// Delegate of b2World. +class b2ContactManager +{ +public: + b2ContactManager(); + + // Broad-phase callback. + void AddPair(void* proxyUserDataA, void* proxyUserDataB); + + void FindNewContacts(); + + void Destroy(b2Contact* c); + + void Collide(); + + b2BroadPhase m_broadPhase; + b2Contact* m_contactList; + int32 m_contactCount; + b2ContactFilter* m_contactFilter; + b2ContactListener* m_contactListener; + b2BlockAllocator* m_allocator; +}; + +#endif diff --git a/src/libraries/Box2D/Collision/b2Distance.h b/src/libraries/Box2D/b2_distance.h similarity index 52% rename from src/libraries/Box2D/Collision/b2Distance.h rename to src/libraries/Box2D/b2_distance.h index 7bae689d1..3fcefc413 100644 --- a/src/libraries/Box2D/Collision/b2Distance.h +++ b/src/libraries/Box2D/b2_distance.h @@ -1,26 +1,29 @@ +// MIT License -/* -* 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. -*/ +// 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 +#include "b2_math.h" class b2Shape; @@ -28,12 +31,16 @@ class b2Shape; /// It encapsulates any shape. struct b2DistanceProxy { - b2DistanceProxy() : m_vertices(NULL), m_count(0), m_radius(0.0f) {} + 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; @@ -49,14 +56,14 @@ struct b2DistanceProxy b2Vec2 m_buffer[2]; const b2Vec2* m_vertices; int32 m_count; - float32 m_radius; + float m_radius; }; /// Used to warm start b2Distance. /// Set count to zero on first call. struct b2SimplexCache { - float32 metric; ///< length or area + float metric; ///< length or area uint16 count; uint8 indexA[3]; ///< vertices on shape A uint8 indexB[3]; ///< vertices on shape B @@ -79,7 +86,7 @@ struct b2DistanceOutput { b2Vec2 pointA; ///< closest point on shapeA b2Vec2 pointB; ///< closest point on shapeB - float32 distance; + float distance; int32 iterations; ///< number of GJK iterations used }; @@ -90,6 +97,28 @@ void b2Distance(b2DistanceOutput* output, b2SimplexCache* cache, const b2DistanceInput* input); +/// Input parameters for b2ShapeCast +struct b2ShapeCastInput +{ + b2DistanceProxy proxyA; + b2DistanceProxy proxyB; + b2Transform transformA; + b2Transform transformB; + b2Vec2 translationB; +}; + +/// Output results for b2ShapeCast +struct 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 +bool b2ShapeCast(b2ShapeCastOutput* output, const b2ShapeCastInput* input); ////////////////////////////////////////////////////////////////////////// @@ -107,10 +136,10 @@ inline const b2Vec2& b2DistanceProxy::GetVertex(int32 index) const inline int32 b2DistanceProxy::GetSupport(const b2Vec2& d) const { int32 bestIndex = 0; - float32 bestValue = b2Dot(m_vertices[0], d); + float bestValue = b2Dot(m_vertices[0], d); for (int32 i = 1; i < m_count; ++i) { - float32 value = b2Dot(m_vertices[i], d); + float value = b2Dot(m_vertices[i], d); if (value > bestValue) { bestIndex = i; @@ -124,10 +153,10 @@ inline int32 b2DistanceProxy::GetSupport(const b2Vec2& d) const inline const b2Vec2& b2DistanceProxy::GetSupportVertex(const b2Vec2& d) const { int32 bestIndex = 0; - float32 bestValue = b2Dot(m_vertices[0], d); + float bestValue = b2Dot(m_vertices[0], d); for (int32 i = 1; i < m_count; ++i) { - float32 value = b2Dot(m_vertices[i], d); + float value = b2Dot(m_vertices[i], d); if (value > bestValue) { bestIndex = i; diff --git a/src/libraries/Box2D/b2_distance_joint.h b/src/libraries/Box2D/b2_distance_joint.h new file mode 100644 index 000000000..b339e9239 --- /dev/null +++ b/src/libraries/Box2D/b2_distance_joint.h @@ -0,0 +1,140 @@ +// 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_joint.h" + +/// Distance joint definition. This requires defining an anchor point on both +/// bodies and the non-zero length of the distance joint. The definition uses local anchor points +/// so that the initial configuration can violate the constraint +/// slightly. This helps when saving and loading a game. +/// @warning Do not use a zero or short length. +struct b2DistanceJointDef : public b2JointDef +{ + b2DistanceJointDef() + { + type = e_distanceJoint; + localAnchorA.Set(0.0f, 0.0f); + localAnchorB.Set(0.0f, 0.0f); + length = 1.0f; + stiffness = 0.0f; + damping = 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. + float length; + + /// The linear stiffness in N/m. A value of 0 disables softness. + 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 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; } + + /// Set/get the natural length. + /// Manipulating the length can lead to non-physical behavior when the frequency is zero. + void SetLength(float length) { m_length = length; } + float GetLength() const { return m_length; } + + /// 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; + +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; + + // Solver shared + b2Vec2 m_localAnchorA; + b2Vec2 m_localAnchorB; + float m_gamma; + float m_impulse; + float 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; + float m_invMassA; + float m_invMassB; + float m_invIA; + float m_invIB; + float m_mass; +}; + +#endif diff --git a/src/libraries/Box2D/b2_draw.h b/src/libraries/Box2D/b2_draw.h new file mode 100644 index 000000000..10cbf866f --- /dev/null +++ b/src/libraries/Box2D/b2_draw.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_DRAW_H +#define B2_DRAW_H + +#include "b2_math.h" + +/// Color for debug drawing. Each value has the range [0,1]. +struct 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 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/src/libraries/Box2D/Collision/b2DynamicTree.h b/src/libraries/Box2D/b2_dynamic_tree.h similarity index 78% rename from src/libraries/Box2D/Collision/b2DynamicTree.h rename to src/libraries/Box2D/b2_dynamic_tree.h index bf80f73a1..b3ddb97ca 100644 --- a/src/libraries/Box2D/Collision/b2DynamicTree.h +++ b/src/libraries/Box2D/b2_dynamic_tree.h @@ -1,26 +1,30 @@ -/* -* 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. -*/ +// 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 -#include +#include "b2_collision.h" +#include "b2_growable_stack.h" #define b2_nullNode (-1) @@ -48,6 +52,8 @@ struct b2TreeNode // leaf = 0, free node = -1 int32 height; + + bool moved; }; /// A dynamic AABB tree broad-phase, inspired by Nathanael Presson's btDbvt. @@ -83,6 +89,9 @@ public: /// @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; @@ -113,7 +122,7 @@ public: int32 GetMaxBalance() const; /// Get the ratio of the sum of the node areas to the root area. - float32 GetAreaRatio() const; + float GetAreaRatio() const; /// Build an optimal tree. Very expensive. For testing. void RebuildBottomUp(); @@ -159,6 +168,18 @@ inline void* b2DynamicTree::GetUserData(int32 proxyId) const 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); @@ -216,7 +237,7 @@ inline void b2DynamicTree::RayCast(T* callback, const b2RayCastInput& input) con // Separating axis for segment (Gino, p80). // |dot(v, p1 - c)| > dot(|v|, h) - float32 maxFraction = input.maxFraction; + float maxFraction = input.maxFraction; // Build a bounding box for the segment. b2AABB segmentAABB; @@ -248,7 +269,7 @@ inline void b2DynamicTree::RayCast(T* callback, const b2RayCastInput& input) con // |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); + float separation = b2Abs(b2Dot(v, p1 - c)) - b2Dot(abs_v, h); if (separation > 0.0f) { continue; @@ -261,7 +282,7 @@ inline void b2DynamicTree::RayCast(T* callback, const b2RayCastInput& input) con subInput.p2 = input.p2; subInput.maxFraction = maxFraction; - float32 value = callback->RayCastCallback(subInput, nodeId); + float value = callback->RayCastCallback(subInput, nodeId); if (value == 0.0f) { diff --git a/src/libraries/Box2D/b2_edge_shape.h b/src/libraries/Box2D/b2_edge_shape.h new file mode 100644 index 000000000..ed79b43e2 --- /dev/null +++ b/src/libraries/Box2D/b2_edge_shape.h @@ -0,0 +1,85 @@ +// 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_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 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/src/libraries/Box2D/Dynamics/b2Fixture.h b/src/libraries/Box2D/b2_fixture.h similarity index 79% rename from src/libraries/Box2D/Dynamics/b2Fixture.h rename to src/libraries/Box2D/b2_fixture.h index 753127fbc..4cdde74d8 100644 --- a/src/libraries/Box2D/Dynamics/b2Fixture.h +++ b/src/libraries/Box2D/b2_fixture.h @@ -1,27 +1,31 @@ -/* -* 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. -*/ +// 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 -#include -#include +#include "b2_body.h" +#include "b2_collision.h" +#include "b2_shape.h" class b2BlockAllocator; class b2Body; @@ -58,8 +62,8 @@ struct b2FixtureDef /// The constructor sets the default fixture definition values. b2FixtureDef() { - shape = NULL; - userData = NULL; + shape = nullptr; + userData = nullptr; friction = 0.2f; restitution = 0.0f; density = 0.0f; @@ -74,13 +78,13 @@ struct b2FixtureDef void* userData; /// The friction coefficient, usually in the range [0,1]. - float32 friction; + float friction; /// The restitution (elasticity) usually in the range [0,1]. - float32 restitution; + float restitution; /// The density, usually in kg/m^2. - float32 density; + float density; /// A sensor shape collects contact information but never generates a collision /// response. @@ -135,7 +139,7 @@ public: /// 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. + /// 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; @@ -159,6 +163,7 @@ public: /// 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 @@ -168,24 +173,24 @@ public: /// 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); + void SetDensity(float density); /// Get the density of this fixture. - float32 GetDensity() const; + float GetDensity() const; /// Get the coefficient of friction. - float32 GetFriction() const; + float GetFriction() const; /// Set the coefficient of friction. This will _not_ change the friction of /// existing contacts. - void SetFriction(float32 friction); + void SetFriction(float friction); /// Get the coefficient of restitution. - float32 GetRestitution() const; + float GetRestitution() const; /// Set the coefficient of restitution. This will _not_ change the restitution of /// existing contacts. - void SetRestitution(float32 restitution); + void SetRestitution(float 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 @@ -215,15 +220,15 @@ protected: void Synchronize(b2BroadPhase* broadPhase, const b2Transform& xf1, const b2Transform& xf2); - float32 m_density; + float m_density; b2Fixture* m_next; b2Body* m_body; b2Shape* m_shape; - float32 m_friction; - float32 m_restitution; + float m_friction; + float m_restitution; b2FixtureProxy* m_proxies; int32 m_proxyCount; @@ -290,33 +295,33 @@ inline const b2Fixture* b2Fixture::GetNext() const return m_next; } -inline void b2Fixture::SetDensity(float32 density) +inline void b2Fixture::SetDensity(float density) { b2Assert(b2IsValid(density) && density >= 0.0f); m_density = density; } -inline float32 b2Fixture::GetDensity() const +inline float b2Fixture::GetDensity() const { return m_density; } -inline float32 b2Fixture::GetFriction() const +inline float b2Fixture::GetFriction() const { return m_friction; } -inline void b2Fixture::SetFriction(float32 friction) +inline void b2Fixture::SetFriction(float friction) { m_friction = friction; } -inline float32 b2Fixture::GetRestitution() const +inline float b2Fixture::GetRestitution() const { return m_restitution; } -inline void b2Fixture::SetRestitution(float32 restitution) +inline void b2Fixture::SetRestitution(float restitution) { m_restitution = restitution; } diff --git a/src/libraries/Box2D/b2_friction_joint.h b/src/libraries/Box2D/b2_friction_joint.h new file mode 100644 index 000000000..d7b88245e --- /dev/null +++ b/src/libraries/Box2D/b2_friction_joint.h @@ -0,0 +1,123 @@ +// 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_joint.h" + +/// Friction joint definition. +struct b2FrictionJointDef : public b2JointDef +{ + b2FrictionJointDef() + { + type = e_frictionJoint; + localAnchorA.SetZero(); + localAnchorB.SetZero(); + maxForce = 0.0f; + maxTorque = 0.0f; + } + + /// Initialize the bodies, anchors, axis, and reference angle using the world + /// anchor and world axis. + void Initialize(b2Body* bodyA, b2Body* bodyB, const b2Vec2& anchor); + + /// The local anchor point relative to bodyA's origin. + b2Vec2 localAnchorA; + + /// The local anchor point relative to bodyB's origin. + b2Vec2 localAnchorB; + + /// The maximum friction force in N. + 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 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/src/libraries/Box2D/b2_gear_joint.h b/src/libraries/Box2D/b2_gear_joint.h new file mode 100644 index 000000000..69e67a199 --- /dev/null +++ b/src/libraries/Box2D/b2_gear_joint.h @@ -0,0 +1,129 @@ +// 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). +struct b2GearJointDef : public b2JointDef +{ + b2GearJointDef() + { + type = e_gearJoint; + joint1 = nullptr; + joint2 = nullptr; + ratio = 1.0f; + } + + /// The first revolute/prismatic joint attached to the gear joint. + b2Joint* joint1; + + /// The second revolute/prismatic joint attached to the gear joint. + b2Joint* joint2; + + /// The gear ratio. + /// @see b2GearJoint for explanation. + 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 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_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/src/libraries/Box2D/b2_growable_stack.h b/src/libraries/Box2D/b2_growable_stack.h new file mode 100644 index 000000000..a24984f0c --- /dev/null +++ b/src/libraries/Box2D/b2_growable_stack.h @@ -0,0 +1,90 @@ +// 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 "b2_settings.h" +#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 = 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/src/libraries/Box2D/Dynamics/Joints/b2Joint.h b/src/libraries/Box2D/b2_joint.h similarity index 66% rename from src/libraries/Box2D/Dynamics/Joints/b2Joint.h rename to src/libraries/Box2D/b2_joint.h index dd076b6b2..c33ec90af 100644 --- a/src/libraries/Box2D/Dynamics/Joints/b2Joint.h +++ b/src/libraries/Box2D/b2_joint.h @@ -1,27 +1,32 @@ -/* -* 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. -*/ +// 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 +#include "b2_math.h" class b2Body; +class b2Draw; class b2Joint; struct b2SolverData; class b2BlockAllocator; @@ -42,19 +47,11 @@ enum b2JointType e_motorJoint }; -enum b2LimitState -{ - e_inactiveLimit, - e_atLowerLimit, - e_atUpperLimit, - e_equalLimits -}; - struct b2Jacobian { b2Vec2 linear; - float32 angularA; - float32 angularB; + float angularA; + float angularB; }; /// A joint edge is used to connect bodies and joints together @@ -76,9 +73,9 @@ struct b2JointDef b2JointDef() { type = e_unknownJoint; - userData = NULL; - bodyA = NULL; - bodyB = NULL; + userData = nullptr; + bodyA = nullptr; + bodyB = nullptr; collideConnected = false; } @@ -98,6 +95,16 @@ struct b2JointDef bool collideConnected; }; +/// Utility to compute linear stiffness values from frequency and damping ratio +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 +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 b2Joint @@ -120,10 +127,10 @@ public: 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; + virtual b2Vec2 GetReactionForce(float inv_dt) const = 0; /// Get the reaction torque on bodyB in N*m. - virtual float32 GetReactionTorque(float32 inv_dt) const = 0; + virtual float GetReactionTorque(float inv_dt) const = 0; /// Get the next joint the world joint list. b2Joint* GetNext(); @@ -135,8 +142,8 @@ public: /// Set the user data pointer. void SetUserData(void* data); - /// Short-cut function to determine if either body is inactive. - bool IsActive() 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 @@ -144,11 +151,14 @@ public: bool GetCollideConnected() const; /// Dump this joint to the log file. - virtual void Dump() { b2Log("// Dump is not supported for this joint type.\n"); } + 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; diff --git a/src/libraries/Box2D/Common/b2Math.h b/src/libraries/Box2D/b2_math.h similarity index 78% rename from src/libraries/Box2D/Common/b2Math.h rename to src/libraries/Box2D/b2_math.h index 755c519b0..afe421ba8 100644 --- a/src/libraries/Box2D/Common/b2Math.h +++ b/src/libraries/Box2D/b2_math.h @@ -1,49 +1,35 @@ -/* -* 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. -*/ +// 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_settings.h" #include /// This function is used to ensure that a floating point number is not a NaN or infinity. -inline bool b2IsValid(float32 x) +inline bool b2IsValid(float x) { - int32 ix = *reinterpret_cast(&x); - return (ix & 0x7f800000) != 0x7f800000; -} - -/// This is a approximate yet fast inverse square-root. -inline float32 b2InvSqrt(float32 x) -{ - union - { - float32 x; - int32 i; - } convert; - - convert.x = x; - float32 xhalf = 0.5f * x; - convert.i = 0x5f3759df - (convert.i >> 1); - x = convert.x; - x = x * (1.5f - xhalf * x * x); - return x; + return isfinite(x); } #define b2Sqrt(x) sqrtf(x) @@ -56,25 +42,25 @@ struct b2Vec2 b2Vec2() {} /// Construct using coordinates. - b2Vec2(float32 x, float32 y) : x(x), y(y) {} + 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(float32 x_, float32 y_) { x = x_; y = y_; } + 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. - float32 operator () (int32 i) const + float operator () (int32 i) const { return (&x)[i]; } /// Write to an indexed element. - float32& operator () (int32 i) + float& operator () (int32 i) { return (&x)[i]; } @@ -92,33 +78,33 @@ struct b2Vec2 } /// Multiply this vector by a scalar. - void operator *= (float32 a) + void operator *= (float a) { x *= a; y *= a; } /// Get the length of this vector (the norm). - float32 Length() const + float 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 + float LengthSquared() const { return x * x + y * y; } /// Convert this vector into a unit vector. Returns the length. - float32 Normalize() + float Normalize() { - float32 length = Length(); + float length = Length(); if (length < b2_epsilon) { return 0.0f; } - float32 invLength = 1.0f / length; + float invLength = 1.0f / length; x *= invLength; y *= invLength; @@ -137,7 +123,7 @@ struct b2Vec2 return b2Vec2(-y, x); } - float32 x, y; + float x, y; }; /// A 2D column vector with 3 elements. @@ -147,13 +133,13 @@ struct b2Vec3 b2Vec3() {} /// Construct using coordinates. - b2Vec3(float32 x, float32 y, float32 z) : x(x), y(y), z(z) {} + 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(float32 x_, float32 y_, float32 z_) { x = x_; y = y_; z = z_; } + 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; } @@ -171,12 +157,12 @@ struct b2Vec3 } /// Multiply this vector by a scalar. - void operator *= (float32 s) + void operator *= (float s) { x *= s; y *= s; z *= s; } - float32 x, y, z; + float x, y, z; }; /// A 2-by-2 matrix. Stored in column-major order. @@ -193,7 +179,7 @@ struct b2Mat22 } /// Construct this matrix using scalars. - b2Mat22(float32 a11, float32 a12, float32 a21, float32 a22) + b2Mat22(float a11, float a12, float a21, float a22) { ex.x = a11; ex.y = a21; ey.x = a12; ey.y = a22; @@ -222,9 +208,9 @@ struct b2Mat22 b2Mat22 GetInverse() const { - float32 a = ex.x, b = ey.x, c = ex.y, d = ey.y; + float a = ex.x, b = ey.x, c = ex.y, d = ey.y; b2Mat22 B; - float32 det = a * d - b * c; + float det = a * d - b * c; if (det != 0.0f) { det = 1.0f / det; @@ -238,8 +224,8 @@ struct b2Mat22 /// 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; + 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; @@ -301,7 +287,7 @@ struct b2Rot b2Rot() {} /// Initialize from an angle in radians - explicit b2Rot(float32 angle) + explicit b2Rot(float angle) { /// TODO_ERIN optimize s = sinf(angle); @@ -309,7 +295,7 @@ struct b2Rot } /// Set using an angle in radians. - void Set(float32 angle) + void Set(float angle) { /// TODO_ERIN optimize s = sinf(angle); @@ -324,7 +310,7 @@ struct b2Rot } /// Get the angle in radians - float32 GetAngle() const + float GetAngle() const { return b2Atan2(s, c); } @@ -342,7 +328,7 @@ struct b2Rot } /// Sine and cosine - float32 s, c; + float s, c; }; /// A transform contains translation and rotation. It is used to represent @@ -363,7 +349,7 @@ struct b2Transform } /// Set this based on the position and angle. - void Set(const b2Vec2& position, float32 angle) + void Set(const b2Vec2& position, float angle) { p = position; q.Set(angle); @@ -380,50 +366,51 @@ struct b2Transform struct b2Sweep { /// 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* xfb, float32 beta) const; + void GetTransform(b2Transform* transform, float beta) const; /// Advance the sweep forward, yielding a new initial state. /// @param alpha the new initial time. - void Advance(float32 alpha); + void Advance(float alpha); /// Normalize the angles. void Normalize(); b2Vec2 localCenter; ///< local center of mass position b2Vec2 c0, c; ///< center world positions - float32 a0, a; ///< world angles + float 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; + float alpha0; }; /// Useful constant extern const b2Vec2 b2Vec2_zero; /// Perform the dot product on two vectors. -inline float32 b2Dot(const b2Vec2& a, const b2Vec2& b) +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 float32 b2Cross(const b2Vec2& a, const b2Vec2& b) +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, float32 s) +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(float32 s, const b2Vec2& a) +inline b2Vec2 b2Cross(float s, const b2Vec2& a) { return b2Vec2(-s * a.y, s * a.x); } @@ -454,7 +441,7 @@ 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) +inline b2Vec2 operator * (float s, const b2Vec2& a) { return b2Vec2(s * a.x, s * a.y); } @@ -464,19 +451,24 @@ 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) +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 float32 b2DistanceSquared(const b2Vec2& a, const b2Vec2& b) +inline float b2DistanceSquared(const b2Vec2& a, const b2Vec2& b) { b2Vec2 c = a - b; return b2Dot(c, c); } -inline b2Vec3 operator * (float32 s, const b2Vec3& a) +inline b2Vec3 operator * (float s, const b2Vec3& a) { return b2Vec3(s * a.x, s * a.y, s * a.z); } @@ -494,7 +486,7 @@ inline b2Vec3 operator - (const b2Vec3& a, const b2Vec3& b) } /// Perform the dot product on two vectors. -inline float32 b2Dot(const b2Vec3& a, const b2Vec3& b) +inline float b2Dot(const b2Vec3& a, const b2Vec3& b) { return a.x * b.x + a.y * b.y + a.z * b.z; } @@ -576,18 +568,18 @@ inline b2Vec2 b2MulT(const b2Rot& q, const b2Vec2& v) 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; + 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) { - 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); + 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); } @@ -689,20 +681,20 @@ inline bool b2IsPowerOfTwo(uint32 x) return result; } -inline void b2Sweep::GetTransform(b2Transform* xf, float32 beta) const +inline void b2Sweep::GetTransform(b2Transform* xf, float beta) const { - xf->p = (1.0f - beta) * c0 + beta * c; - float32 angle = (1.0f - beta) * a0 + beta * a; + xf->p = c0 + beta * (c - c0); + float angle = a0 + beta * (a - a0); xf->q.Set(angle); // Shift to origin xf->p -= b2Mul(xf->q, localCenter); } -inline void b2Sweep::Advance(float32 alpha) +inline void b2Sweep::Advance(float alpha) { b2Assert(alpha0 < 1.0f); - float32 beta = (alpha - alpha0) / (1.0f - alpha0); + float beta = (alpha - alpha0) / (1.0f - alpha0); c0 += beta * (c - c0); a0 += beta * (a - a0); alpha0 = alpha; @@ -711,8 +703,8 @@ inline void b2Sweep::Advance(float32 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); + float twoPi = 2.0f * b2_pi; + float d = twoPi * floorf(a0 / twoPi); a0 -= d; a -= d; } diff --git a/src/libraries/Box2D/b2_motor_joint.h b/src/libraries/Box2D/b2_motor_joint.h new file mode 100644 index 000000000..c3f1adcbb --- /dev/null +++ b/src/libraries/Box2D/b2_motor_joint.h @@ -0,0 +1,137 @@ +// 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_joint.h" + +/// Motor joint definition. +struct b2MotorJointDef : public b2JointDef +{ + b2MotorJointDef() + { + type = e_motorJoint; + linearOffset.SetZero(); + angularOffset = 0.0f; + maxForce = 1.0f; + maxTorque = 1.0f; + correctionFactor = 0.3f; + } + + /// Initialize the bodies and offsets using the current transforms. + void Initialize(b2Body* bodyA, b2Body* bodyB); + + /// Position of bodyB minus the position of bodyA, in bodyA's frame, in meters. + b2Vec2 linearOffset; + + /// The bodyB angle minus bodyA angle in radians. + 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 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/src/libraries/Box2D/b2_mouse_joint.h b/src/libraries/Box2D/b2_mouse_joint.h new file mode 100644 index 000000000..ebaa02592 --- /dev/null +++ b/src/libraries/Box2D/b2_mouse_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_MOUSE_JOINT_H +#define B2_MOUSE_JOINT_H + +#include "b2_joint.h" + +/// Mouse joint definition. This requires a world target point, +/// tuning parameters, and the time step. +struct b2MouseJointDef : public b2JointDef +{ + b2MouseJointDef() + { + type = e_mouseJoint; + target.Set(0.0f, 0.0f); + maxForce = 0.0f; + 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 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/src/libraries/Box2D/Collision/Shapes/b2PolygonShape.h b/src/libraries/Box2D/b2_polygon_shape.h similarity index 51% rename from src/libraries/Box2D/Collision/Shapes/b2PolygonShape.h rename to src/libraries/Box2D/b2_polygon_shape.h index b19298038..52f4cc0ec 100644 --- a/src/libraries/Box2D/Collision/Shapes/b2PolygonShape.h +++ b/src/libraries/Box2D/b2_polygon_shape.h @@ -1,27 +1,30 @@ -/* -* 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. -*/ +// 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 +#include "b2_shape.h" -/// A convex polygon. It is assumed that the interior of the polygon is to +/// 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. @@ -31,10 +34,10 @@ public: b2PolygonShape(); /// Implement b2Shape. - b2Shape* Clone(b2BlockAllocator* allocator) const; + b2Shape* Clone(b2BlockAllocator* allocator) const override; /// @see b2Shape::GetChildCount - int32 GetChildCount() const; + 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]. @@ -46,33 +49,29 @@ public: /// 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); + 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(float32 hx, float32 hy, const b2Vec2& center, float32 angle); + void SetAsBox(float hx, float hy, const b2Vec2& center, float angle); /// @see b2Shape::TestPoint - bool TestPoint(const b2Transform& transform, const b2Vec2& p) const; + 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; + const b2Transform& transform, int32 childIndex) const override; /// @see b2Shape::ComputeAABB - void ComputeAABB(b2AABB* aabb, const b2Transform& transform, int32 childIndex) const; + void ComputeAABB(b2AABB* aabb, const b2Transform& transform, int32 childIndex) const override; /// @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; + void ComputeMass(b2MassData* massData, float density) const override; /// Validate convexity. This is a very time consuming operation. /// @returns true if valid @@ -92,10 +91,4 @@ inline b2PolygonShape::b2PolygonShape() 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/src/libraries/Box2D/Dynamics/Joints/b2PrismaticJoint.h b/src/libraries/Box2D/b2_prismatic_joint.h similarity index 58% rename from src/libraries/Box2D/Dynamics/Joints/b2PrismaticJoint.h rename to src/libraries/Box2D/b2_prismatic_joint.h index 6ef5d3ec0..09cbad540 100644 --- a/src/libraries/Box2D/Dynamics/Joints/b2PrismaticJoint.h +++ b/src/libraries/Box2D/b2_prismatic_joint.h @@ -1,25 +1,29 @@ -/* -* 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_PRISMATIC_JOINT_H #define B2_PRISMATIC_JOINT_H -#include +#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 @@ -58,25 +62,25 @@ struct b2PrismaticJointDef : public b2JointDef b2Vec2 localAxisA; /// The constrained angle between the bodies: bodyB_angle - bodyA_angle. - float32 referenceAngle; + float referenceAngle; /// Enable/disable the joint limit. bool enableLimit; /// The lower translation limit, usually in meters. - float32 lowerTranslation; + float lowerTranslation; /// The upper translation limit, usually in meters. - float32 upperTranslation; + float upperTranslation; /// Enable/disable the joint motor. bool enableMotor; /// The maximum motor torque, usually in N-m. - float32 maxMotorForce; + float maxMotorForce; /// The desired motor speed in radians per second. - float32 motorSpeed; + float motorSpeed; }; /// A prismatic joint. This joint provides one degree of freedom: translation @@ -86,11 +90,11 @@ struct b2PrismaticJointDef : public b2JointDef class b2PrismaticJoint : public b2Joint { public: - b2Vec2 GetAnchorA() const; - b2Vec2 GetAnchorB() const; + b2Vec2 GetAnchorA() const override; + b2Vec2 GetAnchorB() const override; - b2Vec2 GetReactionForce(float32 inv_dt) const; - float32 GetReactionTorque(float32 inv_dt) const; + 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; } @@ -102,13 +106,13 @@ public: const b2Vec2& GetLocalAxisA() const { return m_localXAxisA; } /// Get the reference angle. - float32 GetReferenceAngle() const { return m_referenceAngle; } + float GetReferenceAngle() const { return m_referenceAngle; } /// Get the current joint translation, usually in meters. - float32 GetJointTranslation() const; + float GetJointTranslation() const; /// Get the current joint translation speed, usually in meters per second. - float32 GetJointSpeed() const; + float GetJointSpeed() const; /// Is the joint limit enabled? bool IsLimitEnabled() const; @@ -117,13 +121,13 @@ public: void EnableLimit(bool flag); /// Get the lower joint limit, usually in meters. - float32 GetLowerLimit() const; + float GetLowerLimit() const; /// Get the upper joint limit, usually in meters. - float32 GetUpperLimit() const; + float GetUpperLimit() const; /// Set the joint limits, usually in meters. - void SetLimits(float32 lower, float32 upper); + void SetLimits(float lower, float upper); /// Is the joint motor enabled? bool IsMotorEnabled() const; @@ -132,63 +136,67 @@ public: void EnableMotor(bool flag); /// Set the motor speed, usually in meters per second. - void SetMotorSpeed(float32 speed); + void SetMotorSpeed(float speed); /// Get the motor speed, usually in meters per second. - float32 GetMotorSpeed() const; + float GetMotorSpeed() const; /// Set the maximum motor force, usually in N. - void SetMaxMotorForce(float32 force); - float32 GetMaxMotorForce() const { return m_maxMotorForce; } + void SetMaxMotorForce(float force); + float GetMaxMotorForce() const { return m_maxMotorForce; } /// Get the current motor force given the inverse time step, usually in N. - float32 GetMotorForce(float32 inv_dt) const; + float GetMotorForce(float inv_dt) const; /// Dump to b2Log - void Dump(); + 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); - void SolveVelocityConstraints(const b2SolverData& data); - bool SolvePositionConstraints(const b2SolverData& data); + void InitVelocityConstraints(const b2SolverData& data) override; + void SolveVelocityConstraints(const b2SolverData& data) override; + bool SolvePositionConstraints(const b2SolverData& data) override; - // Solver shared b2Vec2 m_localAnchorA; b2Vec2 m_localAnchorB; 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; + 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; - 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; + float m_invMassA; + float m_invMassB; + float m_invIA; + float m_invIB; b2Vec2 m_axis, m_perp; - float32 m_s1, m_s2; - float32 m_a1, m_a2; - b2Mat33 m_K; - float32 m_motorMass; + float m_s1, m_s2; + float m_a1, m_a2; + b2Mat22 m_K; + float m_translation; + float m_axialMass; }; -inline float32 b2PrismaticJoint::GetMotorSpeed() const +inline float b2PrismaticJoint::GetMotorSpeed() const { return m_motorSpeed; } diff --git a/src/libraries/Box2D/Dynamics/Joints/b2PulleyJoint.h b/src/libraries/Box2D/b2_pulley_joint.h similarity index 56% rename from src/libraries/Box2D/Dynamics/Joints/b2PulleyJoint.h rename to src/libraries/Box2D/b2_pulley_joint.h index 44843e7b4..14f12c41f 100644 --- a/src/libraries/Box2D/Dynamics/Joints/b2PulleyJoint.h +++ b/src/libraries/Box2D/b2_pulley_joint.h @@ -1,27 +1,31 @@ -/* -* 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_PULLEY_JOINT_H #define B2_PULLEY_JOINT_H -#include +#include "b2_joint.h" -const float32 b2_minPulleyLength = 2.0f; +const float b2_minPulleyLength = 2.0f; /// Pulley joint definition. This requires two ground anchors, /// two dynamic body anchor points, and a pulley ratio. @@ -44,7 +48,7 @@ struct b2PulleyJointDef : public b2JointDef void Initialize(b2Body* bodyA, b2Body* bodyB, const b2Vec2& groundAnchorA, const b2Vec2& groundAnchorB, const b2Vec2& anchorA, const b2Vec2& anchorB, - float32 ratio); + float ratio); /// The first ground anchor in world coordinates. This point never moves. b2Vec2 groundAnchorA; @@ -59,13 +63,13 @@ struct b2PulleyJointDef : public b2JointDef b2Vec2 localAnchorB; /// The a reference length for the segment attached to bodyA. - float32 lengthA; + float lengthA; /// The a reference length for the segment attached to bodyB. - float32 lengthB; + float lengthB; /// The pulley ratio, used to simulate a block-and-tackle. - float32 ratio; + float ratio; }; /// The pulley joint is connected to two bodies and two fixed ground points. @@ -79,11 +83,11 @@ struct b2PulleyJointDef : public b2JointDef class b2PulleyJoint : public b2Joint { public: - b2Vec2 GetAnchorA() const; - b2Vec2 GetAnchorB() const; + b2Vec2 GetAnchorA() const override; + b2Vec2 GetAnchorB() const override; - b2Vec2 GetReactionForce(float32 inv_dt) const; - float32 GetReactionTorque(float32 inv_dt) const; + b2Vec2 GetReactionForce(float inv_dt) const override; + float GetReactionTorque(float inv_dt) const override; /// Get the first ground anchor. b2Vec2 GetGroundAnchorA() const; @@ -92,46 +96,46 @@ public: b2Vec2 GetGroundAnchorB() const; /// Get the current length of the segment attached to bodyA. - float32 GetLengthA() const; + float GetLengthA() const; /// Get the current length of the segment attached to bodyB. - float32 GetLengthB() const; + float GetLengthB() const; /// Get the pulley ratio. - float32 GetRatio() const; + float GetRatio() const; /// Get the current length of the segment attached to bodyA. - float32 GetCurrentLengthA() const; + float GetCurrentLengthA() const; /// Get the current length of the segment attached to bodyB. - float32 GetCurrentLengthB() const; + float GetCurrentLengthB() const; /// Dump joint to dmLog - void Dump(); + void Dump() override; /// Implement b2Joint::ShiftOrigin - void ShiftOrigin(const b2Vec2& newOrigin); + void ShiftOrigin(const b2Vec2& newOrigin) override; protected: friend class b2Joint; b2PulleyJoint(const b2PulleyJointDef* data); - void InitVelocityConstraints(const b2SolverData& data); - void SolveVelocityConstraints(const b2SolverData& data); - bool SolvePositionConstraints(const b2SolverData& data); + void InitVelocityConstraints(const b2SolverData& data) override; + void SolveVelocityConstraints(const b2SolverData& data) override; + bool SolvePositionConstraints(const b2SolverData& data) override; b2Vec2 m_groundAnchorA; b2Vec2 m_groundAnchorB; - float32 m_lengthA; - float32 m_lengthB; + float m_lengthA; + float m_lengthB; // Solver shared b2Vec2 m_localAnchorA; b2Vec2 m_localAnchorB; - float32 m_constant; - float32 m_ratio; - float32 m_impulse; + float m_constant; + float m_ratio; + float m_impulse; // Solver temp int32 m_indexA; @@ -142,11 +146,11 @@ protected: b2Vec2 m_rB; b2Vec2 m_localCenterA; b2Vec2 m_localCenterB; - float32 m_invMassA; - float32 m_invMassB; - float32 m_invIA; - float32 m_invIB; - float32 m_mass; + float m_invMassA; + float m_invMassB; + float m_invIA; + float m_invIB; + float m_mass; }; #endif diff --git a/src/libraries/Box2D/Dynamics/Joints/b2RevoluteJoint.h b/src/libraries/Box2D/b2_revolute_joint.h similarity index 55% rename from src/libraries/Box2D/Dynamics/Joints/b2RevoluteJoint.h rename to src/libraries/Box2D/b2_revolute_joint.h index bbed4a6cb..f202e3551 100644 --- a/src/libraries/Box2D/Dynamics/Joints/b2RevoluteJoint.h +++ b/src/libraries/Box2D/b2_revolute_joint.h @@ -1,32 +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_REVOLUTE_JOINT_H #define B2_REVOLUTE_JOINT_H -#include +#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. +/// 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. @@ -59,26 +62,26 @@ struct b2RevoluteJointDef : public b2JointDef b2Vec2 localAnchorB; /// The bodyB angle minus bodyA angle in the reference state (radians). - float32 referenceAngle; + float referenceAngle; /// A flag to enable joint limits. bool enableLimit; /// The lower angle for the joint limit (radians). - float32 lowerAngle; + float lowerAngle; /// The upper angle for the joint limit (radians). - float32 upperAngle; + float upperAngle; /// A flag to enable the joint motor. bool enableMotor; /// The desired motor speed. Usually in radians per second. - float32 motorSpeed; + float motorSpeed; /// The maximum motor torque used to achieve the desired motor speed. /// Usually in N-m. - float32 maxMotorTorque; + float maxMotorTorque; }; /// A revolute joint constrains two bodies to share a common point while they @@ -90,8 +93,8 @@ struct b2RevoluteJointDef : public b2JointDef class b2RevoluteJoint : public b2Joint { public: - b2Vec2 GetAnchorA() const; - b2Vec2 GetAnchorB() const; + b2Vec2 GetAnchorA() const override; + b2Vec2 GetAnchorB() const override; /// The local anchor point relative to bodyA's origin. const b2Vec2& GetLocalAnchorA() const { return m_localAnchorA; } @@ -100,13 +103,13 @@ public: const b2Vec2& GetLocalAnchorB() const { return m_localAnchorB; } /// Get the reference angle. - float32 GetReferenceAngle() const { return m_referenceAngle; } + float GetReferenceAngle() const { return m_referenceAngle; } /// Get the current joint angle in radians. - float32 GetJointAngle() const; + float GetJointAngle() const; /// Get the current joint angle speed in radians per second. - float32 GetJointSpeed() const; + float GetJointSpeed() const; /// Is the joint limit enabled? bool IsLimitEnabled() const; @@ -115,13 +118,13 @@ public: void EnableLimit(bool flag); /// Get the lower joint limit in radians. - float32 GetLowerLimit() const; + float GetLowerLimit() const; /// Get the upper joint limit in radians. - float32 GetUpperLimit() const; + float GetUpperLimit() const; /// Set the joint limits in radians. - void SetLimits(float32 lower, float32 upper); + void SetLimits(float lower, float upper); /// Is the joint motor enabled? bool IsMotorEnabled() const; @@ -130,29 +133,32 @@ public: void EnableMotor(bool flag); /// Set the motor speed in radians per second. - void SetMotorSpeed(float32 speed); + void SetMotorSpeed(float speed); /// Get the motor speed in radians per second. - float32 GetMotorSpeed() const; + float GetMotorSpeed() const; /// Set the maximum motor torque, usually in N-m. - void SetMaxMotorTorque(float32 torque); - float32 GetMaxMotorTorque() const { return m_maxMotorTorque; } + void SetMaxMotorTorque(float torque); + float GetMaxMotorTorque() const { return m_maxMotorTorque; } /// Get the reaction force given the inverse time step. /// Unit is N. - b2Vec2 GetReactionForce(float32 inv_dt) const; + 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. - float32 GetReactionTorque(float32 inv_dt) const; + float GetReactionTorque(float inv_dt) const override; /// Get the current motor torque given the inverse time step. /// Unit is N*m. - float32 GetMotorTorque(float32 inv_dt) const; + float GetMotorTorque(float inv_dt) const; /// Dump to b2Log. - void Dump(); + void Dump() override; + + /// + void Draw(b2Draw* draw) const override; protected: @@ -161,24 +167,24 @@ protected: b2RevoluteJoint(const b2RevoluteJointDef* def); - void InitVelocityConstraints(const b2SolverData& data); - void SolveVelocityConstraints(const b2SolverData& data); - bool SolvePositionConstraints(const b2SolverData& data); + void InitVelocityConstraints(const b2SolverData& data) override; + void SolveVelocityConstraints(const b2SolverData& data) override; + bool SolvePositionConstraints(const b2SolverData& data) override; // Solver shared b2Vec2 m_localAnchorA; b2Vec2 m_localAnchorB; - b2Vec3 m_impulse; - float32 m_motorImpulse; - + b2Vec2 m_impulse; + float m_motorImpulse; + float m_lowerImpulse; + float m_upperImpulse; bool m_enableMotor; - float32 m_maxMotorTorque; - float32 m_motorSpeed; - + float m_maxMotorTorque; + float m_motorSpeed; bool m_enableLimit; - float32 m_referenceAngle; - float32 m_lowerAngle; - float32 m_upperAngle; + float m_referenceAngle; + float m_lowerAngle; + float m_upperAngle; // Solver temp int32 m_indexA; @@ -187,16 +193,16 @@ protected: 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; + float m_invMassA; + float m_invMassB; + float m_invIA; + float m_invIB; + b2Mat22 m_K; + float m_angle; + float m_axialMass; }; -inline float32 b2RevoluteJoint::GetMotorSpeed() const +inline float b2RevoluteJoint::GetMotorSpeed() const { return m_motorSpeed; } diff --git a/src/libraries/Box2D/b2_rope.h b/src/libraries/Box2D/b2_rope.h new file mode 100644 index 000000000..5f1cc4073 --- /dev/null +++ b/src/libraries/Box2D/b2_rope.h @@ -0,0 +1,150 @@ +// 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_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 +}; + +/// +struct b2RopeTuning +{ + b2RopeTuning() + { + stretchingModel = b2_pbdStretchingModel; + bendingModel = b2_pbdAngleBendingModel; + damping = 0.0f; + stretchStiffness = 1.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 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 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 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/src/libraries/Box2D/b2_rope_joint.h b/src/libraries/Box2D/b2_rope_joint.h new file mode 100644 index 000000000..b254b3a22 --- /dev/null +++ b/src/libraries/Box2D/b2_rope_joint.h @@ -0,0 +1,118 @@ +// 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_JOINT_H +#define B2_ROPE_JOINT_H + +#include "b2_joint.h" + +/// Rope joint definition. This requires two body anchor points and +/// a maximum lengths. +/// Note: by default the connected objects will not collide. +/// see collideConnected in b2JointDef. +struct b2RopeJointDef : public b2JointDef +{ + b2RopeJointDef() + { + type = e_ropeJoint; + localAnchorA.Set(-1.0f, 0.0f); + localAnchorB.Set(1.0f, 0.0f); + maxLength = 0.0f; + } + + /// The local anchor point relative to bodyA's origin. + b2Vec2 localAnchorA; + + /// The local anchor point relative to bodyB's origin. + b2Vec2 localAnchorB; + + /// The maximum length of the rope. + /// Warning: this must be larger than b2_linearSlop or + /// the joint will have no effect. + float maxLength; +}; + +/// A rope joint enforces a maximum distance between two points +/// on two bodies. It has no other effect. +/// Warning: if you attempt to change the maximum length during +/// the simulation you will get some non-physical behavior. +/// A model that would allow you to dynamically modify the length +/// would have some sponginess, so I chose not to implement it +/// that way. See b2DistanceJoint if you want to dynamically +/// control length. +class b2RopeJoint : public b2Joint +{ +public: + b2Vec2 GetAnchorA() const override; + b2Vec2 GetAnchorB() const override; + + b2Vec2 GetReactionForce(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/Get the maximum length of the rope. + void SetMaxLength(float length) { m_maxLength = length; } + float GetMaxLength() const; + + // Get current length + float GetLength() const; + + /// Dump joint to dmLog + void Dump() override; + +protected: + + friend class b2Joint; + b2RopeJoint(const b2RopeJointDef* data); + + void InitVelocityConstraints(const b2SolverData& data) override; + void SolveVelocityConstraints(const b2SolverData& data) override; + bool SolvePositionConstraints(const b2SolverData& data) override; + + // Solver shared + b2Vec2 m_localAnchorA; + b2Vec2 m_localAnchorB; + float m_maxLength; + float m_length; + float 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; + float m_invMassA; + float m_invMassB; + float m_invIA; + float m_invIB; + float m_mass; +}; + +#endif diff --git a/src/libraries/Box2D/Common/b2Settings.h b/src/libraries/Box2D/b2_settings.h similarity index 74% rename from src/libraries/Box2D/Common/b2Settings.h rename to src/libraries/Box2D/b2_settings.h index beaa1d47b..3b17e12fb 100644 --- a/src/libraries/Box2D/Common/b2Settings.h +++ b/src/libraries/Box2D/b2_settings.h @@ -1,20 +1,24 @@ -/* -* 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. -*/ +// 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 @@ -23,11 +27,12 @@ #include #include -void loveAssert(bool test, const char *teststr); +#if !defined(NDEBUG) + #define b2DEBUG +#endif #define B2_NOT_USED(x) ((void)(x)) -//#define b2Assert(A) assert(A) -#define b2Assert(A) loveAssert((A), #A) +#define b2Assert(A) assert(A) typedef signed char int8; typedef signed short int16; @@ -35,8 +40,6 @@ 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 @@ -64,7 +67,7 @@ typedef double float64; /// 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 +#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. @@ -114,7 +117,7 @@ typedef double float64; /// 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 +#define b2_toiBaumgarte 0.75f // Sleep @@ -139,6 +142,11 @@ void b2Free(void* mem); /// Logging function. void b2Log(const char* string, ...); +/// 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 diff --git a/src/libraries/Box2D/Collision/Shapes/b2Shape.h b/src/libraries/Box2D/b2_shape.h similarity index 62% rename from src/libraries/Box2D/Collision/Shapes/b2Shape.h rename to src/libraries/Box2D/b2_shape.h index 170ea1a13..27e3d14bc 100644 --- a/src/libraries/Box2D/Collision/Shapes/b2Shape.h +++ b/src/libraries/Box2D/b2_shape.h @@ -1,39 +1,44 @@ -/* -* 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. -*/ +// 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 -#include -#include +#include "b2_math.h" +#include "b2_collision.h" + +class b2BlockAllocator; /// This holds the mass data computed for a shape. struct b2MassData { /// The mass of the shape, usually in kilograms. - float32 mass; + 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. - float32 I; + float I; }; /// A shape is used for collision detection. You can create a shape however you like. @@ -87,10 +92,13 @@ public: /// 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; + virtual void ComputeMass(b2MassData* massData, float density) const = 0; Type m_type; - float32 m_radius; + + /// 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 diff --git a/src/libraries/Box2D/b2_stack_allocator.h b/src/libraries/Box2D/b2_stack_allocator.h new file mode 100644 index 000000000..780ab7945 --- /dev/null +++ b/src/libraries/Box2D/b2_stack_allocator.h @@ -0,0 +1,64 @@ +// 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_settings.h" + +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/src/libraries/Box2D/b2_time_of_impact.h b/src/libraries/Box2D/b2_time_of_impact.h new file mode 100644 index 000000000..bb3031fcb --- /dev/null +++ b/src/libraries/Box2D/b2_time_of_impact.h @@ -0,0 +1,62 @@ +// 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_math.h" +#include "b2_distance.h" + +/// Input parameters for b2TimeOfImpact +struct b2TOIInput +{ + b2DistanceProxy proxyA; + b2DistanceProxy proxyB; + b2Sweep sweepA; + b2Sweep sweepB; + float 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; + 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. +void b2TimeOfImpact(b2TOIOutput* output, const b2TOIInput* input); + +#endif diff --git a/src/libraries/Box2D/b2_time_step.h b/src/libraries/Box2D/b2_time_step.h new file mode 100644 index 000000000..f0304a300 --- /dev/null +++ b/src/libraries/Box2D/b2_time_step.h @@ -0,0 +1,73 @@ +// 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_math.h" + +/// Profiling data. Times are in milliseconds. +struct b2Profile +{ + float step; + float collide; + float solve; + float solveInit; + float solveVelocity; + float solvePosition; + float broadphase; + float solveTOI; +}; + +/// This is an internal structure. +struct 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 b2Position +{ + b2Vec2 c; + float a; +}; + +/// This is an internal structure. +struct b2Velocity +{ + b2Vec2 v; + float w; +}; + +/// Solver Data +struct b2SolverData +{ + b2TimeStep step; + b2Position* positions; + b2Velocity* velocities; +}; + +#endif diff --git a/src/libraries/Box2D/b2_timer.h b/src/libraries/Box2D/b2_timer.h new file mode 100644 index 000000000..a664de833 --- /dev/null +++ b/src/libraries/Box2D/b2_timer.h @@ -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. + +#ifndef B2_TIMER_H +#define B2_TIMER_H + +#include "b2_settings.h" + +/// 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. + 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/src/libraries/Box2D/b2_weld_joint.h b/src/libraries/Box2D/b2_weld_joint.h new file mode 100644 index 000000000..fd5537f34 --- /dev/null +++ b/src/libraries/Box2D/b2_weld_joint.h @@ -0,0 +1,132 @@ +// 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_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 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 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 hz) { m_stiffness = hz; } + 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/src/libraries/Box2D/b2_wheel_joint.h b/src/libraries/Box2D/b2_wheel_joint.h new file mode 100644 index 000000000..1b8f43739 --- /dev/null +++ b/src/libraries/Box2D/b2_wheel_joint.h @@ -0,0 +1,239 @@ +// 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_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 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 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/src/libraries/Box2D/Dynamics/b2World.h b/src/libraries/Box2D/b2_world.h similarity index 82% rename from src/libraries/Box2D/Dynamics/b2World.h rename to src/libraries/Box2D/b2_world.h index 3968cc784..3ddc136e3 100644 --- a/src/libraries/Box2D/Dynamics/b2World.h +++ b/src/libraries/Box2D/b2_world.h @@ -1,30 +1,34 @@ -/* -* 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_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; @@ -62,7 +66,7 @@ public: 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 +95,7 @@ public: /// @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 +109,7 @@ public: 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 +126,19 @@ public: 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,7 +181,7 @@ public: /// 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); @@ -211,14 +215,6 @@ public: private: - // m_flags - enum - { - e_newFixture = 0x0001, - e_locked = 0x0002, - e_clearForces = 0x0004 - }; - friend class b2Body; friend class b2Fixture; friend class b2ContactManager; @@ -227,14 +223,11 @@ private: 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 +240,15 @@ private: 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 +317,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/src/libraries/Box2D/Dynamics/b2WorldCallbacks.h b/src/libraries/Box2D/b2_world_callbacks.h similarity index 76% rename from src/libraries/Box2D/Dynamics/b2WorldCallbacks.h rename to src/libraries/Box2D/b2_world_callbacks.h index 82ffc0229..e1e75c9e9 100644 --- a/src/libraries/Box2D/Dynamics/b2WorldCallbacks.h +++ b/src/libraries/Box2D/b2_world_callbacks.h @@ -1,25 +1,29 @@ -/* -* 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. -*/ +// 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 +#include "b2_settings.h" struct b2Vec2; struct b2Transform; @@ -64,8 +68,8 @@ public: /// match up one-to-one with the contact points in b2Manifold. struct b2ContactImpulse { - float32 normalImpulses[b2_maxManifoldPoints]; - float32 tangentImpulses[b2_maxManifoldPoints]; + float normalImpulses[b2_maxManifoldPoints]; + float tangentImpulses[b2_maxManifoldPoints]; int32 count; }; @@ -146,10 +150,11 @@ public: /// @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 float32 ReportFixture( b2Fixture* fixture, const b2Vec2& point, - const b2Vec2& normal, float32 fraction) = 0; + virtual float ReportFixture( b2Fixture* fixture, const b2Vec2& point, + const b2Vec2& normal, float fraction) = 0; }; #endif diff --git a/src/modules/physics/box2d/Body.cpp b/src/modules/physics/box2d/Body.cpp index 2f8301be0..05a287f8f 100644 --- a/src/modules/physics/box2d/Body.cpp +++ b/src/modules/physics/box2d/Body.cpp @@ -372,7 +372,7 @@ void Body::setBullet(bool bullet) bool Body::isActive() const { - return body->IsActive(); + return body->IsEnabled(); } bool Body::isAwake() const @@ -392,7 +392,7 @@ bool Body::isSleepingAllowed() const void Body::setActive(bool active) { - body->SetActive(active); + body->SetEnabled(active); } void Body::setAwake(bool awake) diff --git a/src/modules/physics/box2d/ChainShape.cpp b/src/modules/physics/box2d/ChainShape.cpp index 7b04c61a1..d0326d741 100644 --- a/src/modules/physics/box2d/ChainShape.cpp +++ b/src/modules/physics/box2d/ChainShape.cpp @@ -47,56 +47,44 @@ void ChainShape::setNextVertex(float x, float y) { b2Vec2 v(x, y); b2ChainShape *c = (b2ChainShape *)shape; - c->SetNextVertex(Physics::scaleDown(v)); + c->m_nextVertex = Physics::scaleDown(v); } void ChainShape::setNextVertex() { b2ChainShape *c = (b2ChainShape *)shape; - c->m_hasNextVertex = false; } void ChainShape::setPreviousVertex(float x, float y) { b2Vec2 v(x, y); b2ChainShape *c = (b2ChainShape *)shape; - c->SetPrevVertex(Physics::scaleDown(v)); + c->m_prevVertex = Physics::scaleDown(v); } void ChainShape::setPreviousVertex() { b2ChainShape *c = (b2ChainShape *)shape; - c->m_hasPrevVertex = false; } bool ChainShape::getNextVertex(float &x, float &y) const { b2ChainShape *c = (b2ChainShape *)shape; - if (c->m_hasNextVertex) - { - b2Vec2 v = Physics::scaleUp(c->m_nextVertex); - x = v.x; - y = v.y; - return true; - } - - return false; + b2Vec2 v = Physics::scaleUp(c->m_nextVertex); + x = v.x; + y = v.y; + return true; } bool ChainShape::getPreviousVertex(float &x, float &y) const { b2ChainShape *c = (b2ChainShape *)shape; - if (c->m_hasPrevVertex) - { - b2Vec2 v = Physics::scaleUp(c->m_prevVertex); - x = v.x; - y = v.y; - return true; - } - - return false; + b2Vec2 v = Physics::scaleUp(c->m_prevVertex); + x = v.x; + y = v.y; + return true; } EdgeShape *ChainShape::getChildEdge(int index) const diff --git a/src/modules/physics/box2d/DistanceJoint.cpp b/src/modules/physics/box2d/DistanceJoint.cpp index 3e7c27597..b1a67cd66 100644 --- a/src/modules/physics/box2d/DistanceJoint.cpp +++ b/src/modules/physics/box2d/DistanceJoint.cpp @@ -58,22 +58,22 @@ float DistanceJoint::getLength() const void DistanceJoint::setFrequency(float hz) { - joint->SetFrequency(hz); + joint->SetStiffness(hz); } float DistanceJoint::getFrequency() const { - return joint->GetFrequency(); + return joint->GetStiffness(); } void DistanceJoint::setDampingRatio(float d) { - joint->SetDampingRatio(d); + joint->SetDamping(d); } float DistanceJoint::getDampingRatio() const { - return joint->GetDampingRatio(); + return joint->GetDamping(); } diff --git a/src/modules/physics/box2d/EdgeShape.cpp b/src/modules/physics/box2d/EdgeShape.cpp index ebe1d0423..68cc4b09a 100644 --- a/src/modules/physics/box2d/EdgeShape.cpp +++ b/src/modules/physics/box2d/EdgeShape.cpp @@ -48,28 +48,21 @@ void EdgeShape::setNextVertex(float x, float y) b2EdgeShape *e = (b2EdgeShape *)shape; b2Vec2 v(x, y); e->m_vertex3 = Physics::scaleDown(v); - e->m_hasVertex3 = true; } void EdgeShape::setNextVertex() { b2EdgeShape *e = (b2EdgeShape *)shape; - e->m_hasVertex3 = false; } bool EdgeShape::getNextVertex(float &x, float &y) const { b2EdgeShape *e = (b2EdgeShape *)shape; - if (e->m_hasVertex3) - { - b2Vec2 v = Physics::scaleUp(e->m_vertex3); - x = v.x; - y = v.y; - return true; - } - - return false; + b2Vec2 v = Physics::scaleUp(e->m_vertex3); + x = v.x; + y = v.y; + return true; } void EdgeShape::setPreviousVertex(float x, float y) @@ -77,28 +70,21 @@ void EdgeShape::setPreviousVertex(float x, float y) b2EdgeShape *e = (b2EdgeShape *)shape; b2Vec2 v(x, y); e->m_vertex0 = Physics::scaleDown(v); - e->m_hasVertex0 = true; } void EdgeShape::setPreviousVertex() { b2EdgeShape *e = (b2EdgeShape *)shape; - e->m_hasVertex0 = false; } bool EdgeShape::getPreviousVertex(float &x, float &y) const { b2EdgeShape *e = (b2EdgeShape *)shape; - if (e->m_hasVertex0) - { - b2Vec2 v = Physics::scaleUp(e->m_vertex0); - x = v.x; - y = v.y; - return true; - } - - return false; + b2Vec2 v = Physics::scaleUp(e->m_vertex0); + x = v.x; + y = v.y; + return true; } int EdgeShape::getPoints(lua_State *L) diff --git a/src/modules/physics/box2d/Joint.cpp b/src/modules/physics/box2d/Joint.cpp index e73ee84d4..0cd08455d 100644 --- a/src/modules/physics/box2d/Joint.cpp +++ b/src/modules/physics/box2d/Joint.cpp @@ -187,7 +187,7 @@ void Joint::destroyJoint(bool implicit) bool Joint::isActive() const { - return joint->IsActive(); + return joint->IsEnabled(); } bool Joint::getCollideConnected() const diff --git a/src/modules/physics/box2d/MouseJoint.cpp b/src/modules/physics/box2d/MouseJoint.cpp index f9ae91745..814e507e2 100644 --- a/src/modules/physics/box2d/MouseJoint.cpp +++ b/src/modules/physics/box2d/MouseJoint.cpp @@ -86,22 +86,22 @@ void MouseJoint::setFrequency(float hz) if (hz <= FLT_EPSILON * 2) throw love::Exception("MouseJoint frequency must be a positive number."); - joint->SetFrequency(hz); + joint->SetStiffness(hz); } float MouseJoint::getFrequency() const { - return joint->GetFrequency(); + return joint->GetStiffness(); } void MouseJoint::setDampingRatio(float d) { - joint->SetDampingRatio(d); + joint->SetDamping(d); } float MouseJoint::getDampingRatio() const { - return joint->GetDampingRatio(); + return joint->GetDamping(); } Body *MouseJoint::getBodyA() const diff --git a/src/modules/physics/box2d/Physics.cpp b/src/modules/physics/box2d/Physics.cpp index 89fadd989..4bc447bc7 100644 --- a/src/modules/physics/box2d/Physics.cpp +++ b/src/modules/physics/box2d/Physics.cpp @@ -86,7 +86,7 @@ PolygonShape *Physics::newRectangleShape(float x, float y, float w, float h, flo EdgeShape *Physics::newEdgeShape(float x1, float y1, float x2, float y2) { b2EdgeShape *s = new b2EdgeShape(); - s->Set(Physics::scaleDown(b2Vec2(x1, y1)), Physics::scaleDown(b2Vec2(x2, y2))); + s->SetTwoSided(Physics::scaleDown(b2Vec2(x1, y1)), Physics::scaleDown(b2Vec2(x2, y2))); return new EdgeShape(s); } @@ -196,7 +196,7 @@ int Physics::newChainShape(lua_State *L) if (loop) s->CreateLoop(vecs, vcount); else - s->CreateChain(vecs, vcount); + s->CreateChain(vecs, vcount, vecs[0], vecs[vcount-1]); } catch (love::Exception &) { @@ -298,29 +298,7 @@ int Physics::getDistance(lua_State *L) { Fixture *fixtureA = luax_checktype(L, 1); Fixture *fixtureB = luax_checktype(L, 2); - b2DistanceProxy pA, pB; - b2DistanceInput i; - b2DistanceOutput o; - b2SimplexCache c; - c.count = 0; - - luax_catchexcept(L, [&]() { - pA.Set(fixtureA->fixture->GetShape(), 0); - pB.Set(fixtureB->fixture->GetShape(), 0); - i.proxyA = pA; - i.proxyB = pB; - i.transformA = fixtureA->fixture->GetBody()->GetTransform(); - i.transformB = fixtureB->fixture->GetBody()->GetTransform(); - i.useRadii = true; - b2Distance(&o, &c, &i); - }); - - lua_pushnumber(L, Physics::scaleUp(o.distance)); - lua_pushnumber(L, Physics::scaleUp(o.pointA.x)); - lua_pushnumber(L, Physics::scaleUp(o.pointA.y)); - lua_pushnumber(L, Physics::scaleUp(o.pointB.x)); - lua_pushnumber(L, Physics::scaleUp(o.pointB.y)); - return 5; + return 0; } void Physics::setMeter(float scale) diff --git a/src/modules/physics/box2d/PolygonShape.cpp b/src/modules/physics/box2d/PolygonShape.cpp index 436692ccc..b9b6e0c53 100644 --- a/src/modules/physics/box2d/PolygonShape.cpp +++ b/src/modules/physics/box2d/PolygonShape.cpp @@ -47,10 +47,10 @@ int PolygonShape::getPoints(lua_State *L) { love::luax_assert_argc(L, 0); b2PolygonShape *p = (b2PolygonShape *)shape; - int count = p->GetVertexCount(); + int count = p->m_count; for (int i = 0; iGetVertex(i)); + b2Vec2 v = Physics::scaleUp(p->m_vertices[i]); lua_pushnumber(L, v.x); lua_pushnumber(L, v.y); } diff --git a/src/modules/physics/box2d/WeldJoint.cpp b/src/modules/physics/box2d/WeldJoint.cpp index 9259ad46c..efdc6e948 100644 --- a/src/modules/physics/box2d/WeldJoint.cpp +++ b/src/modules/physics/box2d/WeldJoint.cpp @@ -68,22 +68,22 @@ void WeldJoint::init(b2WeldJointDef &def, Body *body1, Body *body2, float xA, fl void WeldJoint::setFrequency(float hz) { - joint->SetFrequency(hz); + joint->SetStiffness(hz); } float WeldJoint::getFrequency() const { - return joint->GetFrequency(); + return joint->GetStiffness(); } void WeldJoint::setDampingRatio(float d) { - joint->SetDampingRatio(d); + joint->SetDamping(d); } float WeldJoint::getDampingRatio() const { - return joint->GetDampingRatio(); + return joint->GetDamping(); } float WeldJoint::getReferenceAngle() const diff --git a/src/modules/physics/box2d/WheelJoint.cpp b/src/modules/physics/box2d/WheelJoint.cpp index 80fa529b9..1d3035ee9 100644 --- a/src/modules/physics/box2d/WheelJoint.cpp +++ b/src/modules/physics/box2d/WheelJoint.cpp @@ -57,7 +57,7 @@ float WheelJoint::getJointTranslation() const float WheelJoint::getJointSpeed() const { - return Physics::scaleUp(joint->GetJointSpeed()); + return Physics::scaleUp(joint->GetJointLinearSpeed()); } void WheelJoint::setMotorEnabled(bool enable) @@ -97,22 +97,22 @@ float WheelJoint::getMotorTorque(float inv_dt) const void WheelJoint::setSpringFrequency(float hz) { - joint->SetSpringFrequencyHz(hz); + joint->SetStiffness(hz); } float WheelJoint::getSpringFrequency() const { - return joint->GetSpringFrequencyHz(); + return joint->GetStiffness(); } void WheelJoint::setSpringDampingRatio(float ratio) { - joint->SetSpringDampingRatio(ratio); + joint->SetDamping(ratio); } float WheelJoint::getSpringDampingRatio() const { - return joint->GetSpringDampingRatio(); + return joint->GetDamping(); } int WheelJoint::getAxis(lua_State *L) diff --git a/src/modules/physics/box2d/World.cpp b/src/modules/physics/box2d/World.cpp index 2257f8443..aa05ff826 100644 --- a/src/modules/physics/box2d/World.cpp +++ b/src/modules/physics/box2d/World.cpp @@ -183,7 +183,7 @@ World::RayCastCallback::~RayCastCallback() { } -float32 World::RayCastCallback::ReportFixture(b2Fixture *fixture, const b2Vec2 &point, const b2Vec2 &normal, float32 fraction) +float World::RayCastCallback::ReportFixture(b2Fixture *fixture, const b2Vec2 &point, const b2Vec2 &normal, float fraction) { if (L != nullptr) { @@ -201,7 +201,7 @@ float32 World::RayCastCallback::ReportFixture(b2Fixture *fixture, const b2Vec2 & lua_call(L, 6, 1); if (!lua_isnumber(L, -1)) luaL_error(L, "Raycast callback didn't return a number!"); - float32 fraction = (float32) lua_tonumber(L, -1); + float fraction = (float) lua_tonumber(L, -1); lua_pop(L, 1); return fraction; } diff --git a/src/modules/physics/box2d/World.h b/src/modules/physics/box2d/World.h index 17102ee10..ea5ce3186 100644 --- a/src/modules/physics/box2d/World.h +++ b/src/modules/physics/box2d/World.h @@ -107,7 +107,7 @@ public: public: RayCastCallback(World *world, lua_State *L, int idx); ~RayCastCallback(); - virtual float32 ReportFixture(b2Fixture *fixture, const b2Vec2 &point, const b2Vec2 &normal, float32 fraction); + virtual float ReportFixture(b2Fixture *fixture, const b2Vec2 &point, const b2Vec2 &normal, float fraction); private: World *world; lua_State *L;