From d1b430de48805977bef5dd8566f29a3ef55100f1 Mon Sep 17 00:00:00 2001 From: halx99 Date: Fri, 4 Oct 2024 18:46:06 +0800 Subject: [PATCH] Make box2d 3.0 works and remove all chipmunk related --- 3rdparty/CMakeLists.txt | 16 - 3rdparty/README.md | 7 +- 3rdparty/box2d/CMakeLists.txt | 103 +- 3rdparty/box2d/include/box2d/b2_api.h | 52 - .../box2d/include/box2d/b2_block_allocator.h | 60 - 3rdparty/box2d/include/box2d/b2_body.h | 885 ----- 3rdparty/box2d/include/box2d/b2_broad_phase.h | 238 -- 3rdparty/box2d/include/box2d/b2_chain_shape.h | 101 - .../box2d/include/box2d/b2_circle_shape.h | 67 - 3rdparty/box2d/include/box2d/b2_collision.h | 306 -- 3rdparty/box2d/include/box2d/b2_common.h | 138 - 3rdparty/box2d/include/box2d/b2_contact.h | 386 -- .../box2d/include/box2d/b2_contact_manager.h | 57 - 3rdparty/box2d/include/box2d/b2_distance.h | 171 - .../box2d/include/box2d/b2_distance_joint.h | 176 - 3rdparty/box2d/include/box2d/b2_draw.h | 102 - .../box2d/include/box2d/b2_dynamic_tree.h | 308 -- 3rdparty/box2d/include/box2d/b2_edge_shape.h | 86 - 3rdparty/box2d/include/box2d/b2_fixture.h | 371 -- .../box2d/include/box2d/b2_friction_joint.h | 124 - 3rdparty/box2d/include/box2d/b2_gear_joint.h | 131 - .../box2d/include/box2d/b2_growable_stack.h | 91 - 3rdparty/box2d/include/box2d/b2_joint.h | 233 -- 3rdparty/box2d/include/box2d/b2_math.h | 717 ---- 3rdparty/box2d/include/box2d/b2_motor_joint.h | 138 - 3rdparty/box2d/include/box2d/b2_mouse_joint.h | 134 - .../box2d/include/box2d/b2_polygon_shape.h | 93 - .../box2d/include/box2d/b2_prismatic_joint.h | 205 - .../box2d/include/box2d/b2_pulley_joint.h | 157 - .../box2d/include/box2d/b2_revolute_joint.h | 211 - 3rdparty/box2d/include/box2d/b2_rope.h | 155 - 3rdparty/box2d/include/box2d/b2_settings.h | 127 - 3rdparty/box2d/include/box2d/b2_shape.h | 110 - .../box2d/include/box2d/b2_stack_allocator.h | 65 - .../box2d/include/box2d/b2_time_of_impact.h | 63 - 3rdparty/box2d/include/box2d/b2_time_step.h | 74 - 3rdparty/box2d/include/box2d/b2_timer.h | 55 - 3rdparty/box2d/include/box2d/b2_types.h | 33 - 3rdparty/box2d/include/box2d/b2_weld_joint.h | 133 - 3rdparty/box2d/include/box2d/b2_wheel_joint.h | 240 -- 3rdparty/box2d/include/box2d/b2_world.h | 348 -- .../box2d/include/box2d/b2_world_callbacks.h | 161 - 3rdparty/box2d/include/box2d/base.h | 113 + 3rdparty/box2d/include/box2d/box2d.h | 1156 +++++- 3rdparty/box2d/include/box2d/collision.h | 795 ++++ 3rdparty/box2d/include/box2d/id.h | 92 + 3rdparty/box2d/include/box2d/math_functions.h | 696 ++++ 3rdparty/box2d/include/box2d/types.h | 1315 +++++++ 3rdparty/box2d/src/CMakeLists.txt | 205 + 3rdparty/box2d/src/aabb.c | 155 + 3rdparty/box2d/src/aabb.h | 64 + 3rdparty/box2d/src/array.c | 8 + 3rdparty/box2d/src/array.h | 174 + 3rdparty/box2d/src/bitset.c | 67 + 3rdparty/box2d/src/bitset.h | 65 + 3rdparty/box2d/src/body.c | 1761 +++++++++ 3rdparty/box2d/src/body.h | 172 + 3rdparty/box2d/src/box2d.natvis | 27 + 3rdparty/box2d/src/broad_phase.c | 490 +++ 3rdparty/box2d/src/broad_phase.h | 83 + .../box2d/src/collision/b2_broad_phase.cpp | 131 - .../box2d/src/collision/b2_chain_shape.cpp | 185 - .../box2d/src/collision/b2_circle_shape.cpp | 105 - .../box2d/src/collision/b2_collide_circle.cpp | 158 - .../box2d/src/collision/b2_collide_edge.cpp | 524 --- .../src/collision/b2_collide_polygon.cpp | 243 -- 3rdparty/box2d/src/collision/b2_collision.cpp | 580 --- 3rdparty/box2d/src/collision/b2_distance.cpp | 744 ---- .../box2d/src/collision/b2_dynamic_tree.cpp | 801 ---- .../box2d/src/collision/b2_edge_shape.cpp | 158 - .../box2d/src/collision/b2_polygon_shape.cpp | 366 -- .../box2d/src/collision/b2_time_of_impact.cpp | 490 --- .../box2d/src/common/b2_block_allocator.cpp | 230 -- 3rdparty/box2d/src/common/b2_draw.cpp | 47 - 3rdparty/box2d/src/common/b2_math.cpp | 98 - 3rdparty/box2d/src/common/b2_settings.cpp | 74 - .../box2d/src/common/b2_stack_allocator.cpp | 87 - 3rdparty/box2d/src/common/b2_timer.cpp | 125 - 3rdparty/box2d/src/constraint_graph.c | 318 ++ 3rdparty/box2d/src/constraint_graph.h | 56 + 3rdparty/box2d/src/contact.c | 584 +++ 3rdparty/box2d/src/contact.h | 157 + 3rdparty/box2d/src/contact_solver.c | 2059 ++++++++++ 3rdparty/box2d/src/contact_solver.h | 50 + 3rdparty/box2d/src/core.c | 169 + 3rdparty/box2d/src/core.h | 183 + 3rdparty/box2d/src/ctz.h | 112 + 3rdparty/box2d/src/distance.c | 1237 ++++++ 3rdparty/box2d/src/distance_joint.c | 556 +++ 3rdparty/box2d/src/dynamic_tree.c | 1926 +++++++++ 3rdparty/box2d/src/dynamics/b2_body.cpp | 570 --- .../src/dynamics/b2_chain_circle_contact.cpp | 57 - .../src/dynamics/b2_chain_circle_contact.h | 43 - .../src/dynamics/b2_chain_polygon_contact.cpp | 57 - .../src/dynamics/b2_chain_polygon_contact.h | 43 - .../box2d/src/dynamics/b2_circle_contact.cpp | 56 - .../box2d/src/dynamics/b2_circle_contact.h | 43 - 3rdparty/box2d/src/dynamics/b2_contact.cpp | 252 -- .../box2d/src/dynamics/b2_contact_manager.cpp | 293 -- .../box2d/src/dynamics/b2_contact_solver.cpp | 843 ---- .../box2d/src/dynamics/b2_contact_solver.h | 100 - .../box2d/src/dynamics/b2_distance_joint.cpp | 421 -- .../src/dynamics/b2_edge_circle_contact.cpp | 54 - .../src/dynamics/b2_edge_circle_contact.h | 43 - .../src/dynamics/b2_edge_polygon_contact.cpp | 54 - .../src/dynamics/b2_edge_polygon_contact.h | 43 - 3rdparty/box2d/src/dynamics/b2_fixture.cpp | 305 -- .../box2d/src/dynamics/b2_friction_joint.cpp | 255 -- 3rdparty/box2d/src/dynamics/b2_gear_joint.cpp | 437 --- 3rdparty/box2d/src/dynamics/b2_island.cpp | 544 --- 3rdparty/box2d/src/dynamics/b2_island.h | 97 - 3rdparty/box2d/src/dynamics/b2_joint.cpp | 301 -- .../box2d/src/dynamics/b2_motor_joint.cpp | 311 -- .../box2d/src/dynamics/b2_mouse_joint.cpp | 190 - .../dynamics/b2_polygon_circle_contact.cpp | 54 - .../src/dynamics/b2_polygon_circle_contact.h | 42 - .../box2d/src/dynamics/b2_polygon_contact.cpp | 57 - .../box2d/src/dynamics/b2_polygon_contact.h | 43 - .../box2d/src/dynamics/b2_prismatic_joint.cpp | 643 --- .../box2d/src/dynamics/b2_pulley_joint.cpp | 352 -- .../box2d/src/dynamics/b2_revolute_joint.cpp | 501 --- 3rdparty/box2d/src/dynamics/b2_weld_joint.cpp | 344 -- .../box2d/src/dynamics/b2_wheel_joint.cpp | 672 ---- 3rdparty/box2d/src/dynamics/b2_world.cpp | 1322 ------- .../box2d/src/dynamics/b2_world_callbacks.cpp | 40 - 3rdparty/box2d/src/geometry.c | 893 +++++ 3rdparty/box2d/src/hull.c | 327 ++ 3rdparty/box2d/src/id_pool.c | 73 + 3rdparty/box2d/src/id_pool.h | 34 + 3rdparty/box2d/src/island.c | 979 +++++ 3rdparty/box2d/src/island.h | 90 + 3rdparty/box2d/src/joint.c | 1241 ++++++ 3rdparty/box2d/src/joint.h | 289 ++ 3rdparty/box2d/src/manifold.c | 1601 ++++++++ 3rdparty/box2d/src/math_functions.c | 147 + 3rdparty/box2d/src/motor_joint.c | 282 ++ 3rdparty/box2d/src/mouse_joint.c | 214 + 3rdparty/box2d/src/prismatic_joint.c | 600 +++ 3rdparty/box2d/src/revolute_joint.c | 547 +++ 3rdparty/box2d/src/rope/b2_rope.cpp | 809 ---- 3rdparty/box2d/src/shape.c | 1370 +++++++ 3rdparty/box2d/src/shape.h | 85 + 3rdparty/box2d/src/solver.c | 2045 ++++++++++ 3rdparty/box2d/src/solver.h | 157 + 3rdparty/box2d/src/solver_set.c | 613 +++ 3rdparty/box2d/src/solver_set.h | 57 + 3rdparty/box2d/src/stack_allocator.c | 121 + 3rdparty/box2d/src/stack_allocator.h | 38 + 3rdparty/box2d/src/table.c | 231 ++ 3rdparty/box2d/src/table.h | 37 + 3rdparty/box2d/src/timer.c | 202 + 3rdparty/box2d/src/types.c | 164 + 3rdparty/box2d/src/weld_joint.c | 298 ++ 3rdparty/box2d/src/wheel_joint.c | 554 +++ 3rdparty/box2d/src/world.c | 2972 ++++++++++++++ 3rdparty/box2d/src/world.h | 180 + 3rdparty/chipmunk/CMakeLists.txt | 72 - 3rdparty/chipmunk/include/chipmunk/chipmunk.h | 235 -- .../chipmunk/include/chipmunk/chipmunk_ffi.h | 105 - .../include/chipmunk/chipmunk_private.h | 344 -- .../include/chipmunk/chipmunk_structs.h | 450 --- .../include/chipmunk/chipmunk_types.h | 272 -- .../include/chipmunk/chipmunk_unsafe.h | 66 - .../chipmunk/include/chipmunk/cpArbiter.h | 145 - 3rdparty/chipmunk/include/chipmunk/cpBB.h | 187 - 3rdparty/chipmunk/include/chipmunk/cpBody.h | 189 - .../chipmunk/include/chipmunk/cpConstraint.h | 95 - .../include/chipmunk/cpDampedRotarySpring.h | 58 - .../include/chipmunk/cpDampedSpring.h | 68 - .../chipmunk/include/chipmunk/cpGearJoint.h | 45 - .../chipmunk/include/chipmunk/cpGrooveJoint.h | 50 - .../chipmunk/include/chipmunk/cpHastySpace.h | 27 - 3rdparty/chipmunk/include/chipmunk/cpMarch.h | 28 - .../chipmunk/include/chipmunk/cpPinJoint.h | 50 - .../chipmunk/include/chipmunk/cpPivotJoint.h | 47 - .../chipmunk/include/chipmunk/cpPolyShape.h | 56 - .../chipmunk/include/chipmunk/cpPolyline.h | 70 - .../include/chipmunk/cpRatchetJoint.h | 50 - 3rdparty/chipmunk/include/chipmunk/cpRobust.h | 11 - .../include/chipmunk/cpRotaryLimitJoint.h | 45 - 3rdparty/chipmunk/include/chipmunk/cpShape.h | 199 - .../chipmunk/include/chipmunk/cpSimpleMotor.h | 43 - .../chipmunk/include/chipmunk/cpSlideJoint.h | 55 - 3rdparty/chipmunk/include/chipmunk/cpSpace.h | 319 -- .../include/chipmunk/cpSpatialIndex.h | 227 -- .../chipmunk/include/chipmunk/cpTransform.h | 198 - 3rdparty/chipmunk/include/chipmunk/cpVect.h | 230 -- 3rdparty/chipmunk/src/CMakeLists.txt | 59 - 3rdparty/chipmunk/src/chipmunk.c | 331 -- 3rdparty/chipmunk/src/cpArbiter.c | 498 --- 3rdparty/chipmunk/src/cpArray.c | 101 - 3rdparty/chipmunk/src/cpBBTree.c | 896 ----- 3rdparty/chipmunk/src/cpBody.c | 626 --- 3rdparty/chipmunk/src/cpCollision.c | 726 ---- 3rdparty/chipmunk/src/cpConstraint.c | 173 - 3rdparty/chipmunk/src/cpDampedRotarySpring.c | 178 - 3rdparty/chipmunk/src/cpDampedSpring.c | 216 - 3rdparty/chipmunk/src/cpGearJoint.c | 145 - 3rdparty/chipmunk/src/cpGrooveJoint.c | 197 - 3rdparty/chipmunk/src/cpHashSet.c | 253 -- 3rdparty/chipmunk/src/cpHastySpace.c | 700 ---- 3rdparty/chipmunk/src/cpMarch.c | 157 - 3rdparty/chipmunk/src/cpPinJoint.c | 172 - 3rdparty/chipmunk/src/cpPivotJoint.c | 152 - 3rdparty/chipmunk/src/cpPolyShape.c | 324 -- 3rdparty/chipmunk/src/cpPolyline.c | 652 --- 3rdparty/chipmunk/src/cpRatchetJoint.c | 179 - 3rdparty/chipmunk/src/cpRobust.c | 13 - 3rdparty/chipmunk/src/cpRotaryLimitJoint.c | 160 - 3rdparty/chipmunk/src/cpShape.c | 604 --- 3rdparty/chipmunk/src/cpSimpleMotor.c | 123 - 3rdparty/chipmunk/src/cpSlideJoint.c | 195 - 3rdparty/chipmunk/src/cpSpace.c | 701 ---- 3rdparty/chipmunk/src/cpSpaceComponent.c | 349 -- 3rdparty/chipmunk/src/cpSpaceDebug.c | 187 - 3rdparty/chipmunk/src/cpSpaceHash.c | 634 --- 3rdparty/chipmunk/src/cpSpaceQuery.c | 246 -- 3rdparty/chipmunk/src/cpSpaceStep.c | 445 --- 3rdparty/chipmunk/src/cpSpatialIndex.c | 69 - 3rdparty/chipmunk/src/cpSweep1D.c | 254 -- 3rdparty/chipmunk/src/prime.h | 68 - core/2d/Node.cpp | 2 +- core/2d/Node.h | 4 +- core/2d/Scene.cpp | 22 +- core/2d/Scene.h | 10 +- core/axmol.h | 10 +- core/base/Config.h | 21 - core/base/Director.cpp | 4 +- core/physics/CMakeLists.txt | 18 +- core/physics/PhysicsBody.cpp | 1027 ----- core/physics/PhysicsBody.h | 630 --- core/physics/PhysicsContact.cpp | 435 --- core/physics/PhysicsContact.h | 327 -- core/physics/PhysicsHelper.h | 117 - core/physics/PhysicsJoint.cpp | 936 ----- core/physics/PhysicsJoint.h | 609 --- core/physics/PhysicsShape.cpp | 980 ----- core/physics/PhysicsShape.h | 791 ---- core/physics/PhysicsWorld.cpp | 1107 ------ core/physics/PhysicsWorld.h | 481 --- core/physics/cpCompat62.h | 122 - core/physics3d/Physics3D.cpp | 6 - core/physics3d/Physics3D.h | 4 - core/physics3d/Physics3DComponent.cpp | 4 - core/physics3d/Physics3DComponent.h | 4 - core/physics3d/Physics3DConstraint.cpp | 4 - core/physics3d/Physics3DConstraint.h | 6 - core/physics3d/Physics3DDebugDrawer.cpp | 4 - core/physics3d/Physics3DDebugDrawer.h | 3 - core/physics3d/Physics3DObject.cpp | 4 - core/physics3d/Physics3DObject.h | 4 - core/physics3d/Physics3DShape.cpp | 7 - core/physics3d/Physics3DShape.h | 8 - core/physics3d/Physics3DWorld.cpp | 4 - core/physics3d/Physics3DWorld.h | 6 - core/physics3d/PhysicsMeshRenderer.cpp | 4 - core/physics3d/PhysicsMeshRenderer.h | 4 - extensions/README.md | 2 +- extensions/axmol-ext.h | 4 - extensions/physics-nodes/CMakeLists.txt | 1 + .../src/physics-nodes/PhysicsDebugNode.cpp | 341 +- .../src/physics-nodes/PhysicsDebugNode.h | 96 +- .../physics-nodes/PhysicsDebugNodeBox2D.cpp | 113 - .../src/physics-nodes/PhysicsDebugNodeBox2D.h | 69 - .../PhysicsDebugNodeChipmunk2D.cpp | 254 -- .../PhysicsDebugNodeChipmunk2D.h | 75 - .../src/physics-nodes/PhysicsSprite.cpp | 152 +- .../src/physics-nodes/PhysicsSprite.h | 31 +- .../src/physics-nodes/PhysicsSpriteBox2D.cpp | 352 -- .../src/physics-nodes/PhysicsSpriteBox2D.h | 137 - .../physics-nodes/PhysicsSpriteChipmunk2D.cpp | 329 -- .../physics-nodes/PhysicsSpriteChipmunk2D.h | 136 - tests/cpp-tests/CMakeLists.txt | 109 +- tests/cpp-tests/Source/BaseTest.cpp | 11 + tests/cpp-tests/Source/BaseTest.h | 2 + .../cpp-tests/Source/Box2DTest/Box2dTest.cpp | 219 +- tests/cpp-tests/Source/Box2DTest/Box2dTest.h | 11 +- .../Source/Box2DTestBed/Box2DTestBed.cpp | 394 +- .../Source/Box2DTestBed/Box2DTestBed.h | 54 +- .../Box2DTestBed/Box2DTestDebugDrawNode.cpp | 423 ++ .../Box2DTestBed/Box2DTestDebugDrawNode.h | 105 + .../Box2DTestBed/samples/CMakeLists.txt | 111 + .../samples/LockLessMultiReadPipe.h | 283 ++ .../Box2DTestBed/samples/TaskScheduler.cpp | 1547 ++++++++ .../Box2DTestBed/samples/TaskScheduler.h | 583 +++ .../Source/Box2DTestBed/samples/car.cpp | 295 ++ .../Source/Box2DTestBed/samples/car.h | 49 + .../Box2DTestBed/samples/data/background.fs | 33 + .../Box2DTestBed/samples/data/background.vs | 10 + .../Box2DTestBed/samples/data/circle.fs | 23 + .../Box2DTestBed/samples/data/circle.vs | 29 + .../Box2DTestBed/samples/data/droid_sans.ttf | Bin 0 -> 190044 bytes .../samples/data/solid_capsule.fs | 61 + .../samples/data/solid_capsule.vs | 44 + .../Box2DTestBed/samples/data/solid_circle.fs | 56 + .../Box2DTestBed/samples/data/solid_circle.vs | 34 + .../samples/data/solid_polygon.fs | 106 + .../samples/data/solid_polygon.vs | 79 + .../Source/Box2DTestBed/samples/donut.cpp | 96 + .../Source/Box2DTestBed/samples/donut.h | 24 + .../Source/Box2DTestBed/samples/doohickey.cpp | 101 + .../Source/Box2DTestBed/samples/doohickey.h | 26 + .../Source/Box2DTestBed/samples/draw.h | 3 + .../Source/Box2DTestBed/samples/human.cpp | 573 +++ .../Source/Box2DTestBed/samples/human.h | 49 + .../Source/Box2DTestBed/samples/jsmn/jsmn.h | 471 +++ .../Source/Box2DTestBed/samples/sample.cpp | 511 +++ .../Source/Box2DTestBed/samples/sample.h | 169 + .../Box2DTestBed/samples/sample_benchmark.cpp | 1531 ++++++++ .../Box2DTestBed/samples/sample_bodies.cpp | 839 ++++ .../Box2DTestBed/samples/sample_collision.cpp | 3478 +++++++++++++++++ .../samples/sample_continuous.cpp | 955 +++++ .../samples/sample_determinism.cpp | 188 + .../Box2DTestBed/samples/sample_events.cpp | 1114 ++++++ .../Box2DTestBed/samples/sample_geometry.cpp | 220 ++ .../Box2DTestBed/samples/sample_joints.cpp | 2467 ++++++++++++ .../samples/sample_robustness.cpp | 261 ++ .../Box2DTestBed/samples/sample_shapes.cpp | 1304 ++++++ .../Box2DTestBed/samples/sample_stacking.cpp | 889 +++++ .../Box2DTestBed/samples/sample_world.cpp | 242 ++ .../Source/Box2DTestBed/samples/settings.cpp | 136 + .../Source/Box2DTestBed/samples/settings.h | 43 + tests/cpp-tests/Source/Box2DTestBed/test.cpp | 514 --- .../Source/Box2DTestBed/tests/add_pair.cpp | 67 - .../Source/Box2DTestBed/tests/apply_force.cpp | 199 - .../Source/Box2DTestBed/tests/body_types.cpp | 158 - .../Source/Box2DTestBed/tests/box_stack.cpp | 170 - .../Source/Box2DTestBed/tests/breakable.cpp | 154 - .../Source/Box2DTestBed/tests/bridge.cpp | 124 - .../Source/Box2DTestBed/tests/bullet_test.cpp | 132 - .../Source/Box2DTestBed/tests/cantilever.cpp | 214 - .../Source/Box2DTestBed/tests/car.cpp | 286 -- .../Source/Box2DTestBed/tests/chain.cpp | 89 - .../Box2DTestBed/tests/chain_problem.cpp | 89 - .../tests/character_collision.cpp | 252 -- .../Box2DTestBed/tests/circle_stack.cpp | 85 - .../tests/collision_filtering.cpp | 176 - .../tests/collision_processing.cpp | 189 - .../Box2DTestBed/tests/compound_shapes.cpp | 224 -- .../Source/Box2DTestBed/tests/confined.cpp | 165 - .../Box2DTestBed/tests/continuous_test.cpp | 162 - .../Source/Box2DTestBed/tests/convex_hull.cpp | 108 - .../Box2DTestBed/tests/conveyor_belt.cpp | 94 - .../Box2DTestBed/tests/distance_joint.cpp | 120 - .../Box2DTestBed/tests/distance_test.cpp | 134 - .../Source/Box2DTestBed/tests/dominos.cpp | 216 - .../Source/Box2DTestBed/tests/dump_loader.cpp | 82 - .../Box2DTestBed/tests/dynamic_tree.cpp | 357 -- .../Source/Box2DTestBed/tests/edge_shapes.cpp | 244 -- .../Source/Box2DTestBed/tests/edge_test.cpp | 267 -- .../Source/Box2DTestBed/tests/friction.cpp | 123 - .../Source/Box2DTestBed/tests/gear_joint.cpp | 175 - .../Source/Box2DTestBed/tests/heavy1.cpp | 57 - .../Source/Box2DTestBed/tests/heavy2.cpp | 90 - .../Box2DTestBed/tests/mobile_balanced.cpp | 104 - .../Box2DTestBed/tests/mobile_unbalanced.cpp | 101 - .../Source/Box2DTestBed/tests/motor_joint.cpp | 115 - .../Source/Box2DTestBed/tests/pinball.cpp | 165 - .../Source/Box2DTestBed/tests/platformer.cpp | 128 - .../Box2DTestBed/tests/polygon_collision.cpp | 123 - .../Box2DTestBed/tests/polygon_shapes.cpp | 257 -- .../Box2DTestBed/tests/prismatic_joint.cpp | 114 - .../Box2DTestBed/tests/pulley_joint.cpp | 92 - .../Source/Box2DTestBed/tests/pyramid.cpp | 89 - .../Source/Box2DTestBed/tests/ray_cast.cpp | 463 --- .../Source/Box2DTestBed/tests/restitution.cpp | 75 - .../Box2DTestBed/tests/revolute_joint.cpp | 157 - .../Source/Box2DTestBed/tests/rope.cpp | 282 -- .../Source/Box2DTestBed/tests/sensor.cpp | 191 - .../Source/Box2DTestBed/tests/settings.h | 83 - .../Source/Box2DTestBed/tests/shape_cast.cpp | 189 - .../Box2DTestBed/tests/shape_editing.cpp | 103 - .../Source/Box2DTestBed/tests/skier.cpp | 146 - .../Box2DTestBed/tests/slider_crank_1.cpp | 103 - .../Box2DTestBed/tests/slider_crank_2.cpp | 156 - .../Source/Box2DTestBed/tests/test.h | 150 - .../Source/Box2DTestBed/tests/theo_jansen.cpp | 261 -- .../Source/Box2DTestBed/tests/tiles.cpp | 153 - .../Box2DTestBed/tests/time_of_impact.cpp | 126 - .../Source/Box2DTestBed/tests/tumbler.cpp | 98 - .../Source/Box2DTestBed/tests/web.cpp | 215 - .../Source/Box2DTestBed/tests/wheel_joint.cpp | 121 - .../Box2DTestBed/tests/wrecking_ball.cpp | 161 - .../Source/ChipmunkTest/ChipmunkTest.cpp | 279 -- .../Source/ChipmunkTest/ChipmunkTest.h | 66 - .../Source/DrawNodeTest/DrawNodeTest.cpp | 4 +- .../Source/PhysicsTest/PhysicsTest.cpp | 2 +- .../Source/PhysicsTest/PhysicsTest.h | 2 +- tests/cpp-tests/Source/controller.cpp | 8 +- tests/cpp-tests/Source/shaders/circle.fs | 23 + tests/cpp-tests/Source/shaders/circle.vs | 31 + .../cpp-tests/Source/shaders/solid_capsule.fs | 61 + .../cpp-tests/Source/shaders/solid_capsule.vs | 46 + .../cpp-tests/Source/shaders/solid_circle.fs | 56 + .../cpp-tests/Source/shaders/solid_circle.vs | 36 + tests/cpp-tests/Source/tests.h | 5 - 396 files changed, 51700 insertions(+), 58108 deletions(-) delete mode 100644 3rdparty/box2d/include/box2d/b2_api.h delete mode 100644 3rdparty/box2d/include/box2d/b2_block_allocator.h delete mode 100644 3rdparty/box2d/include/box2d/b2_body.h delete mode 100644 3rdparty/box2d/include/box2d/b2_broad_phase.h delete mode 100644 3rdparty/box2d/include/box2d/b2_chain_shape.h delete mode 100644 3rdparty/box2d/include/box2d/b2_circle_shape.h delete mode 100644 3rdparty/box2d/include/box2d/b2_collision.h delete mode 100644 3rdparty/box2d/include/box2d/b2_common.h delete mode 100644 3rdparty/box2d/include/box2d/b2_contact.h delete mode 100644 3rdparty/box2d/include/box2d/b2_contact_manager.h delete mode 100644 3rdparty/box2d/include/box2d/b2_distance.h delete mode 100644 3rdparty/box2d/include/box2d/b2_distance_joint.h delete mode 100644 3rdparty/box2d/include/box2d/b2_draw.h delete mode 100644 3rdparty/box2d/include/box2d/b2_dynamic_tree.h delete mode 100644 3rdparty/box2d/include/box2d/b2_edge_shape.h delete mode 100644 3rdparty/box2d/include/box2d/b2_fixture.h delete mode 100644 3rdparty/box2d/include/box2d/b2_friction_joint.h delete mode 100644 3rdparty/box2d/include/box2d/b2_gear_joint.h delete mode 100644 3rdparty/box2d/include/box2d/b2_growable_stack.h delete mode 100644 3rdparty/box2d/include/box2d/b2_joint.h delete mode 100644 3rdparty/box2d/include/box2d/b2_math.h delete mode 100644 3rdparty/box2d/include/box2d/b2_motor_joint.h delete mode 100644 3rdparty/box2d/include/box2d/b2_mouse_joint.h delete mode 100644 3rdparty/box2d/include/box2d/b2_polygon_shape.h delete mode 100644 3rdparty/box2d/include/box2d/b2_prismatic_joint.h delete mode 100644 3rdparty/box2d/include/box2d/b2_pulley_joint.h delete mode 100644 3rdparty/box2d/include/box2d/b2_revolute_joint.h delete mode 100644 3rdparty/box2d/include/box2d/b2_rope.h delete mode 100644 3rdparty/box2d/include/box2d/b2_settings.h delete mode 100644 3rdparty/box2d/include/box2d/b2_shape.h delete mode 100644 3rdparty/box2d/include/box2d/b2_stack_allocator.h delete mode 100644 3rdparty/box2d/include/box2d/b2_time_of_impact.h delete mode 100644 3rdparty/box2d/include/box2d/b2_time_step.h delete mode 100644 3rdparty/box2d/include/box2d/b2_timer.h delete mode 100644 3rdparty/box2d/include/box2d/b2_types.h delete mode 100644 3rdparty/box2d/include/box2d/b2_weld_joint.h delete mode 100644 3rdparty/box2d/include/box2d/b2_wheel_joint.h delete mode 100644 3rdparty/box2d/include/box2d/b2_world.h delete mode 100644 3rdparty/box2d/include/box2d/b2_world_callbacks.h create mode 100644 3rdparty/box2d/include/box2d/base.h create mode 100644 3rdparty/box2d/include/box2d/collision.h create mode 100644 3rdparty/box2d/include/box2d/id.h create mode 100644 3rdparty/box2d/include/box2d/math_functions.h create mode 100644 3rdparty/box2d/include/box2d/types.h create mode 100644 3rdparty/box2d/src/CMakeLists.txt create mode 100644 3rdparty/box2d/src/aabb.c create mode 100644 3rdparty/box2d/src/aabb.h create mode 100644 3rdparty/box2d/src/array.c create mode 100644 3rdparty/box2d/src/array.h create mode 100644 3rdparty/box2d/src/bitset.c create mode 100644 3rdparty/box2d/src/bitset.h create mode 100644 3rdparty/box2d/src/body.c create mode 100644 3rdparty/box2d/src/body.h create mode 100644 3rdparty/box2d/src/box2d.natvis create mode 100644 3rdparty/box2d/src/broad_phase.c create mode 100644 3rdparty/box2d/src/broad_phase.h delete mode 100644 3rdparty/box2d/src/collision/b2_broad_phase.cpp delete mode 100644 3rdparty/box2d/src/collision/b2_chain_shape.cpp delete mode 100644 3rdparty/box2d/src/collision/b2_circle_shape.cpp delete mode 100644 3rdparty/box2d/src/collision/b2_collide_circle.cpp delete mode 100644 3rdparty/box2d/src/collision/b2_collide_edge.cpp delete mode 100644 3rdparty/box2d/src/collision/b2_collide_polygon.cpp delete mode 100644 3rdparty/box2d/src/collision/b2_collision.cpp delete mode 100644 3rdparty/box2d/src/collision/b2_distance.cpp delete mode 100644 3rdparty/box2d/src/collision/b2_dynamic_tree.cpp delete mode 100644 3rdparty/box2d/src/collision/b2_edge_shape.cpp delete mode 100644 3rdparty/box2d/src/collision/b2_polygon_shape.cpp delete mode 100644 3rdparty/box2d/src/collision/b2_time_of_impact.cpp delete mode 100644 3rdparty/box2d/src/common/b2_block_allocator.cpp delete mode 100644 3rdparty/box2d/src/common/b2_draw.cpp delete mode 100644 3rdparty/box2d/src/common/b2_math.cpp delete mode 100644 3rdparty/box2d/src/common/b2_settings.cpp delete mode 100644 3rdparty/box2d/src/common/b2_stack_allocator.cpp delete mode 100644 3rdparty/box2d/src/common/b2_timer.cpp create mode 100644 3rdparty/box2d/src/constraint_graph.c create mode 100644 3rdparty/box2d/src/constraint_graph.h create mode 100644 3rdparty/box2d/src/contact.c create mode 100644 3rdparty/box2d/src/contact.h create mode 100644 3rdparty/box2d/src/contact_solver.c create mode 100644 3rdparty/box2d/src/contact_solver.h create mode 100644 3rdparty/box2d/src/core.c create mode 100644 3rdparty/box2d/src/core.h create mode 100644 3rdparty/box2d/src/ctz.h create mode 100644 3rdparty/box2d/src/distance.c create mode 100644 3rdparty/box2d/src/distance_joint.c create mode 100644 3rdparty/box2d/src/dynamic_tree.c delete mode 100644 3rdparty/box2d/src/dynamics/b2_body.cpp delete mode 100644 3rdparty/box2d/src/dynamics/b2_chain_circle_contact.cpp delete mode 100644 3rdparty/box2d/src/dynamics/b2_chain_circle_contact.h delete mode 100644 3rdparty/box2d/src/dynamics/b2_chain_polygon_contact.cpp delete mode 100644 3rdparty/box2d/src/dynamics/b2_chain_polygon_contact.h delete mode 100644 3rdparty/box2d/src/dynamics/b2_circle_contact.cpp delete mode 100644 3rdparty/box2d/src/dynamics/b2_circle_contact.h delete mode 100644 3rdparty/box2d/src/dynamics/b2_contact.cpp delete mode 100644 3rdparty/box2d/src/dynamics/b2_contact_manager.cpp delete mode 100644 3rdparty/box2d/src/dynamics/b2_contact_solver.cpp delete mode 100644 3rdparty/box2d/src/dynamics/b2_contact_solver.h delete mode 100644 3rdparty/box2d/src/dynamics/b2_distance_joint.cpp delete mode 100644 3rdparty/box2d/src/dynamics/b2_edge_circle_contact.cpp delete mode 100644 3rdparty/box2d/src/dynamics/b2_edge_circle_contact.h delete mode 100644 3rdparty/box2d/src/dynamics/b2_edge_polygon_contact.cpp delete mode 100644 3rdparty/box2d/src/dynamics/b2_edge_polygon_contact.h delete mode 100644 3rdparty/box2d/src/dynamics/b2_fixture.cpp delete mode 100644 3rdparty/box2d/src/dynamics/b2_friction_joint.cpp delete mode 100644 3rdparty/box2d/src/dynamics/b2_gear_joint.cpp delete mode 100644 3rdparty/box2d/src/dynamics/b2_island.cpp delete mode 100644 3rdparty/box2d/src/dynamics/b2_island.h delete mode 100644 3rdparty/box2d/src/dynamics/b2_joint.cpp delete mode 100644 3rdparty/box2d/src/dynamics/b2_motor_joint.cpp delete mode 100644 3rdparty/box2d/src/dynamics/b2_mouse_joint.cpp delete mode 100644 3rdparty/box2d/src/dynamics/b2_polygon_circle_contact.cpp delete mode 100644 3rdparty/box2d/src/dynamics/b2_polygon_circle_contact.h delete mode 100644 3rdparty/box2d/src/dynamics/b2_polygon_contact.cpp delete mode 100644 3rdparty/box2d/src/dynamics/b2_polygon_contact.h delete mode 100644 3rdparty/box2d/src/dynamics/b2_prismatic_joint.cpp delete mode 100644 3rdparty/box2d/src/dynamics/b2_pulley_joint.cpp delete mode 100644 3rdparty/box2d/src/dynamics/b2_revolute_joint.cpp delete mode 100644 3rdparty/box2d/src/dynamics/b2_weld_joint.cpp delete mode 100644 3rdparty/box2d/src/dynamics/b2_wheel_joint.cpp delete mode 100644 3rdparty/box2d/src/dynamics/b2_world.cpp delete mode 100644 3rdparty/box2d/src/dynamics/b2_world_callbacks.cpp create mode 100644 3rdparty/box2d/src/geometry.c create mode 100644 3rdparty/box2d/src/hull.c create mode 100644 3rdparty/box2d/src/id_pool.c create mode 100644 3rdparty/box2d/src/id_pool.h create mode 100644 3rdparty/box2d/src/island.c create mode 100644 3rdparty/box2d/src/island.h create mode 100644 3rdparty/box2d/src/joint.c create mode 100644 3rdparty/box2d/src/joint.h create mode 100644 3rdparty/box2d/src/manifold.c create mode 100644 3rdparty/box2d/src/math_functions.c create mode 100644 3rdparty/box2d/src/motor_joint.c create mode 100644 3rdparty/box2d/src/mouse_joint.c create mode 100644 3rdparty/box2d/src/prismatic_joint.c create mode 100644 3rdparty/box2d/src/revolute_joint.c delete mode 100644 3rdparty/box2d/src/rope/b2_rope.cpp create mode 100644 3rdparty/box2d/src/shape.c create mode 100644 3rdparty/box2d/src/shape.h create mode 100644 3rdparty/box2d/src/solver.c create mode 100644 3rdparty/box2d/src/solver.h create mode 100644 3rdparty/box2d/src/solver_set.c create mode 100644 3rdparty/box2d/src/solver_set.h create mode 100644 3rdparty/box2d/src/stack_allocator.c create mode 100644 3rdparty/box2d/src/stack_allocator.h create mode 100644 3rdparty/box2d/src/table.c create mode 100644 3rdparty/box2d/src/table.h create mode 100644 3rdparty/box2d/src/timer.c create mode 100644 3rdparty/box2d/src/types.c create mode 100644 3rdparty/box2d/src/weld_joint.c create mode 100644 3rdparty/box2d/src/wheel_joint.c create mode 100644 3rdparty/box2d/src/world.c create mode 100644 3rdparty/box2d/src/world.h delete mode 100644 3rdparty/chipmunk/CMakeLists.txt delete mode 100644 3rdparty/chipmunk/include/chipmunk/chipmunk.h delete mode 100644 3rdparty/chipmunk/include/chipmunk/chipmunk_ffi.h delete mode 100644 3rdparty/chipmunk/include/chipmunk/chipmunk_private.h delete mode 100644 3rdparty/chipmunk/include/chipmunk/chipmunk_structs.h delete mode 100644 3rdparty/chipmunk/include/chipmunk/chipmunk_types.h delete mode 100644 3rdparty/chipmunk/include/chipmunk/chipmunk_unsafe.h delete mode 100644 3rdparty/chipmunk/include/chipmunk/cpArbiter.h delete mode 100644 3rdparty/chipmunk/include/chipmunk/cpBB.h delete mode 100644 3rdparty/chipmunk/include/chipmunk/cpBody.h delete mode 100644 3rdparty/chipmunk/include/chipmunk/cpConstraint.h delete mode 100644 3rdparty/chipmunk/include/chipmunk/cpDampedRotarySpring.h delete mode 100644 3rdparty/chipmunk/include/chipmunk/cpDampedSpring.h delete mode 100644 3rdparty/chipmunk/include/chipmunk/cpGearJoint.h delete mode 100644 3rdparty/chipmunk/include/chipmunk/cpGrooveJoint.h delete mode 100644 3rdparty/chipmunk/include/chipmunk/cpHastySpace.h delete mode 100644 3rdparty/chipmunk/include/chipmunk/cpMarch.h delete mode 100644 3rdparty/chipmunk/include/chipmunk/cpPinJoint.h delete mode 100644 3rdparty/chipmunk/include/chipmunk/cpPivotJoint.h delete mode 100644 3rdparty/chipmunk/include/chipmunk/cpPolyShape.h delete mode 100644 3rdparty/chipmunk/include/chipmunk/cpPolyline.h delete mode 100644 3rdparty/chipmunk/include/chipmunk/cpRatchetJoint.h delete mode 100644 3rdparty/chipmunk/include/chipmunk/cpRobust.h delete mode 100644 3rdparty/chipmunk/include/chipmunk/cpRotaryLimitJoint.h delete mode 100644 3rdparty/chipmunk/include/chipmunk/cpShape.h delete mode 100644 3rdparty/chipmunk/include/chipmunk/cpSimpleMotor.h delete mode 100644 3rdparty/chipmunk/include/chipmunk/cpSlideJoint.h delete mode 100644 3rdparty/chipmunk/include/chipmunk/cpSpace.h delete mode 100644 3rdparty/chipmunk/include/chipmunk/cpSpatialIndex.h delete mode 100644 3rdparty/chipmunk/include/chipmunk/cpTransform.h delete mode 100644 3rdparty/chipmunk/include/chipmunk/cpVect.h delete mode 100644 3rdparty/chipmunk/src/CMakeLists.txt delete mode 100644 3rdparty/chipmunk/src/chipmunk.c delete mode 100644 3rdparty/chipmunk/src/cpArbiter.c delete mode 100644 3rdparty/chipmunk/src/cpArray.c delete mode 100644 3rdparty/chipmunk/src/cpBBTree.c delete mode 100644 3rdparty/chipmunk/src/cpBody.c delete mode 100644 3rdparty/chipmunk/src/cpCollision.c delete mode 100644 3rdparty/chipmunk/src/cpConstraint.c delete mode 100644 3rdparty/chipmunk/src/cpDampedRotarySpring.c delete mode 100644 3rdparty/chipmunk/src/cpDampedSpring.c delete mode 100644 3rdparty/chipmunk/src/cpGearJoint.c delete mode 100644 3rdparty/chipmunk/src/cpGrooveJoint.c delete mode 100644 3rdparty/chipmunk/src/cpHashSet.c delete mode 100644 3rdparty/chipmunk/src/cpHastySpace.c delete mode 100644 3rdparty/chipmunk/src/cpMarch.c delete mode 100644 3rdparty/chipmunk/src/cpPinJoint.c delete mode 100644 3rdparty/chipmunk/src/cpPivotJoint.c delete mode 100644 3rdparty/chipmunk/src/cpPolyShape.c delete mode 100644 3rdparty/chipmunk/src/cpPolyline.c delete mode 100644 3rdparty/chipmunk/src/cpRatchetJoint.c delete mode 100644 3rdparty/chipmunk/src/cpRobust.c delete mode 100644 3rdparty/chipmunk/src/cpRotaryLimitJoint.c delete mode 100644 3rdparty/chipmunk/src/cpShape.c delete mode 100644 3rdparty/chipmunk/src/cpSimpleMotor.c delete mode 100644 3rdparty/chipmunk/src/cpSlideJoint.c delete mode 100644 3rdparty/chipmunk/src/cpSpace.c delete mode 100644 3rdparty/chipmunk/src/cpSpaceComponent.c delete mode 100644 3rdparty/chipmunk/src/cpSpaceDebug.c delete mode 100644 3rdparty/chipmunk/src/cpSpaceHash.c delete mode 100644 3rdparty/chipmunk/src/cpSpaceQuery.c delete mode 100644 3rdparty/chipmunk/src/cpSpaceStep.c delete mode 100644 3rdparty/chipmunk/src/cpSpatialIndex.c delete mode 100644 3rdparty/chipmunk/src/cpSweep1D.c delete mode 100644 3rdparty/chipmunk/src/prime.h delete mode 100644 core/physics/PhysicsBody.cpp delete mode 100644 core/physics/PhysicsBody.h delete mode 100644 core/physics/PhysicsContact.cpp delete mode 100644 core/physics/PhysicsContact.h delete mode 100644 core/physics/PhysicsHelper.h delete mode 100644 core/physics/PhysicsJoint.cpp delete mode 100644 core/physics/PhysicsJoint.h delete mode 100644 core/physics/PhysicsShape.cpp delete mode 100644 core/physics/PhysicsShape.h delete mode 100644 core/physics/PhysicsWorld.cpp delete mode 100644 core/physics/PhysicsWorld.h delete mode 100644 core/physics/cpCompat62.h delete mode 100644 extensions/physics-nodes/src/physics-nodes/PhysicsDebugNodeBox2D.cpp delete mode 100644 extensions/physics-nodes/src/physics-nodes/PhysicsDebugNodeBox2D.h delete mode 100644 extensions/physics-nodes/src/physics-nodes/PhysicsDebugNodeChipmunk2D.cpp delete mode 100644 extensions/physics-nodes/src/physics-nodes/PhysicsDebugNodeChipmunk2D.h delete mode 100644 extensions/physics-nodes/src/physics-nodes/PhysicsSpriteBox2D.cpp delete mode 100644 extensions/physics-nodes/src/physics-nodes/PhysicsSpriteBox2D.h delete mode 100644 extensions/physics-nodes/src/physics-nodes/PhysicsSpriteChipmunk2D.cpp delete mode 100644 extensions/physics-nodes/src/physics-nodes/PhysicsSpriteChipmunk2D.h create mode 100644 tests/cpp-tests/Source/Box2DTestBed/Box2DTestDebugDrawNode.cpp create mode 100644 tests/cpp-tests/Source/Box2DTestBed/Box2DTestDebugDrawNode.h create mode 100644 tests/cpp-tests/Source/Box2DTestBed/samples/CMakeLists.txt create mode 100644 tests/cpp-tests/Source/Box2DTestBed/samples/LockLessMultiReadPipe.h create mode 100644 tests/cpp-tests/Source/Box2DTestBed/samples/TaskScheduler.cpp create mode 100644 tests/cpp-tests/Source/Box2DTestBed/samples/TaskScheduler.h create mode 100644 tests/cpp-tests/Source/Box2DTestBed/samples/car.cpp create mode 100644 tests/cpp-tests/Source/Box2DTestBed/samples/car.h create mode 100644 tests/cpp-tests/Source/Box2DTestBed/samples/data/background.fs create mode 100644 tests/cpp-tests/Source/Box2DTestBed/samples/data/background.vs create mode 100644 tests/cpp-tests/Source/Box2DTestBed/samples/data/circle.fs create mode 100644 tests/cpp-tests/Source/Box2DTestBed/samples/data/circle.vs create mode 100644 tests/cpp-tests/Source/Box2DTestBed/samples/data/droid_sans.ttf create mode 100644 tests/cpp-tests/Source/Box2DTestBed/samples/data/solid_capsule.fs create mode 100644 tests/cpp-tests/Source/Box2DTestBed/samples/data/solid_capsule.vs create mode 100644 tests/cpp-tests/Source/Box2DTestBed/samples/data/solid_circle.fs create mode 100644 tests/cpp-tests/Source/Box2DTestBed/samples/data/solid_circle.vs create mode 100644 tests/cpp-tests/Source/Box2DTestBed/samples/data/solid_polygon.fs create mode 100644 tests/cpp-tests/Source/Box2DTestBed/samples/data/solid_polygon.vs create mode 100644 tests/cpp-tests/Source/Box2DTestBed/samples/donut.cpp create mode 100644 tests/cpp-tests/Source/Box2DTestBed/samples/donut.h create mode 100644 tests/cpp-tests/Source/Box2DTestBed/samples/doohickey.cpp create mode 100644 tests/cpp-tests/Source/Box2DTestBed/samples/doohickey.h create mode 100644 tests/cpp-tests/Source/Box2DTestBed/samples/draw.h create mode 100644 tests/cpp-tests/Source/Box2DTestBed/samples/human.cpp create mode 100644 tests/cpp-tests/Source/Box2DTestBed/samples/human.h create mode 100644 tests/cpp-tests/Source/Box2DTestBed/samples/jsmn/jsmn.h create mode 100644 tests/cpp-tests/Source/Box2DTestBed/samples/sample.cpp create mode 100644 tests/cpp-tests/Source/Box2DTestBed/samples/sample.h create mode 100644 tests/cpp-tests/Source/Box2DTestBed/samples/sample_benchmark.cpp create mode 100644 tests/cpp-tests/Source/Box2DTestBed/samples/sample_bodies.cpp create mode 100644 tests/cpp-tests/Source/Box2DTestBed/samples/sample_collision.cpp create mode 100644 tests/cpp-tests/Source/Box2DTestBed/samples/sample_continuous.cpp create mode 100644 tests/cpp-tests/Source/Box2DTestBed/samples/sample_determinism.cpp create mode 100644 tests/cpp-tests/Source/Box2DTestBed/samples/sample_events.cpp create mode 100644 tests/cpp-tests/Source/Box2DTestBed/samples/sample_geometry.cpp create mode 100644 tests/cpp-tests/Source/Box2DTestBed/samples/sample_joints.cpp create mode 100644 tests/cpp-tests/Source/Box2DTestBed/samples/sample_robustness.cpp create mode 100644 tests/cpp-tests/Source/Box2DTestBed/samples/sample_shapes.cpp create mode 100644 tests/cpp-tests/Source/Box2DTestBed/samples/sample_stacking.cpp create mode 100644 tests/cpp-tests/Source/Box2DTestBed/samples/sample_world.cpp create mode 100644 tests/cpp-tests/Source/Box2DTestBed/samples/settings.cpp create mode 100644 tests/cpp-tests/Source/Box2DTestBed/samples/settings.h delete mode 100644 tests/cpp-tests/Source/Box2DTestBed/test.cpp delete mode 100644 tests/cpp-tests/Source/Box2DTestBed/tests/add_pair.cpp delete mode 100644 tests/cpp-tests/Source/Box2DTestBed/tests/apply_force.cpp delete mode 100644 tests/cpp-tests/Source/Box2DTestBed/tests/body_types.cpp delete mode 100644 tests/cpp-tests/Source/Box2DTestBed/tests/box_stack.cpp delete mode 100644 tests/cpp-tests/Source/Box2DTestBed/tests/breakable.cpp delete mode 100644 tests/cpp-tests/Source/Box2DTestBed/tests/bridge.cpp delete mode 100644 tests/cpp-tests/Source/Box2DTestBed/tests/bullet_test.cpp delete mode 100644 tests/cpp-tests/Source/Box2DTestBed/tests/cantilever.cpp delete mode 100644 tests/cpp-tests/Source/Box2DTestBed/tests/car.cpp delete mode 100644 tests/cpp-tests/Source/Box2DTestBed/tests/chain.cpp delete mode 100644 tests/cpp-tests/Source/Box2DTestBed/tests/chain_problem.cpp delete mode 100644 tests/cpp-tests/Source/Box2DTestBed/tests/character_collision.cpp delete mode 100644 tests/cpp-tests/Source/Box2DTestBed/tests/circle_stack.cpp delete mode 100644 tests/cpp-tests/Source/Box2DTestBed/tests/collision_filtering.cpp delete mode 100644 tests/cpp-tests/Source/Box2DTestBed/tests/collision_processing.cpp delete mode 100644 tests/cpp-tests/Source/Box2DTestBed/tests/compound_shapes.cpp delete mode 100644 tests/cpp-tests/Source/Box2DTestBed/tests/confined.cpp delete mode 100644 tests/cpp-tests/Source/Box2DTestBed/tests/continuous_test.cpp delete mode 100644 tests/cpp-tests/Source/Box2DTestBed/tests/convex_hull.cpp delete mode 100644 tests/cpp-tests/Source/Box2DTestBed/tests/conveyor_belt.cpp delete mode 100644 tests/cpp-tests/Source/Box2DTestBed/tests/distance_joint.cpp delete mode 100644 tests/cpp-tests/Source/Box2DTestBed/tests/distance_test.cpp delete mode 100644 tests/cpp-tests/Source/Box2DTestBed/tests/dominos.cpp delete mode 100644 tests/cpp-tests/Source/Box2DTestBed/tests/dump_loader.cpp delete mode 100644 tests/cpp-tests/Source/Box2DTestBed/tests/dynamic_tree.cpp delete mode 100644 tests/cpp-tests/Source/Box2DTestBed/tests/edge_shapes.cpp delete mode 100644 tests/cpp-tests/Source/Box2DTestBed/tests/edge_test.cpp delete mode 100644 tests/cpp-tests/Source/Box2DTestBed/tests/friction.cpp delete mode 100644 tests/cpp-tests/Source/Box2DTestBed/tests/gear_joint.cpp delete mode 100644 tests/cpp-tests/Source/Box2DTestBed/tests/heavy1.cpp delete mode 100644 tests/cpp-tests/Source/Box2DTestBed/tests/heavy2.cpp delete mode 100644 tests/cpp-tests/Source/Box2DTestBed/tests/mobile_balanced.cpp delete mode 100644 tests/cpp-tests/Source/Box2DTestBed/tests/mobile_unbalanced.cpp delete mode 100644 tests/cpp-tests/Source/Box2DTestBed/tests/motor_joint.cpp delete mode 100644 tests/cpp-tests/Source/Box2DTestBed/tests/pinball.cpp delete mode 100644 tests/cpp-tests/Source/Box2DTestBed/tests/platformer.cpp delete mode 100644 tests/cpp-tests/Source/Box2DTestBed/tests/polygon_collision.cpp delete mode 100644 tests/cpp-tests/Source/Box2DTestBed/tests/polygon_shapes.cpp delete mode 100644 tests/cpp-tests/Source/Box2DTestBed/tests/prismatic_joint.cpp delete mode 100644 tests/cpp-tests/Source/Box2DTestBed/tests/pulley_joint.cpp delete mode 100644 tests/cpp-tests/Source/Box2DTestBed/tests/pyramid.cpp delete mode 100644 tests/cpp-tests/Source/Box2DTestBed/tests/ray_cast.cpp delete mode 100644 tests/cpp-tests/Source/Box2DTestBed/tests/restitution.cpp delete mode 100644 tests/cpp-tests/Source/Box2DTestBed/tests/revolute_joint.cpp delete mode 100644 tests/cpp-tests/Source/Box2DTestBed/tests/rope.cpp delete mode 100644 tests/cpp-tests/Source/Box2DTestBed/tests/sensor.cpp delete mode 100644 tests/cpp-tests/Source/Box2DTestBed/tests/settings.h delete mode 100644 tests/cpp-tests/Source/Box2DTestBed/tests/shape_cast.cpp delete mode 100644 tests/cpp-tests/Source/Box2DTestBed/tests/shape_editing.cpp delete mode 100644 tests/cpp-tests/Source/Box2DTestBed/tests/skier.cpp delete mode 100644 tests/cpp-tests/Source/Box2DTestBed/tests/slider_crank_1.cpp delete mode 100644 tests/cpp-tests/Source/Box2DTestBed/tests/slider_crank_2.cpp delete mode 100644 tests/cpp-tests/Source/Box2DTestBed/tests/test.h delete mode 100644 tests/cpp-tests/Source/Box2DTestBed/tests/theo_jansen.cpp delete mode 100644 tests/cpp-tests/Source/Box2DTestBed/tests/tiles.cpp delete mode 100644 tests/cpp-tests/Source/Box2DTestBed/tests/time_of_impact.cpp delete mode 100644 tests/cpp-tests/Source/Box2DTestBed/tests/tumbler.cpp delete mode 100644 tests/cpp-tests/Source/Box2DTestBed/tests/web.cpp delete mode 100644 tests/cpp-tests/Source/Box2DTestBed/tests/wheel_joint.cpp delete mode 100644 tests/cpp-tests/Source/Box2DTestBed/tests/wrecking_ball.cpp delete mode 100644 tests/cpp-tests/Source/ChipmunkTest/ChipmunkTest.cpp delete mode 100644 tests/cpp-tests/Source/ChipmunkTest/ChipmunkTest.h create mode 100644 tests/cpp-tests/Source/shaders/circle.fs create mode 100644 tests/cpp-tests/Source/shaders/circle.vs create mode 100644 tests/cpp-tests/Source/shaders/solid_capsule.fs create mode 100644 tests/cpp-tests/Source/shaders/solid_capsule.vs create mode 100644 tests/cpp-tests/Source/shaders/solid_circle.fs create mode 100644 tests/cpp-tests/Source/shaders/solid_circle.vs diff --git a/3rdparty/CMakeLists.txt b/3rdparty/CMakeLists.txt index dda1029e22ad..f4cb7c5e8948 100644 --- a/3rdparty/CMakeLists.txt +++ b/3rdparty/CMakeLists.txt @@ -263,22 +263,6 @@ if(AX_ENABLE_PHYSICS) ax_add_3rd(box2d OPTIONS ${box2d_opts}) endif() -if(AX_ENABLE_PHYSICS) - ax_add_3rd(chipmunk OPTIONS - "CP_BUILD_SHARED OFF" - "CP_BUILD_STATIC ON" - "CP_BUILD_DEMOS OFF" - "CP_INSTALL_STATIC OFF" - ) - set_target_properties(chipmunk PROPERTIES - INTERFACE_INCLUDE_DIRECTORIES "${CMAKE_CURRENT_SOURCE_DIR}/chipmunk/include" - ) - - # !important axmol not use double precision - target_compile_definitions(chipmunk PUBLIC CP_USE_CGTYPES=0) - target_compile_definitions(chipmunk PUBLIC CP_USE_DOUBLES=0) -endif() - ax_add_3rd(freetype OPTIONS "DISABLE_FORCE_DEBUG_POSTFIX ON" "SKIP_INSTALL_ALL TRUE" diff --git a/3rdparty/README.md b/3rdparty/README.md index 7d5998c9dd0c..557373163228 100644 --- a/3rdparty/README.md +++ b/3rdparty/README.md @@ -11,7 +11,7 @@ ## Box2D - [![Upstream](https://img.shields.io/github/v/release/erincatto/box2d?label=Upstream)](https://github.com/erincatto/box2d) -- Version: 2.4.2 +- Version: 3.0.0-df7373c - License: MIT ## Bullet @@ -25,11 +25,6 @@ - Version: 1.33.1 - License: MIT -## Chipmunk2D -- [![Upstream](https://img.shields.io/github/v/tag/slembcke/Chipmunk2D?label=Upstream)](https://github.com/slembcke/Chipmunk2D) -- Version: git 7.0.3-0cb05e7 {until Dec 16, 2021} -- License: MIT - ## Clipper2 - [![Upstream](https://img.shields.io/github/v/tag/AngusJohnson/Clipper2?label=Upstream)](https://github.com/AngusJohnson/Clipper2) - Version: 1.4.0 diff --git a/3rdparty/box2d/CMakeLists.txt b/3rdparty/box2d/CMakeLists.txt index 9a625fdb806f..3bca0f65b152 100644 --- a/3rdparty/box2d/CMakeLists.txt +++ b/3rdparty/box2d/CMakeLists.txt @@ -4,105 +4,14 @@ set(target_name ${lib_name}) project(${lib_name}) -set(BOX2D_SOURCE_FILES - src/collision/b2_broad_phase.cpp - src/collision/b2_chain_shape.cpp - src/collision/b2_circle_shape.cpp - src/collision/b2_collide_circle.cpp - src/collision/b2_collide_edge.cpp - src/collision/b2_collide_polygon.cpp - src/collision/b2_collision.cpp - src/collision/b2_distance.cpp - src/collision/b2_dynamic_tree.cpp - src/collision/b2_edge_shape.cpp - src/collision/b2_polygon_shape.cpp - src/collision/b2_time_of_impact.cpp - src/common/b2_block_allocator.cpp - src/common/b2_draw.cpp - src/common/b2_math.cpp - src/common/b2_settings.cpp - src/common/b2_stack_allocator.cpp - src/common/b2_timer.cpp - src/dynamics/b2_body.cpp - src/dynamics/b2_chain_circle_contact.cpp - src/dynamics/b2_chain_circle_contact.h - src/dynamics/b2_chain_polygon_contact.cpp - src/dynamics/b2_chain_polygon_contact.h - src/dynamics/b2_circle_contact.cpp - src/dynamics/b2_circle_contact.h - src/dynamics/b2_contact.cpp - src/dynamics/b2_contact_manager.cpp - src/dynamics/b2_contact_solver.cpp - src/dynamics/b2_contact_solver.h - src/dynamics/b2_distance_joint.cpp - src/dynamics/b2_edge_circle_contact.cpp - src/dynamics/b2_edge_circle_contact.h - src/dynamics/b2_edge_polygon_contact.cpp - src/dynamics/b2_edge_polygon_contact.h - src/dynamics/b2_fixture.cpp - src/dynamics/b2_friction_joint.cpp - src/dynamics/b2_gear_joint.cpp - src/dynamics/b2_island.cpp - src/dynamics/b2_island.h - src/dynamics/b2_joint.cpp - src/dynamics/b2_motor_joint.cpp - src/dynamics/b2_mouse_joint.cpp - src/dynamics/b2_polygon_circle_contact.cpp - src/dynamics/b2_polygon_circle_contact.h - src/dynamics/b2_polygon_contact.cpp - src/dynamics/b2_polygon_contact.h - src/dynamics/b2_prismatic_joint.cpp - src/dynamics/b2_pulley_joint.cpp - src/dynamics/b2_revolute_joint.cpp - src/dynamics/b2_weld_joint.cpp - src/dynamics/b2_wheel_joint.cpp - src/dynamics/b2_world.cpp - src/dynamics/b2_world_callbacks.cpp - src/rope/b2_rope.cpp) +FILE(GLOB_RECURSE box2d_sources *.h;*.c) -set(BOX2D_HEADER_FILES - include/box2d/b2_api.h - include/box2d/b2_block_allocator.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_common.h - include/box2d/b2_contact.h - include/box2d/b2_contact_manager.h - include/box2d/b2_distance.h - include/box2d/b2_distance_joint.h - include/box2d/b2_draw.h - include/box2d/b2_dynamic_tree.h - include/box2d/b2_edge_shape.h - include/box2d/b2_fixture.h - include/box2d/b2_friction_joint.h - include/box2d/b2_gear_joint.h - include/box2d/b2_growable_stack.h - include/box2d/b2_joint.h - include/box2d/b2_math.h - include/box2d/b2_motor_joint.h - include/box2d/b2_mouse_joint.h - include/box2d/b2_polygon_shape.h - include/box2d/b2_prismatic_joint.h - include/box2d/b2_pulley_joint.h - include/box2d/b2_revolute_joint.h - include/box2d/b2_rope.h - include/box2d/b2_settings.h - include/box2d/b2_shape.h - include/box2d/b2_stack_allocator.h - include/box2d/b2_time_of_impact.h - include/box2d/b2_timer.h - include/box2d/b2_time_step.h - include/box2d/b2_types.h - include/box2d/b2_weld_joint.h - include/box2d/b2_wheel_joint.h - include/box2d/b2_world.h - include/box2d/b2_world_callbacks.h - include/box2d/box2d.h) +add_library(${target_name} STATIC ${box2d_sources}) -add_library(${target_name} STATIC ${BOX2D_SOURCE_FILES} ${BOX2D_HEADER_FILES}) +# Atomics are still considered experimental in Visual Studio 17.8 +if (FULL_MSVC) + target_compile_options(box2d PRIVATE /experimental:c11atomics) +endif() target_include_directories(${target_name} PUBLIC "${CMAKE_CURRENT_SOURCE_DIR}/include" diff --git a/3rdparty/box2d/include/box2d/b2_api.h b/3rdparty/box2d/include/box2d/b2_api.h deleted file mode 100644 index 6730203c64b1..000000000000 --- a/3rdparty/box2d/include/box2d/b2_api.h +++ /dev/null @@ -1,52 +0,0 @@ -// MIT License - -// Copyright (c) 2019 Erin Catto - -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: - -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. - -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -#ifndef B2_API_H -#define B2_API_H - -#ifdef B2_SHARED - #if defined _WIN32 || defined __CYGWIN__ - #ifdef box2d_EXPORTS - #ifdef __GNUC__ - #define B2_API __attribute__ ((dllexport)) - #else - #define B2_API __declspec(dllexport) - #endif - #else - #ifdef __GNUC__ - #define B2_API __attribute__ ((dllimport)) - #else - #define B2_API __declspec(dllimport) - #endif - #endif - #else - #if __GNUC__ >= 4 - #define B2_API __attribute__ ((visibility ("default"))) - #else - #define B2_API - #endif - #endif -#else - #define B2_API -#endif - -#endif diff --git a/3rdparty/box2d/include/box2d/b2_block_allocator.h b/3rdparty/box2d/include/box2d/b2_block_allocator.h deleted file mode 100644 index 95c12de89cd8..000000000000 --- a/3rdparty/box2d/include/box2d/b2_block_allocator.h +++ /dev/null @@ -1,60 +0,0 @@ -// MIT License - -// Copyright (c) 2019 Erin Catto - -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: - -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. - -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -#ifndef B2_BLOCK_ALLOCATOR_H -#define B2_BLOCK_ALLOCATOR_H - -#include "b2_api.h" -#include "b2_settings.h" - -const int32 b2_blockSizeCount = 14; - -struct b2Block; -struct b2Chunk; - -/// This is a small object allocator used for allocating small -/// objects that persist for more than one time step. -/// See: http://www.codeproject.com/useritems/Small_Block_Allocator.asp -class B2_API b2BlockAllocator -{ -public: - b2BlockAllocator(); - ~b2BlockAllocator(); - - /// Allocate memory. This will use b2Alloc if the size is larger than b2_maxBlockSize. - void* Allocate(int32 size); - - /// Free memory. This will use b2Free if the size is larger than b2_maxBlockSize. - void Free(void* p, int32 size); - - void Clear(); - -private: - - b2Chunk* m_chunks; - int32 m_chunkCount; - int32 m_chunkSpace; - - b2Block* m_freeLists[b2_blockSizeCount]; -}; - -#endif diff --git a/3rdparty/box2d/include/box2d/b2_body.h b/3rdparty/box2d/include/box2d/b2_body.h deleted file mode 100644 index 16b2bb002c78..000000000000 --- a/3rdparty/box2d/include/box2d/b2_body.h +++ /dev/null @@ -1,885 +0,0 @@ -// MIT License - -// Copyright (c) 2019 Erin Catto - -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: - -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. - -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -#ifndef B2_BODY_H -#define B2_BODY_H - -#include "b2_api.h" -#include "b2_math.h" -#include "b2_shape.h" - -class b2Fixture; -class b2Joint; -class b2Contact; -class b2Controller; -class b2World; -struct b2FixtureDef; -struct b2JointEdge; -struct b2ContactEdge; - -/// The body type. -/// static: zero mass, zero velocity, may be manually moved -/// kinematic: zero mass, non-zero velocity set by user, moved by solver -/// dynamic: positive mass, non-zero velocity determined by forces, moved by solver -enum b2BodyType -{ - b2_staticBody = 0, - b2_kinematicBody, - b2_dynamicBody -}; - -/// A body definition holds all the data needed to construct a rigid body. -/// You can safely re-use body definitions. Shapes are added to a body after construction. -struct B2_API b2BodyDef -{ - /// This constructor sets the body definition default values. - b2BodyDef() - { - position.Set(0.0f, 0.0f); - angle = 0.0f; - linearVelocity.Set(0.0f, 0.0f); - angularVelocity = 0.0f; - linearDamping = 0.0f; - angularDamping = 0.0f; - allowSleep = true; - awake = true; - fixedRotation = false; - bullet = false; - type = b2_staticBody; - enabled = true; - gravityScale = 1.0f; - } - - /// The body type: static, kinematic, or dynamic. - /// Note: if a dynamic body would have zero mass, the mass is set to one. - b2BodyType type; - - /// The world position of the body. Avoid creating bodies at the origin - /// since this can lead to many overlapping shapes. - b2Vec2 position; - - /// The world angle of the body in radians. - float angle; - - /// The linear velocity of the body's origin in world co-ordinates. - b2Vec2 linearVelocity; - - /// The angular velocity of the body. - float angularVelocity; - - /// Linear damping is use to reduce the linear velocity. The damping parameter - /// can be larger than 1.0f but the damping effect becomes sensitive to the - /// time step when the damping parameter is large. - /// Units are 1/time - float linearDamping; - - /// Angular damping is use to reduce the angular velocity. The damping parameter - /// can be larger than 1.0f but the damping effect becomes sensitive to the - /// time step when the damping parameter is large. - /// Units are 1/time - float angularDamping; - - /// Set this flag to false if this body should never fall asleep. Note that - /// this increases CPU usage. - bool allowSleep; - - /// Is this body initially awake or sleeping? - bool awake; - - /// Should this body be prevented from rotating? Useful for characters. - bool fixedRotation; - - /// Is this a fast moving body that should be prevented from tunneling through - /// other moving bodies? Note that all bodies are prevented from tunneling through - /// kinematic and static bodies. This setting is only considered on dynamic bodies. - /// @warning You should use this flag sparingly since it increases processing time. - bool bullet; - - /// Does this body start out enabled? - bool enabled; - - /// Use this to store application specific body data. - b2BodyUserData userData; - - /// Scale the gravity applied to this body. - float gravityScale; -}; - -/// A rigid body. These are created via b2World::CreateBody. -class B2_API b2Body -{ -public: - /// Creates a fixture and attach it to this body. Use this function if you need - /// to set some fixture parameters, like friction. Otherwise you can create the - /// fixture directly from a shape. - /// If the density is non-zero, this function automatically updates the mass of the body. - /// Contacts are not created until the next time step. - /// @param def the fixture definition. - /// @warning This function is locked during callbacks. - b2Fixture* CreateFixture(const b2FixtureDef* def); - - /// Creates a fixture from a shape and attach it to this body. - /// This is a convenience function. Use b2FixtureDef if you need to set parameters - /// like friction, restitution, user data, or filtering. - /// If the density is non-zero, this function automatically updates the mass of the body. - /// @param shape the shape to be cloned. - /// @param density the shape density (set to zero for static bodies). - /// @warning This function is locked during callbacks. - b2Fixture* CreateFixture(const b2Shape* shape, float density); - - /// Destroy a fixture. This removes the fixture from the broad-phase and - /// destroys all contacts associated with this fixture. This will - /// automatically adjust the mass of the body if the body is dynamic and the - /// fixture has positive density. - /// All fixtures attached to a body are implicitly destroyed when the body is destroyed. - /// @param fixture the fixture to be removed. - /// @warning This function is locked during callbacks. - void DestroyFixture(b2Fixture* fixture); - - /// Set the position of the body's origin and rotation. - /// Manipulating a body's transform may cause non-physical behavior. - /// Note: contacts are updated on the next call to b2World::Step. - /// @param position the world position of the body's local origin. - /// @param angle the world rotation in radians. - void SetTransform(const b2Vec2& position, float angle); - - /// Get the body transform for the body's origin. - /// @return the world transform of the body's origin. - const b2Transform& GetTransform() const; - - /// Get the world body origin position. - /// @return the world position of the body's origin. - const b2Vec2& GetPosition() const; - - /// Get the angle in radians. - /// @return the current world rotation angle in radians. - float GetAngle() const; - - /// Get the world position of the center of mass. - const b2Vec2& GetWorldCenter() const; - - /// Get the local position of the center of mass. - const b2Vec2& GetLocalCenter() const; - - /// Set the linear velocity of the center of mass. - /// @param v the new linear velocity of the center of mass. - void SetLinearVelocity(const b2Vec2& v); - - /// Get the linear velocity of the center of mass. - /// @return the linear velocity of the center of mass. - const b2Vec2& GetLinearVelocity() const; - - /// Set the angular velocity. - /// @param omega the new angular velocity in radians/second. - void SetAngularVelocity(float omega); - - /// Get the angular velocity. - /// @return the angular velocity in radians/second. - float GetAngularVelocity() const; - - /// Apply a force at a world point. If the force is not - /// applied at the center of mass, it will generate a torque and - /// affect the angular velocity. This wakes up the body. - /// @param force the world force vector, usually in Newtons (N). - /// @param point the world position of the point of application. - /// @param wake also wake up the body - void ApplyForce(const b2Vec2& force, const b2Vec2& point, bool wake); - - /// Apply a force to the center of mass. This wakes up the body. - /// @param force the world force vector, usually in Newtons (N). - /// @param wake also wake up the body - void ApplyForceToCenter(const b2Vec2& force, bool wake); - - /// Apply a torque. This affects the angular velocity - /// without affecting the linear velocity of the center of mass. - /// @param torque about the z-axis (out of the screen), usually in N-m. - /// @param wake also wake up the body - void ApplyTorque(float torque, bool wake); - - /// Apply an impulse at a point. This immediately modifies the velocity. - /// It also modifies the angular velocity if the point of application - /// is not at the center of mass. This wakes up the body. - /// @param impulse the world impulse vector, usually in N-seconds or kg-m/s. - /// @param point the world position of the point of application. - /// @param wake also wake up the body - void ApplyLinearImpulse(const b2Vec2& impulse, const b2Vec2& point, bool wake); - - /// Apply an impulse to the center of mass. This immediately modifies the velocity. - /// @param impulse the world impulse vector, usually in N-seconds or kg-m/s. - /// @param wake also wake up the body - void ApplyLinearImpulseToCenter(const b2Vec2& impulse, bool wake); - - /// Apply an angular impulse. - /// @param impulse the angular impulse in units of kg*m*m/s - /// @param wake also wake up the body - void ApplyAngularImpulse(float impulse, bool wake); - - /// Get the total mass of the body. - /// @return the mass, usually in kilograms (kg). - float GetMass() const; - - /// Get the rotational inertia of the body about the local origin. - /// @return the rotational inertia, usually in kg-m^2. - float GetInertia() const; - - /// Get the mass data of the body. - /// @return a struct containing the mass, inertia and center of the body. - b2MassData GetMassData() const; - - /// Set the mass properties to override the mass properties of the fixtures. - /// Note that this changes the center of mass position. - /// Note that creating or destroying fixtures can also alter the mass. - /// This function has no effect if the body isn't dynamic. - /// @param data the mass properties. - void SetMassData(const b2MassData* data); - - /// This resets the mass properties to the sum of the mass properties of the fixtures. - /// This normally does not need to be called unless you called SetMassData to override - /// the mass and you later want to reset the mass. - void ResetMassData(); - - /// Get the world coordinates of a point given the local coordinates. - /// @param localPoint a point on the body measured relative the the body's origin. - /// @return the same point expressed in world coordinates. - b2Vec2 GetWorldPoint(const b2Vec2& localPoint) const; - - /// Get the world coordinates of a vector given the local coordinates. - /// @param localVector a vector fixed in the body. - /// @return the same vector expressed in world coordinates. - b2Vec2 GetWorldVector(const b2Vec2& localVector) const; - - /// Gets a local point relative to the body's origin given a world point. - /// @param worldPoint a point in world coordinates. - /// @return the corresponding local point relative to the body's origin. - b2Vec2 GetLocalPoint(const b2Vec2& worldPoint) const; - - /// Gets a local vector given a world vector. - /// @param worldVector a vector in world coordinates. - /// @return the corresponding local vector. - b2Vec2 GetLocalVector(const b2Vec2& worldVector) const; - - /// Get the world linear velocity of a world point attached to this body. - /// @param worldPoint a point in world coordinates. - /// @return the world velocity of a point. - b2Vec2 GetLinearVelocityFromWorldPoint(const b2Vec2& worldPoint) const; - - /// Get the world velocity of a local point. - /// @param localPoint a point in local coordinates. - /// @return the world velocity of a point. - b2Vec2 GetLinearVelocityFromLocalPoint(const b2Vec2& localPoint) const; - - /// Get the linear damping of the body. - float GetLinearDamping() const; - - /// Set the linear damping of the body. - void SetLinearDamping(float linearDamping); - - /// Get the angular damping of the body. - float GetAngularDamping() const; - - /// Set the angular damping of the body. - void SetAngularDamping(float angularDamping); - - /// Get the gravity scale of the body. - float GetGravityScale() const; - - /// Set the gravity scale of the body. - void SetGravityScale(float scale); - - /// Set the type of this body. This may alter the mass and velocity. - void SetType(b2BodyType type); - - /// Get the type of this body. - b2BodyType GetType() const; - - /// Should this body be treated like a bullet for continuous collision detection? - void SetBullet(bool flag); - - /// Is this body treated like a bullet for continuous collision detection? - bool IsBullet() const; - - /// You can disable sleeping on this body. If you disable sleeping, the - /// body will be woken. - void SetSleepingAllowed(bool flag); - - /// Is this body allowed to sleep - bool IsSleepingAllowed() const; - - /// Set the sleep state of the body. A sleeping body has very - /// low CPU cost. - /// @param flag set to true to wake the body, false to put it to sleep. - void SetAwake(bool flag); - - /// Get the sleeping state of this body. - /// @return true if the body is awake. - bool IsAwake() const; - - /// Allow a body to be disabled. A disabled body is not simulated and cannot - /// be collided with or woken up. - /// If you pass a flag of true, all fixtures will be added to the broad-phase. - /// If you pass a flag of false, all fixtures will be removed from the - /// broad-phase and all contacts will be destroyed. - /// Fixtures and joints are otherwise unaffected. You may continue - /// to create/destroy fixtures and joints on disabled bodies. - /// Fixtures on a disabled body are implicitly disabled and will - /// not participate in collisions, ray-casts, or queries. - /// Joints connected to a disabled body are implicitly disabled. - /// An diabled body is still owned by a b2World object and remains - /// in the body list. - void SetEnabled(bool flag); - - /// Get the active state of the body. - bool IsEnabled() const; - - /// Set this body to have fixed rotation. This causes the mass - /// to be reset. - void SetFixedRotation(bool flag); - - /// Does this body have fixed rotation? - bool IsFixedRotation() const; - - /// Get the list of all fixtures attached to this body. - b2Fixture* GetFixtureList(); - const b2Fixture* GetFixtureList() const; - - /// Get the list of all joints attached to this body. - b2JointEdge* GetJointList(); - const b2JointEdge* GetJointList() const; - - /// Get the list of all contacts attached to this body. - /// @warning this list changes during the time step and you may - /// miss some collisions if you don't use b2ContactListener. - b2ContactEdge* GetContactList(); - const b2ContactEdge* GetContactList() const; - - /// Get the next body in the world's body list. - b2Body* GetNext(); - const b2Body* GetNext() const; - - /// Get the user data pointer that was provided in the body definition. - b2BodyUserData& GetUserData(); - const b2BodyUserData& GetUserData() const; - - /// Get the parent world of this body. - b2World* GetWorld(); - const b2World* GetWorld() const; - - /// Dump this body to a file - void Dump(); - -private: - - friend class b2World; - friend class b2Island; - friend class b2ContactManager; - friend class b2ContactSolver; - friend class b2Contact; - - friend class b2DistanceJoint; - friend class b2FrictionJoint; - friend class b2GearJoint; - friend class b2MotorJoint; - friend class b2MouseJoint; - friend class b2PrismaticJoint; - friend class b2PulleyJoint; - friend class b2RevoluteJoint; - friend class b2WeldJoint; - friend class b2WheelJoint; - - // m_flags - enum - { - e_islandFlag = 0x0001, - e_awakeFlag = 0x0002, - e_autoSleepFlag = 0x0004, - e_bulletFlag = 0x0008, - e_fixedRotationFlag = 0x0010, - e_enabledFlag = 0x0020, - e_toiFlag = 0x0040 - }; - - b2Body(const b2BodyDef* bd, b2World* world); - ~b2Body(); - - void SynchronizeFixtures(); - void SynchronizeTransform(); - - // This is used to prevent connected bodies from colliding. - // It may lie, depending on the collideConnected flag. - bool ShouldCollide(const b2Body* other) const; - - void Advance(float t); - - b2BodyType m_type; - - uint16 m_flags; - - int32 m_islandIndex; - - b2Transform m_xf; // the body origin transform - b2Sweep m_sweep; // the swept motion for CCD - - b2Vec2 m_linearVelocity; - float m_angularVelocity; - - b2Vec2 m_force; - float m_torque; - - b2World* m_world; - b2Body* m_prev; - b2Body* m_next; - - b2Fixture* m_fixtureList; - int32 m_fixtureCount; - - b2JointEdge* m_jointList; - b2ContactEdge* m_contactList; - - float m_mass, m_invMass; - - // Rotational inertia about the center of mass. - float m_I, m_invI; - - float m_linearDamping; - float m_angularDamping; - float m_gravityScale; - - float m_sleepTime; - - b2BodyUserData m_userData; -}; - -inline b2BodyType b2Body::GetType() const -{ - return m_type; -} - -inline const b2Transform& b2Body::GetTransform() const -{ - return m_xf; -} - -inline const b2Vec2& b2Body::GetPosition() const -{ - return m_xf.p; -} - -inline float b2Body::GetAngle() const -{ - return m_sweep.a; -} - -inline const b2Vec2& b2Body::GetWorldCenter() const -{ - return m_sweep.c; -} - -inline const b2Vec2& b2Body::GetLocalCenter() const -{ - return m_sweep.localCenter; -} - -inline void b2Body::SetLinearVelocity(const b2Vec2& v) -{ - if (m_type == b2_staticBody) - { - return; - } - - if (b2Dot(v,v) > 0.0f) - { - SetAwake(true); - } - - m_linearVelocity = v; -} - -inline const b2Vec2& b2Body::GetLinearVelocity() const -{ - return m_linearVelocity; -} - -inline void b2Body::SetAngularVelocity(float w) -{ - if (m_type == b2_staticBody) - { - return; - } - - if (w * w > 0.0f) - { - SetAwake(true); - } - - m_angularVelocity = w; -} - -inline float b2Body::GetAngularVelocity() const -{ - return m_angularVelocity; -} - -inline float b2Body::GetMass() const -{ - return m_mass; -} - -inline float b2Body::GetInertia() const -{ - return m_I + m_mass * b2Dot(m_sweep.localCenter, m_sweep.localCenter); -} - -inline b2MassData b2Body::GetMassData() const -{ - b2MassData data; - data.mass = m_mass; - data.I = m_I + m_mass * b2Dot(m_sweep.localCenter, m_sweep.localCenter); - data.center = m_sweep.localCenter; - return data; -} - -inline b2Vec2 b2Body::GetWorldPoint(const b2Vec2& localPoint) const -{ - return b2Mul(m_xf, localPoint); -} - -inline b2Vec2 b2Body::GetWorldVector(const b2Vec2& localVector) const -{ - return b2Mul(m_xf.q, localVector); -} - -inline b2Vec2 b2Body::GetLocalPoint(const b2Vec2& worldPoint) const -{ - return b2MulT(m_xf, worldPoint); -} - -inline b2Vec2 b2Body::GetLocalVector(const b2Vec2& worldVector) const -{ - return b2MulT(m_xf.q, worldVector); -} - -inline b2Vec2 b2Body::GetLinearVelocityFromWorldPoint(const b2Vec2& worldPoint) const -{ - return m_linearVelocity + b2Cross(m_angularVelocity, worldPoint - m_sweep.c); -} - -inline b2Vec2 b2Body::GetLinearVelocityFromLocalPoint(const b2Vec2& localPoint) const -{ - return GetLinearVelocityFromWorldPoint(GetWorldPoint(localPoint)); -} - -inline float b2Body::GetLinearDamping() const -{ - return m_linearDamping; -} - -inline void b2Body::SetLinearDamping(float linearDamping) -{ - m_linearDamping = linearDamping; -} - -inline float b2Body::GetAngularDamping() const -{ - return m_angularDamping; -} - -inline void b2Body::SetAngularDamping(float angularDamping) -{ - m_angularDamping = angularDamping; -} - -inline float b2Body::GetGravityScale() const -{ - return m_gravityScale; -} - -inline void b2Body::SetGravityScale(float scale) -{ - m_gravityScale = scale; -} - -inline void b2Body::SetBullet(bool flag) -{ - if (flag) - { - m_flags |= e_bulletFlag; - } - else - { - m_flags &= ~e_bulletFlag; - } -} - -inline bool b2Body::IsBullet() const -{ - return (m_flags & e_bulletFlag) == e_bulletFlag; -} - -inline void b2Body::SetAwake(bool flag) -{ - if (m_type == b2_staticBody) - { - return; - } - - if (flag) - { - m_flags |= e_awakeFlag; - m_sleepTime = 0.0f; - } - else - { - m_flags &= ~e_awakeFlag; - m_sleepTime = 0.0f; - m_linearVelocity.SetZero(); - m_angularVelocity = 0.0f; - m_force.SetZero(); - m_torque = 0.0f; - } -} - -inline bool b2Body::IsAwake() const -{ - return (m_flags & e_awakeFlag) == e_awakeFlag; -} - -inline bool b2Body::IsEnabled() const -{ - return (m_flags & e_enabledFlag) == e_enabledFlag; -} - -inline bool b2Body::IsFixedRotation() const -{ - return (m_flags & e_fixedRotationFlag) == e_fixedRotationFlag; -} - -inline void b2Body::SetSleepingAllowed(bool flag) -{ - if (flag) - { - m_flags |= e_autoSleepFlag; - } - else - { - m_flags &= ~e_autoSleepFlag; - SetAwake(true); - } -} - -inline bool b2Body::IsSleepingAllowed() const -{ - return (m_flags & e_autoSleepFlag) == e_autoSleepFlag; -} - -inline b2Fixture* b2Body::GetFixtureList() -{ - return m_fixtureList; -} - -inline const b2Fixture* b2Body::GetFixtureList() const -{ - return m_fixtureList; -} - -inline b2JointEdge* b2Body::GetJointList() -{ - return m_jointList; -} - -inline const b2JointEdge* b2Body::GetJointList() const -{ - return m_jointList; -} - -inline b2ContactEdge* b2Body::GetContactList() -{ - return m_contactList; -} - -inline const b2ContactEdge* b2Body::GetContactList() const -{ - return m_contactList; -} - -inline b2Body* b2Body::GetNext() -{ - return m_next; -} - -inline const b2Body* b2Body::GetNext() const -{ - return m_next; -} - -inline b2BodyUserData& b2Body::GetUserData() -{ - return m_userData; -} - -inline const b2BodyUserData& b2Body::GetUserData() const -{ - return m_userData; -} - -inline void b2Body::ApplyForce(const b2Vec2& force, const b2Vec2& point, bool wake) -{ - if (m_type != b2_dynamicBody) - { - return; - } - - if (wake && (m_flags & e_awakeFlag) == 0) - { - SetAwake(true); - } - - // Don't accumulate a force if the body is sleeping. - if (m_flags & e_awakeFlag) - { - m_force += force; - m_torque += b2Cross(point - m_sweep.c, force); - } -} - -inline void b2Body::ApplyForceToCenter(const b2Vec2& force, bool wake) -{ - if (m_type != b2_dynamicBody) - { - return; - } - - if (wake && (m_flags & e_awakeFlag) == 0) - { - SetAwake(true); - } - - // Don't accumulate a force if the body is sleeping - if (m_flags & e_awakeFlag) - { - m_force += force; - } -} - -inline void b2Body::ApplyTorque(float torque, bool wake) -{ - if (m_type != b2_dynamicBody) - { - return; - } - - if (wake && (m_flags & e_awakeFlag) == 0) - { - SetAwake(true); - } - - // Don't accumulate a force if the body is sleeping - if (m_flags & e_awakeFlag) - { - m_torque += torque; - } -} - -inline void b2Body::ApplyLinearImpulse(const b2Vec2& impulse, const b2Vec2& point, bool wake) -{ - if (m_type != b2_dynamicBody) - { - return; - } - - if (wake && (m_flags & e_awakeFlag) == 0) - { - SetAwake(true); - } - - // Don't accumulate velocity if the body is sleeping - if (m_flags & e_awakeFlag) - { - m_linearVelocity += m_invMass * impulse; - m_angularVelocity += m_invI * b2Cross(point - m_sweep.c, impulse); - } -} - -inline void b2Body::ApplyLinearImpulseToCenter(const b2Vec2& impulse, bool wake) -{ - if (m_type != b2_dynamicBody) - { - return; - } - - if (wake && (m_flags & e_awakeFlag) == 0) - { - SetAwake(true); - } - - // Don't accumulate velocity if the body is sleeping - if (m_flags & e_awakeFlag) - { - m_linearVelocity += m_invMass * impulse; - } -} - -inline void b2Body::ApplyAngularImpulse(float impulse, bool wake) -{ - if (m_type != b2_dynamicBody) - { - return; - } - - if (wake && (m_flags & e_awakeFlag) == 0) - { - SetAwake(true); - } - - // Don't accumulate velocity if the body is sleeping - if (m_flags & e_awakeFlag) - { - m_angularVelocity += m_invI * impulse; - } -} - -inline void b2Body::SynchronizeTransform() -{ - m_xf.q.Set(m_sweep.a); - m_xf.p = m_sweep.c - b2Mul(m_xf.q, m_sweep.localCenter); -} - -inline void b2Body::Advance(float alpha) -{ - // Advance to the new safe time. This doesn't sync the broad-phase. - m_sweep.Advance(alpha); - m_sweep.c = m_sweep.c0; - m_sweep.a = m_sweep.a0; - m_xf.q.Set(m_sweep.a); - m_xf.p = m_sweep.c - b2Mul(m_xf.q, m_sweep.localCenter); -} - -inline b2World* b2Body::GetWorld() -{ - return m_world; -} - -inline const b2World* b2Body::GetWorld() const -{ - return m_world; -} - -#endif diff --git a/3rdparty/box2d/include/box2d/b2_broad_phase.h b/3rdparty/box2d/include/box2d/b2_broad_phase.h deleted file mode 100644 index cc882ab477ce..000000000000 --- a/3rdparty/box2d/include/box2d/b2_broad_phase.h +++ /dev/null @@ -1,238 +0,0 @@ -// MIT License - -// Copyright (c) 2019 Erin Catto - -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: - -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. - -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -#ifndef B2_BROAD_PHASE_H -#define B2_BROAD_PHASE_H - -#include "b2_api.h" -#include "b2_settings.h" -#include "b2_collision.h" -#include "b2_dynamic_tree.h" - -struct B2_API b2Pair -{ - int32 proxyIdA; - int32 proxyIdB; -}; - -/// The broad-phase is used for computing pairs and performing volume queries and ray casts. -/// This broad-phase does not persist pairs. Instead, this reports potentially new pairs. -/// It is up to the client to consume the new pairs and to track subsequent overlap. -class B2_API b2BroadPhase -{ -public: - - enum - { - e_nullProxy = -1 - }; - - b2BroadPhase(); - ~b2BroadPhase(); - - /// Create a proxy with an initial AABB. Pairs are not reported until - /// UpdatePairs is called. - int32 CreateProxy(const b2AABB& aabb, void* userData); - - /// Destroy a proxy. It is up to the client to remove any pairs. - void DestroyProxy(int32 proxyId); - - /// Call MoveProxy as many times as you like, then when you are done - /// call UpdatePairs to finalized the proxy pairs (for your time step). - void MoveProxy(int32 proxyId, const b2AABB& aabb, const b2Vec2& displacement); - - /// Call to trigger a re-processing of it's pairs on the next call to UpdatePairs. - void TouchProxy(int32 proxyId); - - /// Get the fat AABB for a proxy. - const b2AABB& GetFatAABB(int32 proxyId) const; - - /// Get user data from a proxy. Returns nullptr if the id is invalid. - void* GetUserData(int32 proxyId) const; - - /// Test overlap of fat AABBs. - bool TestOverlap(int32 proxyIdA, int32 proxyIdB) const; - - /// Get the number of proxies. - int32 GetProxyCount() const; - - /// Update the pairs. This results in pair callbacks. This can only add pairs. - template - void UpdatePairs(T* callback); - - /// Query an AABB for overlapping proxies. The callback class - /// is called for each proxy that overlaps the supplied AABB. - template - void Query(T* callback, const b2AABB& aabb) const; - - /// Ray-cast against the proxies in the tree. This relies on the callback - /// to perform a exact ray-cast in the case were the proxy contains a shape. - /// The callback also performs the any collision filtering. This has performance - /// roughly equal to k * log(n), where k is the number of collisions and n is the - /// number of proxies in the tree. - /// @param input the ray-cast input data. The ray extends from p1 to p1 + maxFraction * (p2 - p1). - /// @param callback a callback class that is called for each proxy that is hit by the ray. - template - void RayCast(T* callback, const b2RayCastInput& input) const; - - /// Get the height of the embedded tree. - int32 GetTreeHeight() const; - - /// Get the balance of the embedded tree. - int32 GetTreeBalance() const; - - /// Get the quality metric of the embedded tree. - float GetTreeQuality() const; - - /// Shift the world origin. Useful for large worlds. - /// The shift formula is: position -= newOrigin - /// @param newOrigin the new origin with respect to the old origin - void ShiftOrigin(const b2Vec2& newOrigin); - -private: - - friend class b2DynamicTree; - - void BufferMove(int32 proxyId); - void UnBufferMove(int32 proxyId); - - bool QueryCallback(int32 proxyId); - - b2DynamicTree m_tree; - - int32 m_proxyCount; - - int32* m_moveBuffer; - int32 m_moveCapacity; - int32 m_moveCount; - - b2Pair* m_pairBuffer; - int32 m_pairCapacity; - int32 m_pairCount; - - int32 m_queryProxyId; -}; - -inline void* b2BroadPhase::GetUserData(int32 proxyId) const -{ - return m_tree.GetUserData(proxyId); -} - -inline bool b2BroadPhase::TestOverlap(int32 proxyIdA, int32 proxyIdB) const -{ - const b2AABB& aabbA = m_tree.GetFatAABB(proxyIdA); - const b2AABB& aabbB = m_tree.GetFatAABB(proxyIdB); - return b2TestOverlap(aabbA, aabbB); -} - -inline const b2AABB& b2BroadPhase::GetFatAABB(int32 proxyId) const -{ - return m_tree.GetFatAABB(proxyId); -} - -inline int32 b2BroadPhase::GetProxyCount() const -{ - return m_proxyCount; -} - -inline int32 b2BroadPhase::GetTreeHeight() const -{ - return m_tree.GetHeight(); -} - -inline int32 b2BroadPhase::GetTreeBalance() const -{ - return m_tree.GetMaxBalance(); -} - -inline float b2BroadPhase::GetTreeQuality() const -{ - return m_tree.GetAreaRatio(); -} - -template -void b2BroadPhase::UpdatePairs(T* callback) -{ - // Reset pair buffer - m_pairCount = 0; - - // Perform tree queries for all moving proxies. - for (int32 i = 0; i < m_moveCount; ++i) - { - m_queryProxyId = m_moveBuffer[i]; - if (m_queryProxyId == e_nullProxy) - { - continue; - } - - // We have to query the tree with the fat AABB so that - // we don't fail to create a pair that may touch later. - const b2AABB& fatAABB = m_tree.GetFatAABB(m_queryProxyId); - - // Query tree, create pairs and add them pair buffer. - m_tree.Query(this, fatAABB); - } - - // Send pairs to caller - for (int32 i = 0; i < m_pairCount; ++i) - { - b2Pair* primaryPair = m_pairBuffer + i; - void* userDataA = m_tree.GetUserData(primaryPair->proxyIdA); - void* userDataB = m_tree.GetUserData(primaryPair->proxyIdB); - - callback->AddPair(userDataA, userDataB); - } - - // Clear move flags - for (int32 i = 0; i < m_moveCount; ++i) - { - int32 proxyId = m_moveBuffer[i]; - if (proxyId == e_nullProxy) - { - continue; - } - - m_tree.ClearMoved(proxyId); - } - - // Reset move buffer - m_moveCount = 0; -} - -template -inline void b2BroadPhase::Query(T* callback, const b2AABB& aabb) const -{ - m_tree.Query(callback, aabb); -} - -template -inline void b2BroadPhase::RayCast(T* callback, const b2RayCastInput& input) const -{ - m_tree.RayCast(callback, input); -} - -inline void b2BroadPhase::ShiftOrigin(const b2Vec2& newOrigin) -{ - m_tree.ShiftOrigin(newOrigin); -} - -#endif diff --git a/3rdparty/box2d/include/box2d/b2_chain_shape.h b/3rdparty/box2d/include/box2d/b2_chain_shape.h deleted file mode 100644 index da2605d6233b..000000000000 --- a/3rdparty/box2d/include/box2d/b2_chain_shape.h +++ /dev/null @@ -1,101 +0,0 @@ -// MIT License - -// Copyright (c) 2019 Erin Catto - -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: - -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. - -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -#ifndef B2_CHAIN_SHAPE_H -#define B2_CHAIN_SHAPE_H - -#include "b2_api.h" -#include "b2_shape.h" - -class b2EdgeShape; - -/// A chain shape is a free form sequence of line segments. -/// The chain has one-sided collision, with the surface normal pointing to the right of the edge. -/// This provides a counter-clockwise winding like the polygon shape. -/// Connectivity information is used to create smooth collisions. -/// @warning the chain will not collide properly if there are self-intersections. -class B2_API b2ChainShape : public b2Shape -{ -public: - b2ChainShape(); - - /// The destructor frees the vertices using b2Free. - ~b2ChainShape(); - - /// Clear all data. - void Clear(); - - /// Create a loop. This automatically adjusts connectivity. - /// @param vertices an array of vertices, these are copied - /// @param count the vertex count - void CreateLoop(const b2Vec2* vertices, int32 count); - - /// Create a chain with ghost vertices to connect multiple chains together. - /// @param vertices an array of vertices, these are copied - /// @param count the vertex count - /// @param prevVertex previous vertex from chain that connects to the start - /// @param nextVertex next vertex from chain that connects to the end - void CreateChain(const b2Vec2* vertices, int32 count, - const b2Vec2& prevVertex, const b2Vec2& nextVertex); - - /// Implement b2Shape. Vertices are cloned using b2Alloc. - b2Shape* Clone(b2BlockAllocator* allocator) const override; - - /// @see b2Shape::GetChildCount - int32 GetChildCount() const override; - - /// Get a child edge. - void GetChildEdge(b2EdgeShape* edge, int32 index) const; - - /// This always return false. - /// @see b2Shape::TestPoint - bool TestPoint(const b2Transform& transform, const b2Vec2& p) const override; - - /// Implement b2Shape. - bool RayCast(b2RayCastOutput* output, const b2RayCastInput& input, - const b2Transform& transform, int32 childIndex) const override; - - /// @see b2Shape::ComputeAABB - void ComputeAABB(b2AABB* aabb, const b2Transform& transform, int32 childIndex) const override; - - /// Chains have zero mass. - /// @see b2Shape::ComputeMass - void ComputeMass(b2MassData* massData, float density) const override; - - /// The vertices. Owned by this class. - b2Vec2* m_vertices; - - /// The vertex count. - int32 m_count; - - b2Vec2 m_prevVertex, m_nextVertex; -}; - -inline b2ChainShape::b2ChainShape() -{ - m_type = e_chain; - m_radius = b2_polygonRadius; - m_vertices = nullptr; - m_count = 0; -} - -#endif diff --git a/3rdparty/box2d/include/box2d/b2_circle_shape.h b/3rdparty/box2d/include/box2d/b2_circle_shape.h deleted file mode 100644 index 5e330f5a7ea3..000000000000 --- a/3rdparty/box2d/include/box2d/b2_circle_shape.h +++ /dev/null @@ -1,67 +0,0 @@ -// MIT License - -// Copyright (c) 2019 Erin Catto - -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: - -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. - -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -#ifndef B2_CIRCLE_SHAPE_H -#define B2_CIRCLE_SHAPE_H - -#include "b2_api.h" -#include "b2_shape.h" - -/// A solid circle shape -class B2_API b2CircleShape : public b2Shape -{ -public: - b2CircleShape(); - - /// Implement b2Shape. - b2Shape* Clone(b2BlockAllocator* allocator) const override; - - /// @see b2Shape::GetChildCount - int32 GetChildCount() const override; - - /// Implement b2Shape. - bool TestPoint(const b2Transform& transform, const b2Vec2& p) const override; - - /// Implement b2Shape. - /// @note because the circle is solid, rays that start inside do not hit because the normal is - /// not defined. - bool RayCast(b2RayCastOutput* output, const b2RayCastInput& input, - const b2Transform& transform, int32 childIndex) const override; - - /// @see b2Shape::ComputeAABB - void ComputeAABB(b2AABB* aabb, const b2Transform& transform, int32 childIndex) const override; - - /// @see b2Shape::ComputeMass - void ComputeMass(b2MassData* massData, float density) const override; - - /// Position - b2Vec2 m_p; -}; - -inline b2CircleShape::b2CircleShape() -{ - m_type = e_circle; - m_radius = 0.0f; - m_p.SetZero(); -} - -#endif diff --git a/3rdparty/box2d/include/box2d/b2_collision.h b/3rdparty/box2d/include/box2d/b2_collision.h deleted file mode 100644 index 055704e7c8d6..000000000000 --- a/3rdparty/box2d/include/box2d/b2_collision.h +++ /dev/null @@ -1,306 +0,0 @@ -// MIT License - -// Copyright (c) 2019 Erin Catto - -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: - -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. - -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -#ifndef B2_COLLISION_H -#define B2_COLLISION_H - -#include - -#include "b2_api.h" -#include "b2_math.h" - -/// @file -/// Structures and functions used for computing contact points, distance -/// queries, and TOI queries. - -class b2Shape; -class b2CircleShape; -class b2EdgeShape; -class b2PolygonShape; - -const uint8 b2_nullFeature = UCHAR_MAX; - -/// The features that intersect to form the contact point -/// This must be 4 bytes or less. -struct B2_API b2ContactFeature -{ - enum Type - { - e_vertex = 0, - e_face = 1 - }; - - uint8 indexA; ///< Feature index on shapeA - uint8 indexB; ///< Feature index on shapeB - uint8 typeA; ///< The feature type on shapeA - uint8 typeB; ///< The feature type on shapeB -}; - -/// Contact ids to facilitate warm starting. -union B2_API b2ContactID -{ - b2ContactFeature cf; - uint32 key; ///< Used to quickly compare contact ids. -}; - -/// A manifold point is a contact point belonging to a contact -/// manifold. It holds details related to the geometry and dynamics -/// of the contact points. -/// The local point usage depends on the manifold type: -/// -e_circles: the local center of circleB -/// -e_faceA: the local center of cirlceB or the clip point of polygonB -/// -e_faceB: the clip point of polygonA -/// This structure is stored across time steps, so we keep it small. -/// Note: the impulses are used for internal caching and may not -/// provide reliable contact forces, especially for high speed collisions. -struct B2_API b2ManifoldPoint -{ - b2Vec2 localPoint; ///< usage depends on manifold type - float normalImpulse; ///< the non-penetration impulse - float tangentImpulse; ///< the friction impulse - b2ContactID id; ///< uniquely identifies a contact point between two shapes -}; - -/// A manifold for two touching convex shapes. -/// Box2D supports multiple types of contact: -/// - clip point versus plane with radius -/// - point versus point with radius (circles) -/// The local point usage depends on the manifold type: -/// -e_circles: the local center of circleA -/// -e_faceA: the center of faceA -/// -e_faceB: the center of faceB -/// Similarly the local normal usage: -/// -e_circles: not used -/// -e_faceA: the normal on polygonA -/// -e_faceB: the normal on polygonB -/// We store contacts in this way so that position correction can -/// account for movement, which is critical for continuous physics. -/// All contact scenarios must be expressed in one of these types. -/// This structure is stored across time steps, so we keep it small. -struct B2_API b2Manifold -{ - enum Type - { - e_circles, - e_faceA, - e_faceB - }; - - b2ManifoldPoint points[b2_maxManifoldPoints]; ///< the points of contact - b2Vec2 localNormal; ///< not use for Type::e_points - b2Vec2 localPoint; ///< usage depends on manifold type - Type type; - int32 pointCount; ///< the number of manifold points -}; - -/// This is used to compute the current state of a contact manifold. -struct B2_API b2WorldManifold -{ - /// Evaluate the manifold with supplied transforms. This assumes - /// modest motion from the original state. This does not change the - /// point count, impulses, etc. The radii must come from the shapes - /// that generated the manifold. - void Initialize(const b2Manifold* manifold, - const b2Transform& xfA, float radiusA, - const b2Transform& xfB, float radiusB); - - b2Vec2 normal; ///< world vector pointing from A to B - b2Vec2 points[b2_maxManifoldPoints]; ///< world contact point (point of intersection) - float separations[b2_maxManifoldPoints]; ///< a negative value indicates overlap, in meters -}; - -/// This is used for determining the state of contact points. -enum b2PointState -{ - b2_nullState, ///< point does not exist - b2_addState, ///< point was added in the update - b2_persistState, ///< point persisted across the update - b2_removeState ///< point was removed in the update -}; - -/// Compute the point states given two manifolds. The states pertain to the transition from manifold1 -/// to manifold2. So state1 is either persist or remove while state2 is either add or persist. -B2_API void b2GetPointStates(b2PointState state1[b2_maxManifoldPoints], b2PointState state2[b2_maxManifoldPoints], - const b2Manifold* manifold1, const b2Manifold* manifold2); - -/// Used for computing contact manifolds. -struct B2_API b2ClipVertex -{ - b2Vec2 v; - b2ContactID id; -}; - -/// Ray-cast input data. The ray extends from p1 to p1 + maxFraction * (p2 - p1). -struct B2_API b2RayCastInput -{ - b2Vec2 p1, p2; - float maxFraction; -}; - -/// Ray-cast output data. The ray hits at p1 + fraction * (p2 - p1), where p1 and p2 -/// come from b2RayCastInput. -struct B2_API b2RayCastOutput -{ - b2Vec2 normal; - float fraction; -}; - -/// An axis aligned bounding box. -struct B2_API b2AABB -{ - /// Verify that the bounds are sorted. - bool IsValid() const; - - /// Get the center of the AABB. - b2Vec2 GetCenter() const - { - return 0.5f * (lowerBound + upperBound); - } - - /// Get the extents of the AABB (half-widths). - b2Vec2 GetExtents() const - { - return 0.5f * (upperBound - lowerBound); - } - - /// Get the perimeter length - float GetPerimeter() const - { - float wx = upperBound.x - lowerBound.x; - float wy = upperBound.y - lowerBound.y; - return 2.0f * (wx + wy); - } - - /// Combine an AABB into this one. - void Combine(const b2AABB& aabb) - { - lowerBound = b2Min(lowerBound, aabb.lowerBound); - upperBound = b2Max(upperBound, aabb.upperBound); - } - - /// Combine two AABBs into this one. - void Combine(const b2AABB& aabb1, const b2AABB& aabb2) - { - lowerBound = b2Min(aabb1.lowerBound, aabb2.lowerBound); - upperBound = b2Max(aabb1.upperBound, aabb2.upperBound); - } - - /// Does this aabb contain the provided AABB. - bool Contains(const b2AABB& aabb) const - { - bool result = true; - result = result && lowerBound.x <= aabb.lowerBound.x; - result = result && lowerBound.y <= aabb.lowerBound.y; - result = result && aabb.upperBound.x <= upperBound.x; - result = result && aabb.upperBound.y <= upperBound.y; - return result; - } - - bool RayCast(b2RayCastOutput* output, const b2RayCastInput& input) const; - - b2Vec2 lowerBound; ///< the lower vertex - b2Vec2 upperBound; ///< the upper vertex -}; - -/// Compute the collision manifold between two circles. -B2_API void b2CollideCircles(b2Manifold* manifold, - const b2CircleShape* circleA, const b2Transform& xfA, - const b2CircleShape* circleB, const b2Transform& xfB); - -/// Compute the collision manifold between a polygon and a circle. -B2_API void b2CollidePolygonAndCircle(b2Manifold* manifold, - const b2PolygonShape* polygonA, const b2Transform& xfA, - const b2CircleShape* circleB, const b2Transform& xfB); - -/// Compute the collision manifold between two polygons. -B2_API void b2CollidePolygons(b2Manifold* manifold, - const b2PolygonShape* polygonA, const b2Transform& xfA, - const b2PolygonShape* polygonB, const b2Transform& xfB); - -/// Compute the collision manifold between an edge and a circle. -B2_API void b2CollideEdgeAndCircle(b2Manifold* manifold, - const b2EdgeShape* polygonA, const b2Transform& xfA, - const b2CircleShape* circleB, const b2Transform& xfB); - -/// Compute the collision manifold between an edge and a polygon. -B2_API void b2CollideEdgeAndPolygon(b2Manifold* manifold, - const b2EdgeShape* edgeA, const b2Transform& xfA, - const b2PolygonShape* polygonB, const b2Transform& xfB); - -/// Clipping for contact manifolds. -B2_API int32 b2ClipSegmentToLine(b2ClipVertex vOut[2], const b2ClipVertex vIn[2], - const b2Vec2& normal, float offset, int32 vertexIndexA); - -/// Determine if two generic shapes overlap. -B2_API bool b2TestOverlap( const b2Shape* shapeA, int32 indexA, - const b2Shape* shapeB, int32 indexB, - const b2Transform& xfA, const b2Transform& xfB); - -/// Convex hull used for polygon collision -struct b2Hull -{ - b2Vec2 points[b2_maxPolygonVertices]; - int32 count; -}; - -/// Compute the convex hull of a set of points. Returns an empty hull if it fails. -/// Some failure cases: -/// - all points very close together -/// - all points on a line -/// - less than 3 points -/// - more than b2_maxPolygonVertices points -/// This welds close points and removes collinear points. -b2Hull b2ComputeHull(const b2Vec2* points, int32 count); - -/// This determines if a hull is valid. Checks for: -/// - convexity -/// - collinear points -/// This is expensive and should not be called at runtime. -bool b2ValidateHull(const b2Hull& hull); - - -// ---------------- Inline Functions ------------------------------------------ - -inline bool b2AABB::IsValid() const -{ - b2Vec2 d = upperBound - lowerBound; - bool valid = d.x >= 0.0f && d.y >= 0.0f; - valid = valid && lowerBound.IsValid() && upperBound.IsValid(); - return valid; -} - -inline bool b2TestOverlap(const b2AABB& a, const b2AABB& b) -{ - b2Vec2 d1, d2; - d1 = b.lowerBound - a.upperBound; - d2 = a.lowerBound - b.upperBound; - - if (d1.x > 0.0f || d1.y > 0.0f) - return false; - - if (d2.x > 0.0f || d2.y > 0.0f) - return false; - - return true; -} - -#endif diff --git a/3rdparty/box2d/include/box2d/b2_common.h b/3rdparty/box2d/include/box2d/b2_common.h deleted file mode 100644 index dfca8af1bf9d..000000000000 --- a/3rdparty/box2d/include/box2d/b2_common.h +++ /dev/null @@ -1,138 +0,0 @@ -// MIT License - -// Copyright (c) 2019 Erin Catto - -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: - -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. - -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -#ifndef B2_COMMON_H -#define B2_COMMON_H - -#include "b2_settings.h" - -#include -#include -#include - -#if !defined(NDEBUG) - #define b2DEBUG -#endif - -#define B2_NOT_USED(x) ((void)(x)) -#define b2Assert(A) assert(A) - -#define b2_maxFloat FLT_MAX -#define b2_epsilon FLT_EPSILON -#define b2_pi 3.14159265359f - -/// @file -/// Global tuning constants based on meters-kilograms-seconds (MKS) units. -/// - -// Collision - -/// The maximum number of contact points between two convex shapes. Do -/// not change this value. -#define b2_maxManifoldPoints 2 - -/// This is used to fatten AABBs in the dynamic tree. This allows proxies -/// to move by a small amount without triggering a tree adjustment. -/// This is in meters. -#define b2_aabbExtension (0.1f * b2_lengthUnitsPerMeter) - -/// This is used to fatten AABBs in the dynamic tree. This is used to predict -/// the future position based on the current displacement. -/// This is a dimensionless multiplier. -#define b2_aabbMultiplier 4.0f - -/// A small length used as a collision and constraint tolerance. Usually it is -/// chosen to be numerically significant, but visually insignificant. In meters. -#define b2_linearSlop (0.005f * b2_lengthUnitsPerMeter) - -/// A small angle used as a collision and constraint tolerance. Usually it is -/// chosen to be numerically significant, but visually insignificant. -#define b2_angularSlop (2.0f / 180.0f * b2_pi) - -/// The radius of the polygon/edge shape skin. This should not be modified. Making -/// this smaller means polygons will have an insufficient buffer for continuous collision. -/// Making it larger may create artifacts for vertex collision. -#define b2_polygonRadius (2.0f * b2_linearSlop) - -/// Maximum number of sub-steps per contact in continuous physics simulation. -#define b2_maxSubSteps 8 - - -// Dynamics - -/// Maximum number of contacts to be handled to solve a TOI impact. -#define b2_maxTOIContacts 32 - -/// The maximum linear position correction used when solving constraints. This helps to -/// prevent overshoot. Meters. -#define b2_maxLinearCorrection (0.2f * b2_lengthUnitsPerMeter) - -/// The maximum angular position correction used when solving constraints. This helps to -/// prevent overshoot. -#define b2_maxAngularCorrection (8.0f / 180.0f * b2_pi) - -/// The maximum linear translation of a body per step. This limit is very large and is used -/// to prevent numerical problems. You shouldn't need to adjust this. Meters. -#define b2_maxTranslation (2.0f * b2_lengthUnitsPerMeter) -#define b2_maxTranslationSquared (b2_maxTranslation * b2_maxTranslation) - -/// The maximum angular velocity of a body. This limit is very large and is used -/// to prevent numerical problems. You shouldn't need to adjust this. -#define b2_maxRotation (0.5f * b2_pi) -#define b2_maxRotationSquared (b2_maxRotation * b2_maxRotation) - -/// This scale factor controls how fast overlap is resolved. Ideally this would be 1 so -/// that overlap is removed in one time step. However using values close to 1 often lead -/// to overshoot. -#define b2_baumgarte 0.2f -#define b2_toiBaumgarte 0.75f - - -// Sleep - -/// The time that a body must be still before it will go to sleep. -#define b2_timeToSleep 0.5f - -/// A body cannot sleep if its linear velocity is above this tolerance. -#define b2_linearSleepTolerance (0.01f * b2_lengthUnitsPerMeter) - -/// A body cannot sleep if its angular velocity is above this tolerance. -#define b2_angularSleepTolerance (2.0f / 180.0f * b2_pi) - -/// Dump to a file. Only one dump file allowed at a time. -void b2OpenDump(const char* fileName); -void b2Dump(const char* string, ...); -void b2CloseDump(); - -/// Version numbering scheme. -/// See http://en.wikipedia.org/wiki/Software_versioning -struct b2Version -{ - int32 major; ///< significant changes - int32 minor; ///< incremental changes - int32 revision; ///< bug fixes -}; - -/// Current version. -extern B2_API b2Version b2_version; - -#endif diff --git a/3rdparty/box2d/include/box2d/b2_contact.h b/3rdparty/box2d/include/box2d/b2_contact.h deleted file mode 100644 index de7541f1350b..000000000000 --- a/3rdparty/box2d/include/box2d/b2_contact.h +++ /dev/null @@ -1,386 +0,0 @@ -// MIT License - -// Copyright (c) 2019 Erin Catto - -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: - -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. - -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -#ifndef B2_CONTACT_H -#define B2_CONTACT_H - -#include "b2_api.h" -#include "b2_collision.h" -#include "b2_fixture.h" -#include "b2_math.h" -#include "b2_shape.h" - -class b2Body; -class b2Contact; -class b2Fixture; -class b2World; -class b2BlockAllocator; -class b2StackAllocator; -class b2ContactListener; - -/// Friction mixing law. The idea is to allow either fixture to drive the friction to zero. -/// For example, anything slides on ice. -inline float b2MixFriction(float friction1, float friction2) -{ - return b2Sqrt(friction1 * friction2); -} - -/// Restitution mixing law. The idea is allow for anything to bounce off an inelastic surface. -/// For example, a superball bounces on anything. -inline float b2MixRestitution(float restitution1, float restitution2) -{ - return restitution1 > restitution2 ? restitution1 : restitution2; -} - -/// Restitution mixing law. This picks the lowest value. -inline float b2MixRestitutionThreshold(float threshold1, float threshold2) -{ - return threshold1 < threshold2 ? threshold1 : threshold2; -} - -typedef b2Contact* b2ContactCreateFcn( b2Fixture* fixtureA, int32 indexA, - b2Fixture* fixtureB, int32 indexB, - b2BlockAllocator* allocator); -typedef void b2ContactDestroyFcn(b2Contact* contact, b2BlockAllocator* allocator); - -struct B2_API b2ContactRegister -{ - b2ContactCreateFcn* createFcn; - b2ContactDestroyFcn* destroyFcn; - bool primary; -}; - -/// A contact edge is used to connect bodies and contacts together -/// in a contact graph where each body is a node and each contact -/// is an edge. A contact edge belongs to a doubly linked list -/// maintained in each attached body. Each contact has two contact -/// nodes, one for each attached body. -struct B2_API b2ContactEdge -{ - b2Body* other; ///< provides quick access to the other body attached. - b2Contact* contact; ///< the contact - b2ContactEdge* prev; ///< the previous contact edge in the body's contact list - b2ContactEdge* next; ///< the next contact edge in the body's contact list -}; - -/// The class manages contact between two shapes. A contact exists for each overlapping -/// AABB in the broad-phase (except if filtered). Therefore a contact object may exist -/// that has no contact points. -class B2_API b2Contact -{ -public: - - /// Get the contact manifold. Do not modify the manifold unless you understand the - /// internals of Box2D. - b2Manifold* GetManifold(); - const b2Manifold* GetManifold() const; - - /// Get the world manifold. - void GetWorldManifold(b2WorldManifold* worldManifold) const; - - /// Is this contact touching? - bool IsTouching() const; - - /// Enable/disable this contact. This can be used inside the pre-solve - /// contact listener. The contact is only disabled for the current - /// time step (or sub-step in continuous collisions). - void SetEnabled(bool flag); - - /// Has this contact been disabled? - bool IsEnabled() const; - - /// Get the next contact in the world's contact list. - b2Contact* GetNext(); - const b2Contact* GetNext() const; - - /// Get fixture A in this contact. - b2Fixture* GetFixtureA(); - const b2Fixture* GetFixtureA() const; - - /// Get the child primitive index for fixture A. - int32 GetChildIndexA() const; - - /// Get fixture B in this contact. - b2Fixture* GetFixtureB(); - const b2Fixture* GetFixtureB() const; - - /// Get the child primitive index for fixture B. - int32 GetChildIndexB() const; - - /// Override the default friction mixture. You can call this in b2ContactListener::PreSolve. - /// This value persists until set or reset. - void SetFriction(float friction); - - /// Get the friction. - float GetFriction() const; - - /// Reset the friction mixture to the default value. - void ResetFriction(); - - /// Override the default restitution mixture. You can call this in b2ContactListener::PreSolve. - /// The value persists until you set or reset. - void SetRestitution(float restitution); - - /// Get the restitution. - float GetRestitution() const; - - /// Reset the restitution to the default value. - void ResetRestitution(); - - /// Override the default restitution velocity threshold mixture. You can call this in b2ContactListener::PreSolve. - /// The value persists until you set or reset. - void SetRestitutionThreshold(float threshold); - - /// Get the restitution threshold. - float GetRestitutionThreshold() const; - - /// Reset the restitution threshold to the default value. - void ResetRestitutionThreshold(); - - /// Set the desired tangent speed for a conveyor belt behavior. In meters per second. - void SetTangentSpeed(float speed); - - /// Get the desired tangent speed. In meters per second. - float GetTangentSpeed() const; - - /// Evaluate this contact with your own manifold and transforms. - virtual void Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB) = 0; - -protected: - friend class b2ContactManager; - friend class b2World; - friend class b2ContactSolver; - friend class b2Body; - friend class b2Fixture; - - // Flags stored in m_flags - enum - { - // Used when crawling contact graph when forming islands. - e_islandFlag = 0x0001, - - // Set when the shapes are touching. - e_touchingFlag = 0x0002, - - // This contact can be disabled (by user) - e_enabledFlag = 0x0004, - - // This contact needs filtering because a fixture filter was changed. - e_filterFlag = 0x0008, - - // This bullet contact had a TOI event - e_bulletHitFlag = 0x0010, - - // This contact has a valid TOI in m_toi - e_toiFlag = 0x0020 - }; - - /// Flag this contact for filtering. Filtering will occur the next time step. - void FlagForFiltering(); - - static void AddType(b2ContactCreateFcn* createFcn, b2ContactDestroyFcn* destroyFcn, - b2Shape::Type typeA, b2Shape::Type typeB); - static void InitializeRegisters(); - static b2Contact* Create(b2Fixture* fixtureA, int32 indexA, b2Fixture* fixtureB, int32 indexB, b2BlockAllocator* allocator); - static void Destroy(b2Contact* contact, b2Shape::Type typeA, b2Shape::Type typeB, b2BlockAllocator* allocator); - static void Destroy(b2Contact* contact, b2BlockAllocator* allocator); - - b2Contact() : m_fixtureA(nullptr), m_fixtureB(nullptr) {} - b2Contact(b2Fixture* fixtureA, int32 indexA, b2Fixture* fixtureB, int32 indexB); - virtual ~b2Contact() {} - - void Update(b2ContactListener* listener); - - static b2ContactRegister s_registers[b2Shape::e_typeCount][b2Shape::e_typeCount]; - static bool s_initialized; - - uint32 m_flags; - - // World pool and list pointers. - b2Contact* m_prev; - b2Contact* m_next; - - // Nodes for connecting bodies. - b2ContactEdge m_nodeA; - b2ContactEdge m_nodeB; - - b2Fixture* m_fixtureA; - b2Fixture* m_fixtureB; - - int32 m_indexA; - int32 m_indexB; - - b2Manifold m_manifold; - - int32 m_toiCount; - float m_toi; - - float m_friction; - float m_restitution; - float m_restitutionThreshold; - - float m_tangentSpeed; -}; - -inline b2Manifold* b2Contact::GetManifold() -{ - return &m_manifold; -} - -inline const b2Manifold* b2Contact::GetManifold() const -{ - return &m_manifold; -} - -inline void b2Contact::GetWorldManifold(b2WorldManifold* worldManifold) const -{ - const b2Body* bodyA = m_fixtureA->GetBody(); - const b2Body* bodyB = m_fixtureB->GetBody(); - const b2Shape* shapeA = m_fixtureA->GetShape(); - const b2Shape* shapeB = m_fixtureB->GetShape(); - - worldManifold->Initialize(&m_manifold, bodyA->GetTransform(), shapeA->m_radius, bodyB->GetTransform(), shapeB->m_radius); -} - -inline void b2Contact::SetEnabled(bool flag) -{ - if (flag) - { - m_flags |= e_enabledFlag; - } - else - { - m_flags &= ~e_enabledFlag; - } -} - -inline bool b2Contact::IsEnabled() const -{ - return (m_flags & e_enabledFlag) == e_enabledFlag; -} - -inline bool b2Contact::IsTouching() const -{ - return (m_flags & e_touchingFlag) == e_touchingFlag; -} - -inline b2Contact* b2Contact::GetNext() -{ - return m_next; -} - -inline const b2Contact* b2Contact::GetNext() const -{ - return m_next; -} - -inline b2Fixture* b2Contact::GetFixtureA() -{ - return m_fixtureA; -} - -inline const b2Fixture* b2Contact::GetFixtureA() const -{ - return m_fixtureA; -} - -inline b2Fixture* b2Contact::GetFixtureB() -{ - return m_fixtureB; -} - -inline int32 b2Contact::GetChildIndexA() const -{ - return m_indexA; -} - -inline const b2Fixture* b2Contact::GetFixtureB() const -{ - return m_fixtureB; -} - -inline int32 b2Contact::GetChildIndexB() const -{ - return m_indexB; -} - -inline void b2Contact::FlagForFiltering() -{ - m_flags |= e_filterFlag; -} - -inline void b2Contact::SetFriction(float friction) -{ - m_friction = friction; -} - -inline float b2Contact::GetFriction() const -{ - return m_friction; -} - -inline void b2Contact::ResetFriction() -{ - m_friction = b2MixFriction(m_fixtureA->m_friction, m_fixtureB->m_friction); -} - -inline void b2Contact::SetRestitution(float restitution) -{ - m_restitution = restitution; -} - -inline float b2Contact::GetRestitution() const -{ - return m_restitution; -} - -inline void b2Contact::ResetRestitution() -{ - m_restitution = b2MixRestitution(m_fixtureA->m_restitution, m_fixtureB->m_restitution); -} - -inline void b2Contact::SetRestitutionThreshold(float threshold) -{ - m_restitutionThreshold = threshold; -} - -inline float b2Contact::GetRestitutionThreshold() const -{ - return m_restitutionThreshold; -} - -inline void b2Contact::ResetRestitutionThreshold() -{ - m_restitutionThreshold = b2MixRestitutionThreshold(m_fixtureA->m_restitutionThreshold, m_fixtureB->m_restitutionThreshold); -} - -inline void b2Contact::SetTangentSpeed(float speed) -{ - m_tangentSpeed = speed; -} - -inline float b2Contact::GetTangentSpeed() const -{ - return m_tangentSpeed; -} - -#endif diff --git a/3rdparty/box2d/include/box2d/b2_contact_manager.h b/3rdparty/box2d/include/box2d/b2_contact_manager.h deleted file mode 100644 index fbd3b4d401be..000000000000 --- a/3rdparty/box2d/include/box2d/b2_contact_manager.h +++ /dev/null @@ -1,57 +0,0 @@ -// MIT License - -// Copyright (c) 2019 Erin Catto - -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: - -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. - -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -#ifndef B2_CONTACT_MANAGER_H -#define B2_CONTACT_MANAGER_H - -#include "b2_api.h" -#include "b2_broad_phase.h" - -class b2Contact; -class b2ContactFilter; -class b2ContactListener; -class b2BlockAllocator; - -// Delegate of b2World. -class B2_API b2ContactManager -{ -public: - b2ContactManager(); - - // Broad-phase callback. - void AddPair(void* proxyUserDataA, void* proxyUserDataB); - - void FindNewContacts(); - - void Destroy(b2Contact* c); - - void Collide(); - - b2BroadPhase m_broadPhase; - b2Contact* m_contactList; - int32 m_contactCount; - b2ContactFilter* m_contactFilter; - b2ContactListener* m_contactListener; - b2BlockAllocator* m_allocator; -}; - -#endif diff --git a/3rdparty/box2d/include/box2d/b2_distance.h b/3rdparty/box2d/include/box2d/b2_distance.h deleted file mode 100644 index 3e05773de339..000000000000 --- a/3rdparty/box2d/include/box2d/b2_distance.h +++ /dev/null @@ -1,171 +0,0 @@ -// MIT License - -// Copyright (c) 2019 Erin Catto - -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: - -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. - -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -#ifndef B2_DISTANCE_H -#define B2_DISTANCE_H - -#include "b2_api.h" -#include "b2_math.h" - -class b2Shape; - -/// A distance proxy is used by the GJK algorithm. -/// It encapsulates any shape. -struct B2_API b2DistanceProxy -{ - b2DistanceProxy() : m_vertices(nullptr), m_count(0), m_radius(0.0f) {} - - /// Initialize the proxy using the given shape. The shape - /// must remain in scope while the proxy is in use. - void Set(const b2Shape* shape, int32 index); - - /// Initialize the proxy using a vertex cloud and radius. The vertices - /// must remain in scope while the proxy is in use. - void Set(const b2Vec2* vertices, int32 count, float radius); - - /// Get the supporting vertex index in the given direction. - int32 GetSupport(const b2Vec2& d) const; - - /// Get the supporting vertex in the given direction. - const b2Vec2& GetSupportVertex(const b2Vec2& d) const; - - /// Get the vertex count. - int32 GetVertexCount() const; - - /// Get a vertex by index. Used by b2Distance. - const b2Vec2& GetVertex(int32 index) const; - - b2Vec2 m_buffer[2]; - const b2Vec2* m_vertices; - int32 m_count; - float m_radius; -}; - -/// Used to warm start b2Distance. -/// Set count to zero on first call. -struct B2_API b2SimplexCache -{ - float metric; ///< length or area - uint16 count; - uint8 indexA[3]; ///< vertices on shape A - uint8 indexB[3]; ///< vertices on shape B -}; - -/// Input for b2Distance. -/// You have to option to use the shape radii -/// in the computation. Even -struct B2_API b2DistanceInput -{ - b2DistanceProxy proxyA; - b2DistanceProxy proxyB; - b2Transform transformA; - b2Transform transformB; - bool useRadii; -}; - -/// Output for b2Distance. -struct B2_API b2DistanceOutput -{ - b2Vec2 pointA; ///< closest point on shapeA - b2Vec2 pointB; ///< closest point on shapeB - float distance; - int32 iterations; ///< number of GJK iterations used -}; - -/// Compute the closest points between two shapes. Supports any combination of: -/// b2CircleShape, b2PolygonShape, b2EdgeShape. The simplex cache is input/output. -/// On the first call set b2SimplexCache.count to zero. -B2_API void b2Distance(b2DistanceOutput* output, - b2SimplexCache* cache, - const b2DistanceInput* input); - -/// Input parameters for b2ShapeCast -struct B2_API b2ShapeCastInput -{ - b2DistanceProxy proxyA; - b2DistanceProxy proxyB; - b2Transform transformA; - b2Transform transformB; - b2Vec2 translationB; -}; - -/// Output results for b2ShapeCast -struct B2_API b2ShapeCastOutput -{ - b2Vec2 point; - b2Vec2 normal; - float lambda; - int32 iterations; -}; - -/// Perform a linear shape cast of shape B moving and shape A fixed. Determines the hit point, normal, and translation fraction. -/// @returns true if hit, false if there is no hit or an initial overlap -B2_API bool b2ShapeCast(b2ShapeCastOutput* output, const b2ShapeCastInput* input); - -////////////////////////////////////////////////////////////////////////// - -inline int32 b2DistanceProxy::GetVertexCount() const -{ - return m_count; -} - -inline const b2Vec2& b2DistanceProxy::GetVertex(int32 index) const -{ - b2Assert(0 <= index && index < m_count); - return m_vertices[index]; -} - -inline int32 b2DistanceProxy::GetSupport(const b2Vec2& d) const -{ - int32 bestIndex = 0; - float bestValue = b2Dot(m_vertices[0], d); - for (int32 i = 1; i < m_count; ++i) - { - float value = b2Dot(m_vertices[i], d); - if (value > bestValue) - { - bestIndex = i; - bestValue = value; - } - } - - return bestIndex; -} - -inline const b2Vec2& b2DistanceProxy::GetSupportVertex(const b2Vec2& d) const -{ - int32 bestIndex = 0; - float bestValue = b2Dot(m_vertices[0], d); - for (int32 i = 1; i < m_count; ++i) - { - float value = b2Dot(m_vertices[i], d); - if (value > bestValue) - { - bestIndex = i; - bestValue = value; - } - } - - return m_vertices[bestIndex]; -} - -#endif diff --git a/3rdparty/box2d/include/box2d/b2_distance_joint.h b/3rdparty/box2d/include/box2d/b2_distance_joint.h deleted file mode 100644 index cfc75fefba4e..000000000000 --- a/3rdparty/box2d/include/box2d/b2_distance_joint.h +++ /dev/null @@ -1,176 +0,0 @@ -// MIT License - -// Copyright (c) 2019 Erin Catto - -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: - -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. - -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -#ifndef B2_DISTANCE_JOINT_H -#define B2_DISTANCE_JOINT_H - -#include "b2_api.h" -#include "b2_joint.h" - -/// Distance joint definition. This requires defining an anchor point on both -/// bodies and the non-zero distance of the distance joint. The definition uses -/// local anchor points so that the initial configuration can violate the -/// constraint slightly. This helps when saving and loading a game. -struct B2_API b2DistanceJointDef : public b2JointDef -{ - b2DistanceJointDef() - { - type = e_distanceJoint; - localAnchorA.Set(0.0f, 0.0f); - localAnchorB.Set(0.0f, 0.0f); - length = 1.0f; - minLength = 0.0f; - maxLength = FLT_MAX; - stiffness = 0.0f; - damping = 0.0f; - } - - /// Initialize the bodies, anchors, and rest length using world space anchors. - /// The minimum and maximum lengths are set to the rest length. - void Initialize(b2Body* bodyA, b2Body* bodyB, - const b2Vec2& anchorA, const b2Vec2& anchorB); - - /// The local anchor point relative to bodyA's origin. - b2Vec2 localAnchorA; - - /// The local anchor point relative to bodyB's origin. - b2Vec2 localAnchorB; - - /// The rest length of this joint. Clamped to a stable minimum value. - float length; - - /// Minimum length. Clamped to a stable minimum value. - float minLength; - - /// Maximum length. Must be greater than or equal to the minimum length. - float maxLength; - - /// The linear stiffness in N/m. - float stiffness; - - /// The linear damping in N*s/m. - float damping; -}; - -/// A distance joint constrains two points on two bodies to remain at a fixed -/// distance from each other. You can view this as a massless, rigid rod. -class B2_API b2DistanceJoint : public b2Joint -{ -public: - - b2Vec2 GetAnchorA() const override; - b2Vec2 GetAnchorB() const override; - - /// Get the reaction force given the inverse time step. - /// Unit is N. - b2Vec2 GetReactionForce(float inv_dt) const override; - - /// Get the reaction torque given the inverse time step. - /// Unit is N*m. This is always zero for a distance joint. - float GetReactionTorque(float inv_dt) const override; - - /// The local anchor point relative to bodyA's origin. - const b2Vec2& GetLocalAnchorA() const { return m_localAnchorA; } - - /// The local anchor point relative to bodyB's origin. - const b2Vec2& GetLocalAnchorB() const { return m_localAnchorB; } - - /// Get the rest length - float GetLength() const { return m_length; } - - /// Set the rest length - /// @returns clamped rest length - float SetLength(float length); - - /// Get the minimum length - float GetMinLength() const { return m_minLength; } - - /// Set the minimum length - /// @returns the clamped minimum length - float SetMinLength(float minLength); - - /// Get the maximum length - float GetMaxLength() const { return m_maxLength; } - - /// Set the maximum length - /// @returns the clamped maximum length - float SetMaxLength(float maxLength); - - /// Get the current length - float GetCurrentLength() const; - - /// Set/get the linear stiffness in N/m - void SetStiffness(float stiffness) { m_stiffness = stiffness; } - float GetStiffness() const { return m_stiffness; } - - /// Set/get linear damping in N*s/m - void SetDamping(float damping) { m_damping = damping; } - float GetDamping() const { return m_damping; } - - /// Dump joint to dmLog - void Dump() override; - - /// - void Draw(b2Draw* draw) const override; - -protected: - - friend class b2Joint; - b2DistanceJoint(const b2DistanceJointDef* data); - - void InitVelocityConstraints(const b2SolverData& data) override; - void SolveVelocityConstraints(const b2SolverData& data) override; - bool SolvePositionConstraints(const b2SolverData& data) override; - - float m_stiffness; - float m_damping; - float m_bias; - float m_length; - float m_minLength; - float m_maxLength; - - // Solver shared - b2Vec2 m_localAnchorA; - b2Vec2 m_localAnchorB; - float m_gamma; - float m_impulse; - float m_lowerImpulse; - float m_upperImpulse; - - // Solver temp - int32 m_indexA; - int32 m_indexB; - b2Vec2 m_u; - b2Vec2 m_rA; - b2Vec2 m_rB; - b2Vec2 m_localCenterA; - b2Vec2 m_localCenterB; - float m_currentLength; - float m_invMassA; - float m_invMassB; - float m_invIA; - float m_invIB; - float m_softMass; - float m_mass; -}; - -#endif diff --git a/3rdparty/box2d/include/box2d/b2_draw.h b/3rdparty/box2d/include/box2d/b2_draw.h deleted file mode 100644 index a4d17118781b..000000000000 --- a/3rdparty/box2d/include/box2d/b2_draw.h +++ /dev/null @@ -1,102 +0,0 @@ -// MIT License - -// Copyright (c) 2019 Erin Catto - -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: - -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. - -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -#ifndef B2_DRAW_H -#define B2_DRAW_H - -#include "b2_api.h" -#include "b2_math.h" - -/// Color for debug drawing. Each value has the range [0,1]. -struct B2_API b2Color -{ - b2Color() {} - b2Color(float rIn, float gIn, float bIn, float aIn = 1.0f) - { - r = rIn; g = gIn; b = bIn; a = aIn; - } - - void Set(float rIn, float gIn, float bIn, float aIn = 1.0f) - { - r = rIn; g = gIn; b = bIn; a = aIn; - } - - float r, g, b, a; -}; - -/// Implement and register this class with a b2World to provide debug drawing of physics -/// entities in your game. -class B2_API b2Draw -{ -public: - b2Draw(); - - virtual ~b2Draw() {} - - enum - { - e_shapeBit = 0x0001, ///< draw shapes - e_jointBit = 0x0002, ///< draw joint connections - e_aabbBit = 0x0004, ///< draw axis aligned bounding boxes - e_pairBit = 0x0008, ///< draw broad-phase pairs - e_centerOfMassBit = 0x0010 ///< draw center of mass frame - }; - - /// Set the drawing flags. - void SetFlags(uint32 flags); - - /// Get the drawing flags. - uint32 GetFlags() const; - - /// Append flags to the current flags. - void AppendFlags(uint32 flags); - - /// Clear flags from the current flags. - void ClearFlags(uint32 flags); - - /// Draw a closed polygon provided in CCW order. - virtual void DrawPolygon(const b2Vec2* vertices, int32 vertexCount, const b2Color& color) = 0; - - /// Draw a solid closed polygon provided in CCW order. - virtual void DrawSolidPolygon(const b2Vec2* vertices, int32 vertexCount, const b2Color& color) = 0; - - /// Draw a circle. - virtual void DrawCircle(const b2Vec2& center, float radius, const b2Color& color) = 0; - - /// Draw a solid circle. - virtual void DrawSolidCircle(const b2Vec2& center, float radius, const b2Vec2& axis, const b2Color& color) = 0; - - /// Draw a line segment. - virtual void DrawSegment(const b2Vec2& p1, const b2Vec2& p2, const b2Color& color) = 0; - - /// Draw a transform. Choose your own length scale. - /// @param xf a transform. - virtual void DrawTransform(const b2Transform& xf) = 0; - - /// Draw a point. - virtual void DrawPoint(const b2Vec2& p, float size, const b2Color& color) = 0; - -protected: - uint32 m_drawFlags; -}; - -#endif diff --git a/3rdparty/box2d/include/box2d/b2_dynamic_tree.h b/3rdparty/box2d/include/box2d/b2_dynamic_tree.h deleted file mode 100644 index b85491915d92..000000000000 --- a/3rdparty/box2d/include/box2d/b2_dynamic_tree.h +++ /dev/null @@ -1,308 +0,0 @@ -// MIT License - -// Copyright (c) 2019 Erin Catto - -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: - -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. - -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -#ifndef B2_DYNAMIC_TREE_H -#define B2_DYNAMIC_TREE_H - -#include "b2_api.h" -#include "b2_collision.h" -#include "b2_growable_stack.h" - -#define b2_nullNode (-1) - -/// A node in the dynamic tree. The client does not interact with this directly. -struct B2_API b2TreeNode -{ - bool IsLeaf() const - { - return child1 == b2_nullNode; - } - - /// Enlarged AABB - b2AABB aabb; - - void* userData; - - union - { - int32 parent; - int32 next; - }; - - int32 child1; - int32 child2; - - // leaf = 0, free node = -1 - int32 height; - - bool moved; -}; - -/// A dynamic AABB tree broad-phase, inspired by Nathanael Presson's btDbvt. -/// A dynamic tree arranges data in a binary tree to accelerate -/// queries such as volume queries and ray casts. Leafs are proxies -/// with an AABB. In the tree we expand the proxy AABB by b2_fatAABBFactor -/// so that the proxy AABB is bigger than the client object. This allows the client -/// object to move by small amounts without triggering a tree update. -/// -/// Nodes are pooled and relocatable, so we use node indices rather than pointers. -class B2_API b2DynamicTree -{ -public: - /// Constructing the tree initializes the node pool. - b2DynamicTree(); - - /// Destroy the tree, freeing the node pool. - ~b2DynamicTree(); - - /// Create a proxy. Provide a tight fitting AABB and a userData pointer. - int32 CreateProxy(const b2AABB& aabb, void* userData); - - /// Destroy a proxy. This asserts if the id is invalid. - void DestroyProxy(int32 proxyId); - - /// Move a proxy with a swepted AABB. If the proxy has moved outside of its fattened AABB, - /// then the proxy is removed from the tree and re-inserted. Otherwise - /// the function returns immediately. - /// @return true if the proxy was re-inserted. - bool MoveProxy(int32 proxyId, const b2AABB& aabb1, const b2Vec2& displacement); - - /// Get proxy user data. - /// @return the proxy user data or 0 if the id is invalid. - void* GetUserData(int32 proxyId) const; - - bool WasMoved(int32 proxyId) const; - void ClearMoved(int32 proxyId); - - /// Get the fat AABB for a proxy. - const b2AABB& GetFatAABB(int32 proxyId) const; - - /// Query an AABB for overlapping proxies. The callback class - /// is called for each proxy that overlaps the supplied AABB. - template - void Query(T* callback, const b2AABB& aabb) const; - - /// Ray-cast against the proxies in the tree. This relies on the callback - /// to perform a exact ray-cast in the case were the proxy contains a shape. - /// The callback also performs the any collision filtering. This has performance - /// roughly equal to k * log(n), where k is the number of collisions and n is the - /// number of proxies in the tree. - /// @param input the ray-cast input data. The ray extends from p1 to p1 + maxFraction * (p2 - p1). - /// @param callback a callback class that is called for each proxy that is hit by the ray. - template - void RayCast(T* callback, const b2RayCastInput& input) const; - - /// Validate this tree. For testing. - void Validate() const; - - /// Compute the height of the binary tree in O(N) time. Should not be - /// called often. - int32 GetHeight() const; - - /// Get the maximum balance of an node in the tree. The balance is the difference - /// in height of the two children of a node. - int32 GetMaxBalance() const; - - /// Get the ratio of the sum of the node areas to the root area. - float GetAreaRatio() const; - - /// Build an optimal tree. Very expensive. For testing. - void RebuildBottomUp(); - - /// Shift the world origin. Useful for large worlds. - /// The shift formula is: position -= newOrigin - /// @param newOrigin the new origin with respect to the old origin - void ShiftOrigin(const b2Vec2& newOrigin); - -private: - - int32 AllocateNode(); - void FreeNode(int32 node); - - void InsertLeaf(int32 node); - void RemoveLeaf(int32 node); - - int32 Balance(int32 index); - - int32 ComputeHeight() const; - int32 ComputeHeight(int32 nodeId) const; - - void ValidateStructure(int32 index) const; - void ValidateMetrics(int32 index) const; - - int32 m_root; - - b2TreeNode* m_nodes; - int32 m_nodeCount; - int32 m_nodeCapacity; - - int32 m_freeList; - - int32 m_insertionCount; -}; - -inline void* b2DynamicTree::GetUserData(int32 proxyId) const -{ - b2Assert(0 <= proxyId && proxyId < m_nodeCapacity); - return m_nodes[proxyId].userData; -} - -inline bool b2DynamicTree::WasMoved(int32 proxyId) const -{ - b2Assert(0 <= proxyId && proxyId < m_nodeCapacity); - return m_nodes[proxyId].moved; -} - -inline void b2DynamicTree::ClearMoved(int32 proxyId) -{ - b2Assert(0 <= proxyId && proxyId < m_nodeCapacity); - m_nodes[proxyId].moved = false; -} - -inline const b2AABB& b2DynamicTree::GetFatAABB(int32 proxyId) const -{ - b2Assert(0 <= proxyId && proxyId < m_nodeCapacity); - return m_nodes[proxyId].aabb; -} - -template -inline void b2DynamicTree::Query(T* callback, const b2AABB& aabb) const -{ - b2GrowableStack stack; - stack.Push(m_root); - - while (stack.GetCount() > 0) - { - int32 nodeId = stack.Pop(); - if (nodeId == b2_nullNode) - { - continue; - } - - const b2TreeNode* node = m_nodes + nodeId; - - if (b2TestOverlap(node->aabb, aabb)) - { - if (node->IsLeaf()) - { - bool proceed = callback->QueryCallback(nodeId); - if (proceed == false) - { - return; - } - } - else - { - stack.Push(node->child1); - stack.Push(node->child2); - } - } - } -} - -template -inline void b2DynamicTree::RayCast(T* callback, const b2RayCastInput& input) const -{ - b2Vec2 p1 = input.p1; - b2Vec2 p2 = input.p2; - b2Vec2 r = p2 - p1; - b2Assert(r.LengthSquared() > 0.0f); - r.Normalize(); - - // v is perpendicular to the segment. - b2Vec2 v = b2Cross(1.0f, r); - b2Vec2 abs_v = b2Abs(v); - - // Separating axis for segment (Gino, p80). - // |dot(v, p1 - c)| > dot(|v|, h) - - float maxFraction = input.maxFraction; - - // Build a bounding box for the segment. - b2AABB segmentAABB; - { - b2Vec2 t = p1 + maxFraction * (p2 - p1); - segmentAABB.lowerBound = b2Min(p1, t); - segmentAABB.upperBound = b2Max(p1, t); - } - - b2GrowableStack stack; - stack.Push(m_root); - - while (stack.GetCount() > 0) - { - int32 nodeId = stack.Pop(); - if (nodeId == b2_nullNode) - { - continue; - } - - const b2TreeNode* node = m_nodes + nodeId; - - if (b2TestOverlap(node->aabb, segmentAABB) == false) - { - continue; - } - - // Separating axis for segment (Gino, p80). - // |dot(v, p1 - c)| > dot(|v|, h) - b2Vec2 c = node->aabb.GetCenter(); - b2Vec2 h = node->aabb.GetExtents(); - float separation = b2Abs(b2Dot(v, p1 - c)) - b2Dot(abs_v, h); - if (separation > 0.0f) - { - continue; - } - - if (node->IsLeaf()) - { - b2RayCastInput subInput; - subInput.p1 = input.p1; - subInput.p2 = input.p2; - subInput.maxFraction = maxFraction; - - float value = callback->RayCastCallback(subInput, nodeId); - - if (value == 0.0f) - { - // The client has terminated the ray cast. - return; - } - - if (value > 0.0f) - { - // Update segment bounding box. - maxFraction = value; - b2Vec2 t = p1 + maxFraction * (p2 - p1); - segmentAABB.lowerBound = b2Min(p1, t); - segmentAABB.upperBound = b2Max(p1, t); - } - } - else - { - stack.Push(node->child1); - stack.Push(node->child2); - } - } -} - -#endif diff --git a/3rdparty/box2d/include/box2d/b2_edge_shape.h b/3rdparty/box2d/include/box2d/b2_edge_shape.h deleted file mode 100644 index b930ee87281d..000000000000 --- a/3rdparty/box2d/include/box2d/b2_edge_shape.h +++ /dev/null @@ -1,86 +0,0 @@ -// MIT License - -// Copyright (c) 2019 Erin Catto - -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: - -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. - -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -#ifndef B2_EDGE_SHAPE_H -#define B2_EDGE_SHAPE_H - -#include "b2_api.h" -#include "b2_shape.h" - -/// A line segment (edge) shape. These can be connected in chains or loops -/// to other edge shapes. Edges created independently are two-sided and do -/// no provide smooth movement across junctions. -class B2_API b2EdgeShape : public b2Shape -{ -public: - b2EdgeShape(); - - /// Set this as a part of a sequence. Vertex v0 precedes the edge and vertex v3 - /// follows. These extra vertices are used to provide smooth movement - /// across junctions. This also makes the collision one-sided. The edge - /// normal points to the right looking from v1 to v2. - void SetOneSided(const b2Vec2& v0, const b2Vec2& v1,const b2Vec2& v2, const b2Vec2& v3); - - /// Set this as an isolated edge. Collision is two-sided. - void SetTwoSided(const b2Vec2& v1, const b2Vec2& v2); - - /// Implement b2Shape. - b2Shape* Clone(b2BlockAllocator* allocator) const override; - - /// @see b2Shape::GetChildCount - int32 GetChildCount() const override; - - /// @see b2Shape::TestPoint - bool TestPoint(const b2Transform& transform, const b2Vec2& p) const override; - - /// Implement b2Shape. - bool RayCast(b2RayCastOutput* output, const b2RayCastInput& input, - const b2Transform& transform, int32 childIndex) const override; - - /// @see b2Shape::ComputeAABB - void ComputeAABB(b2AABB* aabb, const b2Transform& transform, int32 childIndex) const override; - - /// @see b2Shape::ComputeMass - void ComputeMass(b2MassData* massData, float density) const override; - - /// These are the edge vertices - b2Vec2 m_vertex1, m_vertex2; - - /// Optional adjacent vertices. These are used for smooth collision. - b2Vec2 m_vertex0, m_vertex3; - - /// Uses m_vertex0 and m_vertex3 to create smooth collision. - bool m_oneSided; -}; - -inline b2EdgeShape::b2EdgeShape() -{ - m_type = e_edge; - m_radius = b2_polygonRadius; - m_vertex0.x = 0.0f; - m_vertex0.y = 0.0f; - m_vertex3.x = 0.0f; - m_vertex3.y = 0.0f; - m_oneSided = false; -} - -#endif diff --git a/3rdparty/box2d/include/box2d/b2_fixture.h b/3rdparty/box2d/include/box2d/b2_fixture.h deleted file mode 100644 index 47d321df5ec1..000000000000 --- a/3rdparty/box2d/include/box2d/b2_fixture.h +++ /dev/null @@ -1,371 +0,0 @@ -// MIT License - -// Copyright (c) 2019 Erin Catto - -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: - -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. - -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -#ifndef B2_FIXTURE_H -#define B2_FIXTURE_H - -#include "b2_api.h" -#include "b2_body.h" -#include "b2_collision.h" -#include "b2_shape.h" - -class b2BlockAllocator; -class b2Body; -class b2BroadPhase; -class b2Fixture; - -/// This holds contact filtering data. -struct B2_API b2Filter -{ - b2Filter() - { - categoryBits = 0x0001; - maskBits = 0xFFFF; - groupIndex = 0; - } - - /// The collision category bits. Normally you would just set one bit. - uint16 categoryBits; - - /// The collision mask bits. This states the categories that this - /// shape would accept for collision. - uint16 maskBits; - - /// Collision groups allow a certain group of objects to never collide (negative) - /// or always collide (positive). Zero means no collision group. Non-zero group - /// filtering always wins against the mask bits. - int16 groupIndex; -}; - -/// A fixture definition is used to create a fixture. This class defines an -/// abstract fixture definition. You can reuse fixture definitions safely. -struct B2_API b2FixtureDef -{ - /// The constructor sets the default fixture definition values. - b2FixtureDef() - { - shape = nullptr; - friction = 0.2f; - restitution = 0.0f; - restitutionThreshold = 1.0f * b2_lengthUnitsPerMeter; - density = 0.0f; - isSensor = false; - } - - /// The shape, this must be set. The shape will be cloned, so you - /// can create the shape on the stack. - const b2Shape* shape; - - /// Use this to store application specific fixture data. - b2FixtureUserData userData; - - /// The friction coefficient, usually in the range [0,1]. - float friction; - - /// The restitution (elasticity) usually in the range [0,1]. - float restitution; - - /// Restitution velocity threshold, usually in m/s. Collisions above this - /// speed have restitution applied (will bounce). - float restitutionThreshold; - - /// The density, usually in kg/m^2. - float density; - - /// A sensor shape collects contact information but never generates a collision - /// response. - bool isSensor; - - /// Contact filtering data. - b2Filter filter; -}; - -/// This proxy is used internally to connect fixtures to the broad-phase. -struct B2_API b2FixtureProxy -{ - b2AABB aabb; - b2Fixture* fixture; - int32 childIndex; - int32 proxyId; -}; - -/// A fixture is used to attach a shape to a body for collision detection. A fixture -/// inherits its transform from its parent. Fixtures hold additional non-geometric data -/// such as friction, collision filters, etc. -/// Fixtures are created via b2Body::CreateFixture. -/// @warning you cannot reuse fixtures. -class B2_API b2Fixture -{ -public: - /// Get the type of the child shape. You can use this to down cast to the concrete shape. - /// @return the shape type. - b2Shape::Type GetType() const; - - /// Get the child shape. You can modify the child shape, however you should not change the - /// number of vertices because this will crash some collision caching mechanisms. - /// Manipulating the shape may lead to non-physical behavior. - b2Shape* GetShape(); - const b2Shape* GetShape() const; - - /// Set if this fixture is a sensor. - void SetSensor(bool sensor); - - /// Is this fixture a sensor (non-solid)? - /// @return the true if the shape is a sensor. - bool IsSensor() const; - - /// Set the contact filtering data. This will not update contacts until the next time - /// step when either parent body is active and awake. - /// This automatically calls Refilter. - void SetFilterData(const b2Filter& filter); - - /// Get the contact filtering data. - const b2Filter& GetFilterData() const; - - /// Call this if you want to establish collision that was previously disabled by b2ContactFilter::ShouldCollide. - void Refilter(); - - /// Get the parent body of this fixture. This is nullptr if the fixture is not attached. - /// @return the parent body. - b2Body* GetBody(); - const b2Body* GetBody() const; - - /// Get the next fixture in the parent body's fixture list. - /// @return the next shape. - b2Fixture* GetNext(); - const b2Fixture* GetNext() const; - - /// Get the user data that was assigned in the fixture definition. Use this to - /// store your application specific data. - b2FixtureUserData& GetUserData(); - const b2FixtureUserData& GetUserData() const; - - /// Test a point for containment in this fixture. - /// @param p a point in world coordinates. - bool TestPoint(const b2Vec2& p) const; - - /// Cast a ray against this shape. - /// @param output the ray-cast results. - /// @param input the ray-cast input parameters. - /// @param childIndex the child shape index (e.g. edge index) - bool RayCast(b2RayCastOutput* output, const b2RayCastInput& input, int32 childIndex) const; - - /// Get the mass data for this fixture. The mass data is based on the density and - /// the shape. The rotational inertia is about the shape's origin. This operation - /// may be expensive. - void GetMassData(b2MassData* massData) const; - - /// Set the density of this fixture. This will _not_ automatically adjust the mass - /// of the body. You must call b2Body::ResetMassData to update the body's mass. - void SetDensity(float density); - - /// Get the density of this fixture. - float GetDensity() const; - - /// Get the coefficient of friction. - float GetFriction() const; - - /// Set the coefficient of friction. This will _not_ change the friction of - /// existing contacts. - void SetFriction(float friction); - - /// Get the coefficient of restitution. - float GetRestitution() const; - - /// Set the coefficient of restitution. This will _not_ change the restitution of - /// existing contacts. - void SetRestitution(float restitution); - - /// Get the restitution velocity threshold. - float GetRestitutionThreshold() const; - - /// Set the restitution threshold. This will _not_ change the restitution threshold of - /// existing contacts. - void SetRestitutionThreshold(float threshold); - - /// Get the fixture's AABB. This AABB may be enlarge and/or stale. - /// If you need a more accurate AABB, compute it using the shape and - /// the body transform. - const b2AABB& GetAABB(int32 childIndex) const; - - /// Dump this fixture to the log file. - void Dump(int32 bodyIndex); - -protected: - - friend class b2Body; - friend class b2World; - friend class b2Contact; - friend class b2ContactManager; - - b2Fixture(); - - // We need separation create/destroy functions from the constructor/destructor because - // the destructor cannot access the allocator (no destructor arguments allowed by C++). - void Create(b2BlockAllocator* allocator, b2Body* body, const b2FixtureDef* def); - void Destroy(b2BlockAllocator* allocator); - - // These support body activation/deactivation. - void CreateProxies(b2BroadPhase* broadPhase, const b2Transform& xf); - void DestroyProxies(b2BroadPhase* broadPhase); - - void Synchronize(b2BroadPhase* broadPhase, const b2Transform& xf1, const b2Transform& xf2); - - float m_density; - - b2Fixture* m_next; - b2Body* m_body; - - b2Shape* m_shape; - - float m_friction; - float m_restitution; - float m_restitutionThreshold; - - b2FixtureProxy* m_proxies; - int32 m_proxyCount; - - b2Filter m_filter; - - bool m_isSensor; - - b2FixtureUserData m_userData; -}; - -inline b2Shape::Type b2Fixture::GetType() const -{ - return m_shape->GetType(); -} - -inline b2Shape* b2Fixture::GetShape() -{ - return m_shape; -} - -inline const b2Shape* b2Fixture::GetShape() const -{ - return m_shape; -} - -inline bool b2Fixture::IsSensor() const -{ - return m_isSensor; -} - -inline const b2Filter& b2Fixture::GetFilterData() const -{ - return m_filter; -} - -inline b2FixtureUserData& b2Fixture::GetUserData() -{ - return m_userData; -} - -inline const b2FixtureUserData& b2Fixture::GetUserData() const -{ - return m_userData; -} - -inline b2Body* b2Fixture::GetBody() -{ - return m_body; -} - -inline const b2Body* b2Fixture::GetBody() const -{ - return m_body; -} - -inline b2Fixture* b2Fixture::GetNext() -{ - return m_next; -} - -inline const b2Fixture* b2Fixture::GetNext() const -{ - return m_next; -} - -inline void b2Fixture::SetDensity(float density) -{ - b2Assert(b2IsValid(density) && density >= 0.0f); - m_density = density; -} - -inline float b2Fixture::GetDensity() const -{ - return m_density; -} - -inline float b2Fixture::GetFriction() const -{ - return m_friction; -} - -inline void b2Fixture::SetFriction(float friction) -{ - m_friction = friction; -} - -inline float b2Fixture::GetRestitution() const -{ - return m_restitution; -} - -inline void b2Fixture::SetRestitution(float restitution) -{ - m_restitution = restitution; -} - -inline float b2Fixture::GetRestitutionThreshold() const -{ - return m_restitutionThreshold; -} - -inline void b2Fixture::SetRestitutionThreshold(float threshold) -{ - m_restitutionThreshold = threshold; -} - -inline bool b2Fixture::TestPoint(const b2Vec2& p) const -{ - return m_shape->TestPoint(m_body->GetTransform(), p); -} - -inline bool b2Fixture::RayCast(b2RayCastOutput* output, const b2RayCastInput& input, int32 childIndex) const -{ - return m_shape->RayCast(output, input, m_body->GetTransform(), childIndex); -} - -inline void b2Fixture::GetMassData(b2MassData* massData) const -{ - m_shape->ComputeMass(massData, m_density); -} - -inline const b2AABB& b2Fixture::GetAABB(int32 childIndex) const -{ - b2Assert(0 <= childIndex && childIndex < m_proxyCount); - return m_proxies[childIndex].aabb; -} - -#endif diff --git a/3rdparty/box2d/include/box2d/b2_friction_joint.h b/3rdparty/box2d/include/box2d/b2_friction_joint.h deleted file mode 100644 index 99922c3a375f..000000000000 --- a/3rdparty/box2d/include/box2d/b2_friction_joint.h +++ /dev/null @@ -1,124 +0,0 @@ -// MIT License - -// Copyright (c) 2019 Erin Catto - -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: - -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. - -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -#ifndef B2_FRICTION_JOINT_H -#define B2_FRICTION_JOINT_H - -#include "b2_api.h" -#include "b2_joint.h" - -/// Friction joint definition. -struct B2_API b2FrictionJointDef : public b2JointDef -{ - b2FrictionJointDef() - { - type = e_frictionJoint; - localAnchorA.SetZero(); - localAnchorB.SetZero(); - maxForce = 0.0f; - maxTorque = 0.0f; - } - - /// Initialize the bodies, anchors, axis, and reference angle using the world - /// anchor and world axis. - void Initialize(b2Body* bodyA, b2Body* bodyB, const b2Vec2& anchor); - - /// The local anchor point relative to bodyA's origin. - b2Vec2 localAnchorA; - - /// The local anchor point relative to bodyB's origin. - b2Vec2 localAnchorB; - - /// The maximum friction force in N. - float maxForce; - - /// The maximum friction torque in N-m. - float maxTorque; -}; - -/// Friction joint. This is used for top-down friction. -/// It provides 2D translational friction and angular friction. -class B2_API b2FrictionJoint : public b2Joint -{ -public: - b2Vec2 GetAnchorA() const override; - b2Vec2 GetAnchorB() const override; - - b2Vec2 GetReactionForce(float inv_dt) const override; - float GetReactionTorque(float inv_dt) const override; - - /// The local anchor point relative to bodyA's origin. - const b2Vec2& GetLocalAnchorA() const { return m_localAnchorA; } - - /// The local anchor point relative to bodyB's origin. - const b2Vec2& GetLocalAnchorB() const { return m_localAnchorB; } - - /// Set the maximum friction force in N. - void SetMaxForce(float force); - - /// Get the maximum friction force in N. - float GetMaxForce() const; - - /// Set the maximum friction torque in N*m. - void SetMaxTorque(float torque); - - /// Get the maximum friction torque in N*m. - float GetMaxTorque() const; - - /// Dump joint to dmLog - void Dump() override; - -protected: - - friend class b2Joint; - - b2FrictionJoint(const b2FrictionJointDef* def); - - void InitVelocityConstraints(const b2SolverData& data) override; - void SolveVelocityConstraints(const b2SolverData& data) override; - bool SolvePositionConstraints(const b2SolverData& data) override; - - b2Vec2 m_localAnchorA; - b2Vec2 m_localAnchorB; - - // Solver shared - b2Vec2 m_linearImpulse; - float m_angularImpulse; - float m_maxForce; - float m_maxTorque; - - // Solver temp - int32 m_indexA; - int32 m_indexB; - b2Vec2 m_rA; - b2Vec2 m_rB; - b2Vec2 m_localCenterA; - b2Vec2 m_localCenterB; - float m_invMassA; - float m_invMassB; - float m_invIA; - float m_invIB; - b2Mat22 m_linearMass; - float m_angularMass; -}; - -#endif diff --git a/3rdparty/box2d/include/box2d/b2_gear_joint.h b/3rdparty/box2d/include/box2d/b2_gear_joint.h deleted file mode 100644 index a7e4fa5197d7..000000000000 --- a/3rdparty/box2d/include/box2d/b2_gear_joint.h +++ /dev/null @@ -1,131 +0,0 @@ -// MIT License - -// Copyright (c) 2019 Erin Catto - -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: - -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. - -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -#ifndef B2_GEAR_JOINT_H -#define B2_GEAR_JOINT_H - -#include "b2_joint.h" - -/// Gear joint definition. This definition requires two existing -/// revolute or prismatic joints (any combination will work). -/// @warning bodyB on the input joints must both be dynamic -struct B2_API b2GearJointDef : public b2JointDef -{ - b2GearJointDef() - { - type = e_gearJoint; - joint1 = nullptr; - joint2 = nullptr; - ratio = 1.0f; - } - - /// The first revolute/prismatic joint attached to the gear joint. - b2Joint* joint1; - - /// The second revolute/prismatic joint attached to the gear joint. - b2Joint* joint2; - - /// The gear ratio. - /// @see b2GearJoint for explanation. - float ratio; -}; - -/// A gear joint is used to connect two joints together. Either joint -/// can be a revolute or prismatic joint. You specify a gear ratio -/// to bind the motions together: -/// coordinate1 + ratio * coordinate2 = constant -/// The ratio can be negative or positive. If one joint is a revolute joint -/// and the other joint is a prismatic joint, then the ratio will have units -/// of length or units of 1/length. -/// @warning You have to manually destroy the gear joint if joint1 or joint2 -/// is destroyed. -class B2_API b2GearJoint : public b2Joint -{ -public: - b2Vec2 GetAnchorA() const override; - b2Vec2 GetAnchorB() const override; - - b2Vec2 GetReactionForce(float inv_dt) const override; - float GetReactionTorque(float inv_dt) const override; - - /// Get the first joint. - b2Joint* GetJoint1() { return m_joint1; } - - /// Get the second joint. - b2Joint* GetJoint2() { return m_joint2; } - - /// Set/Get the gear ratio. - void SetRatio(float ratio); - float GetRatio() const; - - /// Dump joint to dmLog - void Dump() override; - -protected: - - friend class b2Joint; - b2GearJoint(const b2GearJointDef* data); - - void InitVelocityConstraints(const b2SolverData& data) override; - void SolveVelocityConstraints(const b2SolverData& data) override; - bool SolvePositionConstraints(const b2SolverData& data) override; - - b2Joint* m_joint1; - b2Joint* m_joint2; - - b2JointType m_typeA; - b2JointType m_typeB; - - // Body A is connected to body C - // Body B is connected to body D - b2Body* m_bodyC; - b2Body* m_bodyD; - - // Solver shared - b2Vec2 m_localAnchorA; - b2Vec2 m_localAnchorB; - b2Vec2 m_localAnchorC; - b2Vec2 m_localAnchorD; - - b2Vec2 m_localAxisC; - b2Vec2 m_localAxisD; - - float m_referenceAngleA; - float m_referenceAngleB; - - float m_constant; - float m_ratio; - float m_tolerance; - - float m_impulse; - - // Solver temp - int32 m_indexA, m_indexB, m_indexC, m_indexD; - b2Vec2 m_lcA, m_lcB, m_lcC, m_lcD; - float m_mA, m_mB, m_mC, m_mD; - float m_iA, m_iB, m_iC, m_iD; - b2Vec2 m_JvAC, m_JvBD; - float m_JwA, m_JwB, m_JwC, m_JwD; - float m_mass; -}; - -#endif diff --git a/3rdparty/box2d/include/box2d/b2_growable_stack.h b/3rdparty/box2d/include/box2d/b2_growable_stack.h deleted file mode 100644 index ec42e5e9e2d7..000000000000 --- a/3rdparty/box2d/include/box2d/b2_growable_stack.h +++ /dev/null @@ -1,91 +0,0 @@ -// MIT License - -// Copyright (c) 2019 Erin Catto - -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: - -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. - -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -#ifndef B2_GROWABLE_STACK_H -#define B2_GROWABLE_STACK_H - -#include - -#include "b2_settings.h" - -/// This is a growable LIFO stack with an initial capacity of N. -/// If the stack size exceeds the initial capacity, the heap is used -/// to increase the size of the stack. -template -class b2GrowableStack -{ -public: - b2GrowableStack() - { - m_stack = m_array; - m_count = 0; - m_capacity = N; - } - - ~b2GrowableStack() - { - if (m_stack != m_array) - { - b2Free(m_stack); - m_stack = nullptr; - } - } - - void Push(const T& element) - { - if (m_count == m_capacity) - { - T* old = m_stack; - m_capacity *= 2; - m_stack = (T*)b2Alloc(m_capacity * sizeof(T)); - memcpy(m_stack, old, m_count * sizeof(T)); - if (old != m_array) - { - b2Free(old); - } - } - - m_stack[m_count] = element; - ++m_count; - } - - T Pop() - { - b2Assert(m_count > 0); - --m_count; - return m_stack[m_count]; - } - - int32 GetCount() - { - return m_count; - } - -private: - T* m_stack; - T m_array[N]; - int32 m_count; - int32 m_capacity; -}; - - -#endif diff --git a/3rdparty/box2d/include/box2d/b2_joint.h b/3rdparty/box2d/include/box2d/b2_joint.h deleted file mode 100644 index 586b13ad1866..000000000000 --- a/3rdparty/box2d/include/box2d/b2_joint.h +++ /dev/null @@ -1,233 +0,0 @@ -// MIT License - -// Copyright (c) 2019 Erin Catto - -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: - -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. - -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -#ifndef B2_JOINT_H -#define B2_JOINT_H - -#include "b2_api.h" -#include "b2_math.h" - -class b2Body; -class b2Draw; -class b2Joint; -struct b2SolverData; -class b2BlockAllocator; - -enum b2JointType -{ - e_unknownJoint, - e_revoluteJoint, - e_prismaticJoint, - e_distanceJoint, - e_pulleyJoint, - e_mouseJoint, - e_gearJoint, - e_wheelJoint, - e_weldJoint, - e_frictionJoint, - e_motorJoint -}; - -struct B2_API b2Jacobian -{ - b2Vec2 linear; - float angularA; - float angularB; -}; - -/// A joint edge is used to connect bodies and joints together -/// in a joint graph where each body is a node and each joint -/// is an edge. A joint edge belongs to a doubly linked list -/// maintained in each attached body. Each joint has two joint -/// nodes, one for each attached body. -struct B2_API b2JointEdge -{ - b2Body* other; ///< provides quick access to the other body attached. - b2Joint* joint; ///< the joint - b2JointEdge* prev; ///< the previous joint edge in the body's joint list - b2JointEdge* next; ///< the next joint edge in the body's joint list -}; - -/// Joint definitions are used to construct joints. -struct B2_API b2JointDef -{ - b2JointDef() - { - type = e_unknownJoint; - bodyA = nullptr; - bodyB = nullptr; - collideConnected = false; - } - - /// The joint type is set automatically for concrete joint types. - b2JointType type; - - /// Use this to attach application specific data to your joints. - b2JointUserData userData; - - /// The first attached body. - b2Body* bodyA; - - /// The second attached body. - b2Body* bodyB; - - /// Set this flag to true if the attached bodies should collide. - bool collideConnected; -}; - -/// Utility to compute linear stiffness values from frequency and damping ratio -B2_API void b2LinearStiffness(float& stiffness, float& damping, - float frequencyHertz, float dampingRatio, - const b2Body* bodyA, const b2Body* bodyB); - -/// Utility to compute rotational stiffness values frequency and damping ratio -B2_API void b2AngularStiffness(float& stiffness, float& damping, - float frequencyHertz, float dampingRatio, - const b2Body* bodyA, const b2Body* bodyB); - -/// The base joint class. Joints are used to constraint two bodies together in -/// various fashions. Some joints also feature limits and motors. -class B2_API b2Joint -{ -public: - - /// Get the type of the concrete joint. - b2JointType GetType() const; - - /// Get the first body attached to this joint. - b2Body* GetBodyA(); - - /// Get the second body attached to this joint. - b2Body* GetBodyB(); - - /// Get the anchor point on bodyA in world coordinates. - virtual b2Vec2 GetAnchorA() const = 0; - - /// Get the anchor point on bodyB in world coordinates. - virtual b2Vec2 GetAnchorB() const = 0; - - /// Get the reaction force on bodyB at the joint anchor in Newtons. - virtual b2Vec2 GetReactionForce(float inv_dt) const = 0; - - /// Get the reaction torque on bodyB in N*m. - virtual float GetReactionTorque(float inv_dt) const = 0; - - /// Get the next joint the world joint list. - b2Joint* GetNext(); - const b2Joint* GetNext() const; - - /// Get the user data pointer. - b2JointUserData& GetUserData(); - const b2JointUserData& GetUserData() const; - - /// Short-cut function to determine if either body is enabled. - bool IsEnabled() const; - - /// Get collide connected. - /// Note: modifying the collide connect flag won't work correctly because - /// the flag is only checked when fixture AABBs begin to overlap. - bool GetCollideConnected() const; - - /// Dump this joint to the log file. - virtual void Dump() { b2Dump("// Dump is not supported for this joint type.\n"); } - - /// Shift the origin for any points stored in world coordinates. - virtual void ShiftOrigin(const b2Vec2& newOrigin) { B2_NOT_USED(newOrigin); } - - /// Debug draw this joint - virtual void Draw(b2Draw* draw) const; - -protected: - friend class b2World; - friend class b2Body; - friend class b2Island; - friend class b2GearJoint; - - static b2Joint* Create(const b2JointDef* def, b2BlockAllocator* allocator); - static void Destroy(b2Joint* joint, b2BlockAllocator* allocator); - - b2Joint(const b2JointDef* def); - virtual ~b2Joint() {} - - virtual void InitVelocityConstraints(const b2SolverData& data) = 0; - virtual void SolveVelocityConstraints(const b2SolverData& data) = 0; - - // This returns true if the position errors are within tolerance. - virtual bool SolvePositionConstraints(const b2SolverData& data) = 0; - - b2JointType m_type; - b2Joint* m_prev; - b2Joint* m_next; - b2JointEdge m_edgeA; - b2JointEdge m_edgeB; - b2Body* m_bodyA; - b2Body* m_bodyB; - - int32 m_index; - - bool m_islandFlag; - bool m_collideConnected; - - b2JointUserData m_userData; -}; - -inline b2JointType b2Joint::GetType() const -{ - return m_type; -} - -inline b2Body* b2Joint::GetBodyA() -{ - return m_bodyA; -} - -inline b2Body* b2Joint::GetBodyB() -{ - return m_bodyB; -} - -inline b2Joint* b2Joint::GetNext() -{ - return m_next; -} - -inline const b2Joint* b2Joint::GetNext() const -{ - return m_next; -} - -inline b2JointUserData& b2Joint::GetUserData() -{ - return m_userData; -} - -inline const b2JointUserData& b2Joint::GetUserData() const -{ - return m_userData; -} - -inline bool b2Joint::GetCollideConnected() const -{ - return m_collideConnected; -} - -#endif diff --git a/3rdparty/box2d/include/box2d/b2_math.h b/3rdparty/box2d/include/box2d/b2_math.h deleted file mode 100644 index c98cf35dfcb2..000000000000 --- a/3rdparty/box2d/include/box2d/b2_math.h +++ /dev/null @@ -1,717 +0,0 @@ -// MIT License - -// Copyright (c) 2019 Erin Catto - -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: - -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. - -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -#ifndef B2_MATH_H -#define B2_MATH_H - -#include - -#include "b2_api.h" -#include "b2_settings.h" - -/// This function is used to ensure that a floating point number is not a NaN or infinity. -inline bool b2IsValid(float x) -{ - return isfinite(x); -} - -#define b2Sqrt(x) sqrtf(x) -#define b2Atan2(y, x) atan2f(y, x) - -/// A 2D column vector. -struct B2_API b2Vec2 -{ - /// Default constructor does nothing (for performance). - b2Vec2() = default; - - /// Construct using coordinates. - b2Vec2(float xIn, float yIn) : x(xIn), y(yIn) {} - - /// Set this vector to all zeros. - void SetZero() { x = 0.0f; y = 0.0f; } - - /// Set this vector to some specified coordinates. - void Set(float x_, float y_) { x = x_; y = y_; } - - /// Negate this vector. - b2Vec2 operator -() const { b2Vec2 v; v.Set(-x, -y); return v; } - - /// Read from and indexed element. - float operator () (int32 i) const - { - return (&x)[i]; - } - - /// Write to an indexed element. - float& operator () (int32 i) - { - return (&x)[i]; - } - - /// Add a vector to this vector. - void operator += (const b2Vec2& v) - { - x += v.x; y += v.y; - } - - /// Subtract a vector from this vector. - void operator -= (const b2Vec2& v) - { - x -= v.x; y -= v.y; - } - - /// Multiply this vector by a scalar. - void operator *= (float a) - { - x *= a; y *= a; - } - - /// Get the length of this vector (the norm). - float Length() const - { - return b2Sqrt(x * x + y * y); - } - - /// Get the length squared. For performance, use this instead of - /// b2Vec2::Length (if possible). - float LengthSquared() const - { - return x * x + y * y; - } - - /// Convert this vector into a unit vector. Returns the length. - float Normalize() - { - float length = Length(); - if (length < b2_epsilon) - { - return 0.0f; - } - float invLength = 1.0f / length; - x *= invLength; - y *= invLength; - - return length; - } - - /// Does this vector contain finite coordinates? - bool IsValid() const - { - return b2IsValid(x) && b2IsValid(y); - } - - /// Get the skew vector such that dot(skew_vec, other) == cross(vec, other) - b2Vec2 Skew() const - { - return b2Vec2(-y, x); - } - - float x, y; -}; - -/// A 2D column vector with 3 elements. -struct B2_API b2Vec3 -{ - /// Default constructor does nothing (for performance). - b2Vec3() = default; - - /// Construct using coordinates. - b2Vec3(float xIn, float yIn, float zIn) : x(xIn), y(yIn), z(zIn) {} - - /// Set this vector to all zeros. - void SetZero() { x = 0.0f; y = 0.0f; z = 0.0f; } - - /// Set this vector to some specified coordinates. - void Set(float x_, float y_, float z_) { x = x_; y = y_; z = z_; } - - /// Negate this vector. - b2Vec3 operator -() const { b2Vec3 v; v.Set(-x, -y, -z); return v; } - - /// Add a vector to this vector. - void operator += (const b2Vec3& v) - { - x += v.x; y += v.y; z += v.z; - } - - /// Subtract a vector from this vector. - void operator -= (const b2Vec3& v) - { - x -= v.x; y -= v.y; z -= v.z; - } - - /// Multiply this vector by a scalar. - void operator *= (float s) - { - x *= s; y *= s; z *= s; - } - - float x, y, z; -}; - -/// A 2-by-2 matrix. Stored in column-major order. -struct B2_API b2Mat22 -{ - /// The default constructor does nothing (for performance). - b2Mat22() = default; - - /// Construct this matrix using columns. - b2Mat22(const b2Vec2& c1, const b2Vec2& c2) - { - ex = c1; - ey = c2; - } - - /// Construct this matrix using scalars. - b2Mat22(float a11, float a12, float a21, float a22) - { - ex.x = a11; ex.y = a21; - ey.x = a12; ey.y = a22; - } - - /// Initialize this matrix using columns. - void Set(const b2Vec2& c1, const b2Vec2& c2) - { - ex = c1; - ey = c2; - } - - /// Set this to the identity matrix. - void SetIdentity() - { - ex.x = 1.0f; ey.x = 0.0f; - ex.y = 0.0f; ey.y = 1.0f; - } - - /// Set this matrix to all zeros. - void SetZero() - { - ex.x = 0.0f; ey.x = 0.0f; - ex.y = 0.0f; ey.y = 0.0f; - } - - b2Mat22 GetInverse() const - { - float a = ex.x, b = ey.x, c = ex.y, d = ey.y; - b2Mat22 B; - float det = a * d - b * c; - if (det != 0.0f) - { - det = 1.0f / det; - } - B.ex.x = det * d; B.ey.x = -det * b; - B.ex.y = -det * c; B.ey.y = det * a; - return B; - } - - /// Solve A * x = b, where b is a column vector. This is more efficient - /// than computing the inverse in one-shot cases. - b2Vec2 Solve(const b2Vec2& b) const - { - float a11 = ex.x, a12 = ey.x, a21 = ex.y, a22 = ey.y; - float det = a11 * a22 - a12 * a21; - if (det != 0.0f) - { - det = 1.0f / det; - } - b2Vec2 x; - x.x = det * (a22 * b.x - a12 * b.y); - x.y = det * (a11 * b.y - a21 * b.x); - return x; - } - - b2Vec2 ex, ey; -}; - -/// A 3-by-3 matrix. Stored in column-major order. -struct B2_API b2Mat33 -{ - /// The default constructor does nothing (for performance). - b2Mat33() = default; - - /// Construct this matrix using columns. - b2Mat33(const b2Vec3& c1, const b2Vec3& c2, const b2Vec3& c3) - { - ex = c1; - ey = c2; - ez = c3; - } - - /// Set this matrix to all zeros. - void SetZero() - { - ex.SetZero(); - ey.SetZero(); - ez.SetZero(); - } - - /// Solve A * x = b, where b is a column vector. This is more efficient - /// than computing the inverse in one-shot cases. - b2Vec3 Solve33(const b2Vec3& b) const; - - /// Solve A * x = b, where b is a column vector. This is more efficient - /// than computing the inverse in one-shot cases. Solve only the upper - /// 2-by-2 matrix equation. - b2Vec2 Solve22(const b2Vec2& b) const; - - /// Get the inverse of this matrix as a 2-by-2. - /// Returns the zero matrix if singular. - void GetInverse22(b2Mat33* M) const; - - /// Get the symmetric inverse of this matrix as a 3-by-3. - /// Returns the zero matrix if singular. - void GetSymInverse33(b2Mat33* M) const; - - b2Vec3 ex, ey, ez; -}; - -/// Rotation -struct B2_API b2Rot -{ - b2Rot() = default; - - /// Initialize from an angle in radians - explicit b2Rot(float angle) - { - /// TODO_ERIN optimize - s = sinf(angle); - c = cosf(angle); - } - - /// Set using an angle in radians. - void Set(float angle) - { - /// TODO_ERIN optimize - s = sinf(angle); - c = cosf(angle); - } - - /// Set to the identity rotation - void SetIdentity() - { - s = 0.0f; - c = 1.0f; - } - - /// Get the angle in radians - float GetAngle() const - { - return b2Atan2(s, c); - } - - /// Get the x-axis - b2Vec2 GetXAxis() const - { - return b2Vec2(c, s); - } - - /// Get the u-axis - b2Vec2 GetYAxis() const - { - return b2Vec2(-s, c); - } - - /// Sine and cosine - float s, c; -}; - -/// A transform contains translation and rotation. It is used to represent -/// the position and orientation of rigid frames. -struct B2_API b2Transform -{ - /// The default constructor does nothing. - b2Transform() = default; - - /// Initialize using a position vector and a rotation. - b2Transform(const b2Vec2& position, const b2Rot& rotation) : p(position), q(rotation) {} - - /// Set this to the identity transform. - void SetIdentity() - { - p.SetZero(); - q.SetIdentity(); - } - - /// Set this based on the position and angle. - void Set(const b2Vec2& position, float angle) - { - p = position; - q.Set(angle); - } - - b2Vec2 p; - b2Rot q; -}; - -/// This describes the motion of a body/shape for TOI computation. -/// Shapes are defined with respect to the body origin, which may -/// no coincide with the center of mass. However, to support dynamics -/// we must interpolate the center of mass position. -struct B2_API b2Sweep -{ - b2Sweep() = default; - - /// Get the interpolated transform at a specific time. - /// @param transform the output transform - /// @param beta is a factor in [0,1], where 0 indicates alpha0. - void GetTransform(b2Transform* transform, float beta) const; - - /// Advance the sweep forward, yielding a new initial state. - /// @param alpha the new initial time. - void Advance(float alpha); - - /// Normalize the angles. - void Normalize(); - - b2Vec2 localCenter; ///< local center of mass position - b2Vec2 c0, c; ///< center world positions - float a0, a; ///< world angles - - /// Fraction of the current time step in the range [0,1] - /// c0 and a0 are the positions at alpha0. - float alpha0; -}; - -/// Useful constant -extern B2_API const b2Vec2 b2Vec2_zero; - -/// Perform the dot product on two vectors. -inline float b2Dot(const b2Vec2& a, const b2Vec2& b) -{ - return a.x * b.x + a.y * b.y; -} - -/// Perform the cross product on two vectors. In 2D this produces a scalar. -inline float b2Cross(const b2Vec2& a, const b2Vec2& b) -{ - return a.x * b.y - a.y * b.x; -} - -/// Perform the cross product on a vector and a scalar. In 2D this produces -/// a vector. -inline b2Vec2 b2Cross(const b2Vec2& a, float s) -{ - return b2Vec2(s * a.y, -s * a.x); -} - -/// Perform the cross product on a scalar and a vector. In 2D this produces -/// a vector. -inline b2Vec2 b2Cross(float s, const b2Vec2& a) -{ - return b2Vec2(-s * a.y, s * a.x); -} - -/// Multiply a matrix times a vector. If a rotation matrix is provided, -/// then this transforms the vector from one frame to another. -inline b2Vec2 b2Mul(const b2Mat22& A, const b2Vec2& v) -{ - return b2Vec2(A.ex.x * v.x + A.ey.x * v.y, A.ex.y * v.x + A.ey.y * v.y); -} - -/// Multiply a matrix transpose times a vector. If a rotation matrix is provided, -/// then this transforms the vector from one frame to another (inverse transform). -inline b2Vec2 b2MulT(const b2Mat22& A, const b2Vec2& v) -{ - return b2Vec2(b2Dot(v, A.ex), b2Dot(v, A.ey)); -} - -/// Add two vectors component-wise. -inline b2Vec2 operator + (const b2Vec2& a, const b2Vec2& b) -{ - return b2Vec2(a.x + b.x, a.y + b.y); -} - -/// Subtract two vectors component-wise. -inline b2Vec2 operator - (const b2Vec2& a, const b2Vec2& b) -{ - return b2Vec2(a.x - b.x, a.y - b.y); -} - -inline b2Vec2 operator * (float s, const b2Vec2& a) -{ - return b2Vec2(s * a.x, s * a.y); -} - -inline bool operator == (const b2Vec2& a, const b2Vec2& b) -{ - return a.x == b.x && a.y == b.y; -} - -inline bool operator != (const b2Vec2& a, const b2Vec2& b) -{ - return a.x != b.x || a.y != b.y; -} - -inline float b2Distance(const b2Vec2& a, const b2Vec2& b) -{ - b2Vec2 c = a - b; - return c.Length(); -} - -inline float b2DistanceSquared(const b2Vec2& a, const b2Vec2& b) -{ - b2Vec2 c = a - b; - return b2Dot(c, c); -} - -inline b2Vec3 operator * (float s, const b2Vec3& a) -{ - return b2Vec3(s * a.x, s * a.y, s * a.z); -} - -/// Add two vectors component-wise. -inline b2Vec3 operator + (const b2Vec3& a, const b2Vec3& b) -{ - return b2Vec3(a.x + b.x, a.y + b.y, a.z + b.z); -} - -/// Subtract two vectors component-wise. -inline b2Vec3 operator - (const b2Vec3& a, const b2Vec3& b) -{ - return b2Vec3(a.x - b.x, a.y - b.y, a.z - b.z); -} - -/// Perform the dot product on two vectors. -inline float b2Dot(const b2Vec3& a, const b2Vec3& b) -{ - return a.x * b.x + a.y * b.y + a.z * b.z; -} - -/// Perform the cross product on two vectors. -inline b2Vec3 b2Cross(const b2Vec3& a, const b2Vec3& b) -{ - return b2Vec3(a.y * b.z - a.z * b.y, a.z * b.x - a.x * b.z, a.x * b.y - a.y * b.x); -} - -inline b2Mat22 operator + (const b2Mat22& A, const b2Mat22& B) -{ - return b2Mat22(A.ex + B.ex, A.ey + B.ey); -} - -// A * B -inline b2Mat22 b2Mul(const b2Mat22& A, const b2Mat22& B) -{ - return b2Mat22(b2Mul(A, B.ex), b2Mul(A, B.ey)); -} - -// A^T * B -inline b2Mat22 b2MulT(const b2Mat22& A, const b2Mat22& B) -{ - b2Vec2 c1(b2Dot(A.ex, B.ex), b2Dot(A.ey, B.ex)); - b2Vec2 c2(b2Dot(A.ex, B.ey), b2Dot(A.ey, B.ey)); - return b2Mat22(c1, c2); -} - -/// Multiply a matrix times a vector. -inline b2Vec3 b2Mul(const b2Mat33& A, const b2Vec3& v) -{ - return v.x * A.ex + v.y * A.ey + v.z * A.ez; -} - -/// Multiply a matrix times a vector. -inline b2Vec2 b2Mul22(const b2Mat33& A, const b2Vec2& v) -{ - return b2Vec2(A.ex.x * v.x + A.ey.x * v.y, A.ex.y * v.x + A.ey.y * v.y); -} - -/// Multiply two rotations: q * r -inline b2Rot b2Mul(const b2Rot& q, const b2Rot& r) -{ - // [qc -qs] * [rc -rs] = [qc*rc-qs*rs -qc*rs-qs*rc] - // [qs qc] [rs rc] [qs*rc+qc*rs -qs*rs+qc*rc] - // s = qs * rc + qc * rs - // c = qc * rc - qs * rs - b2Rot qr; - qr.s = q.s * r.c + q.c * r.s; - qr.c = q.c * r.c - q.s * r.s; - return qr; -} - -/// Transpose multiply two rotations: qT * r -inline b2Rot b2MulT(const b2Rot& q, const b2Rot& r) -{ - // [ qc qs] * [rc -rs] = [qc*rc+qs*rs -qc*rs+qs*rc] - // [-qs qc] [rs rc] [-qs*rc+qc*rs qs*rs+qc*rc] - // s = qc * rs - qs * rc - // c = qc * rc + qs * rs - b2Rot qr; - qr.s = q.c * r.s - q.s * r.c; - qr.c = q.c * r.c + q.s * r.s; - return qr; -} - -/// Rotate a vector -inline b2Vec2 b2Mul(const b2Rot& q, const b2Vec2& v) -{ - return b2Vec2(q.c * v.x - q.s * v.y, q.s * v.x + q.c * v.y); -} - -/// Inverse rotate a vector -inline b2Vec2 b2MulT(const b2Rot& q, const b2Vec2& v) -{ - return b2Vec2(q.c * v.x + q.s * v.y, -q.s * v.x + q.c * v.y); -} - -inline b2Vec2 b2Mul(const b2Transform& T, const b2Vec2& v) -{ - float x = (T.q.c * v.x - T.q.s * v.y) + T.p.x; - float y = (T.q.s * v.x + T.q.c * v.y) + T.p.y; - - return b2Vec2(x, y); -} - -inline b2Vec2 b2MulT(const b2Transform& T, const b2Vec2& v) -{ - float px = v.x - T.p.x; - float py = v.y - T.p.y; - float x = (T.q.c * px + T.q.s * py); - float y = (-T.q.s * px + T.q.c * py); - - return b2Vec2(x, y); -} - -// v2 = A.q.Rot(B.q.Rot(v1) + B.p) + A.p -// = (A.q * B.q).Rot(v1) + A.q.Rot(B.p) + A.p -inline b2Transform b2Mul(const b2Transform& A, const b2Transform& B) -{ - b2Transform C; - C.q = b2Mul(A.q, B.q); - C.p = b2Mul(A.q, B.p) + A.p; - return C; -} - -// v2 = A.q' * (B.q * v1 + B.p - A.p) -// = A.q' * B.q * v1 + A.q' * (B.p - A.p) -inline b2Transform b2MulT(const b2Transform& A, const b2Transform& B) -{ - b2Transform C; - C.q = b2MulT(A.q, B.q); - C.p = b2MulT(A.q, B.p - A.p); - return C; -} - -template -inline T b2Abs(T a) -{ - return a > T(0) ? a : -a; -} - -inline b2Vec2 b2Abs(const b2Vec2& a) -{ - return b2Vec2(b2Abs(a.x), b2Abs(a.y)); -} - -inline b2Mat22 b2Abs(const b2Mat22& A) -{ - return b2Mat22(b2Abs(A.ex), b2Abs(A.ey)); -} - -template -inline T b2Min(T a, T b) -{ - return a < b ? a : b; -} - -inline b2Vec2 b2Min(const b2Vec2& a, const b2Vec2& b) -{ - return b2Vec2(b2Min(a.x, b.x), b2Min(a.y, b.y)); -} - -template -inline T b2Max(T a, T b) -{ - return a > b ? a : b; -} - -inline b2Vec2 b2Max(const b2Vec2& a, const b2Vec2& b) -{ - return b2Vec2(b2Max(a.x, b.x), b2Max(a.y, b.y)); -} - -template -inline T b2Clamp(T a, T low, T high) -{ - return b2Max(low, b2Min(a, high)); -} - -inline b2Vec2 b2Clamp(const b2Vec2& a, const b2Vec2& low, const b2Vec2& high) -{ - return b2Max(low, b2Min(a, high)); -} - -template inline void b2Swap(T& a, T& b) -{ - T tmp = a; - a = b; - b = tmp; -} - -/// "Next Largest Power of 2 -/// Given a binary integer value x, the next largest power of 2 can be computed by a SWAR algorithm -/// that recursively "folds" the upper bits into the lower bits. This process yields a bit vector with -/// the same most significant 1 as x, but all 1's below it. Adding 1 to that value yields the next -/// largest power of 2. For a 32-bit value:" -inline uint32 b2NextPowerOfTwo(uint32 x) -{ - x |= (x >> 1); - x |= (x >> 2); - x |= (x >> 4); - x |= (x >> 8); - x |= (x >> 16); - return x + 1; -} - -inline bool b2IsPowerOfTwo(uint32 x) -{ - bool result = x > 0 && (x & (x - 1)) == 0; - return result; -} - -// https://fgiesen.wordpress.com/2012/08/15/linear-interpolation-past-present-and-future/ -inline void b2Sweep::GetTransform(b2Transform* xf, float beta) const -{ - xf->p = (1.0f - beta) * c0 + beta * c; - float angle = (1.0f - beta) * a0 + beta * a; - xf->q.Set(angle); - - // Shift to origin - xf->p -= b2Mul(xf->q, localCenter); -} - -inline void b2Sweep::Advance(float alpha) -{ - b2Assert(alpha0 < 1.0f); - float beta = (alpha - alpha0) / (1.0f - alpha0); - c0 += beta * (c - c0); - a0 += beta * (a - a0); - alpha0 = alpha; -} - -/// Normalize an angle in radians to be between -pi and pi -inline void b2Sweep::Normalize() -{ - float twoPi = 2.0f * b2_pi; - float d = twoPi * floorf(a0 / twoPi); - a0 -= d; - a -= d; -} - -#endif diff --git a/3rdparty/box2d/include/box2d/b2_motor_joint.h b/3rdparty/box2d/include/box2d/b2_motor_joint.h deleted file mode 100644 index c88115f20b48..000000000000 --- a/3rdparty/box2d/include/box2d/b2_motor_joint.h +++ /dev/null @@ -1,138 +0,0 @@ -// MIT License - -// Copyright (c) 2019 Erin Catto - -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: - -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. - -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -#ifndef B2_MOTOR_JOINT_H -#define B2_MOTOR_JOINT_H - -#include "b2_api.h" -#include "b2_joint.h" - -/// Motor joint definition. -struct B2_API b2MotorJointDef : public b2JointDef -{ - b2MotorJointDef() - { - type = e_motorJoint; - linearOffset.SetZero(); - angularOffset = 0.0f; - maxForce = 1.0f; - maxTorque = 1.0f; - correctionFactor = 0.3f; - } - - /// Initialize the bodies and offsets using the current transforms. - void Initialize(b2Body* bodyA, b2Body* bodyB); - - /// Position of bodyB minus the position of bodyA, in bodyA's frame, in meters. - b2Vec2 linearOffset; - - /// The bodyB angle minus bodyA angle in radians. - float angularOffset; - - /// The maximum motor force in N. - float maxForce; - - /// The maximum motor torque in N-m. - float maxTorque; - - /// Position correction factor in the range [0,1]. - float correctionFactor; -}; - -/// A motor joint is used to control the relative motion -/// between two bodies. A typical usage is to control the movement -/// of a dynamic body with respect to the ground. -class B2_API b2MotorJoint : public b2Joint -{ -public: - b2Vec2 GetAnchorA() const override; - b2Vec2 GetAnchorB() const override; - - b2Vec2 GetReactionForce(float inv_dt) const override; - float GetReactionTorque(float inv_dt) const override; - - /// Set/get the target linear offset, in frame A, in meters. - void SetLinearOffset(const b2Vec2& linearOffset); - const b2Vec2& GetLinearOffset() const; - - /// Set/get the target angular offset, in radians. - void SetAngularOffset(float angularOffset); - float GetAngularOffset() const; - - /// Set the maximum friction force in N. - void SetMaxForce(float force); - - /// Get the maximum friction force in N. - float GetMaxForce() const; - - /// Set the maximum friction torque in N*m. - void SetMaxTorque(float torque); - - /// Get the maximum friction torque in N*m. - float GetMaxTorque() const; - - /// Set the position correction factor in the range [0,1]. - void SetCorrectionFactor(float factor); - - /// Get the position correction factor in the range [0,1]. - float GetCorrectionFactor() const; - - /// Dump to b2Log - void Dump() override; - -protected: - - friend class b2Joint; - - b2MotorJoint(const b2MotorJointDef* def); - - void InitVelocityConstraints(const b2SolverData& data) override; - void SolveVelocityConstraints(const b2SolverData& data) override; - bool SolvePositionConstraints(const b2SolverData& data) override; - - // Solver shared - b2Vec2 m_linearOffset; - float m_angularOffset; - b2Vec2 m_linearImpulse; - float m_angularImpulse; - float m_maxForce; - float m_maxTorque; - float m_correctionFactor; - - // Solver temp - int32 m_indexA; - int32 m_indexB; - b2Vec2 m_rA; - b2Vec2 m_rB; - b2Vec2 m_localCenterA; - b2Vec2 m_localCenterB; - b2Vec2 m_linearError; - float m_angularError; - float m_invMassA; - float m_invMassB; - float m_invIA; - float m_invIB; - b2Mat22 m_linearMass; - float m_angularMass; -}; - -#endif diff --git a/3rdparty/box2d/include/box2d/b2_mouse_joint.h b/3rdparty/box2d/include/box2d/b2_mouse_joint.h deleted file mode 100644 index fcbc56a70ce3..000000000000 --- a/3rdparty/box2d/include/box2d/b2_mouse_joint.h +++ /dev/null @@ -1,134 +0,0 @@ -// MIT License - -// Copyright (c) 2019 Erin Catto - -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: - -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. - -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -#ifndef B2_MOUSE_JOINT_H -#define B2_MOUSE_JOINT_H - -#include "b2_api.h" -#include "b2_joint.h" - -/// Mouse joint definition. This requires a world target point, -/// tuning parameters, and the time step. -struct B2_API b2MouseJointDef : public b2JointDef -{ - b2MouseJointDef() - { - type = e_mouseJoint; - target.Set(0.0f, 0.0f); - maxForce = 0.0f; - stiffness = 0.0f; - damping = 0.0f; - } - - /// The initial world target point. This is assumed - /// to coincide with the body anchor initially. - b2Vec2 target; - - /// The maximum constraint force that can be exerted - /// to move the candidate body. Usually you will express - /// as some multiple of the weight (multiplier * mass * gravity). - float maxForce; - - /// The linear stiffness in N/m - float stiffness; - - /// The linear damping in N*s/m - float damping; -}; - -/// A mouse joint is used to make a point on a body track a -/// specified world point. This a soft constraint with a maximum -/// force. This allows the constraint to stretch and without -/// applying huge forces. -/// NOTE: this joint is not documented in the manual because it was -/// developed to be used in the testbed. If you want to learn how to -/// use the mouse joint, look at the testbed. -class B2_API b2MouseJoint : public b2Joint -{ -public: - - /// Implements b2Joint. - b2Vec2 GetAnchorA() const override; - - /// Implements b2Joint. - b2Vec2 GetAnchorB() const override; - - /// Implements b2Joint. - b2Vec2 GetReactionForce(float inv_dt) const override; - - /// Implements b2Joint. - float GetReactionTorque(float inv_dt) const override; - - /// Use this to update the target point. - void SetTarget(const b2Vec2& target); - const b2Vec2& GetTarget() const; - - /// Set/get the maximum force in Newtons. - void SetMaxForce(float force); - float GetMaxForce() const; - - /// Set/get the linear stiffness in N/m - void SetStiffness(float stiffness) { m_stiffness = stiffness; } - float GetStiffness() const { return m_stiffness; } - - /// Set/get linear damping in N*s/m - void SetDamping(float damping) { m_damping = damping; } - float GetDamping() const { return m_damping; } - - /// The mouse joint does not support dumping. - void Dump() override { b2Log("Mouse joint dumping is not supported.\n"); } - - /// Implement b2Joint::ShiftOrigin - void ShiftOrigin(const b2Vec2& newOrigin) override; - -protected: - friend class b2Joint; - - b2MouseJoint(const b2MouseJointDef* def); - - void InitVelocityConstraints(const b2SolverData& data) override; - void SolveVelocityConstraints(const b2SolverData& data) override; - bool SolvePositionConstraints(const b2SolverData& data) override; - - b2Vec2 m_localAnchorB; - b2Vec2 m_targetA; - float m_stiffness; - float m_damping; - float m_beta; - - // Solver shared - b2Vec2 m_impulse; - float m_maxForce; - float m_gamma; - - // Solver temp - int32 m_indexA; - int32 m_indexB; - b2Vec2 m_rB; - b2Vec2 m_localCenterB; - float m_invMassB; - float m_invIB; - b2Mat22 m_mass; - b2Vec2 m_C; -}; - -#endif diff --git a/3rdparty/box2d/include/box2d/b2_polygon_shape.h b/3rdparty/box2d/include/box2d/b2_polygon_shape.h deleted file mode 100644 index 8a208b72b66d..000000000000 --- a/3rdparty/box2d/include/box2d/b2_polygon_shape.h +++ /dev/null @@ -1,93 +0,0 @@ -// MIT License - -// Copyright (c) 2019 Erin Catto - -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: - -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. - -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. -#ifndef B2_POLYGON_SHAPE_H -#define B2_POLYGON_SHAPE_H - -#include "b2_api.h" -#include "b2_shape.h" - -struct b2Hull; - -/// A solid convex polygon. It is assumed that the interior of the polygon is to -/// the left of each edge. -/// Polygons have a maximum number of vertices equal to b2_maxPolygonVertices. -/// In most cases you should not need many vertices for a convex polygon. -class B2_API b2PolygonShape : public b2Shape -{ -public: - b2PolygonShape(); - - /// Implement b2Shape. - b2Shape* Clone(b2BlockAllocator* allocator) const override; - - /// @see b2Shape::GetChildCount - int32 GetChildCount() const override; - - /// Create a convex hull from the given array of local points. - /// The count must be in the range [3, b2_maxPolygonVertices]. - /// @warning the points may be re-ordered, even if they form a convex polygon - /// @warning if this fails then the polygon is invalid - /// @returns true if valid - bool Set(const b2Vec2* points, int32 count); - - /// Create a polygon from a given convex hull (see b2ComputeHull). - /// @warning the hull must be valid or this will crash or have unexpected behavior - void Set(const b2Hull& hull); - - /// Build vertices to represent an axis-aligned box centered on the local origin. - /// @param hx the half-width. - /// @param hy the half-height. - void SetAsBox(float hx, float hy); - - /// Build vertices to represent an oriented box. - /// @param hx the half-width. - /// @param hy the half-height. - /// @param center the center of the box in local coordinates. - /// @param angle the rotation of the box in local coordinates. - void SetAsBox(float hx, float hy, const b2Vec2& center, float angle); - - /// @see b2Shape::TestPoint - bool TestPoint(const b2Transform& transform, const b2Vec2& p) const override; - - /// Implement b2Shape. - /// @note because the polygon is solid, rays that start inside do not hit because the normal is - /// not defined. - bool RayCast(b2RayCastOutput* output, const b2RayCastInput& input, - const b2Transform& transform, int32 childIndex) const override; - - /// @see b2Shape::ComputeAABB - void ComputeAABB(b2AABB* aabb, const b2Transform& transform, int32 childIndex) const override; - - /// @see b2Shape::ComputeMass - void ComputeMass(b2MassData* massData, float density) const override; - - /// Validate convexity. This is a very time consuming operation. - /// @returns true if valid - bool Validate() const; - - b2Vec2 m_centroid; - b2Vec2 m_vertices[b2_maxPolygonVertices]; - b2Vec2 m_normals[b2_maxPolygonVertices]; - int32 m_count; -}; - -#endif diff --git a/3rdparty/box2d/include/box2d/b2_prismatic_joint.h b/3rdparty/box2d/include/box2d/b2_prismatic_joint.h deleted file mode 100644 index 9d12d2126b3d..000000000000 --- a/3rdparty/box2d/include/box2d/b2_prismatic_joint.h +++ /dev/null @@ -1,205 +0,0 @@ -// MIT License - -// Copyright (c) 2019 Erin Catto - -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: - -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. - -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -#ifndef B2_PRISMATIC_JOINT_H -#define B2_PRISMATIC_JOINT_H - -#include "b2_api.h" -#include "b2_joint.h" - -/// Prismatic joint definition. This requires defining a line of -/// motion using an axis and an anchor point. The definition uses local -/// anchor points and a local axis so that the initial configuration -/// can violate the constraint slightly. The joint translation is zero -/// when the local anchor points coincide in world space. Using local -/// anchors and a local axis helps when saving and loading a game. -struct B2_API b2PrismaticJointDef : public b2JointDef -{ - b2PrismaticJointDef() - { - type = e_prismaticJoint; - localAnchorA.SetZero(); - localAnchorB.SetZero(); - localAxisA.Set(1.0f, 0.0f); - referenceAngle = 0.0f; - enableLimit = false; - lowerTranslation = 0.0f; - upperTranslation = 0.0f; - enableMotor = false; - maxMotorForce = 0.0f; - motorSpeed = 0.0f; - } - - /// Initialize the bodies, anchors, axis, and reference angle using the world - /// anchor and unit world axis. - void Initialize(b2Body* bodyA, b2Body* bodyB, const b2Vec2& anchor, const b2Vec2& axis); - - /// The local anchor point relative to bodyA's origin. - b2Vec2 localAnchorA; - - /// The local anchor point relative to bodyB's origin. - b2Vec2 localAnchorB; - - /// The local translation unit axis in bodyA. - b2Vec2 localAxisA; - - /// The constrained angle between the bodies: bodyB_angle - bodyA_angle. - float referenceAngle; - - /// Enable/disable the joint limit. - bool enableLimit; - - /// The lower translation limit, usually in meters. - float lowerTranslation; - - /// The upper translation limit, usually in meters. - float upperTranslation; - - /// Enable/disable the joint motor. - bool enableMotor; - - /// The maximum motor torque, usually in N-m. - float maxMotorForce; - - /// The desired motor speed in radians per second. - float motorSpeed; -}; - -/// A prismatic joint. This joint provides one degree of freedom: translation -/// along an axis fixed in bodyA. Relative rotation is prevented. You can -/// use a joint limit to restrict the range of motion and a joint motor to -/// drive the motion or to model joint friction. -class B2_API b2PrismaticJoint : public b2Joint -{ -public: - b2Vec2 GetAnchorA() const override; - b2Vec2 GetAnchorB() const override; - - b2Vec2 GetReactionForce(float inv_dt) const override; - float GetReactionTorque(float inv_dt) const override; - - /// The local anchor point relative to bodyA's origin. - const b2Vec2& GetLocalAnchorA() const { return m_localAnchorA; } - - /// The local anchor point relative to bodyB's origin. - const b2Vec2& GetLocalAnchorB() const { return m_localAnchorB; } - - /// The local joint axis relative to bodyA. - const b2Vec2& GetLocalAxisA() const { return m_localXAxisA; } - - /// Get the reference angle. - float GetReferenceAngle() const { return m_referenceAngle; } - - /// Get the current joint translation, usually in meters. - float GetJointTranslation() const; - - /// Get the current joint translation speed, usually in meters per second. - float GetJointSpeed() const; - - /// Is the joint limit enabled? - bool IsLimitEnabled() const; - - /// Enable/disable the joint limit. - void EnableLimit(bool flag); - - /// Get the lower joint limit, usually in meters. - float GetLowerLimit() const; - - /// Get the upper joint limit, usually in meters. - float GetUpperLimit() const; - - /// Set the joint limits, usually in meters. - void SetLimits(float lower, float upper); - - /// Is the joint motor enabled? - bool IsMotorEnabled() const; - - /// Enable/disable the joint motor. - void EnableMotor(bool flag); - - /// Set the motor speed, usually in meters per second. - void SetMotorSpeed(float speed); - - /// Get the motor speed, usually in meters per second. - float GetMotorSpeed() const; - - /// Set the maximum motor force, usually in N. - void SetMaxMotorForce(float force); - float GetMaxMotorForce() const { return m_maxMotorForce; } - - /// Get the current motor force given the inverse time step, usually in N. - float GetMotorForce(float inv_dt) const; - - /// Dump to b2Log - void Dump() override; - - /// - void Draw(b2Draw* draw) const override; - -protected: - friend class b2Joint; - friend class b2GearJoint; - b2PrismaticJoint(const b2PrismaticJointDef* def); - - void InitVelocityConstraints(const b2SolverData& data) override; - void SolveVelocityConstraints(const b2SolverData& data) override; - bool SolvePositionConstraints(const b2SolverData& data) override; - - b2Vec2 m_localAnchorA; - b2Vec2 m_localAnchorB; - b2Vec2 m_localXAxisA; - b2Vec2 m_localYAxisA; - float m_referenceAngle; - b2Vec2 m_impulse; - float m_motorImpulse; - float m_lowerImpulse; - float m_upperImpulse; - float m_lowerTranslation; - float m_upperTranslation; - float m_maxMotorForce; - float m_motorSpeed; - bool m_enableLimit; - bool m_enableMotor; - - // Solver temp - int32 m_indexA; - int32 m_indexB; - b2Vec2 m_localCenterA; - b2Vec2 m_localCenterB; - float m_invMassA; - float m_invMassB; - float m_invIA; - float m_invIB; - b2Vec2 m_axis, m_perp; - float m_s1, m_s2; - float m_a1, m_a2; - b2Mat22 m_K; - float m_translation; - float m_axialMass; -}; - -inline float b2PrismaticJoint::GetMotorSpeed() const -{ - return m_motorSpeed; -} - -#endif diff --git a/3rdparty/box2d/include/box2d/b2_pulley_joint.h b/3rdparty/box2d/include/box2d/b2_pulley_joint.h deleted file mode 100644 index 6b1445641045..000000000000 --- a/3rdparty/box2d/include/box2d/b2_pulley_joint.h +++ /dev/null @@ -1,157 +0,0 @@ -// MIT License - -// Copyright (c) 2019 Erin Catto - -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: - -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. - -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -#ifndef B2_PULLEY_JOINT_H -#define B2_PULLEY_JOINT_H - -#include "b2_api.h" -#include "b2_joint.h" - -const float b2_minPulleyLength = 2.0f; - -/// Pulley joint definition. This requires two ground anchors, -/// two dynamic body anchor points, and a pulley ratio. -struct B2_API b2PulleyJointDef : public b2JointDef -{ - b2PulleyJointDef() - { - type = e_pulleyJoint; - groundAnchorA.Set(-1.0f, 1.0f); - groundAnchorB.Set(1.0f, 1.0f); - localAnchorA.Set(-1.0f, 0.0f); - localAnchorB.Set(1.0f, 0.0f); - lengthA = 0.0f; - lengthB = 0.0f; - ratio = 1.0f; - collideConnected = true; - } - - /// Initialize the bodies, anchors, lengths, max lengths, and ratio using the world anchors. - void Initialize(b2Body* bodyA, b2Body* bodyB, - const b2Vec2& groundAnchorA, const b2Vec2& groundAnchorB, - const b2Vec2& anchorA, const b2Vec2& anchorB, - float ratio); - - /// The first ground anchor in world coordinates. This point never moves. - b2Vec2 groundAnchorA; - - /// The second ground anchor in world coordinates. This point never moves. - b2Vec2 groundAnchorB; - - /// The local anchor point relative to bodyA's origin. - b2Vec2 localAnchorA; - - /// The local anchor point relative to bodyB's origin. - b2Vec2 localAnchorB; - - /// The a reference length for the segment attached to bodyA. - float lengthA; - - /// The a reference length for the segment attached to bodyB. - float lengthB; - - /// The pulley ratio, used to simulate a block-and-tackle. - float ratio; -}; - -/// The pulley joint is connected to two bodies and two fixed ground points. -/// The pulley supports a ratio such that: -/// length1 + ratio * length2 <= constant -/// Yes, the force transmitted is scaled by the ratio. -/// Warning: the pulley joint can get a bit squirrelly by itself. They often -/// work better when combined with prismatic joints. You should also cover the -/// the anchor points with static shapes to prevent one side from going to -/// zero length. -class B2_API b2PulleyJoint : public b2Joint -{ -public: - b2Vec2 GetAnchorA() const override; - b2Vec2 GetAnchorB() const override; - - b2Vec2 GetReactionForce(float inv_dt) const override; - float GetReactionTorque(float inv_dt) const override; - - /// Get the first ground anchor. - b2Vec2 GetGroundAnchorA() const; - - /// Get the second ground anchor. - b2Vec2 GetGroundAnchorB() const; - - /// Get the current length of the segment attached to bodyA. - float GetLengthA() const; - - /// Get the current length of the segment attached to bodyB. - float GetLengthB() const; - - /// Get the pulley ratio. - float GetRatio() const; - - /// Get the current length of the segment attached to bodyA. - float GetCurrentLengthA() const; - - /// Get the current length of the segment attached to bodyB. - float GetCurrentLengthB() const; - - /// Dump joint to dmLog - void Dump() override; - - /// Implement b2Joint::ShiftOrigin - void ShiftOrigin(const b2Vec2& newOrigin) override; - -protected: - - friend class b2Joint; - b2PulleyJoint(const b2PulleyJointDef* data); - - void InitVelocityConstraints(const b2SolverData& data) override; - void SolveVelocityConstraints(const b2SolverData& data) override; - bool SolvePositionConstraints(const b2SolverData& data) override; - - b2Vec2 m_groundAnchorA; - b2Vec2 m_groundAnchorB; - float m_lengthA; - float m_lengthB; - - // Solver shared - b2Vec2 m_localAnchorA; - b2Vec2 m_localAnchorB; - float m_constant; - float m_ratio; - float m_impulse; - - // Solver temp - int32 m_indexA; - int32 m_indexB; - b2Vec2 m_uA; - b2Vec2 m_uB; - b2Vec2 m_rA; - b2Vec2 m_rB; - b2Vec2 m_localCenterA; - b2Vec2 m_localCenterB; - float m_invMassA; - float m_invMassB; - float m_invIA; - float m_invIB; - float m_mass; -}; - -#endif diff --git a/3rdparty/box2d/include/box2d/b2_revolute_joint.h b/3rdparty/box2d/include/box2d/b2_revolute_joint.h deleted file mode 100644 index 36ad5314efac..000000000000 --- a/3rdparty/box2d/include/box2d/b2_revolute_joint.h +++ /dev/null @@ -1,211 +0,0 @@ -// MIT License - -// Copyright (c) 2019 Erin Catto - -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: - -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. - -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -#ifndef B2_REVOLUTE_JOINT_H -#define B2_REVOLUTE_JOINT_H - -#include "b2_api.h" -#include "b2_joint.h" - -/// Revolute joint definition. This requires defining an anchor point where the -/// bodies are joined. The definition uses local anchor points so that the -/// initial configuration can violate the constraint slightly. You also need to -/// specify the initial relative angle for joint limits. This helps when saving -/// and loading a game. -/// The local anchor points are measured from the body's origin -/// rather than the center of mass because: -/// 1. you might not know where the center of mass will be. -/// 2. if you add/remove shapes from a body and recompute the mass, -/// the joints will be broken. -struct B2_API b2RevoluteJointDef : public b2JointDef -{ - b2RevoluteJointDef() - { - type = e_revoluteJoint; - localAnchorA.Set(0.0f, 0.0f); - localAnchorB.Set(0.0f, 0.0f); - referenceAngle = 0.0f; - lowerAngle = 0.0f; - upperAngle = 0.0f; - maxMotorTorque = 0.0f; - motorSpeed = 0.0f; - enableLimit = false; - enableMotor = false; - } - - /// Initialize the bodies, anchors, and reference angle using a world - /// anchor point. - void Initialize(b2Body* bodyA, b2Body* bodyB, const b2Vec2& anchor); - - /// The local anchor point relative to bodyA's origin. - b2Vec2 localAnchorA; - - /// The local anchor point relative to bodyB's origin. - b2Vec2 localAnchorB; - - /// The bodyB angle minus bodyA angle in the reference state (radians). - float referenceAngle; - - /// A flag to enable joint limits. - bool enableLimit; - - /// The lower angle for the joint limit (radians). - float lowerAngle; - - /// The upper angle for the joint limit (radians). - float upperAngle; - - /// A flag to enable the joint motor. - bool enableMotor; - - /// The desired motor speed. Usually in radians per second. - float motorSpeed; - - /// The maximum motor torque used to achieve the desired motor speed. - /// Usually in N-m. - float maxMotorTorque; -}; - -/// A revolute joint constrains two bodies to share a common point while they -/// are free to rotate about the point. The relative rotation about the shared -/// point is the joint angle. You can limit the relative rotation with -/// a joint limit that specifies a lower and upper angle. You can use a motor -/// to drive the relative rotation about the shared point. A maximum motor torque -/// is provided so that infinite forces are not generated. -class B2_API b2RevoluteJoint : public b2Joint -{ -public: - b2Vec2 GetAnchorA() const override; - b2Vec2 GetAnchorB() const override; - - /// The local anchor point relative to bodyA's origin. - const b2Vec2& GetLocalAnchorA() const { return m_localAnchorA; } - - /// The local anchor point relative to bodyB's origin. - const b2Vec2& GetLocalAnchorB() const { return m_localAnchorB; } - - /// Get the reference angle. - float GetReferenceAngle() const { return m_referenceAngle; } - - /// Get the current joint angle in radians. - float GetJointAngle() const; - - /// Get the current joint angle speed in radians per second. - float GetJointSpeed() const; - - /// Is the joint limit enabled? - bool IsLimitEnabled() const; - - /// Enable/disable the joint limit. - void EnableLimit(bool flag); - - /// Get the lower joint limit in radians. - float GetLowerLimit() const; - - /// Get the upper joint limit in radians. - float GetUpperLimit() const; - - /// Set the joint limits in radians. - void SetLimits(float lower, float upper); - - /// Is the joint motor enabled? - bool IsMotorEnabled() const; - - /// Enable/disable the joint motor. - void EnableMotor(bool flag); - - /// Set the motor speed in radians per second. - void SetMotorSpeed(float speed); - - /// Get the motor speed in radians per second. - float GetMotorSpeed() const; - - /// Set the maximum motor torque, usually in N-m. - void SetMaxMotorTorque(float torque); - float GetMaxMotorTorque() const { return m_maxMotorTorque; } - - /// Get the reaction force given the inverse time step. - /// Unit is N. - b2Vec2 GetReactionForce(float inv_dt) const override; - - /// Get the reaction torque due to the joint limit given the inverse time step. - /// Unit is N*m. - float GetReactionTorque(float inv_dt) const override; - - /// Get the current motor torque given the inverse time step. - /// Unit is N*m. - float GetMotorTorque(float inv_dt) const; - - /// Dump to b2Log. - void Dump() override; - - /// - void Draw(b2Draw* draw) const override; - -protected: - - friend class b2Joint; - friend class b2GearJoint; - - b2RevoluteJoint(const b2RevoluteJointDef* def); - - void InitVelocityConstraints(const b2SolverData& data) override; - void SolveVelocityConstraints(const b2SolverData& data) override; - bool SolvePositionConstraints(const b2SolverData& data) override; - - // Solver shared - b2Vec2 m_localAnchorA; - b2Vec2 m_localAnchorB; - b2Vec2 m_impulse; - float m_motorImpulse; - float m_lowerImpulse; - float m_upperImpulse; - bool m_enableMotor; - float m_maxMotorTorque; - float m_motorSpeed; - bool m_enableLimit; - float m_referenceAngle; - float m_lowerAngle; - float m_upperAngle; - - // Solver temp - int32 m_indexA; - int32 m_indexB; - b2Vec2 m_rA; - b2Vec2 m_rB; - b2Vec2 m_localCenterA; - b2Vec2 m_localCenterB; - float m_invMassA; - float m_invMassB; - float m_invIA; - float m_invIB; - b2Mat22 m_K; - float m_angle; - float m_axialMass; -}; - -inline float b2RevoluteJoint::GetMotorSpeed() const -{ - return m_motorSpeed; -} - -#endif diff --git a/3rdparty/box2d/include/box2d/b2_rope.h b/3rdparty/box2d/include/box2d/b2_rope.h deleted file mode 100644 index 0c4f819595e7..000000000000 --- a/3rdparty/box2d/include/box2d/b2_rope.h +++ /dev/null @@ -1,155 +0,0 @@ -// MIT License - -// Copyright (c) 2019 Erin Catto - -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: - -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. - -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -#ifndef B2_ROPE_H -#define B2_ROPE_H - -#include "b2_api.h" -#include "b2_math.h" - -class b2Draw; -struct b2RopeStretch; -struct b2RopeBend; - -enum b2StretchingModel -{ - b2_pbdStretchingModel, - b2_xpbdStretchingModel -}; - -enum b2BendingModel -{ - b2_springAngleBendingModel = 0, - b2_pbdAngleBendingModel, - b2_xpbdAngleBendingModel, - b2_pbdDistanceBendingModel, - b2_pbdHeightBendingModel, - b2_pbdTriangleBendingModel -}; - -/// -struct B2_API b2RopeTuning -{ - b2RopeTuning() - { - stretchingModel = b2_pbdStretchingModel; - bendingModel = b2_pbdAngleBendingModel; - damping = 0.0f; - stretchStiffness = 1.0f; - stretchHertz = 1.0f; - stretchDamping = 0.0f; - bendStiffness = 0.5f; - bendHertz = 1.0f; - bendDamping = 0.0f; - isometric = false; - fixedEffectiveMass = false; - warmStart = false; - } - - b2StretchingModel stretchingModel; - b2BendingModel bendingModel; - float damping; - float stretchStiffness; - float stretchHertz; - float stretchDamping; - float bendStiffness; - float bendHertz; - float bendDamping; - bool isometric; - bool fixedEffectiveMass; - bool warmStart; -}; - -/// -struct B2_API b2RopeDef -{ - b2RopeDef() - { - position.SetZero(); - vertices = nullptr; - count = 0; - masses = nullptr; - gravity.SetZero(); - } - - b2Vec2 position; - b2Vec2* vertices; - int32 count; - float* masses; - b2Vec2 gravity; - b2RopeTuning tuning; -}; - -/// -class B2_API b2Rope -{ -public: - b2Rope(); - ~b2Rope(); - - /// - void Create(const b2RopeDef& def); - - /// - void SetTuning(const b2RopeTuning& tuning); - - /// - void Step(float timeStep, int32 iterations, const b2Vec2& position); - - /// - void Reset(const b2Vec2& position); - - /// - void Draw(b2Draw* draw) const; - -private: - - void SolveStretch_PBD(); - void SolveStretch_XPBD(float dt); - void SolveBend_PBD_Angle(); - void SolveBend_XPBD_Angle(float dt); - void SolveBend_PBD_Distance(); - void SolveBend_PBD_Height(); - void SolveBend_PBD_Triangle(); - void ApplyBendForces(float dt); - - b2Vec2 m_position; - - int32 m_count; - int32 m_stretchCount; - int32 m_bendCount; - - b2RopeStretch* m_stretchConstraints; - b2RopeBend* m_bendConstraints; - - b2Vec2* m_bindPositions; - b2Vec2* m_ps; - b2Vec2* m_p0s; - b2Vec2* m_vs; - - float* m_invMasses; - b2Vec2 m_gravity; - - b2RopeTuning m_tuning; -}; - -#endif diff --git a/3rdparty/box2d/include/box2d/b2_settings.h b/3rdparty/box2d/include/box2d/b2_settings.h deleted file mode 100644 index 48cd95dfe2a5..000000000000 --- a/3rdparty/box2d/include/box2d/b2_settings.h +++ /dev/null @@ -1,127 +0,0 @@ -// MIT License - -// Copyright (c) 2019 Erin Catto - -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: - -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. - -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -#ifndef B2_SETTINGS_H -#define B2_SETTINGS_H - -#include "b2_types.h" -#include "b2_api.h" - -/// @file -/// Settings that can be overriden for your application -/// - -/// Define this macro in your build if you want to override settings -#ifdef B2_USER_SETTINGS - -/// This is a user file that includes custom definitions of the macros, structs, and functions -/// defined below. -#include "b2_user_settings.h" - -#else - -#include -#include - -// Tunable Constants - -/// You can use this to change the length scale used by your game. -/// For example for inches you could use 39.4. -#define b2_lengthUnitsPerMeter 1.0f - -/// The maximum number of vertices on a convex polygon. You cannot increase -/// this too much because b2BlockAllocator has a maximum object size. -#define b2_maxPolygonVertices 8 - -// User data - -/// You can define this to inject whatever data you want in b2Body -struct B2_API b2BodyUserData -{ - b2BodyUserData() - { - pointer = 0; - } - - /// For legacy compatibility - uintptr_t pointer; -}; - -/// You can define this to inject whatever data you want in b2Fixture -struct B2_API b2FixtureUserData -{ - b2FixtureUserData() - { - pointer = 0; - } - - /// For legacy compatibility - uintptr_t pointer; -}; - -/// You can define this to inject whatever data you want in b2Joint -struct B2_API b2JointUserData -{ - b2JointUserData() - { - pointer = 0; - } - - /// For legacy compatibility - uintptr_t pointer; -}; - -// Memory Allocation - -/// Default allocation functions -B2_API void* b2Alloc_Default(int32 size); -B2_API void b2Free_Default(void* mem); - -/// Implement this function to use your own memory allocator. -inline void* b2Alloc(int32 size) -{ - return b2Alloc_Default(size); -} - -/// If you implement b2Alloc, you should also implement this function. -inline void b2Free(void* mem) -{ - b2Free_Default(mem); -} - -/// Default logging function -B2_API void b2Log_Default(const char* string, va_list args); - -/// Implement this to use your own logging. -inline void b2Log(const char* string, ...) -{ - va_list args; - va_start(args, string); - b2Log_Default(string, args); - va_end(args); -} - -#endif // B2_USER_SETTINGS - -#include "b2_common.h" - -#endif diff --git a/3rdparty/box2d/include/box2d/b2_shape.h b/3rdparty/box2d/include/box2d/b2_shape.h deleted file mode 100644 index cbed2b863358..000000000000 --- a/3rdparty/box2d/include/box2d/b2_shape.h +++ /dev/null @@ -1,110 +0,0 @@ -// MIT License - -// Copyright (c) 2019 Erin Catto - -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: - -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. - -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -#ifndef B2_SHAPE_H -#define B2_SHAPE_H - -#include "b2_api.h" -#include "b2_math.h" -#include "b2_collision.h" - -class b2BlockAllocator; - -/// This holds the mass data computed for a shape. -struct B2_API b2MassData -{ - /// The mass of the shape, usually in kilograms. - float mass; - - /// The position of the shape's centroid relative to the shape's origin. - b2Vec2 center; - - /// The rotational inertia of the shape about the local origin. - float I; -}; - -/// A shape is used for collision detection. You can create a shape however you like. -/// Shapes used for simulation in b2World are created automatically when a b2Fixture -/// is created. Shapes may encapsulate a one or more child shapes. -class B2_API b2Shape -{ -public: - - enum Type - { - e_circle = 0, - e_edge = 1, - e_polygon = 2, - e_chain = 3, - e_typeCount = 4 - }; - - virtual ~b2Shape() {} - - /// Clone the concrete shape using the provided allocator. - virtual b2Shape* Clone(b2BlockAllocator* allocator) const = 0; - - /// Get the type of this shape. You can use this to down cast to the concrete shape. - /// @return the shape type. - Type GetType() const; - - /// Get the number of child primitives. - virtual int32 GetChildCount() const = 0; - - /// Test a point for containment in this shape. This only works for convex shapes. - /// @param xf the shape world transform. - /// @param p a point in world coordinates. - virtual bool TestPoint(const b2Transform& xf, const b2Vec2& p) const = 0; - - /// Cast a ray against a child shape. - /// @param output the ray-cast results. - /// @param input the ray-cast input parameters. - /// @param transform the transform to be applied to the shape. - /// @param childIndex the child shape index - virtual bool RayCast(b2RayCastOutput* output, const b2RayCastInput& input, - const b2Transform& transform, int32 childIndex) const = 0; - - /// Given a transform, compute the associated axis aligned bounding box for a child shape. - /// @param aabb returns the axis aligned box. - /// @param xf the world transform of the shape. - /// @param childIndex the child shape - virtual void ComputeAABB(b2AABB* aabb, const b2Transform& xf, int32 childIndex) const = 0; - - /// Compute the mass properties of this shape using its dimensions and density. - /// The inertia tensor is computed about the local origin. - /// @param massData returns the mass data for this shape. - /// @param density the density in kilograms per meter squared. - virtual void ComputeMass(b2MassData* massData, float density) const = 0; - - Type m_type; - - /// Radius of a shape. For polygonal shapes this must be b2_polygonRadius. There is no support for - /// making rounded polygons. - float m_radius; -}; - -inline b2Shape::Type b2Shape::GetType() const -{ - return m_type; -} - -#endif diff --git a/3rdparty/box2d/include/box2d/b2_stack_allocator.h b/3rdparty/box2d/include/box2d/b2_stack_allocator.h deleted file mode 100644 index 1db2af5d95f3..000000000000 --- a/3rdparty/box2d/include/box2d/b2_stack_allocator.h +++ /dev/null @@ -1,65 +0,0 @@ -// MIT License - -// Copyright (c) 2019 Erin Catto - -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: - -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. - -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -#ifndef B2_STACK_ALLOCATOR_H -#define B2_STACK_ALLOCATOR_H - -#include "b2_api.h" -#include "b2_settings.h" - -const int32 b2_stackSize = 100 * 1024; // 100k -const int32 b2_maxStackEntries = 32; - -struct B2_API b2StackEntry -{ - char* data; - int32 size; - bool usedMalloc; -}; - -// This is a stack allocator used for fast per step allocations. -// You must nest allocate/free pairs. The code will assert -// if you try to interleave multiple allocate/free pairs. -class B2_API b2StackAllocator -{ -public: - b2StackAllocator(); - ~b2StackAllocator(); - - void* Allocate(int32 size); - void Free(void* p); - - int32 GetMaxAllocation() const; - -private: - - char m_data[b2_stackSize]; - int32 m_index; - - int32 m_allocation; - int32 m_maxAllocation; - - b2StackEntry m_entries[b2_maxStackEntries]; - int32 m_entryCount; -}; - -#endif diff --git a/3rdparty/box2d/include/box2d/b2_time_of_impact.h b/3rdparty/box2d/include/box2d/b2_time_of_impact.h deleted file mode 100644 index 04d46262e4ca..000000000000 --- a/3rdparty/box2d/include/box2d/b2_time_of_impact.h +++ /dev/null @@ -1,63 +0,0 @@ -// MIT License - -// Copyright (c) 2019 Erin Catto - -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: - -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. - -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -#ifndef B2_TIME_OF_IMPACT_H -#define B2_TIME_OF_IMPACT_H - -#include "b2_api.h" -#include "b2_math.h" -#include "b2_distance.h" - -/// Input parameters for b2TimeOfImpact -struct B2_API b2TOIInput -{ - b2DistanceProxy proxyA; - b2DistanceProxy proxyB; - b2Sweep sweepA; - b2Sweep sweepB; - float tMax; // defines sweep interval [0, tMax] -}; - -/// Output parameters for b2TimeOfImpact. -struct B2_API b2TOIOutput -{ - enum State - { - e_unknown, - e_failed, - e_overlapped, - e_touching, - e_separated - }; - - State state; - float t; -}; - -/// Compute the upper bound on time before two shapes penetrate. Time is represented as -/// a fraction between [0,tMax]. This uses a swept separating axis and may miss some intermediate, -/// non-tunneling collisions. If you change the time interval, you should call this function -/// again. -/// Note: use b2Distance to compute the contact point and normal at the time of impact. -B2_API void b2TimeOfImpact(b2TOIOutput* output, const b2TOIInput* input); - -#endif diff --git a/3rdparty/box2d/include/box2d/b2_time_step.h b/3rdparty/box2d/include/box2d/b2_time_step.h deleted file mode 100644 index 13d6292ec15c..000000000000 --- a/3rdparty/box2d/include/box2d/b2_time_step.h +++ /dev/null @@ -1,74 +0,0 @@ -// MIT License - -// Copyright (c) 2019 Erin Catto - -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: - -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. - -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. -#ifndef B2_TIME_STEP_H -#define B2_TIME_STEP_H - -#include "b2_api.h" -#include "b2_math.h" - -/// Profiling data. Times are in milliseconds. -struct B2_API b2Profile -{ - float step; - float collide; - float solve; - float solveInit; - float solveVelocity; - float solvePosition; - float broadphase; - float solveTOI; -}; - -/// This is an internal structure. -struct B2_API b2TimeStep -{ - float dt; // time step - float inv_dt; // inverse time step (0 if dt == 0). - float dtRatio; // dt * inv_dt0 - int32 velocityIterations; - int32 positionIterations; - bool warmStarting; -}; - -/// This is an internal structure. -struct B2_API b2Position -{ - b2Vec2 c; - float a; -}; - -/// This is an internal structure. -struct B2_API b2Velocity -{ - b2Vec2 v; - float w; -}; - -/// Solver Data -struct B2_API b2SolverData -{ - b2TimeStep step; - b2Position* positions; - b2Velocity* velocities; -}; - -#endif diff --git a/3rdparty/box2d/include/box2d/b2_timer.h b/3rdparty/box2d/include/box2d/b2_timer.h deleted file mode 100644 index 7893c32703bc..000000000000 --- a/3rdparty/box2d/include/box2d/b2_timer.h +++ /dev/null @@ -1,55 +0,0 @@ -// MIT License - -// Copyright (c) 2019 Erin Catto - -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: - -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. - -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -#ifndef B2_TIMER_H -#define B2_TIMER_H - -#include "b2_api.h" -#include "b2_settings.h" - -/// Timer for profiling. This has platform specific code and may -/// not work on every platform. -class B2_API b2Timer -{ -public: - - /// Constructor - b2Timer(); - - /// Reset the timer. - void Reset(); - - /// Get the time since construction or the last reset. - float GetMilliseconds() const; - -private: - -#if defined(_WIN32) - double m_start; - static double s_invFrequency; -#elif defined(__linux__) || defined (__APPLE__) - unsigned long long m_start_sec; - unsigned long long m_start_usec; -#endif -}; - -#endif diff --git a/3rdparty/box2d/include/box2d/b2_types.h b/3rdparty/box2d/include/box2d/b2_types.h deleted file mode 100644 index e0d4377591d6..000000000000 --- a/3rdparty/box2d/include/box2d/b2_types.h +++ /dev/null @@ -1,33 +0,0 @@ -// MIT License - -// Copyright (c) 2020 Erin Catto - -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: - -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. - -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -#ifndef B2_TYPES_H -#define B2_TYPES_H - -typedef signed char int8; -typedef signed short int16; -typedef signed int int32; -typedef unsigned char uint8; -typedef unsigned short uint16; -typedef unsigned int uint32; - -#endif diff --git a/3rdparty/box2d/include/box2d/b2_weld_joint.h b/3rdparty/box2d/include/box2d/b2_weld_joint.h deleted file mode 100644 index 38501f20ab05..000000000000 --- a/3rdparty/box2d/include/box2d/b2_weld_joint.h +++ /dev/null @@ -1,133 +0,0 @@ -// MIT License - -// Copyright (c) 2019 Erin Catto - -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: - -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. - -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -#ifndef B2_WELD_JOINT_H -#define B2_WELD_JOINT_H - -#include "b2_api.h" -#include "b2_joint.h" - -/// Weld joint definition. You need to specify local anchor points -/// where they are attached and the relative body angle. The position -/// of the anchor points is important for computing the reaction torque. -struct B2_API b2WeldJointDef : public b2JointDef -{ - b2WeldJointDef() - { - type = e_weldJoint; - localAnchorA.Set(0.0f, 0.0f); - localAnchorB.Set(0.0f, 0.0f); - referenceAngle = 0.0f; - stiffness = 0.0f; - damping = 0.0f; - } - - /// Initialize the bodies, anchors, reference angle, stiffness, and damping. - /// @param bodyA the first body connected by this joint - /// @param bodyB the second body connected by this joint - /// @param anchor the point of connection in world coordinates - void Initialize(b2Body* bodyA, b2Body* bodyB, const b2Vec2& anchor); - - /// The local anchor point relative to bodyA's origin. - b2Vec2 localAnchorA; - - /// The local anchor point relative to bodyB's origin. - b2Vec2 localAnchorB; - - /// The bodyB angle minus bodyA angle in the reference state (radians). - float referenceAngle; - - /// The rotational stiffness in N*m - /// Disable softness with a value of 0 - float stiffness; - - /// The rotational damping in N*m*s - float damping; -}; - -/// A weld joint essentially glues two bodies together. A weld joint may -/// distort somewhat because the island constraint solver is approximate. -class B2_API b2WeldJoint : public b2Joint -{ -public: - b2Vec2 GetAnchorA() const override; - b2Vec2 GetAnchorB() const override; - - b2Vec2 GetReactionForce(float inv_dt) const override; - float GetReactionTorque(float inv_dt) const override; - - /// The local anchor point relative to bodyA's origin. - const b2Vec2& GetLocalAnchorA() const { return m_localAnchorA; } - - /// The local anchor point relative to bodyB's origin. - const b2Vec2& GetLocalAnchorB() const { return m_localAnchorB; } - - /// Get the reference angle. - float GetReferenceAngle() const { return m_referenceAngle; } - - /// Set/get stiffness in N*m - void SetStiffness(float stiffness) { m_stiffness = stiffness; } - float GetStiffness() const { return m_stiffness; } - - /// Set/get damping in N*m*s - void SetDamping(float damping) { m_damping = damping; } - float GetDamping() const { return m_damping; } - - /// Dump to b2Log - void Dump() override; - -protected: - - friend class b2Joint; - - b2WeldJoint(const b2WeldJointDef* def); - - void InitVelocityConstraints(const b2SolverData& data) override; - void SolveVelocityConstraints(const b2SolverData& data) override; - bool SolvePositionConstraints(const b2SolverData& data) override; - - float m_stiffness; - float m_damping; - float m_bias; - - // Solver shared - b2Vec2 m_localAnchorA; - b2Vec2 m_localAnchorB; - float m_referenceAngle; - float m_gamma; - b2Vec3 m_impulse; - - // Solver temp - int32 m_indexA; - int32 m_indexB; - b2Vec2 m_rA; - b2Vec2 m_rB; - b2Vec2 m_localCenterA; - b2Vec2 m_localCenterB; - float m_invMassA; - float m_invMassB; - float m_invIA; - float m_invIB; - b2Mat33 m_mass; -}; - -#endif diff --git a/3rdparty/box2d/include/box2d/b2_wheel_joint.h b/3rdparty/box2d/include/box2d/b2_wheel_joint.h deleted file mode 100644 index 8576adbd20ec..000000000000 --- a/3rdparty/box2d/include/box2d/b2_wheel_joint.h +++ /dev/null @@ -1,240 +0,0 @@ -// MIT License - -// Copyright (c) 2019 Erin Catto - -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: - -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. - -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -#ifndef B2_WHEEL_JOINT_H -#define B2_WHEEL_JOINT_H - -#include "b2_api.h" -#include "b2_joint.h" - -/// Wheel joint definition. This requires defining a line of -/// motion using an axis and an anchor point. The definition uses local -/// anchor points and a local axis so that the initial configuration -/// can violate the constraint slightly. The joint translation is zero -/// when the local anchor points coincide in world space. Using local -/// anchors and a local axis helps when saving and loading a game. -struct B2_API b2WheelJointDef : public b2JointDef -{ - b2WheelJointDef() - { - type = e_wheelJoint; - localAnchorA.SetZero(); - localAnchorB.SetZero(); - localAxisA.Set(1.0f, 0.0f); - enableLimit = false; - lowerTranslation = 0.0f; - upperTranslation = 0.0f; - enableMotor = false; - maxMotorTorque = 0.0f; - motorSpeed = 0.0f; - stiffness = 0.0f; - damping = 0.0f; - } - - /// Initialize the bodies, anchors, axis, and reference angle using the world - /// anchor and world axis. - void Initialize(b2Body* bodyA, b2Body* bodyB, const b2Vec2& anchor, const b2Vec2& axis); - - /// The local anchor point relative to bodyA's origin. - b2Vec2 localAnchorA; - - /// The local anchor point relative to bodyB's origin. - b2Vec2 localAnchorB; - - /// The local translation axis in bodyA. - b2Vec2 localAxisA; - - /// Enable/disable the joint limit. - bool enableLimit; - - /// The lower translation limit, usually in meters. - float lowerTranslation; - - /// The upper translation limit, usually in meters. - float upperTranslation; - - /// Enable/disable the joint motor. - bool enableMotor; - - /// The maximum motor torque, usually in N-m. - float maxMotorTorque; - - /// The desired motor speed in radians per second. - float motorSpeed; - - /// Suspension stiffness. Typically in units N/m. - float stiffness; - - /// Suspension damping. Typically in units of N*s/m. - float damping; -}; - -/// A wheel joint. This joint provides two degrees of freedom: translation -/// along an axis fixed in bodyA and rotation in the plane. In other words, it is a point to -/// line constraint with a rotational motor and a linear spring/damper. The spring/damper is -/// initialized upon creation. This joint is designed for vehicle suspensions. -class B2_API b2WheelJoint : public b2Joint -{ -public: - b2Vec2 GetAnchorA() const override; - b2Vec2 GetAnchorB() const override; - - b2Vec2 GetReactionForce(float inv_dt) const override; - float GetReactionTorque(float inv_dt) const override; - - /// The local anchor point relative to bodyA's origin. - const b2Vec2& GetLocalAnchorA() const { return m_localAnchorA; } - - /// The local anchor point relative to bodyB's origin. - const b2Vec2& GetLocalAnchorB() const { return m_localAnchorB; } - - /// The local joint axis relative to bodyA. - const b2Vec2& GetLocalAxisA() const { return m_localXAxisA; } - - /// Get the current joint translation, usually in meters. - float GetJointTranslation() const; - - /// Get the current joint linear speed, usually in meters per second. - float GetJointLinearSpeed() const; - - /// Get the current joint angle in radians. - float GetJointAngle() const; - - /// Get the current joint angular speed in radians per second. - float GetJointAngularSpeed() const; - - /// Is the joint limit enabled? - bool IsLimitEnabled() const; - - /// Enable/disable the joint translation limit. - void EnableLimit(bool flag); - - /// Get the lower joint translation limit, usually in meters. - float GetLowerLimit() const; - - /// Get the upper joint translation limit, usually in meters. - float GetUpperLimit() const; - - /// Set the joint translation limits, usually in meters. - void SetLimits(float lower, float upper); - - /// Is the joint motor enabled? - bool IsMotorEnabled() const; - - /// Enable/disable the joint motor. - void EnableMotor(bool flag); - - /// Set the motor speed, usually in radians per second. - void SetMotorSpeed(float speed); - - /// Get the motor speed, usually in radians per second. - float GetMotorSpeed() const; - - /// Set/Get the maximum motor force, usually in N-m. - void SetMaxMotorTorque(float torque); - float GetMaxMotorTorque() const; - - /// Get the current motor torque given the inverse time step, usually in N-m. - float GetMotorTorque(float inv_dt) const; - - /// Access spring stiffness - void SetStiffness(float stiffness); - float GetStiffness() const; - - /// Access damping - void SetDamping(float damping); - float GetDamping() const; - - /// Dump to b2Log - void Dump() override; - - /// - void Draw(b2Draw* draw) const override; - -protected: - - friend class b2Joint; - b2WheelJoint(const b2WheelJointDef* def); - - void InitVelocityConstraints(const b2SolverData& data) override; - void SolveVelocityConstraints(const b2SolverData& data) override; - bool SolvePositionConstraints(const b2SolverData& data) override; - - b2Vec2 m_localAnchorA; - b2Vec2 m_localAnchorB; - b2Vec2 m_localXAxisA; - b2Vec2 m_localYAxisA; - - float m_impulse; - float m_motorImpulse; - float m_springImpulse; - - float m_lowerImpulse; - float m_upperImpulse; - float m_translation; - float m_lowerTranslation; - float m_upperTranslation; - - float m_maxMotorTorque; - float m_motorSpeed; - - bool m_enableLimit; - bool m_enableMotor; - - float m_stiffness; - float m_damping; - - // Solver temp - int32 m_indexA; - int32 m_indexB; - b2Vec2 m_localCenterA; - b2Vec2 m_localCenterB; - float m_invMassA; - float m_invMassB; - float m_invIA; - float m_invIB; - - b2Vec2 m_ax, m_ay; - float m_sAx, m_sBx; - float m_sAy, m_sBy; - - float m_mass; - float m_motorMass; - float m_axialMass; - float m_springMass; - - float m_bias; - float m_gamma; - -}; - -inline float b2WheelJoint::GetMotorSpeed() const -{ - return m_motorSpeed; -} - -inline float b2WheelJoint::GetMaxMotorTorque() const -{ - return m_maxMotorTorque; -} - -#endif diff --git a/3rdparty/box2d/include/box2d/b2_world.h b/3rdparty/box2d/include/box2d/b2_world.h deleted file mode 100644 index afd73bd4657a..000000000000 --- a/3rdparty/box2d/include/box2d/b2_world.h +++ /dev/null @@ -1,348 +0,0 @@ -// MIT License - -// Copyright (c) 2019 Erin Catto - -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: - -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. - -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -#ifndef B2_WORLD_H -#define B2_WORLD_H - -#include "b2_api.h" -#include "b2_block_allocator.h" -#include "b2_contact_manager.h" -#include "b2_math.h" -#include "b2_stack_allocator.h" -#include "b2_time_step.h" -#include "b2_world_callbacks.h" - -struct b2AABB; -struct b2BodyDef; -struct b2Color; -struct b2JointDef; -class b2Body; -class b2Draw; -class b2Fixture; -class b2Joint; - -/// The world class manages all physics entities, dynamic simulation, -/// and asynchronous queries. The world also contains efficient memory -/// management facilities. -class B2_API b2World -{ -public: - /// Construct a world object. - /// @param gravity the world gravity vector. - b2World(const b2Vec2& gravity); - - /// Destruct the world. All physics entities are destroyed and all heap memory is released. - ~b2World(); - - /// Register a destruction listener. The listener is owned by you and must - /// remain in scope. - void SetDestructionListener(b2DestructionListener* listener); - - /// Register a contact filter to provide specific control over collision. - /// Otherwise the default filter is used (b2_defaultFilter). The listener is - /// owned by you and must remain in scope. - void SetContactFilter(b2ContactFilter* filter); - - /// Register a contact event listener. The listener is owned by you and must - /// remain in scope. - void SetContactListener(b2ContactListener* listener); - - /// Register a routine for debug drawing. The debug draw functions are called - /// inside with b2World::DebugDraw method. The debug draw object is owned - /// by you and must remain in scope. - void SetDebugDraw(b2Draw* debugDraw); - - /// Create a rigid body given a definition. No reference to the definition - /// is retained. - /// @warning This function is locked during callbacks. - b2Body* CreateBody(const b2BodyDef* def); - - /// Destroy a rigid body given a definition. No reference to the definition - /// is retained. This function is locked during callbacks. - /// @warning This automatically deletes all associated shapes and joints. - /// @warning This function is locked during callbacks. - void DestroyBody(b2Body* body); - - /// Create a joint to constrain bodies together. No reference to the definition - /// is retained. This may cause the connected bodies to cease colliding. - /// @warning This function is locked during callbacks. - b2Joint* CreateJoint(const b2JointDef* def); - - /// Destroy a joint. This may cause the connected bodies to begin colliding. - /// @warning This function is locked during callbacks. - void DestroyJoint(b2Joint* joint); - - /// Take a time step. This performs collision detection, integration, - /// and constraint solution. - /// @param timeStep the amount of time to simulate, this should not vary. - /// @param velocityIterations for the velocity constraint solver. - /// @param positionIterations for the position constraint solver. - void Step( float timeStep, - int32 velocityIterations, - int32 positionIterations); - - /// Manually clear the force buffer on all bodies. By default, forces are cleared automatically - /// after each call to Step. The default behavior is modified by calling SetAutoClearForces. - /// The purpose of this function is to support sub-stepping. Sub-stepping is often used to maintain - /// a fixed sized time step under a variable frame-rate. - /// When you perform sub-stepping you will disable auto clearing of forces and instead call - /// ClearForces after all sub-steps are complete in one pass of your game loop. - /// @see SetAutoClearForces - void ClearForces(); - - /// Call this to draw shapes and other debug draw data. This is intentionally non-const. - void DebugDraw(); - - /// Query the world for all fixtures that potentially overlap the - /// provided AABB. - /// @param callback a user implemented callback class. - /// @param aabb the query box. - void QueryAABB(b2QueryCallback* callback, const b2AABB& aabb) const; - - /// Ray-cast the world for all fixtures in the path of the ray. Your callback - /// controls whether you get the closest point, any point, or n-points. - /// The ray-cast ignores shapes that contain the starting point. - /// @param callback a user implemented callback class. - /// @param point1 the ray starting point - /// @param point2 the ray ending point - void RayCast(b2RayCastCallback* callback, const b2Vec2& point1, const b2Vec2& point2) const; - - /// Get the world body list. With the returned body, use b2Body::GetNext to get - /// the next body in the world list. A nullptr body indicates the end of the list. - /// @return the head of the world body list. - b2Body* GetBodyList(); - const b2Body* GetBodyList() const; - - /// Get the world joint list. With the returned joint, use b2Joint::GetNext to get - /// the next joint in the world list. A nullptr joint indicates the end of the list. - /// @return the head of the world joint list. - b2Joint* GetJointList(); - const b2Joint* GetJointList() const; - - /// Get the world contact list. With the returned contact, use b2Contact::GetNext to get - /// the next contact in the world list. A nullptr contact indicates the end of the list. - /// @return the head of the world contact list. - /// @warning contacts are created and destroyed in the middle of a time step. - /// Use b2ContactListener to avoid missing contacts. - b2Contact* GetContactList(); - const b2Contact* GetContactList() const; - - /// Enable/disable sleep. - void SetAllowSleeping(bool flag); - bool GetAllowSleeping() const { return m_allowSleep; } - - /// Enable/disable warm starting. For testing. - void SetWarmStarting(bool flag) { m_warmStarting = flag; } - bool GetWarmStarting() const { return m_warmStarting; } - - /// Enable/disable continuous physics. For testing. - void SetContinuousPhysics(bool flag) { m_continuousPhysics = flag; } - bool GetContinuousPhysics() const { return m_continuousPhysics; } - - /// Enable/disable single stepped continuous physics. For testing. - void SetSubStepping(bool flag) { m_subStepping = flag; } - bool GetSubStepping() const { return m_subStepping; } - - /// Get the number of broad-phase proxies. - int32 GetProxyCount() const; - - /// Get the number of bodies. - int32 GetBodyCount() const; - - /// Get the number of joints. - int32 GetJointCount() const; - - /// Get the number of contacts (each may have 0 or more contact points). - int32 GetContactCount() const; - - /// Get the height of the dynamic tree. - int32 GetTreeHeight() const; - - /// Get the balance of the dynamic tree. - int32 GetTreeBalance() const; - - /// Get the quality metric of the dynamic tree. The smaller the better. - /// The minimum is 1. - float GetTreeQuality() const; - - /// Change the global gravity vector. - void SetGravity(const b2Vec2& gravity); - - /// Get the global gravity vector. - b2Vec2 GetGravity() const; - - /// Is the world locked (in the middle of a time step). - bool IsLocked() const; - - /// Set flag to control automatic clearing of forces after each time step. - void SetAutoClearForces(bool flag); - - /// Get the flag that controls automatic clearing of forces after each time step. - bool GetAutoClearForces() const; - - /// Shift the world origin. Useful for large worlds. - /// The body shift formula is: position -= newOrigin - /// @param newOrigin the new origin with respect to the old origin - void ShiftOrigin(const b2Vec2& newOrigin); - - /// Get the contact manager for testing. - const b2ContactManager& GetContactManager() const; - - /// Get the current profile. - const b2Profile& GetProfile() const; - - /// Dump the world into the log file. - /// @warning this should be called outside of a time step. - void Dump(); - -private: - - friend class b2Body; - friend class b2Fixture; - friend class b2ContactManager; - friend class b2Controller; - - b2World(const b2World&) = delete; - void operator=(const b2World&) = delete; - - void Solve(const b2TimeStep& step); - void SolveTOI(const b2TimeStep& step); - - void DrawShape(b2Fixture* shape, const b2Transform& xf, const b2Color& color); - - b2BlockAllocator m_blockAllocator; - b2StackAllocator m_stackAllocator; - - b2ContactManager m_contactManager; - - b2Body* m_bodyList; - b2Joint* m_jointList; - - int32 m_bodyCount; - int32 m_jointCount; - - b2Vec2 m_gravity; - bool m_allowSleep; - - b2DestructionListener* m_destructionListener; - b2Draw* m_debugDraw; - - // This is used to compute the time step ratio to - // support a variable time step. - float m_inv_dt0; - - bool m_newContacts; - bool m_locked; - bool m_clearForces; - - // These are for debugging the solver. - bool m_warmStarting; - bool m_continuousPhysics; - bool m_subStepping; - - bool m_stepComplete; - - b2Profile m_profile; -}; - -inline b2Body* b2World::GetBodyList() -{ - return m_bodyList; -} - -inline const b2Body* b2World::GetBodyList() const -{ - return m_bodyList; -} - -inline b2Joint* b2World::GetJointList() -{ - return m_jointList; -} - -inline const b2Joint* b2World::GetJointList() const -{ - return m_jointList; -} - -inline b2Contact* b2World::GetContactList() -{ - return m_contactManager.m_contactList; -} - -inline const b2Contact* b2World::GetContactList() const -{ - return m_contactManager.m_contactList; -} - -inline int32 b2World::GetBodyCount() const -{ - return m_bodyCount; -} - -inline int32 b2World::GetJointCount() const -{ - return m_jointCount; -} - -inline int32 b2World::GetContactCount() const -{ - return m_contactManager.m_contactCount; -} - -inline void b2World::SetGravity(const b2Vec2& gravity) -{ - m_gravity = gravity; -} - -inline b2Vec2 b2World::GetGravity() const -{ - return m_gravity; -} - -inline bool b2World::IsLocked() const -{ - return m_locked; -} - -inline void b2World::SetAutoClearForces(bool flag) -{ - m_clearForces = flag; -} - -/// Get the flag that controls automatic clearing of forces after each time step. -inline bool b2World::GetAutoClearForces() const -{ - return m_clearForces; -} - -inline const b2ContactManager& b2World::GetContactManager() const -{ - return m_contactManager; -} - -inline const b2Profile& b2World::GetProfile() const -{ - return m_profile; -} - -#endif diff --git a/3rdparty/box2d/include/box2d/b2_world_callbacks.h b/3rdparty/box2d/include/box2d/b2_world_callbacks.h deleted file mode 100644 index da45640e2f08..000000000000 --- a/3rdparty/box2d/include/box2d/b2_world_callbacks.h +++ /dev/null @@ -1,161 +0,0 @@ -// MIT License - -// Copyright (c) 2019 Erin Catto - -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: - -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. - -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -#ifndef B2_WORLD_CALLBACKS_H -#define B2_WORLD_CALLBACKS_H - -#include "b2_api.h" -#include "b2_settings.h" - -struct b2Vec2; -struct b2Transform; -class b2Fixture; -class b2Body; -class b2Joint; -class b2Contact; -struct b2ContactResult; -struct b2Manifold; - -/// Joints and fixtures are destroyed when their associated -/// body is destroyed. Implement this listener so that you -/// may nullify references to these joints and shapes. -class B2_API b2DestructionListener -{ -public: - virtual ~b2DestructionListener() {} - - /// Called when any joint is about to be destroyed due - /// to the destruction of one of its attached bodies. - virtual void SayGoodbye(b2Joint* joint) = 0; - - /// Called when any fixture is about to be destroyed due - /// to the destruction of its parent body. - virtual void SayGoodbye(b2Fixture* fixture) = 0; -}; - -/// Implement this class to provide collision filtering. In other words, you can implement -/// this class if you want finer control over contact creation. -class B2_API b2ContactFilter -{ -public: - virtual ~b2ContactFilter() {} - - /// Return true if contact calculations should be performed between these two shapes. - /// @warning for performance reasons this is only called when the AABBs begin to overlap. - virtual bool ShouldCollide(b2Fixture* fixtureA, b2Fixture* fixtureB); -}; - -/// Contact impulses for reporting. Impulses are used instead of forces because -/// sub-step forces may approach infinity for rigid body collisions. These -/// match up one-to-one with the contact points in b2Manifold. -struct B2_API b2ContactImpulse -{ - float normalImpulses[b2_maxManifoldPoints]; - float tangentImpulses[b2_maxManifoldPoints]; - int32 count; -}; - -/// Implement this class to get contact information. You can use these results for -/// things like sounds and game logic. You can also get contact results by -/// traversing the contact lists after the time step. However, you might miss -/// some contacts because continuous physics leads to sub-stepping. -/// Additionally you may receive multiple callbacks for the same contact in a -/// single time step. -/// You should strive to make your callbacks efficient because there may be -/// many callbacks per time step. -/// @warning You cannot create/destroy Box2D entities inside these callbacks. -class B2_API b2ContactListener -{ -public: - virtual ~b2ContactListener() {} - - /// Called when two fixtures begin to touch. - virtual void BeginContact(b2Contact* contact) { B2_NOT_USED(contact); } - - /// Called when two fixtures cease to touch. - virtual void EndContact(b2Contact* contact) { B2_NOT_USED(contact); } - - /// This is called after a contact is updated. This allows you to inspect a - /// contact before it goes to the solver. If you are careful, you can modify the - /// contact manifold (e.g. disable contact). - /// A copy of the old manifold is provided so that you can detect changes. - /// Note: this is called only for awake bodies. - /// Note: this is called even when the number of contact points is zero. - /// Note: this is not called for sensors. - /// Note: if you set the number of contact points to zero, you will not - /// get an EndContact callback. However, you may get a BeginContact callback - /// the next step. - virtual void PreSolve(b2Contact* contact, const b2Manifold* oldManifold) - { - B2_NOT_USED(contact); - B2_NOT_USED(oldManifold); - } - - /// This lets you inspect a contact after the solver is finished. This is useful - /// for inspecting impulses. - /// Note: the contact manifold does not include time of impact impulses, which can be - /// arbitrarily large if the sub-step is small. Hence the impulse is provided explicitly - /// in a separate data structure. - /// Note: this is only called for contacts that are touching, solid, and awake. - virtual void PostSolve(b2Contact* contact, const b2ContactImpulse* impulse) - { - B2_NOT_USED(contact); - B2_NOT_USED(impulse); - } -}; - -/// Callback class for AABB queries. -/// See b2World::Query -class B2_API b2QueryCallback -{ -public: - virtual ~b2QueryCallback() {} - - /// Called for each fixture found in the query AABB. - /// @return false to terminate the query. - virtual bool ReportFixture(b2Fixture* fixture) = 0; -}; - -/// Callback class for ray casts. -/// See b2World::RayCast -class B2_API b2RayCastCallback -{ -public: - virtual ~b2RayCastCallback() {} - - /// Called for each fixture found in the query. You control how the ray cast - /// proceeds by returning a float: - /// return -1: ignore this fixture and continue - /// return 0: terminate the ray cast - /// return fraction: clip the ray to this point - /// return 1: don't clip the ray and continue - /// @param fixture the fixture hit by the ray - /// @param point the point of initial intersection - /// @param normal the normal vector at the point of intersection - /// @param fraction the fraction along the ray at the point of intersection - /// @return -1 to filter, 0 to terminate, fraction to clip the ray for - /// closest hit, 1 to continue - virtual float ReportFixture( b2Fixture* fixture, const b2Vec2& point, - const b2Vec2& normal, float fraction) = 0; -}; - -#endif diff --git a/3rdparty/box2d/include/box2d/base.h b/3rdparty/box2d/include/box2d/base.h new file mode 100644 index 000000000000..5d8a7deb36db --- /dev/null +++ b/3rdparty/box2d/include/box2d/base.h @@ -0,0 +1,113 @@ +// SPDX-FileCopyrightText: 2023 Erin Catto +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +// clang-format off +// +// Shared library macros +#if defined( _MSC_VER ) && defined( box2d_EXPORTS ) + // build the Windows DLL + #define BOX2D_EXPORT __declspec( dllexport ) +#elif defined( _MSC_VER ) && defined( BOX2D_DLL ) + // using the Windows DLL + #define BOX2D_EXPORT __declspec( dllimport ) +#elif defined( box2d_EXPORTS ) + // building or using the Box2D shared library + #define BOX2D_EXPORT __attribute__( ( visibility( "default" ) ) ) +#else + // static library + #define BOX2D_EXPORT +#endif + +// C++ macros +#ifdef __cplusplus + #define B2_API extern "C" BOX2D_EXPORT + #define B2_INLINE inline + #define B2_LITERAL(T) T + #define B2_ZERO_INIT {} +#else + #define B2_API BOX2D_EXPORT + #define B2_INLINE static inline + /// Used for C literals like (b2Vec2){1.0f, 2.0f} where C++ requires b2Vec2{1.0f, 2.0f} + #define B2_LITERAL(T) (T) + #define B2_ZERO_INIT {0} +#endif +// clang-format on + +/** + * @defgroup base Base + * Base functionality + * @{ + */ + +/// Prototype for user allocation function +/// @param size the allocation size in bytes +/// @param alignment the required alignment, guaranteed to be a power of 2 +typedef void* b2AllocFcn( unsigned int size, int alignment ); + +/// Prototype for user free function +/// @param mem the memory previously allocated through `b2AllocFcn` +typedef void b2FreeFcn( void* mem ); + +/// Prototype for the user assert callback. Return 0 to skip the debugger break. +typedef int b2AssertFcn( const char* condition, const char* fileName, int lineNumber ); + +/// This allows the user to override the allocation functions. These should be +/// set during application startup. +B2_API void b2SetAllocator( b2AllocFcn* allocFcn, b2FreeFcn* freeFcn ); + +/// @return the total bytes allocated by Box2D +B2_API int b2GetByteCount( void ); + +/// Override the default assert callback +/// @param assertFcn a non-null assert callback +B2_API void b2SetAssertFcn( b2AssertFcn* assertFcn ); + +/// Version numbering scheme. +/// See https://semver.org/ +typedef struct b2Version +{ + /// Significant changes + int major; + + /// Incremental changes + int minor; + + /// Bug fixes + int revision; +} b2Version; + +/// Get the current version of Box2D +B2_API b2Version b2GetVersion( void ); + +/**@}*/ + +//! @cond +// Timer for profiling. This has platform specific code and may not work on every platform. +typedef struct b2Timer +{ +#if defined( _WIN32 ) + int64_t start; +#elif defined( __linux__ ) || defined( __APPLE__ ) + unsigned long long start_sec; + unsigned long long start_usec; +#else + int32_t dummy; +#endif +} b2Timer; + +B2_API b2Timer b2CreateTimer( void ); +B2_API int64_t b2GetTicks( b2Timer* timer ); +B2_API float b2GetMilliseconds( const b2Timer* timer ); +B2_API float b2GetMillisecondsAndReset( b2Timer* timer ); +B2_API void b2SleepMilliseconds( int milliseconds ); +B2_API void b2Yield( void ); + +// Simple djb2 hash function for determinism testing +#define B2_HASH_INIT 5381 +B2_API uint32_t b2Hash( uint32_t hash, const uint8_t* data, int count ); + +//! @endcond diff --git a/3rdparty/box2d/include/box2d/box2d.h b/3rdparty/box2d/include/box2d/box2d.h index 55c695822c71..8d7634be1f2a 100644 --- a/3rdparty/box2d/include/box2d/box2d.h +++ b/3rdparty/box2d/include/box2d/box2d.h @@ -1,58 +1,1098 @@ -// MIT License - -// Copyright (c) 2019 Erin Catto - -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: - -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. - -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -#ifndef BOX2D_H -#define BOX2D_H - -// These include files constitute the main Box2D API - -#include "b2_settings.h" -#include "b2_draw.h" -#include "b2_timer.h" - -#include "b2_chain_shape.h" -#include "b2_circle_shape.h" -#include "b2_edge_shape.h" -#include "b2_polygon_shape.h" - -#include "b2_broad_phase.h" -#include "b2_dynamic_tree.h" - -#include "b2_body.h" -#include "b2_contact.h" -#include "b2_fixture.h" -#include "b2_time_step.h" -#include "b2_world.h" -#include "b2_world_callbacks.h" - -#include "b2_distance_joint.h" -#include "b2_friction_joint.h" -#include "b2_gear_joint.h" -#include "b2_motor_joint.h" -#include "b2_mouse_joint.h" -#include "b2_prismatic_joint.h" -#include "b2_pulley_joint.h" -#include "b2_revolute_joint.h" -#include "b2_weld_joint.h" -#include "b2_wheel_joint.h" - -#endif +// SPDX-FileCopyrightText: 2023 Erin Catto +// SPDX-License-Identifier: MIT + +#pragma once + +#include "base.h" +#include "collision.h" +#include "id.h" +#include "types.h" + +#include + +/** + * @defgroup world World + * These functions allow you to create a simulation world. + * + * You can add rigid bodies and joint constraints to the world and run the simulation. You can get contact + * information to get contact points and normals as well as events. You can query to world, checking for overlaps and casting rays + * or shapes. There is also debugging information such as debug draw, timing information, and counters. You can find documentation + * here: https://box2d.org/ + * @{ + */ + +/// Create a world for rigid body simulation. A world contains bodies, shapes, and constraints. You make create +/// up to 128 worlds. Each world is completely independent and may be simulated in parallel. +/// @return the world id. +B2_API b2WorldId b2CreateWorld( const b2WorldDef* def ); + +/// Destroy a world +B2_API void b2DestroyWorld( b2WorldId worldId ); + +/// World id validation. Provides validation for up to 64K allocations. +B2_API bool b2World_IsValid( b2WorldId id ); + +/// Simulate a world for one time step. This performs collision detection, integration, and constraint solution. +/// @param worldId The world to simulate +/// @param timeStep The amount of time to simulate, this should be a fixed number. Typically 1/60. +/// @param subStepCount The number of sub-steps, increasing the sub-step count can increase accuracy. Typically 4. +B2_API void b2World_Step( b2WorldId worldId, float timeStep, int subStepCount ); + +/// Call this to draw shapes and other debug draw data +B2_API void b2World_Draw( b2WorldId worldId, b2DebugDraw* draw ); + +/// Get the body events for the current time step. The event data is transient. Do not store a reference to this data. +B2_API b2BodyEvents b2World_GetBodyEvents( b2WorldId worldId ); + +/// Get sensor events for the current time step. The event data is transient. Do not store a reference to this data. +B2_API b2SensorEvents b2World_GetSensorEvents( b2WorldId worldId ); + +/// Get contact events for this current time step. The event data is transient. Do not store a reference to this data. +B2_API b2ContactEvents b2World_GetContactEvents( b2WorldId worldId ); + +/// Overlap test for all shapes that *potentially* overlap the provided AABB +B2_API void b2World_OverlapAABB( b2WorldId worldId, b2AABB aabb, b2QueryFilter filter, b2OverlapResultFcn* fcn, void* context ); + +/// Overlap test for for all shapes that overlap the provided circle +B2_API void b2World_OverlapCircle( b2WorldId worldId, const b2Circle* circle, b2Transform transform, b2QueryFilter filter, + b2OverlapResultFcn* fcn, void* context ); + +/// Overlap test for all shapes that overlap the provided capsule +B2_API void b2World_OverlapCapsule( b2WorldId worldId, const b2Capsule* capsule, b2Transform transform, b2QueryFilter filter, + b2OverlapResultFcn* fcn, void* context ); + +/// Overlap test for all shapes that overlap the provided polygon +B2_API void b2World_OverlapPolygon( b2WorldId worldId, const b2Polygon* polygon, b2Transform transform, b2QueryFilter filter, + b2OverlapResultFcn* fcn, void* context ); + +/// Cast a ray into the world to collect shapes in the path of the ray. +/// Your callback function controls whether you get the closest point, any point, or n-points. +/// The ray-cast ignores shapes that contain the starting point. +/// @param worldId The world to cast the ray against +/// @param origin The start point of the ray +/// @param translation The translation of the ray from the start point to the end point +/// @param filter Contains bit flags to filter unwanted shapes from the results +/// @param fcn A user implemented callback function +/// @param context A user context that is passed along to the callback function +/// @note The callback function may receive shapes in any order +B2_API void b2World_CastRay( b2WorldId worldId, b2Vec2 origin, b2Vec2 translation, b2QueryFilter filter, b2CastResultFcn* fcn, + void* context ); + +/// Cast a ray into the world to collect the closest hit. This is a convenience function. +/// This is less general than b2World_CastRay() and does not allow for custom filtering. +B2_API b2RayResult b2World_CastRayClosest( b2WorldId worldId, b2Vec2 origin, b2Vec2 translation, b2QueryFilter filter ); + +/// Cast a circle through the world. Similar to a cast ray except that a circle is cast instead of a point. +B2_API void b2World_CastCircle( b2WorldId worldId, const b2Circle* circle, b2Transform originTransform, b2Vec2 translation, + b2QueryFilter filter, b2CastResultFcn* fcn, void* context ); + +/// Cast a capsule through the world. Similar to a cast ray except that a capsule is cast instead of a point. +B2_API void b2World_CastCapsule( b2WorldId worldId, const b2Capsule* capsule, b2Transform originTransform, b2Vec2 translation, + b2QueryFilter filter, b2CastResultFcn* fcn, void* context ); + +/// Cast a polygon through the world. Similar to a cast ray except that a polygon is cast instead of a point. +B2_API void b2World_CastPolygon( b2WorldId worldId, const b2Polygon* polygon, b2Transform originTransform, b2Vec2 translation, + b2QueryFilter filter, b2CastResultFcn* fcn, void* context ); + +/// Enable/disable sleep. If your application does not need sleeping, you can gain some performance +/// by disabling sleep completely at the world level. +/// @see b2WorldDef +B2_API void b2World_EnableSleeping( b2WorldId worldId, bool flag ); + +/// Is body sleeping enabled? +B2_API bool b2World_IsSleepingEnabled( b2WorldId worldId ); + +/// Enable/disable continuous collision between dynamic and static bodies. Generally you should keep continuous +/// collision enabled to prevent fast moving objects from going through static objects. The performance gain from +/// disabling continuous collision is minor. +/// @see b2WorldDef +B2_API void b2World_EnableContinuous( b2WorldId worldId, bool flag ); + +/// Is continuous collision enabled? +B2_API bool b2World_IsContinuousEnabled( b2WorldId worldId ); + +/// Adjust the restitution threshold. It is recommended not to make this value very small +/// because it will prevent bodies from sleeping. Typically in meters per second. +/// @see b2WorldDef +B2_API void b2World_SetRestitutionThreshold( b2WorldId worldId, float value ); + +/// Get the the restitution speed threshold. Typically in meters per second. +B2_API float b2World_GetRestitutionThreshold( b2WorldId worldId ); + +/// Adjust the hit event threshold. This controls the collision velocity needed to generate a b2ContactHitEvent. +/// Typically in meters per second. +/// @see b2WorldDef::hitEventThreshold +B2_API void b2World_SetHitEventThreshold( b2WorldId worldId, float value ); + +/// Get the the hit event speed threshold. Typically in meters per second. +B2_API float b2World_GetHitEventThreshold( b2WorldId worldId ); + +/// Register the custom filter callback. This is optional. +B2_API void b2World_SetCustomFilterCallback( b2WorldId worldId, b2CustomFilterFcn* fcn, void* context ); + +/// Register the pre-solve callback. This is optional. +B2_API void b2World_SetPreSolveCallback( b2WorldId worldId, b2PreSolveFcn* fcn, void* context ); + +/// Set the gravity vector for the entire world. Box2D has no concept of an up direction and this +/// is left as a decision for the application. Typically in m/s^2. +/// @see b2WorldDef +B2_API void b2World_SetGravity( b2WorldId worldId, b2Vec2 gravity ); + +/// Get the gravity vector +B2_API b2Vec2 b2World_GetGravity( b2WorldId worldId ); + +/// Apply a radial explosion +/// @param worldId The world id +/// @param position The center of the explosion +/// @param radius The radius of the explosion +/// @param impulse The impulse of the explosion, typically in kg * m / s or N * s. +B2_API void b2World_Explode( b2WorldId worldId, b2Vec2 position, float radius, float impulse ); + +/// Adjust contact tuning parameters +/// @param worldId The world id +/// @param hertz The contact stiffness (cycles per second) +/// @param dampingRatio The contact bounciness with 1 being critical damping (non-dimensional) +/// @param pushVelocity The maximum contact constraint push out velocity (meters per second) +/// @note Advanced feature +B2_API void b2World_SetContactTuning( b2WorldId worldId, float hertz, float dampingRatio, float pushVelocity ); + +/// Enable/disable constraint warm starting. Advanced feature for testing. Disabling +/// sleeping greatly reduces stability and provides no performance gain. +B2_API void b2World_EnableWarmStarting( b2WorldId worldId, bool flag ); + +/// Is constraint warm starting enabled? +B2_API bool b2World_IsWarmStartingEnabled( b2WorldId worldId ); + +/// Get the current world performance profile +B2_API b2Profile b2World_GetProfile( b2WorldId worldId ); + +/// Get world counters and sizes +B2_API b2Counters b2World_GetCounters( b2WorldId worldId ); + +/// Dump memory stats to box2d_memory.txt +B2_API void b2World_DumpMemoryStats( b2WorldId worldId ); + +/** @} */ + +/** + * @defgroup body Body + * This is the body API. + * @{ + */ + +/// Create a rigid body given a definition. No reference to the definition is retained. So you can create the definition +/// on the stack and pass it as a pointer. +/// @code{.c} +/// b2BodyDef bodyDef = b2DefaultBodyDef(); +/// b2BodyId myBodyId = b2CreateBody(myWorldId, &bodyDef); +/// @endcode +/// @warning This function is locked during callbacks. +B2_API b2BodyId b2CreateBody( b2WorldId worldId, const b2BodyDef* def ); + +/// Destroy a rigid body given an id. This destroys all shapes and joints attached to the body. +/// Do not keep references to the associated shapes and joints. +B2_API void b2DestroyBody( b2BodyId bodyId ); + +/// Body identifier validation. Can be used to detect orphaned ids. Provides validation for up to 64K allocations. +B2_API bool b2Body_IsValid( b2BodyId id ); + +/// Get the body type: static, kinematic, or dynamic +B2_API b2BodyType b2Body_GetType( b2BodyId bodyId ); + +/// Change the body type. This is an expensive operation. This automatically updates the mass +/// properties regardless of the automatic mass setting. +B2_API void b2Body_SetType( b2BodyId bodyId, b2BodyType type ); + +/// Set the user data for a body +B2_API void b2Body_SetUserData( b2BodyId bodyId, void* userData ); + +/// Get the user data stored in a body +B2_API void* b2Body_GetUserData( b2BodyId bodyId ); + +/// Get the world position of a body. This is the location of the body origin. +B2_API b2Vec2 b2Body_GetPosition( b2BodyId bodyId ); + +/// Get the world rotation of a body as a cosine/sine pair (complex number) +B2_API b2Rot b2Body_GetRotation( b2BodyId bodyId ); + +/// Get the world transform of a body. +B2_API b2Transform b2Body_GetTransform( b2BodyId bodyId ); + +/// Set the world transform of a body. This acts as a teleport and is fairly expensive. +/// @note Generally you should create a body with then intended transform. +/// @see b2BodyDef::position and b2BodyDef::angle +B2_API void b2Body_SetTransform( b2BodyId bodyId, b2Vec2 position, b2Rot rotation ); + +/// Get a local point on a body given a world point +B2_API b2Vec2 b2Body_GetLocalPoint( b2BodyId bodyId, b2Vec2 worldPoint ); + +/// Get a world point on a body given a local point +B2_API b2Vec2 b2Body_GetWorldPoint( b2BodyId bodyId, b2Vec2 localPoint ); + +/// Get a local vector on a body given a world vector +B2_API b2Vec2 b2Body_GetLocalVector( b2BodyId bodyId, b2Vec2 worldVector ); + +/// Get a world vector on a body given a local vector +B2_API b2Vec2 b2Body_GetWorldVector( b2BodyId bodyId, b2Vec2 localVector ); + +/// Get the linear velocity of a body's center of mass. Typically in meters per second. +B2_API b2Vec2 b2Body_GetLinearVelocity( b2BodyId bodyId ); + +/// Get the angular velocity of a body in radians per second +B2_API float b2Body_GetAngularVelocity( b2BodyId bodyId ); + +/// Set the linear velocity of a body. Typically in meters per second. +B2_API void b2Body_SetLinearVelocity( b2BodyId bodyId, b2Vec2 linearVelocity ); + +/// Set the angular velocity of a body in radians per second +B2_API void b2Body_SetAngularVelocity( b2BodyId bodyId, float angularVelocity ); + +/// Apply a force at a world point. If the force is not applied at the center of mass, +/// it will generate a torque and affect the angular velocity. This optionally wakes up the body. +/// The force is ignored if the body is not awake. +/// @param bodyId The body id +/// @param force The world force vector, typically in newtons (N) +/// @param point The world position of the point of application +/// @param wake Option to wake up the body +B2_API void b2Body_ApplyForce( b2BodyId bodyId, b2Vec2 force, b2Vec2 point, bool wake ); + +/// Apply a force to the center of mass. This optionally wakes up the body. +/// The force is ignored if the body is not awake. +/// @param bodyId The body id +/// @param force the world force vector, usually in newtons (N). +/// @param wake also wake up the body +B2_API void b2Body_ApplyForceToCenter( b2BodyId bodyId, b2Vec2 force, bool wake ); + +/// Apply a torque. This affects the angular velocity without affecting the linear velocity. +/// This optionally wakes the body. The torque is ignored if the body is not awake. +/// @param bodyId The body id +/// @param torque about the z-axis (out of the screen), typically in N*m. +/// @param wake also wake up the body +B2_API void b2Body_ApplyTorque( b2BodyId bodyId, float torque, bool wake ); + +/// Apply an impulse at a point. This immediately modifies the velocity. +/// It also modifies the angular velocity if the point of application +/// is not at the center of mass. This optionally wakes the body. +/// The impulse is ignored if the body is not awake. +/// @param bodyId The body id +/// @param impulse the world impulse vector, typically in N*s or kg*m/s. +/// @param point the world position of the point of application. +/// @param wake also wake up the body +/// @warning This should be used for one-shot impulses. If you need a steady force, +/// use a force instead, which will work better with the sub-stepping solver. +B2_API void b2Body_ApplyLinearImpulse( b2BodyId bodyId, b2Vec2 impulse, b2Vec2 point, bool wake ); + +/// Apply an impulse to the center of mass. This immediately modifies the velocity. +/// The impulse is ignored if the body is not awake. This optionally wakes the body. +/// @param bodyId The body id +/// @param impulse the world impulse vector, typically in N*s or kg*m/s. +/// @param wake also wake up the body +/// @warning This should be used for one-shot impulses. If you need a steady force, +/// use a force instead, which will work better with the sub-stepping solver. +B2_API void b2Body_ApplyLinearImpulseToCenter( b2BodyId bodyId, b2Vec2 impulse, bool wake ); + +/// Apply an angular impulse. The impulse is ignored if the body is not awake. +/// This optionally wakes the body. +/// @param bodyId The body id +/// @param impulse the angular impulse, typically in units of kg*m*m/s +/// @param wake also wake up the body +/// @warning This should be used for one-shot impulses. If you need a steady force, +/// use a force instead, which will work better with the sub-stepping solver. +B2_API void b2Body_ApplyAngularImpulse( b2BodyId bodyId, float impulse, bool wake ); + +/// Get the mass of the body, typically in kilograms +B2_API float b2Body_GetMass( b2BodyId bodyId ); + +/// Get the rotational inertia of the body, typically in kg*m^2 +B2_API float b2Body_GetRotationalInertia( b2BodyId bodyId ); + +/// Get the center of mass position of the body in local space +B2_API b2Vec2 b2Body_GetLocalCenterOfMass( b2BodyId bodyId ); + +/// Get the center of mass position of the body in world space +B2_API b2Vec2 b2Body_GetWorldCenterOfMass( b2BodyId bodyId ); + +/// Override the body's mass properties. Normally this is computed automatically using the +/// shape geometry and density. This information is lost if a shape is added or removed or if the +/// body type changes. +B2_API void b2Body_SetMassData( b2BodyId bodyId, b2MassData massData ); + +/// Get the mass data for a body +B2_API b2MassData b2Body_GetMassData( b2BodyId bodyId ); + +/// This update the mass properties to the sum of the mass properties of the shapes. +/// This normally does not need to be called unless you called SetMassData to override +/// the mass and you later want to reset the mass. +/// You may also use this when automatic mass computation has been disabled. +/// You should call this regardless of body type. +B2_API void b2Body_ApplyMassFromShapes( b2BodyId bodyId ); + +/// Set the automatic mass setting. Normally this is set in b2BodyDef before creation. +/// @see b2BodyDef::automaticMass +B2_API void b2Body_SetAutomaticMass( b2BodyId bodyId, bool automaticMass ); + +/// Get the automatic mass setting +B2_API bool b2Body_GetAutomaticMass( b2BodyId bodyId ); + +/// Adjust the linear damping. Normally this is set in b2BodyDef before creation. +B2_API void b2Body_SetLinearDamping( b2BodyId bodyId, float linearDamping ); + +/// Get the current linear damping. +B2_API float b2Body_GetLinearDamping( b2BodyId bodyId ); + +/// Adjust the angular damping. Normally this is set in b2BodyDef before creation. +B2_API void b2Body_SetAngularDamping( b2BodyId bodyId, float angularDamping ); + +/// Get the current angular damping. +B2_API float b2Body_GetAngularDamping( b2BodyId bodyId ); + +/// Adjust the gravity scale. Normally this is set in b2BodyDef before creation. +/// @see b2BodyDef::gravityScale +B2_API void b2Body_SetGravityScale( b2BodyId bodyId, float gravityScale ); + +/// Get the current gravity scale +B2_API float b2Body_GetGravityScale( b2BodyId bodyId ); + +/// @return true if this body is awake +B2_API bool b2Body_IsAwake( b2BodyId bodyId ); + +/// Wake a body from sleep. This wakes the entire island the body is touching. +/// @warning Putting a body to sleep will put the entire island of bodies touching this body to sleep, +/// which can be expensive and possibly unintuitive. +B2_API void b2Body_SetAwake( b2BodyId bodyId, bool awake ); + +/// Enable or disable sleeping for this body. If sleeping is disabled the body will wake. +B2_API void b2Body_EnableSleep( b2BodyId bodyId, bool enableSleep ); + +/// Returns true if sleeping is enabled for this body +B2_API bool b2Body_IsSleepEnabled( b2BodyId bodyId ); + +/// Set the sleep threshold, typically in meters per second +B2_API void b2Body_SetSleepThreshold( b2BodyId bodyId, float sleepThreshold ); + +/// Get the sleep threshold, typically in meters per second. +B2_API float b2Body_GetSleepThreshold( b2BodyId bodyId ); + +/// Returns true if this body is enabled +B2_API bool b2Body_IsEnabled( b2BodyId bodyId ); + +/// Disable a body by removing it completely from the simulation. This is expensive. +B2_API void b2Body_Disable( b2BodyId bodyId ); + +/// Enable a body by adding it to the simulation. This is expensive. +B2_API void b2Body_Enable( b2BodyId bodyId ); + +/// Set this body to have fixed rotation. This causes the mass to be reset in all cases. +B2_API void b2Body_SetFixedRotation( b2BodyId bodyId, bool flag ); + +/// Does this body have fixed rotation? +B2_API bool b2Body_IsFixedRotation( b2BodyId bodyId ); + +/// Set this body to be a bullet. A bullet does continuous collision detection +/// against dynamic bodies (but not other bullets). +B2_API void b2Body_SetBullet( b2BodyId bodyId, bool flag ); + +/// Is this body a bullet? +B2_API bool b2Body_IsBullet( b2BodyId bodyId ); + +/// Enable/disable hit events on all shapes +/// @see b2ShapeDef::enableHitEvents +B2_API void b2Body_EnableHitEvents( b2BodyId bodyId, bool enableHitEvents ); + +/// Get the world that owns this body +B2_API b2WorldId b2Body_GetWorld( b2BodyId bodyId ); + +/// Get the number of shapes on this body +B2_API int b2Body_GetShapeCount( b2BodyId bodyId ); + +/// Get the shape ids for all shapes on this body, up to the provided capacity. +/// @returns the number of shape ids stored in the user array +B2_API int b2Body_GetShapes( b2BodyId bodyId, b2ShapeId* shapeArray, int capacity ); + +/// Get the number of joints on this body +B2_API int b2Body_GetJointCount( b2BodyId bodyId ); + +/// Get the joint ids for all joints on this body, up to the provided capacity +/// @returns the number of joint ids stored in the user array +B2_API int b2Body_GetJoints( b2BodyId bodyId, b2JointId* jointArray, int capacity ); + +/// Get the maximum capacity required for retrieving all the touching contacts on a body +B2_API int b2Body_GetContactCapacity( b2BodyId bodyId ); + +/// Get the touching contact data for a body +B2_API int b2Body_GetContactData( b2BodyId bodyId, b2ContactData* contactData, int capacity ); + +/// Get the current world AABB that contains all the attached shapes. Note that this may not encompass the body origin. +/// If there are no shapes attached then the returned AABB is empty and centered on the body origin. +B2_API b2AABB b2Body_ComputeAABB( b2BodyId bodyId ); + +/** @} */ + +/** + * @defgroup shape Shape + * Functions to create, destroy, and access. + * Shapes bind raw geometry to bodies and hold material properties including friction and restitution. + * @{ + */ + +/// Create a circle shape and attach it to a body. The shape definition and geometry are fully cloned. +/// Contacts are not created until the next time step. +/// @return the shape id for accessing the shape +B2_API b2ShapeId b2CreateCircleShape( b2BodyId bodyId, const b2ShapeDef* def, const b2Circle* circle ); + +/// Create a line segment shape and attach it to a body. The shape definition and geometry are fully cloned. +/// Contacts are not created until the next time step. +/// @return the shape id for accessing the shape +B2_API b2ShapeId b2CreateSegmentShape( b2BodyId bodyId, const b2ShapeDef* def, const b2Segment* segment ); + +/// Create a capsule shape and attach it to a body. The shape definition and geometry are fully cloned. +/// Contacts are not created until the next time step. +/// @return the shape id for accessing the shape +B2_API b2ShapeId b2CreateCapsuleShape( b2BodyId bodyId, const b2ShapeDef* def, const b2Capsule* capsule ); + +/// Create a polygon shape and attach it to a body. The shape definition and geometry are fully cloned. +/// Contacts are not created until the next time step. +/// @return the shape id for accessing the shape +B2_API b2ShapeId b2CreatePolygonShape( b2BodyId bodyId, const b2ShapeDef* def, const b2Polygon* polygon ); + +/// Destroy a shape +B2_API void b2DestroyShape( b2ShapeId shapeId ); + +/// Shape identifier validation. Provides validation for up to 64K allocations. +B2_API bool b2Shape_IsValid( b2ShapeId id ); + +/// Get the type of a shape +B2_API b2ShapeType b2Shape_GetType( b2ShapeId shapeId ); + +/// Get the id of the body that a shape is attached to +B2_API b2BodyId b2Shape_GetBody( b2ShapeId shapeId ); + +/// Get the world that owns this shape +B2_API b2WorldId b2Shape_GetWorld( b2ShapeId shapeId ); + +/// Returns true If the shape is a sensor +B2_API bool b2Shape_IsSensor( b2ShapeId shapeId ); + +/// Set the user data for a shape +B2_API void b2Shape_SetUserData( b2ShapeId shapeId, void* userData ); + +/// Get the user data for a shape. This is useful when you get a shape id +/// from an event or query. +B2_API void* b2Shape_GetUserData( b2ShapeId shapeId ); + +/// Set the mass density of a shape, typically in kg/m^2. +/// This will not update the mass properties on the parent body. +/// @see b2ShapeDef::density, b2Body_ApplyMassFromShapes +B2_API void b2Shape_SetDensity( b2ShapeId shapeId, float density ); + +/// Get the density of a shape, typically in kg/m^2 +B2_API float b2Shape_GetDensity( b2ShapeId shapeId ); + +/// Set the friction on a shape +/// @see b2ShapeDef::friction +B2_API void b2Shape_SetFriction( b2ShapeId shapeId, float friction ); + +/// Get the friction of a shape +B2_API float b2Shape_GetFriction( b2ShapeId shapeId ); + +/// Set the shape restitution (bounciness) +/// @see b2ShapeDef::restitution +B2_API void b2Shape_SetRestitution( b2ShapeId shapeId, float restitution ); + +/// Get the shape restitution +B2_API float b2Shape_GetRestitution( b2ShapeId shapeId ); + +/// Get the shape filter +B2_API b2Filter b2Shape_GetFilter( b2ShapeId shapeId ); + +/// Set the current filter. This is almost as expensive as recreating the shape. +/// @see b2ShapeDef::filter +B2_API void b2Shape_SetFilter( b2ShapeId shapeId, b2Filter filter ); + +/// Enable sensor events for this shape. Only applies to kinematic and dynamic bodies. Ignored for sensors. +/// @see b2ShapeDef::isSensor +B2_API void b2Shape_EnableSensorEvents( b2ShapeId shapeId, bool flag ); + +/// Returns true if sensor events are enabled +B2_API bool b2Shape_AreSensorEventsEnabled( b2ShapeId shapeId ); + +/// Enable contact events for this shape. Only applies to kinematic and dynamic bodies. Ignored for sensors. +/// @see b2ShapeDef::enableContactEvents +B2_API void b2Shape_EnableContactEvents( b2ShapeId shapeId, bool flag ); + +/// Returns true if contact events are enabled +B2_API bool b2Shape_AreContactEventsEnabled( b2ShapeId shapeId ); + +/// Enable pre-solve contact events for this shape. Only applies to dynamic bodies. These are expensive +/// and must be carefully handled due to multithreading. Ignored for sensors. +/// @see b2PreSolveFcn +B2_API void b2Shape_EnablePreSolveEvents( b2ShapeId shapeId, bool flag ); + +/// Returns true if pre-solve events are enabled +B2_API bool b2Shape_ArePreSolveEventsEnabled( b2ShapeId shapeId ); + +/// Enable contact hit events for this shape. Ignored for sensors. +/// @see b2WorldDef.hitEventThreshold +B2_API void b2Shape_EnableHitEvents( b2ShapeId shapeId, bool flag ); + +/// Returns true if hit events are enabled +B2_API bool b2Shape_AreHitEventsEnabled( b2ShapeId shapeId ); + +/// Test a point for overlap with a shape +B2_API bool b2Shape_TestPoint( b2ShapeId shapeId, b2Vec2 point ); + +/// Ray cast a shape directly +B2_API b2CastOutput b2Shape_RayCast( b2ShapeId shapeId, const b2RayCastInput* input ); + +/// Get a copy of the shape's circle. Asserts the type is correct. +B2_API b2Circle b2Shape_GetCircle( b2ShapeId shapeId ); + +/// Get a copy of the shape's line segment. Asserts the type is correct. +B2_API b2Segment b2Shape_GetSegment( b2ShapeId shapeId ); + +/// Get a copy of the shape's chain segment. These come from chain shapes. +/// Asserts the type is correct. +B2_API b2ChainSegment b2Shape_GetChainSegment( b2ShapeId shapeId ); + +/// Get a copy of the shape's capsule. Asserts the type is correct. +B2_API b2Capsule b2Shape_GetCapsule( b2ShapeId shapeId ); + +/// Get a copy of the shape's convex polygon. Asserts the type is correct. +B2_API b2Polygon b2Shape_GetPolygon( b2ShapeId shapeId ); + +/// Allows you to change a shape to be a circle or update the current circle. +/// This does not modify the mass properties. +/// @see b2Body_ApplyMassFromShapes +B2_API void b2Shape_SetCircle( b2ShapeId shapeId, const b2Circle* circle ); + +/// Allows you to change a shape to be a capsule or update the current capsule. +/// This does not modify the mass properties. +/// @see b2Body_ApplyMassFromShapes +B2_API void b2Shape_SetCapsule( b2ShapeId shapeId, const b2Capsule* capsule ); + +/// Allows you to change a shape to be a segment or update the current segment. +B2_API void b2Shape_SetSegment( b2ShapeId shapeId, const b2Segment* segment ); + +/// Allows you to change a shape to be a polygon or update the current polygon. +/// This does not modify the mass properties. +/// @see b2Body_ApplyMassFromShapes +B2_API void b2Shape_SetPolygon( b2ShapeId shapeId, const b2Polygon* polygon ); + +/// Get the parent chain id if the shape type is a chain segment, otherwise +/// returns b2_nullChainId. +B2_API b2ChainId b2Shape_GetParentChain( b2ShapeId shapeId ); + +/// Get the maximum capacity required for retrieving all the touching contacts on a shape +B2_API int b2Shape_GetContactCapacity( b2ShapeId shapeId ); + +/// Get the touching contact data for a shape. The provided shapeId will be either shapeIdA or shapeIdB on the contact data. +B2_API int b2Shape_GetContactData( b2ShapeId shapeId, b2ContactData* contactData, int capacity ); + +/// Get the current world AABB +B2_API b2AABB b2Shape_GetAABB( b2ShapeId shapeId ); + +/// Get the closest point on a shape to a target point. Target and result are in world space. +B2_API b2Vec2 b2Shape_GetClosestPoint( b2ShapeId shapeId, b2Vec2 target ); + +/// Chain Shape + +/// Create a chain shape +/// @see b2ChainDef for details +B2_API b2ChainId b2CreateChain( b2BodyId bodyId, const b2ChainDef* def ); + +/// Destroy a chain shape +B2_API void b2DestroyChain( b2ChainId chainId ); + +/// Get the world that owns this chain shape +B2_API b2WorldId b2Chain_GetWorld( b2ChainId chainId ); + +/// Get the number of segments on this chain +B2_API int b2Chain_GetSegmentCount( b2ChainId chainId ); + +/// Fill a user array with chain segment shape ids up to the specified capacity. Returns +/// the actual number of segments returned. +B2_API int b2Chain_GetSegments( b2ChainId chainId, b2ShapeId* segmentArray, int capacity ); + +/// Set the chain friction +/// @see b2ChainDef::friction +B2_API void b2Chain_SetFriction( b2ChainId chainId, float friction ); + +/// Set the chain restitution (bounciness) +/// @see b2ChainDef::restitution +B2_API void b2Chain_SetRestitution( b2ChainId chainId, float restitution ); + +/// Chain identifier validation. Provides validation for up to 64K allocations. +B2_API bool b2Chain_IsValid( b2ChainId id ); + +/** @} */ + +/** + * @defgroup joint Joint + * @brief Joints allow you to connect rigid bodies together while allowing various forms of relative motions. + * @{ + */ + +/// Destroy a joint +B2_API void b2DestroyJoint( b2JointId jointId ); + +/// Joint identifier validation. Provides validation for up to 64K allocations. +B2_API bool b2Joint_IsValid( b2JointId id ); + +/// Get the joint type +B2_API b2JointType b2Joint_GetType( b2JointId jointId ); + +/// Get body A id on a joint +B2_API b2BodyId b2Joint_GetBodyA( b2JointId jointId ); + +/// Get body B id on a joint +B2_API b2BodyId b2Joint_GetBodyB( b2JointId jointId ); + +/// Get the world that owns this joint +B2_API b2WorldId b2Joint_GetWorld( b2JointId jointId ); + +/// Get the local anchor on bodyA +B2_API b2Vec2 b2Joint_GetLocalAnchorA( b2JointId jointId ); + +/// Get the local anchor on bodyB +B2_API b2Vec2 b2Joint_GetLocalAnchorB( b2JointId jointId ); + +/// Toggle collision between connected bodies +B2_API void b2Joint_SetCollideConnected( b2JointId jointId, bool shouldCollide ); + +/// Is collision allowed between connected bodies? +B2_API bool b2Joint_GetCollideConnected( b2JointId jointId ); + +/// Set the user data on a joint +B2_API void b2Joint_SetUserData( b2JointId jointId, void* userData ); + +/// Get the user data on a joint +B2_API void* b2Joint_GetUserData( b2JointId jointId ); + +/// Wake the bodies connect to this joint +B2_API void b2Joint_WakeBodies( b2JointId jointId ); + +/// Get the current constraint force for this joint +B2_API b2Vec2 b2Joint_GetConstraintForce( b2JointId jointId ); + +/// Get the current constraint torque for this joint +B2_API float b2Joint_GetConstraintTorque( b2JointId jointId ); + +/** + * @defgroup distance_joint Distance Joint + * @brief Functions for the distance joint. + * @{ + */ + +/// Create a distance joint +/// @see b2DistanceJointDef for details +B2_API b2JointId b2CreateDistanceJoint( b2WorldId worldId, const b2DistanceJointDef* def ); + +/// Set the rest length of a distance joint +/// @param jointId The id for a distance joint +/// @param length The new distance joint length +B2_API void b2DistanceJoint_SetLength( b2JointId jointId, float length ); + +/// Get the rest length of a distance joint +B2_API float b2DistanceJoint_GetLength( b2JointId jointId ); + +/// Enable/disable the distance joint spring. When disabled the distance joint is rigid. +B2_API void b2DistanceJoint_EnableSpring( b2JointId jointId, bool enableSpring ); + +/// Is the distance joint spring enabled? +B2_API bool b2DistanceJoint_IsSpringEnabled( b2JointId jointId ); + +/// Set the spring stiffness in Hertz +B2_API void b2DistanceJoint_SetSpringHertz( b2JointId jointId, float hertz ); + +/// Set the spring damping ratio, non-dimensional +B2_API void b2DistanceJoint_SetSpringDampingRatio( b2JointId jointId, float dampingRatio ); + +/// Get the spring Hertz +B2_API float b2DistanceJoint_GetSpringHertz( b2JointId jointId ); + +/// Get the spring damping ratio +B2_API float b2DistanceJoint_GetSpringDampingRatio( b2JointId jointId ); + +/// Enable joint limit. The limit only works if the joint spring is enabled. Otherwise the joint is rigid +/// and the limit has no effect. +B2_API void b2DistanceJoint_EnableLimit( b2JointId jointId, bool enableLimit ); + +/// Is the distance joint limit enabled? +B2_API bool b2DistanceJoint_IsLimitEnabled( b2JointId jointId ); + +/// Set the minimum and maximum length parameters of a distance joint +B2_API void b2DistanceJoint_SetLengthRange( b2JointId jointId, float minLength, float maxLength ); + +/// Get the distance joint minimum length +B2_API float b2DistanceJoint_GetMinLength( b2JointId jointId ); + +/// Get the distance joint maximum length +B2_API float b2DistanceJoint_GetMaxLength( b2JointId jointId ); + +/// Get the current length of a distance joint +B2_API float b2DistanceJoint_GetCurrentLength( b2JointId jointId ); + +/// Enable/disable the distance joint motor +B2_API void b2DistanceJoint_EnableMotor( b2JointId jointId, bool enableMotor ); + +/// Is the distance joint motor enabled? +B2_API bool b2DistanceJoint_IsMotorEnabled( b2JointId jointId ); + +/// Set the distance joint motor speed, typically in meters per second +B2_API void b2DistanceJoint_SetMotorSpeed( b2JointId jointId, float motorSpeed ); + +/// Get the distance joint motor speed, typically in meters per second +B2_API float b2DistanceJoint_GetMotorSpeed( b2JointId jointId ); + +/// Set the distance joint maximum motor force, typically in newtons +B2_API void b2DistanceJoint_SetMaxMotorForce( b2JointId jointId, float force ); + +/// Get the distance joint maximum motor force, typically in newtons +B2_API float b2DistanceJoint_GetMaxMotorForce( b2JointId jointId ); + +/// Get the distance joint current motor force, typically in newtons +B2_API float b2DistanceJoint_GetMotorForce( b2JointId jointId ); + +/** @} */ + +/** + * @defgroup motor_joint Motor Joint + * @brief Functions for the motor joint. + * + * The motor joint is used to drive the relative transform between two bodies. It takes + * a relative position and rotation and applies the forces and torques needed to achieve + * that relative transform over time. + * @{ + */ + +/// Create a motor joint +/// @see b2MotorJointDef for details +B2_API b2JointId b2CreateMotorJoint( b2WorldId worldId, const b2MotorJointDef* def ); + +/// Set the motor joint linear offset target +B2_API void b2MotorJoint_SetLinearOffset( b2JointId jointId, b2Vec2 linearOffset ); + +/// Get the motor joint linear offset target +B2_API b2Vec2 b2MotorJoint_GetLinearOffset( b2JointId jointId ); + +/// Set the motor joint angular offset target in radians +B2_API void b2MotorJoint_SetAngularOffset( b2JointId jointId, float angularOffset ); + +/// Get the motor joint angular offset target in radians +B2_API float b2MotorJoint_GetAngularOffset( b2JointId jointId ); + +/// Set the motor joint maximum force, typically in newtons +B2_API void b2MotorJoint_SetMaxForce( b2JointId jointId, float maxForce ); + +/// Get the motor joint maximum force, typically in newtons +B2_API float b2MotorJoint_GetMaxForce( b2JointId jointId ); + +/// Set the motor joint maximum torque, typically in newton-meters +B2_API void b2MotorJoint_SetMaxTorque( b2JointId jointId, float maxTorque ); + +/// Get the motor joint maximum torque, typically in newton-meters +B2_API float b2MotorJoint_GetMaxTorque( b2JointId jointId ); + +/// Set the motor joint correction factor, typically in [0, 1] +B2_API void b2MotorJoint_SetCorrectionFactor( b2JointId jointId, float correctionFactor ); + +/// Get the motor joint correction factor, typically in [0, 1] +B2_API float b2MotorJoint_GetCorrectionFactor( b2JointId jointId ); + +/**@}*/ + +/** + * @defgroup mouse_joint Mouse Joint + * @brief Functions for the mouse joint. + * + * The mouse joint is designed for use in the samples application, but you may find it useful in applications where + * the user moves a rigid body with a cursor. + * @{ + */ + +/// Create a mouse joint +/// @see b2MouseJointDef for details +B2_API b2JointId b2CreateMouseJoint( b2WorldId worldId, const b2MouseJointDef* def ); + +/// Set the mouse joint target +B2_API void b2MouseJoint_SetTarget( b2JointId jointId, b2Vec2 target ); + +/// Get the mouse joint target +B2_API b2Vec2 b2MouseJoint_GetTarget( b2JointId jointId ); + +/// Set the mouse joint spring stiffness in Hertz +B2_API void b2MouseJoint_SetSpringHertz( b2JointId jointId, float hertz ); + +/// Get the mouse joint spring stiffness in Hertz +B2_API float b2MouseJoint_GetSpringHertz( b2JointId jointId ); + +/// Set the mouse joint spring damping ratio, non-dimensional +B2_API void b2MouseJoint_SetSpringDampingRatio( b2JointId jointId, float dampingRatio ); + +/// Get the mouse joint damping ratio, non-dimensional +B2_API float b2MouseJoint_GetSpringDampingRatio( b2JointId jointId ); + +/// Set the mouse joint maximum force, typically in newtons +B2_API void b2MouseJoint_SetMaxForce( b2JointId jointId, float maxForce ); + +/// Get the mouse joint maximum force, typically in newtons +B2_API float b2MouseJoint_GetMaxForce( b2JointId jointId ); + +/**@}*/ + +/** + * @defgroup prismatic_joint Prismatic Joint + * @brief A prismatic joint allows for translation along a single axis with no rotation. + * + * The prismatic joint is useful for things like pistons and moving platforms, where you want a body to translate + * along an axis and have no rotation. Also called a *slider* joint. + * @{ + */ + +/// Create a prismatic (slider) joint. +/// @see b2PrismaticJointDef for details +B2_API b2JointId b2CreatePrismaticJoint( b2WorldId worldId, const b2PrismaticJointDef* def ); + +/// Enable/disable the joint spring. +B2_API void b2PrismaticJoint_EnableSpring( b2JointId jointId, bool enableSpring ); + +/// Is the prismatic joint spring enabled or not? +B2_API bool b2PrismaticJoint_IsSpringEnabled( b2JointId jointId ); + +/// Set the prismatic joint stiffness in Hertz. +/// This should usually be less than a quarter of the simulation rate. For example, if the simulation +/// runs at 60Hz then the joint stiffness should be 15Hz or less. +B2_API void b2PrismaticJoint_SetSpringHertz( b2JointId jointId, float hertz ); + +/// Get the prismatic joint stiffness in Hertz +B2_API float b2PrismaticJoint_GetSpringHertz( b2JointId jointId ); + +/// Set the prismatic joint damping ratio (non-dimensional) +B2_API void b2PrismaticJoint_SetSpringDampingRatio( b2JointId jointId, float dampingRatio ); + +/// Get the prismatic spring damping ratio (non-dimensional) +B2_API float b2PrismaticJoint_GetSpringDampingRatio( b2JointId jointId ); + +/// Enable/disable a prismatic joint limit +B2_API void b2PrismaticJoint_EnableLimit( b2JointId jointId, bool enableLimit ); + +/// Is the prismatic joint limit enabled? +B2_API bool b2PrismaticJoint_IsLimitEnabled( b2JointId jointId ); + +/// Get the prismatic joint lower limit +B2_API float b2PrismaticJoint_GetLowerLimit( b2JointId jointId ); + +/// Get the prismatic joint upper limit +B2_API float b2PrismaticJoint_GetUpperLimit( b2JointId jointId ); + +/// Set the prismatic joint limits +B2_API void b2PrismaticJoint_SetLimits( b2JointId jointId, float lower, float upper ); + +/// Enable/disable a prismatic joint motor +B2_API void b2PrismaticJoint_EnableMotor( b2JointId jointId, bool enableMotor ); + +/// Is the prismatic joint motor enabled? +B2_API bool b2PrismaticJoint_IsMotorEnabled( b2JointId jointId ); + +/// Set the prismatic joint motor speed, typically in meters per second +B2_API void b2PrismaticJoint_SetMotorSpeed( b2JointId jointId, float motorSpeed ); + +/// Get the prismatic joint motor speed, typically in meters per second +B2_API float b2PrismaticJoint_GetMotorSpeed( b2JointId jointId ); + +/// Set the prismatic joint maximum motor force, typically in newtons +B2_API void b2PrismaticJoint_SetMaxMotorForce( b2JointId jointId, float force ); + +/// Get the prismatic joint maximum motor force, typically in newtons +B2_API float b2PrismaticJoint_GetMaxMotorForce( b2JointId jointId ); + +/// Get the prismatic joint current motor force, typically in newtons +B2_API float b2PrismaticJoint_GetMotorForce( b2JointId jointId ); + +/** @} */ + +/** + * @defgroup revolute_joint Revolute Joint + * @brief A revolute joint allows for relative rotation in the 2D plane with no relative translation. + * + * The revolute joint is probably the most common joint. It can be used for ragdolls and chains. + * Also called a *hinge* or *pin* joint. + * @{ + */ + +/// Create a revolute joint +/// @see b2RevoluteJointDef for details +B2_API b2JointId b2CreateRevoluteJoint( b2WorldId worldId, const b2RevoluteJointDef* def ); + +/// Enable/disable the revolute joint spring +B2_API void b2RevoluteJoint_EnableSpring( b2JointId jointId, bool enableSpring ); + +/// It the revolute angular spring enabled? +B2_API bool b2RevoluteJoint_IsSpringEnabled( b2JointId jointId ); + +/// Set the revolute joint spring stiffness in Hertz +B2_API void b2RevoluteJoint_SetSpringHertz( b2JointId jointId, float hertz ); + +/// Get the revolute joint spring stiffness in Hertz +B2_API float b2RevoluteJoint_GetSpringHertz( b2JointId jointId ); + +/// Set the revolute joint spring damping ratio, non-dimensional +B2_API void b2RevoluteJoint_SetSpringDampingRatio( b2JointId jointId, float dampingRatio ); + +/// Get the revolute joint spring damping ratio, non-dimensional +B2_API float b2RevoluteJoint_GetSpringDampingRatio( b2JointId jointId ); + +/// Get the revolute joint current angle in radians relative to the reference angle +/// @see b2RevoluteJointDef::referenceAngle +B2_API float b2RevoluteJoint_GetAngle( b2JointId jointId ); + +/// Enable/disable the revolute joint limit +B2_API void b2RevoluteJoint_EnableLimit( b2JointId jointId, bool enableLimit ); + +/// Is the revolute joint limit enabled? +B2_API bool b2RevoluteJoint_IsLimitEnabled( b2JointId jointId ); + +/// Get the revolute joint lower limit in radians +B2_API float b2RevoluteJoint_GetLowerLimit( b2JointId jointId ); + +/// Get the revolute joint upper limit in radians +B2_API float b2RevoluteJoint_GetUpperLimit( b2JointId jointId ); + +/// Set the revolute joint limits in radians +B2_API void b2RevoluteJoint_SetLimits( b2JointId jointId, float lower, float upper ); + +/// Enable/disable a revolute joint motor +B2_API void b2RevoluteJoint_EnableMotor( b2JointId jointId, bool enableMotor ); + +/// Is the revolute joint motor enabled? +B2_API bool b2RevoluteJoint_IsMotorEnabled( b2JointId jointId ); + +/// Set the revolute joint motor speed in radians per second +B2_API void b2RevoluteJoint_SetMotorSpeed( b2JointId jointId, float motorSpeed ); + +/// Get the revolute joint motor speed in radians per second +B2_API float b2RevoluteJoint_GetMotorSpeed( b2JointId jointId ); + +/// Get the revolute joint current motor torque, typically in newton-meters +B2_API float b2RevoluteJoint_GetMotorTorque( b2JointId jointId ); + +/// Set the revolute joint maximum motor torque, typically in newton-meters +B2_API void b2RevoluteJoint_SetMaxMotorTorque( b2JointId jointId, float torque ); + +/// Get the revolute joint maximum motor torque, typically in newton-meters +B2_API float b2RevoluteJoint_GetMaxMotorTorque( b2JointId jointId ); + +/**@}*/ + +/** + * @defgroup weld_joint Weld Joint + * @brief A weld joint fully constrains the relative transform between two bodies while allowing for springiness + * + * A weld joint constrains the relative rotation and translation between two bodies. Both rotation and translation + * can have damped springs. + * + * @note The accuracy of weld joint is limited by the accuracy of the solver. Long chains of weld joints may flex. + * @{ + */ + +/// Create a weld joint +/// @see b2WeldJointDef for details +B2_API b2JointId b2CreateWeldJoint( b2WorldId worldId, const b2WeldJointDef* def ); + +/// Set the weld joint linear stiffness in Hertz. 0 is rigid. +B2_API void b2WeldJoint_SetLinearHertz( b2JointId jointId, float hertz ); + +/// Get the weld joint linear stiffness in Hertz +B2_API float b2WeldJoint_GetLinearHertz( b2JointId jointId ); + +/// Set the weld joint linear damping ratio (non-dimensional) +B2_API void b2WeldJoint_SetLinearDampingRatio( b2JointId jointId, float dampingRatio ); + +/// Get the weld joint linear damping ratio (non-dimensional) +B2_API float b2WeldJoint_GetLinearDampingRatio( b2JointId jointId ); + +/// Set the weld joint angular stiffness in Hertz. 0 is rigid. +B2_API void b2WeldJoint_SetAngularHertz( b2JointId jointId, float hertz ); + +/// Get the weld joint angular stiffness in Hertz +B2_API float b2WeldJoint_GetAngularHertz( b2JointId jointId ); + +/// Set weld joint angular damping ratio, non-dimensional +B2_API void b2WeldJoint_SetAngularDampingRatio( b2JointId jointId, float dampingRatio ); + +/// Get the weld joint angular damping ratio, non-dimensional +B2_API float b2WeldJoint_GetAngularDampingRatio( b2JointId jointId ); + +/** @} */ + +/** + * @defgroup wheel_joint Wheel Joint + * The wheel joint can be used to simulate wheels on vehicles. + * + * The wheel joint restricts body B to move along a local axis in body A. Body B is free to + * rotate. Supports a linear spring, linear limits, and a rotational motor. + * + * @{ + */ + +/// Create a wheel joint +/// @see b2WheelJointDef for details +B2_API b2JointId b2CreateWheelJoint( b2WorldId worldId, const b2WheelJointDef* def ); + +/// Enable/disable the wheel joint spring +B2_API void b2WheelJoint_EnableSpring( b2JointId jointId, bool enableSpring ); + +/// Is the wheel joint spring enabled? +B2_API bool b2WheelJoint_IsSpringEnabled( b2JointId jointId ); + +/// Set the wheel joint stiffness in Hertz +B2_API void b2WheelJoint_SetSpringHertz( b2JointId jointId, float hertz ); + +/// Get the wheel joint stiffness in Hertz +B2_API float b2WheelJoint_GetSpringHertz( b2JointId jointId ); + +/// Set the wheel joint damping ratio, non-dimensional +B2_API void b2WheelJoint_SetSpringDampingRatio( b2JointId jointId, float dampingRatio ); + +/// Get the wheel joint damping ratio, non-dimensional +B2_API float b2WheelJoint_GetSpringDampingRatio( b2JointId jointId ); + +/// Enable/disable the wheel joint limit +B2_API void b2WheelJoint_EnableLimit( b2JointId jointId, bool enableLimit ); + +/// Is the wheel joint limit enabled? +B2_API bool b2WheelJoint_IsLimitEnabled( b2JointId jointId ); + +/// Get the wheel joint lower limit +B2_API float b2WheelJoint_GetLowerLimit( b2JointId jointId ); + +/// Get the wheel joint upper limit +B2_API float b2WheelJoint_GetUpperLimit( b2JointId jointId ); + +/// Set the wheel joint limits +B2_API void b2WheelJoint_SetLimits( b2JointId jointId, float lower, float upper ); + +/// Enable/disable the wheel joint motor +B2_API void b2WheelJoint_EnableMotor( b2JointId jointId, bool enableMotor ); + +/// Is the wheel joint motor enabled? +B2_API bool b2WheelJoint_IsMotorEnabled( b2JointId jointId ); + +/// Set the wheel joint motor speed in radians per second +B2_API void b2WheelJoint_SetMotorSpeed( b2JointId jointId, float motorSpeed ); + +/// Get the wheel joint motor speed in radians per second +B2_API float b2WheelJoint_GetMotorSpeed( b2JointId jointId ); + +/// Set the wheel joint maximum motor torque, typically in newton-meters +B2_API void b2WheelJoint_SetMaxMotorTorque( b2JointId jointId, float torque ); + +/// Get the wheel joint maximum motor torque, typically in newton-meters +B2_API float b2WheelJoint_GetMaxMotorTorque( b2JointId jointId ); + +/// Get the wheel joint current motor torque, typically in newton-meters +B2_API float b2WheelJoint_GetMotorTorque( b2JointId jointId ); + +/**@}*/ + +/**@}*/ diff --git a/3rdparty/box2d/include/box2d/collision.h b/3rdparty/box2d/include/box2d/collision.h new file mode 100644 index 000000000000..4b296c0b38c3 --- /dev/null +++ b/3rdparty/box2d/include/box2d/collision.h @@ -0,0 +1,795 @@ +// SPDX-FileCopyrightText: 2023 Erin Catto +// SPDX-License-Identifier: MIT + +#pragma once + +#include "base.h" +#include "math_functions.h" + +#include + +typedef struct b2Circle b2Circle; +typedef struct b2Capsule b2Capsule; +typedef struct b2DistanceCache b2DistanceCache; +typedef struct b2Polygon b2Polygon; +typedef struct b2Segment b2Segment; +typedef struct b2ChainSegment b2ChainSegment; + +typedef struct b2Hull b2Hull; + +/** + * @defgroup geometry Geometry + * @brief Geometry types and algorithms + * + * Definitions of circles, capsules, segments, and polygons. Various algorithms to compute hulls, mass properties, and so on. + * @{ + */ + +/// The maximum number of vertices on a convex polygon. Changing this affects performance even if you +/// don't use more vertices. +#define b2_maxPolygonVertices 8 + +/// Low level ray-cast input data +typedef struct b2RayCastInput +{ + /// Start point of the ray cast + b2Vec2 origin; + + /// Translation of the ray cast + b2Vec2 translation; + + /// The maximum fraction of the translation to consider, typically 1 + float maxFraction; +} b2RayCastInput; + +/// Low level shape cast input in generic form. This allows casting an arbitrary point +/// cloud wrap with a radius. For example, a circle is a single point with a non-zero radius. +/// A capsule is two points with a non-zero radius. A box is four points with a zero radius. +typedef struct b2ShapeCastInput +{ + /// A point cloud to cast + b2Vec2 points[b2_maxPolygonVertices]; + + /// The number of points + int32_t count; + + /// The radius around the point cloud + float radius; + + /// The translation of the shape cast + b2Vec2 translation; + + /// The maximum fraction of the translation to consider, typically 1 + float maxFraction; +} b2ShapeCastInput; + +/// Low level ray-cast or shape-cast output data +typedef struct b2CastOutput +{ + /// The surface normal at the hit point + b2Vec2 normal; + + /// The surface hit point + b2Vec2 point; + + /// The fraction of the input translation at collision + float fraction; + + /// The number of iterations used + int32_t iterations; + + /// Did the cast hit? + bool hit; +} b2CastOutput; + +/// This holds the mass data computed for a shape. +typedef struct b2MassData +{ + /// The mass of the shape, usually in kilograms. + float mass; + + /// The position of the shape's centroid relative to the shape's origin. + b2Vec2 center; + + /// The rotational inertia of the shape about the local origin. + float rotationalInertia; +} b2MassData; + +/// A solid circle +typedef struct b2Circle +{ + /// The local center + b2Vec2 center; + + /// The radius + float radius; +} b2Circle; + +/// A solid capsule can be viewed as two semicircles connected +/// by a rectangle. +typedef struct b2Capsule +{ + /// Local center of the first semicircle + b2Vec2 center1; + + /// Local center of the second semicircle + b2Vec2 center2; + + /// The radius of the semicircles + float radius; +} b2Capsule; + +/// 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. +/// @warning DO NOT fill this out manually, instead use a helper function like +/// b2MakePolygon or b2MakeBox. +typedef struct b2Polygon +{ + /// The polygon vertices + b2Vec2 vertices[b2_maxPolygonVertices]; + + /// The outward normal vectors of the polygon sides + b2Vec2 normals[b2_maxPolygonVertices]; + + /// The centroid of the polygon + b2Vec2 centroid; + + /// The external radius for rounded polygons + float radius; + + /// The number of polygon vertices + int32_t count; +} b2Polygon; + +/// A line segment with two-sided collision. +typedef struct b2Segment +{ + /// The first point + b2Vec2 point1; + + /// The second point + b2Vec2 point2; +} b2Segment; + +/// A line segment with one-sided collision. Only collides on the right side. +/// Several of these are generated for a chain shape. +/// ghost1 -> point1 -> point2 -> ghost2 +typedef struct b2ChainSegment +{ + /// The tail ghost vertex + b2Vec2 ghost1; + + /// The line segment + b2Segment segment; + + /// The head ghost vertex + b2Vec2 ghost2; + + /// The owning chain shape index (internal usage only) + int32_t chainId; +} b2ChainSegment; + +/// Validate ray cast input data (NaN, etc) +B2_API bool b2IsValidRay( const b2RayCastInput* input ); + +/// Make a convex polygon from a convex hull. This will assert if the hull is not valid. +/// @warning Do not manually fill in the hull data, it must come directly from b2ComputeHull +B2_API b2Polygon b2MakePolygon( const b2Hull* hull, float radius ); + +/// Make an offset convex polygon from a convex hull. This will assert if the hull is not valid. +/// @warning Do not manually fill in the hull data, it must come directly from b2ComputeHull +B2_API b2Polygon b2MakeOffsetPolygon( const b2Hull* hull, float radius, b2Transform transform ); + +/// Make a square polygon, bypassing the need for a convex hull. +/// @param h the half-width +B2_API b2Polygon b2MakeSquare( float h ); + +/// Make a box (rectangle) polygon, bypassing the need for a convex hull. +/// @param hx the half-width +/// @param hy the half-height +B2_API b2Polygon b2MakeBox( float hx, float hy ); + +/// Make a rounded box, bypassing the need for a convex hull. +/// @param hx the half-width +/// @param hy the half-height +/// @param radius the radius of the rounded extension +B2_API b2Polygon b2MakeRoundedBox( float hx, float hy, float radius ); + +/// Make an offset box, bypassing the need for a convex hull. +/// @param hx the half-width +/// @param hy the half-height +/// @param center the local position of the center of the box +/// @param rotation the local rotation of the box +B2_API b2Polygon b2MakeOffsetBox( float hx, float hy, b2Vec2 center, b2Rot rotation ); + +/// Transform a polygon. This is useful for transferring a shape from one body to another. +B2_API b2Polygon b2TransformPolygon( b2Transform transform, const b2Polygon* polygon ); + +/// Compute mass properties of a circle +B2_API b2MassData b2ComputeCircleMass( const b2Circle* shape, float density ); + +/// Compute mass properties of a capsule +B2_API b2MassData b2ComputeCapsuleMass( const b2Capsule* shape, float density ); + +/// Compute mass properties of a polygon +B2_API b2MassData b2ComputePolygonMass( const b2Polygon* shape, float density ); + +/// Compute the bounding box of a transformed circle +B2_API b2AABB b2ComputeCircleAABB( const b2Circle* shape, b2Transform transform ); + +/// Compute the bounding box of a transformed capsule +B2_API b2AABB b2ComputeCapsuleAABB( const b2Capsule* shape, b2Transform transform ); + +/// Compute the bounding box of a transformed polygon +B2_API b2AABB b2ComputePolygonAABB( const b2Polygon* shape, b2Transform transform ); + +/// Compute the bounding box of a transformed line segment +B2_API b2AABB b2ComputeSegmentAABB( const b2Segment* shape, b2Transform transform ); + +/// Test a point for overlap with a circle in local space +B2_API bool b2PointInCircle( b2Vec2 point, const b2Circle* shape ); + +/// Test a point for overlap with a capsule in local space +B2_API bool b2PointInCapsule( b2Vec2 point, const b2Capsule* shape ); + +/// Test a point for overlap with a convex polygon in local space +B2_API bool b2PointInPolygon( b2Vec2 point, const b2Polygon* shape ); + +/// Ray cast versus circle in shape local space. Initial overlap is treated as a miss. +B2_API b2CastOutput b2RayCastCircle( const b2RayCastInput* input, const b2Circle* shape ); + +/// Ray cast versus capsule in shape local space. Initial overlap is treated as a miss. +B2_API b2CastOutput b2RayCastCapsule( const b2RayCastInput* input, const b2Capsule* shape ); + +/// Ray cast versus segment in shape local space. Optionally treat the segment as one-sided with hits from +/// the left side being treated as a miss. +B2_API b2CastOutput b2RayCastSegment( const b2RayCastInput* input, const b2Segment* shape, bool oneSided ); + +/// Ray cast versus polygon in shape local space. Initial overlap is treated as a miss. +B2_API b2CastOutput b2RayCastPolygon( const b2RayCastInput* input, const b2Polygon* shape ); + +/// Shape cast versus a circle. Initial overlap is treated as a miss. +B2_API b2CastOutput b2ShapeCastCircle( const b2ShapeCastInput* input, const b2Circle* shape ); + +/// Shape cast versus a capsule. Initial overlap is treated as a miss. +B2_API b2CastOutput b2ShapeCastCapsule( const b2ShapeCastInput* input, const b2Capsule* shape ); + +/// Shape cast versus a line segment. Initial overlap is treated as a miss. +B2_API b2CastOutput b2ShapeCastSegment( const b2ShapeCastInput* input, const b2Segment* shape ); + +/// Shape cast versus a convex polygon. Initial overlap is treated as a miss. +B2_API b2CastOutput b2ShapeCastPolygon( const b2ShapeCastInput* input, const b2Polygon* shape ); + +/// A convex hull. Used to create convex polygons. +/// @warning Do not modify these values directly, instead use b2ComputeHull() +typedef struct b2Hull +{ + /// The final points of the hull + b2Vec2 points[b2_maxPolygonVertices]; + + /// The number of points + int32_t count; +} b2Hull; + +/// Compute the convex hull of a set of points. Returns an empty hull if it fails. +/// Some failure cases: +/// - all points very close together +/// - all points on a line +/// - less than 3 points +/// - more than b2_maxPolygonVertices points +/// This welds close points and removes collinear points. +/// @warning Do not modify a hull once it has been computed +B2_API b2Hull b2ComputeHull( const b2Vec2* points, int32_t count ); + +/// This determines if a hull is valid. Checks for: +/// - convexity +/// - collinear points +/// This is expensive and should not be called at runtime. +B2_API bool b2ValidateHull( const b2Hull* hull ); + +/**@}*/ + +/** + * @defgroup distance Distance + * Functions for computing the distance between shapes. + * + * These are advanced functions you can use to perform distance calculations. There + * are functions for computing the closest points between shapes, doing linear shape casts, + * and doing rotational shape casts. The latter is called time of impact (TOI). + * @{ + */ + +/// Result of computing the distance between two line segments +typedef struct b2SegmentDistanceResult +{ + /// The closest point on the first segment + b2Vec2 closest1; + + /// The closest point on the second segment + b2Vec2 closest2; + + /// The barycentric coordinate on the first segment + float fraction1; + + /// The barycentric coordinate on the second segment + float fraction2; + + /// The squared distance between the closest points + float distanceSquared; +} b2SegmentDistanceResult; + +/// Compute the distance between two line segments, clamping at the end points if needed. +B2_API b2SegmentDistanceResult b2SegmentDistance( b2Vec2 p1, b2Vec2 q1, b2Vec2 p2, b2Vec2 q2 ); + +/// A distance proxy is used by the GJK algorithm. It encapsulates any shape. +typedef struct b2DistanceProxy +{ + /// The point cloud + b2Vec2 points[b2_maxPolygonVertices]; + + /// The number of points + int32_t count; + + /// The external radius of the point cloud + float radius; +} b2DistanceProxy; + +/// Used to warm start b2Distance. Set count to zero on first call or +/// use zero initialization. +typedef struct b2DistanceCache +{ + /// The number of stored simplex points + uint16_t count; + + /// The cached simplex indices on shape A + uint8_t indexA[3]; + + /// The cached simplex indices on shape B + uint8_t indexB[3]; +} b2DistanceCache; + +static const b2DistanceCache b2_emptyDistanceCache = B2_ZERO_INIT; + +/// Input for b2ShapeDistance +typedef struct b2DistanceInput +{ + /// The proxy for shape A + b2DistanceProxy proxyA; + + /// The proxy for shape B + b2DistanceProxy proxyB; + + /// The world transform for shape A + b2Transform transformA; + + /// The world transform for shape B + b2Transform transformB; + + /// Should the proxy radius be considered? + bool useRadii; +} b2DistanceInput; + +/// Output for b2ShapeDistance +typedef struct b2DistanceOutput +{ + b2Vec2 pointA; ///< Closest point on shapeA + b2Vec2 pointB; ///< Closest point on shapeB + float distance; ///< The final distance, zero if overlapped + int32_t iterations; ///< Number of GJK iterations used + int32_t simplexCount; ///< The number of simplexes stored in the simplex array +} b2DistanceOutput; + +/// Simplex vertex for debugging the GJK algorithm +typedef struct b2SimplexVertex +{ + b2Vec2 wA; ///< support point in proxyA + b2Vec2 wB; ///< support point in proxyB + b2Vec2 w; ///< wB - wA + float a; ///< barycentric coordinate for closest point + int32_t indexA; ///< wA index + int32_t indexB; ///< wB index +} b2SimplexVertex; + +/// Simplex from the GJK algorithm +typedef struct b2Simplex +{ + b2SimplexVertex v1, v2, v3; ///< vertices + int32_t count; ///< number of valid vertices +} b2Simplex; + +/// Compute the closest points between two shapes represented as point clouds. +/// b2DistanceCache cache is input/output. On the first call set b2DistanceCache.count to zero. +/// The underlying GJK algorithm may be debugged by passing in debug simplexes and capacity. You may pass in NULL and 0 for these. +B2_API b2DistanceOutput b2ShapeDistance( b2DistanceCache* cache, const b2DistanceInput* input, b2Simplex* simplexes, + int simplexCapacity ); + +/// Input parameters for b2ShapeCast +typedef struct b2ShapeCastPairInput +{ + b2DistanceProxy proxyA; ///< The proxy for shape A + b2DistanceProxy proxyB; ///< The proxy for shape B + b2Transform transformA; ///< The world transform for shape A + b2Transform transformB; ///< The world transform for shape B + b2Vec2 translationB; ///< The translation of shape B + float maxFraction; ///< The fraction of the translation to consider, typically 1 +} b2ShapeCastPairInput; + +/// Perform a linear shape cast of shape B moving and shape A fixed. Determines the hit point, normal, and translation fraction. +B2_API b2CastOutput b2ShapeCast( const b2ShapeCastPairInput* input ); + +/// Make a proxy for use in GJK and related functions. +B2_API b2DistanceProxy b2MakeProxy( const b2Vec2* vertices, int32_t count, float radius ); + +/// This describes the motion of a body/shape for TOI computation. Shapes are defined with respect to the body origin, +/// which may not coincide with the center of mass. However, to support dynamics we must interpolate the center of mass +/// position. +typedef struct b2Sweep +{ + b2Vec2 localCenter; ///< Local center of mass position + b2Vec2 c1; ///< Starting center of mass world position + b2Vec2 c2; ///< Ending center of mass world position + b2Rot q1; ///< Starting world rotation + b2Rot q2; ///< Ending world rotation +} b2Sweep; + +/// Evaluate the transform sweep at a specific time. +B2_API b2Transform b2GetSweepTransform( const b2Sweep* sweep, float time ); + +/// Input parameters for b2TimeOfImpact +typedef struct b2TOIInput +{ + b2DistanceProxy proxyA; ///< The proxy for shape A + b2DistanceProxy proxyB; ///< The proxy for shape B + b2Sweep sweepA; ///< The movement of shape A + b2Sweep sweepB; ///< The movement of shape B + float tMax; ///< Defines the sweep interval [0, tMax] +} b2TOIInput; + +/// Describes the TOI output +typedef enum b2TOIState +{ + b2_toiStateUnknown, + b2_toiStateFailed, + b2_toiStateOverlapped, + b2_toiStateHit, + b2_toiStateSeparated +} b2TOIState; + +/// Output parameters for b2TimeOfImpact. +typedef struct b2TOIOutput +{ + b2TOIState state; ///< The type of result + float t; ///< The time of the collision +} b2TOIOutput; + +/// 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. +B2_API b2TOIOutput b2TimeOfImpact( const b2TOIInput* input ); + +/**@}*/ + +/** + * @defgroup collision Collision + * @brief Functions for colliding pairs of shapes + * @{ + */ + +/// A manifold point is a contact point belonging to a contact +/// manifold. It holds details related to the geometry and dynamics +/// of the contact points. +typedef struct b2ManifoldPoint +{ + /// Location of the contact point in world space. Subject to precision loss at large coordinates. + /// @note Should only be used for debugging. + b2Vec2 point; + + /// Location of the contact point relative to bodyA's origin in world space + /// @note When used internally to the Box2D solver, these are relative to the center of mass. + b2Vec2 anchorA; + + /// Location of the contact point relative to bodyB's origin in world space + b2Vec2 anchorB; + + /// The separation of the contact point, negative if penetrating + float separation; + + /// The impulse along the manifold normal vector. + float normalImpulse; + + /// The friction impulse + float tangentImpulse; + + /// The maximum normal impulse applied during sub-stepping + /// todo not sure this is needed + float maxNormalImpulse; + + /// Relative normal velocity pre-solve. Used for hit events. If the normal impulse is + /// zero then there was no hit. Negative means shapes are approaching. + float normalVelocity; + + /// Uniquely identifies a contact point between two shapes + uint16_t id; + + /// Did this contact point exist the previous step? + bool persisted; +} b2ManifoldPoint; + +/// A contact manifold describes the contact points between colliding shapes +typedef struct b2Manifold +{ + /// The manifold points, up to two are possible in 2D + b2ManifoldPoint points[2]; + + /// The unit normal vector in world space, points from shape A to bodyB + b2Vec2 normal; + + /// The number of contacts points, will be 0, 1, or 2 + int32_t pointCount; +} b2Manifold; + +/// Compute the contact manifold between two circles +B2_API b2Manifold b2CollideCircles( const b2Circle* circleA, b2Transform xfA, const b2Circle* circleB, b2Transform xfB ); + +/// Compute the contact manifold between a capsule and circle +B2_API b2Manifold b2CollideCapsuleAndCircle( const b2Capsule* capsuleA, b2Transform xfA, const b2Circle* circleB, + b2Transform xfB ); + +/// Compute the contact manifold between an segment and a circle +B2_API b2Manifold b2CollideSegmentAndCircle( const b2Segment* segmentA, b2Transform xfA, const b2Circle* circleB, + b2Transform xfB ); + +/// Compute the contact manifold between a polygon and a circle +B2_API b2Manifold b2CollidePolygonAndCircle( const b2Polygon* polygonA, b2Transform xfA, const b2Circle* circleB, + b2Transform xfB ); + +/// Compute the contact manifold between a capsule and circle +B2_API b2Manifold b2CollideCapsules( const b2Capsule* capsuleA, b2Transform xfA, const b2Capsule* capsuleB, b2Transform xfB ); + +/// Compute the contact manifold between an segment and a capsule +B2_API b2Manifold b2CollideSegmentAndCapsule( const b2Segment* segmentA, b2Transform xfA, const b2Capsule* capsuleB, + b2Transform xfB ); + +/// Compute the contact manifold between a polygon and capsule +B2_API b2Manifold b2CollidePolygonAndCapsule( const b2Polygon* polygonA, b2Transform xfA, const b2Capsule* capsuleB, + b2Transform xfB ); + +/// Compute the contact manifold between two polygons +B2_API b2Manifold b2CollidePolygons( const b2Polygon* polygonA, b2Transform xfA, const b2Polygon* polygonB, b2Transform xfB ); + +/// Compute the contact manifold between an segment and a polygon +B2_API b2Manifold b2CollideSegmentAndPolygon( const b2Segment* segmentA, b2Transform xfA, const b2Polygon* polygonB, + b2Transform xfB ); + +/// Compute the contact manifold between a chain segment and a circle +B2_API b2Manifold b2CollideChainSegmentAndCircle( const b2ChainSegment* segmentA, b2Transform xfA, + const b2Circle* circleB, b2Transform xfB ); + +/// Compute the contact manifold between a chain segment and a capsule +B2_API b2Manifold b2CollideChainSegmentAndCapsule( const b2ChainSegment* segmentA, b2Transform xfA, + const b2Capsule* capsuleB, b2Transform xfB, b2DistanceCache* cache ); + +/// Compute the contact manifold between a chain segment and a rounded polygon +B2_API b2Manifold b2CollideChainSegmentAndPolygon( const b2ChainSegment* segmentA, b2Transform xfA, + const b2Polygon* polygonB, b2Transform xfB, b2DistanceCache* cache ); + +/**@}*/ + +/** + * @defgroup tree Dynamic Tree + * The dynamic tree is a binary AABB tree to organize and query large numbers of geometric objects + * + * Box2D uses the dynamic tree internally to sort collision shapes into a binary bounding volume hierarchy. + * This data structure may have uses in games for organizing other geometry data and may be used independently + * of Box2D rigid body simulation. + * + * A dynamic AABB tree broad-phase, inspired by Nathanael Presson's btDbvt. + * A dynamic tree arranges data in a binary tree to accelerate + * queries such as AABB queries and ray casts. Leaf nodes are proxies + * with an AABB. These are used to hold a user collision object, such as a reference to a b2Shape. + * Nodes are pooled and relocatable, so I use node indices rather than pointers. + * The dynamic tree is made available for advanced users that would like to use it to organize + * spatial game data besides rigid bodies. + * + * @note This is an advanced feature and normally not used by applications directly. + * @{ + */ + +/// The default category bit for a tree proxy. Used for collision filtering. +#define b2_defaultCategoryBits ( 1 ) + +/// Convenience mask bits to use when you don't need collision filtering and just want +/// all results. +#define b2_defaultMaskBits ( UINT64_MAX ) + +/// A node in the dynamic tree. This is private data placed here for performance reasons. +typedef struct b2TreeNode +{ + /// The node bounding box + b2AABB aabb; // 16 + + /// Category bits for collision filtering + uint64_t categoryBits; // 8 + + union + { + /// The node parent index + int32_t parent; + + /// The node freelist next index + int32_t next; + }; // 4 + + /// Child 1 index + int32_t child1; // 4 + + /// Child 2 index + int32_t child2; // 4 + + /// User data + // todo could be union with child index + int32_t userData; // 4 + + /// Leaf = 0, free node = -1 + int16_t height; // 2 + + /// Has the AABB been enlarged? + bool enlarged; // 1 + + /// Padding for clarity + char pad[5]; +} b2TreeNode; + +/// The dynamic tree structure. This should be considered private data. +/// It is placed here for performance reasons. +typedef struct b2DynamicTree +{ + /// The tree nodes + b2TreeNode* nodes; + + /// The root index + int32_t root; + + /// The number of nodes + int32_t nodeCount; + + /// The allocated node space + int32_t nodeCapacity; + + /// Node free list + int32_t freeList; + + /// Number of proxies created + int32_t proxyCount; + + /// Leaf indices for rebuild + int32_t* leafIndices; + + /// Leaf bounding boxes for rebuild + b2AABB* leafBoxes; + + /// Leaf bounding box centers for rebuild + b2Vec2* leafCenters; + + /// Bins for sorting during rebuild + int32_t* binIndices; + + /// Allocated space for rebuilding + int32_t rebuildCapacity; +} b2DynamicTree; + +/// Constructing the tree initializes the node pool. +B2_API b2DynamicTree b2DynamicTree_Create( void ); + +/// Destroy the tree, freeing the node pool. +B2_API void b2DynamicTree_Destroy( b2DynamicTree* tree ); + +/// Create a proxy. Provide an AABB and a userData value. +B2_API int32_t b2DynamicTree_CreateProxy( b2DynamicTree* tree, b2AABB aabb, uint64_t categoryBits, int32_t userData ); + +/// Destroy a proxy. This asserts if the id is invalid. +B2_API void b2DynamicTree_DestroyProxy( b2DynamicTree* tree, int32_t proxyId ); + +/// Move a proxy to a new AABB by removing and reinserting into the tree. +B2_API void b2DynamicTree_MoveProxy( b2DynamicTree* tree, int32_t proxyId, b2AABB aabb ); + +/// Enlarge a proxy and enlarge ancestors as necessary. +B2_API void b2DynamicTree_EnlargeProxy( b2DynamicTree* tree, int32_t proxyId, b2AABB aabb ); + +/// This function receives proxies found in the AABB query. +/// @return true if the query should continue +typedef bool b2TreeQueryCallbackFcn( int32_t proxyId, int32_t userData, void* context ); + +/// Query an AABB for overlapping proxies. The callback class is called for each proxy that overlaps the supplied AABB. +B2_API void b2DynamicTree_Query( const b2DynamicTree* tree, b2AABB aabb, uint64_t maskBits, b2TreeQueryCallbackFcn* callback, + void* context ); + +/// This function receives clipped raycast input for a proxy. The function +/// returns the new ray fraction. +/// - return a value of 0 to terminate the ray cast +/// - return a value less than input->maxFraction to clip the ray +/// - return a value of input->maxFraction to continue the ray cast without clipping +typedef float b2TreeRayCastCallbackFcn( const b2RayCastInput* input, int32_t proxyId, int32_t userData, void* context ); + +/// Ray-cast against the proxies in the tree. This relies on the callback +/// to perform a exact ray-cast in the case were the proxy contains a shape. +/// The callback also performs the any collision filtering. This has performance +/// roughly equal to k * log(n), where k is the number of collisions and n is the +/// number of proxies in the tree. +/// Bit-wise filtering using mask bits can greatly improve performance in some scenarios. +/// @param tree the dynamic tree to ray cast +/// @param input the ray-cast input data. The ray extends from p1 to p1 + maxFraction * (p2 - p1) +/// @param maskBits filter bits: `bool accept = (maskBits & node->categoryBits) != 0;` +/// @param callback a callback class that is called for each proxy that is hit by the ray +/// @param context user context that is passed to the callback +B2_API void b2DynamicTree_RayCast( const b2DynamicTree* tree, const b2RayCastInput* input, uint64_t maskBits, + b2TreeRayCastCallbackFcn* callback, void* context ); + +/// This function receives clipped ray-cast input for a proxy. The function +/// returns the new ray fraction. +/// - return a value of 0 to terminate the ray-cast +/// - return a value less than input->maxFraction to clip the ray +/// - return a value of input->maxFraction to continue the ray cast without clipping +typedef float b2TreeShapeCastCallbackFcn( const b2ShapeCastInput* input, int32_t proxyId, int32_t userData, void* context ); + +/// Ray-cast against the proxies in the tree. This relies on the callback +/// to perform a exact ray-cast in the case were the proxy contains a shape. +/// The callback also performs the any collision filtering. This has performance +/// roughly equal to k * log(n), where k is the number of collisions and n is the +/// number of proxies in the tree. +/// @param tree the dynamic tree to ray cast +/// @param input the ray-cast input data. The ray extends from p1 to p1 + maxFraction * (p2 - p1). +/// @param maskBits filter bits: `bool accept = (maskBits & node->categoryBits) != 0;` +/// @param callback a callback class that is called for each proxy that is hit by the shape +/// @param context user context that is passed to the callback +B2_API void b2DynamicTree_ShapeCast( const b2DynamicTree* tree, const b2ShapeCastInput* input, uint64_t maskBits, + b2TreeShapeCastCallbackFcn* callback, void* context ); + +/// Validate this tree. For testing. +B2_API void b2DynamicTree_Validate( const b2DynamicTree* tree ); + +/// Compute the height of the binary tree in O(N) time. Should not be +/// called often. +B2_API int b2DynamicTree_GetHeight( const b2DynamicTree* tree ); + +/// Get the maximum balance of the tree. The balance is the difference in height of the two children of a node. +B2_API int b2DynamicTree_GetMaxBalance( const b2DynamicTree* tree ); + +/// Get the ratio of the sum of the node areas to the root area. +B2_API float b2DynamicTree_GetAreaRatio( const b2DynamicTree* tree ); + +/// Build an optimal tree. Very expensive. For testing. +B2_API void b2DynamicTree_RebuildBottomUp( b2DynamicTree* tree ); + +/// Get the number of proxies created +B2_API int b2DynamicTree_GetProxyCount( const b2DynamicTree* tree ); + +/// Rebuild the tree while retaining subtrees that haven't changed. Returns the number of boxes sorted. +B2_API int b2DynamicTree_Rebuild( b2DynamicTree* tree, bool fullBuild ); + +/// Shift the world origin. Useful for large worlds. +/// The shift formula is: position -= newOrigin +/// @param tree the tree to shift +/// @param newOrigin the new origin with respect to the old origin +B2_API void b2DynamicTree_ShiftOrigin( b2DynamicTree* tree, b2Vec2 newOrigin ); + +/// Get the number of bytes used by this tree +B2_API int b2DynamicTree_GetByteCount( const b2DynamicTree* tree ); + +/// Get proxy user data +/// @return the proxy user data or 0 if the id is invalid +B2_INLINE int32_t b2DynamicTree_GetUserData( const b2DynamicTree* tree, int32_t proxyId ) +{ + return tree->nodes[proxyId].userData; +} + +/// Get the AABB of a proxy +B2_INLINE b2AABB b2DynamicTree_GetAABB( const b2DynamicTree* tree, int32_t proxyId ) +{ + return tree->nodes[proxyId].aabb; +} + +/**@}*/ diff --git a/3rdparty/box2d/include/box2d/id.h b/3rdparty/box2d/include/box2d/id.h new file mode 100644 index 000000000000..72c31f628817 --- /dev/null +++ b/3rdparty/box2d/include/box2d/id.h @@ -0,0 +1,92 @@ +// SPDX-FileCopyrightText: 2023 Erin Catto +// SPDX-License-Identifier: MIT + +#pragma once + +#include "base.h" + +#include + +/** + * @defgroup id Ids + * These ids serve as handles to internal Box2D objects. + * These should be considered opaque data and passed by value. + * Include this header if you need the id types and not the whole Box2D API. + * All ids are considered null if initialized to zero. + * + * For example in C++: + * + * @code{.cxx} + * b2WorldId worldId = {}; + * @endcode + * + * Or in C: + * + * @code{.c} + * b2WorldId worldId = {0}; + * @endcode + * + * These are both considered null. + * + * @warning Do not use the internals of these ids. They are subject to change. Ids should be treated as opaque objects. + * @warning You should use ids to access objects in Box2D. Do not access files within the src folder. Such usage is unsupported. + * @{ + */ + +/// World id references a world instance. This should be treated as an opaque handle. +typedef struct b2WorldId +{ + uint16_t index1; + uint16_t revision; +} b2WorldId; + +/// Body id references a body instance. This should be treated as an opaque handle. +typedef struct b2BodyId +{ + int32_t index1; + uint16_t world0; + uint16_t revision; +} b2BodyId; + +/// Shape id references a shape instance. This should be treated as an opaque handle. +typedef struct b2ShapeId +{ + int32_t index1; + uint16_t world0; + uint16_t revision; +} b2ShapeId; + +/// Joint id references a joint instance. This should be treated as an opaque handle. +typedef struct b2JointId +{ + int32_t index1; + uint16_t world0; + uint16_t revision; +} b2JointId; + +/// Chain id references a chain instances. This should be treated as an opaque handle. +typedef struct b2ChainId +{ + int32_t index1; + uint16_t world0; + uint16_t revision; +} b2ChainId; + +/// Use these to make your identifiers null. +/// You may also use zero initialization to get null. +static const b2WorldId b2_nullWorldId = B2_ZERO_INIT; +static const b2BodyId b2_nullBodyId = B2_ZERO_INIT; +static const b2ShapeId b2_nullShapeId = B2_ZERO_INIT; +static const b2JointId b2_nullJointId = B2_ZERO_INIT; +static const b2ChainId b2_nullChainId = B2_ZERO_INIT; + +/// Macro to determine if any id is null. +#define B2_IS_NULL( id ) ( id.index1 == 0 ) + +/// Macro to determine if any id is non-null. +#define B2_IS_NON_NULL( id ) ( id.index1 != 0 ) + +/// Compare two ids for equality. Doesn't work for b2WorldId. +#define B2_ID_EQUALS( id1, id2 ) ( id1.index1 == id2.index1 && id1.world0 == id2.world0 && id1.revision == id2.revision ) + +/**@}*/ diff --git a/3rdparty/box2d/include/box2d/math_functions.h b/3rdparty/box2d/include/box2d/math_functions.h new file mode 100644 index 000000000000..4955953478e8 --- /dev/null +++ b/3rdparty/box2d/include/box2d/math_functions.h @@ -0,0 +1,696 @@ +// SPDX-FileCopyrightText: 2023 Erin Catto +// SPDX-License-Identifier: MIT + +#pragma once + +#include "base.h" + +#include +#include +#include + +/** + * @defgroup math Math + * @brief Vector math types and functions + * @{ + */ + +/// https://en.wikipedia.org/wiki/Pi +#define b2_pi 3.14159265359f + +/// 2D vector +/// This can be used to represent a point or free vector +typedef struct b2Vec2 +{ + /// coordinates + float x, y; +} b2Vec2; + +/// Cosine and sine pair +/// This uses a custom implementation designed for cross platform determinism +typedef struct b2CosSin +{ + /// cosine and sine + float cosine; + float sine; +} b2CosSin; + +/// 2D rotation +/// This is similar to using a complex number for rotation +typedef struct b2Rot +{ + /// cosine and sine + float c, s; +} b2Rot; + +/// A 2D rigid transform +typedef struct b2Transform +{ + b2Vec2 p; + b2Rot q; +} b2Transform; + +/// A 2-by-2 Matrix +typedef struct b2Mat22 +{ + /// columns + b2Vec2 cx, cy; +} b2Mat22; + +/// Axis-aligned bounding box +typedef struct b2AABB +{ + b2Vec2 lowerBound; + b2Vec2 upperBound; +} b2AABB; + +/**@}*/ + +/** + * @addtogroup math + * @{ + */ + +static const b2Vec2 b2Vec2_zero = { 0.0f, 0.0f }; +static const b2Rot b2Rot_identity = { 1.0f, 0.0f }; +static const b2Transform b2Transform_identity = { { 0.0f, 0.0f }, { 1.0f, 0.0f } }; +static const b2Mat22 b2Mat22_zero = { { 0.0f, 0.0f }, { 0.0f, 0.0f } }; + +/// Compute an approximate arctangent in the range [-pi, pi] +/// This is hand coded for cross platform determinism. The atan2f +/// function in the standard library is not cross platform deterministic. +B2_API float b2Atan2( float y, float x ); + +/// @return the minimum of two floats +B2_INLINE float b2MinFloat( float a, float b ) +{ + return a < b ? a : b; +} + +/// @return the maximum of two floats +B2_INLINE float b2MaxFloat( float a, float b ) +{ + return a > b ? a : b; +} + +/// @return the absolute value of a float +B2_INLINE float b2AbsFloat( float a ) +{ + return a < 0 ? -a : a; +} + +/// @return a float clamped between a lower and upper bound +B2_INLINE float b2ClampFloat( float a, float lower, float upper ) +{ + return a < lower ? lower : ( a > upper ? upper : a ); +} + +/// @return the minimum of two integers +B2_INLINE int b2MinInt( int a, int b ) +{ + return a < b ? a : b; +} + +/// @return the maximum of two integers +B2_INLINE int b2MaxInt( int a, int b ) +{ + return a > b ? a : b; +} + +/// @return the absolute value of an integer +B2_INLINE int b2AbsInt( int a ) +{ + return a < 0 ? -a : a; +} + +/// @return an integer clamped between a lower and upper bound +B2_INLINE int b2ClampInt( int a, int lower, int upper ) +{ + return a < lower ? lower : ( a > upper ? upper : a ); +} + +/// Vector dot product +B2_INLINE float b2Dot( b2Vec2 a, b2Vec2 b ) +{ + return a.x * b.x + a.y * b.y; +} + +/// Vector cross product. In 2D this yields a scalar. +B2_INLINE float b2Cross( b2Vec2 a, 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. +B2_INLINE b2Vec2 b2CrossVS( b2Vec2 v, float s ) +{ + return B2_LITERAL( b2Vec2 ){ s * v.y, -s * v.x }; +} + +/// Perform the cross product on a scalar and a vector. In 2D this produces a vector. +B2_INLINE b2Vec2 b2CrossSV( float s, b2Vec2 v ) +{ + return B2_LITERAL( b2Vec2 ){ -s * v.y, s * v.x }; +} + +/// Get a left pointing perpendicular vector. Equivalent to b2CrossSV(1.0f, v) +B2_INLINE b2Vec2 b2LeftPerp( b2Vec2 v ) +{ + return B2_LITERAL( b2Vec2 ){ -v.y, v.x }; +} + +/// Get a right pointing perpendicular vector. Equivalent to b2CrossVS(v, 1.0f) +B2_INLINE b2Vec2 b2RightPerp( b2Vec2 v ) +{ + return B2_LITERAL( b2Vec2 ){ v.y, -v.x }; +} + +/// Vector addition +B2_INLINE b2Vec2 b2Add( b2Vec2 a, b2Vec2 b ) +{ + return B2_LITERAL( b2Vec2 ){ a.x + b.x, a.y + b.y }; +} + +/// Vector subtraction +B2_INLINE b2Vec2 b2Sub( b2Vec2 a, b2Vec2 b ) +{ + return B2_LITERAL( b2Vec2 ){ a.x - b.x, a.y - b.y }; +} + +/// Vector negation +B2_INLINE b2Vec2 b2Neg( b2Vec2 a ) +{ + return B2_LITERAL( b2Vec2 ){ -a.x, -a.y }; +} + +/// Vector linear interpolation +/// https://fgiesen.wordpress.com/2012/08/15/linear-interpolation-past-present-and-future/ +B2_INLINE b2Vec2 b2Lerp( b2Vec2 a, b2Vec2 b, float t ) +{ + return B2_LITERAL( b2Vec2 ){ ( 1.0f - t ) * a.x + t * b.x, ( 1.0f - t ) * a.y + t * b.y }; +} + +/// Component-wise multiplication +B2_INLINE b2Vec2 b2Mul( b2Vec2 a, b2Vec2 b ) +{ + return B2_LITERAL( b2Vec2 ){ a.x * b.x, a.y * b.y }; +} + +/// Multiply a scalar and vector +B2_INLINE b2Vec2 b2MulSV( float s, b2Vec2 v ) +{ + return B2_LITERAL( b2Vec2 ){ s * v.x, s * v.y }; +} + +/// a + s * b +B2_INLINE b2Vec2 b2MulAdd( b2Vec2 a, float s, b2Vec2 b ) +{ + return B2_LITERAL( b2Vec2 ){ a.x + s * b.x, a.y + s * b.y }; +} + +/// a - s * b +B2_INLINE b2Vec2 b2MulSub( b2Vec2 a, float s, b2Vec2 b ) +{ + return B2_LITERAL( b2Vec2 ){ a.x - s * b.x, a.y - s * b.y }; +} + +/// Component-wise absolute vector +B2_INLINE b2Vec2 b2Abs( b2Vec2 a ) +{ + b2Vec2 b; + b.x = b2AbsFloat( a.x ); + b.y = b2AbsFloat( a.y ); + return b; +} + +/// Component-wise minimum vector +B2_INLINE b2Vec2 b2Min( b2Vec2 a, b2Vec2 b ) +{ + b2Vec2 c; + c.x = b2MinFloat( a.x, b.x ); + c.y = b2MinFloat( a.y, b.y ); + return c; +} + +/// Component-wise maximum vector +B2_INLINE b2Vec2 b2Max( b2Vec2 a, b2Vec2 b ) +{ + b2Vec2 c; + c.x = b2MaxFloat( a.x, b.x ); + c.y = b2MaxFloat( a.y, b.y ); + return c; +} + +/// Component-wise clamp vector v into the range [a, b] +B2_INLINE b2Vec2 b2Clamp( b2Vec2 v, b2Vec2 a, b2Vec2 b ) +{ + b2Vec2 c; + c.x = b2ClampFloat( v.x, a.x, b.x ); + c.y = b2ClampFloat( v.y, a.y, b.y ); + return c; +} + +/// Get the length of this vector (the norm) +B2_INLINE float b2Length( b2Vec2 v ) +{ + return sqrtf( v.x * v.x + v.y * v.y ); +} + +/// Get the distance between two points +B2_INLINE float b2Distance( b2Vec2 a, b2Vec2 b ) +{ + float dx = b.x - a.x; + float dy = b.y - a.y; + return sqrtf( dx * dx + dy * dy ); +} + +/// Convert a vector into a unit vector if possible, otherwise returns the zero vector. +B2_INLINE b2Vec2 b2Normalize( b2Vec2 v ) +{ + float length = sqrtf( v.x * v.x + v.y * v.y ); + if ( length < FLT_EPSILON ) + { + return b2Vec2_zero; + } + + float invLength = 1.0f / length; + b2Vec2 n = { invLength * v.x, invLength * v.y }; + return n; +} + +/// Convert a vector into a unit vector if possible, otherwise returns the zero vector. Also +/// outputs the length. +B2_INLINE b2Vec2 b2GetLengthAndNormalize( float* length, b2Vec2 v ) +{ + *length = b2Length( v ); + if ( *length < FLT_EPSILON ) + { + return b2Vec2_zero; + } + + float invLength = 1.0f / *length; + b2Vec2 n = { invLength * v.x, invLength * v.y }; + return n; +} + +/// Normalize rotation +B2_INLINE b2Rot b2NormalizeRot( b2Rot q ) +{ + float mag = sqrtf( q.s * q.s + q.c * q.c ); + float invMag = mag > 0.0 ? 1.0f / mag : 0.0f; + b2Rot qn = { q.c * invMag, q.s * invMag }; + return qn; +} + +/// Integration rotation from angular velocity +/// @param q1 initial rotation +/// @param deltaAngle the angular displacement in radians +B2_INLINE b2Rot b2IntegrateRotation( b2Rot q1, float deltaAngle ) +{ + // dc/dt = -omega * sin(t) + // ds/dt = omega * cos(t) + // c2 = c1 - omega * h * s1 + // s2 = s1 + omega * h * c1 + b2Rot q2 = { q1.c - deltaAngle * q1.s, q1.s + deltaAngle * q1.c }; + float mag = sqrtf( q2.s * q2.s + q2.c * q2.c ); + float invMag = mag > 0.0 ? 1.0f / mag : 0.0f; + b2Rot qn = { q2.c * invMag, q2.s * invMag }; + return qn; +} + +/// Get the length squared of this vector +B2_INLINE float b2LengthSquared( b2Vec2 v ) +{ + return v.x * v.x + v.y * v.y; +} + +/// Get the distance squared between points +B2_INLINE float b2DistanceSquared( b2Vec2 a, b2Vec2 b ) +{ + b2Vec2 c = { b.x - a.x, b.y - a.y }; + return c.x * c.x + c.y * c.y; +} + +/// Make a rotation using an angle in radians +B2_API b2CosSin b2ComputeCosSin( float angle ); + +/// Make a rotation using an angle in radians +B2_INLINE b2Rot b2MakeRot( float angle ) +{ + b2CosSin cs = b2ComputeCosSin( angle ); + return B2_LITERAL( b2Rot ){ cs.cosine, cs.sine }; +} + +/// Is this rotation normalized? +B2_INLINE bool b2IsNormalized( b2Rot q ) +{ + // larger tolerance due to failure on mingw 32-bit + float qq = q.s * q.s + q.c * q.c; + return 1.0f - 0.0006f < qq && qq < 1.0f + 0.0006f; +} + +/// Normalized linear interpolation +/// https://fgiesen.wordpress.com/2012/08/15/linear-interpolation-past-present-and-future/ +B2_INLINE b2Rot b2NLerp( b2Rot q1, b2Rot q2, float t ) +{ + float omt = 1.0f - t; + b2Rot q = { + omt * q1.c + t * q2.c, + omt * q1.s + t * q2.s, + }; + + return b2NormalizeRot( q ); +} + +/// Compute the angular velocity necessary to rotate between two rotations over a give time +/// @param q1 initial rotation +/// @param q2 final rotation +/// @param inv_h inverse time step +B2_INLINE float b2ComputeAngularVelocity( b2Rot q1, b2Rot q2, float inv_h ) +{ + // ds/dt = omega * cos(t) + // dc/dt = -omega * sin(t) + // s2 = s1 + omega * h * c1 + // c2 = c1 - omega * h * s1 + + // omega * h * s1 = c1 - c2 + // omega * h * c1 = s2 - s1 + // omega * h = (c1 - c2) * s1 + (s2 - s1) * c1; + // omega * h = s1 * c1 - c2 * s1 + s2 * c1 - s1 * c1 + // omega * h = s2 * c1 - c2 * s1 = sin(a2 - a1) ~= a2 - a1 for small delta + float omega = inv_h * ( q2.s * q1.c - q2.c * q1.s ); + return omega; +} + +/// Get the angle in radians in the range [-pi, pi] +B2_INLINE float b2Rot_GetAngle( b2Rot q ) +{ + return b2Atan2( q.s, q.c ); +} + +/// Get the x-axis +B2_INLINE b2Vec2 b2Rot_GetXAxis( b2Rot q ) +{ + b2Vec2 v = { q.c, q.s }; + return v; +} + +/// Get the y-axis +B2_INLINE b2Vec2 b2Rot_GetYAxis( b2Rot q ) +{ + b2Vec2 v = { -q.s, q.c }; + return v; +} + +/// Multiply two rotations: q * r +B2_INLINE b2Rot b2MulRot( b2Rot q, b2Rot r ) +{ + // [qc -qs] * [rc -rs] = [qc*rc-qs*rs -qc*rs-qs*rc] + // [qs qc] [rs rc] [qs*rc+qc*rs -qs*rs+qc*rc] + // s(q + r) = qs * rc + qc * rs + // c(q + r) = qc * rc - qs * rs + b2Rot qr; + qr.s = q.s * r.c + q.c * r.s; + qr.c = q.c * r.c - q.s * r.s; + return qr; +} + +/// Transpose multiply two rotations: qT * r +B2_INLINE b2Rot b2InvMulRot( b2Rot q, b2Rot r ) +{ + // [ qc qs] * [rc -rs] = [qc*rc+qs*rs -qc*rs+qs*rc] + // [-qs qc] [rs rc] [-qs*rc+qc*rs qs*rs+qc*rc] + // s(q - r) = qc * rs - qs * rc + // c(q - r) = qc * rc + qs * rs + b2Rot qr; + qr.s = q.c * r.s - q.s * r.c; + qr.c = q.c * r.c + q.s * r.s; + return qr; +} + +/// relative angle between b and a (rot_b * inv(rot_a)) +B2_INLINE float b2RelativeAngle( b2Rot b, b2Rot a ) +{ + // sin(b - a) = bs * ac - bc * as + // cos(b - a) = bc * ac + bs * as + float s = b.s * a.c - b.c * a.s; + float c = b.c * a.c + b.s * a.s; + return b2Atan2( s, c ); +} + +/// Convert an angle in the range [-2*pi, 2*pi] into the range [-pi, pi] +B2_INLINE float b2UnwindAngle( float angle ) +{ + if ( angle < -b2_pi ) + { + return angle + 2.0f * b2_pi; + } + else if ( angle > b2_pi ) + { + return angle - 2.0f * b2_pi; + } + + return angle; +} + +/// Convert any into the range [-pi, pi] (slow) +B2_INLINE float b2UnwindLargeAngle( float angle ) +{ + while ( angle > b2_pi ) + { + angle -= 2.0f * b2_pi; + } + + while ( angle < -b2_pi ) + { + angle += 2.0f * b2_pi; + } + + return angle; +} + +/// Rotate a vector +B2_INLINE b2Vec2 b2RotateVector( b2Rot q, b2Vec2 v ) +{ + return B2_LITERAL( b2Vec2 ){ q.c * v.x - q.s * v.y, q.s * v.x + q.c * v.y }; +} + +/// Inverse rotate a vector +B2_INLINE b2Vec2 b2InvRotateVector( b2Rot q, b2Vec2 v ) +{ + return B2_LITERAL( b2Vec2 ){ q.c * v.x + q.s * v.y, -q.s * v.x + q.c * v.y }; +} + +/// Transform a point (e.g. local space to world space) +B2_INLINE b2Vec2 b2TransformPoint( b2Transform t, const b2Vec2 p ) +{ + float x = ( t.q.c * p.x - t.q.s * p.y ) + t.p.x; + float y = ( t.q.s * p.x + t.q.c * p.y ) + t.p.y; + + return B2_LITERAL( b2Vec2 ){ x, y }; +} + +/// Inverse transform a point (e.g. world space to local space) +B2_INLINE b2Vec2 b2InvTransformPoint( b2Transform t, const b2Vec2 p ) +{ + float vx = p.x - t.p.x; + float vy = p.y - t.p.y; + return B2_LITERAL( b2Vec2 ){ t.q.c * vx + t.q.s * vy, -t.q.s * vx + t.q.c * vy }; +} + +/// v2 = A.q.Rot(B.q.Rot(v1) + B.p) + A.p +/// = (A.q * B.q).Rot(v1) + A.q.Rot(B.p) + A.p +B2_INLINE b2Transform b2MulTransforms( b2Transform A, b2Transform B ) +{ + b2Transform C; + C.q = b2MulRot( A.q, B.q ); + C.p = b2Add( b2RotateVector( A.q, B.p ), A.p ); + return C; +} + +/// v2 = A.q' * (B.q * v1 + B.p - A.p) +/// = A.q' * B.q * v1 + A.q' * (B.p - A.p) +B2_INLINE b2Transform b2InvMulTransforms( b2Transform A, b2Transform B ) +{ + b2Transform C; + C.q = b2InvMulRot( A.q, B.q ); + C.p = b2InvRotateVector( A.q, b2Sub( B.p, A.p ) ); + return C; +} + +/// Multiply a 2-by-2 matrix times a 2D vector +B2_INLINE b2Vec2 b2MulMV( b2Mat22 A, b2Vec2 v ) +{ + b2Vec2 u = { + A.cx.x * v.x + A.cy.x * v.y, + A.cx.y * v.x + A.cy.y * v.y, + }; + return u; +} + +/// Get the inverse of a 2-by-2 matrix +B2_INLINE b2Mat22 b2GetInverse22( b2Mat22 A ) +{ + float a = A.cx.x, b = A.cy.x, c = A.cx.y, d = A.cy.y; + float det = a * d - b * c; + if ( det != 0.0f ) + { + det = 1.0f / det; + } + + b2Mat22 B = { + { det * d, -det * c }, + { -det * b, det * a }, + }; + return B; +} + +/// Solve A * x = b, where b is a column vector. This is more efficient +/// than computing the inverse in one-shot cases. +B2_INLINE b2Vec2 b2Solve22( b2Mat22 A, b2Vec2 b ) +{ + float a11 = A.cx.x, a12 = A.cy.x, a21 = A.cx.y, a22 = A.cy.y; + float det = a11 * a22 - a12 * a21; + if ( det != 0.0f ) + { + det = 1.0f / det; + } + b2Vec2 x = { det * ( a22 * b.x - a12 * b.y ), det * ( a11 * b.y - a21 * b.x ) }; + return x; +} + +/// Does a fully contain b +B2_INLINE bool b2AABB_Contains( b2AABB a, b2AABB b ) +{ + bool s = true; + s = s && a.lowerBound.x <= b.lowerBound.x; + s = s && a.lowerBound.y <= b.lowerBound.y; + s = s && b.upperBound.x <= a.upperBound.x; + s = s && b.upperBound.y <= a.upperBound.y; + return s; +} + +/// Get the center of the AABB. +B2_INLINE b2Vec2 b2AABB_Center( b2AABB a ) +{ + b2Vec2 b = { 0.5f * ( a.lowerBound.x + a.upperBound.x ), 0.5f * ( a.lowerBound.y + a.upperBound.y ) }; + return b; +} + +/// Get the extents of the AABB (half-widths). +B2_INLINE b2Vec2 b2AABB_Extents( b2AABB a ) +{ + b2Vec2 b = { 0.5f * ( a.upperBound.x - a.lowerBound.x ), 0.5f * ( a.upperBound.y - a.lowerBound.y ) }; + return b; +} + +/// Union of two AABBs +B2_INLINE b2AABB b2AABB_Union( b2AABB a, b2AABB b ) +{ + b2AABB c; + c.lowerBound.x = b2MinFloat( a.lowerBound.x, b.lowerBound.x ); + c.lowerBound.y = b2MinFloat( a.lowerBound.y, b.lowerBound.y ); + c.upperBound.x = b2MaxFloat( a.upperBound.x, b.upperBound.x ); + c.upperBound.y = b2MaxFloat( a.upperBound.y, b.upperBound.y ); + return c; +} + +/// Is this a valid number? Not NaN or infinity. +B2_API bool b2IsValid( float a ); + +/// Is this a valid vector? Not NaN or infinity. +B2_API bool b2Vec2_IsValid( b2Vec2 v ); + +/// Is this a valid rotation? Not NaN or infinity. Is normalized. +B2_API bool b2Rot_IsValid( b2Rot q ); + +/// Is this a valid bounding box? Not Nan or infinity. Upper bound greater than or equal to lower bound. +B2_API bool b2AABB_IsValid( b2AABB aabb ); + +/// Box2D bases all length units on meters, but you may need different units for your game. +/// You can set this value to use different units. This should be done at application startup +/// and only modified once. Default value is 1. +/// @warning This must be modified before any calls to Box2D +B2_API void b2SetLengthUnitsPerMeter( float lengthUnits ); + +/// Get the current length units per meter. +B2_API float b2GetLengthUnitsPerMeter( void ); + +/**@}*/ + +/** + * @defgroup math_cpp C++ Math + * @brief Math operator overloads for C++ + * + * See math_functions.h for details. + * @{ + */ + +#ifdef __cplusplus + +/// Unary add one vector to another +inline void operator+=( b2Vec2& a, b2Vec2 b ) +{ + a.x += b.x; + a.y += b.y; +} + +/// Unary subtract one vector from another +inline void operator-=( b2Vec2& a, b2Vec2 b ) +{ + a.x -= b.x; + a.y -= b.y; +} + +/// Unary multiply a vector by a scalar +inline void operator*=( b2Vec2& a, float b ) +{ + a.x *= b; + a.y *= b; +} + +/// Unary negate a vector +inline b2Vec2 operator-( b2Vec2 a ) +{ + return { -a.x, -a.y }; +} + +/// Binary vector addition +inline b2Vec2 operator+( b2Vec2 a, b2Vec2 b ) +{ + return { a.x + b.x, a.y + b.y }; +} + +/// Binary vector subtraction +inline b2Vec2 operator-( b2Vec2 a, b2Vec2 b ) +{ + return { a.x - b.x, a.y - b.y }; +} + +/// Binary scalar and vector multiplication +inline b2Vec2 operator*( float a, b2Vec2 b ) +{ + return { a * b.x, a * b.y }; +} + +/// Binary scalar and vector multiplication +inline b2Vec2 operator*( b2Vec2 a, float b ) +{ + return { a.x * b, a.y * b }; +} + +/// Binary vector equality +inline bool operator==( b2Vec2 a, b2Vec2 b ) +{ + return a.x == b.x && a.y == b.y; +} + +/// Binary vector inequality +inline bool operator!=( b2Vec2 a, b2Vec2 b ) +{ + return a.x != b.x || a.y != b.y; +} + +#endif + +/**@}*/ diff --git a/3rdparty/box2d/include/box2d/types.h b/3rdparty/box2d/include/box2d/types.h new file mode 100644 index 000000000000..2bec968ba177 --- /dev/null +++ b/3rdparty/box2d/include/box2d/types.h @@ -0,0 +1,1315 @@ +// SPDX-FileCopyrightText: 2023 Erin Catto +// SPDX-License-Identifier: MIT + +#pragma once + +#include "base.h" +#include "collision.h" +#include "id.h" +#include "math_functions.h" + +#include +#include + +/// Task interface +/// This is prototype for a Box2D task. Your task system is expected to invoke the Box2D task with these arguments. +/// The task spans a range of the parallel-for: [startIndex, endIndex) +/// The worker index must correctly identify each worker in the user thread pool, expected in [0, workerCount). +/// A worker must only exist on only one thread at a time and is analogous to the thread index. +/// The task context is the context pointer sent from Box2D when it is enqueued. +/// The startIndex and endIndex are expected in the range [0, itemCount) where itemCount is the argument to b2EnqueueTaskCallback +/// below. Box2D expects startIndex < endIndex and will execute a loop like this: +/// +/// @code{.c} +/// for (int i = startIndex; i < endIndex; ++i) +/// { +/// DoWork(); +/// } +/// @endcode +/// @ingroup world +typedef void b2TaskCallback( int32_t startIndex, int32_t endIndex, uint32_t workerIndex, void* taskContext ); + +/// These functions can be provided to Box2D to invoke a task system. These are designed to work well with enkiTS. +/// Returns a pointer to the user's task object. May be nullptr. A nullptr indicates to Box2D that the work was executed +/// serially within the callback and there is no need to call b2FinishTaskCallback. +/// The itemCount is the number of Box2D work items that are to be partitioned among workers by the user's task system. +/// This is essentially a parallel-for. The minRange parameter is a suggestion of the minimum number of items to assign +/// per worker to reduce overhead. For example, suppose the task is small and that itemCount is 16. A minRange of 8 suggests +/// that your task system should split the work items among just two workers, even if you have more available. +/// In general the range [startIndex, endIndex) send to b2TaskCallback should obey: +/// endIndex - startIndex >= minRange +/// The exception of course is when itemCount < minRange. +/// @ingroup world +typedef void* b2EnqueueTaskCallback( b2TaskCallback* task, int32_t itemCount, int32_t minRange, void* taskContext, + void* userContext ); + +/// Finishes a user task object that wraps a Box2D task. +/// @ingroup world +typedef void b2FinishTaskCallback( void* userTask, void* userContext ); + +/// Result from b2World_RayCastClosest +/// @ingroup world +typedef struct b2RayResult +{ + b2ShapeId shapeId; + b2Vec2 point; + b2Vec2 normal; + float fraction; + bool hit; +} b2RayResult; + +/// World definition used to create a simulation world. +/// Must be initialized using b2DefaultWorldDef(). +/// @ingroup world +typedef struct b2WorldDef +{ + /// Gravity vector. Box2D has no up-vector defined. + b2Vec2 gravity; + + /// Restitution velocity threshold, usually in m/s. Collisions above this + /// speed have restitution applied (will bounce). + float restitutionThreshold; + + /// This parameter controls how fast overlap is resolved and has units of meters per second + float contactPushoutVelocity; + + /// Threshold velocity for hit events. Usually meters per second. + float hitEventThreshold; + + /// Contact stiffness. Cycles per second. + float contactHertz; + + /// Contact bounciness. Non-dimensional. + float contactDampingRatio; + + /// Joint stiffness. Cycles per second. + float jointHertz; + + /// Joint bounciness. Non-dimensional. + float jointDampingRatio; + + /// Maximum linear velocity. Usually meters per second. + float maximumLinearVelocity; + + /// Can bodies go to sleep to improve performance + bool enableSleep; + + /// Enable continuous collision + bool enableContinuous; + + /// Number of workers to use with the provided task system. Box2D performs best when using only + /// performance cores and accessing a single L2 cache. Efficiency cores and hyper-threading provide + /// little benefit and may even harm performance. + int32_t workerCount; + + /// Function to spawn tasks + b2EnqueueTaskCallback* enqueueTask; + + /// Function to finish a task + b2FinishTaskCallback* finishTask; + + /// User context that is provided to enqueueTask and finishTask + void* userTaskContext; + + /// Used internally to detect a valid definition. DO NOT SET. + int32_t internalValue; +} b2WorldDef; + +/// Use this to initialize your world definition +/// @ingroup world +B2_API b2WorldDef b2DefaultWorldDef( void ); + +/// The body simulation type. +/// Each body is one of these three types. The type determines how the body behaves in the simulation. +/// @ingroup body +typedef enum b2BodyType +{ + /// zero mass, zero velocity, may be manually moved + b2_staticBody = 0, + + /// zero mass, velocity set by user, moved by solver + b2_kinematicBody = 1, + + /// positive mass, velocity determined by forces, moved by solver + b2_dynamicBody = 2, + + /// number of body types + b2_bodyTypeCount, +} b2BodyType; + +/// A body definition holds all the data needed to construct a rigid body. +/// You can safely re-use body definitions. Shapes are added to a body after construction. +/// Body definitions are temporary objects used to bundle creation parameters. +/// Must be initialized using b2DefaultBodyDef(). +/// @ingroup body +typedef struct b2BodyDef +{ + /// The body type: static, kinematic, or dynamic. + b2BodyType type; + + /// The initial world position of the body. Bodies should be created with the desired position. + /// @note Creating bodies at the origin and then moving them nearly doubles the cost of body creation, especially + /// if the body is moved after shapes have been added. + b2Vec2 position; + + /// The initial world rotation of the body. Use b2MakeRot() if you have an angle. + b2Rot rotation; + + /// The initial linear velocity of the body's origin. Typically in meters per second. + b2Vec2 linearVelocity; + + /// The initial angular velocity of the body. Radians per second. + float angularVelocity; + + /// Linear damping is use to reduce the linear velocity. The damping parameter + /// can be larger than 1 but the damping effect becomes sensitive to the + /// time step when the damping parameter is large. + /// Generally linear damping is undesirable because it makes objects move slowly + /// as if they are floating. + 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. + /// Angular damping can be use slow down rotating bodies. + float angularDamping; + + /// Scale the gravity applied to this body. Non-dimensional. + float gravityScale; + + /// Sleep velocity threshold, default is 0.05 meter per second + float sleepThreshold; + + /// Use this to store application specific body data. + void* userData; + + /// Set this flag to false if this body should never fall asleep. + bool enableSleep; + + /// Is this body initially awake or sleeping? + bool isAwake; + + /// Should this body be prevented from rotating? Useful for characters. + bool fixedRotation; + + /// Treat this body as high speed object that performs continuous collision detection + /// against dynamic and kinematic bodies, but not other bullet bodies. + /// @warning Bullets should be used sparingly. They are not a solution for general dynamic-versus-dynamic + /// continuous collision. They may interfere with joint constraints. + bool isBullet; + + /// Used to disable a body. A disabled body does not move or collide. + bool isEnabled; + + /// Automatically compute mass and related properties on this body from shapes. + /// Triggers whenever a shape is add/removed/changed. Default is true. + bool automaticMass; + + /// This allows this body to bypass rotational speed limits. Should only be used + /// for circular objects, like wheels. + bool allowFastRotation; + + /// Used internally to detect a valid definition. DO NOT SET. + int32_t internalValue; +} b2BodyDef; + +/// Use this to initialize your body definition +/// @ingroup body +B2_API b2BodyDef b2DefaultBodyDef( void ); + +/// This is used to filter collision on shapes. It affects shape-vs-shape collision +/// and shape-versus-query collision (such as b2World_CastRay). +/// @ingroup shape +typedef struct b2Filter +{ + /// The collision category bits. Normally you would just set one bit. The category bits should + /// represent your application object types. For example: + /// @code{.cpp} + /// enum MyCategories + /// { + /// Static = 0x00000001, + /// Dynamic = 0x00000002, + /// Debris = 0x00000004, + /// Player = 0x00000008, + /// // etc + /// }; + /// @endcode + uint64_t categoryBits; + + /// The collision mask bits. This states the categories that this + /// shape would accept for collision. + /// For example, you may want your player to only collide with static objects + /// and other players. + /// @code{.c} + /// maskBits = Static | Player; + /// @endcode + uint64_t maskBits; + + /// Collision groups allow a certain group of objects to never collide (negative) + /// or always collide (positive). A group index of zero has no effect. Non-zero group filtering + /// always wins against the mask bits. + /// For example, you may want ragdolls to collide with other ragdolls but you don't want + /// ragdoll self-collision. In this case you would give each ragdoll a unique negative group index + /// and apply that group index to all shapes on the ragdoll. + int32_t groupIndex; +} b2Filter; + +/// Use this to initialize your filter +/// @ingroup shape +B2_API b2Filter b2DefaultFilter( void ); + +/// The query filter is used to filter collisions between queries and shapes. For example, +/// you may want a ray-cast representing a projectile to hit players and the static environment +/// but not debris. +/// @ingroup shape +typedef struct b2QueryFilter +{ + /// The collision category bits of this query. Normally you would just set one bit. + uint64_t categoryBits; + + /// The collision mask bits. This states the shape categories that this + /// query would accept for collision. + uint64_t maskBits; +} b2QueryFilter; + +/// Use this to initialize your query filter +/// @ingroup shape +B2_API b2QueryFilter b2DefaultQueryFilter( void ); + +/// Shape type +/// @ingroup shape +typedef enum b2ShapeType +{ + /// A circle with an offset + b2_circleShape, + + /// A capsule is an extruded circle + b2_capsuleShape, + + /// A line segment + b2_segmentShape, + + /// A convex polygon + b2_polygonShape, + + /// A line segment owned by a chain shape + b2_chainSegmentShape, + + /// The number of shape types + b2_shapeTypeCount +} b2ShapeType; + +/// Used to create a shape. +/// This is a temporary object used to bundle shape creation parameters. You may use +/// the same shape definition to create multiple shapes. +/// Must be initialized using b2DefaultShapeDef(). +/// @ingroup shape +typedef struct b2ShapeDef +{ + /// Use this to store application specific shape data. + void* userData; + + /// The Coulomb (dry) friction coefficient, usually in the range [0,1]. + float friction; + + /// The restitution (bounce) usually in the range [0,1]. + float restitution; + + /// The density, usually in kg/m^2. + float density; + + /// Collision filtering data. + b2Filter filter; + + /// Custom debug draw color. + uint32_t customColor; + + /// A sensor shape generates overlap events but never generates a collision response. + /// Sensors do not collide with other sensors and do not have continuous collision. + /// Instead use a ray or shape cast for those scenarios. + bool isSensor; + + /// Enable sensor events for this shape. Only applies to kinematic and dynamic bodies. Ignored for sensors. + bool enableSensorEvents; + + /// Enable contact events for this shape. Only applies to kinematic and dynamic bodies. Ignored for sensors. + bool enableContactEvents; + + /// Enable hit events for this shape. Only applies to kinematic and dynamic bodies. Ignored for sensors. + bool enableHitEvents; + + /// Enable pre-solve contact events for this shape. Only applies to dynamic bodies. These are expensive + /// and must be carefully handled due to threading. Ignored for sensors. + bool enablePreSolveEvents; + + /// Normally shapes on static bodies don't invoke contact creation when they are added to the world. This overrides + /// that behavior and causes contact creation. This significantly slows down static body creation which can be important + /// when there are many static shapes. + /// This is implicitly always true for sensors. + bool forceContactCreation; + + /// Used internally to detect a valid definition. DO NOT SET. + int32_t internalValue; +} b2ShapeDef; + +/// Use this to initialize your shape definition +/// @ingroup shape +B2_API b2ShapeDef b2DefaultShapeDef( void ); + +/// Used to create a chain of line segments. This is designed to eliminate ghost collisions with some limitations. +/// - chains are one-sided +/// - chains have no mass and should be used on static bodies +/// - chains have a counter-clockwise winding order +/// - chains are either a loop or open +/// - a chain must have at least 4 points +/// - the distance between any two points must be greater than b2_linearSlop +/// - a chain shape should not self intersect (this is not validated) +/// - an open chain shape has NO COLLISION on the first and final edge +/// - you may overlap two open chains on their first three and/or last three points to get smooth collision +/// - a chain shape creates multiple line segment shapes on the body +/// https://en.wikipedia.org/wiki/Polygonal_chain +/// Must be initialized using b2DefaultChainDef(). +/// @warning Do not use chain shapes unless you understand the limitations. This is an advanced feature. +/// @ingroup shape +typedef struct b2ChainDef +{ + /// Use this to store application specific shape data. + void* userData; + + /// An array of at least 4 points. These are cloned and may be temporary. + const b2Vec2* points; + + /// The point count, must be 4 or more. + int32_t count; + + /// The friction coefficient, usually in the range [0,1]. + float friction; + + /// The restitution (elasticity) usually in the range [0,1]. + float restitution; + + /// Contact filtering data. + b2Filter filter; + + /// Indicates a closed chain formed by connecting the first and last points + bool isLoop; + + /// Used internally to detect a valid definition. DO NOT SET. + int32_t internalValue; +} b2ChainDef; + +/// Use this to initialize your chain definition +/// @ingroup shape +B2_API b2ChainDef b2DefaultChainDef( void ); + +//! @cond +/// Profiling data. Times are in milliseconds. +typedef struct b2Profile +{ + float step; + float pairs; + float collide; + float solve; + float buildIslands; + float solveConstraints; + float prepareTasks; + float solverTasks; + float prepareConstraints; + float integrateVelocities; + float warmStart; + float solveVelocities; + float integratePositions; + float relaxVelocities; + float applyRestitution; + float storeImpulses; + float finalizeBodies; + float splitIslands; + float sleepIslands; + float hitEvents; + float broadphase; + float continuous; +} b2Profile; + +/// Counters that give details of the simulation size. +typedef struct b2Counters +{ + int32_t bodyCount; + int32_t shapeCount; + int32_t contactCount; + int32_t jointCount; + int32_t islandCount; + int32_t stackUsed; + int32_t staticTreeHeight; + int32_t treeHeight; + int32_t byteCount; + int32_t taskCount; + int32_t colorCounts[12]; +} b2Counters; +//! @endcond + +/// Joint type enumeration +/// +/// This is useful because all joint types use b2JointId and sometimes you +/// want to get the type of a joint. +/// @ingroup joint +typedef enum b2JointType +{ + b2_distanceJoint, + b2_motorJoint, + b2_mouseJoint, + b2_prismaticJoint, + b2_revoluteJoint, + b2_weldJoint, + b2_wheelJoint, +} b2JointType; + +/// Distance joint definition +/// +/// This requires defining an anchor point on both +/// bodies and the non-zero distance of the distance joint. The definition uses +/// local anchor points so that the initial configuration can violate the +/// constraint slightly. This helps when saving and loading a game. +/// @ingroup distance_joint +typedef struct b2DistanceJointDef +{ + /// The first attached body + b2BodyId bodyIdA; + + /// The second attached body + b2BodyId bodyIdB; + + /// The local anchor point relative to bodyA's origin + b2Vec2 localAnchorA; + + /// The local anchor point relative to bodyB's origin + b2Vec2 localAnchorB; + + /// The rest length of this joint. Clamped to a stable minimum value. + float length; + + /// Enable the distance constraint to behave like a spring. If false + /// then the distance joint will be rigid, overriding the limit and motor. + bool enableSpring; + + /// The spring linear stiffness Hertz, cycles per second + float hertz; + + /// The spring linear damping ratio, non-dimensional + float dampingRatio; + + /// Enable/disable the joint limit + bool enableLimit; + + /// Minimum length. Clamped to a stable minimum value. + float minLength; + + /// Maximum length. Must be greater than or equal to the minimum length. + float maxLength; + + /// Enable/disable the joint motor + bool enableMotor; + + /// The maximum motor force, usually in newtons + float maxMotorForce; + + /// The desired motor speed, usually in meters per second + float motorSpeed; + + /// Set this flag to true if the attached bodies should collide + bool collideConnected; + + /// User data pointer + void* userData; + + /// Used internally to detect a valid definition. DO NOT SET. + int32_t internalValue; +} b2DistanceJointDef; + +/// Use this to initialize your joint definition +/// @ingroup distance_joint +B2_API b2DistanceJointDef b2DefaultDistanceJointDef( void ); + +/// 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. +/// @ingroup motor_joint +typedef struct b2MotorJointDef +{ + /// The first attached body + b2BodyId bodyIdA; + + /// The second attached body + b2BodyId bodyIdB; + + /// Position of bodyB minus the position of bodyA, in bodyA's frame + b2Vec2 linearOffset; + + /// The bodyB angle minus bodyA angle in radians + float angularOffset; + + /// The maximum motor force in newtons + float maxForce; + + /// The maximum motor torque in newton-meters + float maxTorque; + + /// Position correction factor in the range [0,1] + float correctionFactor; + + /// Set this flag to true if the attached bodies should collide + bool collideConnected; + + /// User data pointer + void* userData; + + /// Used internally to detect a valid definition. DO NOT SET. + int32_t internalValue; +} b2MotorJointDef; + +/// Use this to initialize your joint definition +/// @ingroup motor_joint +B2_API b2MotorJointDef b2DefaultMotorJointDef( void ); + +/// A mouse joint is used to make a point on a body track a specified world point. +/// +/// This a soft constraint and allows the constraint to stretch without +/// applying huge forces. This also applies rotation constraint heuristic to improve control. +/// @ingroup mouse_joint +typedef struct b2MouseJointDef +{ + /// The first attached body. + b2BodyId bodyIdA; + + /// The second attached body. + b2BodyId bodyIdB; + + /// The initial target point in world space + b2Vec2 target; + + /// Stiffness in hertz + float hertz; + + /// Damping ratio, non-dimensional + float dampingRatio; + + /// Maximum force, typically in newtons + float maxForce; + + /// Set this flag to true if the attached bodies should collide. + bool collideConnected; + + /// User data pointer + void* userData; + + /// Used internally to detect a valid definition. DO NOT SET. + int32_t internalValue; +} b2MouseJointDef; + +/// Use this to initialize your joint definition +/// @ingroup mouse_joint +B2_API b2MouseJointDef b2DefaultMouseJointDef( void ); + +/// Prismatic joint definition +/// +/// This requires defining a line of motion using an axis and an anchor point. +/// The definition uses local anchor points and a local axis so that the initial +/// configuration can violate the constraint slightly. The joint translation is zero +/// when the local anchor points coincide in world space. +/// @ingroup prismatic_joint +typedef struct b2PrismaticJointDef +{ + /// The first attached body + b2BodyId bodyIdA; + + /// The second attached body + b2BodyId bodyIdB; + + /// The local anchor point relative to bodyA's origin + b2Vec2 localAnchorA; + + /// The local anchor point relative to bodyB's origin + b2Vec2 localAnchorB; + + /// The local translation unit axis in bodyA + b2Vec2 localAxisA; + + /// The constrained angle between the bodies: bodyB_angle - bodyA_angle + float referenceAngle; + + /// Enable a linear spring along the prismatic joint axis + bool enableSpring; + + /// The spring stiffness Hertz, cycles per second + float hertz; + + /// The spring damping ratio, non-dimensional + float dampingRatio; + + /// Enable/disable the joint limit + bool enableLimit; + + /// The lower translation limit + float lowerTranslation; + + /// The upper translation limit + float upperTranslation; + + /// Enable/disable the joint motor + bool enableMotor; + + /// The maximum motor force, typically in newtons + float maxMotorForce; + + /// The desired motor speed, typically in meters per second + float motorSpeed; + + /// Set this flag to true if the attached bodies should collide + bool collideConnected; + + /// User data pointer + void* userData; + + /// Used internally to detect a valid definition. DO NOT SET. + int32_t internalValue; +} b2PrismaticJointDef; + +/// Use this to initialize your joint definition +/// @ingroupd prismatic_joint +B2_API b2PrismaticJointDef b2DefaultPrismaticJointDef( void ); + +/// Revolute joint definition +/// +/// This requires defining an anchor point where the bodies are joined. +/// The definition uses local anchor points so that the +/// initial configuration can violate the constraint slightly. You also need to +/// specify the initial relative angle for joint limits. This helps when saving +/// and loading a game. +/// The local anchor points are measured from the body's origin +/// rather than the center of mass because: +/// 1. you might not know where the center of mass will be +/// 2. if you add/remove shapes from a body and recompute the mass, the joints will be broken +/// @ingroup revolute_joint +typedef struct b2RevoluteJointDef +{ + /// The first attached body + b2BodyId bodyIdA; + + /// The second attached body + b2BodyId bodyIdB; + + /// 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). + /// This defines the zero angle for the joint limit. + float referenceAngle; + + /// Enable a rotational spring on the revolute hinge axis + bool enableSpring; + + /// The spring stiffness Hertz, cycles per second + float hertz; + + /// The spring damping ratio, non-dimensional + float dampingRatio; + + /// A flag to enable joint limits + bool enableLimit; + + /// The lower angle for the joint limit in radians + float lowerAngle; + + /// The upper angle for the joint limit in radians + float upperAngle; + + /// A flag to enable the joint motor + bool enableMotor; + + /// The maximum motor torque, typically in newton-meters + float maxMotorTorque; + + /// The desired motor speed in radians per second + float motorSpeed; + + /// Scale the debug draw + float drawSize; + + /// Set this flag to true if the attached bodies should collide + bool collideConnected; + + /// User data pointer + void* userData; + + /// Used internally to detect a valid definition. DO NOT SET. + int32_t internalValue; +} b2RevoluteJointDef; + +/// Use this to initialize your joint definition. +/// @ingroup revolute_joint +B2_API b2RevoluteJointDef b2DefaultRevoluteJointDef( void ); + +/// Weld joint definition +/// +/// A weld joint connect to bodies together rigidly. This constraint provides springs to mimic +/// soft-body simulation. +/// @note The approximate solver in Box2D cannot hold many bodies together rigidly +/// @ingroup weld_joint +typedef struct b2WeldJointDef +{ + /// The first attached body + b2BodyId bodyIdA; + + /// The second attached body + b2BodyId bodyIdB; + + /// 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; + + /// Linear stiffness expressed as Hertz (cycles per second). Use zero for maximum stiffness. + float linearHertz; + + /// Angular stiffness as Hertz (cycles per second). Use zero for maximum stiffness. + float angularHertz; + + /// Linear damping ratio, non-dimensional. Use 1 for critical damping. + float linearDampingRatio; + + /// Linear damping ratio, non-dimensional. Use 1 for critical damping. + float angularDampingRatio; + + /// Set this flag to true if the attached bodies should collide + bool collideConnected; + + /// User data pointer + void* userData; + + /// Used internally to detect a valid definition. DO NOT SET. + int32_t internalValue; +} b2WeldJointDef; + +/// Use this to initialize your joint definition +/// @ingroup weld_joint +B2_API b2WeldJointDef b2DefaultWeldJointDef( void ); + +/// 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. +/// @ingroup wheel_joint +typedef struct b2WheelJointDef +{ + /// The first attached body + b2BodyId bodyIdA; + + /// The second attached body + b2BodyId bodyIdB; + + /// The local anchor point relative to bodyA's origin + b2Vec2 localAnchorA; + + /// The local anchor point relative to bodyB's origin + b2Vec2 localAnchorB; + + /// The local translation unit axis in bodyA + b2Vec2 localAxisA; + + /// Enable a linear spring along the local axis + bool enableSpring; + + /// Spring stiffness in Hertz + float hertz; + + /// Spring damping ratio, non-dimensional + float dampingRatio; + + /// Enable/disable the joint linear limit + bool enableLimit; + + /// The lower translation limit + float lowerTranslation; + + /// The upper translation limit + float upperTranslation; + + /// Enable/disable the joint rotational motor + bool enableMotor; + + /// The maximum motor torque, typically in newton-meters + float maxMotorTorque; + + /// The desired motor speed in radians per second + float motorSpeed; + + /// Set this flag to true if the attached bodies should collide + bool collideConnected; + + /// User data pointer + void* userData; + + /// Used internally to detect a valid definition. DO NOT SET. + int32_t internalValue; +} b2WheelJointDef; + +/// Use this to initialize your joint definition +/// @ingroup wheel_joint +B2_API b2WheelJointDef b2DefaultWheelJointDef( void ); + +/** + * @defgroup events Events + * World event types. + * + * Events are used to collect events that occur during the world time step. These events + * are then available to query after the time step is complete. This is preferable to callbacks + * because Box2D uses multithreaded simulation. + * + * Also when events occur in the simulation step it may be problematic to modify the world, which is + * often what applications want to do when events occur. + * + * With event arrays, you can scan the events in a loop and modify the world. However, you need to be careful + * that some event data may become invalid. There are several samples that show how to do this safely. + * + * @{ + */ + +/// A begin touch event is generated when a shape starts to overlap a sensor shape. +typedef struct b2SensorBeginTouchEvent +{ + /// The id of the sensor shape + b2ShapeId sensorShapeId; + + /// The id of the dynamic shape that began touching the sensor shape + b2ShapeId visitorShapeId; +} b2SensorBeginTouchEvent; + +/// An end touch event is generated when a shape stops overlapping a sensor shape. +typedef struct b2SensorEndTouchEvent +{ + /// The id of the sensor shape + b2ShapeId sensorShapeId; + + /// The id of the dynamic shape that stopped touching the sensor shape + b2ShapeId visitorShapeId; +} b2SensorEndTouchEvent; + +/// Sensor events are buffered in the Box2D world and are available +/// as begin/end overlap event arrays after the time step is complete. +/// Note: these may become invalid if bodies and/or shapes are destroyed +typedef struct b2SensorEvents +{ + /// Array of sensor begin touch events + b2SensorBeginTouchEvent* beginEvents; + + /// Array of sensor end touch events + b2SensorEndTouchEvent* endEvents; + + /// The number of begin touch events + int32_t beginCount; + + /// The number of end touch events + int32_t endCount; +} b2SensorEvents; + +/// A begin touch event is generated when two shapes begin touching. +typedef struct b2ContactBeginTouchEvent +{ + /// Id of the first shape + b2ShapeId shapeIdA; + + /// Id of the second shape + b2ShapeId shapeIdB; +} b2ContactBeginTouchEvent; + +/// An end touch event is generated when two shapes stop touching. +typedef struct b2ContactEndTouchEvent +{ + /// Id of the first shape + b2ShapeId shapeIdA; + + /// Id of the second shape + b2ShapeId shapeIdB; +} b2ContactEndTouchEvent; + +/// A hit touch event is generated when two shapes collide with a speed faster than the hit speed threshold. +typedef struct b2ContactHitEvent +{ + /// Id of the first shape + b2ShapeId shapeIdA; + + /// Id of the second shape + b2ShapeId shapeIdB; + + /// Point where the shapes hit + b2Vec2 point; + + /// Normal vector pointing from shape A to shape B + b2Vec2 normal; + + /// The speed the shapes are approaching. Always positive. Typically in meters per second. + float approachSpeed; +} b2ContactHitEvent; + +/// Contact events are buffered in the Box2D world and are available +/// as event arrays after the time step is complete. +/// Note: these may become invalid if bodies and/or shapes are destroyed +typedef struct b2ContactEvents +{ + /// Array of begin touch events + b2ContactBeginTouchEvent* beginEvents; + + /// Array of end touch events + b2ContactEndTouchEvent* endEvents; + + /// Array of hit events + b2ContactHitEvent* hitEvents; + + /// Number of begin touch events + int32_t beginCount; + + /// Number of end touch events + int32_t endCount; + + /// Number of hit events + int32_t hitCount; +} b2ContactEvents; + +/// Body move events triggered when a body moves. +/// Triggered when a body moves due to simulation. Not reported for bodies moved by the user. +/// This also has a flag to indicate that the body went to sleep so the application can also +/// sleep that actor/entity/object associated with the body. +/// On the other hand if the flag does not indicate the body went to sleep then the application +/// can treat the actor/entity/object associated with the body as awake. +/// This is an efficient way for an application to update game object transforms rather than +/// calling functions such as b2Body_GetTransform() because this data is delivered as a contiguous array +/// and it is only populated with bodies that have moved. +/// @note If sleeping is disabled all dynamic and kinematic bodies will trigger move events. +typedef struct b2BodyMoveEvent +{ + b2Transform transform; + b2BodyId bodyId; + void* userData; + bool fellAsleep; +} b2BodyMoveEvent; + +/// Body events are buffered in the Box2D world and are available +/// as event arrays after the time step is complete. +/// Note: this data becomes invalid if bodies are destroyed +typedef struct b2BodyEvents +{ + /// Array of move events + b2BodyMoveEvent* moveEvents; + + /// Number of move events + int32_t moveCount; +} b2BodyEvents; + +/// The contact data for two shapes. By convention the manifold normal points +/// from shape A to shape B. +/// @see b2Shape_GetContactData() and b2Body_GetContactData() +typedef struct b2ContactData +{ + b2ShapeId shapeIdA; + b2ShapeId shapeIdB; + b2Manifold manifold; +} b2ContactData; + +/**@}*/ + +/// Prototype for a contact filter callback. +/// This is called when a contact pair is considered for collision. This allows you to +/// perform custom logic to prevent collision between shapes. This is only called if +/// one of the two shapes has custom filtering enabled. @see b2ShapeDef. +/// Notes: +/// - this function must be thread-safe +/// - this is only called if one of the two shapes has enabled custom filtering +/// - this is called only for awake dynamic bodies +/// Return false if you want to disable the collision +/// @warning Do not attempt to modify the world inside this callback +/// @ingroup world +typedef bool b2CustomFilterFcn( b2ShapeId shapeIdA, b2ShapeId shapeIdB, void* context ); + +/// Prototype for a pre-solve callback. +/// This is called after a contact is updated. This allows you to inspect a +/// contact before it goes to the solver. If you are careful, you can modify the +/// contact manifold (e.g. modify the normal). +/// Notes: +/// - this function must be thread-safe +/// - this is only called if the shape has enabled pre-solve events +/// - this is called only for awake dynamic bodies +/// - this is not called for sensors +/// - the supplied manifold has impulse values from the previous step +/// Return false if you want to disable the contact this step +/// @warning Do not attempt to modify the world inside this callback +/// @ingroup world +typedef bool b2PreSolveFcn( b2ShapeId shapeIdA, b2ShapeId shapeIdB, b2Manifold* manifold, void* context ); + +/// Prototype callback for overlap queries. +/// Called for each shape found in the query. +/// @see b2World_QueryAABB +/// @return false to terminate the query. +/// @ingroup world +typedef bool b2OverlapResultFcn( b2ShapeId shapeId, void* context ); + +/// Prototype callback for ray casts. +/// Called for each shape found in the query. You control how the ray cast +/// proceeds by returning a float: +/// return -1: ignore this shape and continue +/// return 0: terminate the ray cast +/// return fraction: clip the ray to this point +/// return 1: don't clip the ray and continue +/// @param shapeId the shape 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 +/// @param context the user context +/// @return -1 to filter, 0 to terminate, fraction to clip the ray for closest hit, 1 to continue +/// @see b2World_CastRay +/// @ingroup world +typedef float b2CastResultFcn( b2ShapeId shapeId, b2Vec2 point, b2Vec2 normal, float fraction, void* context ); + +/// These colors are used for debug draw. +/// See https://www.rapidtables.com/web/color/index.html +typedef enum b2HexColor +{ + b2_colorAliceBlue = 0xf0f8ff, + b2_colorAntiqueWhite = 0xfaebd7, + b2_colorAquamarine = 0x7fffd4, + b2_colorAzure = 0xf0ffff, + b2_colorBeige = 0xf5f5dc, + b2_colorBisque = 0xffe4c4, + b2_colorBlack = 0x000000, + b2_colorBlanchedAlmond = 0xffebcd, + b2_colorBlue = 0x0000ff, + b2_colorBlueViolet = 0x8a2be2, + b2_colorBrown = 0xa52a2a, + b2_colorBurlywood = 0xdeb887, + b2_colorCadetBlue = 0x5f9ea0, + b2_colorChartreuse = 0x7fff00, + b2_colorChocolate = 0xd2691e, + b2_colorCoral = 0xff7f50, + b2_colorCornflowerBlue = 0x6495ed, + b2_colorCornsilk = 0xfff8dc, + b2_colorCrimson = 0xdc143c, + b2_colorCyan = 0x00ffff, + b2_colorDarkBlue = 0x00008b, + b2_colorDarkCyan = 0x008b8b, + b2_colorDarkGoldenrod = 0xb8860b, + b2_colorDarkGray = 0xa9a9a9, + b2_colorDarkGreen = 0x006400, + b2_colorDarkKhaki = 0xbdb76b, + b2_colorDarkMagenta = 0x8b008b, + b2_colorDarkOliveGreen = 0x556b2f, + b2_colorDarkOrange = 0xff8c00, + b2_colorDarkOrchid = 0x9932cc, + b2_colorDarkRed = 0x8b0000, + b2_colorDarkSalmon = 0xe9967a, + b2_colorDarkSeaGreen = 0x8fbc8f, + b2_colorDarkSlateBlue = 0x483d8b, + b2_colorDarkSlateGray = 0x2f4f4f, + b2_colorDarkTurquoise = 0x00ced1, + b2_colorDarkViolet = 0x9400d3, + b2_colorDeepPink = 0xff1493, + b2_colorDeepSkyBlue = 0x00bfff, + b2_colorDimGray = 0x696969, + b2_colorDodgerBlue = 0x1e90ff, + b2_colorFirebrick = 0xb22222, + b2_colorFloralWhite = 0xfffaf0, + b2_colorForestGreen = 0x228b22, + b2_colorGainsboro = 0xdcdcdc, + b2_colorGhostWhite = 0xf8f8ff, + b2_colorGold = 0xffd700, + b2_colorGoldenrod = 0xdaa520, + b2_colorGray = 0xbebebe, + b2_colorGray1 = 0x1a1a1a, + b2_colorGray2 = 0x333333, + b2_colorGray3 = 0x4d4d4d, + b2_colorGray4 = 0x666666, + b2_colorGray5 = 0x7f7f7f, + b2_colorGray6 = 0x999999, + b2_colorGray7 = 0xb3b3b3, + b2_colorGray8 = 0xcccccc, + b2_colorGray9 = 0xe5e5e5, + b2_colorGreen = 0x00ff00, + b2_colorGreenYellow = 0xadff2f, + b2_colorHoneydew = 0xf0fff0, + b2_colorHotPink = 0xff69b4, + b2_colorIndianRed = 0xcd5c5c, + b2_colorIndigo = 0x4b0082, + b2_colorIvory = 0xfffff0, + b2_colorKhaki = 0xf0e68c, + b2_colorLavender = 0xe6e6fa, + b2_colorLavenderBlush = 0xfff0f5, + b2_colorLawnGreen = 0x7cfc00, + b2_colorLemonChiffon = 0xfffacd, + b2_colorLightBlue = 0xadd8e6, + b2_colorLightCoral = 0xf08080, + b2_colorLightCyan = 0xe0ffff, + b2_colorLightGoldenrod = 0xeedd82, + b2_colorLightGoldenrodYellow = 0xfafad2, + b2_colorLightGray = 0xd3d3d3, + b2_colorLightGreen = 0x90ee90, + b2_colorLightPink = 0xffb6c1, + b2_colorLightSalmon = 0xffa07a, + b2_colorLightSeaGreen = 0x20b2aa, + b2_colorLightSkyBlue = 0x87cefa, + b2_colorLightSlateBlue = 0x8470ff, + b2_colorLightSlateGray = 0x778899, + b2_colorLightSteelBlue = 0xb0c4de, + b2_colorLightYellow = 0xffffe0, + b2_colorLimeGreen = 0x32cd32, + b2_colorLinen = 0xfaf0e6, + b2_colorMagenta = 0xff00ff, + b2_colorMaroon = 0xb03060, + b2_colorMediumAquamarine = 0x66cdaa, + b2_colorMediumBlue = 0x0000cd, + b2_colorMediumOrchid = 0xba55d3, + b2_colorMediumPurple = 0x9370db, + b2_colorMediumSeaGreen = 0x3cb371, + b2_colorMediumSlateBlue = 0x7b68ee, + b2_colorMediumSpringGreen = 0x00fa9a, + b2_colorMediumTurquoise = 0x48d1cc, + b2_colorMediumVioletRed = 0xc71585, + b2_colorMidnightBlue = 0x191970, + b2_colorMintCream = 0xf5fffa, + b2_colorMistyRose = 0xffe4e1, + b2_colorMoccasin = 0xffe4b5, + b2_colorNavajoWhite = 0xffdead, + b2_colorNavyBlue = 0x000080, + b2_colorOldLace = 0xfdf5e6, + b2_colorOlive = 0x808000, + b2_colorOliveDrab = 0x6b8e23, + b2_colorOrange = 0xffa500, + b2_colorOrangeRed = 0xff4500, + b2_colorOrchid = 0xda70d6, + b2_colorPaleGoldenrod = 0xeee8aa, + b2_colorPaleGreen = 0x98fb98, + b2_colorPaleTurquoise = 0xafeeee, + b2_colorPaleVioletRed = 0xdb7093, + b2_colorPapayaWhip = 0xffefd5, + b2_colorPeachPuff = 0xffdab9, + b2_colorPeru = 0xcd853f, + b2_colorPink = 0xffc0cb, + b2_colorPlum = 0xdda0dd, + b2_colorPowderBlue = 0xb0e0e6, + b2_colorPurple = 0xa020f0, + b2_colorRebeccaPurple = 0x663399, + b2_colorRed = 0xff0000, + b2_colorRosyBrown = 0xbc8f8f, + b2_colorRoyalBlue = 0x4169e1, + b2_colorSaddleBrown = 0x8b4513, + b2_colorSalmon = 0xfa8072, + b2_colorSandyBrown = 0xf4a460, + b2_colorSeaGreen = 0x2e8b57, + b2_colorSeashell = 0xfff5ee, + b2_colorSienna = 0xa0522d, + b2_colorSilver = 0xc0c0c0, + b2_colorSkyBlue = 0x87ceeb, + b2_colorSlateBlue = 0x6a5acd, + b2_colorSlateGray = 0x708090, + b2_colorSnow = 0xfffafa, + b2_colorSpringGreen = 0x00ff7f, + b2_colorSteelBlue = 0x4682b4, + b2_colorTan = 0xd2b48c, + b2_colorTeal = 0x008080, + b2_colorThistle = 0xd8bfd8, + b2_colorTomato = 0xff6347, + b2_colorTurquoise = 0x40e0d0, + b2_colorViolet = 0xee82ee, + b2_colorVioletRed = 0xd02090, + b2_colorWheat = 0xf5deb3, + b2_colorWhite = 0xffffff, + b2_colorWhiteSmoke = 0xf5f5f5, + b2_colorYellow = 0xffff00, + b2_colorYellowGreen = 0x9acd32, + b2_colorBox2DRed = 0xdc3132, + b2_colorBox2DBlue = 0x30aebf, + b2_colorBox2DGreen = 0x8cc924, + b2_colorBox2DYellow = 0xffee8c +} b2HexColor; + +/// This struct holds callbacks you can implement to draw a Box2D world. +/// This structure should be zero initialized. +/// @ingroup world +typedef struct b2DebugDraw +{ + /// Draw a closed polygon provided in CCW order. + void ( *DrawPolygon )( const b2Vec2* vertices, int vertexCount, b2HexColor color, void* context ); + + /// Draw a solid closed polygon provided in CCW order. + void ( *DrawSolidPolygon )( b2Transform transform, const b2Vec2* vertices, int vertexCount, float radius, b2HexColor color, + void* context ); + + /// Draw a circle. + void ( *DrawCircle )( b2Vec2 center, float radius, b2HexColor color, void* context ); + + /// Draw a solid circle. + void ( *DrawSolidCircle )( b2Transform transform, float radius, b2HexColor color, void* context ); + + /// Draw a solid capsule. + void ( *DrawSolidCapsule )( b2Vec2 p1, b2Vec2 p2, float radius, b2HexColor color, void* context ); + + /// Draw a line segment. + void ( *DrawSegment )( b2Vec2 p1, b2Vec2 p2, b2HexColor color, void* context ); + + /// Draw a transform. Choose your own length scale. + void ( *DrawTransform )( b2Transform transform, void* context ); + + /// Draw a point. + void ( *DrawPoint )( b2Vec2 p, float size, b2HexColor color, void* context ); + + /// Draw a string. + void ( *DrawString )( b2Vec2 p, const char* s, void* context ); + + /// Bounds to use if restricting drawing to a rectangular region + b2AABB drawingBounds; + + /// Option to restrict drawing to a rectangular region. May suffer from unstable depth sorting. + bool useDrawingBounds; + + /// Option to draw shapes + bool drawShapes; + + /// Option to draw joints + bool drawJoints; + + /// Option to draw additional information for joints + bool drawJointExtras; + + /// Option to draw the bounding boxes for shapes + bool drawAABBs; + + /// Option to draw the mass and center of mass of dynamic bodies + bool drawMass; + + /// Option to draw contact points + bool drawContacts; + + /// Option to visualize the graph coloring used for contacts and joints + bool drawGraphColors; + + /// Option to draw contact normals + bool drawContactNormals; + + /// Option to draw contact normal impulses + bool drawContactImpulses; + + /// Option to draw contact friction impulses + bool drawFrictionImpulses; + + /// User context that is passed as an argument to drawing callback functions + void* context; +} b2DebugDraw; + +/// Use this to initialize your drawing interface. This allows you to implement a sub-set +/// of the drawing functions. +B2_API b2DebugDraw b2DefaultDebugDraw( void ); diff --git a/3rdparty/box2d/src/CMakeLists.txt b/3rdparty/box2d/src/CMakeLists.txt new file mode 100644 index 000000000000..695fff850172 --- /dev/null +++ b/3rdparty/box2d/src/CMakeLists.txt @@ -0,0 +1,205 @@ +set(BOX2D_SOURCE_FILES + aabb.c + aabb.h + array.c + array.h + bitset.c + bitset.h + body.c + body.h + broad_phase.c + broad_phase.h + constraint_graph.c + constraint_graph.h + contact.c + contact.h + contact_solver.c + contact_solver.h + core.c + core.h + ctz.h + distance.c + distance_joint.c + dynamic_tree.c + geometry.c + hull.c + id_pool.c + id_pool.h + island.c + island.h + joint.c + joint.h + manifold.c + math_functions.c + motor_joint.c + mouse_joint.c + prismatic_joint.c + revolute_joint.c + shape.c + shape.h + solver.c + solver.h + solver_set.c + solver_set.h + stack_allocator.c + stack_allocator.h + table.c + table.h + timer.c + types.c + weld_joint.c + wheel_joint.c + world.c + world.h +) + +set(BOX2D_API_FILES + ../include/box2d/base.h + ../include/box2d/box2d.h + ../include/box2d/collision.h + ../include/box2d/id.h + ../include/box2d/math_functions.h + ../include/box2d/types.h +) + +# Hide internal functions +# todo need to investigate this more +# https://gcc.gnu.org/wiki/Visibility +set(CMAKE_C_VISIBILITY_PRESET hidden) +set(CMAKE_VISIBILITY_INLINES_HIDDEN 1) + +add_library(box2d ${BOX2D_SOURCE_FILES} ${BOX2D_API_FILES}) + +# Generate box2d_export.h to handles shared library builds +# turned this off to make Box2D easier to use without cmake +# include(GenerateExportHeader) +# generate_export_header(box2d) + +target_include_directories(box2d + PUBLIC + $ + $ + $ + PRIVATE + ${CMAKE_CURRENT_SOURCE_DIR} +) + +set(CMAKE_DEBUG_POSTFIX "d") + +# Box2D uses C17 +set_target_properties(box2d PROPERTIES + C_STANDARD 17 + C_STANDARD_REQUIRED YES + C_EXTENSIONS YES + VERSION ${PROJECT_VERSION} + SOVERSION ${PROJECT_VERSION_MAJOR} + DEBUG_POSTFIX ${CMAKE_DEBUG_POSTFIX} +) + +if (BOX2D_PROFILE) + target_compile_definitions(box2d PRIVATE BOX2D_PROFILE) + + FetchContent_Declare( + tracy + GIT_REPOSITORY https://github.com/wolfpld/tracy.git + GIT_TAG master + GIT_SHALLOW TRUE + GIT_PROGRESS TRUE + ) + FetchContent_MakeAvailable(tracy) + + target_link_libraries(box2d PUBLIC TracyClient) +endif() + +if (BOX2D_VALIDATE) + target_compile_definitions(box2d PRIVATE BOX2D_VALIDATE) +endif() + +if (BOX2D_ENABLE_SIMD) + target_compile_definitions(box2d PRIVATE BOX2D_ENABLE_SIMD) +endif() + +if (MSVC) + message(STATUS "Box2D on MSVC") + if (BUILD_SHARED_LIBS) + # this is needed by DLL users to import Box2D symbols + target_compile_definitions(box2d INTERFACE BOX2D_DLL) + endif() + + # Visual Studio won't load the natvis unless it is in the project + target_sources(box2d PRIVATE box2d.natvis) + + # Enable asserts in release with debug info + target_compile_definitions(box2d PUBLIC "$<$:B2_ENABLE_ASSERT>") + + # Atomics are still considered experimental in Visual Studio 17.8 + target_compile_options(box2d PRIVATE /experimental:c11atomics) + + if (BOX2D_AVX2) + message(STATUS "Box2D using AVX2") + target_compile_definitions(box2d PRIVATE BOX2D_AVX2) + target_compile_options(box2d PRIVATE /arch:AVX2) + endif() + +elseif (MINGW) + message(STATUS "Box2D on MinGW") + if (BOX2D_AVX2) + message(STATUS "Box2D using AVX2") + target_compile_definitions(box2d PRIVATE BOX2D_AVX2) + target_compile_options(box2d PRIVATE -mavx2) + else() + endif() +elseif (APPLE) + message(STATUS "Box2D on Apple") +elseif (EMSCRIPTEN) + message(STATUS "Box2D on Emscripten") + target_compile_options(box2d PRIVATE -msimd128 -msse2) +elseif (UNIX) + message(STATUS "Box2D using Unix") + if ("${CMAKE_HOST_SYSTEM_PROCESSOR}" STREQUAL "aarch64") + # raspberry pi + # -mfpu=neon + # target_compile_options(box2d PRIVATE) + else() + # x64 + if (BOX2D_AVX2) + message(STATUS "Box2D using AVX2") + # FMA -mfma -mavx -march=native + target_compile_definitions(box2d PRIVATE BOX2D_AVX2) + target_compile_options(box2d PRIVATE -mavx2) + else() + endif() + endif() +endif() + +source_group(TREE "${CMAKE_CURRENT_SOURCE_DIR}" PREFIX "src" FILES ${BOX2D_SOURCE_FILES}) +source_group(TREE "${CMAKE_CURRENT_SOURCE_DIR}/../include" PREFIX "include" FILES ${BOX2D_API_FILES}) + +add_library(box2d::box2d ALIAS box2d) + +install( + TARGETS box2d + EXPORT box2dConfig + LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} + ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} + RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} +) + +install( + EXPORT box2dConfig + NAMESPACE box2d:: + DESTINATION "${CMAKE_INSTALL_LIBDIR}/cmake/box2d" +) + +include(CMakePackageConfigHelpers) + +write_basic_package_version_file( + "${CMAKE_CURRENT_BINARY_DIR}/box2dConfigVersion.cmake" + COMPATIBILITY SameMajorVersion +) + +install( + FILES "${CMAKE_CURRENT_BINARY_DIR}/box2dConfigVersion.cmake" + DESTINATION "${CMAKE_INSTALL_LIBDIR}/cmake/box2d" +) + diff --git a/3rdparty/box2d/src/aabb.c b/3rdparty/box2d/src/aabb.c new file mode 100644 index 000000000000..2fabfa50d987 --- /dev/null +++ b/3rdparty/box2d/src/aabb.c @@ -0,0 +1,155 @@ +// SPDX-FileCopyrightText: 2023 Erin Catto +// SPDX-License-Identifier: MIT + +#include "aabb.h" + +#include "box2d/math_functions.h" + +#include + +bool b2AABB_IsValid( b2AABB a ) +{ + b2Vec2 d = b2Sub( a.upperBound, a.lowerBound ); + bool valid = d.x >= 0.0f && d.y >= 0.0f; + valid = valid && b2Vec2_IsValid( a.lowerBound ) && b2Vec2_IsValid( a.upperBound ); + return valid; +} + +// From Real-time Collision Detection, p179. +b2CastOutput b2AABB_RayCast( b2AABB a, b2Vec2 p1, b2Vec2 p2 ) +{ + // Radius not handled + b2CastOutput output = { 0 }; + + float tmin = -FLT_MAX; + float tmax = FLT_MAX; + + b2Vec2 p = p1; + b2Vec2 d = b2Sub( p2, p1 ); + b2Vec2 absD = b2Abs( d ); + + b2Vec2 normal = b2Vec2_zero; + + // x-coordinate + if ( absD.x < FLT_EPSILON ) + { + // parallel + if ( p.x < a.lowerBound.x || a.upperBound.x < p.x ) + { + return output; + } + } + else + { + float inv_d = 1.0f / d.x; + float t1 = ( a.lowerBound.x - p.x ) * inv_d; + float t2 = ( a.upperBound.x - p.x ) * inv_d; + + // Sign of the normal vector. + float s = -1.0f; + + if ( t1 > t2 ) + { + float tmp = t1; + t1 = t2; + t2 = tmp; + s = 1.0f; + } + + // Push the min up + if ( t1 > tmin ) + { + normal.y = 0.0f; + normal.x = s; + tmin = t1; + } + + // Pull the max down + tmax = b2MinFloat( tmax, t2 ); + + if ( tmin > tmax ) + { + return output; + } + } + + // y-coordinate + if ( absD.y < FLT_EPSILON ) + { + // parallel + if ( p.y < a.lowerBound.y || a.upperBound.y < p.y ) + { + return output; + } + } + else + { + float inv_d = 1.0f / d.y; + float t1 = ( a.lowerBound.y - p.y ) * inv_d; + float t2 = ( a.upperBound.y - p.y ) * inv_d; + + // Sign of the normal vector. + float s = -1.0f; + + if ( t1 > t2 ) + { + float tmp = t1; + t1 = t2; + t2 = tmp; + s = 1.0f; + } + + // Push the min up + if ( t1 > tmin ) + { + normal.x = 0.0f; + normal.y = s; + tmin = t1; + } + + // Pull the max down + tmax = b2MinFloat( tmax, t2 ); + + if ( tmin > tmax ) + { + return output; + } + } + + // Does the ray start inside the box? + // Does the ray intersect beyond the max fraction? + if ( tmin < 0.0f || 1.0f < tmin ) + { + return output; + } + + // Intersection. + output.fraction = tmin; + output.normal = normal; + output.point = b2Lerp( p1, p2, tmin ); + output.hit = true; + return output; +} + +#if 0 +bool b2TestOverlap( const b2Shape* shapeA, int32_t indexA, + const b2Shape* shapeB, int32_t indexB, + b2Transform xfA, b2Transform xfB) +{ + b2DistanceInput input; + input->proxyA.Set(shapeA, indexA); + input->proxyB.Set(shapeB, indexB); + input->transformA = xfA; + input->transformB = xfB; + input->useRadii = true; + + b2DistanceCache cache; + cache.count = 0; + + b2DistanceOutput output; + + b2Distance(&output, &cache, &input); + + return output.distance < 10.0f * b2_epsilon; +} +#endif diff --git a/3rdparty/box2d/src/aabb.h b/3rdparty/box2d/src/aabb.h new file mode 100644 index 000000000000..6fa99c4eafad --- /dev/null +++ b/3rdparty/box2d/src/aabb.h @@ -0,0 +1,64 @@ +// SPDX-FileCopyrightText: 2023 Erin Catto +// SPDX-License-Identifier: MIT + +#pragma once + +#include "box2d/types.h" + +// Ray cast an AABB +b2CastOutput b2AABB_RayCast( b2AABB a, b2Vec2 p1, b2Vec2 p2 ); + +// Get the perimeter length +static inline float b2Perimeter( b2AABB a ) +{ + float wx = a.upperBound.x - a.lowerBound.x; + float wy = a.upperBound.y - a.lowerBound.y; + return 2.0f * ( wx + wy ); +} + +/// Enlarge a to contain b +/// @return true if the AABB grew +static inline bool b2EnlargeAABB( b2AABB* a, b2AABB b ) +{ + bool changed = false; + if ( b.lowerBound.x < a->lowerBound.x ) + { + a->lowerBound.x = b.lowerBound.x; + changed = true; + } + + if ( b.lowerBound.y < a->lowerBound.y ) + { + a->lowerBound.y = b.lowerBound.y; + changed = true; + } + + if ( a->upperBound.x < b.upperBound.x ) + { + a->upperBound.x = b.upperBound.x; + changed = true; + } + + if ( a->upperBound.y < b.upperBound.y ) + { + a->upperBound.y = b.upperBound.y; + changed = true; + } + + return changed; +} + +/// Do a and b overlap +static inline bool b2AABB_Overlaps( b2AABB a, b2AABB b ) +{ + b2Vec2 d1 = { b.lowerBound.x - a.upperBound.x, b.lowerBound.y - a.upperBound.y }; + b2Vec2 d2 = { a.lowerBound.x - b.upperBound.x, a.lowerBound.y - b.upperBound.y }; + + if ( d1.x > 0.0f || d1.y > 0.0f ) + return false; + + if ( d2.x > 0.0f || d2.y > 0.0f ) + return false; + + return true; +} diff --git a/3rdparty/box2d/src/array.c b/3rdparty/box2d/src/array.c new file mode 100644 index 000000000000..7f3847cb2513 --- /dev/null +++ b/3rdparty/box2d/src/array.c @@ -0,0 +1,8 @@ +// SPDX-FileCopyrightText: 2023 Erin Catto +// SPDX-License-Identifier: MIT + +#include "array.h" + +#include + +B2_ARRAY_SOURCE( int, b2Int ); diff --git a/3rdparty/box2d/src/array.h b/3rdparty/box2d/src/array.h new file mode 100644 index 000000000000..e590d292f291 --- /dev/null +++ b/3rdparty/box2d/src/array.h @@ -0,0 +1,174 @@ +// SPDX-FileCopyrightText: 2023 Erin Catto +// SPDX-License-Identifier: MIT + +#pragma once + +#include "core.h" + +// Macro generated functions for dynamic arrays +// Pros +// - type safe +// - array data debuggable (visible count and capacity) +// - bounds checking +// - forward declaration +// - simple implementation +// - generates functions (like C++ templates) +// - functions have https://en.wikipedia.org/wiki/Sequence_point +// - avoids stretchy buffer dropped pointer update bugs +// Cons +// - cannot debug +// - breaks code navigation + +// Array declaration that doesn't need the type T to be defined +#define B2_ARRAY_DECLARE( T, PREFIX ) \ + typedef struct \ + { \ + struct T* data; \ + int count; \ + int capacity; \ + } PREFIX##Array; \ + PREFIX##Array PREFIX##Array_Create( int capacity ); \ + void PREFIX##Array_Reserve( PREFIX##Array* a, int newCapacity ); \ + void PREFIX##Array_Destroy( PREFIX##Array* a ); + +#define B2_DECLARE_ARRAY_NATIVE( T, PREFIX ) \ + typedef struct \ + { \ + T* data; \ + int count; \ + int capacity; \ + } PREFIX##Array; \ + /* Create array with initial capacity. Zero initialization is also supported */ \ + PREFIX##Array PREFIX##Array_Create( int capacity ); \ + void PREFIX##Array_Reserve( PREFIX##Array* a, int newCapacity ); \ + void PREFIX##Array_Destroy( PREFIX##Array* a ); + +// Inline array functions that need the type T to be defined +#define B2_ARRAY_INLINE( T, PREFIX ) \ + /* Resize */ \ + static inline void PREFIX##Array_Resize( PREFIX##Array* a, int count ) \ + { \ + PREFIX##Array_Reserve( a, count ); \ + a->count = count; \ + } \ + /* Get */ \ + static inline T* PREFIX##Array_Get( PREFIX##Array* a, int index ) \ + { \ + B2_ASSERT( 0 <= index && index < a->count ); \ + return a->data + index; \ + } \ + /* Add */ \ + static inline T* PREFIX##Array_Add( PREFIX##Array* a ) \ + { \ + if ( a->count == a->capacity ) \ + { \ + int newCapacity = a->capacity < 2 ? 2 : a->capacity + ( a->capacity >> 1 ); \ + PREFIX##Array_Reserve( a, newCapacity ); \ + } \ + a->count += 1; \ + return a->data + ( a->count - 1 ); \ + } \ + /* Push */ \ + static inline void PREFIX##Array_Push( PREFIX##Array* a, T value ) \ + { \ + if ( a->count == a->capacity ) \ + { \ + int newCapacity = a->capacity < 2 ? 2 : a->capacity + ( a->capacity >> 1 ); \ + PREFIX##Array_Reserve( a, newCapacity ); \ + } \ + a->data[a->count] = value; \ + a->count += 1; \ + } \ + /* Set */ \ + static inline void PREFIX##Array_Set( PREFIX##Array* a, int index, T value ) \ + { \ + B2_ASSERT( 0 <= index && index < a->count ); \ + a->data[index] = value; \ + } \ + /* RemoveSwap */ \ + static inline int PREFIX##Array_RemoveSwap( PREFIX##Array* a, int index ) \ + { \ + B2_ASSERT( 0 <= index && index < a->count ); \ + int movedIndex = B2_NULL_INDEX; \ + if ( index != a->count - 1 ) \ + { \ + movedIndex = a->count - 1; \ + a->data[index] = a->data[movedIndex]; \ + } \ + a->count -= 1; \ + return movedIndex; \ + } \ + /* Pop */ \ + static inline T PREFIX##Array_Pop( PREFIX##Array* a ) \ + { \ + B2_ASSERT( a->count > 0 ); \ + T value = a->data[a->count - 1]; \ + a->count -= 1; \ + return value; \ + } \ + /* Clear */ \ + static inline void PREFIX##Array_Clear( PREFIX##Array* a ) \ + { \ + a->count = 0; \ + } \ + /* ByteCount */ \ + static inline int PREFIX##Array_ByteCount( PREFIX##Array* a ) \ + { \ + return (int)( a->capacity * sizeof( T ) ); \ + } + +// Array implementations to be instantiated in a source file where the type T is known +#define B2_ARRAY_SOURCE( T, PREFIX ) \ + /* Create */ \ + PREFIX##Array PREFIX##Array_Create( int capacity ) \ + { \ + PREFIX##Array a = { 0 }; \ + if ( capacity > 0 ) \ + { \ + a.data = b2Alloc( capacity * sizeof( T ) ); \ + a.capacity = capacity; \ + } \ + return a; \ + } \ + /* Reserve */ \ + void PREFIX##Array_Reserve( PREFIX##Array* a, int newCapacity ) \ + { \ + if ( newCapacity <= a->capacity ) \ + { \ + return; \ + } \ + a->data = b2GrowAlloc( a->data, a->capacity * sizeof( T ), newCapacity * sizeof( T ) ); \ + a->capacity = newCapacity; \ + } \ + /* Destroy */ \ + void PREFIX##Array_Destroy( PREFIX##Array* a ) \ + { \ + b2Free( a->data, a->capacity * sizeof( T ) ); \ + a->data = NULL; \ + a->count = 0; \ + a->capacity = 0; \ + } + +B2_DECLARE_ARRAY_NATIVE( int, b2Int ); +B2_ARRAY_INLINE( int, b2Int ); + +// Declare all the arrays +B2_ARRAY_DECLARE( b2Body, b2Body ); +B2_ARRAY_DECLARE( b2BodyMoveEvent, b2BodyMoveEvent ); +B2_ARRAY_DECLARE( b2BodySim, b2BodySim ); +B2_ARRAY_DECLARE( b2BodyState, b2BodyState ); +B2_ARRAY_DECLARE( b2ChainShape, b2ChainShape ); +B2_ARRAY_DECLARE( b2Contact, b2Contact ); +B2_ARRAY_DECLARE( b2ContactBeginTouchEvent, b2ContactBeginTouchEvent ); +B2_ARRAY_DECLARE( b2ContactEndTouchEvent, b2ContactEndTouchEvent ); +B2_ARRAY_DECLARE( b2ContactHitEvent, b2ContactHitEvent ); +B2_ARRAY_DECLARE( b2ContactSim, b2ContactSim ); +B2_ARRAY_DECLARE( b2Island, b2Island ); +B2_ARRAY_DECLARE( b2IslandSim, b2IslandSim ); +B2_ARRAY_DECLARE( b2Joint, b2Joint ); +B2_ARRAY_DECLARE( b2JointSim, b2JointSim ); +B2_ARRAY_DECLARE( b2SensorBeginTouchEvent, b2SensorBeginTouchEvent ); +B2_ARRAY_DECLARE( b2SensorEndTouchEvent, b2SensorEndTouchEvent ); +B2_ARRAY_DECLARE( b2Shape, b2Shape ); +B2_ARRAY_DECLARE( b2SolverSet, b2SolverSet ); +B2_ARRAY_DECLARE( b2TaskContext, b2TaskContext ); diff --git a/3rdparty/box2d/src/bitset.c b/3rdparty/box2d/src/bitset.c new file mode 100644 index 000000000000..a30464c67037 --- /dev/null +++ b/3rdparty/box2d/src/bitset.c @@ -0,0 +1,67 @@ +// SPDX-FileCopyrightText: 2023 Erin Catto +// SPDX-License-Identifier: MIT + +#include "bitset.h" + +#include + +b2BitSet b2CreateBitSet( uint32_t bitCapacity ) +{ + b2BitSet bitSet = { 0 }; + + bitSet.blockCapacity = ( bitCapacity + sizeof( uint64_t ) * 8 - 1 ) / ( sizeof( uint64_t ) * 8 ); + bitSet.blockCount = 0; + bitSet.bits = b2Alloc( bitSet.blockCapacity * sizeof( uint64_t ) ); + memset( bitSet.bits, 0, bitSet.blockCapacity * sizeof( uint64_t ) ); + return bitSet; +} + +void b2DestroyBitSet( b2BitSet* bitSet ) +{ + b2Free( bitSet->bits, bitSet->blockCapacity * sizeof( uint64_t ) ); + bitSet->blockCapacity = 0; + bitSet->blockCount = 0; + bitSet->bits = NULL; +} + +void b2SetBitCountAndClear( b2BitSet* bitSet, uint32_t bitCount ) +{ + uint32_t blockCount = ( bitCount + sizeof( uint64_t ) * 8 - 1 ) / ( sizeof( uint64_t ) * 8 ); + if ( bitSet->blockCapacity < blockCount ) + { + b2DestroyBitSet( bitSet ); + uint32_t newBitCapacity = bitCount + ( bitCount >> 1 ); + *bitSet = b2CreateBitSet( newBitCapacity ); + } + + bitSet->blockCount = blockCount; + memset( bitSet->bits, 0, bitSet->blockCount * sizeof( uint64_t ) ); +} + +void b2GrowBitSet( b2BitSet* bitSet, uint32_t blockCount ) +{ + B2_ASSERT( blockCount > bitSet->blockCount ); + if ( blockCount > bitSet->blockCapacity ) + { + uint32_t oldCapacity = bitSet->blockCapacity; + bitSet->blockCapacity = blockCount + blockCount / 2; + uint64_t* newBits = b2Alloc( bitSet->blockCapacity * sizeof( uint64_t ) ); + memset( newBits, 0, bitSet->blockCapacity * sizeof( uint64_t ) ); + B2_ASSERT( bitSet->bits != NULL ); + memcpy( newBits, bitSet->bits, oldCapacity * sizeof( uint64_t ) ); + b2Free( bitSet->bits, oldCapacity * sizeof( uint64_t ) ); + bitSet->bits = newBits; + } + + bitSet->blockCount = blockCount; +} + +void b2InPlaceUnion( b2BitSet* B2_RESTRICT setA, const b2BitSet* B2_RESTRICT setB ) +{ + B2_ASSERT( setA->blockCount == setB->blockCount ); + uint32_t blockCount = setA->blockCount; + for ( uint32_t i = 0; i < blockCount; ++i ) + { + setA->bits[i] |= setB->bits[i]; + } +} diff --git a/3rdparty/box2d/src/bitset.h b/3rdparty/box2d/src/bitset.h new file mode 100644 index 000000000000..848b486d3807 --- /dev/null +++ b/3rdparty/box2d/src/bitset.h @@ -0,0 +1,65 @@ +// SPDX-FileCopyrightText: 2023 Erin Catto +// SPDX-License-Identifier: MIT + +#pragma once + +#include "core.h" + +#include +#include + +// Bit set provides fast operations on large arrays of bits. +typedef struct b2BitSet +{ + uint64_t* bits; + uint32_t blockCapacity; + uint32_t blockCount; +} b2BitSet; + +b2BitSet b2CreateBitSet( uint32_t bitCapacity ); +void b2DestroyBitSet( b2BitSet* bitSet ); +void b2SetBitCountAndClear( b2BitSet* bitSet, uint32_t bitCount ); +void b2InPlaceUnion( b2BitSet* setA, const b2BitSet* setB ); +void b2GrowBitSet( b2BitSet* bitSet, uint32_t blockCount ); + +static inline void b2SetBit( b2BitSet* bitSet, uint32_t bitIndex ) +{ + uint32_t blockIndex = bitIndex / 64; + B2_ASSERT( blockIndex < bitSet->blockCount ); + bitSet->bits[blockIndex] |= ( (uint64_t)1 << bitIndex % 64 ); +} + +static inline void b2SetBitGrow( b2BitSet* bitSet, uint32_t bitIndex ) +{ + uint32_t blockIndex = bitIndex / 64; + if ( blockIndex >= bitSet->blockCount ) + { + b2GrowBitSet( bitSet, blockIndex + 1 ); + } + bitSet->bits[blockIndex] |= ( (uint64_t)1 << bitIndex % 64 ); +} + +static inline void b2ClearBit( b2BitSet* bitSet, uint32_t bitIndex ) +{ + uint32_t blockIndex = bitIndex / 64; + if ( blockIndex >= bitSet->blockCount ) + { + return; + } + bitSet->bits[blockIndex] &= ~( (uint64_t)1 << bitIndex % 64 ); +} + +static inline bool b2GetBit( const b2BitSet* bitSet, uint32_t bitIndex ) +{ + uint32_t blockIndex = bitIndex / 64; + if ( blockIndex >= bitSet->blockCount ) + { + return false; + } + return ( bitSet->bits[blockIndex] & ( (uint64_t)1 << bitIndex % 64 ) ) != 0; +} + +static inline int b2GetBitSetBytes( b2BitSet* bitSet ) +{ + return bitSet->blockCapacity * sizeof( uint64_t ); +} diff --git a/3rdparty/box2d/src/body.c b/3rdparty/box2d/src/body.c new file mode 100644 index 000000000000..639914258a57 --- /dev/null +++ b/3rdparty/box2d/src/body.c @@ -0,0 +1,1761 @@ +// SPDX-FileCopyrightText: 2023 Erin Catto +// SPDX-License-Identifier: MIT + +#include "body.h" + +#include "aabb.h" +#include "array.h" +#include "contact.h" +#include "core.h" +#include "id_pool.h" +#include "island.h" +#include "joint.h" +#include "shape.h" +#include "solver_set.h" +#include "world.h" + +// needed for dll export +#include "box2d/box2d.h" +#include "box2d/id.h" + +#include + +// Implement functions for b2BodyArray +B2_ARRAY_SOURCE( b2Body, b2Body ); +B2_ARRAY_SOURCE( b2BodySim, b2BodySim ); +B2_ARRAY_SOURCE( b2BodyState, b2BodyState ); + +// Get a validated body from a world using an id. +b2Body* b2GetBodyFullId( b2World* world, b2BodyId bodyId ) +{ + B2_ASSERT( b2Body_IsValid( bodyId ) ); + + // id index starts at one so that zero can represent null + return b2BodyArray_Get( &world->bodies, bodyId.index1 - 1 ); +} + +b2Transform b2GetBodyTransformQuick( b2World* world, b2Body* body ) +{ + b2SolverSet* set = b2SolverSetArray_Get( &world->solverSets, body->setIndex ); + b2BodySim* bodySim = b2BodySimArray_Get( &set->bodySims, body->localIndex ); + return bodySim->transform; +} + +b2Transform b2GetBodyTransform( b2World* world, int bodyId ) +{ + b2Body* body = b2BodyArray_Get( &world->bodies, bodyId ); + return b2GetBodyTransformQuick( world, body ); +} + +// Create a b2BodyId from a raw id. +b2BodyId b2MakeBodyId( b2World* world, int bodyId ) +{ + b2Body* body = b2BodyArray_Get( &world->bodies, bodyId ); + return ( b2BodyId ){ bodyId + 1, world->worldId, body->revision }; +} + +b2BodySim* b2GetBodySim( b2World* world, b2Body* body ) +{ + b2SolverSet* set = b2SolverSetArray_Get( &world->solverSets, body->setIndex); + b2BodySim* bodySim = b2BodySimArray_Get( &set->bodySims, body->localIndex ); + return bodySim; +} + +b2BodyState* b2GetBodyState( b2World* world, b2Body* body ) +{ + if ( body->setIndex == b2_awakeSet ) + { + b2SolverSet* set = b2SolverSetArray_Get( &world->solverSets, b2_awakeSet ); + return b2BodyStateArray_Get( &set->bodyStates, body->localIndex ); + } + + return NULL; +} + +static void b2CreateIslandForBody( b2World* world, int setIndex, b2Body* body ) +{ + B2_ASSERT( body->islandId == B2_NULL_INDEX ); + B2_ASSERT( body->islandPrev == B2_NULL_INDEX ); + B2_ASSERT( body->islandNext == B2_NULL_INDEX ); + B2_ASSERT( setIndex != b2_disabledSet ); + + b2Island* island = b2CreateIsland( world, setIndex ); + + body->islandId = island->islandId; + island->headBody = body->id; + island->tailBody = body->id; + island->bodyCount = 1; +} + +static void b2RemoveBodyFromIsland( b2World* world, b2Body* body ) +{ + if ( body->islandId == B2_NULL_INDEX ) + { + B2_ASSERT( body->islandPrev == B2_NULL_INDEX ); + B2_ASSERT( body->islandNext == B2_NULL_INDEX ); + return; + } + + int islandId = body->islandId; + b2Island* island = b2IslandArray_Get( &world->islands, islandId ); + + // Fix the island's linked list of sims + if ( body->islandPrev != B2_NULL_INDEX ) + { + b2Body* prevBody = b2BodyArray_Get( &world->bodies, body->islandPrev ); + prevBody->islandNext = body->islandNext; + } + + if ( body->islandNext != B2_NULL_INDEX ) + { + b2Body* nextBody = b2BodyArray_Get( &world->bodies, body->islandNext ); + nextBody->islandPrev = body->islandPrev; + } + + B2_ASSERT( island->bodyCount > 0 ); + island->bodyCount -= 1; + bool islandDestroyed = false; + + if ( island->headBody == body->id ) + { + island->headBody = body->islandNext; + + if ( island->headBody == B2_NULL_INDEX ) + { + // Destroy empty island + B2_ASSERT( island->tailBody == body->id ); + B2_ASSERT( island->bodyCount == 0 ); + B2_ASSERT( island->contactCount == 0 ); + B2_ASSERT( island->jointCount == 0 ); + + // Free the island + b2DestroyIsland( world, island->islandId ); + islandDestroyed = true; + } + } + else if ( island->tailBody == body->id ) + { + island->tailBody = body->islandPrev; + } + + if ( islandDestroyed == false ) + { + b2ValidateIsland( world, islandId ); + } + + body->islandId = B2_NULL_INDEX; + body->islandPrev = B2_NULL_INDEX; + body->islandNext = B2_NULL_INDEX; +} + +static void b2DestroyBodyContacts( b2World* world, b2Body* body, bool wakeBodies ) +{ + // Destroy the attached contacts + int edgeKey = body->headContactKey; + while ( edgeKey != B2_NULL_INDEX ) + { + int contactId = edgeKey >> 1; + int edgeIndex = edgeKey & 1; + + b2Contact* contact = b2ContactArray_Get( &world->contacts, contactId ); + edgeKey = contact->edges[edgeIndex].nextKey; + b2DestroyContact( world, contact, wakeBodies ); + } + + b2ValidateSolverSets( world ); +} + +b2BodyId b2CreateBody( b2WorldId worldId, const b2BodyDef* def ) +{ + b2CheckDef( def ); + B2_ASSERT( b2Vec2_IsValid( def->position ) ); + B2_ASSERT( b2Rot_IsValid( def->rotation ) ); + B2_ASSERT( b2Vec2_IsValid( def->linearVelocity ) ); + B2_ASSERT( b2IsValid( def->angularVelocity ) ); + B2_ASSERT( b2IsValid( def->linearDamping ) && def->linearDamping >= 0.0f ); + B2_ASSERT( b2IsValid( def->angularDamping ) && def->angularDamping >= 0.0f ); + B2_ASSERT( b2IsValid( def->sleepThreshold ) && def->sleepThreshold >= 0.0f ); + B2_ASSERT( b2IsValid( def->gravityScale ) ); + + b2World* world = b2GetWorldFromId( worldId ); + B2_ASSERT( world->locked == false ); + + if ( world->locked ) + { + return b2_nullBodyId; + } + + bool isAwake = ( def->isAwake || def->enableSleep == false ) && def->isEnabled; + + // determine the solver set + int setId; + if ( def->isEnabled == false ) + { + // any body type can be disabled + setId = b2_disabledSet; + } + else if ( def->type == b2_staticBody ) + { + setId = b2_staticSet; + } + else if ( isAwake == true ) + { + setId = b2_awakeSet; + } + else + { + // new set for a sleeping body in its own island + setId = b2AllocId( &world->solverSetIdPool ); + if ( setId == world->solverSets.count ) + { + // Create a zero initialized solver set. All sub-arrays are also zero initialized. + b2SolverSetArray_Push( &world->solverSets, ( b2SolverSet ){ 0 } ); + } + else + { + B2_ASSERT( world->solverSets.data[setId].setIndex == B2_NULL_INDEX ); + } + + world->solverSets.data[setId].setIndex = setId; + } + + B2_ASSERT( 0 <= setId && setId < world->solverSets.count ); + + int bodyId = b2AllocId( &world->bodyIdPool ); + + b2SolverSet* set = b2SolverSetArray_Get( &world->solverSets, setId ); + b2BodySim* bodySim = b2BodySimArray_Add( &set->bodySims ); + *bodySim = ( b2BodySim ){ 0 }; + bodySim->transform.p = def->position; + bodySim->transform.q = def->rotation; + bodySim->center = def->position; + bodySim->rotation0 = bodySim->transform.q; + bodySim->center0 = bodySim->center; + bodySim->localCenter = b2Vec2_zero; + bodySim->force = b2Vec2_zero; + bodySim->torque = 0.0f; + bodySim->mass = 0.0f; + bodySim->invMass = 0.0f; + bodySim->inertia = 0.0f; + bodySim->invInertia = 0.0f; + bodySim->minExtent = b2_huge; + bodySim->maxExtent = 0.0f; + bodySim->linearDamping = def->linearDamping; + bodySim->angularDamping = def->angularDamping; + bodySim->gravityScale = def->gravityScale; + bodySim->bodyId = bodyId; + bodySim->isBullet = def->isBullet; + bodySim->allowFastRotation = def->allowFastRotation; + bodySim->enlargeAABB = false; + bodySim->isFast = false; + bodySim->isSpeedCapped = false; + + if ( setId == b2_awakeSet ) + { + b2BodyState* bodyState = b2BodyStateArray_Add( &set->bodyStates ); + B2_ASSERT( ( (uintptr_t)bodyState & 0x1F ) == 0 ); + + *bodyState = ( b2BodyState ){ 0 }; + bodyState->linearVelocity = def->linearVelocity; + bodyState->angularVelocity = def->angularVelocity; + bodyState->deltaRotation = b2Rot_identity; + } + + if ( bodyId == world->bodies.count ) + { + b2BodyArray_Push( &world->bodies, ( b2Body ){ 0 } ); + } + else + { + B2_ASSERT( world->bodies.data[bodyId].id == B2_NULL_INDEX ); + } + + b2Body* body = b2BodyArray_Get( &world->bodies, bodyId ); + body->userData = def->userData; + body->setIndex = setId; + body->localIndex = set->bodySims.count - 1; + body->revision += 1; + body->headShapeId = B2_NULL_INDEX; + body->shapeCount = 0; + body->headChainId = B2_NULL_INDEX; + body->headContactKey = B2_NULL_INDEX; + body->contactCount = 0; + body->headJointKey = B2_NULL_INDEX; + body->jointCount = 0; + body->islandId = B2_NULL_INDEX; + body->islandPrev = B2_NULL_INDEX; + body->islandNext = B2_NULL_INDEX; + body->bodyMoveIndex = B2_NULL_INDEX; + body->id = bodyId; + body->sleepThreshold = def->sleepThreshold; + body->sleepTime = 0.0f; + body->type = def->type; + body->enableSleep = def->enableSleep; + body->fixedRotation = def->fixedRotation; + body->isSpeedCapped = false; + body->isMarked = false; + body->automaticMass = def->automaticMass; + + // dynamic and kinematic bodies that are enabled need a island + if ( setId >= b2_awakeSet ) + { + b2CreateIslandForBody( world, setId, body ); + } + + b2ValidateSolverSets( world ); + + b2BodyId id = { bodyId + 1, world->worldId, body->revision }; + return id; +} + +bool b2IsBodyAwake( b2World* world, b2Body* body ) +{ + return body->setIndex == b2_awakeSet; +} + +bool b2WakeBody( b2World* world, b2Body* body ) +{ + if ( body->setIndex >= b2_firstSleepingSet ) + { + b2WakeSolverSet( world, body->setIndex ); + return true; + } + + return false; +} + +void b2DestroyBody( b2BodyId bodyId ) +{ + b2World* world = b2GetWorldLocked( bodyId.world0 ); + if ( world == NULL ) + { + return; + } + + b2Body* body = b2GetBodyFullId( world, bodyId ); + + // Wake bodies attached to this body, even if this body is static. + bool wakeBodies = true; + + // Destroy the attached joints + int edgeKey = body->headJointKey; + while ( edgeKey != B2_NULL_INDEX ) + { + int jointId = edgeKey >> 1; + int edgeIndex = edgeKey & 1; + + b2Joint* joint = b2JointArray_Get( &world->joints, jointId ); + edgeKey = joint->edges[edgeIndex].nextKey; + + // Careful because this modifies the list being traversed + b2DestroyJointInternal( world, joint, wakeBodies ); + } + + // Destroy all contacts attached to this body. + b2DestroyBodyContacts( world, body, wakeBodies ); + + // Destroy the attached shapes and their broad-phase proxies. + int shapeId = body->headShapeId; + while ( shapeId != B2_NULL_INDEX ) + { + b2Shape* shape = b2ShapeArray_Get( &world->shapes, shapeId ); + + b2DestroyShapeProxy( shape, &world->broadPhase ); + + // Return shape to free list. + b2FreeId( &world->shapeIdPool, shapeId ); + shape->id = B2_NULL_INDEX; + + shapeId = shape->nextShapeId; + } + + // Destroy the attached chains. The associated shapes have already been destroyed above. + int chainId = body->headChainId; + while ( chainId != B2_NULL_INDEX ) + { + b2ChainShape* chain = b2ChainShapeArray_Get( &world->chainShapes, chainId ); + + b2Free( chain->shapeIndices, chain->count * sizeof( int ) ); + chain->shapeIndices = NULL; + + // Return chain to free list. + b2FreeId( &world->chainIdPool, chainId ); + chain->id = B2_NULL_INDEX; + + chainId = chain->nextChainId; + } + + b2RemoveBodyFromIsland( world, body ); + + // Remove body sim from solver set that owns it + b2SolverSet* set = b2SolverSetArray_Get( &world->solverSets, body->setIndex ); + int movedIndex = b2BodySimArray_RemoveSwap( &set->bodySims, body->localIndex ); + if ( movedIndex != B2_NULL_INDEX ) + { + // Fix moved body index + b2BodySim* movedSim = set->bodySims.data + body->localIndex; + int movedId = movedSim->bodyId; + b2Body* movedBody = b2BodyArray_Get( &world->bodies, movedId ); + B2_ASSERT( movedBody->localIndex == movedIndex ); + movedBody->localIndex = body->localIndex; + } + + // Remove body state from awake set + if ( body->setIndex == b2_awakeSet ) + { + int result = b2BodyStateArray_RemoveSwap( &set->bodyStates, body->localIndex ); + B2_MAYBE_UNUSED( result ); + B2_ASSERT( result == movedIndex ); + } + + // Free body and id (preserve body revision) + b2FreeId( &world->bodyIdPool, body->id ); + + body->setIndex = B2_NULL_INDEX; + body->localIndex = B2_NULL_INDEX; + body->id = B2_NULL_INDEX; + + b2ValidateSolverSets( world ); +} + +int b2Body_GetContactCapacity( b2BodyId bodyId ) +{ + b2World* world = b2GetWorldLocked( bodyId.world0 ); + if ( world == NULL ) + { + return 0; + } + + b2Body* body = b2GetBodyFullId( world, bodyId ); + + // Conservative and fast + return body->contactCount; +} + +// todo what about sensors? +// todo sample needed +int b2Body_GetContactData( b2BodyId bodyId, b2ContactData* contactData, int capacity ) +{ + b2World* world = b2GetWorldLocked( bodyId.world0 ); + if ( world == NULL ) + { + return 0; + } + + b2Body* body = b2GetBodyFullId( world, bodyId ); + + int contactKey = body->headContactKey; + int index = 0; + while ( contactKey != B2_NULL_INDEX && index < capacity ) + { + int contactId = contactKey >> 1; + int edgeIndex = contactKey & 1; + + b2Contact* contact = b2ContactArray_Get( &world->contacts, contactId ); + + // Is contact touching? + if ( contact->flags & b2_contactTouchingFlag ) + { + b2Shape* shapeA = b2ShapeArray_Get( &world->shapes, contact->shapeIdA ); + b2Shape* shapeB = b2ShapeArray_Get( &world->shapes, contact->shapeIdB ); + + contactData[index].shapeIdA = ( b2ShapeId ){ shapeA->id + 1, bodyId.world0, shapeA->revision }; + contactData[index].shapeIdB = ( b2ShapeId ){ shapeB->id + 1, bodyId.world0, shapeB->revision }; + + b2ContactSim* contactSim = b2GetContactSim( world, contact ); + contactData[index].manifold = contactSim->manifold; + + index += 1; + } + + contactKey = contact->edges[edgeIndex].nextKey; + } + + B2_ASSERT( index <= capacity ); + + return index; +} + +b2AABB b2Body_ComputeAABB( b2BodyId bodyId ) +{ + b2World* world = b2GetWorldLocked( bodyId.world0 ); + if ( world == NULL ) + { + return ( b2AABB ){ 0 }; + } + + b2Body* body = b2GetBodyFullId( world, bodyId ); + if ( body->headShapeId == B2_NULL_INDEX ) + { + b2Transform transform = b2GetBodyTransform( world, body->id ); + return ( b2AABB ){ transform.p, transform.p }; + } + + b2Shape* shape = b2ShapeArray_Get( &world->shapes, body->headShapeId ); + b2AABB aabb = shape->aabb; + while ( shape->nextShapeId != B2_NULL_INDEX ) + { + shape = b2ShapeArray_Get( &world->shapes, shape->nextShapeId ); + aabb = b2AABB_Union( aabb, shape->aabb ); + } + + return aabb; +} + +void b2UpdateBodyMassData( b2World* world, b2Body* body ) +{ + b2BodySim* bodySim = b2GetBodySim( world, body ); + + // Compute mass data from shapes. Each shape has its own density. + bodySim->mass = 0.0f; + bodySim->invMass = 0.0f; + bodySim->inertia = 0.0f; + bodySim->invInertia = 0.0f; + bodySim->localCenter = b2Vec2_zero; + bodySim->minExtent = b2_huge; + bodySim->maxExtent = 0.0f; + + // Static and kinematic sims have zero mass. + if ( body->type != b2_dynamicBody ) + { + bodySim->center = bodySim->transform.p; + + // Need extents for kinematic bodies for sleeping to work correctly. + if ( body->type == b2_kinematicBody ) + { + int shapeId = body->headShapeId; + while ( shapeId != B2_NULL_INDEX ) + { + const b2Shape* s = b2ShapeArray_Get( &world->shapes, shapeId ); + + b2ShapeExtent extent = b2ComputeShapeExtent( s, b2Vec2_zero ); + bodySim->minExtent = b2MinFloat( bodySim->minExtent, extent.minExtent ); + bodySim->maxExtent = b2MaxFloat( bodySim->maxExtent, extent.maxExtent ); + + shapeId = s->nextShapeId; + } + } + + return; + } + + // Accumulate mass over all shapes. + b2Vec2 localCenter = b2Vec2_zero; + int shapeId = body->headShapeId; + while ( shapeId != B2_NULL_INDEX ) + { + const b2Shape* s = b2ShapeArray_Get( &world->shapes, shapeId ); + shapeId = s->nextShapeId; + + if ( s->density == 0.0f ) + { + continue; + } + + b2MassData massData = b2ComputeShapeMass( s ); + bodySim->mass += massData.mass; + localCenter = b2MulAdd( localCenter, massData.mass, massData.center ); + bodySim->inertia += massData.rotationalInertia; + } + + // Compute center of mass. + if ( bodySim->mass > 0.0f ) + { + bodySim->invMass = 1.0f / bodySim->mass; + localCenter = b2MulSV( bodySim->invMass, localCenter ); + } + + if ( bodySim->inertia > 0.0f && body->fixedRotation == false ) + { + // Center the inertia about the center of mass. + bodySim->inertia -= bodySim->mass * b2Dot( localCenter, localCenter ); + B2_ASSERT( bodySim->inertia > 0.0f ); + bodySim->invInertia = 1.0f / bodySim->inertia; + } + else + { + bodySim->inertia = 0.0f; + bodySim->invInertia = 0.0f; + } + + // Move center of mass. + b2Vec2 oldCenter = bodySim->center; + bodySim->localCenter = localCenter; + bodySim->center = b2TransformPoint( bodySim->transform, bodySim->localCenter ); + + // Update center of mass velocity + b2BodyState* state = b2GetBodyState( world, body ); + if ( state != NULL ) + { + b2Vec2 deltaLinear = b2CrossSV( state->angularVelocity, b2Sub( bodySim->center, oldCenter ) ); + state->linearVelocity = b2Add( state->linearVelocity, deltaLinear ); + } + + // Compute body extents relative to center of mass + shapeId = body->headShapeId; + while ( shapeId != B2_NULL_INDEX ) + { + const b2Shape* s = b2ShapeArray_Get( &world->shapes, shapeId ); + + b2ShapeExtent extent = b2ComputeShapeExtent( s, localCenter ); + bodySim->minExtent = b2MinFloat( bodySim->minExtent, extent.minExtent ); + bodySim->maxExtent = b2MaxFloat( bodySim->maxExtent, extent.maxExtent ); + + shapeId = s->nextShapeId; + } +} + +b2Vec2 b2Body_GetPosition( b2BodyId bodyId ) +{ + b2World* world = b2GetWorld( bodyId.world0 ); + b2Body* body = b2GetBodyFullId( world, bodyId ); + b2Transform transform = b2GetBodyTransformQuick( world, body ); + return transform.p; +} + +b2Rot b2Body_GetRotation( b2BodyId bodyId ) +{ + b2World* world = b2GetWorld( bodyId.world0 ); + b2Body* body = b2GetBodyFullId( world, bodyId ); + b2Transform transform = b2GetBodyTransformQuick( world, body ); + return transform.q; +} + +b2Transform b2Body_GetTransform( b2BodyId bodyId ) +{ + b2World* world = b2GetWorld( bodyId.world0 ); + b2Body* body = b2GetBodyFullId( world, bodyId ); + return b2GetBodyTransformQuick( world, body ); +} + +b2Vec2 b2Body_GetLocalPoint( b2BodyId bodyId, b2Vec2 worldPoint ) +{ + b2World* world = b2GetWorld( bodyId.world0 ); + b2Body* body = b2GetBodyFullId( world, bodyId ); + b2Transform transform = b2GetBodyTransformQuick( world, body ); + return b2InvTransformPoint( transform, worldPoint ); +} + +b2Vec2 b2Body_GetWorldPoint( b2BodyId bodyId, b2Vec2 localPoint ) +{ + b2World* world = b2GetWorld( bodyId.world0 ); + b2Body* body = b2GetBodyFullId( world, bodyId ); + b2Transform transform = b2GetBodyTransformQuick( world, body ); + return b2TransformPoint( transform, localPoint ); +} + +b2Vec2 b2Body_GetLocalVector( b2BodyId bodyId, b2Vec2 worldVector ) +{ + b2World* world = b2GetWorld( bodyId.world0 ); + b2Body* body = b2GetBodyFullId( world, bodyId ); + b2Transform transform = b2GetBodyTransformQuick( world, body ); + return b2InvRotateVector( transform.q, worldVector ); +} + +b2Vec2 b2Body_GetWorldVector( b2BodyId bodyId, b2Vec2 localVector ) +{ + b2World* world = b2GetWorld( bodyId.world0 ); + b2Body* body = b2GetBodyFullId( world, bodyId ); + b2Transform transform = b2GetBodyTransformQuick( world, body ); + return b2RotateVector( transform.q, localVector ); +} + +void b2Body_SetTransform( b2BodyId bodyId, b2Vec2 position, b2Rot rotation ) +{ + B2_ASSERT( b2Vec2_IsValid( position ) ); + B2_ASSERT( b2Rot_IsValid( rotation ) ); + B2_ASSERT( b2Body_IsValid( bodyId ) ); + b2World* world = b2GetWorld( bodyId.world0 ); + B2_ASSERT( world->locked == false ); + + b2Body* body = b2GetBodyFullId( world, bodyId ); + b2BodySim* bodySim = b2GetBodySim( world, body ); + + bodySim->transform.p = position; + bodySim->transform.q = rotation; + bodySim->center = b2TransformPoint( bodySim->transform, bodySim->localCenter ); + + bodySim->rotation0 = bodySim->transform.q; + bodySim->center0 = bodySim->center; + + b2BroadPhase* broadPhase = &world->broadPhase; + + b2Transform transform = bodySim->transform; + const float margin = b2_aabbMargin; + const float speculativeDistance = b2_speculativeDistance; + + int shapeId = body->headShapeId; + while ( shapeId != B2_NULL_INDEX ) + { + b2Shape* shape = b2ShapeArray_Get( &world->shapes, shapeId ); + b2AABB aabb = b2ComputeShapeAABB( shape, transform ); + aabb.lowerBound.x -= speculativeDistance; + aabb.lowerBound.y -= speculativeDistance; + aabb.upperBound.x += speculativeDistance; + aabb.upperBound.y += speculativeDistance; + shape->aabb = aabb; + + if ( b2AABB_Contains( shape->fatAABB, aabb ) == false ) + { + b2AABB fatAABB; + fatAABB.lowerBound.x = aabb.lowerBound.x - margin; + fatAABB.lowerBound.y = aabb.lowerBound.y - margin; + fatAABB.upperBound.x = aabb.upperBound.x + margin; + fatAABB.upperBound.y = aabb.upperBound.y + margin; + shape->fatAABB = fatAABB; + + // They body could be disabled + if ( shape->proxyKey != B2_NULL_INDEX ) + { + b2BroadPhase_MoveProxy( broadPhase, shape->proxyKey, fatAABB ); + } + } + + shapeId = shape->nextShapeId; + } +} + +b2Vec2 b2Body_GetLinearVelocity( b2BodyId bodyId ) +{ + b2World* world = b2GetWorld( bodyId.world0 ); + b2Body* body = b2GetBodyFullId( world, bodyId ); + b2BodyState* state = b2GetBodyState( world, body ); + if ( state != NULL ) + { + return state->linearVelocity; + } + return b2Vec2_zero; +} + +float b2Body_GetAngularVelocity( b2BodyId bodyId ) +{ + b2World* world = b2GetWorld( bodyId.world0 ); + b2Body* body = b2GetBodyFullId( world, bodyId ); + b2BodyState* state = b2GetBodyState( world, body ); + if ( state != NULL ) + { + return state->angularVelocity; + } + return 0.0; +} + +void b2Body_SetLinearVelocity( b2BodyId bodyId, b2Vec2 linearVelocity ) +{ + b2World* world = b2GetWorld( bodyId.world0 ); + b2Body* body = b2GetBodyFullId( world, bodyId ); + + if ( body->type == b2_staticBody ) + { + return; + } + + if ( b2LengthSquared( linearVelocity ) > 0.0f ) + { + b2WakeBody( world, body ); + } + + b2BodyState* state = b2GetBodyState( world, body ); + if ( state == NULL ) + { + return; + } + + state->linearVelocity = linearVelocity; +} + +void b2Body_SetAngularVelocity( b2BodyId bodyId, float angularVelocity ) +{ + b2World* world = b2GetWorld( bodyId.world0 ); + b2Body* body = b2GetBodyFullId( world, bodyId ); + + if (body->type == b2_staticBody || body->fixedRotation) + { + return; + } + + if ( angularVelocity != 0.0f ) + { + b2WakeBody( world, body ); + } + + b2BodyState* state = b2GetBodyState( world, body ); + if ( state == NULL ) + { + return; + } + + state->angularVelocity = angularVelocity; +} + +void b2Body_ApplyForce( b2BodyId bodyId, b2Vec2 force, b2Vec2 point, bool wake ) +{ + b2World* world = b2GetWorld( bodyId.world0 ); + b2Body* body = b2GetBodyFullId( world, bodyId ); + + if ( wake && body->setIndex >= b2_firstSleepingSet ) + { + b2WakeBody( world, body ); + } + + if ( body->setIndex == b2_awakeSet ) + { + b2BodySim* bodySim = b2GetBodySim( world, body ); + bodySim->force = b2Add( bodySim->force, force ); + bodySim->torque += b2Cross( b2Sub( point, bodySim->center ), force ); + } +} + +void b2Body_ApplyForceToCenter( b2BodyId bodyId, b2Vec2 force, bool wake ) +{ + b2World* world = b2GetWorld( bodyId.world0 ); + b2Body* body = b2GetBodyFullId( world, bodyId ); + + if ( wake && body->setIndex >= b2_firstSleepingSet ) + { + b2WakeBody( world, body ); + } + + if ( body->setIndex == b2_awakeSet ) + { + b2BodySim* bodySim = b2GetBodySim( world, body ); + bodySim->force = b2Add( bodySim->force, force ); + } +} + +void b2Body_ApplyTorque( b2BodyId bodyId, float torque, bool wake ) +{ + b2World* world = b2GetWorld( bodyId.world0 ); + b2Body* body = b2GetBodyFullId( world, bodyId ); + + if ( wake && body->setIndex >= b2_firstSleepingSet ) + { + b2WakeBody( world, body ); + } + + if ( body->setIndex == b2_awakeSet ) + { + b2BodySim* bodySim = b2GetBodySim( world, body ); + bodySim->torque += torque; + } +} + +void b2Body_ApplyLinearImpulse( b2BodyId bodyId, b2Vec2 impulse, b2Vec2 point, bool wake ) +{ + b2World* world = b2GetWorld( bodyId.world0 ); + b2Body* body = b2GetBodyFullId( world, bodyId ); + + if ( wake && body->setIndex >= b2_firstSleepingSet ) + { + b2WakeBody( world, body ); + } + + if ( body->setIndex == b2_awakeSet ) + { + int localIndex = body->localIndex; + b2SolverSet* set = b2SolverSetArray_Get( &world->solverSets, b2_awakeSet ); + b2BodyState* state = b2BodyStateArray_Get( &set->bodyStates, localIndex ); + b2BodySim* bodySim = b2BodySimArray_Get( &set->bodySims, localIndex ); + state->linearVelocity = b2MulAdd( state->linearVelocity, bodySim->invMass, impulse ); + state->angularVelocity += bodySim->invInertia * b2Cross( b2Sub( point, bodySim->center ), impulse ); + } +} + +void b2Body_ApplyLinearImpulseToCenter( b2BodyId bodyId, b2Vec2 impulse, bool wake ) +{ + b2World* world = b2GetWorld( bodyId.world0 ); + b2Body* body = b2GetBodyFullId( world, bodyId ); + + if ( wake && body->setIndex >= b2_firstSleepingSet ) + { + b2WakeBody( world, body ); + } + + if ( body->setIndex == b2_awakeSet ) + { + int localIndex = body->localIndex; + b2SolverSet* set = b2SolverSetArray_Get( &world->solverSets, b2_awakeSet ); + b2BodyState* state = b2BodyStateArray_Get( &set->bodyStates, localIndex ); + b2BodySim* bodySim = b2BodySimArray_Get( &set->bodySims, localIndex ); + state->linearVelocity = b2MulAdd( state->linearVelocity, bodySim->invMass, impulse ); + } +} + +void b2Body_ApplyAngularImpulse( b2BodyId bodyId, float impulse, bool wake ) +{ + B2_ASSERT( b2Body_IsValid( bodyId ) ); + b2World* world = b2GetWorld( bodyId.world0 ); + + int id = bodyId.index1 - 1; + b2Body* body = b2BodyArray_Get( &world->bodies, id ); + B2_ASSERT( body->revision == bodyId.revision ); + + if ( wake && body->setIndex >= b2_firstSleepingSet ) + { + // this will not invalidate body pointer + b2WakeBody( world, body ); + } + + if ( body->setIndex == b2_awakeSet ) + { + int localIndex = body->localIndex; + b2SolverSet* set = b2SolverSetArray_Get( &world->solverSets, b2_awakeSet ); + b2BodyState* state = b2BodyStateArray_Get( &set->bodyStates, localIndex ); + b2BodySim* bodySim = b2BodySimArray_Get( &set->bodySims, localIndex ); + state->angularVelocity += bodySim->invInertia * impulse; + } +} + +b2BodyType b2Body_GetType( b2BodyId bodyId ) +{ + b2World* world = b2GetWorld( bodyId.world0 ); + b2Body* body = b2GetBodyFullId( world, bodyId ); + return body->type; +} + +// Changing the body type is quite complex mainly due to joints. +// Considerations: +// - body and joints must be moved to the correct set +// - islands must be updated +// - graph coloring must be correct +// - any body connected to a joint may be disabled +// - joints between static bodies must go into the static set +void b2Body_SetType( b2BodyId bodyId, b2BodyType type ) +{ + b2World* world = b2GetWorld( bodyId.world0 ); + b2Body* body = b2GetBodyFullId( world, bodyId ); + + b2BodyType originalType = body->type; + if ( originalType == type ) + { + return; + } + + if ( body->setIndex == b2_disabledSet ) + { + // Disabled bodies don't change solver sets or islands when they change type. + body->type = type; + + // Body type affects the mass + b2UpdateBodyMassData( world, body ); + return; + } + + // Destroy all contacts but don't wake bodies. + bool wakeBodies = false; + b2DestroyBodyContacts( world, body, wakeBodies ); + + // Wake this body because we assume below that it is awake or static. + b2WakeBody( world, body ); + + // Unlink all joints and wake attached bodies. + { + int jointKey = body->headJointKey; + while ( jointKey != B2_NULL_INDEX ) + { + int jointId = jointKey >> 1; + int edgeIndex = jointKey & 1; + + b2Joint* joint = b2JointArray_Get( &world->joints, jointId ); + if ( joint->islandId != B2_NULL_INDEX ) + { + b2UnlinkJoint( world, joint ); + } + + // A body going from static to dynamic or kinematic goes to the awake set + // and other attached bodies must be awake as well. For consistency, this is + // done for all cases. + b2Body* bodyA = b2BodyArray_Get( &world->bodies, joint->edges[0].bodyId ); + b2Body* bodyB = b2BodyArray_Get( &world->bodies, joint->edges[1].bodyId ); + b2WakeBody( world, bodyA ); + b2WakeBody( world, bodyB ); + + jointKey = joint->edges[edgeIndex].nextKey; + } + } + + body->type = type; + + if ( originalType == b2_staticBody ) + { + // Body is going from static to dynamic or kinematic. It only makes sense to move it to the awake set. + B2_ASSERT( body->setIndex == b2_staticSet ); + + b2SolverSet* staticSet = b2SolverSetArray_Get( &world->solverSets, b2_staticSet ); + b2SolverSet* awakeSet = b2SolverSetArray_Get( &world->solverSets, b2_awakeSet ); + + // Transfer body to awake set + b2TransferBody( world, awakeSet, staticSet, body ); + + // Create island for body + b2CreateIslandForBody( world, b2_awakeSet, body ); + + // Transfer static joints to awake set + int jointKey = body->headJointKey; + while ( jointKey != B2_NULL_INDEX ) + { + int jointId = jointKey >> 1; + int edgeIndex = jointKey & 1; + + b2Joint* joint = b2JointArray_Get( &world->joints, jointId ); + + // Transfer the joint if it is in the static set + if ( joint->setIndex == b2_staticSet ) + { + b2TransferJoint( world, awakeSet, staticSet, joint ); + } + else if ( joint->setIndex == b2_awakeSet ) + { + // In this case the joint must be re-inserted into the constraint graph to ensure the correct + // graph color. + + // First transfer to the static set. + b2TransferJoint( world, staticSet, awakeSet, joint ); + + // Now transfer it back to the awake set and into the graph coloring. + b2TransferJoint( world, awakeSet, staticSet, joint ); + } + else + { + // Otherwise the joint must be disabled. + B2_ASSERT( joint->setIndex == b2_disabledSet ); + } + + jointKey = joint->edges[edgeIndex].nextKey; + } + + // Recreate shape proxies in movable tree. + b2Transform transform = b2GetBodyTransformQuick( world, body ); + int shapeId = body->headShapeId; + while ( shapeId != B2_NULL_INDEX ) + { + b2Shape* shape = b2ShapeArray_Get( &world->shapes, shapeId ); + shapeId = shape->nextShapeId; + b2DestroyShapeProxy( shape, &world->broadPhase ); + bool forcePairCreation = true; + b2BodyType proxyType = type; + b2CreateShapeProxy( shape, &world->broadPhase, proxyType, transform, forcePairCreation ); + } + } + else if ( type == b2_staticBody ) + { + // The body is going from dynamic/kinematic to static. It should be awake. + B2_ASSERT( body->setIndex == b2_awakeSet ); + + b2SolverSet* staticSet = b2SolverSetArray_Get( &world->solverSets, b2_staticSet ); + b2SolverSet* awakeSet = b2SolverSetArray_Get( &world->solverSets, b2_awakeSet ); + + // Transfer body to static set + b2TransferBody( world, staticSet, awakeSet, body ); + + // Remove body from island. + b2RemoveBodyFromIsland( world, body ); + + // Maybe transfer joints to static set. + int jointKey = body->headJointKey; + while ( jointKey != B2_NULL_INDEX ) + { + int jointId = jointKey >> 1; + int edgeIndex = jointKey & 1; + + b2Joint* joint = b2JointArray_Get( &world->joints, jointId ); + jointKey = joint->edges[edgeIndex].nextKey; + + int otherEdgeIndex = edgeIndex ^ 1; + b2Body* otherBody = b2BodyArray_Get( &world->bodies, joint->edges[otherEdgeIndex].bodyId ); + + // Skip disabled joint + if ( joint->setIndex == b2_disabledSet ) + { + // Joint is disable, should be connected to a disabled body + B2_ASSERT( otherBody->setIndex == b2_disabledSet ); + continue; + } + + // Since the body was not static, the joint must be awake. + B2_ASSERT( joint->setIndex == b2_awakeSet ); + + // Only transfer joint to static set if both bodies are static. + if ( otherBody->setIndex == b2_staticSet ) + { + b2TransferJoint( world, staticSet, awakeSet, joint ); + } + else + { + // The other body must be awake. + B2_ASSERT( otherBody->setIndex == b2_awakeSet ); + + // The joint must live in a graph color. + B2_ASSERT( 0 <= joint->colorIndex && joint->colorIndex < b2_graphColorCount ); + + // In this case the joint must be re-inserted into the constraint graph to ensure the correct + // graph color. + + // First transfer to the static set. + b2TransferJoint( world, staticSet, awakeSet, joint ); + + // Now transfer it back to the awake set and into the graph coloring. + b2TransferJoint( world, awakeSet, staticSet, joint ); + } + } + + // Recreate shape proxies in static tree. + b2Transform transform = b2GetBodyTransformQuick( world, body ); + int shapeId = body->headShapeId; + while ( shapeId != B2_NULL_INDEX ) + { + b2Shape* shape = b2ShapeArray_Get( &world->shapes, shapeId ); + shapeId = shape->nextShapeId; + b2DestroyShapeProxy( shape, &world->broadPhase ); + bool forcePairCreation = true; + b2CreateShapeProxy( shape, &world->broadPhase, b2_staticBody, transform, forcePairCreation ); + } + } + else + { + B2_ASSERT( originalType == b2_dynamicBody || originalType == b2_kinematicBody ); + B2_ASSERT( type == b2_dynamicBody || type == b2_kinematicBody ); + + // Recreate shape proxies in static tree. + b2Transform transform = b2GetBodyTransformQuick( world, body ); + int shapeId = body->headShapeId; + while ( shapeId != B2_NULL_INDEX ) + { + b2Shape* shape = b2ShapeArray_Get( &world->shapes, shapeId ); + shapeId = shape->nextShapeId; + b2DestroyShapeProxy( shape, &world->broadPhase ); + b2BodyType proxyType = type; + bool forcePairCreation = true; + b2CreateShapeProxy( shape, &world->broadPhase, proxyType, transform, forcePairCreation ); + } + } + + // Relink all joints + { + int jointKey = body->headJointKey; + while ( jointKey != B2_NULL_INDEX ) + { + int jointId = jointKey >> 1; + int edgeIndex = jointKey & 1; + + b2Joint* joint = b2JointArray_Get( &world->joints, jointId ); + jointKey = joint->edges[edgeIndex].nextKey; + + int otherEdgeIndex = edgeIndex ^ 1; + int otherBodyId = joint->edges[otherEdgeIndex].bodyId; + b2Body* otherBody = b2BodyArray_Get( &world->bodies, otherBodyId ); + + if ( otherBody->setIndex == b2_disabledSet ) + { + continue; + } + + if ( body->type == b2_staticBody && otherBody->type == b2_staticBody ) + { + continue; + } + + b2LinkJoint( world, joint, false ); + } + + b2MergeAwakeIslands( world ); + } + + // Body type affects the mass + b2UpdateBodyMassData( world, body ); + + b2ValidateSolverSets( world ); +} + +void b2Body_SetUserData( b2BodyId bodyId, void* userData ) +{ + b2World* world = b2GetWorld( bodyId.world0 ); + b2Body* body = b2GetBodyFullId( world, bodyId ); + body->userData = userData; +} + +void* b2Body_GetUserData( b2BodyId bodyId ) +{ + b2World* world = b2GetWorld( bodyId.world0 ); + b2Body* body = b2GetBodyFullId( world, bodyId ); + return body->userData; +} + +float b2Body_GetMass( b2BodyId bodyId ) +{ + b2World* world = b2GetWorld( bodyId.world0 ); + b2Body* body = b2GetBodyFullId( world, bodyId ); + b2BodySim* bodySim = b2GetBodySim( world, body ); + return bodySim->mass; +} + +float b2Body_GetRotationalInertia( b2BodyId bodyId ) +{ + b2World* world = b2GetWorld( bodyId.world0 ); + b2Body* body = b2GetBodyFullId( world, bodyId ); + b2BodySim* bodySim = b2GetBodySim( world, body ); + return bodySim->inertia; +} + +b2Vec2 b2Body_GetLocalCenterOfMass( b2BodyId bodyId ) +{ + b2World* world = b2GetWorld( bodyId.world0 ); + b2Body* body = b2GetBodyFullId( world, bodyId ); + b2BodySim* bodySim = b2GetBodySim( world, body ); + return bodySim->localCenter; +} + +b2Vec2 b2Body_GetWorldCenterOfMass( b2BodyId bodyId ) +{ + b2World* world = b2GetWorld( bodyId.world0 ); + b2Body* body = b2GetBodyFullId( world, bodyId ); + b2BodySim* bodySim = b2GetBodySim( world, body ); + return bodySim->center; +} + +void b2Body_SetMassData( b2BodyId bodyId, b2MassData massData ) +{ + B2_ASSERT( b2IsValid( massData.mass ) && massData.mass >= 0.0f ); + B2_ASSERT( b2IsValid( massData.rotationalInertia ) && massData.rotationalInertia >= 0.0f ); + B2_ASSERT( b2Vec2_IsValid( massData.center ) ); + + b2World* world = b2GetWorldLocked( bodyId.world0 ); + if ( world == NULL ) + { + return; + } + + b2Body* body = b2GetBodyFullId( world, bodyId ); + b2BodySim* bodySim = b2GetBodySim( world, body ); + + bodySim->mass = massData.mass; + bodySim->inertia = massData.rotationalInertia; + bodySim->localCenter = massData.center; + + b2Vec2 center = b2TransformPoint( bodySim->transform, massData.center ); + bodySim->center = center; + bodySim->center0 = center; + + bodySim->invMass = bodySim->mass > 0.0f ? 1.0f / bodySim->mass : 0.0f; + bodySim->invInertia = bodySim->inertia > 0.0f ? 1.0f / bodySim->inertia : 0.0f; +} + +b2MassData b2Body_GetMassData( b2BodyId bodyId ) +{ + b2World* world = b2GetWorld( bodyId.world0 ); + b2Body* body = b2GetBodyFullId( world, bodyId ); + b2BodySim* bodySim = b2GetBodySim( world, body ); + b2MassData massData = { bodySim->mass, bodySim->localCenter, bodySim->inertia }; + return massData; +} + +void b2Body_ApplyMassFromShapes( b2BodyId bodyId ) +{ + b2World* world = b2GetWorldLocked( bodyId.world0 ); + if ( world == NULL ) + { + return; + } + + b2Body* body = b2GetBodyFullId( world, bodyId ); + b2UpdateBodyMassData( world, body ); +} + +void b2Body_SetAutomaticMass( b2BodyId bodyId, bool automaticMass ) +{ + b2World* world = b2GetWorldLocked( bodyId.world0 ); + if ( world == NULL ) + { + return; + } + + b2Body* body = b2GetBodyFullId( world, bodyId ); + body->automaticMass = automaticMass; +} + +bool b2Body_GetAutomaticMass( b2BodyId bodyId ) +{ + b2World* world = b2GetWorld( bodyId.world0 ); + b2Body* body = b2GetBodyFullId( world, bodyId ); + return body->automaticMass; +} + +void b2Body_SetLinearDamping( b2BodyId bodyId, float linearDamping ) +{ + B2_ASSERT( b2IsValid( linearDamping ) && linearDamping >= 0.0f ); + + b2World* world = b2GetWorldLocked( bodyId.world0 ); + if ( world == NULL ) + { + return; + } + + b2Body* body = b2GetBodyFullId( world, bodyId ); + b2BodySim* bodySim = b2GetBodySim( world, body ); + bodySim->linearDamping = linearDamping; +} + +float b2Body_GetLinearDamping( b2BodyId bodyId ) +{ + b2World* world = b2GetWorld( bodyId.world0 ); + b2Body* body = b2GetBodyFullId( world, bodyId ); + b2BodySim* bodySim = b2GetBodySim( world, body ); + return bodySim->linearDamping; +} + +void b2Body_SetAngularDamping( b2BodyId bodyId, float angularDamping ) +{ + B2_ASSERT( b2IsValid( angularDamping ) && angularDamping >= 0.0f ); + + b2World* world = b2GetWorldLocked( bodyId.world0 ); + if ( world == NULL ) + { + return; + } + + b2Body* body = b2GetBodyFullId( world, bodyId ); + b2BodySim* bodySim = b2GetBodySim( world, body ); + bodySim->angularDamping = angularDamping; +} + +float b2Body_GetAngularDamping( b2BodyId bodyId ) +{ + b2World* world = b2GetWorld( bodyId.world0 ); + b2Body* body = b2GetBodyFullId( world, bodyId ); + b2BodySim* bodySim = b2GetBodySim( world, body ); + return bodySim->angularDamping; +} + +void b2Body_SetGravityScale( b2BodyId bodyId, float gravityScale ) +{ + B2_ASSERT( b2Body_IsValid( bodyId ) ); + B2_ASSERT( b2IsValid( gravityScale ) ); + + b2World* world = b2GetWorldLocked( bodyId.world0 ); + if ( world == NULL ) + { + return; + } + + b2Body* body = b2GetBodyFullId( world, bodyId ); + b2BodySim* bodySim = b2GetBodySim( world, body ); + bodySim->gravityScale = gravityScale; +} + +float b2Body_GetGravityScale( b2BodyId bodyId ) +{ + B2_ASSERT( b2Body_IsValid( bodyId ) ); + b2World* world = b2GetWorld( bodyId.world0 ); + b2Body* body = b2GetBodyFullId( world, bodyId ); + b2BodySim* bodySim = b2GetBodySim( world, body ); + return bodySim->gravityScale; +} + +bool b2Body_IsAwake( b2BodyId bodyId ) +{ + b2World* world = b2GetWorld( bodyId.world0 ); + b2Body* body = b2GetBodyFullId( world, bodyId ); + return body->setIndex == b2_awakeSet; +} + +void b2Body_SetAwake( b2BodyId bodyId, bool awake ) +{ + b2World* world = b2GetWorldLocked( bodyId.world0 ); + if ( world == NULL ) + { + return; + } + + b2Body* body = b2GetBodyFullId( world, bodyId ); + + if ( awake && body->setIndex >= b2_firstSleepingSet ) + { + b2WakeBody( world, body ); + } + else if ( awake == false && body->setIndex == b2_awakeSet ) + { + b2Island* island = b2IslandArray_Get( &world->islands, body->islandId ); + if ( island->constraintRemoveCount > 0 ) + { + // Must split the island before sleeping. This is expensive. + b2SplitIsland( world, body->islandId ); + } + + b2TrySleepIsland( world, body->islandId ); + } +} + +bool b2Body_IsEnabled( b2BodyId bodyId ) +{ + b2World* world = b2GetWorld( bodyId.world0 ); + b2Body* body = b2GetBodyFullId( world, bodyId ); + return body->setIndex != b2_disabledSet; +} + +bool b2Body_IsSleepEnabled( b2BodyId bodyId ) +{ + b2World* world = b2GetWorld( bodyId.world0 ); + b2Body* body = b2GetBodyFullId( world, bodyId ); + return body->enableSleep; +} + +void b2Body_SetSleepThreshold( b2BodyId bodyId, float sleepThreshold ) +{ + b2World* world = b2GetWorld( bodyId.world0 ); + b2Body* body = b2GetBodyFullId( world, bodyId ); + body->sleepThreshold = sleepThreshold; +} + +float b2Body_GetSleepThreshold( b2BodyId bodyId ) +{ + b2World* world = b2GetWorld( bodyId.world0 ); + b2Body* body = b2GetBodyFullId( world, bodyId ); + return body->sleepThreshold; +} + +void b2Body_EnableSleep( b2BodyId bodyId, bool enableSleep ) +{ + b2World* world = b2GetWorldLocked( bodyId.world0 ); + if ( world == NULL ) + { + return; + } + + b2Body* body = b2GetBodyFullId( world, bodyId ); + body->enableSleep = enableSleep; + + if ( enableSleep == false ) + { + b2WakeBody( world, body ); + } +} + +// Disabling a body requires a lot of detailed bookkeeping, but it is a valuable feature. +// The most challenging aspect that joints may connect to bodies that are not disabled. +void b2Body_Disable( b2BodyId bodyId ) +{ + b2World* world = b2GetWorldLocked( bodyId.world0 ); + if ( world == NULL ) + { + return; + } + + b2Body* body = b2GetBodyFullId( world, bodyId ); + if ( body->setIndex == b2_disabledSet ) + { + return; + } + + // Destroy contacts and wake bodies touching this body. This avoid floating bodies. + // This is necessary even for static bodies. + bool wakeBodies = true; + b2DestroyBodyContacts( world, body, wakeBodies ); + + // Disabled bodies are not in an island. + b2RemoveBodyFromIsland( world, body ); + + // Remove shapes from broad-phase + int shapeId = body->headShapeId; + while ( shapeId != B2_NULL_INDEX ) + { + b2Shape* shape = b2ShapeArray_Get( &world->shapes, shapeId ); + shapeId = shape->nextShapeId; + b2DestroyShapeProxy( shape, &world->broadPhase ); + } + + // Transfer simulation data to disabled set + b2SolverSet* set = b2SolverSetArray_Get( &world->solverSets, body->setIndex ); + b2SolverSet* disabledSet = b2SolverSetArray_Get( &world->solverSets, b2_disabledSet ); + + // Transfer body sim + b2TransferBody( world, disabledSet, set, body ); + + // Unlink joints and transfer + int jointKey = body->headJointKey; + while ( jointKey != B2_NULL_INDEX ) + { + int jointId = jointKey >> 1; + int edgeIndex = jointKey & 1; + + b2Joint* joint = b2JointArray_Get( &world->joints, jointId ); + jointKey = joint->edges[edgeIndex].nextKey; + + // joint may already be disabled by other body + if ( joint->setIndex == b2_disabledSet ) + { + continue; + } + + B2_ASSERT( joint->setIndex == set->setIndex || set->setIndex == b2_staticSet ); + + // Remove joint from island + if ( joint->islandId != B2_NULL_INDEX ) + { + b2UnlinkJoint( world, joint ); + } + + // Transfer joint to disabled set + b2SolverSet* jointSet = b2SolverSetArray_Get( &world->solverSets, joint->setIndex ); + b2TransferJoint( world, disabledSet, jointSet, joint ); + } + + b2ValidateConnectivity( world ); + b2ValidateSolverSets( world ); +} + +void b2Body_Enable( b2BodyId bodyId ) +{ + b2World* world = b2GetWorldLocked( bodyId.world0 ); + if ( world == NULL ) + { + return; + } + + b2Body* body = b2GetBodyFullId( world, bodyId ); + if ( body->setIndex != b2_disabledSet ) + { + return; + } + + b2SolverSet* disabledSet = b2SolverSetArray_Get( &world->solverSets, b2_disabledSet ); + int setId = body->type == b2_staticBody ? b2_staticSet : b2_awakeSet; + b2SolverSet* targetSet = b2SolverSetArray_Get( &world->solverSets, setId ); + + b2TransferBody( world, targetSet, disabledSet, body ); + + b2Transform transform = b2GetBodyTransformQuick( world, body ); + + // Add shapes to broad-phase + b2BodyType proxyType = body->type; + bool forcePairCreation = true; + int shapeId = body->headShapeId; + while ( shapeId != B2_NULL_INDEX ) + { + b2Shape* shape = b2ShapeArray_Get( &world->shapes, shapeId ); + shapeId = shape->nextShapeId; + + b2CreateShapeProxy( shape, &world->broadPhase, proxyType, transform, forcePairCreation ); + } + + if ( setId != b2_staticSet ) + { + b2CreateIslandForBody( world, setId, body ); + } + + // Transfer joints. If the other body is disabled, don't transfer. + // If the other body is sleeping, wake it. + bool mergeIslands = false; + int jointKey = body->headJointKey; + while ( jointKey != B2_NULL_INDEX ) + { + int jointId = jointKey >> 1; + int edgeIndex = jointKey & 1; + + b2Joint* joint = b2JointArray_Get( &world->joints, jointId ); + B2_ASSERT( joint->setIndex == b2_disabledSet ); + B2_ASSERT( joint->islandId == B2_NULL_INDEX ); + + jointKey = joint->edges[edgeIndex].nextKey; + + b2Body* bodyA = b2BodyArray_Get( &world->bodies, joint->edges[0].bodyId ); + b2Body* bodyB = b2BodyArray_Get( &world->bodies, joint->edges[1].bodyId ); + + if ( bodyA->setIndex == b2_disabledSet || bodyB->setIndex == b2_disabledSet ) + { + // one body is still disabled + continue; + } + + // Transfer joint first + int jointSetId; + if ( bodyA->setIndex == b2_staticSet && bodyB->setIndex == b2_staticSet ) + { + jointSetId = b2_staticSet; + } + else if ( bodyA->setIndex == b2_staticSet ) + { + jointSetId = bodyB->setIndex; + } + else + { + jointSetId = bodyA->setIndex; + } + + b2SolverSet* jointSet = b2SolverSetArray_Get( &world->solverSets, jointSetId ); + b2TransferJoint( world, jointSet, disabledSet, joint ); + + // Now that the joint is in the correct set, I can link the joint in the island. + if ( jointSetId != b2_staticSet ) + { + b2LinkJoint( world, joint, mergeIslands ); + } + } + + // Now merge islands + b2MergeAwakeIslands( world ); + + b2ValidateSolverSets( world ); +} + +void b2Body_SetFixedRotation( b2BodyId bodyId, bool flag ) +{ + b2World* world = b2GetWorldLocked( bodyId.world0 ); + if ( world == NULL ) + { + return; + } + + b2Body* body = b2GetBodyFullId( world, bodyId ); + if ( body->fixedRotation != flag ) + { + body->fixedRotation = flag; + + b2BodyState* state = b2GetBodyState( world, body ); + if ( state != NULL ) + { + state->angularVelocity = 0.0f; + } + b2UpdateBodyMassData( world, body ); + } +} + +bool b2Body_IsFixedRotation( b2BodyId bodyId ) +{ + b2World* world = b2GetWorld( bodyId.world0 ); + b2Body* body = b2GetBodyFullId( world, bodyId ); + return body->fixedRotation; +} + +void b2Body_SetBullet( b2BodyId bodyId, bool flag ) +{ + b2World* world = b2GetWorldLocked( bodyId.world0 ); + if ( world == NULL ) + { + return; + } + + b2Body* body = b2GetBodyFullId( world, bodyId ); + b2BodySim* bodySim = b2GetBodySim( world, body ); + bodySim->isBullet = flag; +} + +bool b2Body_IsBullet( b2BodyId bodyId ) +{ + b2World* world = b2GetWorld( bodyId.world0 ); + b2Body* body = b2GetBodyFullId( world, bodyId ); + b2BodySim* bodySim = b2GetBodySim( world, body ); + return bodySim->isBullet; +} + +void b2Body_EnableHitEvents( b2BodyId bodyId, bool enableHitEvents ) +{ + b2World* world = b2GetWorld( bodyId.world0 ); + b2Body* body = b2GetBodyFullId( world, bodyId ); + int shapeId = body->headShapeId; + while ( shapeId != B2_NULL_INDEX ) + { + b2Shape* shape = b2ShapeArray_Get( &world->shapes, shapeId ); + shape->enableHitEvents = enableHitEvents; + shapeId = shape->nextShapeId; + } +} + +b2WorldId b2Body_GetWorld( b2BodyId bodyId ) +{ + b2World* world = b2GetWorld( bodyId.world0 ); + return ( b2WorldId ){ bodyId.world0 + 1, world->revision }; +} + +int b2Body_GetShapeCount( b2BodyId bodyId ) +{ + b2World* world = b2GetWorld( bodyId.world0 ); + b2Body* body = b2GetBodyFullId( world, bodyId ); + return body->shapeCount; +} + +int b2Body_GetShapes( b2BodyId bodyId, b2ShapeId* shapeArray, int capacity ) +{ + b2World* world = b2GetWorld( bodyId.world0 ); + b2Body* body = b2GetBodyFullId( world, bodyId ); + int shapeId = body->headShapeId; + int shapeCount = 0; + while ( shapeId != B2_NULL_INDEX && shapeCount < capacity ) + { + b2Shape* shape = b2ShapeArray_Get( &world->shapes, shapeId ); + b2ShapeId id = { shape->id + 1, bodyId.world0, shape->revision }; + shapeArray[shapeCount] = id; + shapeCount += 1; + + shapeId = shape->nextShapeId; + } + + return shapeCount; +} + +int b2Body_GetJointCount( b2BodyId bodyId ) +{ + b2World* world = b2GetWorld( bodyId.world0 ); + b2Body* body = b2GetBodyFullId( world, bodyId ); + return body->jointCount; +} + +int b2Body_GetJoints( b2BodyId bodyId, b2JointId* jointArray, int capacity ) +{ + b2World* world = b2GetWorld( bodyId.world0 ); + b2Body* body = b2GetBodyFullId( world, bodyId ); + int jointKey = body->headJointKey; + + int jointCount = 0; + while ( jointKey != B2_NULL_INDEX && jointCount < capacity ) + { + int jointId = jointKey >> 1; + int edgeIndex = jointKey & 1; + + b2Joint* joint = b2JointArray_Get( &world->joints, jointId ); + + b2JointId id = { jointId + 1, bodyId.world0, joint->revision }; + jointArray[jointCount] = id; + jointCount += 1; + + jointKey = joint->edges[edgeIndex].nextKey; + } + + return jointCount; +} + +bool b2ShouldBodiesCollide( b2World* world, b2Body* bodyA, b2Body* bodyB ) +{ + if ( bodyA->type != b2_dynamicBody && bodyB->type != b2_dynamicBody ) + { + return false; + } + + int jointKey; + int otherBodyId; + if ( bodyA->jointCount < bodyB->jointCount ) + { + jointKey = bodyA->headJointKey; + otherBodyId = bodyB->id; + } + else + { + jointKey = bodyB->headJointKey; + otherBodyId = bodyA->id; + } + + while ( jointKey != B2_NULL_INDEX ) + { + int jointId = jointKey >> 1; + int edgeIndex = jointKey & 1; + int otherEdgeIndex = edgeIndex ^ 1; + + b2Joint* joint = b2JointArray_Get( &world->joints, jointId ); + if ( joint->collideConnected == false && joint->edges[otherEdgeIndex].bodyId == otherBodyId ) + { + return false; + } + + jointKey = joint->edges[edgeIndex].nextKey; + } + + return true; +} diff --git a/3rdparty/box2d/src/body.h b/3rdparty/box2d/src/body.h new file mode 100644 index 000000000000..2265ed0b56f1 --- /dev/null +++ b/3rdparty/box2d/src/body.h @@ -0,0 +1,172 @@ +// SPDX-FileCopyrightText: 2023 Erin Catto +// SPDX-License-Identifier: MIT + +#pragma once + +#include "array.h" + +#include "box2d/math_functions.h" +#include "box2d/types.h" + +typedef struct b2Polygon b2Polygon; +typedef struct b2World b2World; +typedef struct b2JointSim b2JointSim; +typedef struct b2ContactSim b2ContactSim; +typedef struct b2Shape b2Shape; +typedef struct b2Body b2Body; + +// Body organizational details that are not used in the solver. +typedef struct b2Body +{ + void* userData; + + // index of solver set stored in b2World + // may be B2_NULL_INDEX + int setIndex; + + // body sim and state index within set + // may be B2_NULL_INDEX + int localIndex; + + // [31 : contactId | 1 : edgeIndex] + int headContactKey; + int contactCount; + + // todo maybe move this to the body sim + int headShapeId; + int shapeCount; + + int headChainId; + + // [31 : jointId | 1 : edgeIndex] + int headJointKey; + int jointCount; + + // All enabled dynamic and kinematic bodies are in an island. + int islandId; + + // doubly-linked island list + int islandPrev; + int islandNext; + + float sleepThreshold; + float sleepTime; + + // this is used to adjust the fellAsleep flag in the body move array + int bodyMoveIndex; + + int id; + + b2BodyType type; + + // This is monotonically advanced when a body is allocated in this slot + // Used to check for invalid b2BodyId + uint16_t revision; + + bool enableSleep; + bool fixedRotation; + bool isSpeedCapped; + bool isMarked; + bool automaticMass; +} b2Body; + +// The body state is designed for fast conversion to and from SIMD via scatter-gather. +// Only awake dynamic and kinematic bodies have a body state. +// This is used in the performance critical constraint solver +// +// 32 bytes +typedef struct b2BodyState +{ + b2Vec2 linearVelocity; // 8 + float angularVelocity; // 4 + int flags; // 4 + + // Using delta position reduces round-off error far from the origin + b2Vec2 deltaPosition; // 8 + + // Using delta rotation because I cannot access the full rotation on static bodies in + // the solver and must use zero delta rotation for static bodies (c,s) = (1,0) + b2Rot deltaRotation; // 8 +} b2BodyState; + +// Identity body state, notice the deltaRotation is {1, 0} +static const b2BodyState b2_identityBodyState = { { 0.0f, 0.0f }, 0.0f, 0, { 0.0f, 0.0f }, { 1.0f, 0.0f } }; + +// Body simulation data used for integration of position and velocity +// Transform data used for collision and solver preparation. +typedef struct b2BodySim +{ + // todo better to have transform in sim or in base body? Try both! + // transform for body origin + b2Transform transform; + + // center of mass position in world space + b2Vec2 center; + + // previous rotation and COM for TOI + b2Rot rotation0; + b2Vec2 center0; + + // location of center of mass relative to the body origin + b2Vec2 localCenter; + + b2Vec2 force; + float torque; + + float mass, invMass; + + // Rotational inertia about the center of mass. + float inertia, invInertia; + + float minExtent; + float maxExtent; + float linearDamping; + float angularDamping; + float gravityScale; + + // body data can be moved around, the id is stable (used in b2BodyId) + int bodyId; + + // todo eliminate + bool isFast; + bool isBullet; + bool isSpeedCapped; + bool allowFastRotation; + bool enlargeAABB; +} b2BodySim; + +// Get a validated body from a world using an id. +b2Body* b2GetBodyFullId( b2World* world, b2BodyId bodyId ); + +b2Transform b2GetBodyTransformQuick( b2World* world, b2Body* body ); +b2Transform b2GetBodyTransform( b2World* world, int bodyId ); + +// Create a b2BodyId from a raw id. +b2BodyId b2MakeBodyId( b2World* world, int bodyId ); + +bool b2ShouldBodiesCollide( b2World* world, b2Body* bodyA, b2Body* bodyB ); +bool b2IsBodyAwake( b2World* world, b2Body* body ); + +b2BodySim* b2GetBodySim( b2World* world, b2Body* body ); +b2BodyState* b2GetBodyState( b2World* world, b2Body* body ); + +// careful calling this because it can invalidate body, state, joint, and contact pointers +bool b2WakeBody( b2World* world, b2Body* body ); + +void b2UpdateBodyMassData( b2World* world, b2Body* body ); + +static inline b2Sweep b2MakeSweep( const b2BodySim* bodySim ) +{ + b2Sweep s; + s.c1 = bodySim->center0; + s.c2 = bodySim->center; + s.q1 = bodySim->rotation0; + s.q2 = bodySim->transform.q; + s.localCenter = bodySim->localCenter; + return s; +} + +// Define inline functions for arrays +B2_ARRAY_INLINE( b2Body, b2Body ); +B2_ARRAY_INLINE( b2BodySim, b2BodySim ); +B2_ARRAY_INLINE( b2BodyState, b2BodyState ); diff --git a/3rdparty/box2d/src/box2d.natvis b/3rdparty/box2d/src/box2d.natvis new file mode 100644 index 000000000000..89e4f92b76d1 --- /dev/null +++ b/3rdparty/box2d/src/box2d.natvis @@ -0,0 +1,27 @@ + + + + [{m128_f32[0]}, {m128_f32[1]}, {m128_f32[2]}, {m128_f32[3]}] + + m128_f32[0] + m128_f32[1] + m128_f32[2] + m128_f32[3] + (void*)this + + + + [{m256_f32[0]}, {m256_f32[1]}, {m256_f32[2]}, {m256_f32[3]}, {m256_f32[4]}, {m256_f32[5]}, {m256_f32[6]}, {m256_f32[7]}] + + m256_f32[0] + m256_f32[1] + m256_f32[2] + m256_f32[3] + m256_f32[4] + m256_f32[5] + m256_f32[6] + m256_f32[7] + (void*)this + + + diff --git a/3rdparty/box2d/src/broad_phase.c b/3rdparty/box2d/src/broad_phase.c new file mode 100644 index 000000000000..8043ea13d10b --- /dev/null +++ b/3rdparty/box2d/src/broad_phase.c @@ -0,0 +1,490 @@ +// SPDX-FileCopyrightText: 2023 Erin Catto +// SPDX-License-Identifier: MIT + +#if defined( _MSC_VER ) && !defined( _CRT_SECURE_NO_WARNINGS ) +#define _CRT_SECURE_NO_WARNINGS +#endif + +#include "broad_phase.h" + +#include "aabb.h" +#include "array.h" +#include "body.h" +#include "contact.h" +#include "core.h" +#include "shape.h" +#include "stack_allocator.h" +#include "world.h" + +#include +#include +#include + +// #include + +// static FILE* s_file = NULL; + +void b2CreateBroadPhase( b2BroadPhase* bp ) +{ + _Static_assert( b2_bodyTypeCount == 3, "must be three body types" ); + + // if (s_file == NULL) + //{ + // s_file = fopen("pairs01.txt", "a"); + // fprintf(s_file, "============\n\n"); + // } + + bp->proxyCount = 0; + bp->moveSet = b2CreateSet( 16 ); + bp->moveArray = b2IntArray_Create( 16 ); + bp->moveResults = NULL; + bp->movePairs = NULL; + bp->movePairCapacity = 0; + bp->movePairIndex = 0; + bp->pairSet = b2CreateSet( 32 ); + + for ( int i = 0; i < b2_bodyTypeCount; ++i ) + { + bp->trees[i] = b2DynamicTree_Create(); + } +} + +void b2DestroyBroadPhase( b2BroadPhase* bp ) +{ + for ( int i = 0; i < b2_bodyTypeCount; ++i ) + { + b2DynamicTree_Destroy( bp->trees + i ); + } + + b2DestroySet( &bp->moveSet ); + b2IntArray_Destroy( &bp->moveArray ); + b2DestroySet( &bp->pairSet ); + + memset( bp, 0, sizeof( b2BroadPhase ) ); + + // if (s_file != NULL) + //{ + // fclose(s_file); + // s_file = NULL; + // } +} + +static inline void b2UnBufferMove( b2BroadPhase* bp, int proxyKey ) +{ + bool found = b2RemoveKey( &bp->moveSet, proxyKey + 1 ); + + if ( found ) + { + // Purge from move buffer. Linear search. + // todo if I can iterate the move set then I don't need the moveArray + int count = bp->moveArray.count; + for ( int i = 0; i < count; ++i ) + { + if ( bp->moveArray.data[i] == proxyKey ) + { + b2IntArray_RemoveSwap( &bp->moveArray, i ); + break; + } + } + } +} + +int b2BroadPhase_CreateProxy( b2BroadPhase* bp, b2BodyType proxyType, b2AABB aabb, uint64_t categoryBits, int shapeIndex, + bool forcePairCreation ) +{ + B2_ASSERT( 0 <= proxyType && proxyType < b2_bodyTypeCount ); + int proxyId = b2DynamicTree_CreateProxy( bp->trees + proxyType, aabb, categoryBits, shapeIndex ); + int proxyKey = B2_PROXY_KEY( proxyId, proxyType ); + if ( proxyType != b2_staticBody || forcePairCreation ) + { + b2BufferMove( bp, proxyKey ); + } + return proxyKey; +} + +void b2BroadPhase_DestroyProxy( b2BroadPhase* bp, int proxyKey ) +{ + B2_ASSERT( bp->moveArray.count == (int)bp->moveSet.count ); + b2UnBufferMove( bp, proxyKey ); + + --bp->proxyCount; + + b2BodyType proxyType = B2_PROXY_TYPE( proxyKey ); + int proxyId = B2_PROXY_ID( proxyKey ); + + B2_ASSERT( 0 <= proxyType && proxyType <= b2_bodyTypeCount ); + b2DynamicTree_DestroyProxy( bp->trees + proxyType, proxyId ); +} + +void b2BroadPhase_MoveProxy( b2BroadPhase* bp, int proxyKey, b2AABB aabb ) +{ + b2BodyType proxyType = B2_PROXY_TYPE( proxyKey ); + int proxyId = B2_PROXY_ID( proxyKey ); + + b2DynamicTree_MoveProxy( bp->trees + proxyType, proxyId, aabb ); + b2BufferMove( bp, proxyKey ); +} + +void b2BroadPhase_EnlargeProxy( b2BroadPhase* bp, int proxyKey, b2AABB aabb ) +{ + B2_ASSERT( proxyKey != B2_NULL_INDEX ); + int typeIndex = B2_PROXY_TYPE( proxyKey ); + int proxyId = B2_PROXY_ID( proxyKey ); + + B2_ASSERT( typeIndex != b2_staticBody ); + + b2DynamicTree_EnlargeProxy( bp->trees + typeIndex, proxyId, aabb ); + b2BufferMove( bp, proxyKey ); +} + +typedef struct b2MovePair +{ + int shapeIndexA; + int shapeIndexB; + b2MovePair* next; + bool heap; +} b2MovePair; + +typedef struct b2MoveResult +{ + b2MovePair* pairList; +} b2MoveResult; + +typedef struct b2QueryPairContext +{ + b2World* world; + b2MoveResult* moveResult; + b2BodyType queryTreeType; + int queryProxyKey; + int queryShapeIndex; +} b2QueryPairContext; + +// This is called from b2DynamicTree::Query when we are gathering pairs. +static bool b2PairQueryCallback( int proxyId, int shapeId, void* context ) +{ + b2QueryPairContext* queryContext = context; + b2BroadPhase* bp = &queryContext->world->broadPhase; + + int proxyKey = B2_PROXY_KEY( proxyId, queryContext->queryTreeType ); + + // A proxy cannot form a pair with itself. + if ( proxyKey == queryContext->queryProxyKey ) + { + return true; + } + + // Is this proxy also moving? + if ( queryContext->queryTreeType != b2_staticBody && proxyKey < queryContext->queryProxyKey ) + { + bool moved = b2ContainsKey( &bp->moveSet, proxyKey + 1 ); + if ( moved ) + { + // Both proxies are moving. Avoid duplicate pairs. + return true; + } + } + + uint64_t pairKey = B2_SHAPE_PAIR_KEY( shapeId, queryContext->queryShapeIndex ); + if ( b2ContainsKey( &bp->pairSet, pairKey ) ) + { + // contact exists + return true; + } + + int shapeIdA, shapeIdB; + if ( proxyKey < queryContext->queryProxyKey ) + { + shapeIdA = shapeId; + shapeIdB = queryContext->queryShapeIndex; + } + else + { + shapeIdA = queryContext->queryShapeIndex; + shapeIdB = shapeId; + } + + b2World* world = queryContext->world; + + b2Shape* shapeA = b2ShapeArray_Get( &world->shapes, shapeIdA ); + b2Shape* shapeB = b2ShapeArray_Get( &world->shapes, shapeIdB ); + + int bodyIdA = shapeA->bodyId; + int bodyIdB = shapeB->bodyId; + + // Are the shapes on the same body? + if ( bodyIdA == bodyIdB ) + { + return true; + } + + if ( b2ShouldShapesCollide( shapeA->filter, shapeB->filter ) == false ) + { + return true; + } + + // Sensors don't collide with other sensors + if ( shapeA->isSensor == true && shapeB->isSensor == true ) + { + return true; + } + + // Does a joint override collision? + b2Body* bodyA = b2BodyArray_Get( &world->bodies, bodyIdA ); + b2Body* bodyB = b2BodyArray_Get( &world->bodies, bodyIdB ); + if ( b2ShouldBodiesCollide( world, bodyA, bodyB ) == false ) + { + return true; + } + + // Custom user filter + b2CustomFilterFcn* customFilterFcn = queryContext->world->customFilterFcn; + if ( customFilterFcn != NULL ) + { + b2ShapeId idA = { shapeIdA + 1, world->worldId, shapeA->revision }; + b2ShapeId idB = { shapeIdB + 1, world->worldId, shapeB->revision }; + bool shouldCollide = customFilterFcn( idA, idB, queryContext->world->customFilterContext ); + if ( shouldCollide == false ) + { + return true; + } + } + + // #todo per thread to eliminate atomic? + int pairIndex = atomic_fetch_add( &bp->movePairIndex, 1 ); + + b2MovePair* pair; + if ( pairIndex < bp->movePairCapacity ) + { + pair = bp->movePairs + pairIndex; + pair->heap = false; + } + else + { + pair = b2Alloc( sizeof( b2MovePair ) ); + pair->heap = true; + } + + pair->shapeIndexA = shapeIdA; + pair->shapeIndexB = shapeIdB; + pair->next = queryContext->moveResult->pairList; + queryContext->moveResult->pairList = pair; + + // continue the query + return true; +} + +void b2FindPairsTask( int startIndex, int endIndex, uint32_t threadIndex, void* context ) +{ + b2TracyCZoneNC( pair_task, "Pair Task", b2_colorAquamarine, true ); + + B2_MAYBE_UNUSED( threadIndex ); + + b2World* world = context; + b2BroadPhase* bp = &world->broadPhase; + + b2QueryPairContext queryContext; + queryContext.world = world; + + for ( int i = startIndex; i < endIndex; ++i ) + { + // Initialize move result for this moved proxy + queryContext.moveResult = bp->moveResults + i; + queryContext.moveResult->pairList = NULL; + + int proxyKey = bp->moveArray.data[i]; + if ( proxyKey == B2_NULL_INDEX ) + { + // proxy was destroyed after it moved + continue; + } + + b2BodyType proxyType = B2_PROXY_TYPE( proxyKey ); + + int proxyId = B2_PROXY_ID( proxyKey ); + queryContext.queryProxyKey = proxyKey; + + const b2DynamicTree* baseTree = bp->trees + proxyType; + + // We have to query the tree with the fat AABB so that + // we don't fail to create a contact that may touch later. + b2AABB fatAABB = b2DynamicTree_GetAABB( baseTree, proxyId ); + queryContext.queryShapeIndex = b2DynamicTree_GetUserData( baseTree, proxyId ); + + // Query trees. Only dynamic proxies collide with kinematic and static proxies. + // Using b2_defaultMaskBits so that b2Filter::groupIndex works. + if ( proxyType == b2_dynamicBody ) + { + // consider using bits = groupIndex > 0 ? b2_defaultMaskBits : maskBits + queryContext.queryTreeType = b2_kinematicBody; + b2DynamicTree_Query( bp->trees + b2_kinematicBody, fatAABB, b2_defaultMaskBits, b2PairQueryCallback, &queryContext ); + + queryContext.queryTreeType = b2_staticBody; + b2DynamicTree_Query( bp->trees + b2_staticBody, fatAABB, b2_defaultMaskBits, b2PairQueryCallback, &queryContext ); + } + + // All proxies collide with dynamic proxies + // Using b2_defaultMaskBits so that b2Filter::groupIndex works. + queryContext.queryTreeType = b2_dynamicBody; + b2DynamicTree_Query( bp->trees + b2_dynamicBody, fatAABB, b2_defaultMaskBits, b2PairQueryCallback, &queryContext ); + } + + b2TracyCZoneEnd( pair_task ); +} + +void b2UpdateBroadPhasePairs( b2World* world ) +{ + b2BroadPhase* bp = &world->broadPhase; + + int moveCount = bp->moveArray.count; + B2_ASSERT( moveCount == (int)bp->moveSet.count ); + + if ( moveCount == 0 ) + { + return; + } + + b2TracyCZoneNC( update_pairs, "Pairs", b2_colorMagenta, true ); + + b2StackAllocator* alloc = &world->stackAllocator; + + // todo these could be in the step context + bp->moveResults = b2AllocateStackItem( alloc, moveCount * sizeof( b2MoveResult ), "move results" ); + bp->movePairCapacity = 16 * moveCount; + bp->movePairs = b2AllocateStackItem( alloc, bp->movePairCapacity * sizeof( b2MovePair ), "move pairs" ); + bp->movePairIndex = 0; + +#ifndef NDEBUG + extern _Atomic int g_probeCount; + g_probeCount = 0; +#endif + + int minRange = 64; + void* userPairTask = world->enqueueTaskFcn( &b2FindPairsTask, moveCount, minRange, world, world->userTaskContext ); + world->finishTaskFcn( userPairTask, world->userTaskContext ); + world->taskCount += 1; + + b2TracyCZoneNC( create_contacts, "Create Contacts", b2_colorGold, true ); + + // Single-threaded work + // - Clear move flags + // - Create contacts in deterministic order + for ( int i = 0; i < moveCount; ++i ) + { + b2MoveResult* result = bp->moveResults + i; + b2MovePair* pair = result->pairList; + while ( pair != NULL ) + { + int shapeIdA = pair->shapeIndexA; + int shapeIdB = pair->shapeIndexB; + + // if (s_file != NULL) + //{ + // fprintf(s_file, "%d %d\n", shapeIdA, shapeIdB); + // } + + b2Shape* shapeA = b2ShapeArray_Get( &world->shapes, shapeIdA ); + b2Shape* shapeB = b2ShapeArray_Get( &world->shapes, shapeIdB ); + + b2CreateContact( world, shapeA, shapeB ); + + if ( pair->heap ) + { + b2MovePair* temp = pair; + pair = pair->next; + b2Free( temp, sizeof( b2MovePair ) ); + } + else + { + pair = pair->next; + } + } + + // if (s_file != NULL) + //{ + // fprintf(s_file, "\n"); + // } + } + + // if (s_file != NULL) + //{ + // fprintf(s_file, "count = %d\n\n", pairCount); + // } + + // Reset move buffer + b2IntArray_Clear( &bp->moveArray ); + b2ClearSet( &bp->moveSet ); + + b2FreeStackItem( alloc, bp->movePairs ); + bp->movePairs = NULL; + b2FreeStackItem( alloc, bp->moveResults ); + bp->moveResults = NULL; + + b2ValidateSolverSets( world ); + + b2TracyCZoneEnd( create_contacts ); + + b2TracyCZoneEnd( update_pairs ); +} + +bool b2BroadPhase_TestOverlap( const b2BroadPhase* bp, int proxyKeyA, int proxyKeyB ) +{ + int typeIndexA = B2_PROXY_TYPE( proxyKeyA ); + int proxyIdA = B2_PROXY_ID( proxyKeyA ); + int typeIndexB = B2_PROXY_TYPE( proxyKeyB ); + int proxyIdB = B2_PROXY_ID( proxyKeyB ); + + b2AABB aabbA = b2DynamicTree_GetAABB( bp->trees + typeIndexA, proxyIdA ); + b2AABB aabbB = b2DynamicTree_GetAABB( bp->trees + typeIndexB, proxyIdB ); + return b2AABB_Overlaps( aabbA, aabbB ); +} + +void b2BroadPhase_RebuildTrees( b2BroadPhase* bp ) +{ + b2DynamicTree_Rebuild( bp->trees + b2_dynamicBody, false ); + b2DynamicTree_Rebuild( bp->trees + b2_kinematicBody, false ); +} + +int b2BroadPhase_GetShapeIndex( b2BroadPhase* bp, int proxyKey ) +{ + int typeIndex = B2_PROXY_TYPE( proxyKey ); + int proxyId = B2_PROXY_ID( proxyKey ); + + return b2DynamicTree_GetUserData( bp->trees + typeIndex, proxyId ); +} + +void b2ValidateBroadphase( const b2BroadPhase* bp ) +{ + b2DynamicTree_Validate( bp->trees + b2_dynamicBody ); + b2DynamicTree_Validate( bp->trees + b2_kinematicBody ); + + // TODO_ERIN validate every shape AABB is contained in tree AABB +} + +void b2ValidateNoEnlarged( const b2BroadPhase* bp ) +{ +#if B2_VALIDATE == 1 + for ( int j = 0; j < b2_bodyTypeCount; ++j ) + { + const b2DynamicTree* tree = bp->trees + j; + int capacity = tree->nodeCapacity; + const b2TreeNode* nodes = tree->nodes; + for ( int i = 0; i < capacity; ++i ) + { + const b2TreeNode* node = nodes + i; + if ( node->height < 0 ) + { + continue; + } + + if ( node->enlarged == true ) + { + capacity += 0; + } + + B2_ASSERT( node->enlarged == false ); + } + } +#else + B2_MAYBE_UNUSED( bp ); +#endif +} diff --git a/3rdparty/box2d/src/broad_phase.h b/3rdparty/box2d/src/broad_phase.h new file mode 100644 index 000000000000..ad53ef12d76b --- /dev/null +++ b/3rdparty/box2d/src/broad_phase.h @@ -0,0 +1,83 @@ +// SPDX-FileCopyrightText: 2023 Erin Catto +// SPDX-License-Identifier: MIT + +#pragma once + +#include "array.h" +#include "table.h" + +#include "box2d/collision.h" +#include "box2d/types.h" + +typedef struct b2Shape b2Shape; +typedef struct b2MovePair b2MovePair; +typedef struct b2MoveResult b2MoveResult; +typedef struct b2StackAllocator b2StackAllocator; +typedef struct b2World b2World; + +// Store the proxy type in the lower 2 bits of the proxy key. This leaves 30 bits for the id. +#define B2_PROXY_TYPE( KEY ) ( (b2BodyType)( ( KEY ) & 3 ) ) +#define B2_PROXY_ID( KEY ) ( ( KEY ) >> 2 ) +#define B2_PROXY_KEY( ID, TYPE ) ( ( ( ID ) << 2 ) | ( TYPE ) ) + +/// The broad-phase is used for computing pairs and performing volume queries and ray casts. +/// This broad-phase does not persist pairs. Instead, this reports potentially new pairs. +/// It is up to the client to consume the new pairs and to track subsequent overlap. +typedef struct b2BroadPhase +{ + b2DynamicTree trees[b2_bodyTypeCount]; + int proxyCount; + + // The move set and array are used to track shapes that have moved significantly + // and need a pair query for new contacts. The array has a deterministic order. + // todo perhaps just a move set? + // todo implement a 32bit hash set for faster lookup + // todo moveSet can grow quite large on the first time step and remain large + b2HashSet moveSet; + b2IntArray moveArray; + + // These are the results from the pair query and are used to create new contacts + // in deterministic order. + // todo these could be in the step context + b2MoveResult* moveResults; + b2MovePair* movePairs; + int movePairCapacity; + _Atomic int movePairIndex; + + // Tracks shape pairs that have a b2Contact + // todo pairSet can grow quite large on the first time step and remain large + b2HashSet pairSet; + +} b2BroadPhase; + +void b2CreateBroadPhase( b2BroadPhase* bp ); +void b2DestroyBroadPhase( b2BroadPhase* bp ); + +int b2BroadPhase_CreateProxy( b2BroadPhase* bp, b2BodyType proxyType, b2AABB aabb, uint64_t categoryBits, int shapeIndex, + bool forcePairCreation ); +void b2BroadPhase_DestroyProxy( b2BroadPhase* bp, int proxyKey ); + +void b2BroadPhase_MoveProxy( b2BroadPhase* bp, int proxyKey, b2AABB aabb ); +void b2BroadPhase_EnlargeProxy( b2BroadPhase* bp, int proxyKey, b2AABB aabb ); + +void b2BroadPhase_RebuildTrees( b2BroadPhase* bp ); + +int b2BroadPhase_GetShapeIndex( b2BroadPhase* bp, int proxyKey ); + +void b2UpdateBroadPhasePairs( b2World* world ); +bool b2BroadPhase_TestOverlap( const b2BroadPhase* bp, int proxyKeyA, int proxyKeyB ); + +void b2ValidateBroadphase( const b2BroadPhase* bp ); +void b2ValidateNoEnlarged( const b2BroadPhase* bp ); + +// This is what triggers new contact pairs to be created +// Warning: this must be called in deterministic order +static inline void b2BufferMove( b2BroadPhase* bp, int queryProxy ) +{ + // Adding 1 because 0 is the sentinel + bool alreadyAdded = b2AddKey( &bp->moveSet, queryProxy + 1 ); + if ( alreadyAdded == false ) + { + b2IntArray_Push( &bp->moveArray, queryProxy ); + } +} diff --git a/3rdparty/box2d/src/collision/b2_broad_phase.cpp b/3rdparty/box2d/src/collision/b2_broad_phase.cpp deleted file mode 100644 index d063a3aa9bf4..000000000000 --- a/3rdparty/box2d/src/collision/b2_broad_phase.cpp +++ /dev/null @@ -1,131 +0,0 @@ -// MIT License - -// Copyright (c) 2019 Erin Catto - -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: - -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. - -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -#include "box2d/b2_broad_phase.h" -#include - -b2BroadPhase::b2BroadPhase() -{ - m_proxyCount = 0; - - m_pairCapacity = 16; - m_pairCount = 0; - m_pairBuffer = (b2Pair*)b2Alloc(m_pairCapacity * sizeof(b2Pair)); - - m_moveCapacity = 16; - m_moveCount = 0; - m_moveBuffer = (int32*)b2Alloc(m_moveCapacity * sizeof(int32)); -} - -b2BroadPhase::~b2BroadPhase() -{ - b2Free(m_moveBuffer); - b2Free(m_pairBuffer); -} - -int32 b2BroadPhase::CreateProxy(const b2AABB& aabb, void* userData) -{ - int32 proxyId = m_tree.CreateProxy(aabb, userData); - ++m_proxyCount; - BufferMove(proxyId); - return proxyId; -} - -void b2BroadPhase::DestroyProxy(int32 proxyId) -{ - UnBufferMove(proxyId); - --m_proxyCount; - m_tree.DestroyProxy(proxyId); -} - -void b2BroadPhase::MoveProxy(int32 proxyId, const b2AABB& aabb, const b2Vec2& displacement) -{ - bool buffer = m_tree.MoveProxy(proxyId, aabb, displacement); - if (buffer) - { - BufferMove(proxyId); - } -} - -void b2BroadPhase::TouchProxy(int32 proxyId) -{ - BufferMove(proxyId); -} - -void b2BroadPhase::BufferMove(int32 proxyId) -{ - if (m_moveCount == m_moveCapacity) - { - int32* oldBuffer = m_moveBuffer; - m_moveCapacity *= 2; - m_moveBuffer = (int32*)b2Alloc(m_moveCapacity * sizeof(int32)); - memcpy(m_moveBuffer, oldBuffer, m_moveCount * sizeof(int32)); - b2Free(oldBuffer); - } - - m_moveBuffer[m_moveCount] = proxyId; - ++m_moveCount; -} - -void b2BroadPhase::UnBufferMove(int32 proxyId) -{ - for (int32 i = 0; i < m_moveCount; ++i) - { - if (m_moveBuffer[i] == proxyId) - { - m_moveBuffer[i] = e_nullProxy; - } - } -} - -// This is called from b2DynamicTree::Query when we are gathering pairs. -bool b2BroadPhase::QueryCallback(int32 proxyId) -{ - // A proxy cannot form a pair with itself. - if (proxyId == m_queryProxyId) - { - return true; - } - - const bool moved = m_tree.WasMoved(proxyId); - if (moved && proxyId > m_queryProxyId) - { - // Both proxies are moving. Avoid duplicate pairs. - return true; - } - - // Grow the pair buffer as needed. - if (m_pairCount == m_pairCapacity) - { - b2Pair* oldBuffer = m_pairBuffer; - m_pairCapacity = m_pairCapacity + (m_pairCapacity >> 1); - m_pairBuffer = (b2Pair*)b2Alloc(m_pairCapacity * sizeof(b2Pair)); - memcpy(m_pairBuffer, oldBuffer, m_pairCount * sizeof(b2Pair)); - b2Free(oldBuffer); - } - - m_pairBuffer[m_pairCount].proxyIdA = b2Min(proxyId, m_queryProxyId); - m_pairBuffer[m_pairCount].proxyIdB = b2Max(proxyId, m_queryProxyId); - ++m_pairCount; - - return true; -} diff --git a/3rdparty/box2d/src/collision/b2_chain_shape.cpp b/3rdparty/box2d/src/collision/b2_chain_shape.cpp deleted file mode 100644 index b964a4303c04..000000000000 --- a/3rdparty/box2d/src/collision/b2_chain_shape.cpp +++ /dev/null @@ -1,185 +0,0 @@ -// MIT License - -// Copyright (c) 2019 Erin Catto - -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: - -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. - -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -#include "box2d/b2_chain_shape.h" -#include "box2d/b2_edge_shape.h" - -#include "box2d/b2_block_allocator.h" - -#include -#include - -b2ChainShape::~b2ChainShape() -{ - Clear(); -} - -void b2ChainShape::Clear() -{ - b2Free(m_vertices); - m_vertices = nullptr; - m_count = 0; -} - -void b2ChainShape::CreateLoop(const b2Vec2* vertices, int32 count) -{ - b2Assert(m_vertices == nullptr && m_count == 0); - b2Assert(count >= 3); - if (count < 3) - { - return; - } - - for (int32 i = 1; i < count; ++i) - { - b2Vec2 v1 = vertices[i-1]; - b2Vec2 v2 = vertices[i]; - // If the code crashes here, it means your vertices are too close together. - b2Assert(b2DistanceSquared(v1, v2) > b2_linearSlop * b2_linearSlop); - } - - m_count = count + 1; - m_vertices = (b2Vec2*)b2Alloc(m_count * sizeof(b2Vec2)); - memcpy(m_vertices, vertices, count * sizeof(b2Vec2)); - m_vertices[count] = m_vertices[0]; - m_prevVertex = m_vertices[m_count - 2]; - m_nextVertex = m_vertices[1]; -} - -void b2ChainShape::CreateChain(const b2Vec2* vertices, int32 count, const b2Vec2& prevVertex, const b2Vec2& nextVertex) -{ - b2Assert(m_vertices == nullptr && m_count == 0); - b2Assert(count >= 2); - for (int32 i = 1; i < count; ++i) - { - // If the code crashes here, it means your vertices are too close together. - b2Assert(b2DistanceSquared(vertices[i-1], vertices[i]) > b2_linearSlop * b2_linearSlop); - } - - m_count = count; - m_vertices = (b2Vec2*)b2Alloc(count * sizeof(b2Vec2)); - memcpy(m_vertices, vertices, m_count * sizeof(b2Vec2)); - - m_prevVertex = prevVertex; - m_nextVertex = nextVertex; -} - -b2Shape* b2ChainShape::Clone(b2BlockAllocator* allocator) const -{ - void* mem = allocator->Allocate(sizeof(b2ChainShape)); - b2ChainShape* clone = new (mem) b2ChainShape; - clone->CreateChain(m_vertices, m_count, m_prevVertex, m_nextVertex); - return clone; -} - -int32 b2ChainShape::GetChildCount() const -{ - // edge count = vertex count - 1 - return m_count - 1; -} - -void b2ChainShape::GetChildEdge(b2EdgeShape* edge, int32 index) const -{ - b2Assert(0 <= index && index < m_count - 1); - edge->m_type = b2Shape::e_edge; - edge->m_radius = m_radius; - - edge->m_vertex1 = m_vertices[index + 0]; - edge->m_vertex2 = m_vertices[index + 1]; - edge->m_oneSided = true; - - if (index > 0) - { - edge->m_vertex0 = m_vertices[index - 1]; - } - else - { - edge->m_vertex0 = m_prevVertex; - } - - if (index < m_count - 2) - { - edge->m_vertex3 = m_vertices[index + 2]; - } - else - { - edge->m_vertex3 = m_nextVertex; - } -} - -bool b2ChainShape::TestPoint(const b2Transform& xf, const b2Vec2& p) const -{ - B2_NOT_USED(xf); - B2_NOT_USED(p); - return false; -} - -bool b2ChainShape::RayCast(b2RayCastOutput* output, const b2RayCastInput& input, - const b2Transform& xf, int32 childIndex) const -{ - b2Assert(childIndex < m_count); - - b2EdgeShape edgeShape; - - int32 i1 = childIndex; - int32 i2 = childIndex + 1; - if (i2 == m_count) - { - i2 = 0; - } - - edgeShape.m_vertex1 = m_vertices[i1]; - edgeShape.m_vertex2 = m_vertices[i2]; - - return edgeShape.RayCast(output, input, xf, 0); -} - -void b2ChainShape::ComputeAABB(b2AABB* aabb, const b2Transform& xf, int32 childIndex) const -{ - b2Assert(childIndex < m_count); - - int32 i1 = childIndex; - int32 i2 = childIndex + 1; - if (i2 == m_count) - { - i2 = 0; - } - - b2Vec2 v1 = b2Mul(xf, m_vertices[i1]); - b2Vec2 v2 = b2Mul(xf, m_vertices[i2]); - - b2Vec2 lower = b2Min(v1, v2); - b2Vec2 upper = b2Max(v1, v2); - - b2Vec2 r(m_radius, m_radius); - aabb->lowerBound = lower - r; - aabb->upperBound = upper + r; -} - -void b2ChainShape::ComputeMass(b2MassData* massData, float density) const -{ - B2_NOT_USED(density); - - massData->mass = 0.0f; - massData->center.SetZero(); - massData->I = 0.0f; -} diff --git a/3rdparty/box2d/src/collision/b2_circle_shape.cpp b/3rdparty/box2d/src/collision/b2_circle_shape.cpp deleted file mode 100644 index ecc69293b584..000000000000 --- a/3rdparty/box2d/src/collision/b2_circle_shape.cpp +++ /dev/null @@ -1,105 +0,0 @@ -// MIT License - -// Copyright (c) 2019 Erin Catto - -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: - -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. - -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -#include "box2d/b2_circle_shape.h" -#include "box2d/b2_block_allocator.h" - -#include - -b2Shape* b2CircleShape::Clone(b2BlockAllocator* allocator) const -{ - void* mem = allocator->Allocate(sizeof(b2CircleShape)); - b2CircleShape* clone = new (mem) b2CircleShape; - *clone = *this; - return clone; -} - -int32 b2CircleShape::GetChildCount() const -{ - return 1; -} - -bool b2CircleShape::TestPoint(const b2Transform& transform, const b2Vec2& p) const -{ - b2Vec2 center = transform.p + b2Mul(transform.q, m_p); - b2Vec2 d = p - center; - return b2Dot(d, d) <= m_radius * m_radius; -} - -// Collision Detection in Interactive 3D Environments by Gino van den Bergen -// From Section 3.1.2 -// x = s + a * r -// norm(x) = radius -bool b2CircleShape::RayCast(b2RayCastOutput* output, const b2RayCastInput& input, - const b2Transform& transform, int32 childIndex) const -{ - B2_NOT_USED(childIndex); - - b2Vec2 position = transform.p + b2Mul(transform.q, m_p); - b2Vec2 s = input.p1 - position; - float b = b2Dot(s, s) - m_radius * m_radius; - - // Solve quadratic equation. - b2Vec2 r = input.p2 - input.p1; - float c = b2Dot(s, r); - float rr = b2Dot(r, r); - float sigma = c * c - rr * b; - - // Check for negative discriminant and short segment. - if (sigma < 0.0f || rr < b2_epsilon) - { - return false; - } - - // Find the point of intersection of the line with the circle. - float a = -(c + b2Sqrt(sigma)); - - // Is the intersection point on the segment? - if (0.0f <= a && a <= input.maxFraction * rr) - { - a /= rr; - output->fraction = a; - output->normal = s + a * r; - output->normal.Normalize(); - return true; - } - - return false; -} - -void b2CircleShape::ComputeAABB(b2AABB* aabb, const b2Transform& transform, int32 childIndex) const -{ - B2_NOT_USED(childIndex); - - b2Vec2 p = transform.p + b2Mul(transform.q, m_p); - aabb->lowerBound.Set(p.x - m_radius, p.y - m_radius); - aabb->upperBound.Set(p.x + m_radius, p.y + m_radius); -} - -void b2CircleShape::ComputeMass(b2MassData* massData, float density) const -{ - massData->mass = density * b2_pi * m_radius * m_radius; - massData->center = m_p; - - // inertia about the local origin - massData->I = massData->mass * (0.5f * m_radius * m_radius + b2Dot(m_p, m_p)); -} diff --git a/3rdparty/box2d/src/collision/b2_collide_circle.cpp b/3rdparty/box2d/src/collision/b2_collide_circle.cpp deleted file mode 100644 index 469da55907e9..000000000000 --- a/3rdparty/box2d/src/collision/b2_collide_circle.cpp +++ /dev/null @@ -1,158 +0,0 @@ -// MIT License - -// Copyright (c) 2019 Erin Catto - -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: - -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. - -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -#include "box2d/b2_collision.h" -#include "box2d/b2_circle_shape.h" -#include "box2d/b2_polygon_shape.h" - -void b2CollideCircles( - b2Manifold* manifold, - const b2CircleShape* circleA, const b2Transform& xfA, - const b2CircleShape* circleB, const b2Transform& xfB) -{ - manifold->pointCount = 0; - - b2Vec2 pA = b2Mul(xfA, circleA->m_p); - b2Vec2 pB = b2Mul(xfB, circleB->m_p); - - b2Vec2 d = pB - pA; - float distSqr = b2Dot(d, d); - float rA = circleA->m_radius, rB = circleB->m_radius; - float radius = rA + rB; - if (distSqr > radius * radius) - { - return; - } - - manifold->type = b2Manifold::e_circles; - manifold->localPoint = circleA->m_p; - manifold->localNormal.SetZero(); - manifold->pointCount = 1; - - manifold->points[0].localPoint = circleB->m_p; - manifold->points[0].id.key = 0; -} - -void b2CollidePolygonAndCircle( - b2Manifold* manifold, - const b2PolygonShape* polygonA, const b2Transform& xfA, - const b2CircleShape* circleB, const b2Transform& xfB) -{ - manifold->pointCount = 0; - - // Compute circle position in the frame of the polygon. - b2Vec2 c = b2Mul(xfB, circleB->m_p); - b2Vec2 cLocal = b2MulT(xfA, c); - - // Find the min separating edge. - int32 normalIndex = 0; - float separation = -b2_maxFloat; - float radius = polygonA->m_radius + circleB->m_radius; - int32 vertexCount = polygonA->m_count; - const b2Vec2* vertices = polygonA->m_vertices; - const b2Vec2* normals = polygonA->m_normals; - - for (int32 i = 0; i < vertexCount; ++i) - { - float s = b2Dot(normals[i], cLocal - vertices[i]); - - if (s > radius) - { - // Early out. - return; - } - - if (s > separation) - { - separation = s; - normalIndex = i; - } - } - - // Vertices that subtend the incident face. - int32 vertIndex1 = normalIndex; - int32 vertIndex2 = vertIndex1 + 1 < vertexCount ? vertIndex1 + 1 : 0; - b2Vec2 v1 = vertices[vertIndex1]; - b2Vec2 v2 = vertices[vertIndex2]; - - // If the center is inside the polygon ... - if (separation < b2_epsilon) - { - manifold->pointCount = 1; - manifold->type = b2Manifold::e_faceA; - manifold->localNormal = normals[normalIndex]; - manifold->localPoint = 0.5f * (v1 + v2); - manifold->points[0].localPoint = circleB->m_p; - manifold->points[0].id.key = 0; - return; - } - - // Compute barycentric coordinates - float u1 = b2Dot(cLocal - v1, v2 - v1); - float u2 = b2Dot(cLocal - v2, v1 - v2); - if (u1 <= 0.0f) - { - if (b2DistanceSquared(cLocal, v1) > radius * radius) - { - return; - } - - manifold->pointCount = 1; - manifold->type = b2Manifold::e_faceA; - manifold->localNormal = cLocal - v1; - manifold->localNormal.Normalize(); - manifold->localPoint = v1; - manifold->points[0].localPoint = circleB->m_p; - manifold->points[0].id.key = 0; - } - else if (u2 <= 0.0f) - { - if (b2DistanceSquared(cLocal, v2) > radius * radius) - { - return; - } - - manifold->pointCount = 1; - manifold->type = b2Manifold::e_faceA; - manifold->localNormal = cLocal - v2; - manifold->localNormal.Normalize(); - manifold->localPoint = v2; - manifold->points[0].localPoint = circleB->m_p; - manifold->points[0].id.key = 0; - } - else - { - b2Vec2 faceCenter = 0.5f * (v1 + v2); - float s = b2Dot(cLocal - faceCenter, normals[vertIndex1]); - if (s > radius) - { - return; - } - - manifold->pointCount = 1; - manifold->type = b2Manifold::e_faceA; - manifold->localNormal = normals[vertIndex1]; - manifold->localPoint = faceCenter; - manifold->points[0].localPoint = circleB->m_p; - manifold->points[0].id.key = 0; - } -} diff --git a/3rdparty/box2d/src/collision/b2_collide_edge.cpp b/3rdparty/box2d/src/collision/b2_collide_edge.cpp deleted file mode 100644 index e06b90097fad..000000000000 --- a/3rdparty/box2d/src/collision/b2_collide_edge.cpp +++ /dev/null @@ -1,524 +0,0 @@ -// MIT License - -// Copyright (c) 2019 Erin Catto - -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: - -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. - -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -#include "box2d/b2_collision.h" -#include "box2d/b2_circle_shape.h" -#include "box2d/b2_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/3rdparty/box2d/src/collision/b2_collide_polygon.cpp b/3rdparty/box2d/src/collision/b2_collide_polygon.cpp deleted file mode 100644 index f3fa850bcb45..000000000000 --- a/3rdparty/box2d/src/collision/b2_collide_polygon.cpp +++ /dev/null @@ -1,243 +0,0 @@ -// MIT License - -// Copyright (c) 2019 Erin Catto - -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: - -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. - -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -#include "box2d/b2_collision.h" -#include "box2d/b2_polygon_shape.h" - -// Find the max separation between poly1 and poly2 using edge normals from poly1. -static float b2FindMaxSeparation(int32* edgeIndex, - const b2PolygonShape* poly1, const b2Transform& xf1, - const b2PolygonShape* poly2, const b2Transform& xf2) -{ - int32 count1 = poly1->m_count; - int32 count2 = poly2->m_count; - const b2Vec2* n1s = poly1->m_normals; - const b2Vec2* v1s = poly1->m_vertices; - const b2Vec2* v2s = poly2->m_vertices; - b2Transform xf = b2MulT(xf2, xf1); - - int32 bestIndex = 0; - float maxSeparation = -b2_maxFloat; - for (int32 i = 0; i < count1; ++i) - { - // Get poly1 normal in frame2. - b2Vec2 n = b2Mul(xf.q, n1s[i]); - b2Vec2 v1 = b2Mul(xf, v1s[i]); - - // Find deepest point for normal i. - float si = b2_maxFloat; - for (int32 j = 0; j < count2; ++j) - { - float sij = b2Dot(n, v2s[j] - v1); - if (sij < si) - { - si = sij; - } - } - - if (si > maxSeparation) - { - maxSeparation = si; - bestIndex = i; - } - } - - *edgeIndex = bestIndex; - return maxSeparation; -} - -static void b2FindIncidentEdge(b2ClipVertex c[2], - const b2PolygonShape* poly1, const b2Transform& xf1, int32 edge1, - const b2PolygonShape* poly2, const b2Transform& xf2) -{ - const b2Vec2* normals1 = poly1->m_normals; - - int32 count2 = poly2->m_count; - const b2Vec2* vertices2 = poly2->m_vertices; - const b2Vec2* normals2 = poly2->m_normals; - - b2Assert(0 <= edge1 && edge1 < poly1->m_count); - - // Get the normal of the reference edge in poly2's frame. - b2Vec2 normal1 = b2MulT(xf2.q, b2Mul(xf1.q, normals1[edge1])); - - // Find the incident edge on poly2. - int32 index = 0; - float minDot = b2_maxFloat; - for (int32 i = 0; i < count2; ++i) - { - float dot = b2Dot(normal1, normals2[i]); - if (dot < minDot) - { - minDot = dot; - index = i; - } - } - - // Build the clip vertices for the incident edge. - int32 i1 = index; - int32 i2 = i1 + 1 < count2 ? i1 + 1 : 0; - - c[0].v = b2Mul(xf2, vertices2[i1]); - c[0].id.cf.indexA = (uint8)edge1; - c[0].id.cf.indexB = (uint8)i1; - c[0].id.cf.typeA = b2ContactFeature::e_face; - c[0].id.cf.typeB = b2ContactFeature::e_vertex; - - c[1].v = b2Mul(xf2, vertices2[i2]); - c[1].id.cf.indexA = (uint8)edge1; - c[1].id.cf.indexB = (uint8)i2; - c[1].id.cf.typeA = b2ContactFeature::e_face; - c[1].id.cf.typeB = b2ContactFeature::e_vertex; -} - -// Find edge normal of max separation on A - return if separating axis is found -// Find edge normal of max separation on B - return if separation axis is found -// Choose reference edge as min(minA, minB) -// Find incident edge -// Clip - -// The normal points from 1 to 2 -void b2CollidePolygons(b2Manifold* manifold, - const b2PolygonShape* polyA, const b2Transform& xfA, - const b2PolygonShape* polyB, const b2Transform& xfB) -{ - manifold->pointCount = 0; - float totalRadius = polyA->m_radius + polyB->m_radius; - - int32 edgeA = 0; - float separationA = b2FindMaxSeparation(&edgeA, polyA, xfA, polyB, xfB); - if (separationA > totalRadius) - return; - - int32 edgeB = 0; - float separationB = b2FindMaxSeparation(&edgeB, polyB, xfB, polyA, xfA); - if (separationB > totalRadius) - return; - - const b2PolygonShape* poly1; // reference polygon - const b2PolygonShape* poly2; // incident polygon - b2Transform xf1, xf2; - int32 edge1; // reference edge - uint8 flip; - const float k_tol = 0.1f * b2_linearSlop; - - if (separationB > separationA + k_tol) - { - poly1 = polyB; - poly2 = polyA; - xf1 = xfB; - xf2 = xfA; - edge1 = edgeB; - manifold->type = b2Manifold::e_faceB; - flip = 1; - } - else - { - poly1 = polyA; - poly2 = polyB; - xf1 = xfA; - xf2 = xfB; - edge1 = edgeA; - manifold->type = b2Manifold::e_faceA; - flip = 0; - } - - b2ClipVertex incidentEdge[2]; - b2FindIncidentEdge(incidentEdge, poly1, xf1, edge1, poly2, xf2); - - int32 count1 = poly1->m_count; - const b2Vec2* vertices1 = poly1->m_vertices; - - int32 iv1 = edge1; - int32 iv2 = edge1 + 1 < count1 ? edge1 + 1 : 0; - - b2Vec2 v11 = vertices1[iv1]; - b2Vec2 v12 = vertices1[iv2]; - - b2Vec2 localTangent = v12 - v11; - localTangent.Normalize(); - - b2Vec2 localNormal = b2Cross(localTangent, 1.0f); - b2Vec2 planePoint = 0.5f * (v11 + v12); - - b2Vec2 tangent = b2Mul(xf1.q, localTangent); - b2Vec2 normal = b2Cross(tangent, 1.0f); - - v11 = b2Mul(xf1, v11); - v12 = b2Mul(xf1, v12); - - // Face offset. - float frontOffset = b2Dot(normal, v11); - - // Side offsets, extended by polytope skin thickness. - float sideOffset1 = -b2Dot(tangent, v11) + totalRadius; - float sideOffset2 = b2Dot(tangent, v12) + totalRadius; - - // Clip incident edge against extruded edge1 side edges. - b2ClipVertex clipPoints1[2]; - b2ClipVertex clipPoints2[2]; - int np; - - // Clip to box side 1 - np = b2ClipSegmentToLine(clipPoints1, incidentEdge, -tangent, sideOffset1, iv1); - - if (np < 2) - return; - - // Clip to negative box side 1 - np = b2ClipSegmentToLine(clipPoints2, clipPoints1, tangent, sideOffset2, iv2); - - if (np < 2) - { - return; - } - - // Now clipPoints2 contains the clipped points. - manifold->localNormal = localNormal; - manifold->localPoint = planePoint; - - int32 pointCount = 0; - for (int32 i = 0; i < b2_maxManifoldPoints; ++i) - { - float separation = b2Dot(normal, clipPoints2[i].v) - frontOffset; - - if (separation <= totalRadius) - { - b2ManifoldPoint* cp = manifold->points + pointCount; - cp->localPoint = b2MulT(xf2, clipPoints2[i].v); - cp->id = clipPoints2[i].id; - if (flip) - { - // Swap features - b2ContactFeature cf = cp->id.cf; - cp->id.cf.indexA = cf.indexB; - cp->id.cf.indexB = cf.indexA; - cp->id.cf.typeA = cf.typeB; - cp->id.cf.typeB = cf.typeA; - } - ++pointCount; - } - } - - manifold->pointCount = pointCount; -} diff --git a/3rdparty/box2d/src/collision/b2_collision.cpp b/3rdparty/box2d/src/collision/b2_collision.cpp deleted file mode 100644 index 750bff00fe7d..000000000000 --- a/3rdparty/box2d/src/collision/b2_collision.cpp +++ /dev/null @@ -1,580 +0,0 @@ -// MIT License - -// Copyright (c) 2019 Erin Catto - -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: - -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. - -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -#include "box2d/b2_collision.h" -#include "box2d/b2_distance.h" - -void b2WorldManifold::Initialize(const b2Manifold* manifold, - const b2Transform& xfA, float radiusA, - const b2Transform& xfB, float radiusB) -{ - if (manifold->pointCount == 0) - { - return; - } - - switch (manifold->type) - { - case b2Manifold::e_circles: - { - normal.Set(1.0f, 0.0f); - b2Vec2 pointA = b2Mul(xfA, manifold->localPoint); - b2Vec2 pointB = b2Mul(xfB, manifold->points[0].localPoint); - if (b2DistanceSquared(pointA, pointB) > b2_epsilon * b2_epsilon) - { - normal = pointB - pointA; - normal.Normalize(); - } - - b2Vec2 cA = pointA + radiusA * normal; - b2Vec2 cB = pointB - radiusB * normal; - points[0] = 0.5f * (cA + cB); - separations[0] = b2Dot(cB - cA, normal); - } - break; - - case b2Manifold::e_faceA: - { - normal = b2Mul(xfA.q, manifold->localNormal); - b2Vec2 planePoint = b2Mul(xfA, manifold->localPoint); - - for (int32 i = 0; i < manifold->pointCount; ++i) - { - b2Vec2 clipPoint = b2Mul(xfB, manifold->points[i].localPoint); - b2Vec2 cA = clipPoint + (radiusA - b2Dot(clipPoint - planePoint, normal)) * normal; - b2Vec2 cB = clipPoint - radiusB * normal; - points[i] = 0.5f * (cA + cB); - separations[i] = b2Dot(cB - cA, normal); - } - } - break; - - case b2Manifold::e_faceB: - { - normal = b2Mul(xfB.q, manifold->localNormal); - b2Vec2 planePoint = b2Mul(xfB, manifold->localPoint); - - for (int32 i = 0; i < manifold->pointCount; ++i) - { - b2Vec2 clipPoint = b2Mul(xfA, manifold->points[i].localPoint); - b2Vec2 cB = clipPoint + (radiusB - b2Dot(clipPoint - planePoint, normal)) * normal; - b2Vec2 cA = clipPoint - radiusA * normal; - points[i] = 0.5f * (cA + cB); - separations[i] = b2Dot(cA - cB, normal); - } - - // Ensure normal points from A to B. - normal = -normal; - } - break; - } -} - -void b2GetPointStates(b2PointState state1[b2_maxManifoldPoints], b2PointState state2[b2_maxManifoldPoints], - const b2Manifold* manifold1, const b2Manifold* manifold2) -{ - for (int32 i = 0; i < b2_maxManifoldPoints; ++i) - { - state1[i] = b2_nullState; - state2[i] = b2_nullState; - } - - // Detect persists and removes. - for (int32 i = 0; i < manifold1->pointCount; ++i) - { - b2ContactID id = manifold1->points[i].id; - - state1[i] = b2_removeState; - - for (int32 j = 0; j < manifold2->pointCount; ++j) - { - if (manifold2->points[j].id.key == id.key) - { - state1[i] = b2_persistState; - break; - } - } - } - - // Detect persists and adds. - for (int32 i = 0; i < manifold2->pointCount; ++i) - { - b2ContactID id = manifold2->points[i].id; - - state2[i] = b2_addState; - - for (int32 j = 0; j < manifold1->pointCount; ++j) - { - if (manifold1->points[j].id.key == id.key) - { - state2[i] = b2_persistState; - break; - } - } - } -} - -// From Real-time Collision Detection, p179. -bool b2AABB::RayCast(b2RayCastOutput* output, const b2RayCastInput& input) const -{ - float tmin = -b2_maxFloat; - float tmax = b2_maxFloat; - - b2Vec2 p = input.p1; - b2Vec2 d = input.p2 - input.p1; - b2Vec2 absD = b2Abs(d); - - b2Vec2 normal; - - for (int32 i = 0; i < 2; ++i) - { - if (absD(i) < b2_epsilon) - { - // Parallel. - if (p(i) < lowerBound(i) || upperBound(i) < p(i)) - { - return false; - } - } - else - { - float inv_d = 1.0f / d(i); - float t1 = (lowerBound(i) - p(i)) * inv_d; - float t2 = (upperBound(i) - p(i)) * inv_d; - - // Sign of the normal vector. - float s = -1.0f; - - if (t1 > t2) - { - b2Swap(t1, t2); - s = 1.0f; - } - - // Push the min up - if (t1 > tmin) - { - normal.SetZero(); - normal(i) = s; - tmin = t1; - } - - // Pull the max down - tmax = b2Min(tmax, t2); - - if (tmin > tmax) - { - return false; - } - } - } - - // Does the ray start inside the box? - // Does the ray intersect beyond the max fraction? - if (tmin < 0.0f || input.maxFraction < tmin) - { - return false; - } - - // Intersection. - output->fraction = tmin; - output->normal = normal; - return true; -} - -// Sutherland-Hodgman clipping. -int32 b2ClipSegmentToLine(b2ClipVertex vOut[2], const b2ClipVertex vIn[2], - const b2Vec2& normal, float offset, int32 vertexIndexA) -{ - // Start with no output points - int32 count = 0; - - // Calculate the distance of end points to the line - float distance0 = b2Dot(normal, vIn[0].v) - offset; - float distance1 = b2Dot(normal, vIn[1].v) - offset; - - // If the points are behind the plane - if (distance0 <= 0.0f) vOut[count++] = vIn[0]; - if (distance1 <= 0.0f) vOut[count++] = vIn[1]; - - // If the points are on different sides of the plane - if (distance0 * distance1 < 0.0f) - { - // Find intersection point of edge and plane - float interp = distance0 / (distance0 - distance1); - vOut[count].v = vIn[0].v + interp * (vIn[1].v - vIn[0].v); - - // VertexA is hitting edgeB. - vOut[count].id.cf.indexA = static_cast(vertexIndexA); - vOut[count].id.cf.indexB = vIn[0].id.cf.indexB; - vOut[count].id.cf.typeA = b2ContactFeature::e_vertex; - vOut[count].id.cf.typeB = b2ContactFeature::e_face; - ++count; - - b2Assert(count == 2); - } - - return count; -} - -bool b2TestOverlap( const b2Shape* shapeA, int32 indexA, - const b2Shape* shapeB, int32 indexB, - const b2Transform& xfA, const b2Transform& xfB) -{ - b2DistanceInput input; - input.proxyA.Set(shapeA, indexA); - input.proxyB.Set(shapeB, indexB); - input.transformA = xfA; - input.transformB = xfB; - input.useRadii = true; - - b2SimplexCache cache; - cache.count = 0; - - b2DistanceOutput output; - - b2Distance(&output, &cache, &input); - - return output.distance < 10.0f * b2_epsilon; -} - -// quickhull recursion -static b2Hull b2RecurseHull(b2Vec2 p1, b2Vec2 p2, b2Vec2* ps, int32 count) -{ - b2Hull hull; - hull.count = 0; - - if (count == 0) - { - return hull; - } - - // create an edge vector pointing from p1 to p2 - b2Vec2 e = p2 - p1; - e.Normalize(); - - // discard points left of e and find point furthest to the right of e - b2Vec2 rightPoints[b2_maxPolygonVertices]{}; - int32 rightCount = 0; - - int32 bestIndex = 0; - float bestDistance = b2Cross(ps[bestIndex] - p1, e); - if (bestDistance > 0.0f) - { - rightPoints[rightCount++] = ps[bestIndex]; - } - - for (int32 i = 1; i < count; ++i) - { - float distance = b2Cross(ps[i] - p1, e); - if (distance > bestDistance) - { - bestIndex = i; - bestDistance = distance; - } - - if (distance > 0.0f) - { - rightPoints[rightCount++] = ps[i]; - } - } - - if (bestDistance < 2.0f * b2_linearSlop) - { - return hull; - } - - b2Vec2 bestPoint = ps[bestIndex]; - - // compute hull to the right of p1-bestPoint - b2Hull hull1 = b2RecurseHull(p1, bestPoint, rightPoints, rightCount); - - // compute hull to the right of bestPoint-p2 - b2Hull hull2 = b2RecurseHull(bestPoint, p2, rightPoints, rightCount); - - // stich together hulls - for (int32 i = 0; i < hull1.count; ++i) - { - hull.points[hull.count++] = hull1.points[i]; - } - - hull.points[hull.count++] = bestPoint; - - for (int32 i = 0; i < hull2.count; ++i) - { - hull.points[hull.count++] = hull2.points[i]; - } - - b2Assert(hull.count < b2_maxPolygonVertices); - - return hull; -} - -// quickhull algorithm -// - merges vertices based on b2_linearSlop -// - removes collinear points using b2_linearSlop -// - returns an empty hull if it fails -b2Hull b2ComputeHull(const b2Vec2* points, int32 count) -{ - b2Hull hull; - hull.count = 0; - - if (count < 3 || count > b2_maxPolygonVertices) - { - // check your data - return hull; - } - - count = b2Min(count, b2_maxPolygonVertices); - - b2AABB aabb = { {b2_maxFloat, b2_maxFloat}, {-b2_maxFloat, -b2_maxFloat} }; - - // Perform aggressive point welding. First point always remains. - // Also compute the bounding box for later. - b2Vec2 ps[b2_maxPolygonVertices]; - int32 n = 0; - const float tolSqr = 16.0f * b2_linearSlop * b2_linearSlop; - for (int32 i = 0; i < count; ++i) - { - aabb.lowerBound = b2Min(aabb.lowerBound, points[i]); - aabb.upperBound = b2Max(aabb.upperBound, points[i]); - - b2Vec2 vi = points[i]; - - bool unique = true; - for (int32 j = 0; j < i; ++j) - { - b2Vec2 vj = points[j]; - - float distSqr = b2DistanceSquared(vi, vj); - if (distSqr < tolSqr) - { - unique = false; - break; - } - } - - if (unique) - { - ps[n++] = vi; - } - } - - if (n < 3) - { - // all points very close together, check your data and check your scale - return hull; - } - - // Find an extreme point as the first point on the hull - b2Vec2 c = aabb.GetCenter(); - int32 i1 = 0; - float dsq1 = b2DistanceSquared(c, ps[i1]); - for (int32 i = 1; i < n; ++i) - { - float dsq = b2DistanceSquared(c, ps[i]); - if (dsq > dsq1) - { - i1 = i; - dsq1 = dsq; - } - } - - // remove p1 from working set - b2Vec2 p1 = ps[i1]; - ps[i1] = ps[n - 1]; - n = n - 1; - - int32 i2 = 0; - float dsq2 = b2DistanceSquared(p1, ps[i2]); - for (int32 i = 1; i < n; ++i) - { - float dsq = b2DistanceSquared(p1, ps[i]); - if (dsq > dsq2) - { - i2 = i; - dsq2 = dsq; - } - } - - // remove p2 from working set - b2Vec2 p2 = ps[i2]; - ps[i2] = ps[n - 1]; - n = n - 1; - - // split the points into points that are left and right of the line p1-p2. - b2Vec2 rightPoints[b2_maxPolygonVertices - 2]; - int32 rightCount = 0; - - b2Vec2 leftPoints[b2_maxPolygonVertices - 2]; - int32 leftCount = 0; - - b2Vec2 e = p2 - p1; - e.Normalize(); - - for (int32 i = 0; i < n; ++i) - { - float d = b2Cross(ps[i] - p1, e); - - // slop used here to skip points that are very close to the line p1-p2 - if (d >= 2.0f * b2_linearSlop) - { - rightPoints[rightCount++] = ps[i]; - } - else if (d <= -2.0f * b2_linearSlop) - { - leftPoints[leftCount++] = ps[i]; - } - } - - // compute hulls on right and left - b2Hull hull1 = b2RecurseHull(p1, p2, rightPoints, rightCount); - b2Hull hull2 = b2RecurseHull(p2, p1, leftPoints, leftCount); - - if (hull1.count == 0 && hull2.count == 0) - { - // all points collinear - return hull; - } - - // stitch hulls together, preserving CCW winding order - hull.points[hull.count++] = p1; - - for (int32 i = 0; i < hull1.count; ++i) - { - hull.points[hull.count++] = hull1.points[i]; - } - - hull.points[hull.count++] = p2; - - for (int32 i = 0; i < hull2.count; ++i) - { - hull.points[hull.count++] = hull2.points[i]; - } - - b2Assert(hull.count <= b2_maxPolygonVertices); - - // merge collinear - bool searching = true; - while (searching && hull.count > 2) - { - searching = false; - - for (int32 i = 0; i < hull.count; ++i) - { - int32 i1 = i; - int32 i2 = (i + 1) % hull.count; - int32 i3 = (i + 2) % hull.count; - - b2Vec2 p1 = hull.points[i1]; - b2Vec2 p2 = hull.points[i2]; - b2Vec2 p3 = hull.points[i3]; - - b2Vec2 e = p3 - p1; - e.Normalize(); - - b2Vec2 v = p2 - p1; - float distance = b2Cross(p2 - p1, e); - if (distance <= 2.0f * b2_linearSlop) - { - // remove midpoint from hull - for (int32 j = i2; j < hull.count - 1; ++j) - { - hull.points[j] = hull.points[j + 1]; - } - hull.count -= 1; - - // continue searching for collinear points - searching = true; - - break; - } - } - } - - if (hull.count < 3) - { - // all points collinear, shouldn't be reached since this was validated above - hull.count = 0; - } - - return hull; -} - -bool b2ValidateHull(const b2Hull& hull) -{ - if (hull.count < 3 || b2_maxPolygonVertices < hull.count) - { - return false; - } - - // test that every point is behind every edge - for (int32 i = 0; i < hull.count; ++i) - { - // create an edge vector - int32 i1 = i; - int32 i2 = i < hull.count - 1 ? i1 + 1 : 0; - b2Vec2 p = hull.points[i1]; - b2Vec2 e = hull.points[i2] - p; - e.Normalize(); - - for (int32 j = 0; j < hull.count; ++j) - { - // skip points that subtend the current edge - if (j == i1 || j == i2) - { - continue; - } - - float distance = b2Cross(hull.points[j] - p, e); - if (distance >= 0.0f) - { - return false; - } - } - } - - // test for collinear points - for (int32 i = 0; i < hull.count; ++i) - { - int32 i1 = i; - int32 i2 = (i + 1) % hull.count; - int32 i3 = (i + 2) % hull.count; - - b2Vec2 p1 = hull.points[i1]; - b2Vec2 p2 = hull.points[i2]; - b2Vec2 p3 = hull.points[i3]; - - b2Vec2 e = p3 - p1; - e.Normalize(); - - b2Vec2 v = p2 - p1; - float distance = b2Cross(p2 - p1, e); - if (distance <= b2_linearSlop) - { - // p1-p2-p3 are collinear - return false; - } - } - - return true; -} diff --git a/3rdparty/box2d/src/collision/b2_distance.cpp b/3rdparty/box2d/src/collision/b2_distance.cpp deleted file mode 100644 index 16fa3cc3d986..000000000000 --- a/3rdparty/box2d/src/collision/b2_distance.cpp +++ /dev/null @@ -1,744 +0,0 @@ -// MIT License - -// Copyright (c) 2019 Erin Catto - -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: - -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. - -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -#include "box2d/b2_circle_shape.h" -#include "box2d/b2_distance.h" -#include "box2d/b2_edge_shape.h" -#include "box2d/b2_chain_shape.h" -#include "box2d/b2_polygon_shape.h" - -// GJK using Voronoi regions (Christer Ericson) and Barycentric coordinates. -B2_API int32 b2_gjkCalls, b2_gjkIters, b2_gjkMaxIters; - -void b2DistanceProxy::Set(const b2Shape* shape, int32 index) -{ - switch (shape->GetType()) - { - case b2Shape::e_circle: - { - const b2CircleShape* circle = static_cast(shape); - m_vertices = &circle->m_p; - m_count = 1; - m_radius = circle->m_radius; - } - break; - - case b2Shape::e_polygon: - { - const b2PolygonShape* polygon = static_cast(shape); - m_vertices = polygon->m_vertices; - m_count = polygon->m_count; - m_radius = polygon->m_radius; - } - break; - - case b2Shape::e_chain: - { - const b2ChainShape* chain = static_cast(shape); - b2Assert(0 <= index && index < chain->m_count); - - m_buffer[0] = chain->m_vertices[index]; - if (index + 1 < chain->m_count) - { - m_buffer[1] = chain->m_vertices[index + 1]; - } - else - { - m_buffer[1] = chain->m_vertices[0]; - } - - m_vertices = m_buffer; - m_count = 2; - m_radius = chain->m_radius; - } - break; - - case b2Shape::e_edge: - { - const b2EdgeShape* edge = static_cast(shape); - m_vertices = &edge->m_vertex1; - m_count = 2; - m_radius = edge->m_radius; - } - break; - - default: - b2Assert(false); - } -} - -void b2DistanceProxy::Set(const b2Vec2* vertices, int32 count, float radius) -{ - m_vertices = vertices; - m_count = count; - m_radius = radius; -} - -struct b2SimplexVertex -{ - b2Vec2 wA; // support point in proxyA - b2Vec2 wB; // support point in proxyB - b2Vec2 w; // wB - wA - float a; // barycentric coordinate for closest point - int32 indexA; // wA index - int32 indexB; // wB index -}; - -struct b2Simplex -{ - void ReadCache( const b2SimplexCache* cache, - const b2DistanceProxy* proxyA, const b2Transform& transformA, - const b2DistanceProxy* proxyB, const b2Transform& transformB) - { - b2Assert(cache->count <= 3); - - // Copy data from cache. - m_count = cache->count; - b2SimplexVertex* vertices = &m_v1; - for (int32 i = 0; i < m_count; ++i) - { - b2SimplexVertex* v = vertices + i; - v->indexA = cache->indexA[i]; - v->indexB = cache->indexB[i]; - b2Vec2 wALocal = proxyA->GetVertex(v->indexA); - b2Vec2 wBLocal = proxyB->GetVertex(v->indexB); - v->wA = b2Mul(transformA, wALocal); - v->wB = b2Mul(transformB, wBLocal); - v->w = v->wB - v->wA; - v->a = 0.0f; - } - - // Compute the new simplex metric, if it is substantially different than - // old metric then flush the simplex. - if (m_count > 1) - { - float metric1 = cache->metric; - float metric2 = GetMetric(); - if (metric2 < 0.5f * metric1 || 2.0f * metric1 < metric2 || metric2 < b2_epsilon) - { - // Reset the simplex. - m_count = 0; - } - } - - // If the cache is empty or invalid ... - if (m_count == 0) - { - b2SimplexVertex* v = vertices + 0; - v->indexA = 0; - v->indexB = 0; - b2Vec2 wALocal = proxyA->GetVertex(0); - b2Vec2 wBLocal = proxyB->GetVertex(0); - v->wA = b2Mul(transformA, wALocal); - v->wB = b2Mul(transformB, wBLocal); - v->w = v->wB - v->wA; - v->a = 1.0f; - m_count = 1; - } - } - - void WriteCache(b2SimplexCache* cache) const - { - cache->metric = GetMetric(); - cache->count = uint16(m_count); - const b2SimplexVertex* vertices = &m_v1; - for (int32 i = 0; i < m_count; ++i) - { - cache->indexA[i] = uint8(vertices[i].indexA); - cache->indexB[i] = uint8(vertices[i].indexB); - } - } - - b2Vec2 GetSearchDirection() const - { - switch (m_count) - { - case 1: - return -m_v1.w; - - case 2: - { - b2Vec2 e12 = m_v2.w - m_v1.w; - float sgn = b2Cross(e12, -m_v1.w); - if (sgn > 0.0f) - { - // Origin is left of e12. - return b2Cross(1.0f, e12); - } - else - { - // Origin is right of e12. - return b2Cross(e12, 1.0f); - } - } - - default: - b2Assert(false); - return b2Vec2_zero; - } - } - - b2Vec2 GetClosestPoint() const - { - switch (m_count) - { - case 0: - b2Assert(false); - return b2Vec2_zero; - - case 1: - return m_v1.w; - - case 2: - return m_v1.a * m_v1.w + m_v2.a * m_v2.w; - - case 3: - return b2Vec2_zero; - - default: - b2Assert(false); - return b2Vec2_zero; - } - } - - void GetWitnessPoints(b2Vec2* pA, b2Vec2* pB) const - { - switch (m_count) - { - case 0: - b2Assert(false); - break; - - case 1: - *pA = m_v1.wA; - *pB = m_v1.wB; - break; - - case 2: - *pA = m_v1.a * m_v1.wA + m_v2.a * m_v2.wA; - *pB = m_v1.a * m_v1.wB + m_v2.a * m_v2.wB; - break; - - case 3: - *pA = m_v1.a * m_v1.wA + m_v2.a * m_v2.wA + m_v3.a * m_v3.wA; - *pB = *pA; - break; - - default: - b2Assert(false); - break; - } - } - - float GetMetric() const - { - switch (m_count) - { - case 0: - b2Assert(false); - return 0.0f; - - case 1: - return 0.0f; - - case 2: - return b2Distance(m_v1.w, m_v2.w); - - case 3: - return b2Cross(m_v2.w - m_v1.w, m_v3.w - m_v1.w); - - default: - b2Assert(false); - return 0.0f; - } - } - - void Solve2(); - void Solve3(); - - b2SimplexVertex m_v1, m_v2, m_v3; - int32 m_count; -}; - - -// Solve a line segment using barycentric coordinates. -// -// p = a1 * w1 + a2 * w2 -// a1 + a2 = 1 -// -// The vector from the origin to the closest point on the line is -// perpendicular to the line. -// e12 = w2 - w1 -// dot(p, e) = 0 -// a1 * dot(w1, e) + a2 * dot(w2, e) = 0 -// -// 2-by-2 linear system -// [1 1 ][a1] = [1] -// [w1.e12 w2.e12][a2] = [0] -// -// Define -// d12_1 = dot(w2, e12) -// d12_2 = -dot(w1, e12) -// d12 = d12_1 + d12_2 -// -// Solution -// a1 = d12_1 / d12 -// a2 = d12_2 / d12 -void b2Simplex::Solve2() -{ - b2Vec2 w1 = m_v1.w; - b2Vec2 w2 = m_v2.w; - b2Vec2 e12 = w2 - w1; - - // w1 region - float d12_2 = -b2Dot(w1, e12); - if (d12_2 <= 0.0f) - { - // a2 <= 0, so we clamp it to 0 - m_v1.a = 1.0f; - m_count = 1; - return; - } - - // w2 region - float d12_1 = b2Dot(w2, e12); - if (d12_1 <= 0.0f) - { - // a1 <= 0, so we clamp it to 0 - m_v2.a = 1.0f; - m_count = 1; - m_v1 = m_v2; - return; - } - - // Must be in e12 region. - float inv_d12 = 1.0f / (d12_1 + d12_2); - m_v1.a = d12_1 * inv_d12; - m_v2.a = d12_2 * inv_d12; - m_count = 2; -} - -// Possible regions: -// - points[2] -// - edge points[0]-points[2] -// - edge points[1]-points[2] -// - inside the triangle -void b2Simplex::Solve3() -{ - b2Vec2 w1 = m_v1.w; - b2Vec2 w2 = m_v2.w; - b2Vec2 w3 = m_v3.w; - - // Edge12 - // [1 1 ][a1] = [1] - // [w1.e12 w2.e12][a2] = [0] - // a3 = 0 - b2Vec2 e12 = w2 - w1; - float w1e12 = b2Dot(w1, e12); - float w2e12 = b2Dot(w2, e12); - float d12_1 = w2e12; - float d12_2 = -w1e12; - - // Edge13 - // [1 1 ][a1] = [1] - // [w1.e13 w3.e13][a3] = [0] - // a2 = 0 - b2Vec2 e13 = w3 - w1; - float w1e13 = b2Dot(w1, e13); - float w3e13 = b2Dot(w3, e13); - float d13_1 = w3e13; - float d13_2 = -w1e13; - - // Edge23 - // [1 1 ][a2] = [1] - // [w2.e23 w3.e23][a3] = [0] - // a1 = 0 - b2Vec2 e23 = w3 - w2; - float w2e23 = b2Dot(w2, e23); - float w3e23 = b2Dot(w3, e23); - float d23_1 = w3e23; - float d23_2 = -w2e23; - - // Triangle123 - float n123 = b2Cross(e12, e13); - - float d123_1 = n123 * b2Cross(w2, w3); - float d123_2 = n123 * b2Cross(w3, w1); - float d123_3 = n123 * b2Cross(w1, w2); - - // w1 region - if (d12_2 <= 0.0f && d13_2 <= 0.0f) - { - m_v1.a = 1.0f; - m_count = 1; - return; - } - - // e12 - if (d12_1 > 0.0f && d12_2 > 0.0f && d123_3 <= 0.0f) - { - float inv_d12 = 1.0f / (d12_1 + d12_2); - m_v1.a = d12_1 * inv_d12; - m_v2.a = d12_2 * inv_d12; - m_count = 2; - return; - } - - // e13 - if (d13_1 > 0.0f && d13_2 > 0.0f && d123_2 <= 0.0f) - { - float inv_d13 = 1.0f / (d13_1 + d13_2); - m_v1.a = d13_1 * inv_d13; - m_v3.a = d13_2 * inv_d13; - m_count = 2; - m_v2 = m_v3; - return; - } - - // w2 region - if (d12_1 <= 0.0f && d23_2 <= 0.0f) - { - m_v2.a = 1.0f; - m_count = 1; - m_v1 = m_v2; - return; - } - - // w3 region - if (d13_1 <= 0.0f && d23_1 <= 0.0f) - { - m_v3.a = 1.0f; - m_count = 1; - m_v1 = m_v3; - return; - } - - // e23 - if (d23_1 > 0.0f && d23_2 > 0.0f && d123_1 <= 0.0f) - { - float inv_d23 = 1.0f / (d23_1 + d23_2); - m_v2.a = d23_1 * inv_d23; - m_v3.a = d23_2 * inv_d23; - m_count = 2; - m_v1 = m_v3; - return; - } - - // Must be in triangle123 - float inv_d123 = 1.0f / (d123_1 + d123_2 + d123_3); - m_v1.a = d123_1 * inv_d123; - m_v2.a = d123_2 * inv_d123; - m_v3.a = d123_3 * inv_d123; - m_count = 3; -} - -void b2Distance(b2DistanceOutput* output, - b2SimplexCache* cache, - const b2DistanceInput* input) -{ - ++b2_gjkCalls; - - const b2DistanceProxy* proxyA = &input->proxyA; - const b2DistanceProxy* proxyB = &input->proxyB; - - b2Transform transformA = input->transformA; - b2Transform transformB = input->transformB; - - // Initialize the simplex. - b2Simplex simplex; - simplex.ReadCache(cache, proxyA, transformA, proxyB, transformB); - - // Get simplex vertices as an array. - b2SimplexVertex* vertices = &simplex.m_v1; - const int32 k_maxIters = 20; - - // These store the vertices of the last simplex so that we - // can check for duplicates and prevent cycling. - int32 saveA[3], saveB[3]; - int32 saveCount = 0; - - // Main iteration loop. - int32 iter = 0; - while (iter < k_maxIters) - { - // Copy simplex so we can identify duplicates. - saveCount = simplex.m_count; - for (int32 i = 0; i < saveCount; ++i) - { - saveA[i] = vertices[i].indexA; - saveB[i] = vertices[i].indexB; - } - - switch (simplex.m_count) - { - case 1: - break; - - case 2: - simplex.Solve2(); - break; - - case 3: - simplex.Solve3(); - break; - - default: - b2Assert(false); - } - - // If we have 3 points, then the origin is in the corresponding triangle. - if (simplex.m_count == 3) - { - break; - } - - // Get search direction. - b2Vec2 d = simplex.GetSearchDirection(); - - // Ensure the search direction is numerically fit. - if (d.LengthSquared() < b2_epsilon * b2_epsilon) - { - // The origin is probably contained by a line segment - // or triangle. Thus the shapes are overlapped. - - // We can't return zero here even though there may be overlap. - // In case the simplex is a point, segment, or triangle it is difficult - // to determine if the origin is contained in the CSO or very close to it. - break; - } - - // Compute a tentative new simplex vertex using support points. - b2SimplexVertex* vertex = vertices + simplex.m_count; - vertex->indexA = proxyA->GetSupport(b2MulT(transformA.q, -d)); - vertex->wA = b2Mul(transformA, proxyA->GetVertex(vertex->indexA)); - vertex->indexB = proxyB->GetSupport(b2MulT(transformB.q, d)); - vertex->wB = b2Mul(transformB, proxyB->GetVertex(vertex->indexB)); - vertex->w = vertex->wB - vertex->wA; - - // Iteration count is equated to the number of support point calls. - ++iter; - ++b2_gjkIters; - - // Check for duplicate support points. This is the main termination criteria. - bool duplicate = false; - for (int32 i = 0; i < saveCount; ++i) - { - if (vertex->indexA == saveA[i] && vertex->indexB == saveB[i]) - { - duplicate = true; - break; - } - } - - // If we found a duplicate support point we must exit to avoid cycling. - if (duplicate) - { - break; - } - - // New vertex is ok and needed. - ++simplex.m_count; - } - - b2_gjkMaxIters = b2Max(b2_gjkMaxIters, iter); - - // Prepare output. - simplex.GetWitnessPoints(&output->pointA, &output->pointB); - output->distance = b2Distance(output->pointA, output->pointB); - output->iterations = iter; - - // Cache the simplex. - simplex.WriteCache(cache); - - // Apply radii if requested - if (input->useRadii) - { - if (output->distance < b2_epsilon) - { - // Shapes are too close to safely compute normal - b2Vec2 p = 0.5f * (output->pointA + output->pointB); - output->pointA = p; - output->pointB = p; - output->distance = 0.0f; - } - else - { - // Keep closest points on perimeter even if overlapped, this way - // the points move smoothly. - float rA = proxyA->m_radius; - float rB = proxyB->m_radius; - b2Vec2 normal = output->pointB - output->pointA; - normal.Normalize(); - output->distance = b2Max(0.0f, output->distance - rA - rB); - output->pointA += rA * normal; - output->pointB -= rB * normal; - } - } -} - -// GJK-raycast -// Algorithm by Gino van den Bergen. -// "Smooth Mesh Contacts with GJK" in Game Physics Pearls. 2010 -bool b2ShapeCast(b2ShapeCastOutput * output, const b2ShapeCastInput * input) -{ - output->iterations = 0; - output->lambda = 1.0f; - output->normal.SetZero(); - output->point.SetZero(); - - const b2DistanceProxy* proxyA = &input->proxyA; - const b2DistanceProxy* proxyB = &input->proxyB; - - float radiusA = b2Max(proxyA->m_radius, b2_polygonRadius); - float radiusB = b2Max(proxyB->m_radius, b2_polygonRadius); - float radius = radiusA + radiusB; - - b2Transform xfA = input->transformA; - b2Transform xfB = input->transformB; - - b2Vec2 r = input->translationB; - b2Vec2 n(0.0f, 0.0f); - float lambda = 0.0f; - - // Initial simplex - b2Simplex simplex; - simplex.m_count = 0; - - // Get simplex vertices as an array. - b2SimplexVertex* vertices = &simplex.m_v1; - - // Get support point in -r direction - int32 indexA = proxyA->GetSupport(b2MulT(xfA.q, -r)); - b2Vec2 wA = b2Mul(xfA, proxyA->GetVertex(indexA)); - int32 indexB = proxyB->GetSupport(b2MulT(xfB.q, r)); - b2Vec2 wB = b2Mul(xfB, proxyB->GetVertex(indexB)); - b2Vec2 v = wA - wB; - - // Sigma is the target distance between polygons - float sigma = b2Max(b2_polygonRadius, radius - b2_polygonRadius); - const float tolerance = 0.5f * b2_linearSlop; - - // Main iteration loop. - const int32 k_maxIters = 20; - int32 iter = 0; - while (iter < k_maxIters && v.Length() - sigma > tolerance) - { - b2Assert(simplex.m_count < 3); - - output->iterations += 1; - - // Support in direction -v (A - B) - indexA = proxyA->GetSupport(b2MulT(xfA.q, -v)); - wA = b2Mul(xfA, proxyA->GetVertex(indexA)); - indexB = proxyB->GetSupport(b2MulT(xfB.q, v)); - wB = b2Mul(xfB, proxyB->GetVertex(indexB)); - b2Vec2 p = wA - wB; - - // -v is a normal at p - v.Normalize(); - - // Intersect ray with plane - float vp = b2Dot(v, p); - float vr = b2Dot(v, r); - if (vp - sigma > lambda * vr) - { - if (vr <= 0.0f) - { - return false; - } - - lambda = (vp - sigma) / vr; - if (lambda > 1.0f) - { - return false; - } - - n = -v; - simplex.m_count = 0; - } - - // Reverse simplex since it works with B - A. - // Shift by lambda * r because we want the closest point to the current clip point. - // Note that the support point p is not shifted because we want the plane equation - // to be formed in unshifted space. - b2SimplexVertex* vertex = vertices + simplex.m_count; - vertex->indexA = indexB; - vertex->wA = wB + lambda * r; - vertex->indexB = indexA; - vertex->wB = wA; - vertex->w = vertex->wB - vertex->wA; - vertex->a = 1.0f; - simplex.m_count += 1; - - switch (simplex.m_count) - { - case 1: - break; - - case 2: - simplex.Solve2(); - break; - - case 3: - simplex.Solve3(); - break; - - default: - b2Assert(false); - } - - // If we have 3 points, then the origin is in the corresponding triangle. - if (simplex.m_count == 3) - { - // Overlap - return false; - } - - // Get search direction. - v = simplex.GetClosestPoint(); - - // Iteration count is equated to the number of support point calls. - ++iter; - } - - if (iter == 0) - { - // Initial overlap - return false; - } - - // Prepare output. - b2Vec2 pointA, pointB; - simplex.GetWitnessPoints(&pointB, &pointA); - - if (v.LengthSquared() > 0.0f) - { - n = -v; - n.Normalize(); - } - - output->point = pointA + radiusA * n; - output->normal = n; - output->lambda = lambda; - output->iterations = iter; - return true; -} diff --git a/3rdparty/box2d/src/collision/b2_dynamic_tree.cpp b/3rdparty/box2d/src/collision/b2_dynamic_tree.cpp deleted file mode 100644 index 55a9d3246ad6..000000000000 --- a/3rdparty/box2d/src/collision/b2_dynamic_tree.cpp +++ /dev/null @@ -1,801 +0,0 @@ -// MIT License - -// Copyright (c) 2019 Erin Catto - -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: - -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. - -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. -#include "box2d/b2_dynamic_tree.h" -#include - -b2DynamicTree::b2DynamicTree() -{ - m_root = b2_nullNode; - - m_nodeCapacity = 16; - m_nodeCount = 0; - m_nodes = (b2TreeNode*)b2Alloc(m_nodeCapacity * sizeof(b2TreeNode)); - memset(m_nodes, 0, m_nodeCapacity * sizeof(b2TreeNode)); - - // Build a linked list for the free list. - for (int32 i = 0; i < m_nodeCapacity - 1; ++i) - { - m_nodes[i].next = i + 1; - m_nodes[i].height = -1; - } - m_nodes[m_nodeCapacity-1].next = b2_nullNode; - m_nodes[m_nodeCapacity-1].height = -1; - m_freeList = 0; - - m_insertionCount = 0; -} - -b2DynamicTree::~b2DynamicTree() -{ - // This frees the entire tree in one shot. - b2Free(m_nodes); -} - -// Allocate a node from the pool. Grow the pool if necessary. -int32 b2DynamicTree::AllocateNode() -{ - // Expand the node pool as needed. - if (m_freeList == b2_nullNode) - { - b2Assert(m_nodeCount == m_nodeCapacity); - - // The free list is empty. Rebuild a bigger pool. - b2TreeNode* oldNodes = m_nodes; - m_nodeCapacity *= 2; - m_nodes = (b2TreeNode*)b2Alloc(m_nodeCapacity * sizeof(b2TreeNode)); - memcpy(m_nodes, oldNodes, m_nodeCount * sizeof(b2TreeNode)); - b2Free(oldNodes); - - // Build a linked list for the free list. The parent - // pointer becomes the "next" pointer. - for (int32 i = m_nodeCount; i < m_nodeCapacity - 1; ++i) - { - m_nodes[i].next = i + 1; - m_nodes[i].height = -1; - } - m_nodes[m_nodeCapacity-1].next = b2_nullNode; - m_nodes[m_nodeCapacity-1].height = -1; - m_freeList = m_nodeCount; - } - - // Peel a node off the free list. - int32 nodeId = m_freeList; - m_freeList = m_nodes[nodeId].next; - m_nodes[nodeId].parent = b2_nullNode; - m_nodes[nodeId].child1 = b2_nullNode; - m_nodes[nodeId].child2 = b2_nullNode; - m_nodes[nodeId].height = 0; - m_nodes[nodeId].userData = nullptr; - m_nodes[nodeId].moved = false; - ++m_nodeCount; - return nodeId; -} - -// Return a node to the pool. -void b2DynamicTree::FreeNode(int32 nodeId) -{ - b2Assert(0 <= nodeId && nodeId < m_nodeCapacity); - b2Assert(0 < m_nodeCount); - m_nodes[nodeId].next = m_freeList; - m_nodes[nodeId].height = -1; - m_freeList = nodeId; - --m_nodeCount; -} - -// Create a proxy in the tree as a leaf node. We return the index -// of the node instead of a pointer so that we can grow -// the node pool. -int32 b2DynamicTree::CreateProxy(const b2AABB& aabb, void* userData) -{ - int32 proxyId = AllocateNode(); - - // Fatten the aabb. - b2Vec2 r(b2_aabbExtension, b2_aabbExtension); - m_nodes[proxyId].aabb.lowerBound = aabb.lowerBound - r; - m_nodes[proxyId].aabb.upperBound = aabb.upperBound + r; - m_nodes[proxyId].userData = userData; - m_nodes[proxyId].height = 0; - m_nodes[proxyId].moved = true; - - InsertLeaf(proxyId); - - return proxyId; -} - -void b2DynamicTree::DestroyProxy(int32 proxyId) -{ - b2Assert(0 <= proxyId && proxyId < m_nodeCapacity); - b2Assert(m_nodes[proxyId].IsLeaf()); - - RemoveLeaf(proxyId); - FreeNode(proxyId); -} - -bool b2DynamicTree::MoveProxy(int32 proxyId, const b2AABB& aabb, const b2Vec2& displacement) -{ - b2Assert(0 <= proxyId && proxyId < m_nodeCapacity); - - b2Assert(m_nodes[proxyId].IsLeaf()); - - // Extend AABB - b2AABB fatAABB; - b2Vec2 r(b2_aabbExtension, b2_aabbExtension); - fatAABB.lowerBound = aabb.lowerBound - r; - fatAABB.upperBound = aabb.upperBound + r; - - // Predict AABB movement - b2Vec2 d = b2_aabbMultiplier * displacement; - - if (d.x < 0.0f) - { - fatAABB.lowerBound.x += d.x; - } - else - { - fatAABB.upperBound.x += d.x; - } - - if (d.y < 0.0f) - { - fatAABB.lowerBound.y += d.y; - } - else - { - fatAABB.upperBound.y += d.y; - } - - const b2AABB& treeAABB = m_nodes[proxyId].aabb; - if (treeAABB.Contains(aabb)) - { - // The tree AABB still contains the object, but it might be too large. - // Perhaps the object was moving fast but has since gone to sleep. - // The huge AABB is larger than the new fat AABB. - b2AABB hugeAABB; - hugeAABB.lowerBound = fatAABB.lowerBound - 4.0f * r; - hugeAABB.upperBound = fatAABB.upperBound + 4.0f * r; - - if (hugeAABB.Contains(treeAABB)) - { - // The tree AABB contains the object AABB and the tree AABB is - // not too large. No tree update needed. - return false; - } - - // Otherwise the tree AABB is huge and needs to be shrunk - } - - RemoveLeaf(proxyId); - - m_nodes[proxyId].aabb = fatAABB; - - InsertLeaf(proxyId); - - m_nodes[proxyId].moved = true; - - return true; -} - -void b2DynamicTree::InsertLeaf(int32 leaf) -{ - ++m_insertionCount; - - if (m_root == b2_nullNode) - { - m_root = leaf; - m_nodes[m_root].parent = b2_nullNode; - return; - } - - // Find the best sibling for this node - b2AABB leafAABB = m_nodes[leaf].aabb; - int32 index = m_root; - while (m_nodes[index].IsLeaf() == false) - { - int32 child1 = m_nodes[index].child1; - int32 child2 = m_nodes[index].child2; - - float area = m_nodes[index].aabb.GetPerimeter(); - - b2AABB combinedAABB; - combinedAABB.Combine(m_nodes[index].aabb, leafAABB); - float combinedArea = combinedAABB.GetPerimeter(); - - // Cost of creating a new parent for this node and the new leaf - float cost = 2.0f * combinedArea; - - // Minimum cost of pushing the leaf further down the tree - float inheritanceCost = 2.0f * (combinedArea - area); - - // Cost of descending into child1 - float cost1; - if (m_nodes[child1].IsLeaf()) - { - b2AABB aabb; - aabb.Combine(leafAABB, m_nodes[child1].aabb); - cost1 = aabb.GetPerimeter() + inheritanceCost; - } - else - { - b2AABB aabb; - aabb.Combine(leafAABB, m_nodes[child1].aabb); - float oldArea = m_nodes[child1].aabb.GetPerimeter(); - float newArea = aabb.GetPerimeter(); - cost1 = (newArea - oldArea) + inheritanceCost; - } - - // Cost of descending into child2 - float cost2; - if (m_nodes[child2].IsLeaf()) - { - b2AABB aabb; - aabb.Combine(leafAABB, m_nodes[child2].aabb); - cost2 = aabb.GetPerimeter() + inheritanceCost; - } - else - { - b2AABB aabb; - aabb.Combine(leafAABB, m_nodes[child2].aabb); - float oldArea = m_nodes[child2].aabb.GetPerimeter(); - float newArea = aabb.GetPerimeter(); - cost2 = newArea - oldArea + inheritanceCost; - } - - // Descend according to the minimum cost. - if (cost < cost1 && cost < cost2) - { - break; - } - - // Descend - if (cost1 < cost2) - { - index = child1; - } - else - { - index = child2; - } - } - - int32 sibling = index; - - // Create a new parent. - int32 oldParent = m_nodes[sibling].parent; - int32 newParent = AllocateNode(); - m_nodes[newParent].parent = oldParent; - m_nodes[newParent].userData = nullptr; - m_nodes[newParent].aabb.Combine(leafAABB, m_nodes[sibling].aabb); - m_nodes[newParent].height = m_nodes[sibling].height + 1; - - if (oldParent != b2_nullNode) - { - // The sibling was not the root. - if (m_nodes[oldParent].child1 == sibling) - { - m_nodes[oldParent].child1 = newParent; - } - else - { - m_nodes[oldParent].child2 = newParent; - } - - m_nodes[newParent].child1 = sibling; - m_nodes[newParent].child2 = leaf; - m_nodes[sibling].parent = newParent; - m_nodes[leaf].parent = newParent; - } - else - { - // The sibling was the root. - m_nodes[newParent].child1 = sibling; - m_nodes[newParent].child2 = leaf; - m_nodes[sibling].parent = newParent; - m_nodes[leaf].parent = newParent; - m_root = newParent; - } - - // Walk back up the tree fixing heights and AABBs - index = m_nodes[leaf].parent; - while (index != b2_nullNode) - { - index = Balance(index); - - int32 child1 = m_nodes[index].child1; - int32 child2 = m_nodes[index].child2; - - b2Assert(child1 != b2_nullNode); - b2Assert(child2 != b2_nullNode); - - m_nodes[index].height = 1 + b2Max(m_nodes[child1].height, m_nodes[child2].height); - m_nodes[index].aabb.Combine(m_nodes[child1].aabb, m_nodes[child2].aabb); - - index = m_nodes[index].parent; - } - - //Validate(); -} - -void b2DynamicTree::RemoveLeaf(int32 leaf) -{ - if (leaf == m_root) - { - m_root = b2_nullNode; - return; - } - - int32 parent = m_nodes[leaf].parent; - int32 grandParent = m_nodes[parent].parent; - int32 sibling; - if (m_nodes[parent].child1 == leaf) - { - sibling = m_nodes[parent].child2; - } - else - { - sibling = m_nodes[parent].child1; - } - - if (grandParent != b2_nullNode) - { - // Destroy parent and connect sibling to grandParent. - if (m_nodes[grandParent].child1 == parent) - { - m_nodes[grandParent].child1 = sibling; - } - else - { - m_nodes[grandParent].child2 = sibling; - } - m_nodes[sibling].parent = grandParent; - FreeNode(parent); - - // Adjust ancestor bounds. - int32 index = grandParent; - while (index != b2_nullNode) - { - index = Balance(index); - - int32 child1 = m_nodes[index].child1; - int32 child2 = m_nodes[index].child2; - - m_nodes[index].aabb.Combine(m_nodes[child1].aabb, m_nodes[child2].aabb); - m_nodes[index].height = 1 + b2Max(m_nodes[child1].height, m_nodes[child2].height); - - index = m_nodes[index].parent; - } - } - else - { - m_root = sibling; - m_nodes[sibling].parent = b2_nullNode; - FreeNode(parent); - } - - //Validate(); -} - -// Perform a left or right rotation if node A is imbalanced. -// Returns the new root index. -int32 b2DynamicTree::Balance(int32 iA) -{ - b2Assert(iA != b2_nullNode); - - b2TreeNode* A = m_nodes + iA; - if (A->IsLeaf() || A->height < 2) - { - return iA; - } - - int32 iB = A->child1; - int32 iC = A->child2; - b2Assert(0 <= iB && iB < m_nodeCapacity); - b2Assert(0 <= iC && iC < m_nodeCapacity); - - b2TreeNode* B = m_nodes + iB; - b2TreeNode* C = m_nodes + iC; - - int32 balance = C->height - B->height; - - // Rotate C up - if (balance > 1) - { - int32 iF = C->child1; - int32 iG = C->child2; - b2TreeNode* F = m_nodes + iF; - b2TreeNode* G = m_nodes + iG; - b2Assert(0 <= iF && iF < m_nodeCapacity); - b2Assert(0 <= iG && iG < m_nodeCapacity); - - // Swap A and C - C->child1 = iA; - C->parent = A->parent; - A->parent = iC; - - // A's old parent should point to C - if (C->parent != b2_nullNode) - { - if (m_nodes[C->parent].child1 == iA) - { - m_nodes[C->parent].child1 = iC; - } - else - { - b2Assert(m_nodes[C->parent].child2 == iA); - m_nodes[C->parent].child2 = iC; - } - } - else - { - m_root = iC; - } - - // Rotate - if (F->height > G->height) - { - C->child2 = iF; - A->child2 = iG; - G->parent = iA; - A->aabb.Combine(B->aabb, G->aabb); - C->aabb.Combine(A->aabb, F->aabb); - - A->height = 1 + b2Max(B->height, G->height); - C->height = 1 + b2Max(A->height, F->height); - } - else - { - C->child2 = iG; - A->child2 = iF; - F->parent = iA; - A->aabb.Combine(B->aabb, F->aabb); - C->aabb.Combine(A->aabb, G->aabb); - - A->height = 1 + b2Max(B->height, F->height); - C->height = 1 + b2Max(A->height, G->height); - } - - return iC; - } - - // Rotate B up - if (balance < -1) - { - int32 iD = B->child1; - int32 iE = B->child2; - b2TreeNode* D = m_nodes + iD; - b2TreeNode* E = m_nodes + iE; - b2Assert(0 <= iD && iD < m_nodeCapacity); - b2Assert(0 <= iE && iE < m_nodeCapacity); - - // Swap A and B - B->child1 = iA; - B->parent = A->parent; - A->parent = iB; - - // A's old parent should point to B - if (B->parent != b2_nullNode) - { - if (m_nodes[B->parent].child1 == iA) - { - m_nodes[B->parent].child1 = iB; - } - else - { - b2Assert(m_nodes[B->parent].child2 == iA); - m_nodes[B->parent].child2 = iB; - } - } - else - { - m_root = iB; - } - - // Rotate - if (D->height > E->height) - { - B->child2 = iD; - A->child1 = iE; - E->parent = iA; - A->aabb.Combine(C->aabb, E->aabb); - B->aabb.Combine(A->aabb, D->aabb); - - A->height = 1 + b2Max(C->height, E->height); - B->height = 1 + b2Max(A->height, D->height); - } - else - { - B->child2 = iE; - A->child1 = iD; - D->parent = iA; - A->aabb.Combine(C->aabb, D->aabb); - B->aabb.Combine(A->aabb, E->aabb); - - A->height = 1 + b2Max(C->height, D->height); - B->height = 1 + b2Max(A->height, E->height); - } - - return iB; - } - - return iA; -} - -int32 b2DynamicTree::GetHeight() const -{ - if (m_root == b2_nullNode) - { - return 0; - } - - return m_nodes[m_root].height; -} - -// -float b2DynamicTree::GetAreaRatio() const -{ - if (m_root == b2_nullNode) - { - return 0.0f; - } - - const b2TreeNode* root = m_nodes + m_root; - float rootArea = root->aabb.GetPerimeter(); - - float totalArea = 0.0f; - for (int32 i = 0; i < m_nodeCapacity; ++i) - { - const b2TreeNode* node = m_nodes + i; - if (node->height < 0) - { - // Free node in pool - continue; - } - - totalArea += node->aabb.GetPerimeter(); - } - - return totalArea / rootArea; -} - -// Compute the height of a sub-tree. -int32 b2DynamicTree::ComputeHeight(int32 nodeId) const -{ - b2Assert(0 <= nodeId && nodeId < m_nodeCapacity); - b2TreeNode* node = m_nodes + nodeId; - - if (node->IsLeaf()) - { - return 0; - } - - int32 height1 = ComputeHeight(node->child1); - int32 height2 = ComputeHeight(node->child2); - return 1 + b2Max(height1, height2); -} - -int32 b2DynamicTree::ComputeHeight() const -{ - int32 height = ComputeHeight(m_root); - return height; -} - -void b2DynamicTree::ValidateStructure(int32 index) const -{ - if (index == b2_nullNode) - { - return; - } - - if (index == m_root) - { - b2Assert(m_nodes[index].parent == b2_nullNode); - } - - const b2TreeNode* node = m_nodes + index; - - int32 child1 = node->child1; - int32 child2 = node->child2; - - if (node->IsLeaf()) - { - b2Assert(child1 == b2_nullNode); - b2Assert(child2 == b2_nullNode); - b2Assert(node->height == 0); - return; - } - - b2Assert(0 <= child1 && child1 < m_nodeCapacity); - b2Assert(0 <= child2 && child2 < m_nodeCapacity); - - b2Assert(m_nodes[child1].parent == index); - b2Assert(m_nodes[child2].parent == index); - - ValidateStructure(child1); - ValidateStructure(child2); -} - -void b2DynamicTree::ValidateMetrics(int32 index) const -{ - if (index == b2_nullNode) - { - return; - } - - const b2TreeNode* node = m_nodes + index; - - int32 child1 = node->child1; - int32 child2 = node->child2; - - if (node->IsLeaf()) - { - b2Assert(child1 == b2_nullNode); - b2Assert(child2 == b2_nullNode); - b2Assert(node->height == 0); - return; - } - - b2Assert(0 <= child1 && child1 < m_nodeCapacity); - b2Assert(0 <= child2 && child2 < m_nodeCapacity); - - int32 height1 = m_nodes[child1].height; - int32 height2 = m_nodes[child2].height; - int32 height; - height = 1 + b2Max(height1, height2); - b2Assert(node->height == height); - - b2AABB aabb; - aabb.Combine(m_nodes[child1].aabb, m_nodes[child2].aabb); - - b2Assert(aabb.lowerBound == node->aabb.lowerBound); - b2Assert(aabb.upperBound == node->aabb.upperBound); - - ValidateMetrics(child1); - ValidateMetrics(child2); -} - -void b2DynamicTree::Validate() const -{ -#if defined(b2DEBUG) - ValidateStructure(m_root); - ValidateMetrics(m_root); - - int32 freeCount = 0; - int32 freeIndex = m_freeList; - while (freeIndex != b2_nullNode) - { - b2Assert(0 <= freeIndex && freeIndex < m_nodeCapacity); - freeIndex = m_nodes[freeIndex].next; - ++freeCount; - } - - b2Assert(GetHeight() == ComputeHeight()); - - b2Assert(m_nodeCount + freeCount == m_nodeCapacity); -#endif -} - -int32 b2DynamicTree::GetMaxBalance() const -{ - int32 maxBalance = 0; - for (int32 i = 0; i < m_nodeCapacity; ++i) - { - const b2TreeNode* node = m_nodes + i; - if (node->height <= 1) - { - continue; - } - - b2Assert(node->IsLeaf() == false); - - int32 child1 = node->child1; - int32 child2 = node->child2; - int32 balance = b2Abs(m_nodes[child2].height - m_nodes[child1].height); - maxBalance = b2Max(maxBalance, balance); - } - - return maxBalance; -} - -void b2DynamicTree::RebuildBottomUp() -{ - int32* nodes = (int32*)b2Alloc(m_nodeCount * sizeof(int32)); - int32 count = 0; - - // Build array of leaves. Free the rest. - for (int32 i = 0; i < m_nodeCapacity; ++i) - { - if (m_nodes[i].height < 0) - { - // free node in pool - continue; - } - - if (m_nodes[i].IsLeaf()) - { - m_nodes[i].parent = b2_nullNode; - nodes[count] = i; - ++count; - } - else - { - FreeNode(i); - } - } - - while (count > 1) - { - float minCost = b2_maxFloat; - int32 iMin = -1, jMin = -1; - for (int32 i = 0; i < count; ++i) - { - b2AABB aabbi = m_nodes[nodes[i]].aabb; - - for (int32 j = i + 1; j < count; ++j) - { - b2AABB aabbj = m_nodes[nodes[j]].aabb; - b2AABB b; - b.Combine(aabbi, aabbj); - float cost = b.GetPerimeter(); - if (cost < minCost) - { - iMin = i; - jMin = j; - minCost = cost; - } - } - } - - int32 index1 = nodes[iMin]; - int32 index2 = nodes[jMin]; - b2TreeNode* child1 = m_nodes + index1; - b2TreeNode* child2 = m_nodes + index2; - - int32 parentIndex = AllocateNode(); - b2TreeNode* parent = m_nodes + parentIndex; - parent->child1 = index1; - parent->child2 = index2; - parent->height = 1 + b2Max(child1->height, child2->height); - parent->aabb.Combine(child1->aabb, child2->aabb); - parent->parent = b2_nullNode; - - child1->parent = parentIndex; - child2->parent = parentIndex; - - nodes[jMin] = nodes[count-1]; - nodes[iMin] = parentIndex; - --count; - } - - m_root = nodes[0]; - b2Free(nodes); - - Validate(); -} - -void b2DynamicTree::ShiftOrigin(const b2Vec2& newOrigin) -{ - // Build array of leaves. Free the rest. - for (int32 i = 0; i < m_nodeCapacity; ++i) - { - m_nodes[i].aabb.lowerBound -= newOrigin; - m_nodes[i].aabb.upperBound -= newOrigin; - } -} diff --git a/3rdparty/box2d/src/collision/b2_edge_shape.cpp b/3rdparty/box2d/src/collision/b2_edge_shape.cpp deleted file mode 100644 index 65f0606a6483..000000000000 --- a/3rdparty/box2d/src/collision/b2_edge_shape.cpp +++ /dev/null @@ -1,158 +0,0 @@ -// MIT License - -// Copyright (c) 2019 Erin Catto - -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: - -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. - -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -#include "box2d/b2_edge_shape.h" -#include "box2d/b2_block_allocator.h" -#include - -void b2EdgeShape::SetOneSided(const b2Vec2& v0, const b2Vec2& v1, const b2Vec2& v2, const b2Vec2& v3) -{ - m_vertex0 = v0; - m_vertex1 = v1; - m_vertex2 = v2; - m_vertex3 = v3; - m_oneSided = true; -} - -void b2EdgeShape::SetTwoSided(const b2Vec2& v1, const b2Vec2& v2) -{ - m_vertex1 = v1; - m_vertex2 = v2; - m_oneSided = false; -} - -b2Shape* b2EdgeShape::Clone(b2BlockAllocator* allocator) const -{ - void* mem = allocator->Allocate(sizeof(b2EdgeShape)); - b2EdgeShape* clone = new (mem) b2EdgeShape; - *clone = *this; - return clone; -} - -int32 b2EdgeShape::GetChildCount() const -{ - return 1; -} - -bool b2EdgeShape::TestPoint(const b2Transform& xf, const b2Vec2& p) const -{ - B2_NOT_USED(xf); - B2_NOT_USED(p); - return false; -} - -// p = p1 + t * d -// v = v1 + s * e -// p1 + t * d = v1 + s * e -// s * e - t * d = p1 - v1 -bool b2EdgeShape::RayCast(b2RayCastOutput* output, const b2RayCastInput& input, - const b2Transform& xf, int32 childIndex) const -{ - B2_NOT_USED(childIndex); - - // Put the ray into the edge's frame of reference. - b2Vec2 p1 = b2MulT(xf.q, input.p1 - xf.p); - b2Vec2 p2 = b2MulT(xf.q, input.p2 - xf.p); - b2Vec2 d = p2 - p1; - - b2Vec2 v1 = m_vertex1; - b2Vec2 v2 = m_vertex2; - b2Vec2 e = v2 - v1; - - // Normal points to the right, looking from v1 at v2 - b2Vec2 normal(e.y, -e.x); - normal.Normalize(); - - // q = p1 + t * d - // dot(normal, q - v1) = 0 - // dot(normal, p1 - v1) + t * dot(normal, d) = 0 - float numerator = b2Dot(normal, v1 - p1); - if (m_oneSided && numerator > 0.0f) - { - return false; - } - - float denominator = b2Dot(normal, d); - - if (denominator == 0.0f) - { - return false; - } - - float t = numerator / denominator; - if (t < 0.0f || input.maxFraction < t) - { - return false; - } - - b2Vec2 q = p1 + t * d; - - // q = v1 + s * r - // s = dot(q - v1, r) / dot(r, r) - b2Vec2 r = v2 - v1; - float rr = b2Dot(r, r); - if (rr == 0.0f) - { - return false; - } - - float s = b2Dot(q - v1, r) / rr; - if (s < 0.0f || 1.0f < s) - { - return false; - } - - output->fraction = t; - if (numerator > 0.0f) - { - output->normal = -b2Mul(xf.q, normal); - } - else - { - output->normal = b2Mul(xf.q, normal); - } - return true; -} - -void b2EdgeShape::ComputeAABB(b2AABB* aabb, const b2Transform& xf, int32 childIndex) const -{ - B2_NOT_USED(childIndex); - - b2Vec2 v1 = b2Mul(xf, m_vertex1); - b2Vec2 v2 = b2Mul(xf, m_vertex2); - - b2Vec2 lower = b2Min(v1, v2); - b2Vec2 upper = b2Max(v1, v2); - - b2Vec2 r(m_radius, m_radius); - aabb->lowerBound = lower - r; - aabb->upperBound = upper + r; -} - -void b2EdgeShape::ComputeMass(b2MassData* massData, float density) const -{ - B2_NOT_USED(density); - - massData->mass = 0.0f; - massData->center = 0.5f * (m_vertex1 + m_vertex2); - massData->I = 0.0f; -} diff --git a/3rdparty/box2d/src/collision/b2_polygon_shape.cpp b/3rdparty/box2d/src/collision/b2_polygon_shape.cpp deleted file mode 100644 index 165919b46fa7..000000000000 --- a/3rdparty/box2d/src/collision/b2_polygon_shape.cpp +++ /dev/null @@ -1,366 +0,0 @@ -// MIT License - -// Copyright (c) 2019 Erin Catto - -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: - -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. - -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -#include "box2d/b2_polygon_shape.h" -#include "box2d/b2_block_allocator.h" - -#include - -b2PolygonShape::b2PolygonShape() -{ - m_type = e_polygon; - m_radius = b2_polygonRadius; - m_count = 0; - m_centroid.SetZero(); -} - -b2Shape* b2PolygonShape::Clone(b2BlockAllocator* allocator) const -{ - void* mem = allocator->Allocate(sizeof(b2PolygonShape)); - b2PolygonShape* clone = new (mem) b2PolygonShape; - *clone = *this; - return clone; -} - -void b2PolygonShape::SetAsBox(float hx, float hy) -{ - m_count = 4; - m_vertices[0].Set(-hx, -hy); - m_vertices[1].Set( hx, -hy); - m_vertices[2].Set( hx, hy); - m_vertices[3].Set(-hx, hy); - m_normals[0].Set(0.0f, -1.0f); - m_normals[1].Set(1.0f, 0.0f); - m_normals[2].Set(0.0f, 1.0f); - m_normals[3].Set(-1.0f, 0.0f); - m_centroid.SetZero(); -} - -void b2PolygonShape::SetAsBox(float hx, float hy, const b2Vec2& center, float angle) -{ - m_count = 4; - m_vertices[0].Set(-hx, -hy); - m_vertices[1].Set( hx, -hy); - m_vertices[2].Set( hx, hy); - m_vertices[3].Set(-hx, hy); - m_normals[0].Set(0.0f, -1.0f); - m_normals[1].Set(1.0f, 0.0f); - m_normals[2].Set(0.0f, 1.0f); - m_normals[3].Set(-1.0f, 0.0f); - m_centroid = center; - - b2Transform xf; - xf.p = center; - xf.q.Set(angle); - - // Transform vertices and normals. - for (int32 i = 0; i < m_count; ++i) - { - m_vertices[i] = b2Mul(xf, m_vertices[i]); - m_normals[i] = b2Mul(xf.q, m_normals[i]); - } -} - -int32 b2PolygonShape::GetChildCount() const -{ - return 1; -} - -static b2Vec2 ComputeCentroid(const b2Vec2* vs, int32 count) -{ - b2Assert(count >= 3); - - b2Vec2 c(0.0f, 0.0f); - float area = 0.0f; - - // Get a reference point for forming triangles. - // Use the first vertex to reduce round-off errors. - b2Vec2 s = vs[0]; - - const float inv3 = 1.0f / 3.0f; - - for (int32 i = 0; i < count; ++i) - { - // Triangle vertices. - b2Vec2 p1 = vs[0] - s; - b2Vec2 p2 = vs[i] - s; - b2Vec2 p3 = i + 1 < count ? vs[i+1] - s : vs[0] - s; - - b2Vec2 e1 = p2 - p1; - b2Vec2 e2 = p3 - p1; - - float D = b2Cross(e1, e2); - - float triangleArea = 0.5f * D; - area += triangleArea; - - // Area weighted centroid - c += triangleArea * inv3 * (p1 + p2 + p3); - } - - // Centroid - b2Assert(area > b2_epsilon); - c = (1.0f / area) * c + s; - return c; -} - -bool b2PolygonShape::Set(const b2Vec2* vertices, int32 count) -{ - b2Hull hull = b2ComputeHull(vertices, count); - - if (hull.count < 3) - { - return false; - } - - Set(hull); - - return true; -} - -void b2PolygonShape::Set(const b2Hull& hull) -{ - b2Assert(hull.count >= 3); - - m_count = hull.count; - - // Copy vertices - for (int32 i = 0; i < hull.count; ++i) - { - m_vertices[i] = hull.points[i]; - } - - // Compute normals. Ensure the edges have non-zero length. - for (int32 i = 0; i < m_count; ++i) - { - int32 i1 = i; - int32 i2 = i + 1 < m_count ? i + 1 : 0; - b2Vec2 edge = m_vertices[i2] - m_vertices[i1]; - b2Assert(edge.LengthSquared() > b2_epsilon * b2_epsilon); - m_normals[i] = b2Cross(edge, 1.0f); - m_normals[i].Normalize(); - } - - // Compute the polygon centroid. - m_centroid = ComputeCentroid(m_vertices, m_count); -} - -bool b2PolygonShape::TestPoint(const b2Transform& xf, const b2Vec2& p) const -{ - b2Vec2 pLocal = b2MulT(xf.q, p - xf.p); - - for (int32 i = 0; i < m_count; ++i) - { - float dot = b2Dot(m_normals[i], pLocal - m_vertices[i]); - if (dot > 0.0f) - { - return false; - } - } - - return true; -} - -bool b2PolygonShape::RayCast(b2RayCastOutput* output, const b2RayCastInput& input, - const b2Transform& xf, int32 childIndex) const -{ - B2_NOT_USED(childIndex); - - // Put the ray into the polygon's frame of reference. - b2Vec2 p1 = b2MulT(xf.q, input.p1 - xf.p); - b2Vec2 p2 = b2MulT(xf.q, input.p2 - xf.p); - b2Vec2 d = p2 - p1; - - float lower = 0.0f, upper = input.maxFraction; - - int32 index = -1; - - for (int32 i = 0; i < m_count; ++i) - { - // p = p1 + a * d - // dot(normal, p - v) = 0 - // dot(normal, p1 - v) + a * dot(normal, d) = 0 - float numerator = b2Dot(m_normals[i], m_vertices[i] - p1); - float denominator = b2Dot(m_normals[i], d); - - if (denominator == 0.0f) - { - if (numerator < 0.0f) - { - return false; - } - } - else - { - // Note: we want this predicate without division: - // lower < numerator / denominator, where denominator < 0 - // Since denominator < 0, we have to flip the inequality: - // lower < numerator / denominator <==> denominator * lower > numerator. - if (denominator < 0.0f && numerator < lower * denominator) - { - // Increase lower. - // The segment enters this half-space. - lower = numerator / denominator; - index = i; - } - else if (denominator > 0.0f && numerator < upper * denominator) - { - // Decrease upper. - // The segment exits this half-space. - upper = numerator / denominator; - } - } - - // The use of epsilon here causes the assert on lower to trip - // in some cases. Apparently the use of epsilon was to make edge - // shapes work, but now those are handled separately. - //if (upper < lower - b2_epsilon) - if (upper < lower) - { - return false; - } - } - - b2Assert(0.0f <= lower && lower <= input.maxFraction); - - if (index >= 0) - { - output->fraction = lower; - output->normal = b2Mul(xf.q, m_normals[index]); - return true; - } - - return false; -} - -void b2PolygonShape::ComputeAABB(b2AABB* aabb, const b2Transform& xf, int32 childIndex) const -{ - B2_NOT_USED(childIndex); - - b2Vec2 lower = b2Mul(xf, m_vertices[0]); - b2Vec2 upper = lower; - - for (int32 i = 1; i < m_count; ++i) - { - b2Vec2 v = b2Mul(xf, m_vertices[i]); - lower = b2Min(lower, v); - upper = b2Max(upper, v); - } - - b2Vec2 r(m_radius, m_radius); - aabb->lowerBound = lower - r; - aabb->upperBound = upper + r; -} - -void b2PolygonShape::ComputeMass(b2MassData* massData, float density) const -{ - // Polygon mass, centroid, and inertia. - // Let rho be the polygon density in mass per unit area. - // Then: - // mass = rho * int(dA) - // centroid.x = (1/mass) * rho * int(x * dA) - // centroid.y = (1/mass) * rho * int(y * dA) - // I = rho * int((x*x + y*y) * dA) - // - // We can compute these integrals by summing all the integrals - // for each triangle of the polygon. To evaluate the integral - // for a single triangle, we make a change of variables to - // the (u,v) coordinates of the triangle: - // x = x0 + e1x * u + e2x * v - // y = y0 + e1y * u + e2y * v - // where 0 <= u && 0 <= v && u + v <= 1. - // - // We integrate u from [0,1-v] and then v from [0,1]. - // We also need to use the Jacobian of the transformation: - // D = cross(e1, e2) - // - // Simplification: triangle centroid = (1/3) * (p1 + p2 + p3) - // - // The rest of the derivation is handled by computer algebra. - - b2Assert(m_count >= 3); - - b2Vec2 center(0.0f, 0.0f); - float area = 0.0f; - float I = 0.0f; - - // Get a reference point for forming triangles. - // Use the first vertex to reduce round-off errors. - b2Vec2 s = m_vertices[0]; - - const float k_inv3 = 1.0f / 3.0f; - - for (int32 i = 0; i < m_count; ++i) - { - // Triangle vertices. - b2Vec2 e1 = m_vertices[i] - s; - b2Vec2 e2 = i + 1 < m_count ? m_vertices[i+1] - s : m_vertices[0] - s; - - float D = b2Cross(e1, e2); - - float triangleArea = 0.5f * D; - area += triangleArea; - - // Area weighted centroid - center += triangleArea * k_inv3 * (e1 + e2); - - float ex1 = e1.x, ey1 = e1.y; - float ex2 = e2.x, ey2 = e2.y; - - float intx2 = ex1*ex1 + ex2*ex1 + ex2*ex2; - float inty2 = ey1*ey1 + ey2*ey1 + ey2*ey2; - - I += (0.25f * k_inv3 * D) * (intx2 + inty2); - } - - // Total mass - massData->mass = density * area; - - // Center of mass - b2Assert(area > b2_epsilon); - center *= 1.0f / area; - massData->center = center + s; - - // Inertia tensor relative to the local origin (point s). - massData->I = density * I; - - // Shift to center of mass then to original body origin. - massData->I += massData->mass * (b2Dot(massData->center, massData->center) - b2Dot(center, center)); -} - -bool b2PolygonShape::Validate() const -{ - if (m_count < 3 || b2_maxPolygonVertices < m_count) - { - return false; - } - - b2Hull hull; - for (int32 i = 0; i < m_count; ++i) - { - hull.points[i] = m_vertices[i]; - } - - hull.count = m_count; - - return b2ValidateHull(hull); -} diff --git a/3rdparty/box2d/src/collision/b2_time_of_impact.cpp b/3rdparty/box2d/src/collision/b2_time_of_impact.cpp deleted file mode 100644 index 7a1011b7e7cc..000000000000 --- a/3rdparty/box2d/src/collision/b2_time_of_impact.cpp +++ /dev/null @@ -1,490 +0,0 @@ -// MIT License - -// Copyright (c) 2019 Erin Catto - -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: - -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. - -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -#include "box2d/b2_collision.h" -#include "box2d/b2_distance.h" -#include "box2d/b2_circle_shape.h" -#include "box2d/b2_polygon_shape.h" -#include "box2d/b2_time_of_impact.h" -#include "box2d/b2_timer.h" - -#include - -B2_API float b2_toiTime, b2_toiMaxTime; -B2_API int32 b2_toiCalls, b2_toiIters, b2_toiMaxIters; -B2_API int32 b2_toiRootIters, b2_toiMaxRootIters; - -// -struct b2SeparationFunction -{ - enum Type - { - e_points, - e_faceA, - e_faceB - }; - - // TODO_ERIN might not need to return the separation - - float Initialize(const b2SimplexCache* cache, - const b2DistanceProxy* proxyA, const b2Sweep& sweepA, - const b2DistanceProxy* proxyB, const b2Sweep& sweepB, - float t1) - { - m_proxyA = proxyA; - m_proxyB = proxyB; - int32 count = cache->count; - b2Assert(0 < count && count < 3); - - m_sweepA = sweepA; - m_sweepB = sweepB; - - b2Transform xfA, xfB; - m_sweepA.GetTransform(&xfA, t1); - m_sweepB.GetTransform(&xfB, t1); - - if (count == 1) - { - m_type = e_points; - b2Vec2 localPointA = m_proxyA->GetVertex(cache->indexA[0]); - b2Vec2 localPointB = m_proxyB->GetVertex(cache->indexB[0]); - b2Vec2 pointA = b2Mul(xfA, localPointA); - b2Vec2 pointB = b2Mul(xfB, localPointB); - m_axis = pointB - pointA; - float s = m_axis.Normalize(); - return s; - } - else if (cache->indexA[0] == cache->indexA[1]) - { - // Two points on B and one on A. - m_type = e_faceB; - b2Vec2 localPointB1 = proxyB->GetVertex(cache->indexB[0]); - b2Vec2 localPointB2 = proxyB->GetVertex(cache->indexB[1]); - - m_axis = b2Cross(localPointB2 - localPointB1, 1.0f); - m_axis.Normalize(); - b2Vec2 normal = b2Mul(xfB.q, m_axis); - - m_localPoint = 0.5f * (localPointB1 + localPointB2); - b2Vec2 pointB = b2Mul(xfB, m_localPoint); - - b2Vec2 localPointA = proxyA->GetVertex(cache->indexA[0]); - b2Vec2 pointA = b2Mul(xfA, localPointA); - - float s = b2Dot(pointA - pointB, normal); - if (s < 0.0f) - { - m_axis = -m_axis; - s = -s; - } - return s; - } - else - { - // Two points on A and one or two points on B. - m_type = e_faceA; - b2Vec2 localPointA1 = m_proxyA->GetVertex(cache->indexA[0]); - b2Vec2 localPointA2 = m_proxyA->GetVertex(cache->indexA[1]); - - m_axis = b2Cross(localPointA2 - localPointA1, 1.0f); - m_axis.Normalize(); - b2Vec2 normal = b2Mul(xfA.q, m_axis); - - m_localPoint = 0.5f * (localPointA1 + localPointA2); - b2Vec2 pointA = b2Mul(xfA, m_localPoint); - - b2Vec2 localPointB = m_proxyB->GetVertex(cache->indexB[0]); - b2Vec2 pointB = b2Mul(xfB, localPointB); - - float s = b2Dot(pointB - pointA, normal); - if (s < 0.0f) - { - m_axis = -m_axis; - s = -s; - } - return s; - } - } - - // - float FindMinSeparation(int32* indexA, int32* indexB, float t) const - { - b2Transform xfA, xfB; - m_sweepA.GetTransform(&xfA, t); - m_sweepB.GetTransform(&xfB, t); - - switch (m_type) - { - case e_points: - { - b2Vec2 axisA = b2MulT(xfA.q, m_axis); - b2Vec2 axisB = b2MulT(xfB.q, -m_axis); - - *indexA = m_proxyA->GetSupport(axisA); - *indexB = m_proxyB->GetSupport(axisB); - - b2Vec2 localPointA = m_proxyA->GetVertex(*indexA); - b2Vec2 localPointB = m_proxyB->GetVertex(*indexB); - - b2Vec2 pointA = b2Mul(xfA, localPointA); - b2Vec2 pointB = b2Mul(xfB, localPointB); - - float separation = b2Dot(pointB - pointA, m_axis); - return separation; - } - - case e_faceA: - { - b2Vec2 normal = b2Mul(xfA.q, m_axis); - b2Vec2 pointA = b2Mul(xfA, m_localPoint); - - b2Vec2 axisB = b2MulT(xfB.q, -normal); - - *indexA = -1; - *indexB = m_proxyB->GetSupport(axisB); - - b2Vec2 localPointB = m_proxyB->GetVertex(*indexB); - b2Vec2 pointB = b2Mul(xfB, localPointB); - - float separation = b2Dot(pointB - pointA, normal); - return separation; - } - - case e_faceB: - { - b2Vec2 normal = b2Mul(xfB.q, m_axis); - b2Vec2 pointB = b2Mul(xfB, m_localPoint); - - b2Vec2 axisA = b2MulT(xfA.q, -normal); - - *indexB = -1; - *indexA = m_proxyA->GetSupport(axisA); - - b2Vec2 localPointA = m_proxyA->GetVertex(*indexA); - b2Vec2 pointA = b2Mul(xfA, localPointA); - - float separation = b2Dot(pointA - pointB, normal); - return separation; - } - - default: - b2Assert(false); - *indexA = -1; - *indexB = -1; - return 0.0f; - } - } - - // - float Evaluate(int32 indexA, int32 indexB, float t) const - { - b2Transform xfA, xfB; - m_sweepA.GetTransform(&xfA, t); - m_sweepB.GetTransform(&xfB, t); - - switch (m_type) - { - case e_points: - { - b2Vec2 localPointA = m_proxyA->GetVertex(indexA); - b2Vec2 localPointB = m_proxyB->GetVertex(indexB); - - b2Vec2 pointA = b2Mul(xfA, localPointA); - b2Vec2 pointB = b2Mul(xfB, localPointB); - float separation = b2Dot(pointB - pointA, m_axis); - - return separation; - } - - case e_faceA: - { - b2Vec2 normal = b2Mul(xfA.q, m_axis); - b2Vec2 pointA = b2Mul(xfA, m_localPoint); - - b2Vec2 localPointB = m_proxyB->GetVertex(indexB); - b2Vec2 pointB = b2Mul(xfB, localPointB); - - float separation = b2Dot(pointB - pointA, normal); - return separation; - } - - case e_faceB: - { - b2Vec2 normal = b2Mul(xfB.q, m_axis); - b2Vec2 pointB = b2Mul(xfB, m_localPoint); - - b2Vec2 localPointA = m_proxyA->GetVertex(indexA); - b2Vec2 pointA = b2Mul(xfA, localPointA); - - float separation = b2Dot(pointA - pointB, normal); - return separation; - } - - default: - b2Assert(false); - return 0.0f; - } - } - - const b2DistanceProxy* m_proxyA; - const b2DistanceProxy* m_proxyB; - b2Sweep m_sweepA, m_sweepB; - Type m_type; - b2Vec2 m_localPoint; - b2Vec2 m_axis; -}; - -// CCD via the local separating axis method. This seeks progression -// by computing the largest time at which separation is maintained. -void b2TimeOfImpact(b2TOIOutput* output, const b2TOIInput* input) -{ - b2Timer timer; - - ++b2_toiCalls; - - output->state = b2TOIOutput::e_unknown; - output->t = input->tMax; - - const b2DistanceProxy* proxyA = &input->proxyA; - const b2DistanceProxy* proxyB = &input->proxyB; - - b2Sweep sweepA = input->sweepA; - b2Sweep sweepB = input->sweepB; - - // Large rotations can make the root finder fail, so we normalize the - // sweep angles. - sweepA.Normalize(); - sweepB.Normalize(); - - float tMax = input->tMax; - - float totalRadius = proxyA->m_radius + proxyB->m_radius; - float target = b2Max(b2_linearSlop, totalRadius - 3.0f * b2_linearSlop); - float tolerance = 0.25f * b2_linearSlop; - b2Assert(target > tolerance); - - float t1 = 0.0f; - const int32 k_maxIterations = 20; // TODO_ERIN b2Settings - int32 iter = 0; - - // Prepare input for distance query. - b2SimplexCache cache; - cache.count = 0; - b2DistanceInput distanceInput; - distanceInput.proxyA = input->proxyA; - distanceInput.proxyB = input->proxyB; - distanceInput.useRadii = false; - - // The outer loop progressively attempts to compute new separating axes. - // This loop terminates when an axis is repeated (no progress is made). - for(;;) - { - b2Transform xfA, xfB; - sweepA.GetTransform(&xfA, t1); - sweepB.GetTransform(&xfB, t1); - - // Get the distance between shapes. We can also use the results - // to get a separating axis. - distanceInput.transformA = xfA; - distanceInput.transformB = xfB; - b2DistanceOutput distanceOutput; - b2Distance(&distanceOutput, &cache, &distanceInput); - - // If the shapes are overlapped, we give up on continuous collision. - if (distanceOutput.distance <= 0.0f) - { - // Failure! - output->state = b2TOIOutput::e_overlapped; - output->t = 0.0f; - break; - } - - if (distanceOutput.distance < target + tolerance) - { - // Victory! - output->state = b2TOIOutput::e_touching; - output->t = t1; - break; - } - - // Initialize the separating axis. - b2SeparationFunction fcn; - fcn.Initialize(&cache, proxyA, sweepA, proxyB, sweepB, t1); -#if 0 - // Dump the curve seen by the root finder - { - const int32 N = 100; - float dx = 1.0f / N; - float xs[N+1]; - float fs[N+1]; - - float x = 0.0f; - - for (int32 i = 0; i <= N; ++i) - { - sweepA.GetTransform(&xfA, x); - sweepB.GetTransform(&xfB, x); - float f = fcn.Evaluate(xfA, xfB) - target; - - printf("%g %g\n", x, f); - - xs[i] = x; - fs[i] = f; - - x += dx; - } - } -#endif - - // Compute the TOI on the separating axis. We do this by successively - // resolving the deepest point. This loop is bounded by the number of vertices. - bool done = false; - float t2 = tMax; - int32 pushBackIter = 0; - for (;;) - { - // Find the deepest point at t2. Store the witness point indices. - int32 indexA, indexB; - float s2 = fcn.FindMinSeparation(&indexA, &indexB, t2); - - // Is the final configuration separated? - if (s2 > target + tolerance) - { - // Victory! - output->state = b2TOIOutput::e_separated; - output->t = tMax; - done = true; - break; - } - - // Has the separation reached tolerance? - if (s2 > target - tolerance) - { - // Advance the sweeps - t1 = t2; - break; - } - - // Compute the initial separation of the witness points. - float s1 = fcn.Evaluate(indexA, indexB, t1); - - // Check for initial overlap. This might happen if the root finder - // runs out of iterations. - if (s1 < target - tolerance) - { - output->state = b2TOIOutput::e_failed; - output->t = t1; - done = true; - break; - } - - // Check for touching - if (s1 <= target + tolerance) - { - // Victory! t1 should hold the TOI (could be 0.0). - output->state = b2TOIOutput::e_touching; - output->t = t1; - done = true; - break; - } - - // Compute 1D root of: f(x) - target = 0 - int32 rootIterCount = 0; - float a1 = t1, a2 = t2; - for (;;) - { - // Use a mix of the secant rule and bisection. - float t; - if (rootIterCount & 1) - { - // Secant rule to improve convergence. - t = a1 + (target - s1) * (a2 - a1) / (s2 - s1); - } - else - { - // Bisection to guarantee progress. - t = 0.5f * (a1 + a2); - } - - ++rootIterCount; - ++b2_toiRootIters; - - float s = fcn.Evaluate(indexA, indexB, t); - - if (b2Abs(s - target) < tolerance) - { - // t2 holds a tentative value for t1 - t2 = t; - break; - } - - // Ensure we continue to bracket the root. - if (s > target) - { - a1 = t; - s1 = s; - } - else - { - a2 = t; - s2 = s; - } - - if (rootIterCount == 50) - { - break; - } - } - - b2_toiMaxRootIters = b2Max(b2_toiMaxRootIters, rootIterCount); - - ++pushBackIter; - - if (pushBackIter == b2_maxPolygonVertices) - { - break; - } - } - - ++iter; - ++b2_toiIters; - - if (done) - { - break; - } - - if (iter == k_maxIterations) - { - // Root finder got stuck. Semi-victory. - output->state = b2TOIOutput::e_failed; - output->t = t1; - break; - } - } - - b2_toiMaxIters = b2Max(b2_toiMaxIters, iter); - - float time = timer.GetMilliseconds(); - b2_toiMaxTime = b2Max(b2_toiMaxTime, time); - b2_toiTime += time; -} diff --git a/3rdparty/box2d/src/common/b2_block_allocator.cpp b/3rdparty/box2d/src/common/b2_block_allocator.cpp deleted file mode 100644 index 595f2ad393a1..000000000000 --- a/3rdparty/box2d/src/common/b2_block_allocator.cpp +++ /dev/null @@ -1,230 +0,0 @@ -// MIT License - -// Copyright (c) 2019 Erin Catto - -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: - -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. - -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -#include "box2d/b2_block_allocator.h" -#include -#include -#include - -static const int32 b2_chunkSize = 16 * 1024; -static const int32 b2_maxBlockSize = 640; -static const int32 b2_chunkArrayIncrement = 128; - -// These are the supported object sizes. Actual allocations are rounded up the next size. -static const int32 b2_blockSizes[b2_blockSizeCount] = -{ - 16, // 0 - 32, // 1 - 64, // 2 - 96, // 3 - 128, // 4 - 160, // 5 - 192, // 6 - 224, // 7 - 256, // 8 - 320, // 9 - 384, // 10 - 448, // 11 - 512, // 12 - 640, // 13 -}; - -// This maps an arbitrary allocation size to a suitable slot in b2_blockSizes. -struct b2SizeMap -{ - b2SizeMap() - { - int32 j = 0; - values[0] = 0; - for (int32 i = 1; i <= b2_maxBlockSize; ++i) - { - b2Assert(j < b2_blockSizeCount); - if (i <= b2_blockSizes[j]) - { - values[i] = (uint8)j; - } - else - { - ++j; - values[i] = (uint8)j; - } - } - } - - uint8 values[b2_maxBlockSize + 1]; -}; - -static const b2SizeMap b2_sizeMap; - -struct b2Chunk -{ - int32 blockSize; - b2Block* blocks; -}; - -struct b2Block -{ - b2Block* next; -}; - -b2BlockAllocator::b2BlockAllocator() -{ - b2Assert(b2_blockSizeCount < UCHAR_MAX); - - m_chunkSpace = b2_chunkArrayIncrement; - m_chunkCount = 0; - m_chunks = (b2Chunk*)b2Alloc(m_chunkSpace * sizeof(b2Chunk)); - - memset(m_chunks, 0, m_chunkSpace * sizeof(b2Chunk)); - memset(m_freeLists, 0, sizeof(m_freeLists)); -} - -b2BlockAllocator::~b2BlockAllocator() -{ - for (int32 i = 0; i < m_chunkCount; ++i) - { - b2Free(m_chunks[i].blocks); - } - - b2Free(m_chunks); -} - -void* b2BlockAllocator::Allocate(int32 size) -{ - if (size == 0) - { - return nullptr; - } - - b2Assert(0 < size); - - if (size > b2_maxBlockSize) - { - return b2Alloc(size); - } - - int32 index = b2_sizeMap.values[size]; - b2Assert(0 <= index && index < b2_blockSizeCount); - - if (m_freeLists[index]) - { - b2Block* block = m_freeLists[index]; - m_freeLists[index] = block->next; - return block; - } - else - { - if (m_chunkCount == m_chunkSpace) - { - b2Chunk* oldChunks = m_chunks; - m_chunkSpace += b2_chunkArrayIncrement; - m_chunks = (b2Chunk*)b2Alloc(m_chunkSpace * sizeof(b2Chunk)); - memcpy(m_chunks, oldChunks, m_chunkCount * sizeof(b2Chunk)); - memset(m_chunks + m_chunkCount, 0, b2_chunkArrayIncrement * sizeof(b2Chunk)); - b2Free(oldChunks); - } - - b2Chunk* chunk = m_chunks + m_chunkCount; - chunk->blocks = (b2Block*)b2Alloc(b2_chunkSize); -#if defined(_DEBUG) - memset(chunk->blocks, 0xcd, b2_chunkSize); -#endif - int32 blockSize = b2_blockSizes[index]; - chunk->blockSize = blockSize; - int32 blockCount = b2_chunkSize / blockSize; - b2Assert(blockCount * blockSize <= b2_chunkSize); - for (int32 i = 0; i < blockCount - 1; ++i) - { - b2Block* block = (b2Block*)((int8*)chunk->blocks + blockSize * i); - b2Block* next = (b2Block*)((int8*)chunk->blocks + blockSize * (i + 1)); - block->next = next; - } - b2Block* last = (b2Block*)((int8*)chunk->blocks + blockSize * (blockCount - 1)); - last->next = nullptr; - - m_freeLists[index] = chunk->blocks->next; - ++m_chunkCount; - - return chunk->blocks; - } -} - -void b2BlockAllocator::Free(void* p, int32 size) -{ - if (size == 0) - { - return; - } - - b2Assert(0 < size); - - if (size > b2_maxBlockSize) - { - b2Free(p); - return; - } - - int32 index = b2_sizeMap.values[size]; - b2Assert(0 <= index && index < b2_blockSizeCount); - -#if defined(_DEBUG) - // Verify the memory address and size is valid. - int32 blockSize = b2_blockSizes[index]; - bool found = false; - for (int32 i = 0; i < m_chunkCount; ++i) - { - b2Chunk* chunk = m_chunks + i; - if (chunk->blockSize != blockSize) - { - b2Assert( (int8*)p + blockSize <= (int8*)chunk->blocks || - (int8*)chunk->blocks + b2_chunkSize <= (int8*)p); - } - else - { - if ((int8*)chunk->blocks <= (int8*)p && (int8*)p + blockSize <= (int8*)chunk->blocks + b2_chunkSize) - { - found = true; - } - } - } - - b2Assert(found); - - memset(p, 0xfd, blockSize); -#endif - - b2Block* block = (b2Block*)p; - block->next = m_freeLists[index]; - m_freeLists[index] = block; -} - -void b2BlockAllocator::Clear() -{ - for (int32 i = 0; i < m_chunkCount; ++i) - { - b2Free(m_chunks[i].blocks); - } - - m_chunkCount = 0; - memset(m_chunks, 0, m_chunkSpace * sizeof(b2Chunk)); - memset(m_freeLists, 0, sizeof(m_freeLists)); -} diff --git a/3rdparty/box2d/src/common/b2_draw.cpp b/3rdparty/box2d/src/common/b2_draw.cpp deleted file mode 100644 index 1ec11e54aca3..000000000000 --- a/3rdparty/box2d/src/common/b2_draw.cpp +++ /dev/null @@ -1,47 +0,0 @@ -// MIT License - -// Copyright (c) 2019 Erin Catto - -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: - -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. - -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. -#include "box2d/b2_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/3rdparty/box2d/src/common/b2_math.cpp b/3rdparty/box2d/src/common/b2_math.cpp deleted file mode 100644 index a14460cd501e..000000000000 --- a/3rdparty/box2d/src/common/b2_math.cpp +++ /dev/null @@ -1,98 +0,0 @@ -// MIT License - -// Copyright (c) 2019 Erin Catto - -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: - -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. - -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -#include "box2d/b2_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/3rdparty/box2d/src/common/b2_settings.cpp b/3rdparty/box2d/src/common/b2_settings.cpp deleted file mode 100644 index dde28bb3abd7..000000000000 --- a/3rdparty/box2d/src/common/b2_settings.cpp +++ /dev/null @@ -1,74 +0,0 @@ -// MIT License - -// Copyright (c) 2019 Erin Catto - -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: - -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. - -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -#define _CRT_SECURE_NO_WARNINGS - -#include "box2d/b2_settings.h" -#include -#include -#include - -b2Version b2_version = {2, 4, 1}; - -// Memory allocators. Modify these to use your own allocator. -void* b2Alloc_Default(int32 size) -{ - return malloc(size); -} - -void b2Free_Default(void* mem) -{ - free(mem); -} - -// You can modify this to use your logging facility. -void b2Log_Default(const char* string, va_list args) -{ - vprintf(string, args); -} - -FILE* b2_dumpFile = nullptr; - -void b2OpenDump(const char* fileName) -{ - b2Assert(b2_dumpFile == nullptr); - b2_dumpFile = fopen(fileName, "w"); -} - -void b2Dump(const char* string, ...) -{ - if (b2_dumpFile == nullptr) - { - return; - } - - va_list args; - va_start(args, string); - vfprintf(b2_dumpFile, string, args); - va_end(args); -} - -void b2CloseDump() -{ - fclose(b2_dumpFile); - b2_dumpFile = nullptr; -} diff --git a/3rdparty/box2d/src/common/b2_stack_allocator.cpp b/3rdparty/box2d/src/common/b2_stack_allocator.cpp deleted file mode 100644 index 602db1a00494..000000000000 --- a/3rdparty/box2d/src/common/b2_stack_allocator.cpp +++ /dev/null @@ -1,87 +0,0 @@ -// MIT License - -// Copyright (c) 2019 Erin Catto - -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: - -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. - -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -#include "box2d/b2_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/3rdparty/box2d/src/common/b2_timer.cpp b/3rdparty/box2d/src/common/b2_timer.cpp deleted file mode 100644 index dd7cde7f4709..000000000000 --- a/3rdparty/box2d/src/common/b2_timer.cpp +++ /dev/null @@ -1,125 +0,0 @@ -// MIT License - -// Copyright (c) 2019 Erin Catto - -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: - -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. - -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -#include "box2d/b2_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/3rdparty/box2d/src/constraint_graph.c b/3rdparty/box2d/src/constraint_graph.c new file mode 100644 index 000000000000..7f8e1ee1405a --- /dev/null +++ b/3rdparty/box2d/src/constraint_graph.c @@ -0,0 +1,318 @@ +// SPDX-FileCopyrightText: 2023 Erin Catto +// SPDX-License-Identifier: MIT + +#include "constraint_graph.h" + +#include "array.h" +#include "bitset.h" +#include "body.h" +#include "contact.h" +#include "joint.h" +#include "solver_set.h" +#include "world.h" + +#include + +// Solver using graph coloring. Islands are only used for sleep. +// High-Performance Physical Simulations on Next-Generation Architecture with Many Cores +// http://web.eecs.umich.edu/~msmelyan/papers/physsim_onmanycore_itj.pdf + +// Kinematic bodies have to be treated like dynamic bodies in graph coloring. Unlike static bodies, we cannot use a dummy solver +// body for kinematic bodies. We cannot access a kinematic body from multiple threads efficiently because the SIMD solver body +// scatter would write to the same kinematic body from multiple threads. Even if these writes don't modify the body, they will +// cause horrible cache stalls. To make this feasible I would need a way to block these writes. + +// This is used for debugging by making all constraints be assigned to overflow. +#define B2_FORCE_OVERFLOW 0 + +_Static_assert( b2_graphColorCount == 12, "graph color count assumed to be 12" ); + +void b2CreateGraph( b2ConstraintGraph* graph, int bodyCapacity ) +{ + _Static_assert( b2_graphColorCount >= 2, "must have at least two constraint graph colors" ); + _Static_assert( b2_overflowIndex == b2_graphColorCount - 1, "bad over flow index" ); + + *graph = ( b2ConstraintGraph ){ 0 }; + + bodyCapacity = b2MaxInt( bodyCapacity, 8 ); + + // Initialize graph color bit set. + // No bitset for overflow color. + for ( int i = 0; i < b2_overflowIndex; ++i ) + { + b2GraphColor* color = graph->colors + i; + color->bodySet = b2CreateBitSet( bodyCapacity ); + b2SetBitCountAndClear( &color->bodySet, bodyCapacity ); + } +} + +void b2DestroyGraph( b2ConstraintGraph* graph ) +{ + for ( int i = 0; i < b2_graphColorCount; ++i ) + { + b2GraphColor* color = graph->colors + i; + + // The bit set should never be used on the overflow color + B2_ASSERT( i != b2_overflowIndex || color->bodySet.bits == NULL ); + + b2DestroyBitSet( &color->bodySet ); + + b2ContactSimArray_Destroy( &color->contactSims ); + b2JointSimArray_Destroy( &color->jointSims ); + } +} + +// Contacts are always created as non-touching. They get cloned into the constraint +// graph once they are found to be touching. +// todo maybe kinematic bodies should not go into graph +void b2AddContactToGraph( b2World* world, b2ContactSim* contactSim, b2Contact* contact ) +{ + B2_ASSERT( contactSim->manifold.pointCount > 0 ); + B2_ASSERT( contactSim->simFlags & b2_simTouchingFlag ); + B2_ASSERT( contact->flags & b2_contactTouchingFlag ); + + b2ConstraintGraph* graph = &world->constraintGraph; + int colorIndex = b2_overflowIndex; + + int bodyIdA = contact->edges[0].bodyId; + int bodyIdB = contact->edges[1].bodyId; + b2Body* bodyA = b2BodyArray_Get( &world->bodies, bodyIdA ); + b2Body* bodyB = b2BodyArray_Get( &world->bodies, bodyIdB ); + bool staticA = bodyA->setIndex == b2_staticSet; + bool staticB = bodyB->setIndex == b2_staticSet; + B2_ASSERT( staticA == false || staticB == false ); + +#if B2_FORCE_OVERFLOW == 0 + if ( staticA == false && staticB == false ) + { + for ( int i = 0; i < b2_overflowIndex; ++i ) + { + b2GraphColor* color = graph->colors + i; + if ( b2GetBit( &color->bodySet, bodyIdA ) || b2GetBit( &color->bodySet, bodyIdB ) ) + { + continue; + } + + b2SetBitGrow( &color->bodySet, bodyIdA ); + b2SetBitGrow( &color->bodySet, bodyIdB ); + colorIndex = i; + break; + } + } + else if ( staticA == false ) + { + // No static contacts in color 0 + for ( int i = 1; i < b2_overflowIndex; ++i ) + { + b2GraphColor* color = graph->colors + i; + if ( b2GetBit( &color->bodySet, bodyIdA ) ) + { + continue; + } + + b2SetBitGrow( &color->bodySet, bodyIdA ); + colorIndex = i; + break; + } + } + else if ( staticB == false ) + { + // No static contacts in color 0 + for ( int i = 1; i < b2_overflowIndex; ++i ) + { + b2GraphColor* color = graph->colors + i; + if ( b2GetBit( &color->bodySet, bodyIdB ) ) + { + continue; + } + + b2SetBitGrow( &color->bodySet, bodyIdB ); + colorIndex = i; + break; + } + } +#endif + + b2GraphColor* color = graph->colors + colorIndex; + contact->colorIndex = colorIndex; + contact->localIndex = color->contactSims.count; + + b2ContactSim* newContact = b2ContactSimArray_Add( &color->contactSims ); + memcpy( newContact, contactSim, sizeof( b2ContactSim ) ); + + // todo perhaps skip this if the contact is already awake + + if ( staticA ) + { + newContact->bodySimIndexA = B2_NULL_INDEX; + newContact->invMassA = 0.0f; + newContact->invIA = 0.0f; + } + else + { + B2_ASSERT( bodyA->setIndex == b2_awakeSet ); + b2SolverSet* awakeSet = b2SolverSetArray_Get( &world->solverSets, b2_awakeSet ); + + int localIndex = bodyA->localIndex; + newContact->bodySimIndexA = localIndex; + + b2BodySim* bodySimA = b2BodySimArray_Get( &awakeSet->bodySims, localIndex ); + newContact->invMassA = bodySimA->invMass; + newContact->invIA = bodySimA->invInertia; + } + + if ( staticB ) + { + newContact->bodySimIndexB = B2_NULL_INDEX; + newContact->invMassB = 0.0f; + newContact->invIB = 0.0f; + } + else + { + B2_ASSERT( bodyB->setIndex == b2_awakeSet ); + b2SolverSet* awakeSet = b2SolverSetArray_Get( &world->solverSets, b2_awakeSet ); + + int localIndex = bodyB->localIndex; + newContact->bodySimIndexB = localIndex; + + b2BodySim* bodySimB = b2BodySimArray_Get( &awakeSet->bodySims, localIndex ); + newContact->invMassB = bodySimB->invMass; + newContact->invIB = bodySimB->invInertia; + } +} + +void b2RemoveContactFromGraph( b2World* world, int bodyIdA, int bodyIdB, int colorIndex, int localIndex ) +{ + b2ConstraintGraph* graph = &world->constraintGraph; + + B2_ASSERT( 0 <= colorIndex && colorIndex < b2_graphColorCount ); + b2GraphColor* color = graph->colors + colorIndex; + + if ( colorIndex != b2_overflowIndex ) + { + // might clear a bit for a static body, but this has no effect + b2ClearBit( &color->bodySet, bodyIdA ); + b2ClearBit( &color->bodySet, bodyIdB ); + } + + int movedIndex = b2ContactSimArray_RemoveSwap( &color->contactSims, localIndex ); + if ( movedIndex != B2_NULL_INDEX ) + { + // Fix index on swapped contact + b2ContactSim* movedContactSim = color->contactSims.data + localIndex; + + // Fix moved contact + int movedId = movedContactSim->contactId; + b2Contact* movedContact = b2ContactArray_Get( &world->contacts, movedId ); + B2_ASSERT( movedContact->setIndex == b2_awakeSet ); + B2_ASSERT( movedContact->colorIndex == colorIndex ); + B2_ASSERT( movedContact->localIndex == movedIndex ); + movedContact->localIndex = localIndex; + } +} + +static int b2AssignJointColor( b2ConstraintGraph* graph, int bodyIdA, int bodyIdB, bool staticA, bool staticB ) +{ + B2_ASSERT( staticA == false || staticB == false ); + +#if B2_FORCE_OVERFLOW == 0 + if ( staticA == false && staticB == false ) + { + for ( int i = 0; i < b2_overflowIndex; ++i ) + { + b2GraphColor* color = graph->colors + i; + if ( b2GetBit( &color->bodySet, bodyIdA ) || b2GetBit( &color->bodySet, bodyIdB ) ) + { + continue; + } + + b2SetBitGrow( &color->bodySet, bodyIdA ); + b2SetBitGrow( &color->bodySet, bodyIdB ); + return i; + } + } + else if ( staticA == false ) + { + for ( int i = 0; i < b2_overflowIndex; ++i ) + { + b2GraphColor* color = graph->colors + i; + if ( b2GetBit( &color->bodySet, bodyIdA ) ) + { + continue; + } + + b2SetBitGrow( &color->bodySet, bodyIdA ); + return i; + } + } + else if ( staticB == false ) + { + for ( int i = 0; i < b2_overflowIndex; ++i ) + { + b2GraphColor* color = graph->colors + i; + if ( b2GetBit( &color->bodySet, bodyIdB ) ) + { + continue; + } + + b2SetBitGrow( &color->bodySet, bodyIdB ); + return i; + } + } +#endif + + return b2_overflowIndex; +} + +b2JointSim* b2CreateJointInGraph( b2World* world, b2Joint* joint ) +{ + b2ConstraintGraph* graph = &world->constraintGraph; + + int bodyIdA = joint->edges[0].bodyId; + int bodyIdB = joint->edges[1].bodyId; + b2Body* bodyA = b2BodyArray_Get( &world->bodies, bodyIdA ); + b2Body* bodyB = b2BodyArray_Get( &world->bodies, bodyIdB ); + bool staticA = bodyA->setIndex == b2_staticSet; + bool staticB = bodyB->setIndex == b2_staticSet; + + int colorIndex = b2AssignJointColor( graph, bodyIdA, bodyIdB, staticA, staticB ); + + b2JointSim* jointSim = b2JointSimArray_Add( &graph->colors[colorIndex].jointSims ); + joint->colorIndex = colorIndex; + joint->localIndex = graph->colors[colorIndex].jointSims.count - 1; + return jointSim; +} + +void b2AddJointToGraph( b2World* world, b2JointSim* jointSim, b2Joint* joint ) +{ + b2JointSim* jointDst = b2CreateJointInGraph( world, joint ); + memcpy( jointDst, jointSim, sizeof( b2JointSim ) ); +} + +void b2RemoveJointFromGraph( b2World* world, int bodyIdA, int bodyIdB, int colorIndex, int localIndex ) +{ + b2ConstraintGraph* graph = &world->constraintGraph; + + B2_ASSERT( 0 <= colorIndex && colorIndex < b2_graphColorCount ); + b2GraphColor* color = graph->colors + colorIndex; + + if ( colorIndex != b2_overflowIndex ) + { + // May clear static bodies, no effect + b2ClearBit( &color->bodySet, bodyIdA ); + b2ClearBit( &color->bodySet, bodyIdB ); + } + + int movedIndex = b2JointSimArray_RemoveSwap( &color->jointSims, localIndex ); + if ( movedIndex != B2_NULL_INDEX ) + { + // Fix moved joint + b2JointSim* movedJointSim = color->jointSims.data + localIndex; + int movedId = movedJointSim->jointId; + b2Joint* movedJoint = b2JointArray_Get( &world->joints, movedId ); + B2_ASSERT( movedJoint->setIndex == b2_awakeSet ); + B2_ASSERT( movedJoint->colorIndex == colorIndex ); + B2_ASSERT( movedJoint->localIndex == movedIndex ); + movedJoint->localIndex = localIndex; + } +} diff --git a/3rdparty/box2d/src/constraint_graph.h b/3rdparty/box2d/src/constraint_graph.h new file mode 100644 index 000000000000..36b1961c0a5d --- /dev/null +++ b/3rdparty/box2d/src/constraint_graph.h @@ -0,0 +1,56 @@ +// SPDX-FileCopyrightText: 2023 Erin Catto +// SPDX-License-Identifier: MIT + +#pragma once + +#include "array.h" +#include "bitset.h" + +typedef struct b2Body b2Body; +typedef struct b2ContactSim b2ContactSim; +typedef struct b2Contact b2Contact; +typedef struct b2ContactConstraint b2ContactConstraint; +typedef struct b2ContactConstraintSIMD b2ContactConstraintSIMD; +typedef struct b2JointSim b2JointSim; +typedef struct b2Joint b2Joint; +typedef struct b2StepContext b2StepContext; +typedef struct b2World b2World; + +// This holds constraints that cannot fit the graph color limit. This happens when a single dynamic body +// is touching many other bodies. +#define b2_overflowIndex b2_graphColorCount - 1 + +typedef struct b2GraphColor +{ + // This bitset is indexed by bodyId so this is over-sized to encompass static bodies + // however I never traverse these bits or use the bit count for anything + // This bitset is unused on the overflow color. + b2BitSet bodySet; + + // cache friendly arrays + b2ContactSimArray contactSims; + b2JointSimArray jointSims; + + // transient + union + { + b2ContactConstraintSIMD* simdConstraints; + b2ContactConstraint* overflowConstraints; + }; +} b2GraphColor; + +typedef struct b2ConstraintGraph +{ + // including overflow at the end + b2GraphColor colors[b2_graphColorCount]; +} b2ConstraintGraph; + +void b2CreateGraph( b2ConstraintGraph* graph, int bodyCapacity ); +void b2DestroyGraph( b2ConstraintGraph* graph ); + +void b2AddContactToGraph( b2World* world, b2ContactSim* contactSim, b2Contact* contact ); +void b2RemoveContactFromGraph( b2World* world, int bodyIdA, int bodyIdB, int colorIndex, int localIndex ); + +b2JointSim* b2CreateJointInGraph( b2World* world, b2Joint* joint ); +void b2AddJointToGraph( b2World* world, b2JointSim* jointSim, b2Joint* joint ); +void b2RemoveJointFromGraph( b2World* world, int bodyIdA, int bodyIdB, int colorIndex, int localIndex ); diff --git a/3rdparty/box2d/src/contact.c b/3rdparty/box2d/src/contact.c new file mode 100644 index 000000000000..24049b7cb9ae --- /dev/null +++ b/3rdparty/box2d/src/contact.c @@ -0,0 +1,584 @@ +// SPDX-FileCopyrightText: 2023 Erin Catto +// SPDX-License-Identifier: MIT + +#include "contact.h" + +#include "array.h" +#include "body.h" +#include "core.h" +#include "island.h" +#include "shape.h" +#include "solver_set.h" +#include "table.h" +#include "world.h" + +#include "box2d/collision.h" + +#include +#include +#include + +B2_ARRAY_SOURCE( b2Contact, b2Contact ); +B2_ARRAY_SOURCE( b2ContactSim, b2ContactSim ); + +// Contacts and determinism +// A deterministic simulation requires contacts to exist in the same order in b2Island no matter the thread count. +// The order must reproduce from run to run. This is necessary because the Gauss-Seidel constraint solver is order dependent. +// +// Creation: +// - Contacts are created using results from b2UpdateBroadPhasePairs +// - These results are ordered according to the order of the broad-phase move array +// - The move array is ordered according to the shape creation order using a bitset. +// - The island/shape/body order is determined by creation order +// - Logically contacts are only created for awake bodies, so they are immediately added to the awake contact array (serially) +// +// Island linking: +// - The awake contact array is built from the body-contact graph for all awake bodies in awake islands. +// - Awake contacts are solved in parallel and they generate contact state changes. +// - These state changes may link islands together using union find. +// - The state changes are ordered using a bit array that encompasses all contacts +// - As long as contacts are created in deterministic order, island link order is deterministic. +// - This keeps the order of contacts in islands deterministic + +// Friction mixing law. The idea is to allow either shape to drive the friction to zero. +// For example, anything slides on ice. +static inline float b2MixFriction( float friction1, float friction2 ) +{ + return sqrtf( friction1 * friction2 ); +} + +// Restitution mixing law. The idea is allow for anything to bounce off an inelastic surface. +// For example, a superball bounces on anything. +static inline float b2MixRestitution( float restitution1, float restitution2 ) +{ + return restitution1 > restitution2 ? restitution1 : restitution2; +} + +// todo make relative for all +// typedef b2Manifold b2ManifoldFcn(const b2Shape* shapeA, const b2Shape* shapeB, b2Transform xfB, b2DistanceCache* cache); +typedef b2Manifold b2ManifoldFcn( const b2Shape* shapeA, b2Transform xfA, const b2Shape* shapeB, b2Transform xfB, + b2DistanceCache* cache ); + +struct b2ContactRegister +{ + b2ManifoldFcn* fcn; + bool primary; +}; + +static struct b2ContactRegister s_registers[b2_shapeTypeCount][b2_shapeTypeCount]; +static bool s_initialized = false; + +static b2Manifold b2CircleManifold( const b2Shape* shapeA, b2Transform xfA, const b2Shape* shapeB, b2Transform xfB, + b2DistanceCache* cache ) +{ + B2_MAYBE_UNUSED( cache ); + return b2CollideCircles( &shapeA->circle, xfA, &shapeB->circle, xfB ); +} + +static b2Manifold b2CapsuleAndCircleManifold( const b2Shape* shapeA, b2Transform xfA, const b2Shape* shapeB, b2Transform xfB, + b2DistanceCache* cache ) +{ + B2_MAYBE_UNUSED( cache ); + return b2CollideCapsuleAndCircle( &shapeA->capsule, xfA, &shapeB->circle, xfB ); +} + +static b2Manifold b2CapsuleManifold( const b2Shape* shapeA, b2Transform xfA, const b2Shape* shapeB, b2Transform xfB, + b2DistanceCache* cache ) +{ + B2_MAYBE_UNUSED( cache ); + return b2CollideCapsules( &shapeA->capsule, xfA, &shapeB->capsule, xfB ); +} + +static b2Manifold b2PolygonAndCircleManifold( const b2Shape* shapeA, b2Transform xfA, const b2Shape* shapeB, b2Transform xfB, + b2DistanceCache* cache ) +{ + B2_MAYBE_UNUSED( cache ); + return b2CollidePolygonAndCircle( &shapeA->polygon, xfA, &shapeB->circle, xfB ); +} + +static b2Manifold b2PolygonAndCapsuleManifold( const b2Shape* shapeA, b2Transform xfA, const b2Shape* shapeB, b2Transform xfB, + b2DistanceCache* cache ) +{ + B2_MAYBE_UNUSED( cache ); + return b2CollidePolygonAndCapsule( &shapeA->polygon, xfA, &shapeB->capsule, xfB ); +} + +static b2Manifold b2PolygonManifold( const b2Shape* shapeA, b2Transform xfA, const b2Shape* shapeB, b2Transform xfB, + b2DistanceCache* cache ) +{ + B2_MAYBE_UNUSED( cache ); + return b2CollidePolygons( &shapeA->polygon, xfA, &shapeB->polygon, xfB ); +} + +static b2Manifold b2SegmentAndCircleManifold( const b2Shape* shapeA, b2Transform xfA, const b2Shape* shapeB, b2Transform xfB, + b2DistanceCache* cache ) +{ + B2_MAYBE_UNUSED( cache ); + return b2CollideSegmentAndCircle( &shapeA->segment, xfA, &shapeB->circle, xfB ); +} + +static b2Manifold b2SegmentAndCapsuleManifold( const b2Shape* shapeA, b2Transform xfA, const b2Shape* shapeB, b2Transform xfB, + b2DistanceCache* cache ) +{ + B2_MAYBE_UNUSED( cache ); + return b2CollideSegmentAndCapsule( &shapeA->segment, xfA, &shapeB->capsule, xfB ); +} + +static b2Manifold b2SegmentAndPolygonManifold( const b2Shape* shapeA, b2Transform xfA, const b2Shape* shapeB, b2Transform xfB, + b2DistanceCache* cache ) +{ + B2_MAYBE_UNUSED( cache ); + return b2CollideSegmentAndPolygon( &shapeA->segment, xfA, &shapeB->polygon, xfB ); +} + +static b2Manifold b2ChainSegmentAndCircleManifold( const b2Shape* shapeA, b2Transform xfA, const b2Shape* shapeB, b2Transform xfB, + b2DistanceCache* cache ) +{ + B2_MAYBE_UNUSED( cache ); + return b2CollideChainSegmentAndCircle( &shapeA->chainSegment, xfA, &shapeB->circle, xfB ); +} + +static b2Manifold b2ChainSegmentAndCapsuleManifold( const b2Shape* shapeA, b2Transform xfA, const b2Shape* shapeB, + b2Transform xfB, b2DistanceCache* cache ) +{ + return b2CollideChainSegmentAndCapsule( &shapeA->chainSegment, xfA, &shapeB->capsule, xfB, cache ); +} + +static b2Manifold b2ChainSegmentAndPolygonManifold( const b2Shape* shapeA, b2Transform xfA, const b2Shape* shapeB, + b2Transform xfB, b2DistanceCache* cache ) +{ + return b2CollideChainSegmentAndPolygon( &shapeA->chainSegment, xfA, &shapeB->polygon, xfB, cache ); +} + +static void b2AddType( b2ManifoldFcn* fcn, b2ShapeType type1, b2ShapeType type2 ) +{ + B2_ASSERT( 0 <= type1 && type1 < b2_shapeTypeCount ); + B2_ASSERT( 0 <= type2 && type2 < b2_shapeTypeCount ); + + s_registers[type1][type2].fcn = fcn; + s_registers[type1][type2].primary = true; + + if ( type1 != type2 ) + { + s_registers[type2][type1].fcn = fcn; + s_registers[type2][type1].primary = false; + } +} + +void b2InitializeContactRegisters( void ) +{ + if ( s_initialized == false ) + { + b2AddType( b2CircleManifold, b2_circleShape, b2_circleShape ); + b2AddType( b2CapsuleAndCircleManifold, b2_capsuleShape, b2_circleShape ); + b2AddType( b2CapsuleManifold, b2_capsuleShape, b2_capsuleShape ); + b2AddType( b2PolygonAndCircleManifold, b2_polygonShape, b2_circleShape ); + b2AddType( b2PolygonAndCapsuleManifold, b2_polygonShape, b2_capsuleShape ); + b2AddType( b2PolygonManifold, b2_polygonShape, b2_polygonShape ); + b2AddType( b2SegmentAndCircleManifold, b2_segmentShape, b2_circleShape ); + b2AddType( b2SegmentAndCapsuleManifold, b2_segmentShape, b2_capsuleShape ); + b2AddType( b2SegmentAndPolygonManifold, b2_segmentShape, b2_polygonShape ); + b2AddType( b2ChainSegmentAndCircleManifold, b2_chainSegmentShape, b2_circleShape ); + b2AddType( b2ChainSegmentAndCapsuleManifold, b2_chainSegmentShape, b2_capsuleShape ); + b2AddType( b2ChainSegmentAndPolygonManifold, b2_chainSegmentShape, b2_polygonShape ); + s_initialized = true; + } +} + +void b2CreateContact( b2World* world, b2Shape* shapeA, b2Shape* shapeB ) +{ + b2ShapeType type1 = shapeA->type; + b2ShapeType type2 = shapeB->type; + + B2_ASSERT( 0 <= type1 && type1 < b2_shapeTypeCount ); + B2_ASSERT( 0 <= type2 && type2 < b2_shapeTypeCount ); + + if ( s_registers[type1][type2].fcn == NULL ) + { + // For example, no segment vs segment collision + return; + } + + if ( s_registers[type1][type2].primary == false ) + { + // flip order + b2CreateContact( world, shapeB, shapeA ); + return; + } + + b2Body* bodyA = b2BodyArray_Get( &world->bodies, shapeA->bodyId ); + b2Body* bodyB = b2BodyArray_Get( &world->bodies, shapeB->bodyId ); + + B2_ASSERT( bodyA->setIndex != b2_disabledSet && bodyB->setIndex != b2_disabledSet ); + B2_ASSERT( bodyA->setIndex != b2_staticSet || bodyB->setIndex != b2_staticSet ); + + int setIndex; + if ( bodyA->setIndex == b2_awakeSet || bodyB->setIndex == b2_awakeSet ) + { + setIndex = b2_awakeSet; + } + else + { + // sleeping and non-touching contacts live in the disabled set + // later if this set is found to be touching then the sleeping + // islands will be linked and the contact moved to the merged island + setIndex = b2_disabledSet; + } + + b2SolverSet* set = b2SolverSetArray_Get( &world->solverSets, setIndex ); + + // Create contact key and contact + int contactId = b2AllocId( &world->contactIdPool ); + if ( contactId == world->contacts.count ) + { + b2ContactArray_Push( &world->contacts, ( b2Contact ){ 0 } ); + } + + int shapeIdA = shapeA->id; + int shapeIdB = shapeB->id; + + b2Contact* contact = b2ContactArray_Get( &world->contacts, contactId ); + contact->contactId = contactId; + contact->setIndex = setIndex; + contact->colorIndex = B2_NULL_INDEX; + contact->localIndex = set->contactSims.count; + contact->islandId = B2_NULL_INDEX; + contact->islandPrev = B2_NULL_INDEX; + contact->islandNext = B2_NULL_INDEX; + contact->shapeIdA = shapeIdA; + contact->shapeIdB = shapeIdB; + contact->isMarked = false; + contact->flags = 0; + + if ( shapeA->isSensor || shapeB->isSensor ) + { + contact->flags |= b2_contactSensorFlag; + } + + if ( shapeA->enableSensorEvents || shapeB->enableSensorEvents ) + { + contact->flags |= b2_contactEnableSensorEvents; + } + + if ( shapeA->enableContactEvents || shapeB->enableContactEvents ) + { + contact->flags |= b2_contactEnableContactEvents; + } + + // Connect to body A + { + contact->edges[0].bodyId = shapeA->bodyId; + contact->edges[0].prevKey = B2_NULL_INDEX; + contact->edges[0].nextKey = bodyA->headContactKey; + + int keyA = ( contactId << 1 ) | 0; + int headContactKey = bodyA->headContactKey; + if ( headContactKey != B2_NULL_INDEX ) + { + b2Contact* headContact = b2ContactArray_Get( &world->contacts, headContactKey >> 1 ); + headContact->edges[headContactKey & 1].prevKey = keyA; + } + bodyA->headContactKey = keyA; + bodyA->contactCount += 1; + } + + // Connect to body B + { + contact->edges[1].bodyId = shapeB->bodyId; + contact->edges[1].prevKey = B2_NULL_INDEX; + contact->edges[1].nextKey = bodyB->headContactKey; + + int keyB = ( contactId << 1 ) | 1; + int headContactKey = bodyB->headContactKey; + if ( bodyB->headContactKey != B2_NULL_INDEX ) + { + b2Contact* headContact = b2ContactArray_Get( &world->contacts, headContactKey >> 1 ); + headContact->edges[headContactKey & 1].prevKey = keyB; + } + bodyB->headContactKey = keyB; + bodyB->contactCount += 1; + } + + // Add to pair set for fast lookup + uint64_t pairKey = B2_SHAPE_PAIR_KEY( shapeIdA, shapeIdB ); + b2AddKey( &world->broadPhase.pairSet, pairKey ); + + // Contacts are created as non-touching. Later if they are found to be touching + // they will link islands and be moved into the constraint graph. + b2ContactSim* contactSim = b2ContactSimArray_Add( &set->contactSims ); + contactSim->contactId = contactId; + +#if B2_VALIDATE + contactSim->bodyIdA = shapeA->bodyId; + contactSim->bodyIdB = shapeB->bodyId; +#endif + + contactSim->bodySimIndexA = B2_NULL_INDEX; + contactSim->bodySimIndexB = B2_NULL_INDEX; + contactSim->invMassA = 0.0f; + contactSim->invIA = 0.0f; + contactSim->invMassB = 0.0f; + contactSim->invIB = 0.0f; + contactSim->shapeIdA = shapeIdA; + contactSim->shapeIdB = shapeIdB; + contactSim->cache = b2_emptyDistanceCache; + contactSim->manifold = ( b2Manifold ){ 0 }; + contactSim->friction = b2MixFriction( shapeA->friction, shapeB->friction ); + contactSim->restitution = b2MixRestitution( shapeA->restitution, shapeB->restitution ); + contactSim->tangentSpeed = 0.0f; + contactSim->simFlags = 0; + + if ( shapeA->enablePreSolveEvents || shapeB->enablePreSolveEvents ) + { + contactSim->simFlags |= b2_simEnablePreSolveEvents; + } +} + +// A contact is destroyed when: +// - broad-phase proxies stop overlapping +// - a body is destroyed +// - a body is disabled +// - a body changes type from dynamic to kinematic or static +// - a shape is destroyed +// - contact filtering is modified +// - a shape becomes a sensor (check this!!!) +void b2DestroyContact( b2World* world, b2Contact* contact, bool wakeBodies ) +{ + // Remove pair from set + uint64_t pairKey = B2_SHAPE_PAIR_KEY( contact->shapeIdA, contact->shapeIdB ); + b2RemoveKey( &world->broadPhase.pairSet, pairKey ); + + b2ContactEdge* edgeA = contact->edges + 0; + b2ContactEdge* edgeB = contact->edges + 1; + + int bodyIdA = edgeA->bodyId; + int bodyIdB = edgeB->bodyId; + b2Body* bodyA = b2BodyArray_Get( &world->bodies, bodyIdA ); + b2Body* bodyB = b2BodyArray_Get( &world->bodies, bodyIdB ); + + // if (contactListener && contact->IsTouching()) + //{ + // contactListener->EndContact(contact); + // } + + // Remove from body A + if ( edgeA->prevKey != B2_NULL_INDEX ) + { + b2Contact* prevContact = b2ContactArray_Get( &world->contacts, edgeA->prevKey >> 1 ); + b2ContactEdge* prevEdge = prevContact->edges + ( edgeA->prevKey & 1 ); + prevEdge->nextKey = edgeA->nextKey; + } + + if ( edgeA->nextKey != B2_NULL_INDEX ) + { + b2Contact* nextContact = b2ContactArray_Get( &world->contacts, edgeA->nextKey >> 1 ); + b2ContactEdge* nextEdge = nextContact->edges + ( edgeA->nextKey & 1 ); + nextEdge->prevKey = edgeA->prevKey; + } + + int contactId = contact->contactId; + + int edgeKeyA = ( contactId << 1 ) | 0; + if ( bodyA->headContactKey == edgeKeyA ) + { + bodyA->headContactKey = edgeA->nextKey; + } + + bodyA->contactCount -= 1; + + // Remove from body B + if ( edgeB->prevKey != B2_NULL_INDEX ) + { + b2Contact* prevContact = b2ContactArray_Get( &world->contacts, edgeB->prevKey >> 1 ); + b2ContactEdge* prevEdge = prevContact->edges + ( edgeB->prevKey & 1 ); + prevEdge->nextKey = edgeB->nextKey; + } + + if ( edgeB->nextKey != B2_NULL_INDEX ) + { + b2Contact* nextContact = b2ContactArray_Get( &world->contacts, edgeB->nextKey >> 1 ); + b2ContactEdge* nextEdge = nextContact->edges + ( edgeB->nextKey & 1 ); + nextEdge->prevKey = edgeB->prevKey; + } + + int edgeKeyB = ( contactId << 1 ) | 1; + if ( bodyB->headContactKey == edgeKeyB ) + { + bodyB->headContactKey = edgeB->nextKey; + } + + bodyB->contactCount -= 1; + + // Remove contact from the array that owns it + if ( contact->islandId != B2_NULL_INDEX ) + { + b2UnlinkContact( world, contact ); + } + + if ( contact->colorIndex != B2_NULL_INDEX ) + { + // contact is an active constraint + B2_ASSERT( contact->setIndex == b2_awakeSet ); + b2RemoveContactFromGraph( world, bodyIdA, bodyIdB, contact->colorIndex, contact->localIndex ); + } + else + { + // contact is non-touching or is sleeping or is a sensor + B2_ASSERT( contact->setIndex != b2_awakeSet || ( contact->flags & b2_contactTouchingFlag ) == 0 || + ( contact->flags & b2_contactSensorFlag ) != 0 ); + b2SolverSet* set = b2SolverSetArray_Get( &world->solverSets, contact->setIndex ); + int movedIndex = b2ContactSimArray_RemoveSwap( &set->contactSims, contact->localIndex ); + if ( movedIndex != B2_NULL_INDEX ) + { + b2ContactSim* movedContactSim = set->contactSims.data + contact->localIndex; + b2Contact* movedContact = b2ContactArray_Get( &world->contacts, movedContactSim->contactId ); + movedContact->localIndex = contact->localIndex; + } + } + + contact->contactId = B2_NULL_INDEX; + contact->setIndex = B2_NULL_INDEX; + contact->colorIndex = B2_NULL_INDEX; + contact->localIndex = B2_NULL_INDEX; + + b2FreeId( &world->contactIdPool, contactId ); + + if ( wakeBodies ) + { + b2WakeBody( world, bodyA ); + b2WakeBody( world, bodyB ); + } +} + +b2ContactSim* b2GetContactSim( b2World* world, b2Contact* contact ) +{ + if ( contact->setIndex == b2_awakeSet && contact->colorIndex != B2_NULL_INDEX ) + { + // contact lives in constraint graph + B2_ASSERT( 0 <= contact->colorIndex && contact->colorIndex < b2_graphColorCount ); + b2GraphColor* color = world->constraintGraph.colors + contact->colorIndex; + return b2ContactSimArray_Get( &color->contactSims, contact->localIndex ); + } + + b2SolverSet* set = b2SolverSetArray_Get( &world->solverSets, contact->setIndex ); + return b2ContactSimArray_Get( &set->contactSims, contact->localIndex ); +} + +bool b2ShouldShapesCollide( b2Filter filterA, b2Filter filterB ) +{ + 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; +} + +static bool b2TestShapeOverlap( const b2Shape* shapeA, b2Transform xfA, const b2Shape* shapeB, b2Transform xfB, + b2DistanceCache* cache ) +{ + b2DistanceInput input; + input.proxyA = b2MakeShapeDistanceProxy( shapeA ); + input.proxyB = b2MakeShapeDistanceProxy( shapeB ); + input.transformA = xfA; + input.transformB = xfB; + input.useRadii = true; + + b2DistanceOutput output = b2ShapeDistance( cache, &input, NULL, 0 ); + + return output.distance < 10.0f * FLT_EPSILON; +} + +// Update the contact manifold and touching status. Also updates sensor overlap. +// Note: do not assume the shape AABBs are overlapping or are valid. +bool b2UpdateContact( b2World* world, b2ContactSim* contactSim, b2Shape* shapeA, b2Transform transformA, b2Vec2 centerOffsetA, + b2Shape* shapeB, b2Transform transformB, b2Vec2 centerOffsetB ) +{ + bool touching; + + // Is this contact a sensor? + if ( shapeA->isSensor || shapeB->isSensor ) + { + // Sensors don't generate manifolds or hit events + touching = b2TestShapeOverlap( shapeA, transformA, shapeB, transformB, &contactSim->cache ); + } + else + { + b2Manifold oldManifold = contactSim->manifold; + + // Compute TOI + b2ManifoldFcn* fcn = s_registers[shapeA->type][shapeB->type].fcn; + + contactSim->manifold = fcn( shapeA, transformA, shapeB, transformB, &contactSim->cache ); + + int pointCount = contactSim->manifold.pointCount; + touching = pointCount > 0; + + if ( touching && world->preSolveFcn && ( contactSim->simFlags & b2_simEnablePreSolveEvents ) != 0 ) + { + b2ShapeId shapeIdA = { shapeA->id + 1, world->worldId, shapeA->revision }; + b2ShapeId shapeIdB = { shapeB->id + 1, world->worldId, shapeB->revision }; + + // this call assumes thread safety + touching = world->preSolveFcn( shapeIdA, shapeIdB, &contactSim->manifold, world->preSolveContext ); + if ( touching == false ) + { + // disable contact + contactSim->manifold.pointCount = 0; + } + } + + if ( touching && ( shapeA->enableHitEvents || shapeB->enableHitEvents ) ) + { + contactSim->simFlags |= b2_simEnableHitEvent; + } + else + { + contactSim->simFlags &= ~b2_simEnableHitEvent; + } + + // Match old contact ids to new contact ids and copy the + // stored impulses to warm start the solver. + for ( int i = 0; i < pointCount; ++i ) + { + b2ManifoldPoint* mp2 = contactSim->manifold.points + i; + + // shift anchors to be center of mass relative + mp2->anchorA = b2Sub( mp2->anchorA, centerOffsetA ); + mp2->anchorB = b2Sub( mp2->anchorB, centerOffsetB ); + + mp2->normalImpulse = 0.0f; + mp2->tangentImpulse = 0.0f; + mp2->maxNormalImpulse = 0.0f; + mp2->normalVelocity = 0.0f; + mp2->persisted = false; + + uint16_t id2 = mp2->id; + + for ( int j = 0; j < oldManifold.pointCount; ++j ) + { + b2ManifoldPoint* mp1 = oldManifold.points + j; + + if ( mp1->id == id2 ) + { + mp2->normalImpulse = mp1->normalImpulse; + mp2->tangentImpulse = mp1->tangentImpulse; + mp2->persisted = true; + break; + } + } + } + } + + if ( touching ) + { + contactSim->simFlags |= b2_simTouchingFlag; + } + else + { + contactSim->simFlags &= ~b2_simTouchingFlag; + } + + return touching; +} diff --git a/3rdparty/box2d/src/contact.h b/3rdparty/box2d/src/contact.h new file mode 100644 index 000000000000..6cf570e4e394 --- /dev/null +++ b/3rdparty/box2d/src/contact.h @@ -0,0 +1,157 @@ +// SPDX-FileCopyrightText: 2023 Erin Catto +// SPDX-License-Identifier: MIT + +#pragma once + +#include "array.h" +#include "core.h" + +#include "box2d/collision.h" +#include "box2d/types.h" + +typedef struct b2Shape b2Shape; +typedef struct b2World b2World; + +enum b2ContactFlags +{ + // Set when the solid shapes are touching. + b2_contactTouchingFlag = 0x00000001, + + // Contact has a hit event + b2_contactHitEventFlag = 0x00000002, + + // One of the shapes is a sensor + b2_contactSensorFlag = 0x0000004, + + // Set when a sensor is touching + // todo this is not used, perhaps have b2Body_GetSensorContactData() + b2_contactSensorTouchingFlag = 0x00000008, + + // This contact wants sensor events + b2_contactEnableSensorEvents = 0x00000010, + + // This contact wants contact events + b2_contactEnableContactEvents = 0x00000020, +}; + +// A contact edge is used to connect bodies and contacts together +// in a contact graph where each body is a node and each contact +// is an edge. A contact edge belongs to a doubly linked list +// maintained in each attached body. Each contact has two contact +// edges, one for each attached body. +typedef struct b2ContactEdge +{ + int bodyId; + int prevKey; + int nextKey; +} b2ContactEdge; + +// Cold contact data. Used as a persistent handle and for persistent island +// connectivity. +typedef struct b2Contact +{ + // index of simulation set stored in b2World + // B2_NULL_INDEX when slot is free + int setIndex; + + // index into the constraint graph color array + // B2_NULL_INDEX for non-touching or sleeping contacts + // B2_NULL_INDEX when slot is free + int colorIndex; + + // contact index within set or graph color + // B2_NULL_INDEX when slot is free + int localIndex; + + b2ContactEdge edges[2]; + int shapeIdA; + int shapeIdB; + + // A contact only belongs to an island if touching, otherwise B2_NULL_INDEX. + int islandPrev; + int islandNext; + int islandId; + + int contactId; + + // b2ContactFlags + uint32_t flags; + + bool isMarked; +} b2Contact; + +// Shifted to be distinct from b2ContactFlags +enum b2ContactSimFlags +{ + // Set when the shapes are touching, including sensors + b2_simTouchingFlag = 0x00010000, + + // This contact no longer has overlapping AABBs + b2_simDisjoint = 0x00020000, + + // This contact started touching + b2_simStartedTouching = 0x00040000, + + // This contact stopped touching + b2_simStoppedTouching = 0x00080000, + + // This contact has a hit event + b2_simEnableHitEvent = 0x00100000, + + // This contact wants pre-solve events + b2_simEnablePreSolveEvents = 0x00200000, +}; + +/// The class manages contact between two shapes. A contact exists for each overlapping +/// AABB in the broad-phase (except if filtered). Therefore a contact object may exist +/// that has no contact points. +typedef struct b2ContactSim +{ + int contactId; + +#if B2_VALIDATE + int bodyIdA; + int bodyIdB; +#endif + + int bodySimIndexA; + int bodySimIndexB; + + int shapeIdA; + int shapeIdB; + + float invMassA; + float invIA; + + float invMassB; + float invIB; + + b2Manifold manifold; + + // Mixed friction and restitution + float friction; + float restitution; + + // todo for conveyor belts + float tangentSpeed; + + // b2ContactSimFlags + uint32_t simFlags; + + b2DistanceCache cache; +} b2ContactSim; + +void b2InitializeContactRegisters( void ); + +void b2CreateContact( b2World* world, b2Shape* shapeA, b2Shape* shapeB ); +void b2DestroyContact( b2World* world, b2Contact* contact, bool wakeBodies ); + +b2ContactSim* b2GetContactSim( b2World* world, b2Contact* contact ); + +bool b2ShouldShapesCollide( b2Filter filterA, b2Filter filterB ); + +bool b2UpdateContact( b2World* world, b2ContactSim* contactSim, b2Shape* shapeA, b2Transform transformA, b2Vec2 centerOffsetA, + b2Shape* shapeB, b2Transform transformB, b2Vec2 centerOffsetB ); + +B2_ARRAY_INLINE( b2Contact, b2Contact ); +B2_ARRAY_INLINE( b2ContactSim, b2ContactSim ); diff --git a/3rdparty/box2d/src/contact_solver.c b/3rdparty/box2d/src/contact_solver.c new file mode 100644 index 000000000000..9c9d547fba27 --- /dev/null +++ b/3rdparty/box2d/src/contact_solver.c @@ -0,0 +1,2059 @@ +// SPDX-FileCopyrightText: 2023 Erin Catto +// SPDX-License-Identifier: MIT + +#include "contact_solver.h" + +#include "body.h" +#include "constraint_graph.h" +#include "contact.h" +#include "core.h" +#include "solver_set.h" +#include "world.h" + +#include + +void b2PrepareOverflowContacts( b2StepContext* context ) +{ + b2TracyCZoneNC( prepare_overflow_contact, "Prepare Overflow Contact", b2_colorYellow, true ); + + b2World* world = context->world; + b2ConstraintGraph* graph = context->graph; + b2GraphColor* color = graph->colors + b2_overflowIndex; + b2ContactConstraint* constraints = color->overflowConstraints; + int contactCount = color->contactSims.count; + b2ContactSim* contacts = color->contactSims.data; + b2BodyState* awakeStates = context->states; + +#if B2_VALIDATE + b2Body* bodies = world->bodies.data; +#endif + + // Stiffer for static contacts to avoid bodies getting pushed through the ground + b2Softness contactSoftness = context->contactSoftness; + b2Softness staticSoftness = context->staticSoftness; + + float warmStartScale = world->enableWarmStarting ? 1.0f : 0.0f; + + for ( int i = 0; i < contactCount; ++i ) + { + b2ContactSim* contactSim = contacts + i; + + const b2Manifold* manifold = &contactSim->manifold; + int pointCount = manifold->pointCount; + + B2_ASSERT( 0 < pointCount && pointCount <= 2 ); + + int indexA = contactSim->bodySimIndexA; + int indexB = contactSim->bodySimIndexB; + +#if B2_VALIDATE + b2Body* bodyA = bodies + contactSim->bodyIdA; + int validIndexA = bodyA->setIndex == b2_awakeSet ? bodyA->localIndex : B2_NULL_INDEX; + B2_ASSERT( indexA == validIndexA ); + + b2Body* bodyB = bodies + contactSim->bodyIdB; + int validIndexB = bodyB->setIndex == b2_awakeSet ? bodyB->localIndex : B2_NULL_INDEX; + B2_ASSERT( indexB == validIndexB ); +#endif + + b2ContactConstraint* constraint = constraints + i; + constraint->indexA = indexA; + constraint->indexB = indexB; + constraint->normal = manifold->normal; + constraint->friction = contactSim->friction; + constraint->restitution = contactSim->restitution; + constraint->pointCount = pointCount; + + b2Vec2 vA = b2Vec2_zero; + float wA = 0.0f; + float mA = contactSim->invMassA; + float iA = contactSim->invIA; + if ( indexA != B2_NULL_INDEX ) + { + b2BodyState* stateA = awakeStates + indexA; + vA = stateA->linearVelocity; + wA = stateA->angularVelocity; + } + + b2Vec2 vB = b2Vec2_zero; + float wB = 0.0f; + float mB = contactSim->invMassB; + float iB = contactSim->invIB; + if ( indexB != B2_NULL_INDEX ) + { + b2BodyState* stateB = awakeStates + indexB; + vB = stateB->linearVelocity; + wB = stateB->angularVelocity; + } + + if ( indexA == B2_NULL_INDEX || indexB == B2_NULL_INDEX ) + { + constraint->softness = staticSoftness; + } + else + { + constraint->softness = contactSoftness; + } + + // copy mass into constraint to avoid cache misses during sub-stepping + constraint->invMassA = mA; + constraint->invIA = iA; + constraint->invMassB = mB; + constraint->invIB = iB; + + b2Vec2 normal = constraint->normal; + b2Vec2 tangent = b2RightPerp( constraint->normal ); + + for ( int j = 0; j < pointCount; ++j ) + { + const b2ManifoldPoint* mp = manifold->points + j; + b2ContactConstraintPoint* cp = constraint->points + j; + + cp->normalImpulse = warmStartScale * mp->normalImpulse; + cp->tangentImpulse = warmStartScale * mp->tangentImpulse; + cp->maxNormalImpulse = 0.0f; + + b2Vec2 rA = mp->anchorA; + b2Vec2 rB = mp->anchorB; + + cp->anchorA = rA; + cp->anchorB = rB; + cp->baseSeparation = mp->separation - b2Dot( b2Sub( rB, rA ), normal ); + + float rnA = b2Cross( rA, normal ); + float rnB = b2Cross( rB, normal ); + float kNormal = mA + mB + iA * rnA * rnA + iB * rnB * rnB; + cp->normalMass = kNormal > 0.0f ? 1.0f / kNormal : 0.0f; + + float rtA = b2Cross( rA, tangent ); + float rtB = b2Cross( rB, tangent ); + float kTangent = mA + mB + iA * rtA * rtA + iB * rtB * rtB; + cp->tangentMass = kTangent > 0.0f ? 1.0f / kTangent : 0.0f; + + // Save relative velocity for restitution + b2Vec2 vrA = b2Add( vA, b2CrossSV( wA, rA ) ); + b2Vec2 vrB = b2Add( vB, b2CrossSV( wB, rB ) ); + cp->relativeVelocity = b2Dot( normal, b2Sub( vrB, vrA ) ); + } + } + + b2TracyCZoneEnd( prepare_overflow_contact ); +} + +void b2WarmStartOverflowContacts( b2StepContext* context ) +{ + b2TracyCZoneNC( warmstart_overflow_contact, "WarmStart Overflow Contact", b2_colorDarkOrange, true ); + + b2ConstraintGraph* graph = context->graph; + b2GraphColor* color = graph->colors + b2_overflowIndex; + b2ContactConstraint* constraints = color->overflowConstraints; + int contactCount = color->contactSims.count; + b2World* world = context->world; + b2SolverSet* awakeSet = b2SolverSetArray_Get( &world->solverSets, b2_awakeSet ); + b2BodyState* states = awakeSet->bodyStates.data; + + // This is a dummy state to represent a static body because static bodies don't have a solver body. + b2BodyState dummyState = b2_identityBodyState; + + for ( int i = 0; i < contactCount; ++i ) + { + const b2ContactConstraint* constraint = constraints + i; + + int indexA = constraint->indexA; + int indexB = constraint->indexB; + + b2BodyState* stateA = indexA == B2_NULL_INDEX ? &dummyState : states + indexA; + b2BodyState* stateB = indexB == B2_NULL_INDEX ? &dummyState : states + indexB; + + b2Vec2 vA = stateA->linearVelocity; + float wA = stateA->angularVelocity; + b2Vec2 vB = stateB->linearVelocity; + float wB = stateB->angularVelocity; + + float mA = constraint->invMassA; + float iA = constraint->invIA; + float mB = constraint->invMassB; + float iB = constraint->invIB; + + // Stiffer for static contacts to avoid bodies getting pushed through the ground + b2Vec2 normal = constraint->normal; + b2Vec2 tangent = b2RightPerp( constraint->normal ); + int pointCount = constraint->pointCount; + + for ( int j = 0; j < pointCount; ++j ) + { + const b2ContactConstraintPoint* cp = constraint->points + j; + + // fixed anchors + b2Vec2 rA = cp->anchorA; + b2Vec2 rB = cp->anchorB; + + b2Vec2 P = b2Add( b2MulSV( cp->normalImpulse, normal ), b2MulSV( cp->tangentImpulse, tangent ) ); + wA -= iA * b2Cross( rA, P ); + vA = b2MulAdd( vA, -mA, P ); + wB += iB * b2Cross( rB, P ); + vB = b2MulAdd( vB, mB, P ); + } + + stateA->linearVelocity = vA; + stateA->angularVelocity = wA; + stateB->linearVelocity = vB; + stateB->angularVelocity = wB; + } + + b2TracyCZoneEnd( warmstart_overflow_contact ); +} + +void b2SolveOverflowContacts( b2StepContext* context, bool useBias ) +{ + b2TracyCZoneNC( solve_contact, "Solve Contact", b2_colorAliceBlue, true ); + + b2ConstraintGraph* graph = context->graph; + b2GraphColor* color = graph->colors + b2_overflowIndex; + b2ContactConstraint* constraints = color->overflowConstraints; + int contactCount = color->contactSims.count; + b2World* world = context->world; + b2SolverSet* awakeSet = b2SolverSetArray_Get( &world->solverSets, b2_awakeSet ); + b2BodyState* states = awakeSet->bodyStates.data; + + float inv_h = context->inv_h; + const float pushout = context->world->contactPushoutVelocity; + + // This is a dummy body to represent a static body since static bodies don't have a solver body. + b2BodyState dummyState = b2_identityBodyState; + + for ( int i = 0; i < contactCount; ++i ) + { + b2ContactConstraint* constraint = constraints + i; + float mA = constraint->invMassA; + float iA = constraint->invIA; + float mB = constraint->invMassB; + float iB = constraint->invIB; + + b2BodyState* stateA = constraint->indexA == B2_NULL_INDEX ? &dummyState : states + constraint->indexA; + b2Vec2 vA = stateA->linearVelocity; + float wA = stateA->angularVelocity; + b2Rot dqA = stateA->deltaRotation; + + b2BodyState* stateB = constraint->indexB == B2_NULL_INDEX ? &dummyState : states + constraint->indexB; + b2Vec2 vB = stateB->linearVelocity; + float wB = stateB->angularVelocity; + b2Rot dqB = stateB->deltaRotation; + + b2Vec2 dp = b2Sub( stateB->deltaPosition, stateA->deltaPosition ); + + b2Vec2 normal = constraint->normal; + b2Vec2 tangent = b2RightPerp( normal ); + float friction = constraint->friction; + b2Softness softness = constraint->softness; + + int pointCount = constraint->pointCount; + + for ( int j = 0; j < pointCount; ++j ) + { + b2ContactConstraintPoint* cp = constraint->points + j; + + // compute current separation + // this is subject to round-off error if the anchor is far from the body center of mass + b2Vec2 ds = b2Add( dp, b2Sub( b2RotateVector( dqB, cp->anchorB ), b2RotateVector( dqA, cp->anchorA ) ) ); + float s = b2Dot( ds, normal ) + cp->baseSeparation; + + float velocityBias = 0.0f; + float massScale = 1.0f; + float impulseScale = 0.0f; + if ( s > 0.0f ) + { + // speculative bias + velocityBias = s * inv_h; + } + else if ( useBias ) + { + velocityBias = b2MaxFloat( softness.biasRate * s, -pushout ); + massScale = softness.massScale; + impulseScale = softness.impulseScale; + } + + // fixed anchor points + b2Vec2 rA = cp->anchorA; + b2Vec2 rB = cp->anchorB; + + // relative normal velocity at contact + b2Vec2 vrA = b2Add( vA, b2CrossSV( wA, rA ) ); + b2Vec2 vrB = b2Add( vB, b2CrossSV( wB, rB ) ); + float vn = b2Dot( b2Sub( vrB, vrA ), normal ); + + // incremental normal impulse + float impulse = -cp->normalMass * massScale * ( vn + velocityBias ) - impulseScale * cp->normalImpulse; + + // clamp the accumulated impulse + float newImpulse = b2MaxFloat( cp->normalImpulse + impulse, 0.0f ); + impulse = newImpulse - cp->normalImpulse; + cp->normalImpulse = newImpulse; + cp->maxNormalImpulse = b2MaxFloat( cp->maxNormalImpulse, impulse ); + + // apply normal impulse + b2Vec2 P = b2MulSV( impulse, normal ); + vA = b2MulSub( vA, mA, P ); + wA -= iA * b2Cross( rA, P ); + + vB = b2MulAdd( vB, mB, P ); + wB += iB * b2Cross( rB, P ); + } + + for ( int j = 0; j < pointCount; ++j ) + { + b2ContactConstraintPoint* cp = constraint->points + j; + + // fixed anchor points + b2Vec2 rA = cp->anchorA; + b2Vec2 rB = cp->anchorB; + + // relative tangent velocity at contact + b2Vec2 vrB = b2Add( vB, b2CrossSV( wB, rB ) ); + b2Vec2 vrA = b2Add( vA, b2CrossSV( wA, rA ) ); + float vt = b2Dot( b2Sub( vrB, vrA ), tangent ); + + // incremental tangent impulse + float impulse = cp->tangentMass * ( -vt ); + + // clamp the accumulated force + float maxFriction = friction * cp->normalImpulse; + float newImpulse = b2ClampFloat( cp->tangentImpulse + impulse, -maxFriction, maxFriction ); + impulse = newImpulse - cp->tangentImpulse; + cp->tangentImpulse = newImpulse; + + // apply tangent impulse + b2Vec2 P = b2MulSV( impulse, tangent ); + vA = b2MulSub( vA, mA, P ); + wA -= iA * b2Cross( rA, P ); + vB = b2MulAdd( vB, mB, P ); + wB += iB * b2Cross( rB, P ); + } + + stateA->linearVelocity = vA; + stateA->angularVelocity = wA; + stateB->linearVelocity = vB; + stateB->angularVelocity = wB; + } + + b2TracyCZoneEnd( solve_contact ); +} + +void b2ApplyOverflowRestitution( b2StepContext* context ) +{ + b2TracyCZoneNC( overflow_resitution, "Overflow Restitution", b2_colorViolet, true ); + + b2ConstraintGraph* graph = context->graph; + b2GraphColor* color = graph->colors + b2_overflowIndex; + b2ContactConstraint* constraints = color->overflowConstraints; + int contactCount = color->contactSims.count; + b2World* world = context->world; + b2SolverSet* awakeSet = b2SolverSetArray_Get( &world->solverSets, b2_awakeSet ); + b2BodyState* states = awakeSet->bodyStates.data; + + float threshold = context->world->restitutionThreshold; + + // dummy state to represent a static body + b2BodyState dummyState = b2_identityBodyState; + + for ( int i = 0; i < contactCount; ++i ) + { + b2ContactConstraint* constraint = constraints + i; + + float restitution = constraint->restitution; + if ( restitution == 0.0f ) + { + continue; + } + + float mA = constraint->invMassA; + float iA = constraint->invIA; + float mB = constraint->invMassB; + float iB = constraint->invIB; + + b2BodyState* stateA = constraint->indexA == B2_NULL_INDEX ? &dummyState : states + constraint->indexA; + b2Vec2 vA = stateA->linearVelocity; + float wA = stateA->angularVelocity; + + b2BodyState* stateB = constraint->indexB == B2_NULL_INDEX ? &dummyState : states + constraint->indexB; + b2Vec2 vB = stateB->linearVelocity; + float wB = stateB->angularVelocity; + + b2Vec2 normal = constraint->normal; + int pointCount = constraint->pointCount; + + // it is possible to get more accurate restitution by iterating + // this only makes a difference if there are two contact points + // for (int iter = 0; iter < 10; ++iter) + { + for ( int j = 0; j < pointCount; ++j ) + { + b2ContactConstraintPoint* cp = constraint->points + j; + + // if the normal impulse is zero then there was no collision + // this skips speculative contact points that didn't generate an impulse + // The max normal impulse is used in case there was a collision that moved away within the sub-step process + if ( cp->relativeVelocity > -threshold || cp->maxNormalImpulse == 0.0f ) + { + continue; + } + + // fixed anchor points + b2Vec2 rA = cp->anchorA; + b2Vec2 rB = cp->anchorB; + + // relative normal velocity at contact + b2Vec2 vrB = b2Add( vB, b2CrossSV( wB, rB ) ); + b2Vec2 vrA = b2Add( vA, b2CrossSV( wA, rA ) ); + float vn = b2Dot( b2Sub( vrB, vrA ), normal ); + + // compute normal impulse + float impulse = -cp->normalMass * ( vn + restitution * cp->relativeVelocity ); + + // clamp the accumulated impulse + // todo should this be stored? + float newImpulse = b2MaxFloat( cp->normalImpulse + impulse, 0.0f ); + impulse = newImpulse - cp->normalImpulse; + cp->normalImpulse = newImpulse; + cp->maxNormalImpulse = b2MaxFloat( cp->maxNormalImpulse, impulse ); + + // apply contact impulse + b2Vec2 P = b2MulSV( impulse, normal ); + vA = b2MulSub( vA, mA, P ); + wA -= iA * b2Cross( rA, P ); + vB = b2MulAdd( vB, mB, P ); + wB += iB * b2Cross( rB, P ); + } + } + + stateA->linearVelocity = vA; + stateA->angularVelocity = wA; + stateB->linearVelocity = vB; + stateB->angularVelocity = wB; + } + + b2TracyCZoneEnd( overflow_resitution ); +} + +void b2StoreOverflowImpulses( b2StepContext* context ) +{ + b2TracyCZoneNC( store_impulses, "Store", b2_colorFirebrick, true ); + + b2ConstraintGraph* graph = context->graph; + b2GraphColor* color = graph->colors + b2_overflowIndex; + b2ContactConstraint* constraints = color->overflowConstraints; + b2ContactSim* contacts = color->contactSims.data; + int contactCount = color->contactSims.count; + + // float hitEventThreshold = context->world->hitEventThreshold; + + for ( int i = 0; i < contactCount; ++i ) + { + const b2ContactConstraint* constraint = constraints + i; + b2ContactSim* contact = contacts + i; + b2Manifold* manifold = &contact->manifold; + int pointCount = manifold->pointCount; + + for ( int j = 0; j < pointCount; ++j ) + { + manifold->points[j].normalImpulse = constraint->points[j].normalImpulse; + manifold->points[j].tangentImpulse = constraint->points[j].tangentImpulse; + manifold->points[j].maxNormalImpulse = constraint->points[j].maxNormalImpulse; + manifold->points[j].normalVelocity = constraint->points[j].relativeVelocity; + } + } + + b2TracyCZoneEnd( store_impulses ); +} + +#if defined( B2_SIMD_AVX2 ) + +#include + +// wide float holds 8 numbers +typedef __m256 b2FloatW; + +#elif defined( B2_SIMD_NEON ) + +#include + +// wide float holds 4 numbers +typedef float32x4_t b2FloatW; + +#elif defined( B2_SIMD_SSE2 ) + +#include + +// wide float holds 4 numbers +typedef __m128 b2FloatW; + +#else + +// scalar math +typedef struct b2FloatW +{ + float x, y, z, w; +} b2FloatW; + +#endif + +// Wide vec2 +typedef struct b2Vec2W +{ + b2FloatW X, Y; +} b2Vec2W; + +// Wide rotation +typedef struct b2RotW +{ + b2FloatW C, S; +} b2RotW; + +#if defined( B2_SIMD_AVX2 ) + +static inline b2FloatW b2ZeroW() +{ + return _mm256_setzero_ps(); +} + +static inline b2FloatW b2SplatW( float scalar ) +{ + return _mm256_set1_ps( scalar ); +} + +static inline b2FloatW b2AddW( b2FloatW a, b2FloatW b ) +{ + return _mm256_add_ps( a, b ); +} + +static inline b2FloatW b2SubW( b2FloatW a, b2FloatW b ) +{ + return _mm256_sub_ps( a, b ); +} + +static inline b2FloatW b2MulW( b2FloatW a, b2FloatW b ) +{ + return _mm256_mul_ps( a, b ); +} + +static inline b2FloatW b2MulAddW( b2FloatW a, b2FloatW b, b2FloatW c ) +{ + // FMA can be emulated: https://github.com/lattera/glibc/blob/master/sysdeps/ieee754/dbl-64/s_fmaf.c#L34 + // return _mm256_fmadd_ps( b, c, a ); + return _mm256_add_ps( _mm256_mul_ps( b, c ), a ); +} + +static inline b2FloatW b2MulSubW( b2FloatW a, b2FloatW b, b2FloatW c ) +{ + // return _mm256_fnmadd_ps(b, c, a); + return _mm256_sub_ps( a, _mm256_mul_ps( b, c ) ); +} + +static inline b2FloatW b2MinW( b2FloatW a, b2FloatW b ) +{ + return _mm256_min_ps( a, b ); +} + +static inline b2FloatW b2MaxW( b2FloatW a, b2FloatW b ) +{ + return _mm256_max_ps( a, b ); +} + +static inline b2FloatW b2OrW( b2FloatW a, b2FloatW b ) +{ + return _mm256_or_ps( a, b ); +} + +static inline b2FloatW b2GreaterThanW( b2FloatW a, b2FloatW b ) +{ + return _mm256_cmp_ps( a, b, _CMP_GT_OQ ); +} + +static inline b2FloatW b2EqualsW( b2FloatW a, b2FloatW b ) +{ + return _mm256_cmp_ps( a, b, _CMP_EQ_OQ ); +} + +// component-wise returns mask ? b : a +static inline b2FloatW b2BlendW( b2FloatW a, b2FloatW b, b2FloatW mask ) +{ + return _mm256_blendv_ps( a, b, mask ); +} + +#elif defined( B2_SIMD_NEON ) + +static inline b2FloatW b2ZeroW() +{ + return vdupq_n_f32( 0.0f ); +} + +static inline b2FloatW b2SplatW( float scalar ) +{ + return vdupq_n_f32( scalar ); +} + +static inline b2FloatW b2SetW( float a, float b, float c, float d ) +{ + float32_t array[4] = { a, b, c, d }; + return vld1q_f32( array ); +} + +static inline b2FloatW b2AddW( b2FloatW a, b2FloatW b ) +{ + return vaddq_f32( a, b ); +} + +static inline b2FloatW b2SubW( b2FloatW a, b2FloatW b ) +{ + return vsubq_f32( a, b ); +} + +static inline b2FloatW b2MulW( b2FloatW a, b2FloatW b ) +{ + return vmulq_f32( a, b ); +} + +static inline b2FloatW b2MulAddW( b2FloatW a, b2FloatW b, b2FloatW c ) +{ + return vmlaq_f32( a, b, c ); +} + +static inline b2FloatW b2MulSubW( b2FloatW a, b2FloatW b, b2FloatW c ) +{ + return vmlsq_f32( a, b, c ); +} + +static inline b2FloatW b2MinW( b2FloatW a, b2FloatW b ) +{ + return vminq_f32( a, b ); +} + +static inline b2FloatW b2MaxW( b2FloatW a, b2FloatW b ) +{ + return vmaxq_f32( a, b ); +} + +static inline b2FloatW b2OrW( b2FloatW a, b2FloatW b ) +{ + return vreinterpretq_f32_u32( vorrq_u32( vreinterpretq_u32_f32( a ), vreinterpretq_u32_f32( b ) ) ); +} + +static inline b2FloatW b2GreaterThanW( b2FloatW a, b2FloatW b ) +{ + return vreinterpretq_f32_u32( vcgtq_f32( a, b ) ); +} + +static inline b2FloatW b2EqualsW( b2FloatW a, b2FloatW b ) +{ + return vreinterpretq_f32_u32( vceqq_f32( a, b ) ); +} + +// component-wise returns mask ? b : a +static inline b2FloatW b2BlendW( b2FloatW a, b2FloatW b, b2FloatW mask ) +{ + uint32x4_t mask32 = vreinterpretq_u32_f32( mask ); + return vbslq_f32( mask32, b, a ); +} + +static inline b2FloatW b2LoadW( const float32_t* data ) +{ + return vld1q_f32( data ); +} + +static inline void b2StoreW( float32_t* data, b2FloatW a ) +{ + return vst1q_f32( data, a ); +} + +static inline b2FloatW b2UnpackLoW( b2FloatW a, b2FloatW b ) +{ + return vzip1q_f32( a, b ); +} + +static inline b2FloatW b2UnpackHiW( b2FloatW a, b2FloatW b ) +{ + return vzip2q_f32( a, b ); +} + +#elif defined( B2_SIMD_SSE2 ) + +static inline b2FloatW b2ZeroW() +{ + return _mm_setzero_ps(); +} + +static inline b2FloatW b2SplatW( float scalar ) +{ + return _mm_set1_ps( scalar ); +} + +static inline b2FloatW b2SetW( float a, float b, float c, float d ) +{ + return _mm_setr_ps( a, b, c, d ); +} + +static inline b2FloatW b2AddW( b2FloatW a, b2FloatW b ) +{ + return _mm_add_ps( a, b ); +} + +static inline b2FloatW b2SubW( b2FloatW a, b2FloatW b ) +{ + return _mm_sub_ps( a, b ); +} + +static inline b2FloatW b2MulW( b2FloatW a, b2FloatW b ) +{ + return _mm_mul_ps( a, b ); +} + +static inline b2FloatW b2MulAddW( b2FloatW a, b2FloatW b, b2FloatW c ) +{ + return _mm_add_ps( a, _mm_mul_ps( b, c ) ); +} + +static inline b2FloatW b2MulSubW( b2FloatW a, b2FloatW b, b2FloatW c ) +{ + return _mm_sub_ps( a, _mm_mul_ps( b, c ) ); +} + +static inline b2FloatW b2MinW( b2FloatW a, b2FloatW b ) +{ + return _mm_min_ps( a, b ); +} + +static inline b2FloatW b2MaxW( b2FloatW a, b2FloatW b ) +{ + return _mm_max_ps( a, b ); +} + +static inline b2FloatW b2OrW( b2FloatW a, b2FloatW b ) +{ + return _mm_or_ps( a, b ); +} + +static inline b2FloatW b2GreaterThanW( b2FloatW a, b2FloatW b ) +{ + return _mm_cmpgt_ps( a, b ); +} + +static inline b2FloatW b2EqualsW( b2FloatW a, b2FloatW b ) +{ + return _mm_cmpeq_ps( a, b ); +} + +// component-wise returns mask ? b : a +static inline b2FloatW b2BlendW( b2FloatW a, b2FloatW b, b2FloatW mask ) +{ + return _mm_or_ps( _mm_and_ps( mask, b ), _mm_andnot_ps( mask, a ) ); +} + +static inline b2FloatW b2LoadW( const float* data ) +{ + return _mm_load_ps( data ); +} + +static inline void b2StoreW( float* data, b2FloatW a ) +{ + _mm_store_ps( data, a ); +} + +static inline b2FloatW b2UnpackLoW( b2FloatW a, b2FloatW b ) +{ + return _mm_unpacklo_ps( a, b ); +} + +static inline b2FloatW b2UnpackHiW( b2FloatW a, b2FloatW b ) +{ + return _mm_unpackhi_ps( a, b ); +} + +#else + +static inline b2FloatW b2ZeroW() +{ + return ( b2FloatW ){ 0.0f, 0.0f, 0.0f, 0.0f }; +} + +static inline b2FloatW b2SplatW( float scalar ) +{ + return ( b2FloatW ){ scalar, scalar, scalar, scalar }; +} + +static inline b2FloatW b2AddW( b2FloatW a, b2FloatW b ) +{ + return ( b2FloatW ){ a.x + b.x, a.y + b.y, a.z + b.z, a.w + b.w }; +} + +static inline b2FloatW b2SubW( b2FloatW a, b2FloatW b ) +{ + return ( b2FloatW ){ a.x - b.x, a.y - b.y, a.z - b.z, a.w - b.w }; +} + +static inline b2FloatW b2MulW( b2FloatW a, b2FloatW b ) +{ + return ( b2FloatW ){ a.x * b.x, a.y * b.y, a.z * b.z, a.w * b.w }; +} + +static inline b2FloatW b2MulAddW( b2FloatW a, b2FloatW b, b2FloatW c ) +{ + return ( b2FloatW ){ a.x + b.x * c.x, a.y + b.y * c.y, a.z + b.z * c.z, a.w + b.w * c.w }; +} + +static inline b2FloatW b2MulSubW( b2FloatW a, b2FloatW b, b2FloatW c ) +{ + return ( b2FloatW ){ a.x - b.x * c.x, a.y - b.y * c.y, a.z - b.z * c.z, a.w - b.w * c.w }; +} + +static inline b2FloatW b2MinW( b2FloatW a, b2FloatW b ) +{ + b2FloatW r; + r.x = a.x <= b.x ? a.x : b.x; + r.y = a.y <= b.y ? a.y : b.y; + r.z = a.z <= b.z ? a.z : b.z; + r.w = a.w <= b.w ? a.w : b.w; + return r; +} + +static inline b2FloatW b2MaxW( b2FloatW a, b2FloatW b ) +{ + b2FloatW r; + r.x = a.x >= b.x ? a.x : b.x; + r.y = a.y >= b.y ? a.y : b.y; + r.z = a.z >= b.z ? a.z : b.z; + r.w = a.w >= b.w ? a.w : b.w; + return r; +} + +static inline b2FloatW b2OrW( b2FloatW a, b2FloatW b ) +{ + b2FloatW r; + r.x = a.x != 0.0f || b.x != 0.0f ? 1.0f : 0.0f; + r.y = a.y != 0.0f || b.y != 0.0f ? 1.0f : 0.0f; + r.z = a.z != 0.0f || b.z != 0.0f ? 1.0f : 0.0f; + r.w = a.w != 0.0f || b.w != 0.0f ? 1.0f : 0.0f; + return r; +} + +static inline b2FloatW b2GreaterThanW( b2FloatW a, b2FloatW b ) +{ + b2FloatW r; + r.x = a.x > b.x ? 1.0f : 0.0f; + r.y = a.y > b.y ? 1.0f : 0.0f; + r.z = a.z > b.z ? 1.0f : 0.0f; + r.w = a.w > b.w ? 1.0f : 0.0f; + return r; +} + +static inline b2FloatW b2EqualsW( b2FloatW a, b2FloatW b ) +{ + b2FloatW r; + r.x = a.x == b.x ? 1.0f : 0.0f; + r.y = a.y == b.y ? 1.0f : 0.0f; + r.z = a.z == b.z ? 1.0f : 0.0f; + r.w = a.w == b.w ? 1.0f : 0.0f; + return r; +} + +// component-wise returns mask ? b : a +static inline b2FloatW b2BlendW( b2FloatW a, b2FloatW b, b2FloatW mask ) +{ + b2FloatW r; + r.x = mask.x != 0.0f ? b.x : a.x; + r.y = mask.y != 0.0f ? b.y : a.y; + r.z = mask.z != 0.0f ? b.z : a.z; + r.w = mask.w != 0.0f ? b.w : a.w; + return r; +} + +#endif + +static inline b2FloatW b2DotW( b2Vec2W a, b2Vec2W b ) +{ + return b2AddW( b2MulW( a.X, b.X ), b2MulW( a.Y, b.Y ) ); +} + +static inline b2FloatW b2CrossW( b2Vec2W a, b2Vec2W b ) +{ + return b2SubW( b2MulW( a.X, b.Y ), b2MulW( a.Y, b.X ) ); +} + +static inline b2Vec2W b2RotateVectorW( b2RotW q, b2Vec2W v ) +{ + return ( b2Vec2W ){ b2SubW( b2MulW( q.C, v.X ), b2MulW( q.S, v.Y ) ), b2AddW( b2MulW( q.S, v.X ), b2MulW( q.C, v.Y ) ) }; +} + +// Soft contact constraints with sub-stepping support +// Uses fixed anchors for Jacobians for better behavior on rolling shapes (circles & capsules) +// http://mmacklin.com/smallsteps.pdf +// https://box2d.org/files/ErinCatto_SoftConstraints_GDC2011.pdf + +typedef struct b2ContactConstraintSIMD +{ + int indexA[B2_SIMD_WIDTH]; + int indexB[B2_SIMD_WIDTH]; + + b2FloatW invMassA, invMassB; + b2FloatW invIA, invIB; + b2Vec2W normal; + b2FloatW friction; + b2FloatW biasRate; + b2FloatW massScale; + b2FloatW impulseScale; + b2Vec2W anchorA1, anchorB1; + b2FloatW normalMass1, tangentMass1; + b2FloatW baseSeparation1; + b2FloatW normalImpulse1; + b2FloatW maxNormalImpulse1; + b2FloatW tangentImpulse1; + b2Vec2W anchorA2, anchorB2; + b2FloatW baseSeparation2; + b2FloatW normalImpulse2; + b2FloatW maxNormalImpulse2; + b2FloatW tangentImpulse2; + b2FloatW normalMass2, tangentMass2; + b2FloatW restitution; + b2FloatW relativeVelocity1, relativeVelocity2; +} b2ContactConstraintSIMD; + +int b2GetContactConstraintSIMDByteCount( void ) +{ + return sizeof( b2ContactConstraintSIMD ); +} + +// wide version of b2BodyState +typedef struct b2SimdBody +{ + b2Vec2W v; + b2FloatW w; + b2FloatW flags; + b2Vec2W dp; + b2RotW dq; +} b2SimdBody; + +// Custom gather/scatter for each SIMD type +#if defined( B2_SIMD_AVX2 ) + +// This is a load and 8x8 transpose +static b2SimdBody b2GatherBodies( const b2BodyState* B2_RESTRICT states, int* B2_RESTRICT indices ) +{ + _Static_assert( sizeof( b2BodyState ) == 32, "b2BodyState not 32 bytes" ); + B2_ASSERT( ( (uintptr_t)states & 0x1F ) == 0 ); + // b2BodyState b2_identityBodyState = {{0.0f, 0.0f}, 0.0f, 0, {0.0f, 0.0f}, {1.0f, 0.0f}}; + b2FloatW identity = _mm256_setr_ps( 0.0f, 0.0f, 0.0f, 0, 0.0f, 0.0f, 1.0f, 0.0f ); + b2FloatW b0 = indices[0] == B2_NULL_INDEX ? identity : _mm256_load_ps( (float*)( states + indices[0] ) ); + b2FloatW b1 = indices[1] == B2_NULL_INDEX ? identity : _mm256_load_ps( (float*)( states + indices[1] ) ); + b2FloatW b2 = indices[2] == B2_NULL_INDEX ? identity : _mm256_load_ps( (float*)( states + indices[2] ) ); + b2FloatW b3 = indices[3] == B2_NULL_INDEX ? identity : _mm256_load_ps( (float*)( states + indices[3] ) ); + b2FloatW b4 = indices[4] == B2_NULL_INDEX ? identity : _mm256_load_ps( (float*)( states + indices[4] ) ); + b2FloatW b5 = indices[5] == B2_NULL_INDEX ? identity : _mm256_load_ps( (float*)( states + indices[5] ) ); + b2FloatW b6 = indices[6] == B2_NULL_INDEX ? identity : _mm256_load_ps( (float*)( states + indices[6] ) ); + b2FloatW b7 = indices[7] == B2_NULL_INDEX ? identity : _mm256_load_ps( (float*)( states + indices[7] ) ); + + b2FloatW t0 = _mm256_unpacklo_ps( b0, b1 ); + b2FloatW t1 = _mm256_unpackhi_ps( b0, b1 ); + b2FloatW t2 = _mm256_unpacklo_ps( b2, b3 ); + b2FloatW t3 = _mm256_unpackhi_ps( b2, b3 ); + b2FloatW t4 = _mm256_unpacklo_ps( b4, b5 ); + b2FloatW t5 = _mm256_unpackhi_ps( b4, b5 ); + b2FloatW t6 = _mm256_unpacklo_ps( b6, b7 ); + b2FloatW t7 = _mm256_unpackhi_ps( b6, b7 ); + b2FloatW tt0 = _mm256_shuffle_ps( t0, t2, _MM_SHUFFLE( 1, 0, 1, 0 ) ); + b2FloatW tt1 = _mm256_shuffle_ps( t0, t2, _MM_SHUFFLE( 3, 2, 3, 2 ) ); + b2FloatW tt2 = _mm256_shuffle_ps( t1, t3, _MM_SHUFFLE( 1, 0, 1, 0 ) ); + b2FloatW tt3 = _mm256_shuffle_ps( t1, t3, _MM_SHUFFLE( 3, 2, 3, 2 ) ); + b2FloatW tt4 = _mm256_shuffle_ps( t4, t6, _MM_SHUFFLE( 1, 0, 1, 0 ) ); + b2FloatW tt5 = _mm256_shuffle_ps( t4, t6, _MM_SHUFFLE( 3, 2, 3, 2 ) ); + b2FloatW tt6 = _mm256_shuffle_ps( t5, t7, _MM_SHUFFLE( 1, 0, 1, 0 ) ); + b2FloatW tt7 = _mm256_shuffle_ps( t5, t7, _MM_SHUFFLE( 3, 2, 3, 2 ) ); + + b2SimdBody simdBody; + simdBody.v.X = _mm256_permute2f128_ps( tt0, tt4, 0x20 ); + simdBody.v.Y = _mm256_permute2f128_ps( tt1, tt5, 0x20 ); + simdBody.w = _mm256_permute2f128_ps( tt2, tt6, 0x20 ); + simdBody.flags = _mm256_permute2f128_ps( tt3, tt7, 0x20 ); + simdBody.dp.X = _mm256_permute2f128_ps( tt0, tt4, 0x31 ); + simdBody.dp.Y = _mm256_permute2f128_ps( tt1, tt5, 0x31 ); + simdBody.dq.C = _mm256_permute2f128_ps( tt2, tt6, 0x31 ); + simdBody.dq.S = _mm256_permute2f128_ps( tt3, tt7, 0x31 ); + return simdBody; +} + +// This writes everything back to the solver bodies but only the velocities change +static void b2ScatterBodies( b2BodyState* B2_RESTRICT states, int* B2_RESTRICT indices, const b2SimdBody* B2_RESTRICT simdBody ) +{ + _Static_assert( sizeof( b2BodyState ) == 32, "b2BodyState not 32 bytes" ); + B2_ASSERT( ( (uintptr_t)states & 0x1F ) == 0 ); + b2FloatW t0 = _mm256_unpacklo_ps( simdBody->v.X, simdBody->v.Y ); + b2FloatW t1 = _mm256_unpackhi_ps( simdBody->v.X, simdBody->v.Y ); + b2FloatW t2 = _mm256_unpacklo_ps( simdBody->w, simdBody->flags ); + b2FloatW t3 = _mm256_unpackhi_ps( simdBody->w, simdBody->flags ); + b2FloatW t4 = _mm256_unpacklo_ps( simdBody->dp.X, simdBody->dp.Y ); + b2FloatW t5 = _mm256_unpackhi_ps( simdBody->dp.X, simdBody->dp.Y ); + b2FloatW t6 = _mm256_unpacklo_ps( simdBody->dq.C, simdBody->dq.S ); + b2FloatW t7 = _mm256_unpackhi_ps( simdBody->dq.C, simdBody->dq.S ); + b2FloatW tt0 = _mm256_shuffle_ps( t0, t2, _MM_SHUFFLE( 1, 0, 1, 0 ) ); + b2FloatW tt1 = _mm256_shuffle_ps( t0, t2, _MM_SHUFFLE( 3, 2, 3, 2 ) ); + b2FloatW tt2 = _mm256_shuffle_ps( t1, t3, _MM_SHUFFLE( 1, 0, 1, 0 ) ); + b2FloatW tt3 = _mm256_shuffle_ps( t1, t3, _MM_SHUFFLE( 3, 2, 3, 2 ) ); + b2FloatW tt4 = _mm256_shuffle_ps( t4, t6, _MM_SHUFFLE( 1, 0, 1, 0 ) ); + b2FloatW tt5 = _mm256_shuffle_ps( t4, t6, _MM_SHUFFLE( 3, 2, 3, 2 ) ); + b2FloatW tt6 = _mm256_shuffle_ps( t5, t7, _MM_SHUFFLE( 1, 0, 1, 0 ) ); + b2FloatW tt7 = _mm256_shuffle_ps( t5, t7, _MM_SHUFFLE( 3, 2, 3, 2 ) ); + + // I don't use any dummy body in the body array because this will lead to multithreaded sharing and the + // associated cache flushing. + if ( indices[0] != B2_NULL_INDEX ) + _mm256_store_ps( (float*)( states + indices[0] ), _mm256_permute2f128_ps( tt0, tt4, 0x20 ) ); + if ( indices[1] != B2_NULL_INDEX ) + _mm256_store_ps( (float*)( states + indices[1] ), _mm256_permute2f128_ps( tt1, tt5, 0x20 ) ); + if ( indices[2] != B2_NULL_INDEX ) + _mm256_store_ps( (float*)( states + indices[2] ), _mm256_permute2f128_ps( tt2, tt6, 0x20 ) ); + if ( indices[3] != B2_NULL_INDEX ) + _mm256_store_ps( (float*)( states + indices[3] ), _mm256_permute2f128_ps( tt3, tt7, 0x20 ) ); + if ( indices[4] != B2_NULL_INDEX ) + _mm256_store_ps( (float*)( states + indices[4] ), _mm256_permute2f128_ps( tt0, tt4, 0x31 ) ); + if ( indices[5] != B2_NULL_INDEX ) + _mm256_store_ps( (float*)( states + indices[5] ), _mm256_permute2f128_ps( tt1, tt5, 0x31 ) ); + if ( indices[6] != B2_NULL_INDEX ) + _mm256_store_ps( (float*)( states + indices[6] ), _mm256_permute2f128_ps( tt2, tt6, 0x31 ) ); + if ( indices[7] != B2_NULL_INDEX ) + _mm256_store_ps( (float*)( states + indices[7] ), _mm256_permute2f128_ps( tt3, tt7, 0x31 ) ); +} + +#elif defined( B2_SIMD_NEON ) + +// This is a load and transpose +static b2SimdBody b2GatherBodies( const b2BodyState* B2_RESTRICT states, int* B2_RESTRICT indices ) +{ + _Static_assert( sizeof( b2BodyState ) == 32, "b2BodyState not 32 bytes" ); + B2_ASSERT( ( (uintptr_t)states & 0x1F ) == 0 ); + + // [vx vy w flags] + b2FloatW identityA = b2ZeroW(); + + // [dpx dpy dqc dqs] + + b2FloatW identityB = b2SetW( 0.0f, 0.0f, 1.0f, 0.0f ); + + b2FloatW b1a = indices[0] == B2_NULL_INDEX ? identityA : b2LoadW( (float*)( states + indices[0] ) + 0 ); + b2FloatW b1b = indices[0] == B2_NULL_INDEX ? identityB : b2LoadW( (float*)( states + indices[0] ) + 4 ); + b2FloatW b2a = indices[1] == B2_NULL_INDEX ? identityA : b2LoadW( (float*)( states + indices[1] ) + 0 ); + b2FloatW b2b = indices[1] == B2_NULL_INDEX ? identityB : b2LoadW( (float*)( states + indices[1] ) + 4 ); + b2FloatW b3a = indices[2] == B2_NULL_INDEX ? identityA : b2LoadW( (float*)( states + indices[2] ) + 0 ); + b2FloatW b3b = indices[2] == B2_NULL_INDEX ? identityB : b2LoadW( (float*)( states + indices[2] ) + 4 ); + b2FloatW b4a = indices[3] == B2_NULL_INDEX ? identityA : b2LoadW( (float*)( states + indices[3] ) + 0 ); + b2FloatW b4b = indices[3] == B2_NULL_INDEX ? identityB : b2LoadW( (float*)( states + indices[3] ) + 4 ); + + // [vx1 vx3 vy1 vy3] + b2FloatW t1a = b2UnpackLoW( b1a, b3a ); + + // [vx2 vx4 vy2 vy4] + b2FloatW t2a = b2UnpackLoW( b2a, b4a ); + + // [w1 w3 f1 f3] + b2FloatW t3a = b2UnpackHiW( b1a, b3a ); + + // [w2 w4 f2 f4] + b2FloatW t4a = b2UnpackHiW( b2a, b4a ); + + b2SimdBody simdBody; + simdBody.v.X = b2UnpackLoW( t1a, t2a ); + simdBody.v.Y = b2UnpackHiW( t1a, t2a ); + simdBody.w = b2UnpackLoW( t3a, t4a ); + simdBody.flags = b2UnpackHiW( t3a, t4a ); + + b2FloatW t1b = b2UnpackLoW( b1b, b3b ); + b2FloatW t2b = b2UnpackLoW( b2b, b4b ); + b2FloatW t3b = b2UnpackHiW( b1b, b3b ); + b2FloatW t4b = b2UnpackHiW( b2b, b4b ); + + simdBody.dp.X = b2UnpackLoW( t1b, t2b ); + simdBody.dp.Y = b2UnpackHiW( t1b, t2b ); + simdBody.dq.C = b2UnpackLoW( t3b, t4b ); + simdBody.dq.S = b2UnpackHiW( t3b, t4b ); + + return simdBody; +} + +// This writes only the velocities back to the solver bodies +// https://developer.arm.com/documentation/102107a/0100/Floating-point-4x4-matrix-transposition +static void b2ScatterBodies( b2BodyState* B2_RESTRICT states, int* B2_RESTRICT indices, const b2SimdBody* B2_RESTRICT simdBody ) +{ + _Static_assert( sizeof( b2BodyState ) == 32, "b2BodyState not 32 bytes" ); + B2_ASSERT( ( (uintptr_t)states & 0x1F ) == 0 ); + + // b2FloatW x = b2SetW(0.0f, 1.0f, 2.0f, 3.0f); + // b2FloatW y = b2SetW(4.0f, 5.0f, 6.0f, 7.0f); + // b2FloatW z = b2SetW(8.0f, 9.0f, 10.0f, 11.0f); + // b2FloatW w = b2SetW(12.0f, 13.0f, 14.0f, 15.0f); + // + // float32x4x2_t rr1 = vtrnq_f32( x, y ); + // float32x4x2_t rr2 = vtrnq_f32( z, w ); + // + // float32x4_t b1 = vcombine_f32(vget_low_f32(rr1.val[0]), vget_low_f32(rr2.val[0])); + // float32x4_t b2 = vcombine_f32(vget_low_f32(rr1.val[1]), vget_low_f32(rr2.val[1])); + // float32x4_t b3 = vcombine_f32(vget_high_f32(rr1.val[0]), vget_high_f32(rr2.val[0])); + // float32x4_t b4 = vcombine_f32(vget_high_f32(rr1.val[1]), vget_high_f32(rr2.val[1])); + + // transpose + float32x4x2_t r1 = vtrnq_f32( simdBody->v.X, simdBody->v.Y ); + float32x4x2_t r2 = vtrnq_f32( simdBody->w, simdBody->flags ); + + // I don't use any dummy body in the body array because this will lead to multithreaded sharing and the + // associated cache flushing. + if ( indices[0] != B2_NULL_INDEX ) + { + float32x4_t body1 = vcombine_f32( vget_low_f32( r1.val[0] ), vget_low_f32( r2.val[0] ) ); + b2StoreW( (float*)( states + indices[0] ), body1 ); + } + + if ( indices[1] != B2_NULL_INDEX ) + { + float32x4_t body2 = vcombine_f32( vget_low_f32( r1.val[1] ), vget_low_f32( r2.val[1] ) ); + b2StoreW( (float*)( states + indices[1] ), body2 ); + } + + if ( indices[2] != B2_NULL_INDEX ) + { + float32x4_t body3 = vcombine_f32( vget_high_f32( r1.val[0] ), vget_high_f32( r2.val[0] ) ); + b2StoreW( (float*)( states + indices[2] ), body3 ); + } + + if ( indices[3] != B2_NULL_INDEX ) + { + float32x4_t body4 = vcombine_f32( vget_high_f32( r1.val[1] ), vget_high_f32( r2.val[1] ) ); + b2StoreW( (float*)( states + indices[3] ), body4 ); + } +} + +#elif defined( B2_SIMD_SSE2 ) + +// This is a load and transpose +static b2SimdBody b2GatherBodies( const b2BodyState* B2_RESTRICT states, int* B2_RESTRICT indices ) +{ + _Static_assert( sizeof( b2BodyState ) == 32, "b2BodyState not 32 bytes" ); + B2_ASSERT( ( (uintptr_t)states & 0x1F ) == 0 ); + + // [vx vy w flags] + b2FloatW identityA = b2ZeroW(); + + // [dpx dpy dqc dqs] + b2FloatW identityB = b2SetW( 0.0f, 0.0f, 1.0f, 0.0f ); + + b2FloatW b1a = indices[0] == B2_NULL_INDEX ? identityA : b2LoadW( (float*)( states + indices[0] ) + 0 ); + b2FloatW b1b = indices[0] == B2_NULL_INDEX ? identityB : b2LoadW( (float*)( states + indices[0] ) + 4 ); + b2FloatW b2a = indices[1] == B2_NULL_INDEX ? identityA : b2LoadW( (float*)( states + indices[1] ) + 0 ); + b2FloatW b2b = indices[1] == B2_NULL_INDEX ? identityB : b2LoadW( (float*)( states + indices[1] ) + 4 ); + b2FloatW b3a = indices[2] == B2_NULL_INDEX ? identityA : b2LoadW( (float*)( states + indices[2] ) + 0 ); + b2FloatW b3b = indices[2] == B2_NULL_INDEX ? identityB : b2LoadW( (float*)( states + indices[2] ) + 4 ); + b2FloatW b4a = indices[3] == B2_NULL_INDEX ? identityA : b2LoadW( (float*)( states + indices[3] ) + 0 ); + b2FloatW b4b = indices[3] == B2_NULL_INDEX ? identityB : b2LoadW( (float*)( states + indices[3] ) + 4 ); + + // [vx1 vx3 vy1 vy3] + b2FloatW t1a = b2UnpackLoW( b1a, b3a ); + + // [vx2 vx4 vy2 vy4] + b2FloatW t2a = b2UnpackLoW( b2a, b4a ); + + // [w1 w3 f1 f3] + b2FloatW t3a = b2UnpackHiW( b1a, b3a ); + + // [w2 w4 f2 f4] + b2FloatW t4a = b2UnpackHiW( b2a, b4a ); + + b2SimdBody simdBody; + simdBody.v.X = b2UnpackLoW( t1a, t2a ); + simdBody.v.Y = b2UnpackHiW( t1a, t2a ); + simdBody.w = b2UnpackLoW( t3a, t4a ); + simdBody.flags = b2UnpackHiW( t3a, t4a ); + + b2FloatW t1b = b2UnpackLoW( b1b, b3b ); + b2FloatW t2b = b2UnpackLoW( b2b, b4b ); + b2FloatW t3b = b2UnpackHiW( b1b, b3b ); + b2FloatW t4b = b2UnpackHiW( b2b, b4b ); + + simdBody.dp.X = b2UnpackLoW( t1b, t2b ); + simdBody.dp.Y = b2UnpackHiW( t1b, t2b ); + simdBody.dq.C = b2UnpackLoW( t3b, t4b ); + simdBody.dq.S = b2UnpackHiW( t3b, t4b ); + + return simdBody; +} + +// This writes only the velocities back to the solver bodies +static void b2ScatterBodies( b2BodyState* B2_RESTRICT states, int* B2_RESTRICT indices, const b2SimdBody* B2_RESTRICT simdBody ) +{ + _Static_assert( sizeof( b2BodyState ) == 32, "b2BodyState not 32 bytes" ); + B2_ASSERT( ( (uintptr_t)states & 0x1F ) == 0 ); + + // [vx1 vy1 vx2 vy2] + b2FloatW t1 = b2UnpackLoW( simdBody->v.X, simdBody->v.Y ); + // [vx3 vy3 vx4 vy4] + b2FloatW t2 = b2UnpackHiW( simdBody->v.X, simdBody->v.Y ); + // [w1 f1 w2 f2] + b2FloatW t3 = b2UnpackLoW( simdBody->w, simdBody->flags ); + // [w3 f3 w4 f4] + b2FloatW t4 = b2UnpackHiW( simdBody->w, simdBody->flags ); + + // I don't use any dummy body in the body array because this will lead to multithreaded sharing and the + // associated cache flushing. + if ( indices[0] != B2_NULL_INDEX ) + { + // [t1.x t1.y t3.x t3.y] + b2StoreW( (float*)( states + indices[0] ), _mm_shuffle_ps( t1, t3, _MM_SHUFFLE( 1, 0, 1, 0 ) ) ); + } + + if ( indices[1] != B2_NULL_INDEX ) + { + // [t1.z t1.w t3.z t3.w] + b2StoreW( (float*)( states + indices[1] ), _mm_shuffle_ps( t1, t3, _MM_SHUFFLE( 3, 2, 3, 2 ) ) ); + } + + if ( indices[2] != B2_NULL_INDEX ) + { + // [t2.x t2.y t4.x t4.y] + b2StoreW( (float*)( states + indices[2] ), _mm_shuffle_ps( t2, t4, _MM_SHUFFLE( 1, 0, 1, 0 ) ) ); + } + + if ( indices[3] != B2_NULL_INDEX ) + { + // [t2.z t2.w t4.z t4.w] + b2StoreW( (float*)( states + indices[3] ), _mm_shuffle_ps( t2, t4, _MM_SHUFFLE( 3, 2, 3, 2 ) ) ); + } +} + +#else + +// This is a load and transpose +static b2SimdBody b2GatherBodies( const b2BodyState* B2_RESTRICT states, int* B2_RESTRICT indices ) +{ + b2BodyState identity = b2_identityBodyState; + + b2BodyState s1 = indices[0] == B2_NULL_INDEX ? identity : states[indices[0]]; + b2BodyState s2 = indices[1] == B2_NULL_INDEX ? identity : states[indices[1]]; + b2BodyState s3 = indices[2] == B2_NULL_INDEX ? identity : states[indices[2]]; + b2BodyState s4 = indices[3] == B2_NULL_INDEX ? identity : states[indices[3]]; + + b2SimdBody simdBody; + simdBody.v.X = ( b2FloatW ){ s1.linearVelocity.x, s2.linearVelocity.x, s3.linearVelocity.x, s4.linearVelocity.x }; + simdBody.v.Y = ( b2FloatW ){ s1.linearVelocity.y, s2.linearVelocity.y, s3.linearVelocity.y, s4.linearVelocity.y }; + simdBody.w = ( b2FloatW ){ s1.angularVelocity, s2.angularVelocity, s3.angularVelocity, s4.angularVelocity }; + simdBody.flags = ( b2FloatW ){ (float)s1.flags, (float)s2.flags, (float)s3.flags, (float)s4.flags }; + simdBody.dp.X = ( b2FloatW ){ s1.deltaPosition.x, s2.deltaPosition.x, s3.deltaPosition.x, s4.deltaPosition.x }; + simdBody.dp.Y = ( b2FloatW ){ s1.deltaPosition.y, s2.deltaPosition.y, s3.deltaPosition.y, s4.deltaPosition.y }; + simdBody.dq.C = ( b2FloatW ){ s1.deltaRotation.c, s2.deltaRotation.c, s3.deltaRotation.c, s4.deltaRotation.c }; + simdBody.dq.S = ( b2FloatW ){ s1.deltaRotation.s, s2.deltaRotation.s, s3.deltaRotation.s, s4.deltaRotation.s }; + + return simdBody; +} + +// This writes only the velocities back to the solver bodies +static void b2ScatterBodies( b2BodyState* B2_RESTRICT states, int* B2_RESTRICT indices, const b2SimdBody* B2_RESTRICT simdBody ) +{ + if ( indices[0] != B2_NULL_INDEX ) + { + b2BodyState* state = states + indices[0]; + state->linearVelocity.x = simdBody->v.X.x; + state->linearVelocity.y = simdBody->v.Y.x; + state->angularVelocity = simdBody->w.x; + } + + if ( indices[1] != B2_NULL_INDEX ) + { + b2BodyState* state = states + indices[1]; + state->linearVelocity.x = simdBody->v.X.y; + state->linearVelocity.y = simdBody->v.Y.y; + state->angularVelocity = simdBody->w.y; + } + + if ( indices[2] != B2_NULL_INDEX ) + { + b2BodyState* state = states + indices[2]; + state->linearVelocity.x = simdBody->v.X.z; + state->linearVelocity.y = simdBody->v.Y.z; + state->angularVelocity = simdBody->w.z; + } + + if ( indices[3] != B2_NULL_INDEX ) + { + b2BodyState* state = states + indices[3]; + state->linearVelocity.x = simdBody->v.X.w; + state->linearVelocity.y = simdBody->v.Y.w; + state->angularVelocity = simdBody->w.w; + } +} + +#endif + +void b2PrepareContactsTask( int startIndex, int endIndex, b2StepContext* context ) +{ + b2TracyCZoneNC( prepare_contact, "Prepare Contact", b2_colorYellow, true ); + b2World* world = context->world; + b2ContactSim** contacts = context->contacts; + b2ContactConstraintSIMD* constraints = context->simdContactConstraints; + b2BodyState* awakeStates = context->states; +#if B2_VALIDATE + b2Body* bodies = world->bodies.data; +#endif + + // Stiffer for static contacts to avoid bodies getting pushed through the ground + b2Softness contactSoftness = context->contactSoftness; + b2Softness staticSoftness = context->staticSoftness; + + float warmStartScale = world->enableWarmStarting ? 1.0f : 0.0f; + + for ( int i = startIndex; i < endIndex; ++i ) + { + b2ContactConstraintSIMD* constraint = constraints + i; + + for ( int j = 0; j < B2_SIMD_WIDTH; ++j ) + { + b2ContactSim* contactSim = contacts[B2_SIMD_WIDTH * i + j]; + + if ( contactSim != NULL ) + { + const b2Manifold* manifold = &contactSim->manifold; + + int indexA = contactSim->bodySimIndexA; + int indexB = contactSim->bodySimIndexB; + +#if B2_VALIDATE + b2Body* bodyA = bodies + contactSim->bodyIdA; + int validIndexA = bodyA->setIndex == b2_awakeSet ? bodyA->localIndex : B2_NULL_INDEX; + b2Body* bodyB = bodies + contactSim->bodyIdB; + int validIndexB = bodyB->setIndex == b2_awakeSet ? bodyB->localIndex : B2_NULL_INDEX; + + B2_ASSERT( indexA == validIndexA ); + B2_ASSERT( indexB == validIndexB ); +#endif + constraint->indexA[j] = indexA; + constraint->indexB[j] = indexB; + + b2Vec2 vA = b2Vec2_zero; + float wA = 0.0f; + float mA = contactSim->invMassA; + float iA = contactSim->invIA; + if ( indexA != B2_NULL_INDEX ) + { + b2BodyState* stateA = awakeStates + indexA; + vA = stateA->linearVelocity; + wA = stateA->angularVelocity; + } + + b2Vec2 vB = b2Vec2_zero; + float wB = 0.0f; + float mB = contactSim->invMassB; + float iB = contactSim->invIB; + if ( indexB != B2_NULL_INDEX ) + { + b2BodyState* stateB = awakeStates + indexB; + vB = stateB->linearVelocity; + wB = stateB->angularVelocity; + } + + ( (float*)&constraint->invMassA )[j] = mA; + ( (float*)&constraint->invMassB )[j] = mB; + ( (float*)&constraint->invIA )[j] = iA; + ( (float*)&constraint->invIB )[j] = iB; + + b2Softness soft = ( indexA == B2_NULL_INDEX || indexB == B2_NULL_INDEX ) ? staticSoftness : contactSoftness; + + b2Vec2 normal = manifold->normal; + ( (float*)&constraint->normal.X )[j] = normal.x; + ( (float*)&constraint->normal.Y )[j] = normal.y; + + ( (float*)&constraint->friction )[j] = contactSim->friction; + ( (float*)&constraint->restitution )[j] = contactSim->restitution; + ( (float*)&constraint->biasRate )[j] = soft.biasRate; + ( (float*)&constraint->massScale )[j] = soft.massScale; + ( (float*)&constraint->impulseScale )[j] = soft.impulseScale; + + b2Vec2 tangent = b2RightPerp( normal ); + + { + const b2ManifoldPoint* mp = manifold->points + 0; + + b2Vec2 rA = mp->anchorA; + b2Vec2 rB = mp->anchorB; + + ( (float*)&constraint->anchorA1.X )[j] = rA.x; + ( (float*)&constraint->anchorA1.Y )[j] = rA.y; + ( (float*)&constraint->anchorB1.X )[j] = rB.x; + ( (float*)&constraint->anchorB1.Y )[j] = rB.y; + + ( (float*)&constraint->baseSeparation1 )[j] = mp->separation - b2Dot( b2Sub( rB, rA ), normal ); + + ( (float*)&constraint->normalImpulse1 )[j] = warmStartScale * mp->normalImpulse; + ( (float*)&constraint->tangentImpulse1 )[j] = warmStartScale * mp->tangentImpulse; + ( (float*)&constraint->maxNormalImpulse1 )[j] = 0.0f; + + float rnA = b2Cross( rA, normal ); + float rnB = b2Cross( rB, normal ); + float kNormal = mA + mB + iA * rnA * rnA + iB * rnB * rnB; + ( (float*)&constraint->normalMass1 )[j] = kNormal > 0.0f ? 1.0f / kNormal : 0.0f; + + float rtA = b2Cross( rA, tangent ); + float rtB = b2Cross( rB, tangent ); + float kTangent = mA + mB + iA * rtA * rtA + iB * rtB * rtB; + ( (float*)&constraint->tangentMass1 )[j] = kTangent > 0.0f ? 1.0f / kTangent : 0.0f; + + // relative velocity for restitution + b2Vec2 vrA = b2Add( vA, b2CrossSV( wA, rA ) ); + b2Vec2 vrB = b2Add( vB, b2CrossSV( wB, rB ) ); + ( (float*)&constraint->relativeVelocity1 )[j] = b2Dot( normal, b2Sub( vrB, vrA ) ); + } + + int pointCount = manifold->pointCount; + B2_ASSERT( 0 < pointCount && pointCount <= 2 ); + + if ( pointCount == 2 ) + { + const b2ManifoldPoint* mp = manifold->points + 1; + + b2Vec2 rA = mp->anchorA; + b2Vec2 rB = mp->anchorB; + + ( (float*)&constraint->anchorA2.X )[j] = rA.x; + ( (float*)&constraint->anchorA2.Y )[j] = rA.y; + ( (float*)&constraint->anchorB2.X )[j] = rB.x; + ( (float*)&constraint->anchorB2.Y )[j] = rB.y; + + ( (float*)&constraint->baseSeparation2 )[j] = mp->separation - b2Dot( b2Sub( rB, rA ), normal ); + + ( (float*)&constraint->normalImpulse2 )[j] = warmStartScale * mp->normalImpulse; + ( (float*)&constraint->tangentImpulse2 )[j] = warmStartScale * mp->tangentImpulse; + ( (float*)&constraint->maxNormalImpulse2 )[j] = 0.0f; + + float rnA = b2Cross( rA, normal ); + float rnB = b2Cross( rB, normal ); + float kNormal = mA + mB + iA * rnA * rnA + iB * rnB * rnB; + ( (float*)&constraint->normalMass2 )[j] = kNormal > 0.0f ? 1.0f / kNormal : 0.0f; + + float rtA = b2Cross( rA, tangent ); + float rtB = b2Cross( rB, tangent ); + float kTangent = mA + mB + iA * rtA * rtA + iB * rtB * rtB; + ( (float*)&constraint->tangentMass2 )[j] = kTangent > 0.0f ? 1.0f / kTangent : 0.0f; + + // relative velocity for restitution + b2Vec2 vrA = b2Add( vA, b2CrossSV( wA, rA ) ); + b2Vec2 vrB = b2Add( vB, b2CrossSV( wB, rB ) ); + ( (float*)&constraint->relativeVelocity2 )[j] = b2Dot( normal, b2Sub( vrB, vrA ) ); + } + else + { + // dummy data that has no effect + ( (float*)&constraint->baseSeparation2 )[j] = 0.0f; + ( (float*)&constraint->normalImpulse2 )[j] = 0.0f; + ( (float*)&constraint->tangentImpulse2 )[j] = 0.0f; + ( (float*)&constraint->maxNormalImpulse2 )[j] = 0.0f; + ( (float*)&constraint->anchorA2.X )[j] = 0.0f; + ( (float*)&constraint->anchorA2.Y )[j] = 0.0f; + ( (float*)&constraint->anchorB2.X )[j] = 0.0f; + ( (float*)&constraint->anchorB2.Y )[j] = 0.0f; + ( (float*)&constraint->normalMass2 )[j] = 0.0f; + ( (float*)&constraint->tangentMass2 )[j] = 0.0f; + ( (float*)&constraint->relativeVelocity2 )[j] = 0.0f; + } + } + else + { + // SIMD remainder + constraint->indexA[j] = B2_NULL_INDEX; + constraint->indexB[j] = B2_NULL_INDEX; + + ( (float*)&constraint->invMassA )[j] = 0.0f; + ( (float*)&constraint->invMassB )[j] = 0.0f; + ( (float*)&constraint->invIA )[j] = 0.0f; + ( (float*)&constraint->invIB )[j] = 0.0f; + + ( (float*)&constraint->normal.X )[j] = 0.0f; + ( (float*)&constraint->normal.Y )[j] = 0.0f; + ( (float*)&constraint->friction )[j] = 0.0f; + ( (float*)&constraint->biasRate )[j] = 0.0f; + ( (float*)&constraint->massScale )[j] = 0.0f; + ( (float*)&constraint->impulseScale )[j] = 0.0f; + + ( (float*)&constraint->anchorA1.X )[j] = 0.0f; + ( (float*)&constraint->anchorA1.Y )[j] = 0.0f; + ( (float*)&constraint->anchorB1.X )[j] = 0.0f; + ( (float*)&constraint->anchorB1.Y )[j] = 0.0f; + ( (float*)&constraint->baseSeparation1 )[j] = 0.0f; + ( (float*)&constraint->normalImpulse1 )[j] = 0.0f; + ( (float*)&constraint->tangentImpulse1 )[j] = 0.0f; + ( (float*)&constraint->maxNormalImpulse1 )[j] = 0.0f; + ( (float*)&constraint->normalMass1 )[j] = 0.0f; + ( (float*)&constraint->tangentMass1 )[j] = 0.0f; + + ( (float*)&constraint->anchorA2.X )[j] = 0.0f; + ( (float*)&constraint->anchorA2.Y )[j] = 0.0f; + ( (float*)&constraint->anchorB2.X )[j] = 0.0f; + ( (float*)&constraint->anchorB2.Y )[j] = 0.0f; + ( (float*)&constraint->baseSeparation2 )[j] = 0.0f; + ( (float*)&constraint->normalImpulse2 )[j] = 0.0f; + ( (float*)&constraint->tangentImpulse2 )[j] = 0.0f; + ( (float*)&constraint->maxNormalImpulse2 )[j] = 0.0f; + ( (float*)&constraint->normalMass2 )[j] = 0.0f; + ( (float*)&constraint->tangentMass2 )[j] = 0.0f; + + ( (float*)&constraint->restitution )[j] = 0.0f; + ( (float*)&constraint->relativeVelocity1 )[j] = 0.0f; + ( (float*)&constraint->relativeVelocity2 )[j] = 0.0f; + } + } + } + + b2TracyCZoneEnd( prepare_contact ); +} + +void b2WarmStartContactsTask( int startIndex, int endIndex, b2StepContext* context, int colorIndex ) +{ + b2TracyCZoneNC( warm_start_contact, "Warm Start", b2_colorGreen, true ); + + b2BodyState* states = context->states; + b2ContactConstraintSIMD* constraints = context->graph->colors[colorIndex].simdConstraints; + + for ( int i = startIndex; i < endIndex; ++i ) + { + b2ContactConstraintSIMD* c = constraints + i; + b2SimdBody bA = b2GatherBodies( states, c->indexA ); + b2SimdBody bB = b2GatherBodies( states, c->indexB ); + + b2FloatW tangentX = c->normal.Y; + b2FloatW tangentY = b2SubW( b2ZeroW(), c->normal.X ); + + { + // fixed anchors + b2Vec2W rA = c->anchorA1; + b2Vec2W rB = c->anchorB1; + + b2Vec2W P; + P.X = b2AddW( b2MulW( c->normalImpulse1, c->normal.X ), b2MulW( c->tangentImpulse1, tangentX ) ); + P.Y = b2AddW( b2MulW( c->normalImpulse1, c->normal.Y ), b2MulW( c->tangentImpulse1, tangentY ) ); + bA.w = b2MulSubW( bA.w, c->invIA, b2CrossW( rA, P ) ); + bA.v.X = b2MulSubW( bA.v.X, c->invMassA, P.X ); + bA.v.Y = b2MulSubW( bA.v.Y, c->invMassA, P.Y ); + bB.w = b2MulAddW( bB.w, c->invIB, b2CrossW( rB, P ) ); + bB.v.X = b2MulAddW( bB.v.X, c->invMassB, P.X ); + bB.v.Y = b2MulAddW( bB.v.Y, c->invMassB, P.Y ); + } + + { + // fixed anchors + b2Vec2W rA = c->anchorA2; + b2Vec2W rB = c->anchorB2; + + b2Vec2W P; + P.X = b2AddW( b2MulW( c->normalImpulse2, c->normal.X ), b2MulW( c->tangentImpulse2, tangentX ) ); + P.Y = b2AddW( b2MulW( c->normalImpulse2, c->normal.Y ), b2MulW( c->tangentImpulse2, tangentY ) ); + bA.w = b2MulSubW( bA.w, c->invIA, b2CrossW( rA, P ) ); + bA.v.X = b2MulSubW( bA.v.X, c->invMassA, P.X ); + bA.v.Y = b2MulSubW( bA.v.Y, c->invMassA, P.Y ); + bB.w = b2MulAddW( bB.w, c->invIB, b2CrossW( rB, P ) ); + bB.v.X = b2MulAddW( bB.v.X, c->invMassB, P.X ); + bB.v.Y = b2MulAddW( bB.v.Y, c->invMassB, P.Y ); + } + + b2ScatterBodies( states, c->indexA, &bA ); + b2ScatterBodies( states, c->indexB, &bB ); + } + + b2TracyCZoneEnd( warm_start_contact ); +} + +void b2SolveContactsTask( int startIndex, int endIndex, b2StepContext* context, int colorIndex, bool useBias ) +{ + b2TracyCZoneNC( solve_contact, "Solve Contact", b2_colorAliceBlue, true ); + + b2BodyState* states = context->states; + b2ContactConstraintSIMD* constraints = context->graph->colors[colorIndex].simdConstraints; + b2FloatW inv_h = b2SplatW( context->inv_h ); + b2FloatW minBiasVel = b2SplatW( -context->world->contactPushoutVelocity ); + + for ( int i = startIndex; i < endIndex; ++i ) + { + b2ContactConstraintSIMD* c = constraints + i; + + b2SimdBody bA = b2GatherBodies( states, c->indexA ); + b2SimdBody bB = b2GatherBodies( states, c->indexB ); + + b2FloatW biasRate, massScale, impulseScale; + if ( useBias ) + { + biasRate = c->biasRate; + massScale = c->massScale; + impulseScale = c->impulseScale; + } + else + { + biasRate = b2ZeroW(); + massScale = b2SplatW( 1.0f ); + impulseScale = b2ZeroW(); + } + + b2Vec2W dp = { b2SubW( bB.dp.X, bA.dp.X ), b2SubW( bB.dp.Y, bA.dp.Y ) }; + + // point1 non-penetration constraint + { + // moving anchors for current separation + b2Vec2W rsA = b2RotateVectorW( bA.dq, c->anchorA1 ); + b2Vec2W rsB = b2RotateVectorW( bB.dq, c->anchorB1 ); + + // compute current separation + // this is subject to round-off error if the anchor is far from the body center of mass + b2Vec2W ds = { b2AddW( dp.X, b2SubW( rsB.X, rsA.X ) ), b2AddW( dp.Y, b2SubW( rsB.Y, rsA.Y ) ) }; + b2FloatW s = b2AddW( b2DotW( c->normal, ds ), c->baseSeparation1 ); + + // Apply speculative bias if separation is greater than zero, otherwise apply soft constraint bias + b2FloatW mask = b2GreaterThanW( s, b2ZeroW() ); + b2FloatW specBias = b2MulW( s, inv_h ); + b2FloatW softBias = b2MaxW( b2MulW( biasRate, s ), minBiasVel ); + b2FloatW bias = b2BlendW( softBias, specBias, mask ); + + // fixed anchors for Jacobians + b2Vec2W rA = c->anchorA1; + b2Vec2W rB = c->anchorB1; + + // Relative velocity at contact + b2FloatW dvx = b2SubW( b2SubW( bB.v.X, b2MulW( bB.w, rB.Y ) ), b2SubW( bA.v.X, b2MulW( bA.w, rA.Y ) ) ); + b2FloatW dvy = b2SubW( b2AddW( bB.v.Y, b2MulW( bB.w, rB.X ) ), b2AddW( bA.v.Y, b2MulW( bA.w, rA.X ) ) ); + b2FloatW vn = b2AddW( b2MulW( dvx, c->normal.X ), b2MulW( dvy, c->normal.Y ) ); + + // Compute normal impulse + b2FloatW negImpulse = b2AddW( b2MulW( c->normalMass1, b2MulW( massScale, b2AddW( vn, bias ) ) ), + b2MulW( impulseScale, c->normalImpulse1 ) ); + + // Clamp the accumulated impulse + b2FloatW newImpulse = b2MaxW( b2SubW( c->normalImpulse1, negImpulse ), b2ZeroW() ); + b2FloatW impulse = b2SubW( newImpulse, c->normalImpulse1 ); + c->normalImpulse1 = newImpulse; + c->maxNormalImpulse1 = b2MaxW( c->maxNormalImpulse1, newImpulse ); + + // Apply contact impulse + b2FloatW Px = b2MulW( impulse, c->normal.X ); + b2FloatW Py = b2MulW( impulse, c->normal.Y ); + + bA.v.X = b2MulSubW( bA.v.X, c->invMassA, Px ); + bA.v.Y = b2MulSubW( bA.v.Y, c->invMassA, Py ); + bA.w = b2MulSubW( bA.w, c->invIA, b2SubW( b2MulW( rA.X, Py ), b2MulW( rA.Y, Px ) ) ); + + bB.v.X = b2MulAddW( bB.v.X, c->invMassB, Px ); + bB.v.Y = b2MulAddW( bB.v.Y, c->invMassB, Py ); + bB.w = b2MulAddW( bB.w, c->invIB, b2SubW( b2MulW( rB.X, Py ), b2MulW( rB.Y, Px ) ) ); + } + + // second point non-penetration constraint + { + // moving anchors for current separation + b2Vec2W rsA = b2RotateVectorW( bA.dq, c->anchorA2 ); + b2Vec2W rsB = b2RotateVectorW( bB.dq, c->anchorB2 ); + + // compute current separation + b2Vec2W ds = { b2AddW( dp.X, b2SubW( rsB.X, rsA.X ) ), b2AddW( dp.Y, b2SubW( rsB.Y, rsA.Y ) ) }; + b2FloatW s = b2AddW( b2DotW( c->normal, ds ), c->baseSeparation2 ); + + b2FloatW mask = b2GreaterThanW( s, b2ZeroW() ); + b2FloatW specBias = b2MulW( s, inv_h ); + b2FloatW softBias = b2MaxW( b2MulW( biasRate, s ), minBiasVel ); + b2FloatW bias = b2BlendW( softBias, specBias, mask ); + + // fixed anchors for Jacobians + b2Vec2W rA = c->anchorA2; + b2Vec2W rB = c->anchorB2; + + // Relative velocity at contact + b2FloatW dvx = b2SubW( b2SubW( bB.v.X, b2MulW( bB.w, rB.Y ) ), b2SubW( bA.v.X, b2MulW( bA.w, rA.Y ) ) ); + b2FloatW dvy = b2SubW( b2AddW( bB.v.Y, b2MulW( bB.w, rB.X ) ), b2AddW( bA.v.Y, b2MulW( bA.w, rA.X ) ) ); + b2FloatW vn = b2AddW( b2MulW( dvx, c->normal.X ), b2MulW( dvy, c->normal.Y ) ); + + // Compute normal impulse + b2FloatW negImpulse = b2AddW( b2MulW( c->normalMass2, b2MulW( massScale, b2AddW( vn, bias ) ) ), + b2MulW( impulseScale, c->normalImpulse2 ) ); + + // Clamp the accumulated impulse + b2FloatW newImpulse = b2MaxW( b2SubW( c->normalImpulse2, negImpulse ), b2ZeroW() ); + b2FloatW impulse = b2SubW( newImpulse, c->normalImpulse2 ); + c->normalImpulse2 = newImpulse; + c->maxNormalImpulse2 = b2MaxW( c->maxNormalImpulse2, newImpulse ); + + // Apply contact impulse + b2FloatW Px = b2MulW( impulse, c->normal.X ); + b2FloatW Py = b2MulW( impulse, c->normal.Y ); + + bA.v.X = b2MulSubW( bA.v.X, c->invMassA, Px ); + bA.v.Y = b2MulSubW( bA.v.Y, c->invMassA, Py ); + bA.w = b2MulSubW( bA.w, c->invIA, b2SubW( b2MulW( rA.X, Py ), b2MulW( rA.Y, Px ) ) ); + + bB.v.X = b2MulAddW( bB.v.X, c->invMassB, Px ); + bB.v.Y = b2MulAddW( bB.v.Y, c->invMassB, Py ); + bB.w = b2MulAddW( bB.w, c->invIB, b2SubW( b2MulW( rB.X, Py ), b2MulW( rB.Y, Px ) ) ); + } + + b2FloatW tangentX = c->normal.Y; + b2FloatW tangentY = b2SubW( b2ZeroW(), c->normal.X ); + + // point 1 friction constraint + { + // fixed anchors for Jacobians + b2Vec2W rA = c->anchorA1; + b2Vec2W rB = c->anchorB1; + + // Relative velocity at contact + b2FloatW dvx = b2SubW( b2SubW( bB.v.X, b2MulW( bB.w, rB.Y ) ), b2SubW( bA.v.X, b2MulW( bA.w, rA.Y ) ) ); + b2FloatW dvy = b2SubW( b2AddW( bB.v.Y, b2MulW( bB.w, rB.X ) ), b2AddW( bA.v.Y, b2MulW( bA.w, rA.X ) ) ); + b2FloatW vt = b2AddW( b2MulW( dvx, tangentX ), b2MulW( dvy, tangentY ) ); + + // Compute tangent force + b2FloatW negImpulse = b2MulW( c->tangentMass1, vt ); + + // Clamp the accumulated force + b2FloatW maxFriction = b2MulW( c->friction, c->normalImpulse1 ); + b2FloatW newImpulse = b2SubW( c->tangentImpulse1, negImpulse ); + newImpulse = b2MaxW( b2SubW( b2ZeroW(), maxFriction ), b2MinW( newImpulse, maxFriction ) ); + b2FloatW impulse = b2SubW( newImpulse, c->tangentImpulse1 ); + c->tangentImpulse1 = newImpulse; + + // Apply contact impulse + b2FloatW Px = b2MulW( impulse, tangentX ); + b2FloatW Py = b2MulW( impulse, tangentY ); + + bA.v.X = b2MulSubW( bA.v.X, c->invMassA, Px ); + bA.v.Y = b2MulSubW( bA.v.Y, c->invMassA, Py ); + bA.w = b2MulSubW( bA.w, c->invIA, b2SubW( b2MulW( rA.X, Py ), b2MulW( rA.Y, Px ) ) ); + + bB.v.X = b2MulAddW( bB.v.X, c->invMassB, Px ); + bB.v.Y = b2MulAddW( bB.v.Y, c->invMassB, Py ); + bB.w = b2MulAddW( bB.w, c->invIB, b2SubW( b2MulW( rB.X, Py ), b2MulW( rB.Y, Px ) ) ); + } + + // second point friction constraint + { + // fixed anchors for Jacobians + b2Vec2W rA = c->anchorA2; + b2Vec2W rB = c->anchorB2; + + // Relative velocity at contact + b2FloatW dvx = b2SubW( b2SubW( bB.v.X, b2MulW( bB.w, rB.Y ) ), b2SubW( bA.v.X, b2MulW( bA.w, rA.Y ) ) ); + b2FloatW dvy = b2SubW( b2AddW( bB.v.Y, b2MulW( bB.w, rB.X ) ), b2AddW( bA.v.Y, b2MulW( bA.w, rA.X ) ) ); + b2FloatW vt = b2AddW( b2MulW( dvx, tangentX ), b2MulW( dvy, tangentY ) ); + + // Compute tangent force + b2FloatW negImpulse = b2MulW( c->tangentMass2, vt ); + + // Clamp the accumulated force + b2FloatW maxFriction = b2MulW( c->friction, c->normalImpulse2 ); + b2FloatW newImpulse = b2SubW( c->tangentImpulse2, negImpulse ); + newImpulse = b2MaxW( b2SubW( b2ZeroW(), maxFriction ), b2MinW( newImpulse, maxFriction ) ); + b2FloatW impulse = b2SubW( newImpulse, c->tangentImpulse2 ); + c->tangentImpulse2 = newImpulse; + + // Apply contact impulse + b2FloatW Px = b2MulW( impulse, tangentX ); + b2FloatW Py = b2MulW( impulse, tangentY ); + + bA.v.X = b2MulSubW( bA.v.X, c->invMassA, Px ); + bA.v.Y = b2MulSubW( bA.v.Y, c->invMassA, Py ); + bA.w = b2MulSubW( bA.w, c->invIA, b2SubW( b2MulW( rA.X, Py ), b2MulW( rA.Y, Px ) ) ); + + bB.v.X = b2MulAddW( bB.v.X, c->invMassB, Px ); + bB.v.Y = b2MulAddW( bB.v.Y, c->invMassB, Py ); + bB.w = b2MulAddW( bB.w, c->invIB, b2SubW( b2MulW( rB.X, Py ), b2MulW( rB.Y, Px ) ) ); + } + + b2ScatterBodies( states, c->indexA, &bA ); + b2ScatterBodies( states, c->indexB, &bB ); + } + + b2TracyCZoneEnd( solve_contact ); +} + +void b2ApplyRestitutionTask( int startIndex, int endIndex, b2StepContext* context, int colorIndex ) +{ + b2TracyCZoneNC( restitution, "Restitution", b2_colorDodgerBlue, true ); + + b2BodyState* states = context->states; + b2ContactConstraintSIMD* constraints = context->graph->colors[colorIndex].simdConstraints; + b2FloatW threshold = b2SplatW( context->world->restitutionThreshold ); + b2FloatW zero = b2ZeroW(); + + for ( int i = startIndex; i < endIndex; ++i ) + { + b2ContactConstraintSIMD* c = constraints + i; + + b2SimdBody bA = b2GatherBodies( states, c->indexA ); + b2SimdBody bB = b2GatherBodies( states, c->indexB ); + + // first point non-penetration constraint + { + // Set effective mass to zero if restitution should not be applied + b2FloatW mask1 = b2GreaterThanW( b2AddW( c->relativeVelocity1, threshold ), zero ); + b2FloatW mask2 = b2EqualsW( c->maxNormalImpulse1, zero ); + b2FloatW mask = b2OrW( mask1, mask2 ); + b2FloatW mass = b2BlendW( c->normalMass1, zero, mask ); + + // fixed anchors for Jacobians + b2Vec2W rA = c->anchorA1; + b2Vec2W rB = c->anchorB1; + + // Relative velocity at contact + b2FloatW dvx = b2SubW( b2SubW( bB.v.X, b2MulW( bB.w, rB.Y ) ), b2SubW( bA.v.X, b2MulW( bA.w, rA.Y ) ) ); + b2FloatW dvy = b2SubW( b2AddW( bB.v.Y, b2MulW( bB.w, rB.X ) ), b2AddW( bA.v.Y, b2MulW( bA.w, rA.X ) ) ); + b2FloatW vn = b2AddW( b2MulW( dvx, c->normal.X ), b2MulW( dvy, c->normal.Y ) ); + + // Compute normal impulse + b2FloatW negImpulse = b2MulW( mass, b2AddW( vn, b2MulW( c->restitution, c->relativeVelocity1 ) ) ); + + // Clamp the accumulated impulse + b2FloatW newImpulse = b2MaxW( b2SubW( c->normalImpulse1, negImpulse ), b2ZeroW() ); + b2FloatW impulse = b2SubW( newImpulse, c->normalImpulse1 ); + c->normalImpulse1 = newImpulse; + + // Apply contact impulse + b2FloatW Px = b2MulW( impulse, c->normal.X ); + b2FloatW Py = b2MulW( impulse, c->normal.Y ); + + bA.v.X = b2MulSubW( bA.v.X, c->invMassA, Px ); + bA.v.Y = b2MulSubW( bA.v.Y, c->invMassA, Py ); + bA.w = b2MulSubW( bA.w, c->invIA, b2SubW( b2MulW( rA.X, Py ), b2MulW( rA.Y, Px ) ) ); + + bB.v.X = b2MulAddW( bB.v.X, c->invMassB, Px ); + bB.v.Y = b2MulAddW( bB.v.Y, c->invMassB, Py ); + bB.w = b2MulAddW( bB.w, c->invIB, b2SubW( b2MulW( rB.X, Py ), b2MulW( rB.Y, Px ) ) ); + } + + // second point non-penetration constraint + { + // Set effective mass to zero if restitution should not be applied + b2FloatW mask1 = b2GreaterThanW( b2AddW( c->relativeVelocity2, threshold ), zero ); + b2FloatW mask2 = b2EqualsW( c->maxNormalImpulse2, zero ); + b2FloatW mask = b2OrW( mask1, mask2 ); + b2FloatW mass = b2BlendW( c->normalMass2, zero, mask ); + + // fixed anchors for Jacobians + b2Vec2W rA = c->anchorA2; + b2Vec2W rB = c->anchorB2; + + // Relative velocity at contact + b2FloatW dvx = b2SubW( b2SubW( bB.v.X, b2MulW( bB.w, rB.Y ) ), b2SubW( bA.v.X, b2MulW( bA.w, rA.Y ) ) ); + b2FloatW dvy = b2SubW( b2AddW( bB.v.Y, b2MulW( bB.w, rB.X ) ), b2AddW( bA.v.Y, b2MulW( bA.w, rA.X ) ) ); + b2FloatW vn = b2AddW( b2MulW( dvx, c->normal.X ), b2MulW( dvy, c->normal.Y ) ); + + // Compute normal impulse + b2FloatW negImpulse = b2MulW( mass, b2AddW( vn, b2MulW( c->restitution, c->relativeVelocity2 ) ) ); + + // Clamp the accumulated impulse + b2FloatW newImpulse = b2MaxW( b2SubW( c->normalImpulse2, negImpulse ), b2ZeroW() ); + b2FloatW impulse = b2SubW( newImpulse, c->normalImpulse2 ); + c->normalImpulse2 = newImpulse; + + // Apply contact impulse + b2FloatW Px = b2MulW( impulse, c->normal.X ); + b2FloatW Py = b2MulW( impulse, c->normal.Y ); + + bA.v.X = b2MulSubW( bA.v.X, c->invMassA, Px ); + bA.v.Y = b2MulSubW( bA.v.Y, c->invMassA, Py ); + bA.w = b2MulSubW( bA.w, c->invIA, b2SubW( b2MulW( rA.X, Py ), b2MulW( rA.Y, Px ) ) ); + + bB.v.X = b2MulAddW( bB.v.X, c->invMassB, Px ); + bB.v.Y = b2MulAddW( bB.v.Y, c->invMassB, Py ); + bB.w = b2MulAddW( bB.w, c->invIB, b2SubW( b2MulW( rB.X, Py ), b2MulW( rB.Y, Px ) ) ); + } + + b2ScatterBodies( states, c->indexA, &bA ); + b2ScatterBodies( states, c->indexB, &bB ); + } + + b2TracyCZoneEnd( restitution ); +} + +#if B2_SIMD_WIDTH == 8 + +void b2StoreImpulsesTask( int startIndex, int endIndex, b2StepContext* context ) +{ + b2TracyCZoneNC( store_impulses, "Store", b2_colorFirebrick, true ); + + b2ContactSim** contacts = context->contacts; + const b2ContactConstraintSIMD* constraints = context->simdContactConstraints; + + b2Manifold dummy = { 0 }; + + for ( int i = startIndex; i < endIndex; ++i ) + { + const b2ContactConstraintSIMD* c = constraints + i; + const float* normalImpulse1 = (float*)&c->normalImpulse1; + const float* normalImpulse2 = (float*)&c->normalImpulse2; + const float* tangentImpulse1 = (float*)&c->tangentImpulse1; + const float* tangentImpulse2 = (float*)&c->tangentImpulse2; + const float* maxNormalImpulse1 = (float*)&c->maxNormalImpulse1; + const float* maxNormalImpulse2 = (float*)&c->maxNormalImpulse2; + const float* normalVelocity1 = (float*)&c->relativeVelocity1; + const float* normalVelocity2 = (float*)&c->relativeVelocity2; + + int base = 8 * i; + b2Manifold* m0 = contacts[base + 0] == NULL ? &dummy : &contacts[base + 0]->manifold; + b2Manifold* m1 = contacts[base + 1] == NULL ? &dummy : &contacts[base + 1]->manifold; + b2Manifold* m2 = contacts[base + 2] == NULL ? &dummy : &contacts[base + 2]->manifold; + b2Manifold* m3 = contacts[base + 3] == NULL ? &dummy : &contacts[base + 3]->manifold; + b2Manifold* m4 = contacts[base + 4] == NULL ? &dummy : &contacts[base + 4]->manifold; + b2Manifold* m5 = contacts[base + 5] == NULL ? &dummy : &contacts[base + 5]->manifold; + b2Manifold* m6 = contacts[base + 6] == NULL ? &dummy : &contacts[base + 6]->manifold; + b2Manifold* m7 = contacts[base + 7] == NULL ? &dummy : &contacts[base + 7]->manifold; + + m0->points[0].normalImpulse = normalImpulse1[0]; + m0->points[0].tangentImpulse = tangentImpulse1[0]; + m0->points[0].maxNormalImpulse = maxNormalImpulse1[0]; + m0->points[0].normalVelocity = normalVelocity1[0]; + + m0->points[1].normalImpulse = normalImpulse2[0]; + m0->points[1].tangentImpulse = tangentImpulse2[0]; + m0->points[1].maxNormalImpulse = maxNormalImpulse2[0]; + m0->points[1].normalVelocity = normalVelocity2[0]; + + m1->points[0].normalImpulse = normalImpulse1[1]; + m1->points[0].tangentImpulse = tangentImpulse1[1]; + m1->points[0].maxNormalImpulse = maxNormalImpulse1[1]; + m1->points[0].normalVelocity = normalVelocity1[1]; + + m1->points[1].normalImpulse = normalImpulse2[1]; + m1->points[1].tangentImpulse = tangentImpulse2[1]; + m1->points[1].maxNormalImpulse = maxNormalImpulse2[1]; + m1->points[1].normalVelocity = normalVelocity2[1]; + + m2->points[0].normalImpulse = normalImpulse1[2]; + m2->points[0].tangentImpulse = tangentImpulse1[2]; + m2->points[0].maxNormalImpulse = maxNormalImpulse1[2]; + m2->points[0].normalVelocity = normalVelocity1[2]; + + m2->points[1].normalImpulse = normalImpulse2[2]; + m2->points[1].tangentImpulse = tangentImpulse2[2]; + m2->points[1].maxNormalImpulse = maxNormalImpulse2[2]; + m2->points[1].normalVelocity = normalVelocity2[2]; + + m3->points[0].normalImpulse = normalImpulse1[3]; + m3->points[0].tangentImpulse = tangentImpulse1[3]; + m3->points[0].maxNormalImpulse = maxNormalImpulse1[3]; + m3->points[0].normalVelocity = normalVelocity1[3]; + + m3->points[1].normalImpulse = normalImpulse2[3]; + m3->points[1].tangentImpulse = tangentImpulse2[3]; + m3->points[1].maxNormalImpulse = maxNormalImpulse2[3]; + m3->points[1].normalVelocity = normalVelocity2[3]; + + m4->points[0].normalImpulse = normalImpulse1[4]; + m4->points[0].tangentImpulse = tangentImpulse1[4]; + m4->points[0].maxNormalImpulse = maxNormalImpulse1[4]; + m4->points[0].normalVelocity = normalVelocity1[4]; + + m4->points[1].normalImpulse = normalImpulse2[4]; + m4->points[1].tangentImpulse = tangentImpulse2[4]; + m4->points[1].maxNormalImpulse = maxNormalImpulse2[4]; + m4->points[1].normalVelocity = normalVelocity2[4]; + + m5->points[0].normalImpulse = normalImpulse1[5]; + m5->points[0].tangentImpulse = tangentImpulse1[5]; + m5->points[0].maxNormalImpulse = maxNormalImpulse1[5]; + m5->points[0].normalVelocity = normalVelocity1[5]; + + m5->points[1].normalImpulse = normalImpulse2[5]; + m5->points[1].tangentImpulse = tangentImpulse2[5]; + m5->points[1].maxNormalImpulse = maxNormalImpulse2[5]; + m5->points[1].normalVelocity = normalVelocity2[5]; + + m6->points[0].normalImpulse = normalImpulse1[6]; + m6->points[0].tangentImpulse = tangentImpulse1[6]; + m6->points[0].maxNormalImpulse = maxNormalImpulse1[6]; + m6->points[0].normalVelocity = normalVelocity1[6]; + + m6->points[1].normalImpulse = normalImpulse2[6]; + m6->points[1].tangentImpulse = tangentImpulse2[6]; + m6->points[1].maxNormalImpulse = maxNormalImpulse2[6]; + m6->points[1].normalVelocity = normalVelocity2[6]; + + m7->points[0].normalImpulse = normalImpulse1[7]; + m7->points[0].tangentImpulse = tangentImpulse1[7]; + m7->points[0].maxNormalImpulse = maxNormalImpulse1[7]; + m7->points[0].normalVelocity = normalVelocity1[7]; + + m7->points[1].normalImpulse = normalImpulse2[7]; + m7->points[1].tangentImpulse = tangentImpulse2[7]; + m7->points[1].maxNormalImpulse = maxNormalImpulse2[7]; + m7->points[1].normalVelocity = normalVelocity2[7]; + } + + b2TracyCZoneEnd( store_impulses ); +} + +#else + +void b2StoreImpulsesTask( int startIndex, int endIndex, b2StepContext* context ) +{ + b2TracyCZoneNC( store_impulses, "Store", b2_colorFirebrick, true ); + + b2ContactSim** contacts = context->contacts; + const b2ContactConstraintSIMD* constraints = context->simdContactConstraints; + + b2Manifold dummy = { 0 }; + + for ( int i = startIndex; i < endIndex; ++i ) + { + const b2ContactConstraintSIMD* c = constraints + i; + const float* normalImpulse1 = (float*)&c->normalImpulse1; + const float* normalImpulse2 = (float*)&c->normalImpulse2; + const float* tangentImpulse1 = (float*)&c->tangentImpulse1; + const float* tangentImpulse2 = (float*)&c->tangentImpulse2; + const float* maxNormalImpulse1 = (float*)&c->maxNormalImpulse1; + const float* maxNormalImpulse2 = (float*)&c->maxNormalImpulse2; + const float* normalVelocity1 = (float*)&c->relativeVelocity1; + const float* normalVelocity2 = (float*)&c->relativeVelocity2; + + int base = 4 * i; + b2Manifold* m0 = contacts[base + 0] == NULL ? &dummy : &contacts[base + 0]->manifold; + b2Manifold* m1 = contacts[base + 1] == NULL ? &dummy : &contacts[base + 1]->manifold; + b2Manifold* m2 = contacts[base + 2] == NULL ? &dummy : &contacts[base + 2]->manifold; + b2Manifold* m3 = contacts[base + 3] == NULL ? &dummy : &contacts[base + 3]->manifold; + + m0->points[0].normalImpulse = normalImpulse1[0]; + m0->points[0].tangentImpulse = tangentImpulse1[0]; + m0->points[0].maxNormalImpulse = maxNormalImpulse1[0]; + m0->points[0].normalVelocity = normalVelocity1[0]; + + m0->points[1].normalImpulse = normalImpulse2[0]; + m0->points[1].tangentImpulse = tangentImpulse2[0]; + m0->points[1].maxNormalImpulse = maxNormalImpulse2[0]; + m0->points[1].normalVelocity = normalVelocity2[0]; + + m1->points[0].normalImpulse = normalImpulse1[1]; + m1->points[0].tangentImpulse = tangentImpulse1[1]; + m1->points[0].maxNormalImpulse = maxNormalImpulse1[1]; + m1->points[0].normalVelocity = normalVelocity1[1]; + + m1->points[1].normalImpulse = normalImpulse2[1]; + m1->points[1].tangentImpulse = tangentImpulse2[1]; + m1->points[1].maxNormalImpulse = maxNormalImpulse2[1]; + m1->points[1].normalVelocity = normalVelocity2[1]; + + m2->points[0].normalImpulse = normalImpulse1[2]; + m2->points[0].tangentImpulse = tangentImpulse1[2]; + m2->points[0].maxNormalImpulse = maxNormalImpulse1[2]; + m2->points[0].normalVelocity = normalVelocity1[2]; + + m2->points[1].normalImpulse = normalImpulse2[2]; + m2->points[1].tangentImpulse = tangentImpulse2[2]; + m2->points[1].maxNormalImpulse = maxNormalImpulse2[2]; + m2->points[1].normalVelocity = normalVelocity2[2]; + + m3->points[0].normalImpulse = normalImpulse1[3]; + m3->points[0].tangentImpulse = tangentImpulse1[3]; + m3->points[0].maxNormalImpulse = maxNormalImpulse1[3]; + m3->points[0].normalVelocity = normalVelocity1[3]; + + m3->points[1].normalImpulse = normalImpulse2[3]; + m3->points[1].tangentImpulse = tangentImpulse2[3]; + m3->points[1].maxNormalImpulse = maxNormalImpulse2[3]; + m3->points[1].normalVelocity = normalVelocity2[3]; + } + + b2TracyCZoneEnd( store_impulses ); +} + +#endif diff --git a/3rdparty/box2d/src/contact_solver.h b/3rdparty/box2d/src/contact_solver.h new file mode 100644 index 000000000000..911e6a9d840f --- /dev/null +++ b/3rdparty/box2d/src/contact_solver.h @@ -0,0 +1,50 @@ +// SPDX-FileCopyrightText: 2023 Erin Catto +// SPDX-License-Identifier: MIT + +#pragma once + +#include "solver.h" + +typedef struct b2ContactSim b2ContactSim; + +typedef struct b2ContactConstraintPoint +{ + b2Vec2 anchorA, anchorB; + float baseSeparation; + float relativeVelocity; + float normalImpulse; + float tangentImpulse; + float maxNormalImpulse; + float normalMass; + float tangentMass; +} b2ContactConstraintPoint; + +typedef struct b2ContactConstraint +{ + int indexA; + int indexB; + b2ContactConstraintPoint points[2]; + b2Vec2 normal; + float invMassA, invMassB; + float invIA, invIB; + float friction; + float restitution; + b2Softness softness; + int pointCount; +} b2ContactConstraint; + +int b2GetContactConstraintSIMDByteCount( void ); + +// Overflow contacts don't fit into the constraint graph coloring +void b2PrepareOverflowContacts( b2StepContext* context ); +void b2WarmStartOverflowContacts( b2StepContext* context ); +void b2SolveOverflowContacts( b2StepContext* context, bool useBias ); +void b2ApplyOverflowRestitution( b2StepContext* context ); +void b2StoreOverflowImpulses( b2StepContext* context ); + +// Contacts that live within the constraint graph coloring +void b2PrepareContactsTask( int startIndex, int endIndex, b2StepContext* context ); +void b2WarmStartContactsTask( int startIndex, int endIndex, b2StepContext* context, int colorIndex ); +void b2SolveContactsTask( int startIndex, int endIndex, b2StepContext* context, int colorIndex, bool useBias ); +void b2ApplyRestitutionTask( int startIndex, int endIndex, b2StepContext* context, int colorIndex ); +void b2StoreImpulsesTask( int startIndex, int endIndex, b2StepContext* context ); diff --git a/3rdparty/box2d/src/core.c b/3rdparty/box2d/src/core.c new file mode 100644 index 000000000000..030fb72827ae --- /dev/null +++ b/3rdparty/box2d/src/core.c @@ -0,0 +1,169 @@ +// SPDX-FileCopyrightText: 2023 Erin Catto +// SPDX-License-Identifier: MIT + +#include "core.h" + +#if defined( B2_COMPILER_MSVC ) +#define _CRTDBG_MAP_ALLOC +#include +#include +#else +#include +#endif + +#include +#include + +#ifdef BOX2D_PROFILE + +#include +#define b2TracyCAlloc( ptr, size ) TracyCAlloc( ptr, size ) +#define b2TracyCFree( ptr ) TracyCFree( ptr ) + +#else + +#define b2TracyCAlloc( ptr, size ) +#define b2TracyCFree( ptr ) + +#endif + +#include "box2d/math_functions.h" + +#include + +// This allows the user to change the length units at runtime +float b2_lengthUnitsPerMeter = 1.0f; + +void b2SetLengthUnitsPerMeter( float lengthUnits ) +{ + B2_ASSERT( b2IsValid( lengthUnits ) && lengthUnits > 0.0f ); + b2_lengthUnitsPerMeter = lengthUnits; +} + +float b2GetLengthUnitsPerMeter( void ) +{ + return b2_lengthUnitsPerMeter; +} + +static int b2DefaultAssertFcn( const char* condition, const char* fileName, int lineNumber ) +{ + printf( "BOX2D ASSERTION: %s, %s, line %d\n", condition, fileName, lineNumber ); + + // return non-zero to break to debugger + return 1; +} + +b2AssertFcn* b2AssertHandler = b2DefaultAssertFcn; + +void b2SetAssertFcn( b2AssertFcn* assertFcn ) +{ + B2_ASSERT( assertFcn != NULL ); + b2AssertHandler = assertFcn; +} + +b2Version b2GetVersion( void ) +{ + return ( b2Version ){ 3, 1, 0 }; +} + +static b2AllocFcn* b2_allocFcn = NULL; +static b2FreeFcn* b2_freeFcn = NULL; + +static _Atomic int b2_byteCount; + +void b2SetAllocator( b2AllocFcn* allocFcn, b2FreeFcn* freeFcn ) +{ + b2_allocFcn = allocFcn; + b2_freeFcn = freeFcn; +} + +// Use 32 byte alignment for everything. Works with 256bit SIMD. +#define B2_ALIGNMENT 32 + +void* b2Alloc( int size ) +{ + if (size == 0) + { + return NULL; + } + + // This could cause some sharing issues, however Box2D rarely calls b2Alloc. + atomic_fetch_add_explicit( &b2_byteCount, size, memory_order_relaxed ); + + // Allocation must be a multiple of 32 or risk a seg fault + // https://en.cppreference.com/w/c/memory/aligned_alloc + int size32 = ( ( size - 1 ) | 0x1F ) + 1; + + if ( b2_allocFcn != NULL ) + { + void* ptr = b2_allocFcn( size32, B2_ALIGNMENT ); + b2TracyCAlloc( ptr, size ); + + B2_ASSERT( ptr != NULL ); + B2_ASSERT( ( (uintptr_t)ptr & 0x1F ) == 0 ); + + return ptr; + } + +#ifdef B2_PLATFORM_WINDOWS + void* ptr = _aligned_malloc( size32, B2_ALIGNMENT ); +#elif defined( B2_PLATFORM_ANDROID ) + void* ptr = NULL; + if ( posix_memalign( &ptr, B2_ALIGNMENT, size32 ) != 0 ) + { + // allocation failed, exit the application + exit( EXIT_FAILURE ); + } +#else + void* ptr = aligned_alloc( B2_ALIGNMENT, size32 ); +#endif + + b2TracyCAlloc( ptr, size ); + + B2_ASSERT( ptr != NULL ); + B2_ASSERT( ( (uintptr_t)ptr & 0x1F ) == 0 ); + + return ptr; +} + +void b2Free( void* mem, int size ) +{ + if ( mem == NULL ) + { + return; + } + + b2TracyCFree( mem ); + + if ( b2_freeFcn != NULL ) + { + b2_freeFcn( mem ); + } + else + { +#ifdef B2_PLATFORM_WINDOWS + _aligned_free( mem ); +#else + free( mem ); +#endif + } + + atomic_fetch_sub_explicit( &b2_byteCount, size, memory_order_relaxed ); +} + +void* b2GrowAlloc( void* oldMem, int oldSize, int newSize ) +{ + B2_ASSERT( newSize > oldSize ); + void* newMem = b2Alloc( newSize ); + if ( oldSize > 0 ) + { + memcpy( newMem, oldMem, oldSize ); + b2Free( oldMem, oldSize ); + } + return newMem; +} + +int b2GetByteCount( void ) +{ + return atomic_load_explicit( &b2_byteCount, memory_order_relaxed ); +} diff --git a/3rdparty/box2d/src/core.h b/3rdparty/box2d/src/core.h new file mode 100644 index 000000000000..283a224292b7 --- /dev/null +++ b/3rdparty/box2d/src/core.h @@ -0,0 +1,183 @@ +// SPDX-FileCopyrightText: 2023 Erin Catto +// SPDX-License-Identifier: MIT + +#pragma once + +#include "box2d/base.h" + +// clang-format off + +#define B2_NULL_INDEX ( -1 ) + +// for performance comparisons +#define B2_RESTRICT restrict + +#ifdef NDEBUG + #define B2_DEBUG 0 +#else + #define B2_DEBUG 1 +#endif + +#if defined( BOX2D_VALIDATE ) && !defined( NDEBUG ) + #define B2_VALIDATE 1 +#else + #define B2_VALIDATE 0 +#endif + +// Define platform +#if defined( _WIN64 ) + #define B2_PLATFORM_WINDOWS +#elif defined( __ANDROID__ ) + #define B2_PLATFORM_ANDROID +#elif defined( __linux__ ) + #define B2_PLATFORM_LINUX +#elif defined( __APPLE__ ) + #include + #if defined( TARGET_OS_IPHONE ) && !TARGET_OS_IPHONE + #define B2_PLATFORM_MACOS + #else + #define B2_PLATFORM_IOS + #endif +#elif defined( __EMSCRIPTEN__ ) + #define B2_PLATFORM_WASM +#else + #define B2_PLATFORM_UNKNOWN +#endif + +// Define CPU +#if defined( __x86_64__ ) || defined( _M_X64 ) || defined( __i386__ ) || defined( _M_IX86 ) + #define B2_CPU_X86_X64 +#elif defined( __aarch64__ ) || defined( _M_ARM64 ) || defined( __arm__ ) || defined( _M_ARM ) + #define B2_CPU_ARM +#elif defined( __EMSCRIPTEN__ ) + #define B2_CPU_WASM +#else + #define B2_CPU_UNKNOWN +#endif + +// Define SIMD +#if defined( BOX2D_ENABLE_SIMD ) + #if defined( B2_CPU_X86_X64 ) + #if defined( BOX2D_AVX2 ) + #define B2_SIMD_AVX2 + #define B2_SIMD_WIDTH 8 + #else + #define B2_SIMD_SSE2 + #define B2_SIMD_WIDTH 4 + #endif + #elif defined( B2_CPU_ARM ) + #define B2_SIMD_NEON + #define B2_SIMD_WIDTH 4 + #elif defined( B2_CPU_WASM ) + #define B2_CPU_WASM + #define B2_SIMD_SSE2 + #define B2_SIMD_WIDTH 4 + #else + #define B2_SIMD_NONE + #define B2_SIMD_WIDTH 4 + #endif +#else + #define B2_SIMD_NONE + #define B2_SIMD_WIDTH 4 +#endif + +// Define compiler +#if defined( __clang__ ) + #define B2_COMPILER_CLANG +#elif defined( __GNUC__ ) + #define B2_COMPILER_GCC +#elif defined( _MSC_VER ) + #define B2_COMPILER_MSVC +#endif + +// see https://github.com/scottt/debugbreak +#if defined( B2_COMPILER_MSVC ) + #define B2_BREAKPOINT __debugbreak() +#elif defined( B2_COMPILER_GCC ) || defined( B2_COMPILER_CLANG ) + #define B2_BREAKPOINT __builtin_trap() +#else + // Unknown compiler + #include + #definef B2_BREAKPOINT assert(0) +#endif + +#if !defined( NDEBUG ) || defined( B2_ENABLE_ASSERT ) + extern b2AssertFcn* b2AssertHandler; + #define B2_ASSERT( condition ) \ + do \ + { \ + if ( !( condition ) && b2AssertHandler( #condition, __FILE__, (int)__LINE__ ) ) \ + B2_BREAKPOINT; \ + } \ + while ( 0 ) +#else + #define B2_ASSERT( ... ) ( (void)0 ) +#endif + +/// Tracy profiler instrumentation +/// https://github.com/wolfpld/tracy +#ifdef BOX2D_PROFILE + #include + #define b2TracyCZoneC( ctx, color, active ) TracyCZoneC( ctx, color, active ) + #define b2TracyCZoneNC( ctx, name, color, active ) TracyCZoneNC( ctx, name, color, active ) + #define b2TracyCZoneEnd( ctx ) TracyCZoneEnd( ctx ) +#else + #define b2TracyCZoneC( ctx, color, active ) + #define b2TracyCZoneNC( ctx, name, color, active ) + #define b2TracyCZoneEnd( ctx ) +#endif + +// clang-format on + +extern float b2_lengthUnitsPerMeter; + +// Used to detect bad values. Positions greater than about 16km will have precision +// problems, so 100km as a limit should be fine in all cases. +#define b2_huge ( 100000.0f * b2_lengthUnitsPerMeter ) + +// Maximum parallel workers. Used to size some static arrays. +#define b2_maxWorkers 64 + +// Maximum number of colors in the constraint graph. Constraints that cannot +// find a color are added to the overflow set which are solved single-threaded. +#define b2_graphColorCount 12 + +// A small length used as a collision and constraint tolerance. Usually it is +// chosen to be numerically significant, but visually insignificant. In meters. +// @warning modifying this can have a significant impact on stability +#define b2_linearSlop ( 0.005f * b2_lengthUnitsPerMeter ) + +// Maximum number of simultaneous worlds that can be allocated +#define b2_maxWorlds 128 + +// The maximum rotation of a body per time step. This limit is very large and is used +// to prevent numerical problems. You shouldn't need to adjust this. +// @warning increasing this to 0.5f * b2_pi or greater will break continuous collision. +#define b2_maxRotation ( 0.25f * b2_pi ) + +// @warning modifying this can have a significant impact on performance and stability +#define b2_speculativeDistance ( 4.0f * b2_linearSlop ) + +// This is used to fatten AABBs in the dynamic tree. This allows proxies +// to move by a small amount without triggering a tree adjustment. +// This is in meters. +// @warning modifying this can have a significant impact on performance +#define b2_aabbMargin ( 0.1f * b2_lengthUnitsPerMeter ) + +// The time that a body must be still before it will go to sleep. In seconds. +#define b2_timeToSleep 0.5f + +// Returns the number of elements of an array +#define B2_ARRAY_COUNT( A ) (int)( sizeof( A ) / sizeof( A[0] ) ) + +// Used to prevent the compiler from warning about unused variables +#define B2_MAYBE_UNUSED( x ) ( (void)( x ) ) + +// Use to validate definitions. Do not take my cookie. +#define B2_SECRET_COOKIE 1152023 + +#define b2CheckDef( DEF ) B2_ASSERT( DEF->internalValue == B2_SECRET_COOKIE ) + +void* b2Alloc( int size ); +void b2Free( void* mem, int size ); +void* b2GrowAlloc( void* oldMem, int oldSize, int newSize ); diff --git a/3rdparty/box2d/src/ctz.h b/3rdparty/box2d/src/ctz.h new file mode 100644 index 000000000000..9959527c0871 --- /dev/null +++ b/3rdparty/box2d/src/ctz.h @@ -0,0 +1,112 @@ +// SPDX-FileCopyrightText: 2023 Erin Catto +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +#if defined( _MSC_VER ) && !defined( __clang__ ) + #include + +// https://en.wikipedia.org/wiki/Find_first_set + +static inline uint32_t b2CTZ32( uint32_t block ) +{ + unsigned long index; + _BitScanForward( &index, block ); + return index; +} + +// This function doesn't need to be fast, so using the Ivy Bridge fallback. +static inline uint32_t b2CLZ32( uint32_t value ) +{ + #if 1 + + // Use BSR (Bit Scan Reverse) which is available on Ivy Bridge + unsigned long index; + if ( _BitScanReverse( &index, value ) ) + { + // BSR gives the index of the most significant 1-bit + // We need to invert this to get the number of leading zeros + return 31 - index; + } + else + { + // If x is 0, BSR sets the zero flag and doesn't modify index + // LZCNT should return 32 for an input of 0 + return 32; + } + + #else + + return __lzcnt( value ); + + #endif +} + +static inline uint32_t b2CTZ64( uint64_t block ) +{ + unsigned long index; + + #ifdef _WIN64 + _BitScanForward64( &index, block ); + #else + // 32-bit fall back + if ( (uint32_t)block != 0 ) + { + _BitScanForward( &index, (uint32_t)block ); + } + else + { + _BitScanForward( &index, (uint32_t)( block >> 32 ) ); + index += 32; + } + #endif + + return index; +} + +#else + +static inline uint32_t b2CTZ32( uint32_t block ) +{ + return __builtin_ctz( block ); +} + +static inline uint32_t b2CLZ32( uint32_t value ) +{ + return __builtin_clz( value ); +} + +static inline uint32_t b2CTZ64( uint64_t block ) +{ + return __builtin_ctzll( block ); +} + +#endif + +static inline bool b2IsPowerOf2( int x ) +{ + return ( x & ( x - 1 ) ) == 0; +} + +static inline int b2BoundingPowerOf2( int x ) +{ + if ( x <= 1 ) + { + return 1; + } + + return 32 - (int)b2CLZ32( (uint32_t)x - 1 ); +} + +static inline int b2RoundUpPowerOf2( int x ) +{ + if ( x <= 1 ) + { + return 1; + } + + return 1 << ( 32 - (int)b2CLZ32( (uint32_t)x - 1 ) ); +} diff --git a/3rdparty/box2d/src/distance.c b/3rdparty/box2d/src/distance.c new file mode 100644 index 000000000000..25f2abc2c1c1 --- /dev/null +++ b/3rdparty/box2d/src/distance.c @@ -0,0 +1,1237 @@ + +// SPDX-FileCopyrightText: 2023 Erin Catto +// SPDX-License-Identifier: MIT + +#include "core.h" + +#include "box2d/collision.h" +#include "box2d/math_functions.h" + +#include +#include + +b2Transform b2GetSweepTransform( const b2Sweep* sweep, float time ) +{ + // https://fgiesen.wordpress.com/2012/08/15/linear-interpolation-past-present-and-future/ + b2Transform xf; + xf.p = b2Add( b2MulSV( 1.0f - time, sweep->c1 ), b2MulSV( time, sweep->c2 ) ); + + b2Rot q = { + ( 1.0f - time ) * sweep->q1.c + time * sweep->q2.c, + ( 1.0f - time ) * sweep->q1.s + time * sweep->q2.s, + }; + + xf.q = b2NormalizeRot( q ); + + // Shift to origin + xf.p = b2Sub( xf.p, b2RotateVector( xf.q, sweep->localCenter ) ); + return xf; +} + +/// Follows Ericson 5.1.9 Closest Points of Two Line Segments +b2SegmentDistanceResult b2SegmentDistance( b2Vec2 p1, b2Vec2 q1, b2Vec2 p2, b2Vec2 q2 ) +{ + b2SegmentDistanceResult result = { 0 }; + + b2Vec2 d1 = b2Sub( q1, p1 ); + b2Vec2 d2 = b2Sub( q2, p2 ); + b2Vec2 r = b2Sub( p1, p2 ); + float dd1 = b2Dot( d1, d1 ); + float dd2 = b2Dot( d2, d2 ); + float rd1 = b2Dot( r, d1 ); + float rd2 = b2Dot( r, d2 ); + + const float epsSqr = FLT_EPSILON * FLT_EPSILON; + + if ( dd1 < epsSqr || dd2 < epsSqr ) + { + // Handle all degeneracies + if ( dd1 >= epsSqr ) + { + // Segment 2 is degenerate + result.fraction1 = b2ClampFloat( -rd1 / dd1, 0.0f, 1.0f ); + result.fraction2 = 0.0f; + } + else if ( dd2 >= epsSqr ) + { + // Segment 1 is degenerate + result.fraction1 = 0.0f; + result.fraction2 = b2ClampFloat( rd2 / dd2, 0.0f, 1.0f ); + } + else + { + result.fraction1 = 0.0f; + result.fraction2 = 0.0f; + } + } + else + { + // Non-degenerate segments + float d12 = b2Dot( d1, d2 ); + + float denom = dd1 * dd2 - d12 * d12; + + // Fraction on segment 1 + float f1 = 0.0f; + if ( denom != 0.0f ) + { + // not parallel + f1 = b2ClampFloat( ( d12 * rd2 - rd1 * dd2 ) / denom, 0.0f, 1.0f ); + } + + // Compute point on segment 2 closest to p1 + f1 * d1 + float f2 = ( d12 * f1 + rd2 ) / dd2; + + // Clamping of segment 2 requires a do over on segment 1 + if ( f2 < 0.0f ) + { + f2 = 0.0f; + f1 = b2ClampFloat( -rd1 / dd1, 0.0f, 1.0f ); + } + else if ( f2 > 1.0f ) + { + f2 = 1.0f; + f1 = b2ClampFloat( ( d12 - rd1 ) / dd1, 0.0f, 1.0f ); + } + + result.fraction1 = f1; + result.fraction2 = f2; + } + + result.closest1 = b2MulAdd( p1, result.fraction1, d1 ); + result.closest2 = b2MulAdd( p2, result.fraction2, d2 ); + result.distanceSquared = b2DistanceSquared( result.closest1, result.closest2 ); + return result; +} + +// GJK using Voronoi regions (Christer Ericson) and Barycentric coordinates. +// todo try not copying +b2DistanceProxy b2MakeProxy( const b2Vec2* vertices, int count, float radius ) +{ + count = b2MinInt( count, b2_maxPolygonVertices ); + b2DistanceProxy proxy; + for ( int i = 0; i < count; ++i ) + { + proxy.points[i] = vertices[i]; + } + proxy.count = count; + proxy.radius = radius; + return proxy; +} + +static b2Vec2 b2Weight2( float a1, b2Vec2 w1, float a2, b2Vec2 w2 ) +{ + return ( b2Vec2 ){ a1 * w1.x + a2 * w2.x, a1 * w1.y + a2 * w2.y }; +} + +static b2Vec2 b2Weight3( float a1, b2Vec2 w1, float a2, b2Vec2 w2, float a3, b2Vec2 w3 ) +{ + return ( b2Vec2 ){ a1 * w1.x + a2 * w2.x + a3 * w3.x, a1 * w1.y + a2 * w2.y + a3 * w3.y }; +} + +static int b2FindSupport( const b2DistanceProxy* proxy, b2Vec2 direction ) +{ + int bestIndex = 0; + float bestValue = b2Dot( proxy->points[0], direction ); + for ( int i = 1; i < proxy->count; ++i ) + { + float value = b2Dot( proxy->points[i], direction ); + if ( value > bestValue ) + { + bestIndex = i; + bestValue = value; + } + } + + return bestIndex; +} + +static b2Simplex b2MakeSimplexFromCache( const b2DistanceCache* cache, const b2DistanceProxy* proxyA, b2Transform transformA, + const b2DistanceProxy* proxyB, b2Transform transformB ) +{ + B2_ASSERT( cache->count <= 3 ); + b2Simplex s; + + // Copy data from cache. + s.count = cache->count; + + b2SimplexVertex* vertices[] = { &s.v1, &s.v2, &s.v3 }; + for ( int i = 0; i < s.count; ++i ) + { + b2SimplexVertex* v = vertices[i]; + v->indexA = cache->indexA[i]; + v->indexB = cache->indexB[i]; + b2Vec2 wALocal = proxyA->points[v->indexA]; + b2Vec2 wBLocal = proxyB->points[v->indexB]; + v->wA = b2TransformPoint( transformA, wALocal ); + v->wB = b2TransformPoint( transformB, wBLocal ); + v->w = b2Sub( v->wB, v->wA ); + + // invalid + v->a = -1.0f; + } + + // If the cache is empty or invalid ... + if ( s.count == 0 ) + { + b2SimplexVertex* v = vertices[0]; + v->indexA = 0; + v->indexB = 0; + b2Vec2 wALocal = proxyA->points[0]; + b2Vec2 wBLocal = proxyB->points[0]; + v->wA = b2TransformPoint( transformA, wALocal ); + v->wB = b2TransformPoint( transformB, wBLocal ); + v->w = b2Sub( v->wB, v->wA ); + v->a = 1.0f; + s.count = 1; + } + + return s; +} + +static void b2MakeSimplexCache( b2DistanceCache* cache, const b2Simplex* simplex ) +{ + cache->count = (uint16_t)simplex->count; + const b2SimplexVertex* vertices[] = { &simplex->v1, &simplex->v2, &simplex->v3 }; + for ( int i = 0; i < simplex->count; ++i ) + { + cache->indexA[i] = (uint8_t)vertices[i]->indexA; + cache->indexB[i] = (uint8_t)vertices[i]->indexB; + } +} + +// Compute the search direction from the current simplex. +// This is the vector pointing from the closest point on the simplex +// to the origin. +// A more accurate search direction can be computed by using the normal +// vector of the simplex. For example, the normal vector of a line segment +// can be computed more accurately because it does not involve barycentric +// coordinates. +b2Vec2 b2ComputeSimplexSearchDirection( const b2Simplex* simplex ) +{ + switch ( simplex->count ) + { + case 1: + return b2Neg( simplex->v1.w ); + + case 2: + { + b2Vec2 e12 = b2Sub( simplex->v2.w, simplex->v1.w ); + float sgn = b2Cross( e12, b2Neg( simplex->v1.w ) ); + if ( sgn > 0.0f ) + { + // Origin is left of e12. + return b2LeftPerp( e12 ); + } + else + { + // Origin is right of e12. + return b2RightPerp( e12 ); + } + } + + default: + B2_ASSERT( false ); + return b2Vec2_zero; + } +} + +b2Vec2 b2ComputeSimplexClosestPoint( const b2Simplex* s ) +{ + switch ( s->count ) + { + case 0: + B2_ASSERT( false ); + return b2Vec2_zero; + + case 1: + return s->v1.w; + + case 2: + return b2Weight2( s->v1.a, s->v1.w, s->v2.a, s->v2.w ); + + case 3: + return b2Vec2_zero; + + default: + B2_ASSERT( false ); + return b2Vec2_zero; + } +} + +void b2ComputeSimplexWitnessPoints( b2Vec2* a, b2Vec2* b, const b2Simplex* s ) +{ + switch ( s->count ) + { + case 0: + B2_ASSERT( false ); + break; + + case 1: + *a = s->v1.wA; + *b = s->v1.wB; + break; + + case 2: + *a = b2Weight2( s->v1.a, s->v1.wA, s->v2.a, s->v2.wA ); + *b = b2Weight2( s->v1.a, s->v1.wB, s->v2.a, s->v2.wB ); + break; + + case 3: + *a = b2Weight3( s->v1.a, s->v1.wA, s->v2.a, s->v2.wA, s->v3.a, s->v3.wA ); + // TODO_ERIN why are these not equal? + //*b = b2Weight3(s->v1.a, s->v1.wB, s->v2.a, s->v2.wB, s->v3.a, s->v3.wB); + *b = *a; + break; + + default: + B2_ASSERT( false ); + break; + } +} + +// Solve a line segment using barycentric coordinates. +// +// p = a1 * w1 + a2 * w2 +// a1 + a2 = 1 +// +// The vector from the origin to the closest point on the line is +// perpendicular to the line. +// e12 = w2 - w1 +// dot(p, e) = 0 +// a1 * dot(w1, e) + a2 * dot(w2, e) = 0 +// +// 2-by-2 linear system +// [1 1 ][a1] = [1] +// [w1.e12 w2.e12][a2] = [0] +// +// Define +// d12_1 = dot(w2, e12) +// d12_2 = -dot(w1, e12) +// d12 = d12_1 + d12_2 +// +// Solution +// a1 = d12_1 / d12 +// a2 = d12_2 / d12 +void b2SolveSimplex2( b2Simplex* s ) +{ + b2Vec2 w1 = s->v1.w; + b2Vec2 w2 = s->v2.w; + b2Vec2 e12 = b2Sub( w2, w1 ); + + // w1 region + float d12_2 = -b2Dot( w1, e12 ); + if ( d12_2 <= 0.0f ) + { + // a2 <= 0, so we clamp it to 0 + s->v1.a = 1.0f; + s->count = 1; + return; + } + + // w2 region + float d12_1 = b2Dot( w2, e12 ); + if ( d12_1 <= 0.0f ) + { + // a1 <= 0, so we clamp it to 0 + s->v2.a = 1.0f; + s->count = 1; + s->v1 = s->v2; + return; + } + + // Must be in e12 region. + float inv_d12 = 1.0f / ( d12_1 + d12_2 ); + s->v1.a = d12_1 * inv_d12; + s->v2.a = d12_2 * inv_d12; + s->count = 2; +} + +void b2SolveSimplex3( b2Simplex* s ) +{ + b2Vec2 w1 = s->v1.w; + b2Vec2 w2 = s->v2.w; + b2Vec2 w3 = s->v3.w; + + // Edge12 + // [1 1 ][a1] = [1] + // [w1.e12 w2.e12][a2] = [0] + // a3 = 0 + b2Vec2 e12 = b2Sub( w2, w1 ); + float w1e12 = b2Dot( w1, e12 ); + float w2e12 = b2Dot( w2, e12 ); + float d12_1 = w2e12; + float d12_2 = -w1e12; + + // Edge13 + // [1 1 ][a1] = [1] + // [w1.e13 w3.e13][a3] = [0] + // a2 = 0 + b2Vec2 e13 = b2Sub( w3, w1 ); + float w1e13 = b2Dot( w1, e13 ); + float w3e13 = b2Dot( w3, e13 ); + float d13_1 = w3e13; + float d13_2 = -w1e13; + + // Edge23 + // [1 1 ][a2] = [1] + // [w2.e23 w3.e23][a3] = [0] + // a1 = 0 + b2Vec2 e23 = b2Sub( w3, w2 ); + float w2e23 = b2Dot( w2, e23 ); + float w3e23 = b2Dot( w3, e23 ); + float d23_1 = w3e23; + float d23_2 = -w2e23; + + // Triangle123 + float n123 = b2Cross( e12, e13 ); + + float d123_1 = n123 * b2Cross( w2, w3 ); + float d123_2 = n123 * b2Cross( w3, w1 ); + float d123_3 = n123 * b2Cross( w1, w2 ); + + // w1 region + if ( d12_2 <= 0.0f && d13_2 <= 0.0f ) + { + s->v1.a = 1.0f; + s->count = 1; + return; + } + + // e12 + if ( d12_1 > 0.0f && d12_2 > 0.0f && d123_3 <= 0.0f ) + { + float inv_d12 = 1.0f / ( d12_1 + d12_2 ); + s->v1.a = d12_1 * inv_d12; + s->v2.a = d12_2 * inv_d12; + s->count = 2; + return; + } + + // e13 + if ( d13_1 > 0.0f && d13_2 > 0.0f && d123_2 <= 0.0f ) + { + float inv_d13 = 1.0f / ( d13_1 + d13_2 ); + s->v1.a = d13_1 * inv_d13; + s->v3.a = d13_2 * inv_d13; + s->count = 2; + s->v2 = s->v3; + return; + } + + // w2 region + if ( d12_1 <= 0.0f && d23_2 <= 0.0f ) + { + s->v2.a = 1.0f; + s->count = 1; + s->v1 = s->v2; + return; + } + + // w3 region + if ( d13_1 <= 0.0f && d23_1 <= 0.0f ) + { + s->v3.a = 1.0f; + s->count = 1; + s->v1 = s->v3; + return; + } + + // e23 + if ( d23_1 > 0.0f && d23_2 > 0.0f && d123_1 <= 0.0f ) + { + float inv_d23 = 1.0f / ( d23_1 + d23_2 ); + s->v2.a = d23_1 * inv_d23; + s->v3.a = d23_2 * inv_d23; + s->count = 2; + s->v1 = s->v3; + return; + } + + // Must be in triangle123 + float inv_d123 = 1.0f / ( d123_1 + d123_2 + d123_3 ); + s->v1.a = d123_1 * inv_d123; + s->v2.a = d123_2 * inv_d123; + s->v3.a = d123_3 * inv_d123; + s->count = 3; +} + +b2DistanceOutput b2ShapeDistance( b2DistanceCache* cache, const b2DistanceInput* input, b2Simplex* simplexes, + int simplexCapacity ) +{ + b2DistanceOutput output = { 0 }; + + const b2DistanceProxy* proxyA = &input->proxyA; + const b2DistanceProxy* proxyB = &input->proxyB; + + b2Transform transformA = input->transformA; + b2Transform transformB = input->transformB; + + // Initialize the simplex. + b2Simplex simplex = b2MakeSimplexFromCache( cache, proxyA, transformA, proxyB, transformB ); + + int simplexIndex = 0; + if ( simplexes != NULL && simplexIndex < simplexCapacity ) + { + simplexes[simplexIndex] = simplex; + simplexIndex += 1; + } + + // Get simplex vertices as an array. + b2SimplexVertex* vertices[] = { &simplex.v1, &simplex.v2, &simplex.v3 }; + const int k_maxIters = 20; + + // These store the vertices of the last simplex so that we can check for duplicates and prevent cycling. + int saveA[3], saveB[3]; + + // Main iteration loop. + int iter = 0; + while ( iter < k_maxIters ) + { + // Copy simplex so we can identify duplicates. + int saveCount = simplex.count; + for ( int i = 0; i < saveCount; ++i ) + { + saveA[i] = vertices[i]->indexA; + saveB[i] = vertices[i]->indexB; + } + + switch ( simplex.count ) + { + case 1: + break; + + case 2: + b2SolveSimplex2( &simplex ); + break; + + case 3: + b2SolveSimplex3( &simplex ); + break; + + default: + B2_ASSERT( false ); + } + + // If we have 3 points, then the origin is in the corresponding triangle. + if ( simplex.count == 3 ) + { + break; + } + + if ( simplexes != NULL && simplexIndex < simplexCapacity ) + { + simplexes[simplexIndex] = simplex; + simplexIndex += 1; + } + + // Get search direction. + b2Vec2 d = b2ComputeSimplexSearchDirection( &simplex ); + + // Ensure the search direction is numerically fit. + if ( b2Dot( d, d ) < FLT_EPSILON * FLT_EPSILON ) + { + // The origin is probably contained by a line segment + // or triangle. Thus the shapes are overlapped. + + // We can't return zero here even though there may be overlap. + // In case the simplex is a point, segment, or triangle it is difficult + // to determine if the origin is contained in the CSO or very close to it. + break; + } + + // Compute a tentative new simplex vertex using support points. + // support = support(b, d) - support(a, -d) + b2SimplexVertex* vertex = vertices[simplex.count]; + vertex->indexA = b2FindSupport( proxyA, b2InvRotateVector( transformA.q, b2Neg( d ) ) ); + vertex->wA = b2TransformPoint( transformA, proxyA->points[vertex->indexA] ); + vertex->indexB = b2FindSupport( proxyB, b2InvRotateVector( transformB.q, d ) ); + vertex->wB = b2TransformPoint( transformB, proxyB->points[vertex->indexB] ); + vertex->w = b2Sub( vertex->wB, vertex->wA ); + + // Iteration count is equated to the number of support point calls. + ++iter; + + // Check for duplicate support points. This is the main termination criteria. + bool duplicate = false; + for ( int i = 0; i < saveCount; ++i ) + { + if ( vertex->indexA == saveA[i] && vertex->indexB == saveB[i] ) + { + duplicate = true; + break; + } + } + + // If we found a duplicate support point we must exit to avoid cycling. + if ( duplicate ) + { + break; + } + + // New vertex is ok and needed. + ++simplex.count; + } + + if ( simplexes != NULL && simplexIndex < simplexCapacity ) + { + simplexes[simplexIndex] = simplex; + simplexIndex += 1; + } + + // Prepare output + b2ComputeSimplexWitnessPoints( &output.pointA, &output.pointB, &simplex ); + output.distance = b2Distance( output.pointA, output.pointB ); + output.iterations = iter; + output.simplexCount = simplexIndex; + + // Cache the simplex + b2MakeSimplexCache( cache, &simplex ); + + // Apply radii if requested + if ( input->useRadii ) + { + if ( output.distance < FLT_EPSILON ) + { + // Shapes are too close to safely compute normal + b2Vec2 p = ( b2Vec2 ){ 0.5f * ( output.pointA.x + output.pointB.x ), 0.5f * ( output.pointA.y + output.pointB.y ) }; + output.pointA = p; + output.pointB = p; + output.distance = 0.0f; + } + else + { + // Keep closest points on perimeter even if overlapped, this way + // the points move smoothly. + float rA = proxyA->radius; + float rB = proxyB->radius; + output.distance = b2MaxFloat( 0.0f, output.distance - rA - rB ); + b2Vec2 normal = b2Normalize( b2Sub( output.pointB, output.pointA ) ); + b2Vec2 offsetA = ( b2Vec2 ){ rA * normal.x, rA * normal.y }; + b2Vec2 offsetB = ( b2Vec2 ){ rB * normal.x, rB * normal.y }; + output.pointA = b2Add( output.pointA, offsetA ); + output.pointB = b2Sub( output.pointB, offsetB ); + } + } + + return output; +} + +// GJK-raycast +// Algorithm by Gino van den Bergen. +// "Smooth Mesh Contacts with GJK" in Game Physics Pearls. 2010 +// todo this is failing when used to raycast a box +// todo this converges slowly with a radius +b2CastOutput b2ShapeCast( const b2ShapeCastPairInput* input ) +{ + b2CastOutput output = { 0 }; + output.fraction = input->maxFraction; + + b2DistanceProxy proxyA = input->proxyA; + + b2Transform xfA = input->transformA; + b2Transform xfB = input->transformB; + b2Transform xf = b2InvMulTransforms( xfA, xfB ); + + // Put proxyB in proxyA's frame to reduce round-off error + b2DistanceProxy proxyB; + proxyB.count = input->proxyB.count; + proxyB.radius = input->proxyB.radius; + B2_ASSERT( proxyB.count <= b2_maxPolygonVertices ); + + for ( int i = 0; i < proxyB.count; ++i ) + { + proxyB.points[i] = b2TransformPoint( xf, input->proxyB.points[i] ); + } + + float radius = proxyA.radius + proxyB.radius; + + b2Vec2 r = b2RotateVector( xf.q, input->translationB ); + float lambda = 0.0f; + float maxFraction = input->maxFraction; + + // Initial simplex + b2Simplex simplex; + simplex.count = 0; + + // Get simplex vertices as an array. + b2SimplexVertex* vertices[] = { &simplex.v1, &simplex.v2, &simplex.v3 }; + + // Get an initial point in A - B + int indexA = b2FindSupport( &proxyA, b2Neg( r ) ); + b2Vec2 wA = proxyA.points[indexA]; + int indexB = b2FindSupport( &proxyB, r ); + b2Vec2 wB = proxyB.points[indexB]; + b2Vec2 v = b2Sub( wA, wB ); + + // Sigma is the target distance between proxies + const float linearSlop = b2_linearSlop; + const float sigma = b2MaxFloat( linearSlop, radius - linearSlop ); + + // Main iteration loop. + const int k_maxIters = 20; + int iter = 0; + while ( iter < k_maxIters && b2Length( v ) > sigma + 0.5f * linearSlop ) + { + B2_ASSERT( simplex.count < 3 ); + + output.iterations += 1; + + // Support in direction -v (A - B) + indexA = b2FindSupport( &proxyA, b2Neg( v ) ); + wA = proxyA.points[indexA]; + indexB = b2FindSupport( &proxyB, v ); + wB = proxyB.points[indexB]; + b2Vec2 p = b2Sub( wA, wB ); + + // -v is a normal at p, normalize to work with sigma + v = b2Normalize( v ); + + // Intersect ray with plane + float vp = b2Dot( v, p ); + float vr = b2Dot( v, r ); + if ( vp - sigma > lambda * vr ) + { + if ( vr <= 0.0f ) + { + // miss + return output; + } + + lambda = ( vp - sigma ) / vr; + if ( lambda > maxFraction ) + { + // too far + return output; + } + + // reset the simplex + simplex.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.count]; + vertex->indexA = indexB; + vertex->wA = ( b2Vec2 ){ wB.x + lambda * r.x, wB.y + lambda * r.y }; + vertex->indexB = indexA; + vertex->wB = wA; + vertex->w = b2Sub( vertex->wB, vertex->wA ); + vertex->a = 1.0f; + simplex.count += 1; + + switch ( simplex.count ) + { + case 1: + break; + + case 2: + b2SolveSimplex2( &simplex ); + break; + + case 3: + b2SolveSimplex3( &simplex ); + break; + + default: + B2_ASSERT( false ); + } + + // If we have 3 points, then the origin is in the corresponding triangle. + if ( simplex.count == 3 ) + { + // Overlap + return output; + } + + // Get search direction. + // todo use more accurate segment perpendicular + v = b2ComputeSimplexClosestPoint( &simplex ); + + // Iteration count is equated to the number of support point calls. + ++iter; + } + + if ( iter == 0 || lambda == 0.0f ) + { + // Initial overlap + return output; + } + + // Prepare output. + b2Vec2 pointA, pointB; + b2ComputeSimplexWitnessPoints( &pointB, &pointA, &simplex ); + + b2Vec2 n = b2Normalize( b2Neg( v ) ); + b2Vec2 point = { pointA.x + proxyA.radius * n.x, pointA.y + proxyA.radius * n.y }; + + output.point = b2TransformPoint( xfA, point ); + output.normal = b2RotateVector( xfA.q, n ); + output.fraction = lambda; + output.iterations = iter; + output.hit = true; + return output; +} + +#define B2_TOI_DEBUG 0 + +// Warning: writing to these globals significantly slows multithreading performance +#if B2_TOI_DEBUG +float b2_toiTime, b2_toiMaxTime; +int b2_toiCalls, b2_toiIters, b2_toiMaxIters; +int b2_toiRootIters, b2_toiMaxRootIters; +#endif + +typedef enum b2SeparationType +{ + b2_pointsType, + b2_faceAType, + b2_faceBType +} b2SeparationType; + +typedef struct b2SeparationFunction +{ + const b2DistanceProxy* proxyA; + const b2DistanceProxy* proxyB; + b2Sweep sweepA, sweepB; + b2Vec2 localPoint; + b2Vec2 axis; + b2SeparationType type; +} b2SeparationFunction; + +b2SeparationFunction b2MakeSeparationFunction( const b2DistanceCache* cache, const b2DistanceProxy* proxyA, const b2Sweep* sweepA, + const b2DistanceProxy* proxyB, const b2Sweep* sweepB, float t1 ) +{ + b2SeparationFunction f; + + f.proxyA = proxyA; + f.proxyB = proxyB; + int count = cache->count; + B2_ASSERT( 0 < count && count < 3 ); + + f.sweepA = *sweepA; + f.sweepB = *sweepB; + + b2Transform xfA = b2GetSweepTransform( sweepA, t1 ); + b2Transform xfB = b2GetSweepTransform( sweepB, t1 ); + + if ( count == 1 ) + { + f.type = b2_pointsType; + b2Vec2 localPointA = proxyA->points[cache->indexA[0]]; + b2Vec2 localPointB = proxyB->points[cache->indexB[0]]; + b2Vec2 pointA = b2TransformPoint( xfA, localPointA ); + b2Vec2 pointB = b2TransformPoint( xfB, localPointB ); + f.axis = b2Normalize( b2Sub( pointB, pointA ) ); + f.localPoint = b2Vec2_zero; + return f; + } + + if ( cache->indexA[0] == cache->indexA[1] ) + { + // Two points on B and one on A. + f.type = b2_faceBType; + b2Vec2 localPointB1 = proxyB->points[cache->indexB[0]]; + b2Vec2 localPointB2 = proxyB->points[cache->indexB[1]]; + + f.axis = b2CrossVS( b2Sub( localPointB2, localPointB1 ), 1.0f ); + f.axis = b2Normalize( f.axis ); + b2Vec2 normal = b2RotateVector( xfB.q, f.axis ); + + f.localPoint = ( b2Vec2 ){ 0.5f * ( localPointB1.x + localPointB2.x ), 0.5f * ( localPointB1.y + localPointB2.y ) }; + b2Vec2 pointB = b2TransformPoint( xfB, f.localPoint ); + + b2Vec2 localPointA = proxyA->points[cache->indexA[0]]; + b2Vec2 pointA = b2TransformPoint( xfA, localPointA ); + + float s = b2Dot( b2Sub( pointA, pointB ), normal ); + if ( s < 0.0f ) + { + f.axis = b2Neg( f.axis ); + } + return f; + } + + // Two points on A and one or two points on B. + f.type = b2_faceAType; + b2Vec2 localPointA1 = proxyA->points[cache->indexA[0]]; + b2Vec2 localPointA2 = proxyA->points[cache->indexA[1]]; + + f.axis = b2CrossVS( b2Sub( localPointA2, localPointA1 ), 1.0f ); + f.axis = b2Normalize( f.axis ); + b2Vec2 normal = b2RotateVector( xfA.q, f.axis ); + + f.localPoint = ( b2Vec2 ){ 0.5f * ( localPointA1.x + localPointA2.x ), 0.5f * ( localPointA1.y + localPointA2.y ) }; + b2Vec2 pointA = b2TransformPoint( xfA, f.localPoint ); + + b2Vec2 localPointB = proxyB->points[cache->indexB[0]]; + b2Vec2 pointB = b2TransformPoint( xfB, localPointB ); + + float s = b2Dot( b2Sub( pointB, pointA ), normal ); + if ( s < 0.0f ) + { + f.axis = b2Neg( f.axis ); + } + return f; +} + +static float b2FindMinSeparation( const b2SeparationFunction* f, int* indexA, int* indexB, float t ) +{ + b2Transform xfA = b2GetSweepTransform( &f->sweepA, t ); + b2Transform xfB = b2GetSweepTransform( &f->sweepB, t ); + + switch ( f->type ) + { + case b2_pointsType: + { + b2Vec2 axisA = b2InvRotateVector( xfA.q, f->axis ); + b2Vec2 axisB = b2InvRotateVector( xfB.q, b2Neg( f->axis ) ); + + *indexA = b2FindSupport( f->proxyA, axisA ); + *indexB = b2FindSupport( f->proxyB, axisB ); + + b2Vec2 localPointA = f->proxyA->points[*indexA]; + b2Vec2 localPointB = f->proxyB->points[*indexB]; + + b2Vec2 pointA = b2TransformPoint( xfA, localPointA ); + b2Vec2 pointB = b2TransformPoint( xfB, localPointB ); + + float separation = b2Dot( b2Sub( pointB, pointA ), f->axis ); + return separation; + } + + case b2_faceAType: + { + b2Vec2 normal = b2RotateVector( xfA.q, f->axis ); + b2Vec2 pointA = b2TransformPoint( xfA, f->localPoint ); + + b2Vec2 axisB = b2InvRotateVector( xfB.q, b2Neg( normal ) ); + + *indexA = -1; + *indexB = b2FindSupport( f->proxyB, axisB ); + + b2Vec2 localPointB = f->proxyB->points[*indexB]; + b2Vec2 pointB = b2TransformPoint( xfB, localPointB ); + + float separation = b2Dot( b2Sub( pointB, pointA ), normal ); + return separation; + } + + case b2_faceBType: + { + b2Vec2 normal = b2RotateVector( xfB.q, f->axis ); + b2Vec2 pointB = b2TransformPoint( xfB, f->localPoint ); + + b2Vec2 axisA = b2InvRotateVector( xfA.q, b2Neg( normal ) ); + + *indexB = -1; + *indexA = b2FindSupport( f->proxyA, axisA ); + + b2Vec2 localPointA = f->proxyA->points[*indexA]; + b2Vec2 pointA = b2TransformPoint( xfA, localPointA ); + + float separation = b2Dot( b2Sub( pointA, pointB ), normal ); + return separation; + } + + default: + B2_ASSERT( false ); + *indexA = -1; + *indexB = -1; + return 0.0f; + } +} + +// +float b2EvaluateSeparation( const b2SeparationFunction* f, int indexA, int indexB, float t ) +{ + b2Transform xfA = b2GetSweepTransform( &f->sweepA, t ); + b2Transform xfB = b2GetSweepTransform( &f->sweepB, t ); + + switch ( f->type ) + { + case b2_pointsType: + { + b2Vec2 localPointA = f->proxyA->points[indexA]; + b2Vec2 localPointB = f->proxyB->points[indexB]; + + b2Vec2 pointA = b2TransformPoint( xfA, localPointA ); + b2Vec2 pointB = b2TransformPoint( xfB, localPointB ); + + float separation = b2Dot( b2Sub( pointB, pointA ), f->axis ); + return separation; + } + + case b2_faceAType: + { + b2Vec2 normal = b2RotateVector( xfA.q, f->axis ); + b2Vec2 pointA = b2TransformPoint( xfA, f->localPoint ); + + b2Vec2 localPointB = f->proxyB->points[indexB]; + b2Vec2 pointB = b2TransformPoint( xfB, localPointB ); + + float separation = b2Dot( b2Sub( pointB, pointA ), normal ); + return separation; + } + + case b2_faceBType: + { + b2Vec2 normal = b2RotateVector( xfB.q, f->axis ); + b2Vec2 pointB = b2TransformPoint( xfB, f->localPoint ); + + b2Vec2 localPointA = f->proxyA->points[indexA]; + b2Vec2 pointA = b2TransformPoint( xfA, localPointA ); + + float separation = b2Dot( b2Sub( pointA, pointB ), normal ); + return separation; + } + + default: + B2_ASSERT( false ); + return 0.0f; + } +} + +// CCD via the local separating axis method. This seeks progression +// by computing the largest time at which separation is maintained. +b2TOIOutput b2TimeOfImpact( const b2TOIInput* input ) +{ +#if B2_TOI_DEBUG + b2Timer timer = b2CreateTimer(); + ++b2_toiCalls; +#endif + + b2TOIOutput output; + output.state = b2_toiStateUnknown; + output.t = input->tMax; + + const b2DistanceProxy* proxyA = &input->proxyA; + const b2DistanceProxy* proxyB = &input->proxyB; + + b2Sweep sweepA = input->sweepA; + b2Sweep sweepB = input->sweepB; + B2_ASSERT( b2IsNormalized( sweepA.q1 ) && b2IsNormalized( sweepA.q2 ) ); + B2_ASSERT( b2IsNormalized( sweepB.q1 ) && b2IsNormalized( sweepB.q2 ) ); + + float tMax = input->tMax; + + float totalRadius = proxyA->radius + proxyB->radius; + float target = b2MaxFloat( b2_linearSlop, totalRadius - b2_linearSlop ); + float tolerance = 0.25f * b2_linearSlop; + B2_ASSERT( target > tolerance ); + + float t1 = 0.0f; + const int k_maxIterations = 20; + int iter = 0; + + // Prepare input for distance query. + b2DistanceCache cache = { 0 }; + b2DistanceInput distanceInput; + distanceInput.proxyA = input->proxyA; + distanceInput.proxyB = input->proxyB; + distanceInput.useRadii = false; + + // The outer loop progressively attempts to compute new separating axes. + // This loop terminates when an axis is repeated (no progress is made). + for ( ;; ) + { + b2Transform xfA = b2GetSweepTransform( &sweepA, t1 ); + b2Transform xfB = b2GetSweepTransform( &sweepB, t1 ); + + // Get the distance between shapes. We can also use the results + // to get a separating axis. + distanceInput.transformA = xfA; + distanceInput.transformB = xfB; + b2DistanceOutput distanceOutput = b2ShapeDistance( &cache, &distanceInput, NULL, 0 ); + + // If the shapes are overlapped, we give up on continuous collision. + if ( distanceOutput.distance <= 0.0f ) + { + // Failure! + output.state = b2_toiStateOverlapped; + output.t = 0.0f; + break; + } + + if ( distanceOutput.distance < target + tolerance ) + { + // Victory! + output.state = b2_toiStateHit; + output.t = t1; + break; + } + + // Initialize the separating axis. + b2SeparationFunction fcn = b2MakeSeparationFunction( &cache, proxyA, &sweepA, proxyB, &sweepB, t1 ); +#if 0 + // Dump the curve seen by the root finder + { + const int N = 100; + float dx = 1.0f / N; + float xs[N + 1]; + float fs[N + 1]; + + float x = 0.0f; + + for (int i = 0; i <= N; ++i) + { + sweepA.GetTransform(&xfA, x); + sweepB.GetTransform(&xfB, x); + float f = fcn.Evaluate(xfA, xfB) - target; + + printf("%g %g\n", x, f); + + xs[i] = x; + fs[i] = f; + + x += dx; + } + } +#endif + + // Compute the TOI on the separating axis. We do this by successively + // resolving the deepest point. This loop is bounded by the number of vertices. + bool done = false; + float t2 = tMax; + int pushBackIter = 0; + for ( ;; ) + { + // Find the deepest point at t2. Store the witness point indices. + int indexA, indexB; + float s2 = b2FindMinSeparation( &fcn, &indexA, &indexB, t2 ); + + // Is the final configuration separated? + if ( s2 > target + tolerance ) + { + // Victory! + output.state = b2_toiStateSeparated; + output.t = tMax; + done = true; + break; + } + + // Has the separation reached tolerance? + if ( s2 > target - tolerance ) + { + // Advance the sweeps + t1 = t2; + break; + } + + // Compute the initial separation of the witness points. + float s1 = b2EvaluateSeparation( &fcn, indexA, indexB, t1 ); + + // Check for initial overlap. This might happen if the root finder + // runs out of iterations. + if ( s1 < target - tolerance ) + { + output.state = b2_toiStateFailed; + output.t = t1; + done = true; + break; + } + + // Check for touching + if ( s1 <= target + tolerance ) + { + // Victory! t1 should hold the TOI (could be 0.0). + output.state = b2_toiStateHit; + output.t = t1; + done = true; + break; + } + + // Compute 1D root of: f(x) - target = 0 + int rootIterCount = 0; + float a1 = t1, a2 = t2; + for ( ;; ) + { + // Use a mix of the secant rule and bisection. + float t; + if ( rootIterCount & 1 ) + { + // Secant rule to improve convergence. + t = a1 + ( target - s1 ) * ( a2 - a1 ) / ( s2 - s1 ); + } + else + { + // Bisection to guarantee progress. + t = 0.5f * ( a1 + a2 ); + } + + ++rootIterCount; + +#if B2_TOI_DEBUG + ++b2_toiRootIters; +#endif + + float s = b2EvaluateSeparation( &fcn, indexA, indexB, t ); + + if ( b2AbsFloat( s - target ) < tolerance ) + { + // t2 holds a tentative value for t1 + t2 = t; + break; + } + + // Ensure we continue to bracket the root. + if ( s > target ) + { + a1 = t; + s1 = s; + } + else + { + a2 = t; + s2 = s; + } + + if ( rootIterCount == 50 ) + { + break; + } + } + +#if B2_TOI_DEBUG + b2_toiMaxRootIters = b2MaxInt( b2_toiMaxRootIters, rootIterCount ); +#endif + + ++pushBackIter; + + if ( pushBackIter == b2_maxPolygonVertices ) + { + break; + } + } + + ++iter; +#if B2_TOI_DEBUG + ++b2_toiIters; +#endif + + if ( done ) + { + break; + } + + if ( iter == k_maxIterations ) + { + // Root finder got stuck. Semi-victory. + output.state = b2_toiStateFailed; + output.t = t1; + break; + } + } + +#if B2_TOI_DEBUG + b2_toiMaxIters = b2MaxInt( b2_toiMaxIters, iter ); + + float time = b2GetMilliseconds( &timer ); + b2_toiMaxTime = b2MaxFloat( b2_toiMaxTime, time ); + b2_toiTime += time; +#endif + + return output; +} diff --git a/3rdparty/box2d/src/distance_joint.c b/3rdparty/box2d/src/distance_joint.c new file mode 100644 index 000000000000..cec4573959f1 --- /dev/null +++ b/3rdparty/box2d/src/distance_joint.c @@ -0,0 +1,556 @@ +// SPDX-FileCopyrightText: 2023 Erin Catto +// SPDX-License-Identifier: MIT + +#if defined( _MSC_VER ) && !defined( _CRT_SECURE_NO_WARNINGS ) +#define _CRT_SECURE_NO_WARNINGS +#endif + +#include "body.h" +#include "core.h" +#include "joint.h" +#include "solver.h" +#include "solver_set.h" +#include "world.h" + +// needed for dll export +#include "box2d/box2d.h" + +#include + +void b2DistanceJoint_SetLength( b2JointId jointId, float length ) +{ + b2JointSim* base = b2GetJointSimCheckType( jointId, b2_distanceJoint ); + b2DistanceJoint* joint = &base->distanceJoint; + + joint->length = b2ClampFloat( length, b2_linearSlop, b2_huge ); + joint->impulse = 0.0f; + joint->lowerImpulse = 0.0f; + joint->upperImpulse = 0.0f; +} + +float b2DistanceJoint_GetLength( b2JointId jointId ) +{ + b2JointSim* base = b2GetJointSimCheckType( jointId, b2_distanceJoint ); + b2DistanceJoint* joint = &base->distanceJoint; + return joint->length; +} + +void b2DistanceJoint_EnableLimit( b2JointId jointId, bool enableLimit ) +{ + b2JointSim* base = b2GetJointSimCheckType( jointId, b2_distanceJoint ); + b2DistanceJoint* joint = &base->distanceJoint; + joint->enableLimit = enableLimit; +} + +bool b2DistanceJoint_IsLimitEnabled( b2JointId jointId ) +{ + b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_distanceJoint ); + return joint->distanceJoint.enableLimit; +} + +void b2DistanceJoint_SetLengthRange( b2JointId jointId, float minLength, float maxLength ) +{ + b2JointSim* base = b2GetJointSimCheckType( jointId, b2_distanceJoint ); + b2DistanceJoint* joint = &base->distanceJoint; + + minLength = b2ClampFloat( minLength, b2_linearSlop, b2_huge ); + maxLength = b2ClampFloat( maxLength, b2_linearSlop, b2_huge ); + joint->minLength = b2MinFloat( minLength, maxLength ); + joint->maxLength = b2MaxFloat( minLength, maxLength ); + joint->impulse = 0.0f; + joint->lowerImpulse = 0.0f; + joint->upperImpulse = 0.0f; +} + +float b2DistanceJoint_GetMinLength( b2JointId jointId ) +{ + b2JointSim* base = b2GetJointSimCheckType( jointId, b2_distanceJoint ); + b2DistanceJoint* joint = &base->distanceJoint; + return joint->minLength; +} + +float b2DistanceJoint_GetMaxLength( b2JointId jointId ) +{ + b2JointSim* base = b2GetJointSimCheckType( jointId, b2_distanceJoint ); + b2DistanceJoint* joint = &base->distanceJoint; + return joint->maxLength; +} + +float b2DistanceJoint_GetCurrentLength( b2JointId jointId ) +{ + b2JointSim* base = b2GetJointSimCheckType( jointId, b2_distanceJoint ); + + b2World* world = b2GetWorld( jointId.world0 ); + B2_ASSERT( world->locked == false ); + if ( world->locked ) + { + return 0.0f; + } + + b2Transform transformA = b2GetBodyTransform( world, base->bodyIdA ); + b2Transform transformB = b2GetBodyTransform( world, base->bodyIdB ); + + b2Vec2 pA = b2TransformPoint( transformA, base->localOriginAnchorA ); + b2Vec2 pB = b2TransformPoint( transformB, base->localOriginAnchorB ); + b2Vec2 d = b2Sub( pB, pA ); + float length = b2Length( d ); + return length; +} + +void b2DistanceJoint_EnableSpring( b2JointId jointId, bool enableSpring ) +{ + b2JointSim* base = b2GetJointSimCheckType( jointId, b2_distanceJoint ); + base->distanceJoint.enableSpring = enableSpring; +} + +bool b2DistanceJoint_IsSpringEnabled( b2JointId jointId ) +{ + b2JointSim* base = b2GetJointSimCheckType( jointId, b2_distanceJoint ); + return base->distanceJoint.enableSpring; +} + +void b2DistanceJoint_SetSpringHertz( b2JointId jointId, float hertz ) +{ + b2JointSim* base = b2GetJointSimCheckType( jointId, b2_distanceJoint ); + base->distanceJoint.hertz = hertz; +} + +void b2DistanceJoint_SetSpringDampingRatio( b2JointId jointId, float dampingRatio ) +{ + b2JointSim* base = b2GetJointSimCheckType( jointId, b2_distanceJoint ); + base->distanceJoint.dampingRatio = dampingRatio; +} + +float b2DistanceJoint_GetSpringHertz( b2JointId jointId ) +{ + b2JointSim* base = b2GetJointSimCheckType( jointId, b2_distanceJoint ); + b2DistanceJoint* joint = &base->distanceJoint; + return joint->hertz; +} + +float b2DistanceJoint_GetSpringDampingRatio( b2JointId jointId ) +{ + b2JointSim* base = b2GetJointSimCheckType( jointId, b2_distanceJoint ); + b2DistanceJoint* joint = &base->distanceJoint; + return joint->dampingRatio; +} + +void b2DistanceJoint_EnableMotor( b2JointId jointId, bool enableMotor ) +{ + b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_distanceJoint ); + if ( enableMotor != joint->distanceJoint.enableMotor ) + { + joint->distanceJoint.enableMotor = enableMotor; + joint->distanceJoint.motorImpulse = 0.0f; + } +} + +bool b2DistanceJoint_IsMotorEnabled( b2JointId jointId ) +{ + b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_distanceJoint ); + return joint->distanceJoint.enableMotor; +} + +void b2DistanceJoint_SetMotorSpeed( b2JointId jointId, float motorSpeed ) +{ + b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_distanceJoint ); + joint->distanceJoint.motorSpeed = motorSpeed; +} + +float b2DistanceJoint_GetMotorSpeed( b2JointId jointId ) +{ + b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_distanceJoint ); + return joint->distanceJoint.motorSpeed; +} + +float b2DistanceJoint_GetMotorForce( b2JointId jointId ) +{ + b2World* world = b2GetWorld( jointId.world0 ); + b2JointSim* base = b2GetJointSimCheckType( jointId, b2_distanceJoint ); + return world->inv_h * base->distanceJoint.motorImpulse; +} + +void b2DistanceJoint_SetMaxMotorForce( b2JointId jointId, float force ) +{ + b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_distanceJoint ); + joint->distanceJoint.maxMotorForce = force; +} + +float b2DistanceJoint_GetMaxMotorForce( b2JointId jointId ) +{ + b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_distanceJoint ); + return joint->distanceJoint.maxMotorForce; +} + +b2Vec2 b2GetDistanceJointForce( b2World* world, b2JointSim* base ) +{ + b2DistanceJoint* joint = &base->distanceJoint; + + b2Transform transformA = b2GetBodyTransform( world, base->bodyIdA ); + b2Transform transformB = b2GetBodyTransform( world, base->bodyIdB ); + + b2Vec2 pA = b2TransformPoint( transformA, base->localOriginAnchorA ); + b2Vec2 pB = b2TransformPoint( transformB, base->localOriginAnchorB ); + b2Vec2 d = b2Sub( pB, pA ); + b2Vec2 axis = b2Normalize( d ); + float force = ( joint->impulse + joint->lowerImpulse - joint->upperImpulse + joint->motorImpulse ) * world->inv_h; + return b2MulSV( force, axis ); +} + +// 1-D constrained system +// m (v2 - v1) = lambda +// v2 + (beta/h) * x1 + gamma * lambda = 0, gamma has units of inverse mass. +// x2 = x1 + h * v2 + +// 1-D mass-damper-spring system +// m (v2 - v1) + h * d * v2 + h * k * + +// C = norm(p2 - p1) - L +// u = (p2 - p1) / norm(p2 - p1) +// Cdot = dot(u, v2 + cross(w2, r2) - v1 - cross(w1, r1)) +// J = [-u -cross(r1, u) u cross(r2, u)] +// K = J * invM * JT +// = invMass1 + invI1 * cross(r1, u)^2 + invMass2 + invI2 * cross(r2, u)^2 + +void b2PrepareDistanceJoint( b2JointSim* base, b2StepContext* context ) +{ + B2_ASSERT( base->type == b2_distanceJoint ); + + // chase body id to the solver set where the body lives + int idA = base->bodyIdA; + int idB = base->bodyIdB; + + b2World* world = context->world; + b2Body* bodyA = b2BodyArray_Get( &world->bodies, idA ); + b2Body* bodyB = b2BodyArray_Get( &world->bodies, idB ); + + B2_ASSERT( bodyA->setIndex == b2_awakeSet || bodyB->setIndex == b2_awakeSet ); + + b2SolverSet* setA = b2SolverSetArray_Get( &world->solverSets, bodyA->setIndex ); + b2SolverSet* setB = b2SolverSetArray_Get( &world->solverSets, bodyB->setIndex ); + + int localIndexA = bodyA->localIndex; + int localIndexB = bodyB->localIndex; + + b2BodySim* bodySimA = b2BodySimArray_Get( &setA->bodySims, localIndexA ); + b2BodySim* bodySimB = b2BodySimArray_Get( &setB->bodySims, localIndexB ); + + float mA = bodySimA->invMass; + float iA = bodySimA->invInertia; + float mB = bodySimB->invMass; + float iB = bodySimB->invInertia; + + base->invMassA = mA; + base->invMassB = mB; + base->invIA = iA; + base->invIB = iB; + + b2DistanceJoint* joint = &base->distanceJoint; + + joint->indexA = bodyA->setIndex == b2_awakeSet ? localIndexA : B2_NULL_INDEX; + joint->indexB = bodyB->setIndex == b2_awakeSet ? localIndexB : B2_NULL_INDEX; + + // initial anchors in world space + joint->anchorA = b2RotateVector( bodySimA->transform.q, b2Sub( base->localOriginAnchorA, bodySimA->localCenter ) ); + joint->anchorB = b2RotateVector( bodySimB->transform.q, b2Sub( base->localOriginAnchorB, bodySimB->localCenter ) ); + joint->deltaCenter = b2Sub( bodySimB->center, bodySimA->center ); + + b2Vec2 rA = joint->anchorA; + b2Vec2 rB = joint->anchorB; + b2Vec2 separation = b2Add( b2Sub( rB, rA ), joint->deltaCenter ); + b2Vec2 axis = b2Normalize( separation ); + + // compute effective mass + float crA = b2Cross( rA, axis ); + float crB = b2Cross( rB, axis ); + float k = mA + mB + iA * crA * crA + iB * crB * crB; + joint->axialMass = k > 0.0f ? 1.0f / k : 0.0f; + + joint->distanceSoftness = b2MakeSoft( joint->hertz, joint->dampingRatio, context->h ); + + if ( context->enableWarmStarting == false ) + { + joint->impulse = 0.0f; + joint->lowerImpulse = 0.0f; + joint->upperImpulse = 0.0f; + joint->motorImpulse = 0.0f; + } +} + +void b2WarmStartDistanceJoint( b2JointSim* base, b2StepContext* context ) +{ + B2_ASSERT( base->type == b2_distanceJoint ); + + float mA = base->invMassA; + float mB = base->invMassB; + float iA = base->invIA; + float iB = base->invIB; + + // dummy state for static bodies + b2BodyState dummyState = b2_identityBodyState; + + b2DistanceJoint* joint = &base->distanceJoint; + b2BodyState* stateA = joint->indexA == B2_NULL_INDEX ? &dummyState : context->states + joint->indexA; + b2BodyState* stateB = joint->indexB == B2_NULL_INDEX ? &dummyState : context->states + joint->indexB; + + b2Vec2 rA = b2RotateVector( stateA->deltaRotation, joint->anchorA ); + b2Vec2 rB = b2RotateVector( stateB->deltaRotation, joint->anchorB ); + + b2Vec2 ds = b2Add( b2Sub( stateB->deltaPosition, stateA->deltaPosition ), b2Sub( rB, rA ) ); + b2Vec2 separation = b2Add( joint->deltaCenter, ds ); + b2Vec2 axis = b2Normalize( separation ); + + float axialImpulse = joint->impulse + joint->lowerImpulse - joint->upperImpulse + joint->motorImpulse; + b2Vec2 P = b2MulSV( axialImpulse, axis ); + + stateA->linearVelocity = b2MulSub( stateA->linearVelocity, mA, P ); + stateA->angularVelocity -= iA * b2Cross( rA, P ); + stateB->linearVelocity = b2MulAdd( stateB->linearVelocity, mB, P ); + stateB->angularVelocity += iB * b2Cross( rB, P ); +} + +void b2SolveDistanceJoint( b2JointSim* base, b2StepContext* context, bool useBias ) +{ + B2_ASSERT( base->type == b2_distanceJoint ); + + float mA = base->invMassA; + float mB = base->invMassB; + float iA = base->invIA; + float iB = base->invIB; + + // dummy state for static bodies + b2BodyState dummyState = b2_identityBodyState; + + b2DistanceJoint* joint = &base->distanceJoint; + b2BodyState* stateA = joint->indexA == B2_NULL_INDEX ? &dummyState : context->states + joint->indexA; + b2BodyState* stateB = joint->indexB == B2_NULL_INDEX ? &dummyState : context->states + joint->indexB; + + b2Vec2 vA = stateA->linearVelocity; + float wA = stateA->angularVelocity; + b2Vec2 vB = stateB->linearVelocity; + float wB = stateB->angularVelocity; + + // current anchors + b2Vec2 rA = b2RotateVector( stateA->deltaRotation, joint->anchorA ); + b2Vec2 rB = b2RotateVector( stateB->deltaRotation, joint->anchorB ); + + // current separation + b2Vec2 ds = b2Add( b2Sub( stateB->deltaPosition, stateA->deltaPosition ), b2Sub( rB, rA ) ); + b2Vec2 separation = b2Add( joint->deltaCenter, ds ); + + float length = b2Length( separation ); + b2Vec2 axis = b2Normalize( separation ); + + // joint is soft if + // - spring is enabled + // - and (joint limit is disabled or limits are not equal) + if ( joint->enableSpring && ( joint->minLength < joint->maxLength || joint->enableLimit == false ) ) + { + // spring + if ( joint->hertz > 0.0f ) + { + // Cdot = dot(u, v + cross(w, r)) + b2Vec2 vr = b2Add( b2Sub( vB, vA ), b2Sub( b2CrossSV( wB, rB ), b2CrossSV( wA, rA ) ) ); + float Cdot = b2Dot( axis, vr ); + float C = length - joint->length; + float bias = joint->distanceSoftness.biasRate * C; + + float m = joint->distanceSoftness.massScale * joint->axialMass; + float impulse = -m * ( Cdot + bias ) - joint->distanceSoftness.impulseScale * joint->impulse; + joint->impulse += impulse; + + b2Vec2 P = b2MulSV( impulse, axis ); + vA = b2MulSub( vA, mA, P ); + wA -= iA * b2Cross( rA, P ); + vB = b2MulAdd( vB, mB, P ); + wB += iB * b2Cross( rB, P ); + } + + if ( joint->enableLimit ) + { + // lower limit + { + b2Vec2 vr = b2Add( b2Sub( vB, vA ), b2Sub( b2CrossSV( wB, rB ), b2CrossSV( wA, rA ) ) ); + float Cdot = b2Dot( axis, vr ); + + float C = length - joint->minLength; + + float bias = 0.0f; + float massCoeff = 1.0f; + float impulseCoeff = 0.0f; + if ( C > 0.0f ) + { + // speculative + bias = C * context->inv_h; + } + else if ( useBias ) + { + bias = context->jointSoftness.biasRate * C; + massCoeff = context->jointSoftness.massScale; + impulseCoeff = context->jointSoftness.impulseScale; + } + + float impulse = -massCoeff * joint->axialMass * ( Cdot + bias ) - impulseCoeff * joint->lowerImpulse; + float newImpulse = b2MaxFloat( 0.0f, joint->lowerImpulse + impulse ); + impulse = newImpulse - joint->lowerImpulse; + joint->lowerImpulse = newImpulse; + + b2Vec2 P = b2MulSV( impulse, axis ); + vA = b2MulSub( vA, mA, P ); + wA -= iA * b2Cross( rA, P ); + vB = b2MulAdd( vB, mB, P ); + wB += iB * b2Cross( rB, P ); + } + + // upper + { + b2Vec2 vr = b2Add( b2Sub( vA, vB ), b2Sub( b2CrossSV( wA, rA ), b2CrossSV( wB, rB ) ) ); + float Cdot = b2Dot( axis, vr ); + + float C = joint->maxLength - length; + + float bias = 0.0f; + float massScale = 1.0f; + float impulseScale = 0.0f; + if ( C > 0.0f ) + { + // speculative + bias = C * context->inv_h; + } + else if ( useBias ) + { + bias = context->jointSoftness.biasRate * C; + massScale = context->jointSoftness.massScale; + impulseScale = context->jointSoftness.impulseScale; + } + + float impulse = -massScale * joint->axialMass * ( Cdot + bias ) - impulseScale * joint->upperImpulse; + float newImpulse = b2MaxFloat( 0.0f, joint->upperImpulse + impulse ); + impulse = newImpulse - joint->upperImpulse; + joint->upperImpulse = newImpulse; + + b2Vec2 P = b2MulSV( -impulse, axis ); + vA = b2MulSub( vA, mA, P ); + wA -= iA * b2Cross( rA, P ); + vB = b2MulAdd( vB, mB, P ); + wB += iB * b2Cross( rB, P ); + } + } + + if ( joint->enableMotor ) + { + b2Vec2 vr = b2Add( b2Sub( vB, vA ), b2Sub( b2CrossSV( wB, rB ), b2CrossSV( wA, rA ) ) ); + float Cdot = b2Dot( axis, vr ); + float impulse = joint->axialMass * ( joint->motorSpeed - Cdot ); + float oldImpulse = joint->motorImpulse; + float maxImpulse = context->h * joint->maxMotorForce; + joint->motorImpulse = b2ClampFloat( joint->motorImpulse + impulse, -maxImpulse, maxImpulse ); + impulse = joint->motorImpulse - oldImpulse; + + b2Vec2 P = b2MulSV( impulse, axis ); + vA = b2MulSub( vA, mA, P ); + wA -= iA * b2Cross( rA, P ); + vB = b2MulAdd( vB, mB, P ); + wB += iB * b2Cross( rB, P ); + } + } + else + { + // rigid constraint + b2Vec2 vr = b2Add( b2Sub( vB, vA ), b2Sub( b2CrossSV( wB, rB ), b2CrossSV( wA, rA ) ) ); + float Cdot = b2Dot( axis, vr ); + + float C = length - joint->length; + + float bias = 0.0f; + float massScale = 1.0f; + float impulseScale = 0.0f; + if ( useBias ) + { + bias = context->jointSoftness.biasRate * C; + massScale = context->jointSoftness.massScale; + impulseScale = context->jointSoftness.impulseScale; + } + + float impulse = -massScale * joint->axialMass * ( Cdot + bias ) - impulseScale * joint->impulse; + joint->impulse += impulse; + + b2Vec2 P = b2MulSV( impulse, axis ); + vA = b2MulSub( vA, mA, P ); + wA -= iA * b2Cross( rA, P ); + vB = b2MulAdd( vB, mB, P ); + wB += iB * b2Cross( rB, P ); + } + + stateA->linearVelocity = vA; + stateA->angularVelocity = wA; + stateB->linearVelocity = vB; + stateB->angularVelocity = wB; +} + +#if 0 +void b2DistanceJoint::Dump() +{ + int32 indexA = m_bodyA->m_islandIndex; + int32 indexB = m_bodyB->m_islandIndex; + + b2Dump(" b2DistanceJointDef jd;\n"); + b2Dump(" jd.bodyA = sims[%d];\n", indexA); + b2Dump(" jd.bodyB = sims[%d];\n", indexB); + b2Dump(" jd.collideConnected = bool(%d);\n", m_collideConnected); + b2Dump(" jd.localAnchorA.Set(%.9g, %.9g);\n", m_localAnchorA.x, m_localAnchorA.y); + b2Dump(" jd.localAnchorB.Set(%.9g, %.9g);\n", m_localAnchorB.x, m_localAnchorB.y); + b2Dump(" jd.length = %.9g;\n", m_length); + b2Dump(" jd.minLength = %.9g;\n", m_minLength); + b2Dump(" jd.maxLength = %.9g;\n", m_maxLength); + b2Dump(" jd.stiffness = %.9g;\n", m_stiffness); + b2Dump(" jd.damping = %.9g;\n", m_damping); + b2Dump(" joints[%d] = m_world->CreateJoint(&jd);\n", m_index); +} +#endif + +void b2DrawDistanceJoint( b2DebugDraw* draw, b2JointSim* base, b2Transform transformA, b2Transform transformB ) +{ + B2_ASSERT( base->type == b2_distanceJoint ); + + b2DistanceJoint* joint = &base->distanceJoint; + + b2Vec2 pA = b2TransformPoint( transformA, base->localOriginAnchorA ); + b2Vec2 pB = b2TransformPoint( transformB, base->localOriginAnchorB ); + + b2Vec2 axis = b2Normalize( b2Sub( pB, pA ) ); + + if ( joint->minLength < joint->maxLength && joint->enableLimit ) + { + b2Vec2 pMin = b2MulAdd( pA, joint->minLength, axis ); + b2Vec2 pMax = b2MulAdd( pA, joint->maxLength, axis ); + b2Vec2 offset = b2MulSV( 0.05f * b2_lengthUnitsPerMeter, b2RightPerp( axis ) ); + + if ( joint->minLength > b2_linearSlop ) + { + // draw->DrawPoint(pMin, 4.0f, c2, draw->context); + draw->DrawSegment( b2Sub( pMin, offset ), b2Add( pMin, offset ), b2_colorLightGreen, draw->context ); + } + + if ( joint->maxLength < b2_huge ) + { + // draw->DrawPoint(pMax, 4.0f, c3, draw->context); + draw->DrawSegment( b2Sub( pMax, offset ), b2Add( pMax, offset ), b2_colorRed, draw->context ); + } + + if ( joint->minLength > b2_linearSlop && joint->maxLength < b2_huge ) + { + draw->DrawSegment( pMin, pMax, b2_colorGray, draw->context ); + } + } + + draw->DrawSegment( pA, pB, b2_colorWhite, draw->context ); + draw->DrawPoint( pA, 4.0f, b2_colorWhite, draw->context ); + draw->DrawPoint( pB, 4.0f, b2_colorWhite, draw->context ); + + if ( joint->hertz > 0.0f && joint->enableSpring ) + { + b2Vec2 pRest = b2MulAdd( pA, joint->length, axis ); + draw->DrawPoint( pRest, 4.0f, b2_colorBlue, draw->context ); + } +} diff --git a/3rdparty/box2d/src/dynamic_tree.c b/3rdparty/box2d/src/dynamic_tree.c new file mode 100644 index 000000000000..322b5221c0a9 --- /dev/null +++ b/3rdparty/box2d/src/dynamic_tree.c @@ -0,0 +1,1926 @@ +// SPDX-FileCopyrightText: 2023 Erin Catto +// SPDX-License-Identifier: MIT + +#include "aabb.h" +#include "core.h" + +#include "box2d/collision.h" +#include "box2d/math_functions.h" + +#include +#include + +#define b2_treeStackSize 1024 + +// TODO_ERIN +// - try incrementally sorting internal nodes by height for better cache efficiency during depth first traversal. + +static b2TreeNode b2_defaultTreeNode = { + { { 0.0f, 0.0f }, { 0.0f, 0.0f } }, 0, { B2_NULL_INDEX }, B2_NULL_INDEX, B2_NULL_INDEX, -1, -2, false, + { 0, 0, 0, 0, 0 } }; + +static inline bool b2IsLeaf( const b2TreeNode* node ) +{ + return node->height == 0; +} + +static inline int16_t b2MaxInt16( int16_t a, int16_t b ) +{ + return a > b ? a : b; +} + +b2DynamicTree b2DynamicTree_Create( void ) +{ + _Static_assert( ( sizeof( b2TreeNode ) & 0xF ) == 0, "tree node size not a multiple of 16" ); + + b2DynamicTree tree; + tree.root = B2_NULL_INDEX; + + tree.nodeCapacity = 16; + tree.nodeCount = 0; + tree.nodes = (b2TreeNode*)b2Alloc( tree.nodeCapacity * sizeof( b2TreeNode ) ); + memset( tree.nodes, 0, tree.nodeCapacity * sizeof( b2TreeNode ) ); + + // Build a linked list for the free list. + for ( int32_t i = 0; i < tree.nodeCapacity - 1; ++i ) + { + tree.nodes[i].next = i + 1; + tree.nodes[i].height = -1; + } + tree.nodes[tree.nodeCapacity - 1].next = B2_NULL_INDEX; + tree.nodes[tree.nodeCapacity - 1].height = -1; + tree.freeList = 0; + + tree.proxyCount = 0; + + tree.leafIndices = NULL; + tree.leafBoxes = NULL; + tree.leafCenters = NULL; + tree.binIndices = NULL; + tree.rebuildCapacity = 0; + + return tree; +} + +void b2DynamicTree_Destroy( b2DynamicTree* tree ) +{ + b2Free( tree->nodes, tree->nodeCapacity * sizeof( b2TreeNode ) ); + b2Free( tree->leafIndices, tree->rebuildCapacity * sizeof( int32_t ) ); + b2Free( tree->leafBoxes, tree->rebuildCapacity * sizeof( b2AABB ) ); + b2Free( tree->leafCenters, tree->rebuildCapacity * sizeof( b2Vec2 ) ); + b2Free( tree->binIndices, tree->rebuildCapacity * sizeof( int32_t ) ); + + memset( tree, 0, sizeof( b2DynamicTree ) ); +} + +// Allocate a node from the pool. Grow the pool if necessary. +static int32_t b2AllocateNode( b2DynamicTree* tree ) +{ + // Expand the node pool as needed. + if ( tree->freeList == B2_NULL_INDEX ) + { + B2_ASSERT( tree->nodeCount == tree->nodeCapacity ); + + // The free list is empty. Rebuild a bigger pool. + b2TreeNode* oldNodes = tree->nodes; + int32_t oldCapcity = tree->nodeCapacity; + tree->nodeCapacity += oldCapcity >> 1; + tree->nodes = (b2TreeNode*)b2Alloc( tree->nodeCapacity * sizeof( b2TreeNode ) ); + B2_ASSERT( oldNodes != NULL ); + memcpy( tree->nodes, oldNodes, tree->nodeCount * sizeof( b2TreeNode ) ); + b2Free( oldNodes, oldCapcity * sizeof( b2TreeNode ) ); + + // Build a linked list for the free list. The parent pointer becomes the "next" pointer. + // todo avoid building freelist? + for ( int32_t i = tree->nodeCount; i < tree->nodeCapacity - 1; ++i ) + { + tree->nodes[i].next = i + 1; + tree->nodes[i].height = -1; + } + tree->nodes[tree->nodeCapacity - 1].next = B2_NULL_INDEX; + tree->nodes[tree->nodeCapacity - 1].height = -1; + tree->freeList = tree->nodeCount; + } + + // Peel a node off the free list. + int32_t nodeIndex = tree->freeList; + b2TreeNode* node = tree->nodes + nodeIndex; + tree->freeList = node->next; + *node = b2_defaultTreeNode; + ++tree->nodeCount; + return nodeIndex; +} + +// Return a node to the pool. +static void b2FreeNode( b2DynamicTree* tree, int32_t nodeId ) +{ + B2_ASSERT( 0 <= nodeId && nodeId < tree->nodeCapacity ); + B2_ASSERT( 0 < tree->nodeCount ); + tree->nodes[nodeId].next = tree->freeList; + tree->nodes[nodeId].height = -1; + tree->freeList = nodeId; + --tree->nodeCount; +} + +// Greedy algorithm for sibling selection using the SAH +// We have three nodes A-(B,C) and want to add a leaf D, there are three choices. +// 1: make a new parent for A and D : E-(A-(B,C), D) +// 2: associate D with B +// a: B is a leaf : A-(E-(B,D), C) +// b: B is an internal node: A-(B{D},C) +// 3: associate D with C +// a: C is a leaf : A-(B, E-(C,D)) +// b: C is an internal node: A-(B, C{D}) +// All of these have a clear cost except when B or C is an internal node. Hence we need to be greedy. + +// The cost for cases 1, 2a, and 3a can be computed using the sibling cost formula. +// cost of sibling H = area(union(H, D)) + increased are of ancestors + +// Suppose B (or C) is an internal node, then the lowest cost would be one of two cases: +// case1: D becomes a sibling of B +// case2: D becomes a descendant of B along with a new internal node of area(D). +static int32_t b2FindBestSibling( const b2DynamicTree* tree, b2AABB boxD ) +{ + b2Vec2 centerD = b2AABB_Center( boxD ); + float areaD = b2Perimeter( boxD ); + + const b2TreeNode* nodes = tree->nodes; + int32_t rootIndex = tree->root; + + b2AABB rootBox = nodes[rootIndex].aabb; + + // Area of current node + float areaBase = b2Perimeter( rootBox ); + + // Area of inflated node + float directCost = b2Perimeter( b2AABB_Union( rootBox, boxD ) ); + float inheritedCost = 0.0f; + + int32_t bestSibling = rootIndex; + float bestCost = directCost; + + // Descend the tree from root, following a single greedy path. + int32_t index = rootIndex; + while ( nodes[index].height > 0 ) + { + int32_t child1 = nodes[index].child1; + int32_t child2 = nodes[index].child2; + + // Cost of creating a new parent for this node and the new leaf + float cost = directCost + inheritedCost; + + // Sometimes there are multiple identical costs within tolerance. + // This breaks the ties using the centroid distance. + if ( cost < bestCost ) + { + bestSibling = index; + bestCost = cost; + } + + // Inheritance cost seen by children + inheritedCost += directCost - areaBase; + + bool leaf1 = nodes[child1].height == 0; + bool leaf2 = nodes[child2].height == 0; + + // Cost of descending into child 1 + float lowerCost1 = FLT_MAX; + b2AABB box1 = nodes[child1].aabb; + float directCost1 = b2Perimeter( b2AABB_Union( box1, boxD ) ); + float area1 = 0.0f; + if ( leaf1 ) + { + // Child 1 is a leaf + // Cost of creating new node and increasing area of node P + float cost1 = directCost1 + inheritedCost; + + // Need this here due to while condition above + if ( cost1 < bestCost ) + { + bestSibling = child1; + bestCost = cost1; + } + } + else + { + // Child 1 is an internal node + area1 = b2Perimeter( box1 ); + + // Lower bound cost of inserting under child 1. + lowerCost1 = inheritedCost + directCost1 + b2MinFloat( areaD - area1, 0.0f ); + } + + // Cost of descending into child 2 + float lowerCost2 = FLT_MAX; + b2AABB box2 = nodes[child2].aabb; + float directCost2 = b2Perimeter( b2AABB_Union( box2, boxD ) ); + float area2 = 0.0f; + if ( leaf2 ) + { + // Child 2 is a leaf + // Cost of creating new node and increasing area of node P + float cost2 = directCost2 + inheritedCost; + + // Need this here due to while condition above + if ( cost2 < bestCost ) + { + bestSibling = child2; + bestCost = cost2; + } + } + else + { + // Child 2 is an internal node + area2 = b2Perimeter( box2 ); + + // Lower bound cost of inserting under child 2. This is not the cost + // of child 2, it is the best we can hope for under child 2. + lowerCost2 = inheritedCost + directCost2 + b2MinFloat( areaD - area2, 0.0f ); + } + + if ( leaf1 && leaf2 ) + { + break; + } + + // Can the cost possibly be decreased? + if ( bestCost <= lowerCost1 && bestCost <= lowerCost2 ) + { + break; + } + + if ( lowerCost1 == lowerCost2 && leaf1 == false ) + { + B2_ASSERT( lowerCost1 < FLT_MAX ); + B2_ASSERT( lowerCost2 < FLT_MAX ); + + // No clear choice based on lower bound surface area. This can happen when both + // children fully contain D. Fall back to node distance. + b2Vec2 d1 = b2Sub( b2AABB_Center( box1 ), centerD ); + b2Vec2 d2 = b2Sub( b2AABB_Center( box2 ), centerD ); + lowerCost1 = b2LengthSquared( d1 ); + lowerCost2 = b2LengthSquared( d2 ); + } + + // Descend + if ( lowerCost1 < lowerCost2 && leaf1 == false ) + { + index = child1; + areaBase = area1; + directCost = directCost1; + } + else + { + index = child2; + areaBase = area2; + directCost = directCost2; + } + + B2_ASSERT( nodes[index].height > 0 ); + } + + return bestSibling; +} + +enum b2RotateType +{ + b2_rotateNone, + b2_rotateBF, + b2_rotateBG, + b2_rotateCD, + b2_rotateCE +}; + +// Perform a left or right rotation if node A is imbalanced. +// Returns the new root index. +static void b2RotateNodes( b2DynamicTree* tree, int32_t iA ) +{ + B2_ASSERT( iA != B2_NULL_INDEX ); + + b2TreeNode* nodes = tree->nodes; + + b2TreeNode* A = nodes + iA; + if ( A->height < 2 ) + { + return; + } + + int32_t iB = A->child1; + int32_t iC = A->child2; + B2_ASSERT( 0 <= iB && iB < tree->nodeCapacity ); + B2_ASSERT( 0 <= iC && iC < tree->nodeCapacity ); + + b2TreeNode* B = nodes + iB; + b2TreeNode* C = nodes + iC; + + if ( B->height == 0 ) + { + // B is a leaf and C is internal + B2_ASSERT( C->height > 0 ); + + int32_t iF = C->child1; + int32_t iG = C->child2; + b2TreeNode* F = nodes + iF; + b2TreeNode* G = nodes + iG; + B2_ASSERT( 0 <= iF && iF < tree->nodeCapacity ); + B2_ASSERT( 0 <= iG && iG < tree->nodeCapacity ); + + // Base cost + float costBase = b2Perimeter( C->aabb ); + + // Cost of swapping B and F + b2AABB aabbBG = b2AABB_Union( B->aabb, G->aabb ); + float costBF = b2Perimeter( aabbBG ); + + // Cost of swapping B and G + b2AABB aabbBF = b2AABB_Union( B->aabb, F->aabb ); + float costBG = b2Perimeter( aabbBF ); + + if ( costBase < costBF && costBase < costBG ) + { + // Rotation does not improve cost + return; + } + + if ( costBF < costBG ) + { + // Swap B and F + A->child1 = iF; + C->child1 = iB; + + B->parent = iC; + F->parent = iA; + + C->aabb = aabbBG; + + C->height = 1 + b2MaxInt16( B->height, G->height ); + A->height = 1 + b2MaxInt16( C->height, F->height ); + C->categoryBits = B->categoryBits | G->categoryBits; + A->categoryBits = C->categoryBits | F->categoryBits; + C->enlarged = B->enlarged || G->enlarged; + A->enlarged = C->enlarged || F->enlarged; + } + else + { + // Swap B and G + A->child1 = iG; + C->child2 = iB; + + B->parent = iC; + G->parent = iA; + + C->aabb = aabbBF; + + C->height = 1 + b2MaxInt16( B->height, F->height ); + A->height = 1 + b2MaxInt16( C->height, G->height ); + C->categoryBits = B->categoryBits | F->categoryBits; + A->categoryBits = C->categoryBits | G->categoryBits; + C->enlarged = B->enlarged || F->enlarged; + A->enlarged = C->enlarged || G->enlarged; + } + } + else if ( C->height == 0 ) + { + // C is a leaf and B is internal + B2_ASSERT( B->height > 0 ); + + int iD = B->child1; + int iE = B->child2; + b2TreeNode* D = nodes + iD; + b2TreeNode* E = nodes + iE; + B2_ASSERT( 0 <= iD && iD < tree->nodeCapacity ); + B2_ASSERT( 0 <= iE && iE < tree->nodeCapacity ); + + // Base cost + float costBase = b2Perimeter( B->aabb ); + + // Cost of swapping C and D + b2AABB aabbCE = b2AABB_Union( C->aabb, E->aabb ); + float costCD = b2Perimeter( aabbCE ); + + // Cost of swapping C and E + b2AABB aabbCD = b2AABB_Union( C->aabb, D->aabb ); + float costCE = b2Perimeter( aabbCD ); + + if ( costBase < costCD && costBase < costCE ) + { + // Rotation does not improve cost + return; + } + + if ( costCD < costCE ) + { + // Swap C and D + A->child2 = iD; + B->child1 = iC; + + C->parent = iB; + D->parent = iA; + + B->aabb = aabbCE; + + B->height = 1 + b2MaxInt16( C->height, E->height ); + A->height = 1 + b2MaxInt16( B->height, D->height ); + B->categoryBits = C->categoryBits | E->categoryBits; + A->categoryBits = B->categoryBits | D->categoryBits; + B->enlarged = C->enlarged || E->enlarged; + A->enlarged = B->enlarged || D->enlarged; + } + else + { + // Swap C and E + A->child2 = iE; + B->child2 = iC; + + C->parent = iB; + E->parent = iA; + + B->aabb = aabbCD; + B->height = 1 + b2MaxInt16( C->height, D->height ); + A->height = 1 + b2MaxInt16( B->height, E->height ); + B->categoryBits = C->categoryBits | D->categoryBits; + A->categoryBits = B->categoryBits | E->categoryBits; + B->enlarged = C->enlarged || D->enlarged; + A->enlarged = B->enlarged || E->enlarged; + } + } + else + { + int iD = B->child1; + int iE = B->child2; + int iF = C->child1; + int iG = C->child2; + + b2TreeNode* D = nodes + iD; + b2TreeNode* E = nodes + iE; + b2TreeNode* F = nodes + iF; + b2TreeNode* G = nodes + iG; + + B2_ASSERT( 0 <= iD && iD < tree->nodeCapacity ); + B2_ASSERT( 0 <= iE && iE < tree->nodeCapacity ); + B2_ASSERT( 0 <= iF && iF < tree->nodeCapacity ); + B2_ASSERT( 0 <= iG && iG < tree->nodeCapacity ); + + // Base cost + float areaB = b2Perimeter( B->aabb ); + float areaC = b2Perimeter( C->aabb ); + float costBase = areaB + areaC; + enum b2RotateType bestRotation = b2_rotateNone; + float bestCost = costBase; + + // Cost of swapping B and F + b2AABB aabbBG = b2AABB_Union( B->aabb, G->aabb ); + float costBF = areaB + b2Perimeter( aabbBG ); + if ( costBF < bestCost ) + { + bestRotation = b2_rotateBF; + bestCost = costBF; + } + + // Cost of swapping B and G + b2AABB aabbBF = b2AABB_Union( B->aabb, F->aabb ); + float costBG = areaB + b2Perimeter( aabbBF ); + if ( costBG < bestCost ) + { + bestRotation = b2_rotateBG; + bestCost = costBG; + } + + // Cost of swapping C and D + b2AABB aabbCE = b2AABB_Union( C->aabb, E->aabb ); + float costCD = areaC + b2Perimeter( aabbCE ); + if ( costCD < bestCost ) + { + bestRotation = b2_rotateCD; + bestCost = costCD; + } + + // Cost of swapping C and E + b2AABB aabbCD = b2AABB_Union( C->aabb, D->aabb ); + float costCE = areaC + b2Perimeter( aabbCD ); + if ( costCE < bestCost ) + { + bestRotation = b2_rotateCE; + // bestCost = costCE; + } + + switch ( bestRotation ) + { + case b2_rotateNone: + break; + + case b2_rotateBF: + A->child1 = iF; + C->child1 = iB; + + B->parent = iC; + F->parent = iA; + + C->aabb = aabbBG; + C->height = 1 + b2MaxInt16( B->height, G->height ); + A->height = 1 + b2MaxInt16( C->height, F->height ); + C->categoryBits = B->categoryBits | G->categoryBits; + A->categoryBits = C->categoryBits | F->categoryBits; + C->enlarged = B->enlarged || G->enlarged; + A->enlarged = C->enlarged || F->enlarged; + break; + + case b2_rotateBG: + A->child1 = iG; + C->child2 = iB; + + B->parent = iC; + G->parent = iA; + + C->aabb = aabbBF; + C->height = 1 + b2MaxInt16( B->height, F->height ); + A->height = 1 + b2MaxInt16( C->height, G->height ); + C->categoryBits = B->categoryBits | F->categoryBits; + A->categoryBits = C->categoryBits | G->categoryBits; + C->enlarged = B->enlarged || F->enlarged; + A->enlarged = C->enlarged || G->enlarged; + break; + + case b2_rotateCD: + A->child2 = iD; + B->child1 = iC; + + C->parent = iB; + D->parent = iA; + + B->aabb = aabbCE; + B->height = 1 + b2MaxInt16( C->height, E->height ); + A->height = 1 + b2MaxInt16( B->height, D->height ); + B->categoryBits = C->categoryBits | E->categoryBits; + A->categoryBits = B->categoryBits | D->categoryBits; + B->enlarged = C->enlarged || E->enlarged; + A->enlarged = B->enlarged || D->enlarged; + break; + + case b2_rotateCE: + A->child2 = iE; + B->child2 = iC; + + C->parent = iB; + E->parent = iA; + + B->aabb = aabbCD; + B->height = 1 + b2MaxInt16( C->height, D->height ); + A->height = 1 + b2MaxInt16( B->height, E->height ); + B->categoryBits = C->categoryBits | D->categoryBits; + A->categoryBits = B->categoryBits | E->categoryBits; + B->enlarged = C->enlarged || D->enlarged; + A->enlarged = B->enlarged || E->enlarged; + break; + + default: + B2_ASSERT( false ); + break; + } + } +} + +static void b2InsertLeaf( b2DynamicTree* tree, int32_t leaf, bool shouldRotate ) +{ + if ( tree->root == B2_NULL_INDEX ) + { + tree->root = leaf; + tree->nodes[tree->root].parent = B2_NULL_INDEX; + return; + } + + // Stage 1: find the best sibling for this node + b2AABB leafAABB = tree->nodes[leaf].aabb; + int32_t sibling = b2FindBestSibling( tree, leafAABB ); + + // Stage 2: create a new parent for the leaf and sibling + int32_t oldParent = tree->nodes[sibling].parent; + int32_t newParent = b2AllocateNode( tree ); + + // warning: node pointer can change after allocation + b2TreeNode* nodes = tree->nodes; + nodes[newParent].parent = oldParent; + nodes[newParent].userData = -1; + nodes[newParent].aabb = b2AABB_Union( leafAABB, nodes[sibling].aabb ); + nodes[newParent].categoryBits = nodes[leaf].categoryBits | nodes[sibling].categoryBits; + nodes[newParent].height = nodes[sibling].height + 1; + + if ( oldParent != B2_NULL_INDEX ) + { + // The sibling was not the root. + if ( nodes[oldParent].child1 == sibling ) + { + nodes[oldParent].child1 = newParent; + } + else + { + nodes[oldParent].child2 = newParent; + } + + nodes[newParent].child1 = sibling; + nodes[newParent].child2 = leaf; + nodes[sibling].parent = newParent; + nodes[leaf].parent = newParent; + } + else + { + // The sibling was the root. + nodes[newParent].child1 = sibling; + nodes[newParent].child2 = leaf; + nodes[sibling].parent = newParent; + nodes[leaf].parent = newParent; + tree->root = newParent; + } + + // Stage 3: walk back up the tree fixing heights and AABBs + int32_t index = nodes[leaf].parent; + while ( index != B2_NULL_INDEX ) + { + int32_t child1 = nodes[index].child1; + int32_t child2 = nodes[index].child2; + + B2_ASSERT( child1 != B2_NULL_INDEX ); + B2_ASSERT( child2 != B2_NULL_INDEX ); + + nodes[index].aabb = b2AABB_Union( nodes[child1].aabb, nodes[child2].aabb ); + nodes[index].categoryBits = nodes[child1].categoryBits | nodes[child2].categoryBits; + nodes[index].height = 1 + b2MaxInt16( nodes[child1].height, nodes[child2].height ); + nodes[index].enlarged = nodes[child1].enlarged || nodes[child2].enlarged; + + if ( shouldRotate ) + { + b2RotateNodes( tree, index ); + } + + index = nodes[index].parent; + } +} + +static void b2RemoveLeaf( b2DynamicTree* tree, int32_t leaf ) +{ + if ( leaf == tree->root ) + { + tree->root = B2_NULL_INDEX; + return; + } + + b2TreeNode* nodes = tree->nodes; + + int32_t parent = nodes[leaf].parent; + int32_t grandParent = nodes[parent].parent; + int32_t sibling; + if ( nodes[parent].child1 == leaf ) + { + sibling = nodes[parent].child2; + } + else + { + sibling = nodes[parent].child1; + } + + if ( grandParent != B2_NULL_INDEX ) + { + // Destroy parent and connect sibling to grandParent. + if ( nodes[grandParent].child1 == parent ) + { + nodes[grandParent].child1 = sibling; + } + else + { + nodes[grandParent].child2 = sibling; + } + nodes[sibling].parent = grandParent; + b2FreeNode( tree, parent ); + + // Adjust ancestor bounds. + int32_t index = grandParent; + while ( index != B2_NULL_INDEX ) + { + b2TreeNode* node = nodes + index; + b2TreeNode* child1 = nodes + node->child1; + b2TreeNode* child2 = nodes + node->child2; + + // Fast union using SSE + //__m128 aabb1 = _mm_load_ps(&child1->aabb.lowerBound.x); + //__m128 aabb2 = _mm_load_ps(&child2->aabb.lowerBound.x); + //__m128 lower = _mm_min_ps(aabb1, aabb2); + //__m128 upper = _mm_max_ps(aabb1, aabb2); + //__m128 aabb = _mm_shuffle_ps(lower, upper, _MM_SHUFFLE(3, 2, 1, 0)); + //_mm_store_ps(&node->aabb.lowerBound.x, aabb); + + node->aabb = b2AABB_Union( child1->aabb, child2->aabb ); + node->categoryBits = child1->categoryBits | child2->categoryBits; + node->height = 1 + b2MaxInt16( child1->height, child2->height ); + + index = node->parent; + } + } + else + { + tree->root = sibling; + tree->nodes[sibling].parent = B2_NULL_INDEX; + b2FreeNode( tree, parent ); + } +} + +// Create a proxy in the tree as a leaf node. We return the index of the node instead of a pointer so that we can grow +// the node pool. +int32_t b2DynamicTree_CreateProxy( b2DynamicTree* tree, b2AABB aabb, uint64_t categoryBits, int32_t userData ) +{ + B2_ASSERT( -b2_huge < aabb.lowerBound.x && aabb.lowerBound.x < b2_huge ); + B2_ASSERT( -b2_huge < aabb.lowerBound.y && aabb.lowerBound.y < b2_huge ); + B2_ASSERT( -b2_huge < aabb.upperBound.x && aabb.upperBound.x < b2_huge ); + B2_ASSERT( -b2_huge < aabb.upperBound.y && aabb.upperBound.y < b2_huge ); + + int32_t proxyId = b2AllocateNode( tree ); + b2TreeNode* node = tree->nodes + proxyId; + + node->aabb = aabb; + node->userData = userData; + node->categoryBits = categoryBits; + node->height = 0; + + bool shouldRotate = true; + b2InsertLeaf( tree, proxyId, shouldRotate ); + + tree->proxyCount += 1; + + return proxyId; +} + +void b2DynamicTree_DestroyProxy( b2DynamicTree* tree, int32_t proxyId ) +{ + B2_ASSERT( 0 <= proxyId && proxyId < tree->nodeCapacity ); + B2_ASSERT( b2IsLeaf( tree->nodes + proxyId ) ); + + b2RemoveLeaf( tree, proxyId ); + b2FreeNode( tree, proxyId ); + + B2_ASSERT( tree->proxyCount > 0 ); + tree->proxyCount -= 1; +} + +int32_t b2DynamicTree_GetProxyCount( const b2DynamicTree* tree ) +{ + return tree->proxyCount; +} + +void b2DynamicTree_MoveProxy( b2DynamicTree* tree, int32_t proxyId, b2AABB aabb ) +{ + B2_ASSERT( b2AABB_IsValid( aabb ) ); + B2_ASSERT( aabb.upperBound.x - aabb.lowerBound.x < b2_huge ); + B2_ASSERT( aabb.upperBound.y - aabb.lowerBound.y < b2_huge ); + B2_ASSERT( 0 <= proxyId && proxyId < tree->nodeCapacity ); + B2_ASSERT( b2IsLeaf( tree->nodes + proxyId ) ); + + b2RemoveLeaf( tree, proxyId ); + + tree->nodes[proxyId].aabb = aabb; + + bool shouldRotate = false; + b2InsertLeaf( tree, proxyId, shouldRotate ); +} + +void b2DynamicTree_EnlargeProxy( b2DynamicTree* tree, int32_t proxyId, b2AABB aabb ) +{ + b2TreeNode* nodes = tree->nodes; + + B2_ASSERT( b2AABB_IsValid( aabb ) ); + B2_ASSERT( aabb.upperBound.x - aabb.lowerBound.x < b2_huge ); + B2_ASSERT( aabb.upperBound.y - aabb.lowerBound.y < b2_huge ); + B2_ASSERT( 0 <= proxyId && proxyId < tree->nodeCapacity ); + B2_ASSERT( b2IsLeaf( tree->nodes + proxyId ) ); + + // Caller must ensure this + B2_ASSERT( b2AABB_Contains( nodes[proxyId].aabb, aabb ) == false ); + + nodes[proxyId].aabb = aabb; + + int32_t parentIndex = nodes[proxyId].parent; + while ( parentIndex != B2_NULL_INDEX ) + { + bool changed = b2EnlargeAABB( &nodes[parentIndex].aabb, aabb ); + nodes[parentIndex].enlarged = true; + parentIndex = nodes[parentIndex].parent; + + if ( changed == false ) + { + break; + } + } + + while ( parentIndex != B2_NULL_INDEX ) + { + if ( nodes[parentIndex].enlarged == true ) + { + // early out because this ancestor was previously ascended and marked as enlarged + break; + } + + nodes[parentIndex].enlarged = true; + parentIndex = nodes[parentIndex].parent; + } +} + +int b2DynamicTree_GetHeight( const b2DynamicTree* tree ) +{ + if ( tree->root == B2_NULL_INDEX ) + { + return 0; + } + + return tree->nodes[tree->root].height; +} + +float b2DynamicTree_GetAreaRatio( const b2DynamicTree* tree ) +{ + if ( tree->root == B2_NULL_INDEX ) + { + return 0.0f; + } + + const b2TreeNode* root = tree->nodes + tree->root; + float rootArea = b2Perimeter( root->aabb ); + + float totalArea = 0.0f; + for ( int32_t i = 0; i < tree->nodeCapacity; ++i ) + { + const b2TreeNode* node = tree->nodes + i; + if ( node->height < 0 || b2IsLeaf( node ) || i == tree->root ) + { + // Free node in pool + continue; + } + + totalArea += b2Perimeter( node->aabb ); + } + + return totalArea / rootArea; +} + +// Compute the height of a sub-tree. +static int b2ComputeHeight( const b2DynamicTree* tree, int32_t nodeId ) +{ + B2_ASSERT( 0 <= nodeId && nodeId < tree->nodeCapacity ); + b2TreeNode* node = tree->nodes + nodeId; + + if ( b2IsLeaf( node ) ) + { + return 0; + } + + int32_t height1 = b2ComputeHeight( tree, node->child1 ); + int32_t height2 = b2ComputeHeight( tree, node->child2 ); + return 1 + b2MaxInt16( height1, height2 ); +} + +int b2DynamicTree_ComputeHeight( const b2DynamicTree* tree ) +{ + int height = b2ComputeHeight( tree, tree->root ); + return height; +} + +#if B2_VALIDATE +static void b2ValidateStructure( const b2DynamicTree* tree, int32_t index ) +{ + if ( index == B2_NULL_INDEX ) + { + return; + } + + if ( index == tree->root ) + { + B2_ASSERT( tree->nodes[index].parent == B2_NULL_INDEX ); + } + + const b2TreeNode* node = tree->nodes + index; + + int32_t child1 = node->child1; + int32_t child2 = node->child2; + + if ( b2IsLeaf( node ) ) + { + B2_ASSERT( child1 == B2_NULL_INDEX ); + B2_ASSERT( child2 == B2_NULL_INDEX ); + B2_ASSERT( node->height == 0 ); + return; + } + + B2_ASSERT( 0 <= child1 && child1 < tree->nodeCapacity ); + B2_ASSERT( 0 <= child2 && child2 < tree->nodeCapacity ); + + B2_ASSERT( tree->nodes[child1].parent == index ); + B2_ASSERT( tree->nodes[child2].parent == index ); + + if ( tree->nodes[child1].enlarged || tree->nodes[child2].enlarged ) + { + B2_ASSERT( node->enlarged == true ); + } + + b2ValidateStructure( tree, child1 ); + b2ValidateStructure( tree, child2 ); +} + +static void b2ValidateMetrics( const b2DynamicTree* tree, int32_t index ) +{ + if ( index == B2_NULL_INDEX ) + { + return; + } + + const b2TreeNode* node = tree->nodes + index; + + int32_t child1 = node->child1; + int32_t child2 = node->child2; + + if ( b2IsLeaf( node ) ) + { + B2_ASSERT( child1 == B2_NULL_INDEX ); + B2_ASSERT( child2 == B2_NULL_INDEX ); + B2_ASSERT( node->height == 0 ); + return; + } + + B2_ASSERT( 0 <= child1 && child1 < tree->nodeCapacity ); + B2_ASSERT( 0 <= child2 && child2 < tree->nodeCapacity ); + + int32_t height1 = tree->nodes[child1].height; + int32_t height2 = tree->nodes[child2].height; + int32_t height; + height = 1 + b2MaxInt16( height1, height2 ); + B2_ASSERT( node->height == height ); + + // b2AABB aabb = b2AABB_Union(tree->nodes[child1].aabb, tree->nodes[child2].aabb); + + B2_ASSERT( b2AABB_Contains( node->aabb, tree->nodes[child1].aabb ) ); + B2_ASSERT( b2AABB_Contains( node->aabb, tree->nodes[child2].aabb ) ); + + // B2_ASSERT(aabb.lowerBound.x == node->aabb.lowerBound.x); + // B2_ASSERT(aabb.lowerBound.y == node->aabb.lowerBound.y); + // B2_ASSERT(aabb.upperBound.x == node->aabb.upperBound.x); + // B2_ASSERT(aabb.upperBound.y == node->aabb.upperBound.y); + + uint64_t categoryBits = tree->nodes[child1].categoryBits | tree->nodes[child2].categoryBits; + B2_ASSERT( node->categoryBits == categoryBits ); + + b2ValidateMetrics( tree, child1 ); + b2ValidateMetrics( tree, child2 ); +} +#endif + +void b2DynamicTree_Validate( const b2DynamicTree* tree ) +{ +#if B2_VALIDATE + if ( tree->root == B2_NULL_INDEX ) + { + return; + } + + b2ValidateStructure( tree, tree->root ); + b2ValidateMetrics( tree, tree->root ); + + int32_t freeCount = 0; + int32_t freeIndex = tree->freeList; + while ( freeIndex != B2_NULL_INDEX ) + { + B2_ASSERT( 0 <= freeIndex && freeIndex < tree->nodeCapacity ); + freeIndex = tree->nodes[freeIndex].next; + ++freeCount; + } + + int32_t height = b2DynamicTree_GetHeight( tree ); + int32_t computedHeight = b2DynamicTree_ComputeHeight( tree ); + B2_ASSERT( height == computedHeight ); + + B2_ASSERT( tree->nodeCount + freeCount == tree->nodeCapacity ); +#else + B2_MAYBE_UNUSED( tree ); +#endif +} + +int32_t b2DynamicTree_GetMaxBalance( const b2DynamicTree* tree ) +{ + int32_t maxBalance = 0; + for ( int32_t i = 0; i < tree->nodeCapacity; ++i ) + { + const b2TreeNode* node = tree->nodes + i; + if ( node->height <= 1 ) + { + continue; + } + + B2_ASSERT( b2IsLeaf( node ) == false ); + + int32_t child1 = node->child1; + int32_t child2 = node->child2; + int32_t balance = b2AbsFloat( tree->nodes[child2].height - tree->nodes[child1].height ); + maxBalance = b2MaxInt( maxBalance, balance ); + } + + return maxBalance; +} + +void b2DynamicTree_RebuildBottomUp( b2DynamicTree* tree ) +{ + int32_t* nodes = b2Alloc( tree->nodeCount * sizeof( int32_t ) ); + int32_t count = 0; + + // Build array of leaves. Free the rest. + for ( int32_t i = 0; i < tree->nodeCapacity; ++i ) + { + if ( tree->nodes[i].height < 0 ) + { + // free node in pool + continue; + } + + if ( b2IsLeaf( tree->nodes + i ) ) + { + tree->nodes[i].parent = B2_NULL_INDEX; + nodes[count] = i; + ++count; + } + else + { + b2FreeNode( tree, i ); + } + } + + while ( count > 1 ) + { + float minCost = FLT_MAX; + int32_t iMin = -1, jMin = -1; + for ( int32_t i = 0; i < count; ++i ) + { + b2AABB aabbi = tree->nodes[nodes[i]].aabb; + + for ( int32_t j = i + 1; j < count; ++j ) + { + b2AABB aabbj = tree->nodes[nodes[j]].aabb; + b2AABB b = b2AABB_Union( aabbi, aabbj ); + float cost = b2Perimeter( b ); + if ( cost < minCost ) + { + iMin = i; + jMin = j; + minCost = cost; + } + } + } + + int32_t index1 = nodes[iMin]; + int32_t index2 = nodes[jMin]; + b2TreeNode* child1 = tree->nodes + index1; + b2TreeNode* child2 = tree->nodes + index2; + + int32_t parentIndex = b2AllocateNode( tree ); + b2TreeNode* parent = tree->nodes + parentIndex; + parent->child1 = index1; + parent->child2 = index2; + parent->aabb = b2AABB_Union( child1->aabb, child2->aabb ); + parent->categoryBits = child1->categoryBits | child2->categoryBits; + parent->height = 1 + b2MaxInt16( child1->height, child2->height ); + parent->parent = B2_NULL_INDEX; + + child1->parent = parentIndex; + child2->parent = parentIndex; + + nodes[jMin] = nodes[count - 1]; + nodes[iMin] = parentIndex; + --count; + } + + tree->root = nodes[0]; + b2Free( nodes, tree->nodeCount * sizeof( b2TreeNode ) ); + + b2DynamicTree_Validate( tree ); +} + +void b2DynamicTree_ShiftOrigin( b2DynamicTree* tree, b2Vec2 newOrigin ) +{ + // shift all AABBs + for ( int32_t i = 0; i < tree->nodeCapacity; ++i ) + { + b2TreeNode* n = tree->nodes + i; + n->aabb.lowerBound.x -= newOrigin.x; + n->aabb.lowerBound.y -= newOrigin.y; + n->aabb.upperBound.x -= newOrigin.x; + n->aabb.upperBound.y -= newOrigin.y; + } +} + +int b2DynamicTree_GetByteCount( const b2DynamicTree* tree ) +{ + size_t size = sizeof( b2DynamicTree ) + sizeof( b2TreeNode ) * tree->nodeCapacity + + tree->rebuildCapacity * ( sizeof( int32_t ) + sizeof( b2AABB ) + sizeof( b2Vec2 ) + sizeof( int32_t ) ); + + return (int)size; +} + +void b2DynamicTree_Query( const b2DynamicTree* tree, b2AABB aabb, uint64_t maskBits, b2TreeQueryCallbackFcn* callback, + void* context ) +{ + int32_t stack[b2_treeStackSize]; + int32_t stackCount = 0; + stack[stackCount++] = tree->root; + + while ( stackCount > 0 ) + { + int32_t nodeId = stack[--stackCount]; + if ( nodeId == B2_NULL_INDEX ) + { + continue; + } + + const b2TreeNode* node = tree->nodes + nodeId; + + if ( b2AABB_Overlaps( node->aabb, aabb ) && ( node->categoryBits & maskBits ) != 0 ) + { + if ( b2IsLeaf( node ) ) + { + // callback to user code with proxy id + bool proceed = callback( nodeId, node->userData, context ); + if ( proceed == false ) + { + return; + } + } + else + { + B2_ASSERT( stackCount < b2_treeStackSize - 1 ); + if ( stackCount < b2_treeStackSize - 1 ) + { + stack[stackCount++] = node->child1; + stack[stackCount++] = node->child2; + } + } + } + } +} + +void b2DynamicTree_RayCast( const b2DynamicTree* tree, const b2RayCastInput* input, uint64_t maskBits, + b2TreeRayCastCallbackFcn* callback, void* context ) +{ + b2Vec2 p1 = input->origin; + b2Vec2 d = input->translation; + + b2Vec2 r = b2Normalize( d ); + + // v is perpendicular to the segment. + b2Vec2 v = b2CrossSV( 1.0f, r ); + b2Vec2 abs_v = b2Abs( v ); + + // Separating axis for segment (Gino, p80). + // |dot(v, p1 - c)| > dot(|v|, h) + + float maxFraction = input->maxFraction; + + b2Vec2 p2 = b2MulAdd( p1, maxFraction, d ); + + // Build a bounding box for the segment. + b2AABB segmentAABB = { b2Min( p1, p2 ), b2Max( p1, p2 ) }; + + int32_t stack[b2_treeStackSize]; + int32_t stackCount = 0; + stack[stackCount++] = tree->root; + + b2RayCastInput subInput = *input; + + while ( stackCount > 0 ) + { + int32_t nodeId = stack[--stackCount]; + if ( nodeId == B2_NULL_INDEX ) + { + continue; + } + + const b2TreeNode* node = tree->nodes + nodeId; + if ( b2AABB_Overlaps( node->aabb, segmentAABB ) == false || ( node->categoryBits & maskBits ) == 0 ) + { + continue; + } + + // Separating axis for segment (Gino, p80). + // |dot(v, p1 - c)| > dot(|v|, h) + // radius extension is added to the node in this case + b2Vec2 c = b2AABB_Center( node->aabb ); + b2Vec2 h = b2AABB_Extents( node->aabb ); + float term1 = b2AbsFloat( b2Dot( v, b2Sub( p1, c ) ) ); + float term2 = b2Dot( abs_v, h ); + if ( term2 < term1 ) + { + continue; + } + + if ( b2IsLeaf( node ) ) + { + subInput.maxFraction = maxFraction; + + float value = callback( &subInput, nodeId, node->userData, context ); + + if ( value == 0.0f ) + { + // The client has terminated the ray cast. + return; + } + + if ( 0.0f < value && value < maxFraction ) + { + // Update segment bounding box. + maxFraction = value; + p2 = b2MulAdd( p1, maxFraction, d ); + segmentAABB.lowerBound = b2Min( p1, p2 ); + segmentAABB.upperBound = b2Max( p1, p2 ); + } + } + else + { + B2_ASSERT( stackCount < b2_treeStackSize - 1 ); + if ( stackCount < b2_treeStackSize - 1 ) + { + // TODO_ERIN just put one node on the stack, continue on a child node + // TODO_ERIN test ordering children by nearest to ray origin + stack[stackCount++] = node->child1; + stack[stackCount++] = node->child2; + } + } + } +} + +void b2DynamicTree_ShapeCast( const b2DynamicTree* tree, const b2ShapeCastInput* input, uint64_t maskBits, + b2TreeShapeCastCallbackFcn* callback, void* context ) +{ + if ( input->count == 0 ) + { + return; + } + + b2AABB originAABB = { input->points[0], input->points[0] }; + for ( int i = 1; i < input->count; ++i ) + { + originAABB.lowerBound = b2Min( originAABB.lowerBound, input->points[i] ); + originAABB.upperBound = b2Max( originAABB.upperBound, input->points[i] ); + } + + b2Vec2 radius = { input->radius, input->radius }; + + originAABB.lowerBound = b2Sub( originAABB.lowerBound, radius ); + originAABB.upperBound = b2Add( originAABB.upperBound, radius ); + + b2Vec2 p1 = b2AABB_Center( originAABB ); + b2Vec2 extension = b2AABB_Extents( originAABB ); + + // v is perpendicular to the segment. + b2Vec2 r = input->translation; + b2Vec2 v = b2CrossSV( 1.0f, r ); + b2Vec2 abs_v = b2Abs( v ); + + // Separating axis for segment (Gino, p80). + // |dot(v, p1 - c)| > dot(|v|, h) + + float maxFraction = input->maxFraction; + + // Build total box for the shape cast + b2Vec2 t = b2MulSV( maxFraction, input->translation ); + b2AABB totalAABB = { + b2Min( originAABB.lowerBound, b2Add( originAABB.lowerBound, t ) ), + b2Max( originAABB.upperBound, b2Add( originAABB.upperBound, t ) ), + }; + + b2ShapeCastInput subInput = *input; + + int32_t stack[b2_treeStackSize]; + int32_t stackCount = 0; + stack[stackCount++] = tree->root; + + while ( stackCount > 0 ) + { + int32_t nodeId = stack[--stackCount]; + if ( nodeId == B2_NULL_INDEX ) + { + continue; + } + + const b2TreeNode* node = tree->nodes + nodeId; + if ( b2AABB_Overlaps( node->aabb, totalAABB ) == false || ( node->categoryBits & maskBits ) == 0 ) + { + continue; + } + + // Separating axis for segment (Gino, p80). + // |dot(v, p1 - c)| > dot(|v|, h) + // radius extension is added to the node in this case + b2Vec2 c = b2AABB_Center( node->aabb ); + b2Vec2 h = b2Add( b2AABB_Extents( node->aabb ), extension ); + float term1 = b2AbsFloat( b2Dot( v, b2Sub( p1, c ) ) ); + float term2 = b2Dot( abs_v, h ); + if ( term2 < term1 ) + { + continue; + } + + if ( b2IsLeaf( node ) ) + { + subInput.maxFraction = maxFraction; + + float value = callback( &subInput, nodeId, node->userData, context ); + + if ( value == 0.0f ) + { + // The client has terminated the ray cast. + return; + } + + if ( 0.0f < value && value < maxFraction ) + { + // Update segment bounding box. + maxFraction = value; + t = b2MulSV( maxFraction, input->translation ); + totalAABB.lowerBound = b2Min( originAABB.lowerBound, b2Add( originAABB.lowerBound, t ) ); + totalAABB.upperBound = b2Max( originAABB.upperBound, b2Add( originAABB.upperBound, t ) ); + } + } + else + { + B2_ASSERT( stackCount < b2_treeStackSize - 1 ); + if ( stackCount < b2_treeStackSize - 1 ) + { + // TODO_ERIN just put one node on the stack, continue on a child node + // TODO_ERIN test ordering children by nearest to ray origin + stack[stackCount++] = node->child1; + stack[stackCount++] = node->child2; + } + } + } +} + +// Median split == 0, Surface area heuristic == 1 +#define B2_TREE_HEURISTIC 0 + +#if B2_TREE_HEURISTIC == 0 + +// Median split heuristic +static int32_t b2PartitionMid( int32_t* indices, b2Vec2* centers, int32_t count ) +{ + // Handle trivial case + if ( count <= 2 ) + { + return count / 2; + } + + // todo SIMD? + b2Vec2 lowerBound = centers[0]; + b2Vec2 upperBound = centers[0]; + + for ( int32_t i = 1; i < count; ++i ) + { + lowerBound = b2Min( lowerBound, centers[i] ); + upperBound = b2Max( upperBound, centers[i] ); + } + + b2Vec2 d = b2Sub( upperBound, lowerBound ); + b2Vec2 c = { 0.5f * ( lowerBound.x + upperBound.x ), 0.5f * ( lowerBound.y + upperBound.y ) }; + + // Partition longest axis using the Hoare partition scheme + // https://en.wikipedia.org/wiki/Quicksort + // https://nicholasvadivelu.com/2021/01/11/array-partition/ + int32_t i1 = 0, i2 = count; + if ( d.x > d.y ) + { + float pivot = c.x; + + while ( i1 < i2 ) + { + while ( i1 < i2 && centers[i1].x < pivot ) + { + i1 += 1; + }; + + while ( i1 < i2 && centers[i2 - 1].x >= pivot ) + { + i2 -= 1; + }; + + if ( i1 < i2 ) + { + // Swap indices + { + int32_t temp = indices[i1]; + indices[i1] = indices[i2 - 1]; + indices[i2 - 1] = temp; + } + + // Swap centers + { + b2Vec2 temp = centers[i1]; + centers[i1] = centers[i2 - 1]; + centers[i2 - 1] = temp; + } + + i1 += 1; + i2 -= 1; + } + } + } + else + { + float pivot = c.y; + + while ( i1 < i2 ) + { + while ( i1 < i2 && centers[i1].y < pivot ) + { + i1 += 1; + }; + + while ( i1 < i2 && centers[i2 - 1].y >= pivot ) + { + i2 -= 1; + }; + + if ( i1 < i2 ) + { + // Swap indices + { + int32_t temp = indices[i1]; + indices[i1] = indices[i2 - 1]; + indices[i2 - 1] = temp; + } + + // Swap centers + { + b2Vec2 temp = centers[i1]; + centers[i1] = centers[i2 - 1]; + centers[i2 - 1] = temp; + } + + i1 += 1; + i2 -= 1; + } + } + } + B2_ASSERT( i1 == i2 ); + + if ( i1 > 0 && i1 < count ) + { + return i1; + } + else + { + return count / 2; + } +} + +#else + + #define B2_BIN_COUNT 8 + +typedef struct b2TreeBin +{ + b2AABB aabb; + int32_t count; +} b2TreeBin; + +typedef struct b2TreePlane +{ + b2AABB leftAABB; + b2AABB rightAABB; + int32_t leftCount; + int32_t rightCount; +} b2TreePlane; + +// "On Fast Construction of SAH-based Bounding Volume Hierarchies" by Ingo Wald +// Returns the left child count +static int32_t b2PartitionSAH( int32_t* indices, int32_t* binIndices, b2AABB* boxes, int32_t count ) +{ + B2_ASSERT( count > 0 ); + + b2TreeBin bins[B2_BIN_COUNT]; + b2TreePlane planes[B2_BIN_COUNT - 1]; + + b2Vec2 center = b2AABB_Center( boxes[0] ); + b2AABB centroidAABB; + centroidAABB.lowerBound = center; + centroidAABB.upperBound = center; + + for ( int32_t i = 1; i < count; ++i ) + { + center = b2AABB_Center( boxes[i] ); + centroidAABB.lowerBound = b2Min( centroidAABB.lowerBound, center ); + centroidAABB.upperBound = b2Max( centroidAABB.upperBound, center ); + } + + b2Vec2 d = b2Sub( centroidAABB.upperBound, centroidAABB.lowerBound ); + + // Find longest axis + int32_t axisIndex; + float invD; + if ( d.x > d.y ) + { + axisIndex = 0; + invD = d.x; + } + else + { + axisIndex = 1; + invD = d.y; + } + + invD = invD > 0.0f ? 1.0f / invD : 0.0f; + + // Initialize bin bounds and count + for ( int32_t i = 0; i < B2_BIN_COUNT; ++i ) + { + bins[i].aabb.lowerBound = ( b2Vec2 ){ FLT_MAX, FLT_MAX }; + bins[i].aabb.upperBound = ( b2Vec2 ){ -FLT_MAX, -FLT_MAX }; + bins[i].count = 0; + } + + // Assign boxes to bins and compute bin boxes + // TODO_ERIN optimize + float binCount = B2_BIN_COUNT; + float lowerBoundArray[2] = { centroidAABB.lowerBound.x, centroidAABB.lowerBound.y }; + float minC = lowerBoundArray[axisIndex]; + for ( int32_t i = 0; i < count; ++i ) + { + b2Vec2 c = b2AABB_Center( boxes[i] ); + float cArray[2] = { c.x, c.y }; + int32_t binIndex = (int32_t)( binCount * ( cArray[axisIndex] - minC ) * invD ); + binIndex = b2ClampInt( binIndex, 0, B2_BIN_COUNT - 1 ); + binIndices[i] = binIndex; + bins[binIndex].count += 1; + bins[binIndex].aabb = b2AABB_Union( bins[binIndex].aabb, boxes[i] ); + } + + int32_t planeCount = B2_BIN_COUNT - 1; + + // Prepare all the left planes, candidates for left child + planes[0].leftCount = bins[0].count; + planes[0].leftAABB = bins[0].aabb; + for ( int32_t i = 1; i < planeCount; ++i ) + { + planes[i].leftCount = planes[i - 1].leftCount + bins[i].count; + planes[i].leftAABB = b2AABB_Union( planes[i - 1].leftAABB, bins[i].aabb ); + } + + // Prepare all the right planes, candidates for right child + planes[planeCount - 1].rightCount = bins[planeCount].count; + planes[planeCount - 1].rightAABB = bins[planeCount].aabb; + for ( int32_t i = planeCount - 2; i >= 0; --i ) + { + planes[i].rightCount = planes[i + 1].rightCount + bins[i + 1].count; + planes[i].rightAABB = b2AABB_Union( planes[i + 1].rightAABB, bins[i + 1].aabb ); + } + + // Find best split to minimize SAH + float minCost = FLT_MAX; + int32_t bestPlane = 0; + for ( int32_t i = 0; i < planeCount; ++i ) + { + float leftArea = b2Perimeter( planes[i].leftAABB ); + float rightArea = b2Perimeter( planes[i].rightAABB ); + int32_t leftCount = planes[i].leftCount; + int32_t rightCount = planes[i].rightCount; + + float cost = leftCount * leftArea + rightCount * rightArea; + if ( cost < minCost ) + { + bestPlane = i; + minCost = cost; + } + } + + // Partition node indices and boxes using the Hoare partition scheme + // https://en.wikipedia.org/wiki/Quicksort + // https://nicholasvadivelu.com/2021/01/11/array-partition/ + int32_t i1 = 0, i2 = count; + while ( i1 < i2 ) + { + while ( i1 < i2 && binIndices[i1] < bestPlane ) + { + i1 += 1; + }; + + while ( i1 < i2 && binIndices[i2 - 1] >= bestPlane ) + { + i2 -= 1; + }; + + if ( i1 < i2 ) + { + // Swap indices + { + int32_t temp = indices[i1]; + indices[i1] = indices[i2 - 1]; + indices[i2 - 1] = temp; + } + + // Swap boxes + { + b2AABB temp = boxes[i1]; + boxes[i1] = boxes[i2 - 1]; + boxes[i2 - 1] = temp; + } + + i1 += 1; + i2 -= 1; + } + } + B2_ASSERT( i1 == i2 ); + + if ( i1 > 0 && i1 < count ) + { + return i1; + } + else + { + return count / 2; + } +} + +#endif + +// Temporary data used to track the rebuild of a tree node +struct b2RebuildItem +{ + int32_t nodeIndex; + int32_t childCount; + + // Leaf indices + int32_t startIndex; + int32_t splitIndex; + int32_t endIndex; +}; + +// Returns root node index +static int32_t b2BuildTree( b2DynamicTree* tree, int32_t leafCount ) +{ + b2TreeNode* nodes = tree->nodes; + int32_t* leafIndices = tree->leafIndices; + + if ( leafCount == 1 ) + { + nodes[leafIndices[0]].parent = B2_NULL_INDEX; + return leafIndices[0]; + } + +#if B2_TREE_HEURISTIC == 0 + b2Vec2* leafCenters = tree->leafCenters; +#else + b2AABB* leafBoxes = tree->leafBoxes; + int32_t* binIndices = tree->binIndices; +#endif + + // todo large stack item + struct b2RebuildItem stack[b2_treeStackSize]; + int32_t top = 0; + + stack[0].nodeIndex = b2AllocateNode( tree ); + stack[0].childCount = -1; + stack[0].startIndex = 0; + stack[0].endIndex = leafCount; +#if B2_TREE_HEURISTIC == 0 + stack[0].splitIndex = b2PartitionMid( leafIndices, leafCenters, leafCount ); +#else + stack[0].splitIndex = b2PartitionSAH( leafIndices, binIndices, leafBoxes, leafCount ); +#endif + + while ( true ) + { + struct b2RebuildItem* item = stack + top; + + item->childCount += 1; + + if ( item->childCount == 2 ) + { + // This internal node has both children established + + if ( top == 0 ) + { + // all done + break; + } + + struct b2RebuildItem* parentItem = stack + ( top - 1 ); + b2TreeNode* parentNode = nodes + parentItem->nodeIndex; + + if ( parentItem->childCount == 0 ) + { + B2_ASSERT( parentNode->child1 == B2_NULL_INDEX ); + parentNode->child1 = item->nodeIndex; + } + else + { + B2_ASSERT( parentItem->childCount == 1 ); + B2_ASSERT( parentNode->child2 == B2_NULL_INDEX ); + parentNode->child2 = item->nodeIndex; + } + + b2TreeNode* node = nodes + item->nodeIndex; + + B2_ASSERT( node->parent == B2_NULL_INDEX ); + node->parent = parentItem->nodeIndex; + + B2_ASSERT( node->child1 != B2_NULL_INDEX ); + B2_ASSERT( node->child2 != B2_NULL_INDEX ); + b2TreeNode* child1 = nodes + node->child1; + b2TreeNode* child2 = nodes + node->child2; + + node->aabb = b2AABB_Union( child1->aabb, child2->aabb ); + node->height = 1 + b2MaxInt16( child1->height, child2->height ); + node->categoryBits = child1->categoryBits | child2->categoryBits; + + // Pop stack + top -= 1; + } + else + { + int32_t startIndex, endIndex; + if ( item->childCount == 0 ) + { + startIndex = item->startIndex; + endIndex = item->splitIndex; + } + else + { + B2_ASSERT( item->childCount == 1 ); + startIndex = item->splitIndex; + endIndex = item->endIndex; + } + + int32_t count = endIndex - startIndex; + + if ( count == 1 ) + { + int32_t childIndex = leafIndices[startIndex]; + b2TreeNode* node = nodes + item->nodeIndex; + + if ( item->childCount == 0 ) + { + B2_ASSERT( node->child1 == B2_NULL_INDEX ); + node->child1 = childIndex; + } + else + { + B2_ASSERT( item->childCount == 1 ); + B2_ASSERT( node->child2 == B2_NULL_INDEX ); + node->child2 = childIndex; + } + + b2TreeNode* childNode = nodes + childIndex; + B2_ASSERT( childNode->parent == B2_NULL_INDEX ); + childNode->parent = item->nodeIndex; + } + else + { + B2_ASSERT( count > 0 ); + B2_ASSERT( top < b2_treeStackSize ); + + top += 1; + struct b2RebuildItem* newItem = stack + top; + newItem->nodeIndex = b2AllocateNode( tree ); + newItem->childCount = -1; + newItem->startIndex = startIndex; + newItem->endIndex = endIndex; +#if B2_TREE_HEURISTIC == 0 + newItem->splitIndex = b2PartitionMid( leafIndices + startIndex, leafCenters + startIndex, count ); +#else + newItem->splitIndex = + b2PartitionSAH( leafIndices + startIndex, binIndices + startIndex, leafBoxes + startIndex, count ); +#endif + newItem->splitIndex += startIndex; + } + } + } + + b2TreeNode* rootNode = nodes + stack[0].nodeIndex; + B2_ASSERT( rootNode->parent == B2_NULL_INDEX ); + B2_ASSERT( rootNode->child1 != B2_NULL_INDEX ); + B2_ASSERT( rootNode->child2 != B2_NULL_INDEX ); + + b2TreeNode* child1 = nodes + rootNode->child1; + b2TreeNode* child2 = nodes + rootNode->child2; + + rootNode->aabb = b2AABB_Union( child1->aabb, child2->aabb ); + rootNode->height = 1 + b2MaxInt16( child1->height, child2->height ); + rootNode->categoryBits = child1->categoryBits | child2->categoryBits; + + return stack[0].nodeIndex; +} + +// Not safe to access tree during this operation because it may grow +int32_t b2DynamicTree_Rebuild( b2DynamicTree* tree, bool fullBuild ) +{ + int32_t proxyCount = tree->proxyCount; + if ( proxyCount == 0 ) + { + return 0; + } + + // Ensure capacity for rebuild space + if ( proxyCount > tree->rebuildCapacity ) + { + int32_t newCapacity = proxyCount + proxyCount / 2; + + b2Free( tree->leafIndices, tree->rebuildCapacity * sizeof( int32_t ) ); + tree->leafIndices = b2Alloc( newCapacity * sizeof( int32_t ) ); + +#if B2_TREE_HEURISTIC == 0 + b2Free( tree->leafCenters, tree->rebuildCapacity * sizeof( b2Vec2 ) ); + tree->leafCenters = b2Alloc( newCapacity * sizeof( b2Vec2 ) ); +#else + b2Free( tree->leafBoxes, tree->rebuildCapacity * sizeof( b2AABB ) ); + tree->leafBoxes = b2Alloc( newCapacity * sizeof( b2AABB ) ); + b2Free( tree->binIndices, tree->rebuildCapacity * sizeof( int32_t ) ); + tree->binIndices = b2Alloc( newCapacity * sizeof( int32_t ) ); +#endif + tree->rebuildCapacity = newCapacity; + } + + int32_t leafCount = 0; + int32_t stack[b2_treeStackSize]; + int32_t stackCount = 0; + + int32_t nodeIndex = tree->root; + b2TreeNode* nodes = tree->nodes; + b2TreeNode* node = nodes + nodeIndex; + + // These are the nodes that get sorted to rebuild the tree. + // I'm using indices because the node pool may grow during the build. + int32_t* leafIndices = tree->leafIndices; + +#if B2_TREE_HEURISTIC == 0 + b2Vec2* leafCenters = tree->leafCenters; +#else + b2AABB* leafBoxes = tree->leafBoxes; +#endif + + // Gather all proxy nodes that have grown and all internal nodes that haven't grown. Both are + // considered leaves in the tree rebuild. + // Free all internal nodes that have grown. + // todo use a node growth metric instead of simply enlarged to reduce rebuild size and frequency + // this should be weighed against b2_aabbMargin + while ( true ) + { + if ( node->height == 0 || ( node->enlarged == false && fullBuild == false ) ) + { + leafIndices[leafCount] = nodeIndex; +#if B2_TREE_HEURISTIC == 0 + leafCenters[leafCount] = b2AABB_Center( node->aabb ); +#else + leafBoxes[leafCount] = node->aabb; +#endif + leafCount += 1; + + // Detach + node->parent = B2_NULL_INDEX; + } + else + { + int32_t doomedNodeIndex = nodeIndex; + + // Handle children + nodeIndex = node->child1; + + B2_ASSERT( stackCount < b2_treeStackSize ); + if ( stackCount < b2_treeStackSize ) + { + stack[stackCount++] = node->child2; + } + + node = nodes + nodeIndex; + + // Remove doomed node + b2FreeNode( tree, doomedNodeIndex ); + + continue; + } + + if ( stackCount == 0 ) + { + break; + } + + nodeIndex = stack[--stackCount]; + node = nodes + nodeIndex; + } + +#if B2_VALIDATE == 1 + int32_t capacity = tree->nodeCapacity; + for ( int32_t i = 0; i < capacity; ++i ) + { + if ( nodes[i].height >= 0 ) + { + B2_ASSERT( nodes[i].enlarged == false ); + } + } +#endif + + B2_ASSERT( leafCount <= proxyCount ); + + tree->root = b2BuildTree( tree, leafCount ); + + b2DynamicTree_Validate( tree ); + + return leafCount; +} diff --git a/3rdparty/box2d/src/dynamics/b2_body.cpp b/3rdparty/box2d/src/dynamics/b2_body.cpp deleted file mode 100644 index 00f36856dde1..000000000000 --- a/3rdparty/box2d/src/dynamics/b2_body.cpp +++ /dev/null @@ -1,570 +0,0 @@ -// MIT License - -// Copyright (c) 2019 Erin Catto - -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: - -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. - -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -#include "box2d/b2_body.h" -#include "box2d/b2_contact.h" -#include "box2d/b2_fixture.h" -#include "box2d/b2_joint.h" -#include "box2d/b2_world.h" - -#include - -b2Body::b2Body(const b2BodyDef* bd, b2World* world) -{ - b2Assert(bd->position.IsValid()); - b2Assert(bd->linearVelocity.IsValid()); - b2Assert(b2IsValid(bd->angle)); - b2Assert(b2IsValid(bd->angularVelocity)); - b2Assert(b2IsValid(bd->angularDamping) && bd->angularDamping >= 0.0f); - b2Assert(b2IsValid(bd->linearDamping) && bd->linearDamping >= 0.0f); - - m_flags = 0; - - if (bd->bullet) - { - m_flags |= e_bulletFlag; - } - if (bd->fixedRotation) - { - m_flags |= e_fixedRotationFlag; - } - if (bd->allowSleep) - { - m_flags |= e_autoSleepFlag; - } - if (bd->awake && bd->type != b2_staticBody) - { - m_flags |= e_awakeFlag; - } - if (bd->enabled) - { - m_flags |= e_enabledFlag; - } - - m_world = world; - - m_xf.p = bd->position; - m_xf.q.Set(bd->angle); - - m_sweep.localCenter.SetZero(); - m_sweep.c0 = m_xf.p; - m_sweep.c = m_xf.p; - m_sweep.a0 = bd->angle; - m_sweep.a = bd->angle; - m_sweep.alpha0 = 0.0f; - - m_jointList = nullptr; - m_contactList = nullptr; - m_prev = nullptr; - m_next = nullptr; - - m_linearVelocity = bd->linearVelocity; - m_angularVelocity = bd->angularVelocity; - - m_linearDamping = bd->linearDamping; - m_angularDamping = bd->angularDamping; - m_gravityScale = bd->gravityScale; - - m_force.SetZero(); - m_torque = 0.0f; - - m_sleepTime = 0.0f; - - m_type = bd->type; - - m_mass = 0.0f; - m_invMass = 0.0f; - - m_I = 0.0f; - m_invI = 0.0f; - - m_userData = bd->userData; - - m_fixtureList = nullptr; - m_fixtureCount = 0; -} - -b2Body::~b2Body() -{ - // shapes and joints are destroyed in b2World::Destroy -} - -void b2Body::SetType(b2BodyType type) -{ - b2Assert(m_world->IsLocked() == false); - if (m_world->IsLocked() == true) - { - return; - } - - if (m_type == type) - { - return; - } - - m_type = type; - - ResetMassData(); - - if (m_type == b2_staticBody) - { - m_linearVelocity.SetZero(); - m_angularVelocity = 0.0f; - m_sweep.a0 = m_sweep.a; - m_sweep.c0 = m_sweep.c; - m_flags &= ~e_awakeFlag; - SynchronizeFixtures(); - } - - SetAwake(true); - - m_force.SetZero(); - m_torque = 0.0f; - - // Delete the attached contacts. - b2ContactEdge* ce = m_contactList; - while (ce) - { - b2ContactEdge* ce0 = ce; - ce = ce->next; - m_world->m_contactManager.Destroy(ce0->contact); - } - m_contactList = nullptr; - - // Touch the proxies so that new contacts will be created (when appropriate) - b2BroadPhase* broadPhase = &m_world->m_contactManager.m_broadPhase; - for (b2Fixture* f = m_fixtureList; f; f = f->m_next) - { - int32 proxyCount = f->m_proxyCount; - for (int32 i = 0; i < proxyCount; ++i) - { - broadPhase->TouchProxy(f->m_proxies[i].proxyId); - } - } -} - -b2Fixture* b2Body::CreateFixture(const b2FixtureDef* def) -{ - b2Assert(m_world->IsLocked() == false); - if (m_world->IsLocked() == true) - { - return nullptr; - } - - b2BlockAllocator* allocator = &m_world->m_blockAllocator; - - void* memory = allocator->Allocate(sizeof(b2Fixture)); - b2Fixture* fixture = new (memory) b2Fixture; - fixture->Create(allocator, this, def); - - if (m_flags & e_enabledFlag) - { - b2BroadPhase* broadPhase = &m_world->m_contactManager.m_broadPhase; - fixture->CreateProxies(broadPhase, m_xf); - } - - fixture->m_next = m_fixtureList; - m_fixtureList = fixture; - ++m_fixtureCount; - - fixture->m_body = this; - - // Adjust mass properties if needed. - if (fixture->m_density > 0.0f) - { - ResetMassData(); - } - - // Let the world know we have a new fixture. This will cause new contacts - // to be created at the beginning of the next time step. - m_world->m_newContacts = true; - - return fixture; -} - -b2Fixture* b2Body::CreateFixture(const b2Shape* shape, float density) -{ - b2FixtureDef def; - def.shape = shape; - def.density = density; - - return CreateFixture(&def); -} - -void b2Body::DestroyFixture(b2Fixture* fixture) -{ - if (fixture == NULL) - { - return; - } - - b2Assert(m_world->IsLocked() == false); - if (m_world->IsLocked() == true) - { - return; - } - - b2Assert(fixture->m_body == this); - - // Remove the fixture from this body's singly linked list. - b2Assert(m_fixtureCount > 0); - b2Fixture** node = &m_fixtureList; - bool found = false; - while (*node != nullptr) - { - if (*node == fixture) - { - *node = fixture->m_next; - found = true; - break; - } - - node = &(*node)->m_next; - } - - // You tried to remove a shape that is not attached to this body. - b2Assert(found); - - const float density = fixture->m_density; - - // Destroy any contacts associated with the fixture. - b2ContactEdge* edge = m_contactList; - while (edge) - { - b2Contact* c = edge->contact; - edge = edge->next; - - b2Fixture* fixtureA = c->GetFixtureA(); - b2Fixture* fixtureB = c->GetFixtureB(); - - if (fixture == fixtureA || fixture == fixtureB) - { - // This destroys the contact and removes it from - // this body's contact list. - m_world->m_contactManager.Destroy(c); - } - } - - b2BlockAllocator* allocator = &m_world->m_blockAllocator; - - if (m_flags & e_enabledFlag) - { - b2BroadPhase* broadPhase = &m_world->m_contactManager.m_broadPhase; - fixture->DestroyProxies(broadPhase); - } - - fixture->m_body = nullptr; - fixture->m_next = nullptr; - fixture->Destroy(allocator); - fixture->~b2Fixture(); - allocator->Free(fixture, sizeof(b2Fixture)); - - --m_fixtureCount; - - // Reset the mass data - if (density > 0.0f) - { - ResetMassData(); - } -} - -void b2Body::ResetMassData() -{ - // Compute mass data from shapes. Each shape has its own density. - m_mass = 0.0f; - m_invMass = 0.0f; - m_I = 0.0f; - m_invI = 0.0f; - m_sweep.localCenter.SetZero(); - - // Static and kinematic bodies have zero mass. - if (m_type == b2_staticBody || m_type == b2_kinematicBody) - { - m_sweep.c0 = m_xf.p; - m_sweep.c = m_xf.p; - m_sweep.a0 = m_sweep.a; - return; - } - - b2Assert(m_type == b2_dynamicBody); - - // Accumulate mass over all fixtures. - b2Vec2 localCenter = b2Vec2_zero; - for (b2Fixture* f = m_fixtureList; f; f = f->m_next) - { - if (f->m_density == 0.0f) - { - continue; - } - - b2MassData massData; - f->GetMassData(&massData); - m_mass += massData.mass; - localCenter += massData.mass * massData.center; - m_I += massData.I; - } - - // Compute center of mass. - if (m_mass > 0.0f) - { - m_invMass = 1.0f / m_mass; - localCenter *= m_invMass; - } - - if (m_I > 0.0f && (m_flags & e_fixedRotationFlag) == 0) - { - // Center the inertia about the center of mass. - m_I -= m_mass * b2Dot(localCenter, localCenter); - b2Assert(m_I > 0.0f); - m_invI = 1.0f / m_I; - - } - else - { - m_I = 0.0f; - m_invI = 0.0f; - } - - // Move center of mass. - b2Vec2 oldCenter = m_sweep.c; - m_sweep.localCenter = localCenter; - m_sweep.c0 = m_sweep.c = b2Mul(m_xf, m_sweep.localCenter); - - // Update center of mass velocity. - m_linearVelocity += b2Cross(m_angularVelocity, m_sweep.c - oldCenter); -} - -void b2Body::SetMassData(const b2MassData* massData) -{ - b2Assert(m_world->IsLocked() == false); - if (m_world->IsLocked() == true) - { - return; - } - - if (m_type != b2_dynamicBody) - { - return; - } - - m_invMass = 0.0f; - m_I = 0.0f; - m_invI = 0.0f; - - m_mass = massData->mass; - if (m_mass <= 0.0f) - { - m_mass = 1.0f; - } - - m_invMass = 1.0f / m_mass; - - if (massData->I > 0.0f && (m_flags & b2Body::e_fixedRotationFlag) == 0) - { - m_I = massData->I - m_mass * b2Dot(massData->center, massData->center); - b2Assert(m_I > 0.0f); - m_invI = 1.0f / m_I; - } - - // Move center of mass. - b2Vec2 oldCenter = m_sweep.c; - m_sweep.localCenter = massData->center; - m_sweep.c0 = m_sweep.c = b2Mul(m_xf, m_sweep.localCenter); - - // Update center of mass velocity. - m_linearVelocity += b2Cross(m_angularVelocity, m_sweep.c - oldCenter); -} - -bool b2Body::ShouldCollide(const b2Body* other) const -{ - // At least one body should be dynamic. - if (m_type != b2_dynamicBody && other->m_type != b2_dynamicBody) - { - return false; - } - - // Does a joint prevent collision? - for (b2JointEdge* jn = m_jointList; jn; jn = jn->next) - { - if (jn->other == other) - { - if (jn->joint->m_collideConnected == false) - { - return false; - } - } - } - - return true; -} - -void b2Body::SetTransform(const b2Vec2& position, float angle) -{ - b2Assert(m_world->IsLocked() == false); - if (m_world->IsLocked() == true) - { - return; - } - - m_xf.q.Set(angle); - m_xf.p = position; - - m_sweep.c = b2Mul(m_xf, m_sweep.localCenter); - m_sweep.a = angle; - - m_sweep.c0 = m_sweep.c; - m_sweep.a0 = angle; - - b2BroadPhase* broadPhase = &m_world->m_contactManager.m_broadPhase; - for (b2Fixture* f = m_fixtureList; f; f = f->m_next) - { - f->Synchronize(broadPhase, m_xf, m_xf); - } - - // Check for new contacts the next step - m_world->m_newContacts = true; -} - -void b2Body::SynchronizeFixtures() -{ - b2BroadPhase* broadPhase = &m_world->m_contactManager.m_broadPhase; - - if (m_flags & b2Body::e_awakeFlag) - { - b2Transform xf1; - xf1.q.Set(m_sweep.a0); - xf1.p = m_sweep.c0 - b2Mul(xf1.q, m_sweep.localCenter); - - for (b2Fixture* f = m_fixtureList; f; f = f->m_next) - { - f->Synchronize(broadPhase, xf1, m_xf); - } - } - else - { - for (b2Fixture* f = m_fixtureList; f; f = f->m_next) - { - f->Synchronize(broadPhase, m_xf, m_xf); - } - } -} - -void b2Body::SetEnabled(bool flag) -{ - b2Assert(m_world->IsLocked() == false); - - if (flag == IsEnabled()) - { - return; - } - - if (flag) - { - m_flags |= e_enabledFlag; - - // Create all proxies. - b2BroadPhase* broadPhase = &m_world->m_contactManager.m_broadPhase; - for (b2Fixture* f = m_fixtureList; f; f = f->m_next) - { - f->CreateProxies(broadPhase, m_xf); - } - - // Contacts are created at the beginning of the next - m_world->m_newContacts = true; - } - else - { - m_flags &= ~e_enabledFlag; - - // Destroy all proxies. - b2BroadPhase* broadPhase = &m_world->m_contactManager.m_broadPhase; - for (b2Fixture* f = m_fixtureList; f; f = f->m_next) - { - f->DestroyProxies(broadPhase); - } - - // Destroy the attached contacts. - b2ContactEdge* ce = m_contactList; - while (ce) - { - b2ContactEdge* ce0 = ce; - ce = ce->next; - m_world->m_contactManager.Destroy(ce0->contact); - } - m_contactList = nullptr; - } -} - -void b2Body::SetFixedRotation(bool flag) -{ - bool status = (m_flags & e_fixedRotationFlag) == e_fixedRotationFlag; - if (status == flag) - { - return; - } - - if (flag) - { - m_flags |= e_fixedRotationFlag; - } - else - { - m_flags &= ~e_fixedRotationFlag; - } - - m_angularVelocity = 0.0f; - - ResetMassData(); -} - -void b2Body::Dump() -{ - int32 bodyIndex = m_islandIndex; - - // %.9g is sufficient to save and load the same value using text - // FLT_DECIMAL_DIG == 9 - - b2Dump("{\n"); - b2Dump(" b2BodyDef bd;\n"); - b2Dump(" bd.type = b2BodyType(%d);\n", m_type); - b2Dump(" bd.position.Set(%.9g, %.9g);\n", m_xf.p.x, m_xf.p.y); - b2Dump(" bd.angle = %.9g;\n", m_sweep.a); - b2Dump(" bd.linearVelocity.Set(%.9g, %.9g);\n", m_linearVelocity.x, m_linearVelocity.y); - b2Dump(" bd.angularVelocity = %.9g;\n", m_angularVelocity); - b2Dump(" bd.linearDamping = %.9g;\n", m_linearDamping); - b2Dump(" bd.angularDamping = %.9g;\n", m_angularDamping); - b2Dump(" bd.allowSleep = bool(%d);\n", m_flags & e_autoSleepFlag); - b2Dump(" bd.awake = bool(%d);\n", m_flags & e_awakeFlag); - b2Dump(" bd.fixedRotation = bool(%d);\n", m_flags & e_fixedRotationFlag); - b2Dump(" bd.bullet = bool(%d);\n", m_flags & e_bulletFlag); - b2Dump(" bd.enabled = bool(%d);\n", m_flags & e_enabledFlag); - b2Dump(" bd.gravityScale = %.9g;\n", m_gravityScale); - b2Dump(" bodies[%d] = m_world->CreateBody(&bd);\n", m_islandIndex); - b2Dump("\n"); - for (b2Fixture* f = m_fixtureList; f; f = f->m_next) - { - b2Dump(" {\n"); - f->Dump(bodyIndex); - b2Dump(" }\n"); - } - b2Dump("}\n"); -} diff --git a/3rdparty/box2d/src/dynamics/b2_chain_circle_contact.cpp b/3rdparty/box2d/src/dynamics/b2_chain_circle_contact.cpp deleted file mode 100644 index 8464fe836312..000000000000 --- a/3rdparty/box2d/src/dynamics/b2_chain_circle_contact.cpp +++ /dev/null @@ -1,57 +0,0 @@ -// MIT License - -// Copyright (c) 2019 Erin Catto - -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: - -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. - -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -#include "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/3rdparty/box2d/src/dynamics/b2_chain_circle_contact.h b/3rdparty/box2d/src/dynamics/b2_chain_circle_contact.h deleted file mode 100644 index 33ced64d80be..000000000000 --- a/3rdparty/box2d/src/dynamics/b2_chain_circle_contact.h +++ /dev/null @@ -1,43 +0,0 @@ -// MIT License - -// Copyright (c) 2019 Erin Catto - -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: - -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. - -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -#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/3rdparty/box2d/src/dynamics/b2_chain_polygon_contact.cpp b/3rdparty/box2d/src/dynamics/b2_chain_polygon_contact.cpp deleted file mode 100644 index b8257aab9687..000000000000 --- a/3rdparty/box2d/src/dynamics/b2_chain_polygon_contact.cpp +++ /dev/null @@ -1,57 +0,0 @@ -// MIT License - -// Copyright (c) 2019 Erin Catto - -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: - -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. - -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -#include "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/3rdparty/box2d/src/dynamics/b2_chain_polygon_contact.h b/3rdparty/box2d/src/dynamics/b2_chain_polygon_contact.h deleted file mode 100644 index 058154fb1529..000000000000 --- a/3rdparty/box2d/src/dynamics/b2_chain_polygon_contact.h +++ /dev/null @@ -1,43 +0,0 @@ -// MIT License - -// Copyright (c) 2019 Erin Catto - -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: - -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. - -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -#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/3rdparty/box2d/src/dynamics/b2_circle_contact.cpp b/3rdparty/box2d/src/dynamics/b2_circle_contact.cpp deleted file mode 100644 index 738aa84ca9ca..000000000000 --- a/3rdparty/box2d/src/dynamics/b2_circle_contact.cpp +++ /dev/null @@ -1,56 +0,0 @@ -// MIT License - -// Copyright (c) 2019 Erin Catto - -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: - -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. - -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -#include "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/3rdparty/box2d/src/dynamics/b2_circle_contact.h b/3rdparty/box2d/src/dynamics/b2_circle_contact.h deleted file mode 100644 index 98e09ea5b93a..000000000000 --- a/3rdparty/box2d/src/dynamics/b2_circle_contact.h +++ /dev/null @@ -1,43 +0,0 @@ -// MIT License - -// Copyright (c) 2019 Erin Catto - -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: - -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. - -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -#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/3rdparty/box2d/src/dynamics/b2_contact.cpp b/3rdparty/box2d/src/dynamics/b2_contact.cpp deleted file mode 100644 index 1c65bc895f54..000000000000 --- a/3rdparty/box2d/src/dynamics/b2_contact.cpp +++ /dev/null @@ -1,252 +0,0 @@ -// MIT License - -// Copyright (c) 2019 Erin Catto - -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: - -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. - -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -#include "b2_chain_circle_contact.h" -#include "b2_chain_polygon_contact.h" -#include "b2_circle_contact.h" -#include "b2_contact_solver.h" -#include "b2_edge_circle_contact.h" -#include "b2_edge_polygon_contact.h" -#include "b2_polygon_circle_contact.h" -#include "b2_polygon_contact.h" - -#include "box2d/b2_contact.h" -#include "box2d/b2_block_allocator.h" -#include "box2d/b2_body.h" -#include "box2d/b2_collision.h" -#include "box2d/b2_fixture.h" -#include "box2d/b2_shape.h" -#include "box2d/b2_time_of_impact.h" -#include "box2d/b2_world.h" - -b2ContactRegister b2Contact::s_registers[b2Shape::e_typeCount][b2Shape::e_typeCount]; -bool b2Contact::s_initialized = false; - -void b2Contact::InitializeRegisters() -{ - AddType(b2CircleContact::Create, b2CircleContact::Destroy, b2Shape::e_circle, b2Shape::e_circle); - AddType(b2PolygonAndCircleContact::Create, b2PolygonAndCircleContact::Destroy, b2Shape::e_polygon, b2Shape::e_circle); - AddType(b2PolygonContact::Create, b2PolygonContact::Destroy, b2Shape::e_polygon, b2Shape::e_polygon); - AddType(b2EdgeAndCircleContact::Create, b2EdgeAndCircleContact::Destroy, b2Shape::e_edge, b2Shape::e_circle); - AddType(b2EdgeAndPolygonContact::Create, b2EdgeAndPolygonContact::Destroy, b2Shape::e_edge, b2Shape::e_polygon); - AddType(b2ChainAndCircleContact::Create, b2ChainAndCircleContact::Destroy, b2Shape::e_chain, b2Shape::e_circle); - AddType(b2ChainAndPolygonContact::Create, b2ChainAndPolygonContact::Destroy, b2Shape::e_chain, b2Shape::e_polygon); -} - -void b2Contact::AddType(b2ContactCreateFcn* createFcn, b2ContactDestroyFcn* destoryFcn, - b2Shape::Type type1, b2Shape::Type type2) -{ - b2Assert(0 <= type1 && type1 < b2Shape::e_typeCount); - b2Assert(0 <= type2 && type2 < b2Shape::e_typeCount); - - s_registers[type1][type2].createFcn = createFcn; - s_registers[type1][type2].destroyFcn = destoryFcn; - s_registers[type1][type2].primary = true; - - if (type1 != type2) - { - s_registers[type2][type1].createFcn = createFcn; - s_registers[type2][type1].destroyFcn = destoryFcn; - s_registers[type2][type1].primary = false; - } -} - -b2Contact* b2Contact::Create(b2Fixture* fixtureA, int32 indexA, b2Fixture* fixtureB, int32 indexB, b2BlockAllocator* allocator) -{ - if (s_initialized == false) - { - InitializeRegisters(); - s_initialized = true; - } - - b2Shape::Type type1 = fixtureA->GetType(); - b2Shape::Type type2 = fixtureB->GetType(); - - b2Assert(0 <= type1 && type1 < b2Shape::e_typeCount); - b2Assert(0 <= type2 && type2 < b2Shape::e_typeCount); - - b2ContactCreateFcn* createFcn = s_registers[type1][type2].createFcn; - if (createFcn) - { - if (s_registers[type1][type2].primary) - { - return createFcn(fixtureA, indexA, fixtureB, indexB, allocator); - } - else - { - return createFcn(fixtureB, indexB, fixtureA, indexA, allocator); - } - } - else - { - return nullptr; - } -} - -void b2Contact::Destroy(b2Contact* contact, b2BlockAllocator* allocator) -{ - b2Assert(s_initialized == true); - - b2Fixture* fixtureA = contact->m_fixtureA; - b2Fixture* fixtureB = contact->m_fixtureB; - - if (contact->m_manifold.pointCount > 0 && - fixtureA->IsSensor() == false && - fixtureB->IsSensor() == false) - { - fixtureA->GetBody()->SetAwake(true); - fixtureB->GetBody()->SetAwake(true); - } - - b2Shape::Type typeA = fixtureA->GetType(); - b2Shape::Type typeB = fixtureB->GetType(); - - b2Assert(0 <= typeA && typeA < b2Shape::e_typeCount); - b2Assert(0 <= typeB && typeB < b2Shape::e_typeCount); - - b2ContactDestroyFcn* destroyFcn = s_registers[typeA][typeB].destroyFcn; - destroyFcn(contact, allocator); -} - -b2Contact::b2Contact(b2Fixture* fA, int32 indexA, b2Fixture* fB, int32 indexB) -{ - m_flags = e_enabledFlag; - - m_fixtureA = fA; - m_fixtureB = fB; - - m_indexA = indexA; - m_indexB = indexB; - - m_manifold.pointCount = 0; - - m_prev = nullptr; - m_next = nullptr; - - m_nodeA.contact = nullptr; - m_nodeA.prev = nullptr; - m_nodeA.next = nullptr; - m_nodeA.other = nullptr; - - m_nodeB.contact = nullptr; - m_nodeB.prev = nullptr; - m_nodeB.next = nullptr; - m_nodeB.other = nullptr; - - m_toiCount = 0; - - m_friction = b2MixFriction(m_fixtureA->m_friction, m_fixtureB->m_friction); - m_restitution = b2MixRestitution(m_fixtureA->m_restitution, m_fixtureB->m_restitution); - m_restitutionThreshold = b2MixRestitutionThreshold(m_fixtureA->m_restitutionThreshold, m_fixtureB->m_restitutionThreshold); - - m_tangentSpeed = 0.0f; -} - -// Update the contact manifold and touching status. -// Note: do not assume the fixture AABBs are overlapping or are valid. -void b2Contact::Update(b2ContactListener* listener) -{ - b2Manifold oldManifold = m_manifold; - - // Re-enable this contact. - m_flags |= e_enabledFlag; - - bool touching = false; - bool wasTouching = (m_flags & e_touchingFlag) == e_touchingFlag; - - bool sensorA = m_fixtureA->IsSensor(); - bool sensorB = m_fixtureB->IsSensor(); - bool sensor = sensorA || sensorB; - - b2Body* bodyA = m_fixtureA->GetBody(); - b2Body* bodyB = m_fixtureB->GetBody(); - const b2Transform& xfA = bodyA->GetTransform(); - const b2Transform& xfB = bodyB->GetTransform(); - - // Is this contact a sensor? - if (sensor) - { - const b2Shape* shapeA = m_fixtureA->GetShape(); - const b2Shape* shapeB = m_fixtureB->GetShape(); - touching = b2TestOverlap(shapeA, m_indexA, shapeB, m_indexB, xfA, xfB); - - // Sensors don't generate manifolds. - m_manifold.pointCount = 0; - } - else - { - Evaluate(&m_manifold, xfA, xfB); - touching = m_manifold.pointCount > 0; - - // Match old contact ids to new contact ids and copy the - // stored impulses to warm start the solver. - for (int32 i = 0; i < m_manifold.pointCount; ++i) - { - b2ManifoldPoint* mp2 = m_manifold.points + i; - mp2->normalImpulse = 0.0f; - mp2->tangentImpulse = 0.0f; - b2ContactID id2 = mp2->id; - - for (int32 j = 0; j < oldManifold.pointCount; ++j) - { - b2ManifoldPoint* mp1 = oldManifold.points + j; - - if (mp1->id.key == id2.key) - { - mp2->normalImpulse = mp1->normalImpulse; - mp2->tangentImpulse = mp1->tangentImpulse; - break; - } - } - } - - if (touching != wasTouching) - { - bodyA->SetAwake(true); - bodyB->SetAwake(true); - } - } - - if (touching) - { - m_flags |= e_touchingFlag; - } - else - { - m_flags &= ~e_touchingFlag; - } - - if (wasTouching == false && touching == true && listener) - { - listener->BeginContact(this); - } - - if (wasTouching == true && touching == false && listener) - { - listener->EndContact(this); - } - - if (sensor == false && touching && listener) - { - listener->PreSolve(this, &oldManifold); - } -} diff --git a/3rdparty/box2d/src/dynamics/b2_contact_manager.cpp b/3rdparty/box2d/src/dynamics/b2_contact_manager.cpp deleted file mode 100644 index 56666084cdd8..000000000000 --- a/3rdparty/box2d/src/dynamics/b2_contact_manager.cpp +++ /dev/null @@ -1,293 +0,0 @@ -// MIT License - -// Copyright (c) 2019 Erin Catto - -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: - -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. - -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -#include "box2d/b2_body.h" -#include "box2d/b2_contact.h" -#include "box2d/b2_contact_manager.h" -#include "box2d/b2_fixture.h" -#include "box2d/b2_world_callbacks.h" - -b2ContactFilter b2_defaultFilter; -b2ContactListener b2_defaultListener; - -b2ContactManager::b2ContactManager() -{ - m_contactList = nullptr; - m_contactCount = 0; - m_contactFilter = &b2_defaultFilter; - m_contactListener = &b2_defaultListener; - m_allocator = nullptr; -} - -void b2ContactManager::Destroy(b2Contact* c) -{ - b2Fixture* fixtureA = c->GetFixtureA(); - b2Fixture* fixtureB = c->GetFixtureB(); - b2Body* bodyA = fixtureA->GetBody(); - b2Body* bodyB = fixtureB->GetBody(); - - if (m_contactListener && c->IsTouching()) - { - m_contactListener->EndContact(c); - } - - // Remove from the world. - if (c->m_prev) - { - c->m_prev->m_next = c->m_next; - } - - if (c->m_next) - { - c->m_next->m_prev = c->m_prev; - } - - if (c == m_contactList) - { - m_contactList = c->m_next; - } - - // Remove from body 1 - if (c->m_nodeA.prev) - { - c->m_nodeA.prev->next = c->m_nodeA.next; - } - - if (c->m_nodeA.next) - { - c->m_nodeA.next->prev = c->m_nodeA.prev; - } - - if (&c->m_nodeA == bodyA->m_contactList) - { - bodyA->m_contactList = c->m_nodeA.next; - } - - // Remove from body 2 - if (c->m_nodeB.prev) - { - c->m_nodeB.prev->next = c->m_nodeB.next; - } - - if (c->m_nodeB.next) - { - c->m_nodeB.next->prev = c->m_nodeB.prev; - } - - if (&c->m_nodeB == bodyB->m_contactList) - { - bodyB->m_contactList = c->m_nodeB.next; - } - - // Call the factory. - b2Contact::Destroy(c, m_allocator); - --m_contactCount; -} - -// This is the top level collision call for the time step. Here -// all the narrow phase collision is processed for the world -// contact list. -void b2ContactManager::Collide() -{ - // Update awake contacts. - b2Contact* c = m_contactList; - while (c) - { - b2Fixture* fixtureA = c->GetFixtureA(); - b2Fixture* fixtureB = c->GetFixtureB(); - int32 indexA = c->GetChildIndexA(); - int32 indexB = c->GetChildIndexB(); - b2Body* bodyA = fixtureA->GetBody(); - b2Body* bodyB = fixtureB->GetBody(); - - // Is this contact flagged for filtering? - if (c->m_flags & b2Contact::e_filterFlag) - { - // Should these bodies collide? - if (bodyB->ShouldCollide(bodyA) == false) - { - b2Contact* cNuke = c; - c = cNuke->GetNext(); - Destroy(cNuke); - continue; - } - - // Check user filtering. - if (m_contactFilter && m_contactFilter->ShouldCollide(fixtureA, fixtureB) == false) - { - b2Contact* cNuke = c; - c = cNuke->GetNext(); - Destroy(cNuke); - continue; - } - - // Clear the filtering flag. - c->m_flags &= ~b2Contact::e_filterFlag; - } - - bool activeA = bodyA->IsAwake() && bodyA->m_type != b2_staticBody; - bool activeB = bodyB->IsAwake() && bodyB->m_type != b2_staticBody; - - // At least one body must be awake and it must be dynamic or kinematic. - if (activeA == false && activeB == false) - { - c = c->GetNext(); - continue; - } - - int32 proxyIdA = fixtureA->m_proxies[indexA].proxyId; - int32 proxyIdB = fixtureB->m_proxies[indexB].proxyId; - bool overlap = m_broadPhase.TestOverlap(proxyIdA, proxyIdB); - - // Here we destroy contacts that cease to overlap in the broad-phase. - if (overlap == false) - { - b2Contact* cNuke = c; - c = cNuke->GetNext(); - Destroy(cNuke); - continue; - } - - // The contact persists. - c->Update(m_contactListener); - c = c->GetNext(); - } -} - -void b2ContactManager::FindNewContacts() -{ - m_broadPhase.UpdatePairs(this); -} - -void b2ContactManager::AddPair(void* proxyUserDataA, void* proxyUserDataB) -{ - b2FixtureProxy* proxyA = (b2FixtureProxy*)proxyUserDataA; - b2FixtureProxy* proxyB = (b2FixtureProxy*)proxyUserDataB; - - b2Fixture* fixtureA = proxyA->fixture; - b2Fixture* fixtureB = proxyB->fixture; - - int32 indexA = proxyA->childIndex; - int32 indexB = proxyB->childIndex; - - b2Body* bodyA = fixtureA->GetBody(); - b2Body* bodyB = fixtureB->GetBody(); - - // Are the fixtures on the same body? - if (bodyA == bodyB) - { - return; - } - - // TODO_ERIN use a hash table to remove a potential bottleneck when both - // bodies have a lot of contacts. - // Does a contact already exist? - b2ContactEdge* edge = bodyB->GetContactList(); - while (edge) - { - if (edge->other == bodyA) - { - b2Fixture* fA = edge->contact->GetFixtureA(); - b2Fixture* fB = edge->contact->GetFixtureB(); - int32 iA = edge->contact->GetChildIndexA(); - int32 iB = edge->contact->GetChildIndexB(); - - if (fA == fixtureA && fB == fixtureB && iA == indexA && iB == indexB) - { - // A contact already exists. - return; - } - - if (fA == fixtureB && fB == fixtureA && iA == indexB && iB == indexA) - { - // A contact already exists. - return; - } - } - - edge = edge->next; - } - - // Does a joint override collision? Is at least one body dynamic? - if (bodyB->ShouldCollide(bodyA) == false) - { - return; - } - - // Check user filtering. - if (m_contactFilter && m_contactFilter->ShouldCollide(fixtureA, fixtureB) == false) - { - return; - } - - // Call the factory. - b2Contact* c = b2Contact::Create(fixtureA, indexA, fixtureB, indexB, m_allocator); - if (c == nullptr) - { - return; - } - - // Contact creation may swap fixtures. - fixtureA = c->GetFixtureA(); - fixtureB = c->GetFixtureB(); - indexA = c->GetChildIndexA(); - indexB = c->GetChildIndexB(); - bodyA = fixtureA->GetBody(); - bodyB = fixtureB->GetBody(); - - // Insert into the world. - c->m_prev = nullptr; - c->m_next = m_contactList; - if (m_contactList != nullptr) - { - m_contactList->m_prev = c; - } - m_contactList = c; - - // Connect to island graph. - - // Connect to body A - c->m_nodeA.contact = c; - c->m_nodeA.other = bodyB; - - c->m_nodeA.prev = nullptr; - c->m_nodeA.next = bodyA->m_contactList; - if (bodyA->m_contactList != nullptr) - { - bodyA->m_contactList->prev = &c->m_nodeA; - } - bodyA->m_contactList = &c->m_nodeA; - - // Connect to body B - c->m_nodeB.contact = c; - c->m_nodeB.other = bodyA; - - c->m_nodeB.prev = nullptr; - c->m_nodeB.next = bodyB->m_contactList; - if (bodyB->m_contactList != nullptr) - { - bodyB->m_contactList->prev = &c->m_nodeB; - } - bodyB->m_contactList = &c->m_nodeB; - - ++m_contactCount; -} diff --git a/3rdparty/box2d/src/dynamics/b2_contact_solver.cpp b/3rdparty/box2d/src/dynamics/b2_contact_solver.cpp deleted file mode 100644 index d6c08fb17972..000000000000 --- a/3rdparty/box2d/src/dynamics/b2_contact_solver.cpp +++ /dev/null @@ -1,843 +0,0 @@ -// MIT License - -// Copyright (c) 2019 Erin Catto - -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: - -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. - -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -#include "b2_contact_solver.h" - -#include "box2d/b2_body.h" -#include "box2d/b2_contact.h" -#include "box2d/b2_fixture.h" -#include "box2d/b2_stack_allocator.h" -#include "box2d/b2_world.h" - -// Solver debugging is normally disabled because the block solver sometimes has to deal with a poorly conditioned effective mass matrix. -#define B2_DEBUG_SOLVER 0 - -B2_API bool g_blockSolve = true; - -struct b2ContactPositionConstraint -{ - b2Vec2 localPoints[b2_maxManifoldPoints]; - b2Vec2 localNormal; - b2Vec2 localPoint; - int32 indexA; - int32 indexB; - float invMassA, invMassB; - b2Vec2 localCenterA, localCenterB; - float invIA, invIB; - b2Manifold::Type type; - float radiusA, radiusB; - int32 pointCount; -}; - -b2ContactSolver::b2ContactSolver(b2ContactSolverDef* def) -{ - m_step = def->step; - m_allocator = def->allocator; - m_count = def->count; - m_positionConstraints = (b2ContactPositionConstraint*)m_allocator->Allocate(m_count * sizeof(b2ContactPositionConstraint)); - m_velocityConstraints = (b2ContactVelocityConstraint*)m_allocator->Allocate(m_count * sizeof(b2ContactVelocityConstraint)); - m_positions = def->positions; - m_velocities = def->velocities; - m_contacts = def->contacts; - - // Initialize position independent portions of the constraints. - for (int32 i = 0; i < m_count; ++i) - { - b2Contact* contact = m_contacts[i]; - - b2Fixture* fixtureA = contact->m_fixtureA; - b2Fixture* fixtureB = contact->m_fixtureB; - b2Shape* shapeA = fixtureA->GetShape(); - b2Shape* shapeB = fixtureB->GetShape(); - float radiusA = shapeA->m_radius; - float radiusB = shapeB->m_radius; - b2Body* bodyA = fixtureA->GetBody(); - b2Body* bodyB = fixtureB->GetBody(); - b2Manifold* manifold = contact->GetManifold(); - - int32 pointCount = manifold->pointCount; - b2Assert(pointCount > 0); - - b2ContactVelocityConstraint* vc = m_velocityConstraints + i; - vc->friction = contact->m_friction; - vc->restitution = contact->m_restitution; - vc->threshold = contact->m_restitutionThreshold; - vc->tangentSpeed = contact->m_tangentSpeed; - vc->indexA = bodyA->m_islandIndex; - vc->indexB = bodyB->m_islandIndex; - vc->invMassA = bodyA->m_invMass; - vc->invMassB = bodyB->m_invMass; - vc->invIA = bodyA->m_invI; - vc->invIB = bodyB->m_invI; - vc->contactIndex = i; - vc->pointCount = pointCount; - vc->K.SetZero(); - vc->normalMass.SetZero(); - - b2ContactPositionConstraint* pc = m_positionConstraints + i; - pc->indexA = bodyA->m_islandIndex; - pc->indexB = bodyB->m_islandIndex; - pc->invMassA = bodyA->m_invMass; - pc->invMassB = bodyB->m_invMass; - pc->localCenterA = bodyA->m_sweep.localCenter; - pc->localCenterB = bodyB->m_sweep.localCenter; - pc->invIA = bodyA->m_invI; - pc->invIB = bodyB->m_invI; - pc->localNormal = manifold->localNormal; - pc->localPoint = manifold->localPoint; - pc->pointCount = pointCount; - pc->radiusA = radiusA; - pc->radiusB = radiusB; - pc->type = manifold->type; - - for (int32 j = 0; j < pointCount; ++j) - { - b2ManifoldPoint* cp = manifold->points + j; - b2VelocityConstraintPoint* vcp = vc->points + j; - - if (m_step.warmStarting) - { - vcp->normalImpulse = m_step.dtRatio * cp->normalImpulse; - vcp->tangentImpulse = m_step.dtRatio * cp->tangentImpulse; - } - else - { - vcp->normalImpulse = 0.0f; - vcp->tangentImpulse = 0.0f; - } - - vcp->rA.SetZero(); - vcp->rB.SetZero(); - vcp->normalMass = 0.0f; - vcp->tangentMass = 0.0f; - vcp->velocityBias = 0.0f; - - pc->localPoints[j] = cp->localPoint; - } - } -} - -b2ContactSolver::~b2ContactSolver() -{ - m_allocator->Free(m_velocityConstraints); - m_allocator->Free(m_positionConstraints); -} - -// Initialize position dependent portions of the velocity constraints. -void b2ContactSolver::InitializeVelocityConstraints() -{ - for (int32 i = 0; i < m_count; ++i) - { - b2ContactVelocityConstraint* vc = m_velocityConstraints + i; - b2ContactPositionConstraint* pc = m_positionConstraints + i; - - float radiusA = pc->radiusA; - float radiusB = pc->radiusB; - b2Manifold* manifold = m_contacts[vc->contactIndex]->GetManifold(); - - int32 indexA = vc->indexA; - int32 indexB = vc->indexB; - - float mA = vc->invMassA; - float mB = vc->invMassB; - float iA = vc->invIA; - float iB = vc->invIB; - b2Vec2 localCenterA = pc->localCenterA; - b2Vec2 localCenterB = pc->localCenterB; - - b2Vec2 cA = m_positions[indexA].c; - float aA = m_positions[indexA].a; - b2Vec2 vA = m_velocities[indexA].v; - float wA = m_velocities[indexA].w; - - b2Vec2 cB = m_positions[indexB].c; - float aB = m_positions[indexB].a; - b2Vec2 vB = m_velocities[indexB].v; - float wB = m_velocities[indexB].w; - - b2Assert(manifold->pointCount > 0); - - b2Transform xfA, xfB; - xfA.q.Set(aA); - xfB.q.Set(aB); - xfA.p = cA - b2Mul(xfA.q, localCenterA); - xfB.p = cB - b2Mul(xfB.q, localCenterB); - - b2WorldManifold worldManifold; - worldManifold.Initialize(manifold, xfA, radiusA, xfB, radiusB); - - vc->normal = worldManifold.normal; - - int32 pointCount = vc->pointCount; - for (int32 j = 0; j < pointCount; ++j) - { - b2VelocityConstraintPoint* vcp = vc->points + j; - - vcp->rA = worldManifold.points[j] - cA; - vcp->rB = worldManifold.points[j] - cB; - - float rnA = b2Cross(vcp->rA, vc->normal); - float rnB = b2Cross(vcp->rB, vc->normal); - - float kNormal = mA + mB + iA * rnA * rnA + iB * rnB * rnB; - - vcp->normalMass = kNormal > 0.0f ? 1.0f / kNormal : 0.0f; - - b2Vec2 tangent = b2Cross(vc->normal, 1.0f); - - float rtA = b2Cross(vcp->rA, tangent); - float rtB = b2Cross(vcp->rB, tangent); - - float kTangent = mA + mB + iA * rtA * rtA + iB * rtB * rtB; - - vcp->tangentMass = kTangent > 0.0f ? 1.0f / kTangent : 0.0f; - - // Setup a velocity bias for restitution. - vcp->velocityBias = 0.0f; - float vRel = b2Dot(vc->normal, vB + b2Cross(wB, vcp->rB) - vA - b2Cross(wA, vcp->rA)); - if (vRel < -vc->threshold) - { - vcp->velocityBias = -vc->restitution * vRel; - } - } - - // If we have two points, then prepare the block solver. - if (vc->pointCount == 2 && g_blockSolve) - { - b2VelocityConstraintPoint* vcp1 = vc->points + 0; - b2VelocityConstraintPoint* vcp2 = vc->points + 1; - - float rn1A = b2Cross(vcp1->rA, vc->normal); - float rn1B = b2Cross(vcp1->rB, vc->normal); - float rn2A = b2Cross(vcp2->rA, vc->normal); - float rn2B = b2Cross(vcp2->rB, vc->normal); - - float k11 = mA + mB + iA * rn1A * rn1A + iB * rn1B * rn1B; - float k22 = mA + mB + iA * rn2A * rn2A + iB * rn2B * rn2B; - float k12 = mA + mB + iA * rn1A * rn2A + iB * rn1B * rn2B; - - // Ensure a reasonable condition number. - const float k_maxConditionNumber = 1000.0f; - if (k11 * k11 < k_maxConditionNumber * (k11 * k22 - k12 * k12)) - { - // K is safe to invert. - vc->K.ex.Set(k11, k12); - vc->K.ey.Set(k12, k22); - vc->normalMass = vc->K.GetInverse(); - } - else - { - // The constraints are redundant, just use one. - // TODO_ERIN use deepest? - vc->pointCount = 1; - } - } - } -} - -void b2ContactSolver::WarmStart() -{ - // Warm start. - for (int32 i = 0; i < m_count; ++i) - { - b2ContactVelocityConstraint* vc = m_velocityConstraints + i; - - int32 indexA = vc->indexA; - int32 indexB = vc->indexB; - float mA = vc->invMassA; - float iA = vc->invIA; - float mB = vc->invMassB; - float iB = vc->invIB; - int32 pointCount = vc->pointCount; - - b2Vec2 vA = m_velocities[indexA].v; - float wA = m_velocities[indexA].w; - b2Vec2 vB = m_velocities[indexB].v; - float wB = m_velocities[indexB].w; - - b2Vec2 normal = vc->normal; - b2Vec2 tangent = b2Cross(normal, 1.0f); - - for (int32 j = 0; j < pointCount; ++j) - { - b2VelocityConstraintPoint* vcp = vc->points + j; - b2Vec2 P = vcp->normalImpulse * normal + vcp->tangentImpulse * tangent; - wA -= iA * b2Cross(vcp->rA, P); - vA -= mA * P; - wB += iB * b2Cross(vcp->rB, P); - vB += mB * P; - } - - m_velocities[indexA].v = vA; - m_velocities[indexA].w = wA; - m_velocities[indexB].v = vB; - m_velocities[indexB].w = wB; - } -} - -void b2ContactSolver::SolveVelocityConstraints() -{ - for (int32 i = 0; i < m_count; ++i) - { - b2ContactVelocityConstraint* vc = m_velocityConstraints + i; - - int32 indexA = vc->indexA; - int32 indexB = vc->indexB; - float mA = vc->invMassA; - float iA = vc->invIA; - float mB = vc->invMassB; - float iB = vc->invIB; - int32 pointCount = vc->pointCount; - - b2Vec2 vA = m_velocities[indexA].v; - float wA = m_velocities[indexA].w; - b2Vec2 vB = m_velocities[indexB].v; - float wB = m_velocities[indexB].w; - - b2Vec2 normal = vc->normal; - b2Vec2 tangent = b2Cross(normal, 1.0f); - float friction = vc->friction; - - b2Assert(pointCount == 1 || pointCount == 2); - - // Solve tangent constraints first because non-penetration is more important - // than friction. - for (int32 j = 0; j < pointCount; ++j) - { - b2VelocityConstraintPoint* vcp = vc->points + j; - - // Relative velocity at contact - b2Vec2 dv = vB + b2Cross(wB, vcp->rB) - vA - b2Cross(wA, vcp->rA); - - // Compute tangent force - float vt = b2Dot(dv, tangent) - vc->tangentSpeed; - float lambda = vcp->tangentMass * (-vt); - - // b2Clamp the accumulated force - float maxFriction = friction * vcp->normalImpulse; - float newImpulse = b2Clamp(vcp->tangentImpulse + lambda, -maxFriction, maxFriction); - lambda = newImpulse - vcp->tangentImpulse; - vcp->tangentImpulse = newImpulse; - - // Apply contact impulse - b2Vec2 P = lambda * tangent; - - vA -= mA * P; - wA -= iA * b2Cross(vcp->rA, P); - - vB += mB * P; - wB += iB * b2Cross(vcp->rB, P); - } - - // Solve normal constraints - if (pointCount == 1 || g_blockSolve == false) - { - for (int32 j = 0; j < pointCount; ++j) - { - b2VelocityConstraintPoint* vcp = vc->points + j; - - // Relative velocity at contact - b2Vec2 dv = vB + b2Cross(wB, vcp->rB) - vA - b2Cross(wA, vcp->rA); - - // Compute normal impulse - float vn = b2Dot(dv, normal); - float lambda = -vcp->normalMass * (vn - vcp->velocityBias); - - // b2Clamp the accumulated impulse - float newImpulse = b2Max(vcp->normalImpulse + lambda, 0.0f); - lambda = newImpulse - vcp->normalImpulse; - vcp->normalImpulse = newImpulse; - - // Apply contact impulse - b2Vec2 P = lambda * normal; - vA -= mA * P; - wA -= iA * b2Cross(vcp->rA, P); - - vB += mB * P; - wB += iB * b2Cross(vcp->rB, P); - } - } - else - { - // Block solver developed in collaboration with Dirk Gregorius (back in 01/07 on Box2D_Lite). - // Build the mini LCP for this contact patch - // - // vn = A * x + b, vn >= 0, x >= 0 and vn_i * x_i = 0 with i = 1..2 - // - // A = J * W * JT and J = ( -n, -r1 x n, n, r2 x n ) - // b = vn0 - velocityBias - // - // The system is solved using the "Total enumeration method" (s. Murty). The complementary constraint vn_i * x_i - // implies that we must have in any solution either vn_i = 0 or x_i = 0. So for the 2D contact problem the cases - // vn1 = 0 and vn2 = 0, x1 = 0 and x2 = 0, x1 = 0 and vn2 = 0, x2 = 0 and vn1 = 0 need to be tested. The first valid - // solution that satisfies the problem is chosen. - // - // In order to account of the accumulated impulse 'a' (because of the iterative nature of the solver which only requires - // that the accumulated impulse is clamped and not the incremental impulse) we change the impulse variable (x_i). - // - // Substitute: - // - // x = a + d - // - // a := old total impulse - // x := new total impulse - // d := incremental impulse - // - // For the current iteration we extend the formula for the incremental impulse - // to compute the new total impulse: - // - // vn = A * d + b - // = A * (x - a) + b - // = A * x + b - A * a - // = A * x + b' - // b' = b - A * a; - - b2VelocityConstraintPoint* cp1 = vc->points + 0; - b2VelocityConstraintPoint* cp2 = vc->points + 1; - - b2Vec2 a(cp1->normalImpulse, cp2->normalImpulse); - b2Assert(a.x >= 0.0f && a.y >= 0.0f); - - // Relative velocity at contact - b2Vec2 dv1 = vB + b2Cross(wB, cp1->rB) - vA - b2Cross(wA, cp1->rA); - b2Vec2 dv2 = vB + b2Cross(wB, cp2->rB) - vA - b2Cross(wA, cp2->rA); - - // Compute normal velocity - float vn1 = b2Dot(dv1, normal); - float vn2 = b2Dot(dv2, normal); - - b2Vec2 b; - b.x = vn1 - cp1->velocityBias; - b.y = vn2 - cp2->velocityBias; - - // Compute b' - b -= b2Mul(vc->K, a); - - const float k_errorTol = 1e-3f; - B2_NOT_USED(k_errorTol); - - for (;;) - { - // - // Case 1: vn = 0 - // - // 0 = A * x + b' - // - // Solve for x: - // - // x = - inv(A) * b' - // - b2Vec2 x = - b2Mul(vc->normalMass, b); - - if (x.x >= 0.0f && x.y >= 0.0f) - { - // Get the incremental impulse - b2Vec2 d = x - a; - - // Apply incremental impulse - b2Vec2 P1 = d.x * normal; - b2Vec2 P2 = d.y * normal; - vA -= mA * (P1 + P2); - wA -= iA * (b2Cross(cp1->rA, P1) + b2Cross(cp2->rA, P2)); - - vB += mB * (P1 + P2); - wB += iB * (b2Cross(cp1->rB, P1) + b2Cross(cp2->rB, P2)); - - // Accumulate - cp1->normalImpulse = x.x; - cp2->normalImpulse = x.y; - -#if B2_DEBUG_SOLVER == 1 - // Postconditions - dv1 = vB + b2Cross(wB, cp1->rB) - vA - b2Cross(wA, cp1->rA); - dv2 = vB + b2Cross(wB, cp2->rB) - vA - b2Cross(wA, cp2->rA); - - // Compute normal velocity - vn1 = b2Dot(dv1, normal); - vn2 = b2Dot(dv2, normal); - - b2Assert(b2Abs(vn1 - cp1->velocityBias) < k_errorTol); - b2Assert(b2Abs(vn2 - cp2->velocityBias) < k_errorTol); -#endif - break; - } - - // - // Case 2: vn1 = 0 and x2 = 0 - // - // 0 = a11 * x1 + a12 * 0 + b1' - // vn2 = a21 * x1 + a22 * 0 + b2' - // - x.x = - cp1->normalMass * b.x; - x.y = 0.0f; - vn1 = 0.0f; - vn2 = vc->K.ex.y * x.x + b.y; - if (x.x >= 0.0f && vn2 >= 0.0f) - { - // Get the incremental impulse - b2Vec2 d = x - a; - - // Apply incremental impulse - b2Vec2 P1 = d.x * normal; - b2Vec2 P2 = d.y * normal; - vA -= mA * (P1 + P2); - wA -= iA * (b2Cross(cp1->rA, P1) + b2Cross(cp2->rA, P2)); - - vB += mB * (P1 + P2); - wB += iB * (b2Cross(cp1->rB, P1) + b2Cross(cp2->rB, P2)); - - // Accumulate - cp1->normalImpulse = x.x; - cp2->normalImpulse = x.y; - -#if B2_DEBUG_SOLVER == 1 - // Postconditions - dv1 = vB + b2Cross(wB, cp1->rB) - vA - b2Cross(wA, cp1->rA); - - // Compute normal velocity - vn1 = b2Dot(dv1, normal); - - b2Assert(b2Abs(vn1 - cp1->velocityBias) < k_errorTol); -#endif - break; - } - - - // - // Case 3: vn2 = 0 and x1 = 0 - // - // vn1 = a11 * 0 + a12 * x2 + b1' - // 0 = a21 * 0 + a22 * x2 + b2' - // - x.x = 0.0f; - x.y = - cp2->normalMass * b.y; - vn1 = vc->K.ey.x * x.y + b.x; - vn2 = 0.0f; - - if (x.y >= 0.0f && vn1 >= 0.0f) - { - // Resubstitute for the incremental impulse - b2Vec2 d = x - a; - - // Apply incremental impulse - b2Vec2 P1 = d.x * normal; - b2Vec2 P2 = d.y * normal; - vA -= mA * (P1 + P2); - wA -= iA * (b2Cross(cp1->rA, P1) + b2Cross(cp2->rA, P2)); - - vB += mB * (P1 + P2); - wB += iB * (b2Cross(cp1->rB, P1) + b2Cross(cp2->rB, P2)); - - // Accumulate - cp1->normalImpulse = x.x; - cp2->normalImpulse = x.y; - -#if B2_DEBUG_SOLVER == 1 - // Postconditions - dv2 = vB + b2Cross(wB, cp2->rB) - vA - b2Cross(wA, cp2->rA); - - // Compute normal velocity - vn2 = b2Dot(dv2, normal); - - b2Assert(b2Abs(vn2 - cp2->velocityBias) < k_errorTol); -#endif - break; - } - - // - // Case 4: x1 = 0 and x2 = 0 - // - // vn1 = b1 - // vn2 = b2; - x.x = 0.0f; - x.y = 0.0f; - vn1 = b.x; - vn2 = b.y; - - if (vn1 >= 0.0f && vn2 >= 0.0f ) - { - // Resubstitute for the incremental impulse - b2Vec2 d = x - a; - - // Apply incremental impulse - b2Vec2 P1 = d.x * normal; - b2Vec2 P2 = d.y * normal; - vA -= mA * (P1 + P2); - wA -= iA * (b2Cross(cp1->rA, P1) + b2Cross(cp2->rA, P2)); - - vB += mB * (P1 + P2); - wB += iB * (b2Cross(cp1->rB, P1) + b2Cross(cp2->rB, P2)); - - // Accumulate - cp1->normalImpulse = x.x; - cp2->normalImpulse = x.y; - - break; - } - - // No solution, give up. This is hit sometimes, but it doesn't seem to matter. - break; - } - } - - m_velocities[indexA].v = vA; - m_velocities[indexA].w = wA; - m_velocities[indexB].v = vB; - m_velocities[indexB].w = wB; - } -} - -void b2ContactSolver::StoreImpulses() -{ - for (int32 i = 0; i < m_count; ++i) - { - b2ContactVelocityConstraint* vc = m_velocityConstraints + i; - b2Manifold* manifold = m_contacts[vc->contactIndex]->GetManifold(); - - for (int32 j = 0; j < vc->pointCount; ++j) - { - manifold->points[j].normalImpulse = vc->points[j].normalImpulse; - manifold->points[j].tangentImpulse = vc->points[j].tangentImpulse; - } - } -} - -struct b2PositionSolverManifold -{ - void Initialize(b2ContactPositionConstraint* pc, const b2Transform& xfA, const b2Transform& xfB, int32 index) - { - b2Assert(pc->pointCount > 0); - - switch (pc->type) - { - case b2Manifold::e_circles: - { - b2Vec2 pointA = b2Mul(xfA, pc->localPoint); - b2Vec2 pointB = b2Mul(xfB, pc->localPoints[0]); - normal = pointB - pointA; - normal.Normalize(); - point = 0.5f * (pointA + pointB); - separation = b2Dot(pointB - pointA, normal) - pc->radiusA - pc->radiusB; - } - break; - - case b2Manifold::e_faceA: - { - normal = b2Mul(xfA.q, pc->localNormal); - b2Vec2 planePoint = b2Mul(xfA, pc->localPoint); - - b2Vec2 clipPoint = b2Mul(xfB, pc->localPoints[index]); - separation = b2Dot(clipPoint - planePoint, normal) - pc->radiusA - pc->radiusB; - point = clipPoint; - } - break; - - case b2Manifold::e_faceB: - { - normal = b2Mul(xfB.q, pc->localNormal); - b2Vec2 planePoint = b2Mul(xfB, pc->localPoint); - - b2Vec2 clipPoint = b2Mul(xfA, pc->localPoints[index]); - separation = b2Dot(clipPoint - planePoint, normal) - pc->radiusA - pc->radiusB; - point = clipPoint; - - // Ensure normal points from A to B - normal = -normal; - } - break; - } - } - - b2Vec2 normal; - b2Vec2 point; - float separation; -}; - -// Sequential solver. -bool b2ContactSolver::SolvePositionConstraints() -{ - float minSeparation = 0.0f; - - for (int32 i = 0; i < m_count; ++i) - { - b2ContactPositionConstraint* pc = m_positionConstraints + i; - - int32 indexA = pc->indexA; - int32 indexB = pc->indexB; - b2Vec2 localCenterA = pc->localCenterA; - float mA = pc->invMassA; - float iA = pc->invIA; - b2Vec2 localCenterB = pc->localCenterB; - float mB = pc->invMassB; - float iB = pc->invIB; - int32 pointCount = pc->pointCount; - - b2Vec2 cA = m_positions[indexA].c; - float aA = m_positions[indexA].a; - - b2Vec2 cB = m_positions[indexB].c; - float aB = m_positions[indexB].a; - - // Solve normal constraints - for (int32 j = 0; j < pointCount; ++j) - { - b2Transform xfA, xfB; - xfA.q.Set(aA); - xfB.q.Set(aB); - xfA.p = cA - b2Mul(xfA.q, localCenterA); - xfB.p = cB - b2Mul(xfB.q, localCenterB); - - b2PositionSolverManifold psm; - psm.Initialize(pc, xfA, xfB, j); - b2Vec2 normal = psm.normal; - - b2Vec2 point = psm.point; - float separation = psm.separation; - - b2Vec2 rA = point - cA; - b2Vec2 rB = point - cB; - - // Track max constraint error. - minSeparation = b2Min(minSeparation, separation); - - // Prevent large corrections and allow slop. - float C = b2Clamp(b2_baumgarte * (separation + b2_linearSlop), -b2_maxLinearCorrection, 0.0f); - - // Compute the effective mass. - float rnA = b2Cross(rA, normal); - float rnB = b2Cross(rB, normal); - float K = mA + mB + iA * rnA * rnA + iB * rnB * rnB; - - // Compute normal impulse - float impulse = K > 0.0f ? - C / K : 0.0f; - - b2Vec2 P = impulse * normal; - - cA -= mA * P; - aA -= iA * b2Cross(rA, P); - - cB += mB * P; - aB += iB * b2Cross(rB, P); - } - - m_positions[indexA].c = cA; - m_positions[indexA].a = aA; - - m_positions[indexB].c = cB; - m_positions[indexB].a = aB; - } - - // We can't expect minSpeparation >= -b2_linearSlop because we don't - // push the separation above -b2_linearSlop. - return minSeparation >= -3.0f * b2_linearSlop; -} - -// Sequential position solver for position constraints. -bool b2ContactSolver::SolveTOIPositionConstraints(int32 toiIndexA, int32 toiIndexB) -{ - float minSeparation = 0.0f; - - for (int32 i = 0; i < m_count; ++i) - { - b2ContactPositionConstraint* pc = m_positionConstraints + i; - - int32 indexA = pc->indexA; - int32 indexB = pc->indexB; - b2Vec2 localCenterA = pc->localCenterA; - b2Vec2 localCenterB = pc->localCenterB; - int32 pointCount = pc->pointCount; - - float mA = 0.0f; - float iA = 0.0f; - if (indexA == toiIndexA || indexA == toiIndexB) - { - mA = pc->invMassA; - iA = pc->invIA; - } - - float mB = 0.0f; - float iB = 0.0f; - if (indexB == toiIndexA || indexB == toiIndexB) - { - mB = pc->invMassB; - iB = pc->invIB; - } - - b2Vec2 cA = m_positions[indexA].c; - float aA = m_positions[indexA].a; - - b2Vec2 cB = m_positions[indexB].c; - float aB = m_positions[indexB].a; - - // Solve normal constraints - for (int32 j = 0; j < pointCount; ++j) - { - b2Transform xfA, xfB; - xfA.q.Set(aA); - xfB.q.Set(aB); - xfA.p = cA - b2Mul(xfA.q, localCenterA); - xfB.p = cB - b2Mul(xfB.q, localCenterB); - - b2PositionSolverManifold psm; - psm.Initialize(pc, xfA, xfB, j); - b2Vec2 normal = psm.normal; - - b2Vec2 point = psm.point; - float separation = psm.separation; - - b2Vec2 rA = point - cA; - b2Vec2 rB = point - cB; - - // Track max constraint error. - minSeparation = b2Min(minSeparation, separation); - - // Prevent large corrections and allow slop. - float C = b2Clamp(b2_toiBaumgarte * (separation + b2_linearSlop), -b2_maxLinearCorrection, 0.0f); - - // Compute the effective mass. - float rnA = b2Cross(rA, normal); - float rnB = b2Cross(rB, normal); - float K = mA + mB + iA * rnA * rnA + iB * rnB * rnB; - - // Compute normal impulse - float impulse = K > 0.0f ? - C / K : 0.0f; - - b2Vec2 P = impulse * normal; - - cA -= mA * P; - aA -= iA * b2Cross(rA, P); - - cB += mB * P; - aB += iB * b2Cross(rB, P); - } - - m_positions[indexA].c = cA; - m_positions[indexA].a = aA; - - m_positions[indexB].c = cB; - m_positions[indexB].a = aB; - } - - // We can't expect minSpeparation >= -b2_linearSlop because we don't - // push the separation above -b2_linearSlop. - return minSeparation >= -1.5f * b2_linearSlop; -} diff --git a/3rdparty/box2d/src/dynamics/b2_contact_solver.h b/3rdparty/box2d/src/dynamics/b2_contact_solver.h deleted file mode 100644 index 1064738ef793..000000000000 --- a/3rdparty/box2d/src/dynamics/b2_contact_solver.h +++ /dev/null @@ -1,100 +0,0 @@ -// MIT License - -// Copyright (c) 2019 Erin Catto - -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: - -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. - -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -#ifndef B2_CONTACT_SOLVER_H -#define B2_CONTACT_SOLVER_H - -#include "box2d/b2_collision.h" -#include "box2d/b2_math.h" -#include "box2d/b2_time_step.h" - -class b2Contact; -class b2Body; -class b2StackAllocator; -struct b2ContactPositionConstraint; - -struct b2VelocityConstraintPoint -{ - b2Vec2 rA; - b2Vec2 rB; - float normalImpulse; - float tangentImpulse; - float normalMass; - float tangentMass; - float velocityBias; -}; - -struct b2ContactVelocityConstraint -{ - b2VelocityConstraintPoint points[b2_maxManifoldPoints]; - b2Vec2 normal; - b2Mat22 normalMass; - b2Mat22 K; - int32 indexA; - int32 indexB; - float invMassA, invMassB; - float invIA, invIB; - float friction; - float restitution; - float threshold; - float tangentSpeed; - int32 pointCount; - int32 contactIndex; -}; - -struct b2ContactSolverDef -{ - b2TimeStep step; - b2Contact** contacts; - int32 count; - b2Position* positions; - b2Velocity* velocities; - b2StackAllocator* allocator; -}; - -class b2ContactSolver -{ -public: - b2ContactSolver(b2ContactSolverDef* def); - ~b2ContactSolver(); - - void InitializeVelocityConstraints(); - - void WarmStart(); - void SolveVelocityConstraints(); - void StoreImpulses(); - - bool SolvePositionConstraints(); - bool SolveTOIPositionConstraints(int32 toiIndexA, int32 toiIndexB); - - b2TimeStep m_step; - b2Position* m_positions; - b2Velocity* m_velocities; - b2StackAllocator* m_allocator; - b2ContactPositionConstraint* m_positionConstraints; - b2ContactVelocityConstraint* m_velocityConstraints; - b2Contact** m_contacts; - int m_count; -}; - -#endif - diff --git a/3rdparty/box2d/src/dynamics/b2_distance_joint.cpp b/3rdparty/box2d/src/dynamics/b2_distance_joint.cpp deleted file mode 100644 index 221309ef00ab..000000000000 --- a/3rdparty/box2d/src/dynamics/b2_distance_joint.cpp +++ /dev/null @@ -1,421 +0,0 @@ -// MIT License - -// Copyright (c) 2019 Erin Catto - -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: - -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. - -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -#include "box2d/b2_body.h" -#include "box2d/b2_draw.h" -#include "box2d/b2_distance_joint.h" -#include "box2d/b2_time_step.h" - -// 1-D constrained system -// m (v2 - v1) = lambda -// v2 + (beta/h) * x1 + gamma * lambda = 0, gamma has units of inverse mass. -// x2 = x1 + h * v2 - -// 1-D mass-damper-spring system -// m (v2 - v1) + h * d * v2 + h * k * - -// C = norm(p2 - p1) - L -// u = (p2 - p1) / norm(p2 - p1) -// Cdot = dot(u, v2 + cross(w2, r2) - v1 - cross(w1, r1)) -// J = [-u -cross(r1, u) u cross(r2, u)] -// K = J * invM * JT -// = invMass1 + invI1 * cross(r1, u)^2 + invMass2 + invI2 * cross(r2, u)^2 - - -void b2DistanceJointDef::Initialize(b2Body* b1, b2Body* b2, - const b2Vec2& anchor1, const b2Vec2& anchor2) -{ - bodyA = b1; - bodyB = b2; - localAnchorA = bodyA->GetLocalPoint(anchor1); - localAnchorB = bodyB->GetLocalPoint(anchor2); - b2Vec2 d = anchor2 - anchor1; - length = b2Max(d.Length(), b2_linearSlop); - minLength = length; - maxLength = length; -} - -b2DistanceJoint::b2DistanceJoint(const b2DistanceJointDef* def) -: b2Joint(def) -{ - m_localAnchorA = def->localAnchorA; - m_localAnchorB = def->localAnchorB; - m_length = b2Max(def->length, b2_linearSlop); - m_minLength = b2Max(def->minLength, b2_linearSlop); - m_maxLength = b2Max(def->maxLength, m_minLength); - m_stiffness = def->stiffness; - m_damping = def->damping; - - m_gamma = 0.0f; - m_bias = 0.0f; - m_impulse = 0.0f; - m_lowerImpulse = 0.0f; - m_upperImpulse = 0.0f; - m_currentLength = 0.0f; -} - -void b2DistanceJoint::InitVelocityConstraints(const b2SolverData& data) -{ - m_indexA = m_bodyA->m_islandIndex; - m_indexB = m_bodyB->m_islandIndex; - m_localCenterA = m_bodyA->m_sweep.localCenter; - m_localCenterB = m_bodyB->m_sweep.localCenter; - m_invMassA = m_bodyA->m_invMass; - m_invMassB = m_bodyB->m_invMass; - m_invIA = m_bodyA->m_invI; - m_invIB = m_bodyB->m_invI; - - b2Vec2 cA = data.positions[m_indexA].c; - float aA = data.positions[m_indexA].a; - b2Vec2 vA = data.velocities[m_indexA].v; - float wA = data.velocities[m_indexA].w; - - b2Vec2 cB = data.positions[m_indexB].c; - float aB = data.positions[m_indexB].a; - b2Vec2 vB = data.velocities[m_indexB].v; - float wB = data.velocities[m_indexB].w; - - b2Rot qA(aA), qB(aB); - - m_rA = b2Mul(qA, m_localAnchorA - m_localCenterA); - m_rB = b2Mul(qB, m_localAnchorB - m_localCenterB); - m_u = cB + m_rB - cA - m_rA; - - // Handle singularity. - m_currentLength = m_u.Length(); - if (m_currentLength > b2_linearSlop) - { - m_u *= 1.0f / m_currentLength; - } - else - { - m_u.Set(0.0f, 0.0f); - m_mass = 0.0f; - m_impulse = 0.0f; - m_lowerImpulse = 0.0f; - m_upperImpulse = 0.0f; - } - - float crAu = b2Cross(m_rA, m_u); - float crBu = b2Cross(m_rB, m_u); - float invMass = m_invMassA + m_invIA * crAu * crAu + m_invMassB + m_invIB * crBu * crBu; - m_mass = invMass != 0.0f ? 1.0f / invMass : 0.0f; - - if (m_stiffness > 0.0f && m_minLength < m_maxLength) - { - // soft - float C = m_currentLength - m_length; - - float d = m_damping; - float k = m_stiffness; - - // magic formulas - float h = data.step.dt; - - // gamma = 1 / (h * (d + h * k)) - // the extra factor of h in the denominator is since the lambda is an impulse, not a force - m_gamma = h * (d + h * k); - m_gamma = m_gamma != 0.0f ? 1.0f / m_gamma : 0.0f; - m_bias = C * h * k * m_gamma; - - invMass += m_gamma; - m_softMass = invMass != 0.0f ? 1.0f / invMass : 0.0f; - } - else - { - // rigid - m_gamma = 0.0f; - m_bias = 0.0f; - m_softMass = m_mass; - } - - if (data.step.warmStarting) - { - // Scale the impulse to support a variable time step. - m_impulse *= data.step.dtRatio; - m_lowerImpulse *= data.step.dtRatio; - m_upperImpulse *= data.step.dtRatio; - - b2Vec2 P = (m_impulse + m_lowerImpulse - m_upperImpulse) * m_u; - vA -= m_invMassA * P; - wA -= m_invIA * b2Cross(m_rA, P); - vB += m_invMassB * P; - wB += m_invIB * b2Cross(m_rB, P); - } - else - { - m_impulse = 0.0f; - } - - data.velocities[m_indexA].v = vA; - data.velocities[m_indexA].w = wA; - data.velocities[m_indexB].v = vB; - data.velocities[m_indexB].w = wB; -} - -void b2DistanceJoint::SolveVelocityConstraints(const b2SolverData& data) -{ - b2Vec2 vA = data.velocities[m_indexA].v; - float wA = data.velocities[m_indexA].w; - b2Vec2 vB = data.velocities[m_indexB].v; - float wB = data.velocities[m_indexB].w; - - if (m_minLength < m_maxLength) - { - if (m_stiffness > 0.0f) - { - // Cdot = dot(u, v + cross(w, r)) - b2Vec2 vpA = vA + b2Cross(wA, m_rA); - b2Vec2 vpB = vB + b2Cross(wB, m_rB); - float Cdot = b2Dot(m_u, vpB - vpA); - - float impulse = -m_softMass * (Cdot + m_bias + m_gamma * m_impulse); - m_impulse += impulse; - - b2Vec2 P = impulse * m_u; - vA -= m_invMassA * P; - wA -= m_invIA * b2Cross(m_rA, P); - vB += m_invMassB * P; - wB += m_invIB * b2Cross(m_rB, P); - } - - // lower - { - float C = m_currentLength - m_minLength; - float bias = b2Max(0.0f, C) * data.step.inv_dt; - - b2Vec2 vpA = vA + b2Cross(wA, m_rA); - b2Vec2 vpB = vB + b2Cross(wB, m_rB); - float Cdot = b2Dot(m_u, vpB - vpA); - - float impulse = -m_mass * (Cdot + bias); - float oldImpulse = m_lowerImpulse; - m_lowerImpulse = b2Max(0.0f, m_lowerImpulse + impulse); - impulse = m_lowerImpulse - oldImpulse; - b2Vec2 P = impulse * m_u; - - vA -= m_invMassA * P; - wA -= m_invIA * b2Cross(m_rA, P); - vB += m_invMassB * P; - wB += m_invIB * b2Cross(m_rB, P); - } - - // upper - { - float C = m_maxLength - m_currentLength; - float bias = b2Max(0.0f, C) * data.step.inv_dt; - - b2Vec2 vpA = vA + b2Cross(wA, m_rA); - b2Vec2 vpB = vB + b2Cross(wB, m_rB); - float Cdot = b2Dot(m_u, vpA - vpB); - - float impulse = -m_mass * (Cdot + bias); - float oldImpulse = m_upperImpulse; - m_upperImpulse = b2Max(0.0f, m_upperImpulse + impulse); - impulse = m_upperImpulse - oldImpulse; - b2Vec2 P = -impulse * m_u; - - vA -= m_invMassA * P; - wA -= m_invIA * b2Cross(m_rA, P); - vB += m_invMassB * P; - wB += m_invIB * b2Cross(m_rB, P); - } - } - else - { - // Equal limits - - // Cdot = dot(u, v + cross(w, r)) - b2Vec2 vpA = vA + b2Cross(wA, m_rA); - b2Vec2 vpB = vB + b2Cross(wB, m_rB); - float Cdot = b2Dot(m_u, vpB - vpA); - - float impulse = -m_mass * Cdot; - m_impulse += impulse; - - b2Vec2 P = impulse * m_u; - vA -= m_invMassA * P; - wA -= m_invIA * b2Cross(m_rA, P); - vB += m_invMassB * P; - wB += m_invIB * b2Cross(m_rB, P); - } - - data.velocities[m_indexA].v = vA; - data.velocities[m_indexA].w = wA; - data.velocities[m_indexB].v = vB; - data.velocities[m_indexB].w = wB; -} - -bool b2DistanceJoint::SolvePositionConstraints(const b2SolverData& data) -{ - b2Vec2 cA = data.positions[m_indexA].c; - float aA = data.positions[m_indexA].a; - b2Vec2 cB = data.positions[m_indexB].c; - float aB = data.positions[m_indexB].a; - - b2Rot qA(aA), qB(aB); - - b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_localCenterA); - b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_localCenterB); - b2Vec2 u = cB + rB - cA - rA; - - float length = u.Normalize(); - float C; - if (m_minLength == m_maxLength) - { - C = length - m_minLength; - } - else if (length < m_minLength) - { - C = length - m_minLength; - } - else if (m_maxLength < length) - { - C = length - m_maxLength; - } - else - { - return true; - } - - float impulse = -m_mass * C; - b2Vec2 P = impulse * u; - - cA -= m_invMassA * P; - aA -= m_invIA * b2Cross(rA, P); - cB += m_invMassB * P; - aB += m_invIB * b2Cross(rB, P); - - data.positions[m_indexA].c = cA; - data.positions[m_indexA].a = aA; - data.positions[m_indexB].c = cB; - data.positions[m_indexB].a = aB; - - return b2Abs(C) < b2_linearSlop; -} - -b2Vec2 b2DistanceJoint::GetAnchorA() const -{ - return m_bodyA->GetWorldPoint(m_localAnchorA); -} - -b2Vec2 b2DistanceJoint::GetAnchorB() const -{ - return m_bodyB->GetWorldPoint(m_localAnchorB); -} - -b2Vec2 b2DistanceJoint::GetReactionForce(float inv_dt) const -{ - b2Vec2 F = inv_dt * (m_impulse + m_lowerImpulse - m_upperImpulse) * m_u; - return F; -} - -float b2DistanceJoint::GetReactionTorque(float inv_dt) const -{ - B2_NOT_USED(inv_dt); - return 0.0f; -} - -float b2DistanceJoint::SetLength(float length) -{ - m_impulse = 0.0f; - m_length = b2Max(b2_linearSlop, length); - return m_length; -} - -float b2DistanceJoint::SetMinLength(float minLength) -{ - m_lowerImpulse = 0.0f; - m_minLength = b2Clamp(minLength, b2_linearSlop, m_maxLength); - return m_minLength; -} - -float b2DistanceJoint::SetMaxLength(float maxLength) -{ - m_upperImpulse = 0.0f; - m_maxLength = b2Max(maxLength, m_minLength); - return m_maxLength; -} - -float b2DistanceJoint::GetCurrentLength() const -{ - b2Vec2 pA = m_bodyA->GetWorldPoint(m_localAnchorA); - b2Vec2 pB = m_bodyB->GetWorldPoint(m_localAnchorB); - b2Vec2 d = pB - pA; - float length = d.Length(); - return length; -} - -void b2DistanceJoint::Dump() -{ - int32 indexA = m_bodyA->m_islandIndex; - int32 indexB = m_bodyB->m_islandIndex; - - b2Dump(" b2DistanceJointDef jd;\n"); - b2Dump(" jd.bodyA = bodies[%d];\n", indexA); - b2Dump(" jd.bodyB = bodies[%d];\n", indexB); - b2Dump(" jd.collideConnected = bool(%d);\n", m_collideConnected); - b2Dump(" jd.localAnchorA.Set(%.9g, %.9g);\n", m_localAnchorA.x, m_localAnchorA.y); - b2Dump(" jd.localAnchorB.Set(%.9g, %.9g);\n", m_localAnchorB.x, m_localAnchorB.y); - b2Dump(" jd.length = %.9g;\n", m_length); - b2Dump(" jd.minLength = %.9g;\n", m_minLength); - b2Dump(" jd.maxLength = %.9g;\n", m_maxLength); - b2Dump(" jd.stiffness = %.9g;\n", m_stiffness); - b2Dump(" jd.damping = %.9g;\n", m_damping); - b2Dump(" joints[%d] = m_world->CreateJoint(&jd);\n", m_index); -} - -void b2DistanceJoint::Draw(b2Draw* draw) const -{ - const b2Transform& xfA = m_bodyA->GetTransform(); - const b2Transform& xfB = m_bodyB->GetTransform(); - b2Vec2 pA = b2Mul(xfA, m_localAnchorA); - b2Vec2 pB = b2Mul(xfB, m_localAnchorB); - - b2Vec2 axis = pB - pA; - axis.Normalize(); - - b2Color c1(0.7f, 0.7f, 0.7f); - b2Color c2(0.3f, 0.9f, 0.3f); - b2Color c3(0.9f, 0.3f, 0.3f); - b2Color c4(0.4f, 0.4f, 0.4f); - - draw->DrawSegment(pA, pB, c4); - - b2Vec2 pRest = pA + m_length * axis; - draw->DrawPoint(pRest, 8.0f, c1); - - if (m_minLength != m_maxLength) - { - if (m_minLength > b2_linearSlop) - { - b2Vec2 pMin = pA + m_minLength * axis; - draw->DrawPoint(pMin, 4.0f, c2); - } - - if (m_maxLength < FLT_MAX) - { - b2Vec2 pMax = pA + m_maxLength * axis; - draw->DrawPoint(pMax, 4.0f, c3); - } - } -} diff --git a/3rdparty/box2d/src/dynamics/b2_edge_circle_contact.cpp b/3rdparty/box2d/src/dynamics/b2_edge_circle_contact.cpp deleted file mode 100644 index 8a126de6c630..000000000000 --- a/3rdparty/box2d/src/dynamics/b2_edge_circle_contact.cpp +++ /dev/null @@ -1,54 +0,0 @@ -// MIT License - -// Copyright (c) 2019 Erin Catto - -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: - -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. - -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -#include "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/3rdparty/box2d/src/dynamics/b2_edge_circle_contact.h b/3rdparty/box2d/src/dynamics/b2_edge_circle_contact.h deleted file mode 100644 index 3efc88e60712..000000000000 --- a/3rdparty/box2d/src/dynamics/b2_edge_circle_contact.h +++ /dev/null @@ -1,43 +0,0 @@ -// MIT License - -// Copyright (c) 2019 Erin Catto - -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: - -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. - -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -#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/3rdparty/box2d/src/dynamics/b2_edge_polygon_contact.cpp b/3rdparty/box2d/src/dynamics/b2_edge_polygon_contact.cpp deleted file mode 100644 index e617e3516bbc..000000000000 --- a/3rdparty/box2d/src/dynamics/b2_edge_polygon_contact.cpp +++ /dev/null @@ -1,54 +0,0 @@ -// MIT License - -// Copyright (c) 2019 Erin Catto - -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: - -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. - -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -#include "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/3rdparty/box2d/src/dynamics/b2_edge_polygon_contact.h b/3rdparty/box2d/src/dynamics/b2_edge_polygon_contact.h deleted file mode 100644 index e6616b534ebb..000000000000 --- a/3rdparty/box2d/src/dynamics/b2_edge_polygon_contact.h +++ /dev/null @@ -1,43 +0,0 @@ -// MIT License - -// Copyright (c) 2019 Erin Catto - -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: - -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. - -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -#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/3rdparty/box2d/src/dynamics/b2_fixture.cpp b/3rdparty/box2d/src/dynamics/b2_fixture.cpp deleted file mode 100644 index 9fd700aa9a2a..000000000000 --- a/3rdparty/box2d/src/dynamics/b2_fixture.cpp +++ /dev/null @@ -1,305 +0,0 @@ -// MIT License - -// Copyright (c) 2019 Erin Catto - -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: - -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. - -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -#include "box2d/b2_fixture.h" -#include "box2d/b2_block_allocator.h" -#include "box2d/b2_broad_phase.h" -#include "box2d/b2_chain_shape.h" -#include "box2d/b2_circle_shape.h" -#include "box2d/b2_collision.h" -#include "box2d/b2_contact.h" -#include "box2d/b2_edge_shape.h" -#include "box2d/b2_polygon_shape.h" -#include "box2d/b2_world.h" - -b2Fixture::b2Fixture() -{ - m_body = nullptr; - m_next = nullptr; - m_proxies = nullptr; - m_proxyCount = 0; - m_shape = nullptr; - m_density = 0.0f; -} - -void b2Fixture::Create(b2BlockAllocator* allocator, b2Body* body, const b2FixtureDef* def) -{ - m_userData = def->userData; - m_friction = def->friction; - m_restitution = def->restitution; - m_restitutionThreshold = def->restitutionThreshold; - - m_body = body; - m_next = nullptr; - - m_filter = def->filter; - - m_isSensor = def->isSensor; - - m_shape = def->shape->Clone(allocator); - - // Reserve proxy space - int32 childCount = m_shape->GetChildCount(); - m_proxies = (b2FixtureProxy*)allocator->Allocate(childCount * sizeof(b2FixtureProxy)); - for (int32 i = 0; i < childCount; ++i) - { - m_proxies[i].fixture = nullptr; - m_proxies[i].proxyId = b2BroadPhase::e_nullProxy; - } - m_proxyCount = 0; - - m_density = def->density; -} - -void b2Fixture::Destroy(b2BlockAllocator* allocator) -{ - // The proxies must be destroyed before calling this. - b2Assert(m_proxyCount == 0); - - // Free the proxy array. - int32 childCount = m_shape->GetChildCount(); - allocator->Free(m_proxies, childCount * sizeof(b2FixtureProxy)); - m_proxies = nullptr; - - // Free the child shape. - switch (m_shape->m_type) - { - case b2Shape::e_circle: - { - b2CircleShape* s = (b2CircleShape*)m_shape; - s->~b2CircleShape(); - allocator->Free(s, sizeof(b2CircleShape)); - } - break; - - case b2Shape::e_edge: - { - b2EdgeShape* s = (b2EdgeShape*)m_shape; - s->~b2EdgeShape(); - allocator->Free(s, sizeof(b2EdgeShape)); - } - break; - - case b2Shape::e_polygon: - { - b2PolygonShape* s = (b2PolygonShape*)m_shape; - s->~b2PolygonShape(); - allocator->Free(s, sizeof(b2PolygonShape)); - } - break; - - case b2Shape::e_chain: - { - b2ChainShape* s = (b2ChainShape*)m_shape; - s->~b2ChainShape(); - allocator->Free(s, sizeof(b2ChainShape)); - } - break; - - default: - b2Assert(false); - break; - } - - m_shape = nullptr; -} - -void b2Fixture::CreateProxies(b2BroadPhase* broadPhase, const b2Transform& xf) -{ - b2Assert(m_proxyCount == 0); - - // Create proxies in the broad-phase. - m_proxyCount = m_shape->GetChildCount(); - - for (int32 i = 0; i < m_proxyCount; ++i) - { - b2FixtureProxy* proxy = m_proxies + i; - m_shape->ComputeAABB(&proxy->aabb, xf, i); - proxy->proxyId = broadPhase->CreateProxy(proxy->aabb, proxy); - proxy->fixture = this; - proxy->childIndex = i; - } -} - -void b2Fixture::DestroyProxies(b2BroadPhase* broadPhase) -{ - // Destroy proxies in the broad-phase. - for (int32 i = 0; i < m_proxyCount; ++i) - { - b2FixtureProxy* proxy = m_proxies + i; - broadPhase->DestroyProxy(proxy->proxyId); - proxy->proxyId = b2BroadPhase::e_nullProxy; - } - - m_proxyCount = 0; -} - -void b2Fixture::Synchronize(b2BroadPhase* broadPhase, const b2Transform& transform1, const b2Transform& transform2) -{ - if (m_proxyCount == 0) - { - return; - } - - for (int32 i = 0; i < m_proxyCount; ++i) - { - b2FixtureProxy* proxy = m_proxies + i; - - // Compute an AABB that covers the swept shape (may miss some rotation effect). - b2AABB aabb1, aabb2; - m_shape->ComputeAABB(&aabb1, transform1, proxy->childIndex); - m_shape->ComputeAABB(&aabb2, transform2, proxy->childIndex); - - proxy->aabb.Combine(aabb1, aabb2); - - b2Vec2 displacement = aabb2.GetCenter() - aabb1.GetCenter(); - - broadPhase->MoveProxy(proxy->proxyId, proxy->aabb, displacement); - } -} - -void b2Fixture::SetFilterData(const b2Filter& filter) -{ - m_filter = filter; - - Refilter(); -} - -void b2Fixture::Refilter() -{ - if (m_body == nullptr) - { - return; - } - - // Flag associated contacts for filtering. - b2ContactEdge* edge = m_body->GetContactList(); - while (edge) - { - b2Contact* contact = edge->contact; - b2Fixture* fixtureA = contact->GetFixtureA(); - b2Fixture* fixtureB = contact->GetFixtureB(); - if (fixtureA == this || fixtureB == this) - { - contact->FlagForFiltering(); - } - - edge = edge->next; - } - - b2World* world = m_body->GetWorld(); - - if (world == nullptr) - { - return; - } - - // Touch each proxy so that new pairs may be created - b2BroadPhase* broadPhase = &world->m_contactManager.m_broadPhase; - for (int32 i = 0; i < m_proxyCount; ++i) - { - broadPhase->TouchProxy(m_proxies[i].proxyId); - } -} - -void b2Fixture::SetSensor(bool sensor) -{ - if (sensor != m_isSensor) - { - m_body->SetAwake(true); - m_isSensor = sensor; - } -} - -void b2Fixture::Dump(int32 bodyIndex) -{ - b2Dump(" b2FixtureDef fd;\n"); - b2Dump(" fd.friction = %.9g;\n", m_friction); - b2Dump(" fd.restitution = %.9g;\n", m_restitution); - b2Dump(" fd.restitutionThreshold = %.9g;\n", m_restitutionThreshold); - b2Dump(" fd.density = %.9g;\n", m_density); - b2Dump(" fd.isSensor = bool(%d);\n", m_isSensor); - b2Dump(" fd.filter.categoryBits = uint16(%d);\n", m_filter.categoryBits); - b2Dump(" fd.filter.maskBits = uint16(%d);\n", m_filter.maskBits); - b2Dump(" fd.filter.groupIndex = int16(%d);\n", m_filter.groupIndex); - - switch (m_shape->m_type) - { - case b2Shape::e_circle: - { - b2CircleShape* s = (b2CircleShape*)m_shape; - b2Dump(" b2CircleShape shape;\n"); - b2Dump(" shape.m_radius = %.9g;\n", s->m_radius); - b2Dump(" shape.m_p.Set(%.9g, %.9g);\n", s->m_p.x, s->m_p.y); - } - break; - - case b2Shape::e_edge: - { - b2EdgeShape* s = (b2EdgeShape*)m_shape; - b2Dump(" b2EdgeShape shape;\n"); - b2Dump(" shape.m_radius = %.9g;\n", s->m_radius); - b2Dump(" shape.m_vertex0.Set(%.9g, %.9g);\n", s->m_vertex0.x, s->m_vertex0.y); - b2Dump(" shape.m_vertex1.Set(%.9g, %.9g);\n", s->m_vertex1.x, s->m_vertex1.y); - b2Dump(" shape.m_vertex2.Set(%.9g, %.9g);\n", s->m_vertex2.x, s->m_vertex2.y); - b2Dump(" shape.m_vertex3.Set(%.9g, %.9g);\n", s->m_vertex3.x, s->m_vertex3.y); - b2Dump(" shape.m_oneSided = bool(%d);\n", s->m_oneSided); - } - break; - - case b2Shape::e_polygon: - { - b2PolygonShape* s = (b2PolygonShape*)m_shape; - b2Dump(" b2PolygonShape shape;\n"); - b2Dump(" b2Vec2 vs[%d];\n", b2_maxPolygonVertices); - for (int32 i = 0; i < s->m_count; ++i) - { - b2Dump(" vs[%d].Set(%.9g, %.9g);\n", i, s->m_vertices[i].x, s->m_vertices[i].y); - } - b2Dump(" shape.Set(vs, %d);\n", s->m_count); - } - break; - - case b2Shape::e_chain: - { - b2ChainShape* s = (b2ChainShape*)m_shape; - b2Dump(" b2ChainShape shape;\n"); - b2Dump(" b2Vec2 vs[%d];\n", s->m_count); - for (int32 i = 0; i < s->m_count; ++i) - { - b2Dump(" vs[%d].Set(%.9g, %.9g);\n", i, s->m_vertices[i].x, s->m_vertices[i].y); - } - b2Dump(" shape.CreateChain(vs, %d);\n", s->m_count); - b2Dump(" shape.m_prevVertex.Set(%.9g, %.9g);\n", s->m_prevVertex.x, s->m_prevVertex.y); - b2Dump(" shape.m_nextVertex.Set(%.9g, %.9g);\n", s->m_nextVertex.x, s->m_nextVertex.y); - } - break; - - default: - return; - } - - b2Dump("\n"); - b2Dump(" fd.shape = &shape;\n"); - b2Dump("\n"); - b2Dump(" bodies[%d]->CreateFixture(&fd);\n", bodyIndex); -} diff --git a/3rdparty/box2d/src/dynamics/b2_friction_joint.cpp b/3rdparty/box2d/src/dynamics/b2_friction_joint.cpp deleted file mode 100644 index d9d893ad3e09..000000000000 --- a/3rdparty/box2d/src/dynamics/b2_friction_joint.cpp +++ /dev/null @@ -1,255 +0,0 @@ -// MIT License - -// Copyright (c) 2019 Erin Catto - -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: - -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. - -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -#include "box2d/b2_friction_joint.h" -#include "box2d/b2_body.h" -#include "box2d/b2_time_step.h" - -// Point-to-point constraint -// Cdot = v2 - v1 -// = v2 + cross(w2, r2) - v1 - cross(w1, r1) -// J = [-I -r1_skew I r2_skew ] -// Identity used: -// w k % (rx i + ry j) = w * (-ry i + rx j) - -// Angle constraint -// Cdot = w2 - w1 -// J = [0 0 -1 0 0 1] -// K = invI1 + invI2 - -void b2FrictionJointDef::Initialize(b2Body* bA, b2Body* bB, const b2Vec2& anchor) -{ - bodyA = bA; - bodyB = bB; - localAnchorA = bodyA->GetLocalPoint(anchor); - localAnchorB = bodyB->GetLocalPoint(anchor); -} - -b2FrictionJoint::b2FrictionJoint(const b2FrictionJointDef* def) -: b2Joint(def) -{ - m_localAnchorA = def->localAnchorA; - m_localAnchorB = def->localAnchorB; - - m_linearImpulse.SetZero(); - m_angularImpulse = 0.0f; - - m_maxForce = def->maxForce; - m_maxTorque = def->maxTorque; -} - -void b2FrictionJoint::InitVelocityConstraints(const b2SolverData& data) -{ - m_indexA = m_bodyA->m_islandIndex; - m_indexB = m_bodyB->m_islandIndex; - m_localCenterA = m_bodyA->m_sweep.localCenter; - m_localCenterB = m_bodyB->m_sweep.localCenter; - m_invMassA = m_bodyA->m_invMass; - m_invMassB = m_bodyB->m_invMass; - m_invIA = m_bodyA->m_invI; - m_invIB = m_bodyB->m_invI; - - float aA = data.positions[m_indexA].a; - b2Vec2 vA = data.velocities[m_indexA].v; - float wA = data.velocities[m_indexA].w; - - float aB = data.positions[m_indexB].a; - b2Vec2 vB = data.velocities[m_indexB].v; - float wB = data.velocities[m_indexB].w; - - b2Rot qA(aA), qB(aB); - - // Compute the effective mass matrix. - m_rA = b2Mul(qA, m_localAnchorA - m_localCenterA); - m_rB = b2Mul(qB, m_localAnchorB - m_localCenterB); - - // J = [-I -r1_skew I r2_skew] - // [ 0 -1 0 1] - // r_skew = [-ry; rx] - - // Matlab - // K = [ mA+r1y^2*iA+mB+r2y^2*iB, -r1y*iA*r1x-r2y*iB*r2x, -r1y*iA-r2y*iB] - // [ -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB, r1x*iA+r2x*iB] - // [ -r1y*iA-r2y*iB, r1x*iA+r2x*iB, iA+iB] - - float mA = m_invMassA, mB = m_invMassB; - float iA = m_invIA, iB = m_invIB; - - b2Mat22 K; - K.ex.x = mA + mB + iA * m_rA.y * m_rA.y + iB * m_rB.y * m_rB.y; - K.ex.y = -iA * m_rA.x * m_rA.y - iB * m_rB.x * m_rB.y; - K.ey.x = K.ex.y; - K.ey.y = mA + mB + iA * m_rA.x * m_rA.x + iB * m_rB.x * m_rB.x; - - m_linearMass = K.GetInverse(); - - m_angularMass = iA + iB; - if (m_angularMass > 0.0f) - { - m_angularMass = 1.0f / m_angularMass; - } - - if (data.step.warmStarting) - { - // Scale impulses to support a variable time step. - m_linearImpulse *= data.step.dtRatio; - m_angularImpulse *= data.step.dtRatio; - - b2Vec2 P(m_linearImpulse.x, m_linearImpulse.y); - vA -= mA * P; - wA -= iA * (b2Cross(m_rA, P) + m_angularImpulse); - vB += mB * P; - wB += iB * (b2Cross(m_rB, P) + m_angularImpulse); - } - else - { - m_linearImpulse.SetZero(); - m_angularImpulse = 0.0f; - } - - data.velocities[m_indexA].v = vA; - data.velocities[m_indexA].w = wA; - data.velocities[m_indexB].v = vB; - data.velocities[m_indexB].w = wB; -} - -void b2FrictionJoint::SolveVelocityConstraints(const b2SolverData& data) -{ - b2Vec2 vA = data.velocities[m_indexA].v; - float wA = data.velocities[m_indexA].w; - b2Vec2 vB = data.velocities[m_indexB].v; - float wB = data.velocities[m_indexB].w; - - float mA = m_invMassA, mB = m_invMassB; - float iA = m_invIA, iB = m_invIB; - - float h = data.step.dt; - - // Solve angular friction - { - float Cdot = wB - wA; - float impulse = -m_angularMass * Cdot; - - float oldImpulse = m_angularImpulse; - float maxImpulse = h * m_maxTorque; - m_angularImpulse = b2Clamp(m_angularImpulse + impulse, -maxImpulse, maxImpulse); - impulse = m_angularImpulse - oldImpulse; - - wA -= iA * impulse; - wB += iB * impulse; - } - - // Solve linear friction - { - b2Vec2 Cdot = vB + b2Cross(wB, m_rB) - vA - b2Cross(wA, m_rA); - - b2Vec2 impulse = -b2Mul(m_linearMass, Cdot); - b2Vec2 oldImpulse = m_linearImpulse; - m_linearImpulse += impulse; - - float maxImpulse = h * m_maxForce; - - if (m_linearImpulse.LengthSquared() > maxImpulse * maxImpulse) - { - m_linearImpulse.Normalize(); - m_linearImpulse *= maxImpulse; - } - - impulse = m_linearImpulse - oldImpulse; - - vA -= mA * impulse; - wA -= iA * b2Cross(m_rA, impulse); - - vB += mB * impulse; - wB += iB * b2Cross(m_rB, impulse); - } - - data.velocities[m_indexA].v = vA; - data.velocities[m_indexA].w = wA; - data.velocities[m_indexB].v = vB; - data.velocities[m_indexB].w = wB; -} - -bool b2FrictionJoint::SolvePositionConstraints(const b2SolverData& data) -{ - B2_NOT_USED(data); - - return true; -} - -b2Vec2 b2FrictionJoint::GetAnchorA() const -{ - return m_bodyA->GetWorldPoint(m_localAnchorA); -} - -b2Vec2 b2FrictionJoint::GetAnchorB() const -{ - return m_bodyB->GetWorldPoint(m_localAnchorB); -} - -b2Vec2 b2FrictionJoint::GetReactionForce(float inv_dt) const -{ - return inv_dt * m_linearImpulse; -} - -float b2FrictionJoint::GetReactionTorque(float inv_dt) const -{ - return inv_dt * m_angularImpulse; -} - -void b2FrictionJoint::SetMaxForce(float force) -{ - b2Assert(b2IsValid(force) && force >= 0.0f); - m_maxForce = force; -} - -float b2FrictionJoint::GetMaxForce() const -{ - return m_maxForce; -} - -void b2FrictionJoint::SetMaxTorque(float torque) -{ - b2Assert(b2IsValid(torque) && torque >= 0.0f); - m_maxTorque = torque; -} - -float b2FrictionJoint::GetMaxTorque() const -{ - return m_maxTorque; -} - -void b2FrictionJoint::Dump() -{ - int32 indexA = m_bodyA->m_islandIndex; - int32 indexB = m_bodyB->m_islandIndex; - - b2Dump(" b2FrictionJointDef jd;\n"); - b2Dump(" jd.bodyA = bodies[%d];\n", indexA); - b2Dump(" jd.bodyB = bodies[%d];\n", indexB); - b2Dump(" jd.collideConnected = bool(%d);\n", m_collideConnected); - b2Dump(" jd.localAnchorA.Set(%.9g, %.9g);\n", m_localAnchorA.x, m_localAnchorA.y); - b2Dump(" jd.localAnchorB.Set(%.9g, %.9g);\n", m_localAnchorB.x, m_localAnchorB.y); - b2Dump(" jd.maxForce = %.9g;\n", m_maxForce); - b2Dump(" jd.maxTorque = %.9g;\n", m_maxTorque); - b2Dump(" joints[%d] = m_world->CreateJoint(&jd);\n", m_index); -} diff --git a/3rdparty/box2d/src/dynamics/b2_gear_joint.cpp b/3rdparty/box2d/src/dynamics/b2_gear_joint.cpp deleted file mode 100644 index 5fb547245e93..000000000000 --- a/3rdparty/box2d/src/dynamics/b2_gear_joint.cpp +++ /dev/null @@ -1,437 +0,0 @@ -// MIT License - -// Copyright (c) 2019 Erin Catto - -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: - -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. - -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -#include "box2d/b2_gear_joint.h" -#include "box2d/b2_revolute_joint.h" -#include "box2d/b2_prismatic_joint.h" -#include "box2d/b2_body.h" -#include "box2d/b2_time_step.h" - -// Gear Joint: -// C0 = (coordinate1 + ratio * coordinate2)_initial -// C = (coordinate1 + ratio * coordinate2) - C0 = 0 -// J = [J1 ratio * J2] -// K = J * invM * JT -// = J1 * invM1 * J1T + ratio * ratio * J2 * invM2 * J2T -// -// Revolute: -// coordinate = rotation -// Cdot = angularVelocity -// J = [0 0 1] -// K = J * invM * JT = invI -// -// Prismatic: -// coordinate = dot(p - pg, ug) -// Cdot = dot(v + cross(w, r), ug) -// J = [ug cross(r, ug)] -// K = J * invM * JT = invMass + invI * cross(r, ug)^2 - -b2GearJoint::b2GearJoint(const b2GearJointDef* def) -: b2Joint(def) -{ - m_joint1 = def->joint1; - m_joint2 = def->joint2; - - m_typeA = m_joint1->GetType(); - m_typeB = m_joint2->GetType(); - - b2Assert(m_typeA == e_revoluteJoint || m_typeA == e_prismaticJoint); - b2Assert(m_typeB == e_revoluteJoint || m_typeB == e_prismaticJoint); - - float coordinateA, coordinateB; - - // TODO_ERIN there might be some problem with the joint edges in b2Joint. - - m_bodyC = m_joint1->GetBodyA(); - m_bodyA = m_joint1->GetBodyB(); - - // Body B on joint1 must be dynamic - b2Assert(m_bodyA->m_type == b2_dynamicBody); - - // Get geometry of joint1 - b2Transform xfA = m_bodyA->m_xf; - float aA = m_bodyA->m_sweep.a; - b2Transform xfC = m_bodyC->m_xf; - float aC = m_bodyC->m_sweep.a; - - if (m_typeA == e_revoluteJoint) - { - b2RevoluteJoint* revolute = (b2RevoluteJoint*)def->joint1; - m_localAnchorC = revolute->m_localAnchorA; - m_localAnchorA = revolute->m_localAnchorB; - m_referenceAngleA = revolute->m_referenceAngle; - m_localAxisC.SetZero(); - - coordinateA = aA - aC - m_referenceAngleA; - - // position error is measured in radians - m_tolerance = b2_angularSlop; - } - else - { - b2PrismaticJoint* prismatic = (b2PrismaticJoint*)def->joint1; - m_localAnchorC = prismatic->m_localAnchorA; - m_localAnchorA = prismatic->m_localAnchorB; - m_referenceAngleA = prismatic->m_referenceAngle; - m_localAxisC = prismatic->m_localXAxisA; - - b2Vec2 pC = m_localAnchorC; - b2Vec2 pA = b2MulT(xfC.q, b2Mul(xfA.q, m_localAnchorA) + (xfA.p - xfC.p)); - coordinateA = b2Dot(pA - pC, m_localAxisC); - - // position error is measured in meters - m_tolerance = b2_linearSlop; - } - - m_bodyD = m_joint2->GetBodyA(); - m_bodyB = m_joint2->GetBodyB(); - - // Body B on joint2 must be dynamic - b2Assert(m_bodyB->m_type == b2_dynamicBody); - - // Get geometry of joint2 - b2Transform xfB = m_bodyB->m_xf; - float aB = m_bodyB->m_sweep.a; - b2Transform xfD = m_bodyD->m_xf; - float aD = m_bodyD->m_sweep.a; - - if (m_typeB == e_revoluteJoint) - { - b2RevoluteJoint* revolute = (b2RevoluteJoint*)def->joint2; - m_localAnchorD = revolute->m_localAnchorA; - m_localAnchorB = revolute->m_localAnchorB; - m_referenceAngleB = revolute->m_referenceAngle; - m_localAxisD.SetZero(); - - coordinateB = aB - aD - m_referenceAngleB; - } - else - { - b2PrismaticJoint* prismatic = (b2PrismaticJoint*)def->joint2; - m_localAnchorD = prismatic->m_localAnchorA; - m_localAnchorB = prismatic->m_localAnchorB; - m_referenceAngleB = prismatic->m_referenceAngle; - m_localAxisD = prismatic->m_localXAxisA; - - b2Vec2 pD = m_localAnchorD; - b2Vec2 pB = b2MulT(xfD.q, b2Mul(xfB.q, m_localAnchorB) + (xfB.p - xfD.p)); - coordinateB = b2Dot(pB - pD, m_localAxisD); - } - - m_ratio = def->ratio; - - m_constant = coordinateA + m_ratio * coordinateB; - - m_impulse = 0.0f; -} - -void b2GearJoint::InitVelocityConstraints(const b2SolverData& data) -{ - m_indexA = m_bodyA->m_islandIndex; - m_indexB = m_bodyB->m_islandIndex; - m_indexC = m_bodyC->m_islandIndex; - m_indexD = m_bodyD->m_islandIndex; - m_lcA = m_bodyA->m_sweep.localCenter; - m_lcB = m_bodyB->m_sweep.localCenter; - m_lcC = m_bodyC->m_sweep.localCenter; - m_lcD = m_bodyD->m_sweep.localCenter; - m_mA = m_bodyA->m_invMass; - m_mB = m_bodyB->m_invMass; - m_mC = m_bodyC->m_invMass; - m_mD = m_bodyD->m_invMass; - m_iA = m_bodyA->m_invI; - m_iB = m_bodyB->m_invI; - m_iC = m_bodyC->m_invI; - m_iD = m_bodyD->m_invI; - - float aA = data.positions[m_indexA].a; - b2Vec2 vA = data.velocities[m_indexA].v; - float wA = data.velocities[m_indexA].w; - - float aB = data.positions[m_indexB].a; - b2Vec2 vB = data.velocities[m_indexB].v; - float wB = data.velocities[m_indexB].w; - - float aC = data.positions[m_indexC].a; - b2Vec2 vC = data.velocities[m_indexC].v; - float wC = data.velocities[m_indexC].w; - - float aD = data.positions[m_indexD].a; - b2Vec2 vD = data.velocities[m_indexD].v; - float wD = data.velocities[m_indexD].w; - - b2Rot qA(aA), qB(aB), qC(aC), qD(aD); - - m_mass = 0.0f; - - if (m_typeA == e_revoluteJoint) - { - m_JvAC.SetZero(); - m_JwA = 1.0f; - m_JwC = 1.0f; - m_mass += m_iA + m_iC; - } - else - { - b2Vec2 u = b2Mul(qC, m_localAxisC); - b2Vec2 rC = b2Mul(qC, m_localAnchorC - m_lcC); - b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_lcA); - m_JvAC = u; - m_JwC = b2Cross(rC, u); - m_JwA = b2Cross(rA, u); - m_mass += m_mC + m_mA + m_iC * m_JwC * m_JwC + m_iA * m_JwA * m_JwA; - } - - if (m_typeB == e_revoluteJoint) - { - m_JvBD.SetZero(); - m_JwB = m_ratio; - m_JwD = m_ratio; - m_mass += m_ratio * m_ratio * (m_iB + m_iD); - } - else - { - b2Vec2 u = b2Mul(qD, m_localAxisD); - b2Vec2 rD = b2Mul(qD, m_localAnchorD - m_lcD); - b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_lcB); - m_JvBD = m_ratio * u; - m_JwD = m_ratio * b2Cross(rD, u); - m_JwB = m_ratio * b2Cross(rB, u); - m_mass += m_ratio * m_ratio * (m_mD + m_mB) + m_iD * m_JwD * m_JwD + m_iB * m_JwB * m_JwB; - } - - // Compute effective mass. - m_mass = m_mass > 0.0f ? 1.0f / m_mass : 0.0f; - - if (data.step.warmStarting) - { - vA += (m_mA * m_impulse) * m_JvAC; - wA += m_iA * m_impulse * m_JwA; - vB += (m_mB * m_impulse) * m_JvBD; - wB += m_iB * m_impulse * m_JwB; - vC -= (m_mC * m_impulse) * m_JvAC; - wC -= m_iC * m_impulse * m_JwC; - vD -= (m_mD * m_impulse) * m_JvBD; - wD -= m_iD * m_impulse * m_JwD; - } - else - { - m_impulse = 0.0f; - } - - data.velocities[m_indexA].v = vA; - data.velocities[m_indexA].w = wA; - data.velocities[m_indexB].v = vB; - data.velocities[m_indexB].w = wB; - data.velocities[m_indexC].v = vC; - data.velocities[m_indexC].w = wC; - data.velocities[m_indexD].v = vD; - data.velocities[m_indexD].w = wD; -} - -void b2GearJoint::SolveVelocityConstraints(const b2SolverData& data) -{ - b2Vec2 vA = data.velocities[m_indexA].v; - float wA = data.velocities[m_indexA].w; - b2Vec2 vB = data.velocities[m_indexB].v; - float wB = data.velocities[m_indexB].w; - b2Vec2 vC = data.velocities[m_indexC].v; - float wC = data.velocities[m_indexC].w; - b2Vec2 vD = data.velocities[m_indexD].v; - float wD = data.velocities[m_indexD].w; - - float Cdot = b2Dot(m_JvAC, vA - vC) + b2Dot(m_JvBD, vB - vD); - Cdot += (m_JwA * wA - m_JwC * wC) + (m_JwB * wB - m_JwD * wD); - - float impulse = -m_mass * Cdot; - m_impulse += impulse; - - vA += (m_mA * impulse) * m_JvAC; - wA += m_iA * impulse * m_JwA; - vB += (m_mB * impulse) * m_JvBD; - wB += m_iB * impulse * m_JwB; - vC -= (m_mC * impulse) * m_JvAC; - wC -= m_iC * impulse * m_JwC; - vD -= (m_mD * impulse) * m_JvBD; - wD -= m_iD * impulse * m_JwD; - - data.velocities[m_indexA].v = vA; - data.velocities[m_indexA].w = wA; - data.velocities[m_indexB].v = vB; - data.velocities[m_indexB].w = wB; - data.velocities[m_indexC].v = vC; - data.velocities[m_indexC].w = wC; - data.velocities[m_indexD].v = vD; - data.velocities[m_indexD].w = wD; -} - -bool b2GearJoint::SolvePositionConstraints(const b2SolverData& data) -{ - b2Vec2 cA = data.positions[m_indexA].c; - float aA = data.positions[m_indexA].a; - b2Vec2 cB = data.positions[m_indexB].c; - float aB = data.positions[m_indexB].a; - b2Vec2 cC = data.positions[m_indexC].c; - float aC = data.positions[m_indexC].a; - b2Vec2 cD = data.positions[m_indexD].c; - float aD = data.positions[m_indexD].a; - - b2Rot qA(aA), qB(aB), qC(aC), qD(aD); - - float coordinateA, coordinateB; - - b2Vec2 JvAC, JvBD; - float JwA, JwB, JwC, JwD; - float mass = 0.0f; - - if (m_typeA == e_revoluteJoint) - { - JvAC.SetZero(); - JwA = 1.0f; - JwC = 1.0f; - mass += m_iA + m_iC; - - coordinateA = aA - aC - m_referenceAngleA; - } - else - { - b2Vec2 u = b2Mul(qC, m_localAxisC); - b2Vec2 rC = b2Mul(qC, m_localAnchorC - m_lcC); - b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_lcA); - JvAC = u; - JwC = b2Cross(rC, u); - JwA = b2Cross(rA, u); - mass += m_mC + m_mA + m_iC * JwC * JwC + m_iA * JwA * JwA; - - b2Vec2 pC = m_localAnchorC - m_lcC; - b2Vec2 pA = b2MulT(qC, rA + (cA - cC)); - coordinateA = b2Dot(pA - pC, m_localAxisC); - } - - if (m_typeB == e_revoluteJoint) - { - JvBD.SetZero(); - JwB = m_ratio; - JwD = m_ratio; - mass += m_ratio * m_ratio * (m_iB + m_iD); - - coordinateB = aB - aD - m_referenceAngleB; - } - else - { - b2Vec2 u = b2Mul(qD, m_localAxisD); - b2Vec2 rD = b2Mul(qD, m_localAnchorD - m_lcD); - b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_lcB); - JvBD = m_ratio * u; - JwD = m_ratio * b2Cross(rD, u); - JwB = m_ratio * b2Cross(rB, u); - mass += m_ratio * m_ratio * (m_mD + m_mB) + m_iD * JwD * JwD + m_iB * JwB * JwB; - - b2Vec2 pD = m_localAnchorD - m_lcD; - b2Vec2 pB = b2MulT(qD, rB + (cB - cD)); - coordinateB = b2Dot(pB - pD, m_localAxisD); - } - - float C = (coordinateA + m_ratio * coordinateB) - m_constant; - - float impulse = 0.0f; - if (mass > 0.0f) - { - impulse = -C / mass; - } - - cA += m_mA * impulse * JvAC; - aA += m_iA * impulse * JwA; - cB += m_mB * impulse * JvBD; - aB += m_iB * impulse * JwB; - cC -= m_mC * impulse * JvAC; - aC -= m_iC * impulse * JwC; - cD -= m_mD * impulse * JvBD; - aD -= m_iD * impulse * JwD; - - data.positions[m_indexA].c = cA; - data.positions[m_indexA].a = aA; - data.positions[m_indexB].c = cB; - data.positions[m_indexB].a = aB; - data.positions[m_indexC].c = cC; - data.positions[m_indexC].a = aC; - data.positions[m_indexD].c = cD; - data.positions[m_indexD].a = aD; - - if (b2Abs(C) < m_tolerance) - { - return true; - } - - return false; -} - -b2Vec2 b2GearJoint::GetAnchorA() const -{ - return m_bodyA->GetWorldPoint(m_localAnchorA); -} - -b2Vec2 b2GearJoint::GetAnchorB() const -{ - return m_bodyB->GetWorldPoint(m_localAnchorB); -} - -b2Vec2 b2GearJoint::GetReactionForce(float inv_dt) const -{ - b2Vec2 P = m_impulse * m_JvAC; - return inv_dt * P; -} - -float b2GearJoint::GetReactionTorque(float inv_dt) const -{ - float L = m_impulse * m_JwA; - return inv_dt * L; -} - -void b2GearJoint::SetRatio(float ratio) -{ - b2Assert(b2IsValid(ratio)); - m_ratio = ratio; -} - -float b2GearJoint::GetRatio() const -{ - return m_ratio; -} - -void b2GearJoint::Dump() -{ - int32 indexA = m_bodyA->m_islandIndex; - int32 indexB = m_bodyB->m_islandIndex; - - int32 index1 = m_joint1->m_index; - int32 index2 = m_joint2->m_index; - - b2Dump(" b2GearJointDef jd;\n"); - b2Dump(" jd.bodyA = bodies[%d];\n", indexA); - b2Dump(" jd.bodyB = bodies[%d];\n", indexB); - b2Dump(" jd.collideConnected = bool(%d);\n", m_collideConnected); - b2Dump(" jd.joint1 = joints[%d];\n", index1); - b2Dump(" jd.joint2 = joints[%d];\n", index2); - b2Dump(" jd.ratio = %.9g;\n", m_ratio); - b2Dump(" joints[%d] = m_world->CreateJoint(&jd);\n", m_index); -} diff --git a/3rdparty/box2d/src/dynamics/b2_island.cpp b/3rdparty/box2d/src/dynamics/b2_island.cpp deleted file mode 100644 index 34413056b1f4..000000000000 --- a/3rdparty/box2d/src/dynamics/b2_island.cpp +++ /dev/null @@ -1,544 +0,0 @@ -// MIT License - -// Copyright (c) 2019 Erin Catto - -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: - -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. - -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -#include "box2d/b2_body.h" -#include "box2d/b2_contact.h" -#include "box2d/b2_distance.h" -#include "box2d/b2_fixture.h" -#include "box2d/b2_joint.h" -#include "box2d/b2_stack_allocator.h" -#include "box2d/b2_timer.h" -#include "box2d/b2_world.h" - -#include "b2_contact_solver.h" -#include "b2_island.h" - -/* -Position Correction Notes -========================= -I tried the several algorithms for position correction of the 2D revolute joint. -I looked at these systems: -- simple pendulum (1m diameter sphere on massless 5m stick) with initial angular velocity of 100 rad/s. -- suspension bridge with 30 1m long planks of length 1m. -- multi-link chain with 30 1m long links. - -Here are the algorithms: - -Baumgarte - A fraction of the position error is added to the velocity error. There is no -separate position solver. - -Pseudo Velocities - After the velocity solver and position integration, -the position error, Jacobian, and effective mass are recomputed. Then -the velocity constraints are solved with pseudo velocities and a fraction -of the position error is added to the pseudo velocity error. The pseudo -velocities are initialized to zero and there is no warm-starting. After -the position solver, the pseudo velocities are added to the positions. -This is also called the First Order World method or the Position LCP method. - -Modified Nonlinear Gauss-Seidel (NGS) - Like Pseudo Velocities except the -position error is re-computed for each constraint and the positions are updated -after the constraint is solved. The radius vectors (aka Jacobians) are -re-computed too (otherwise the algorithm has horrible instability). The pseudo -velocity states are not needed because they are effectively zero at the beginning -of each iteration. Since we have the current position error, we allow the -iterations to terminate early if the error becomes smaller than b2_linearSlop. - -Full NGS or just NGS - Like Modified NGS except the effective mass are re-computed -each time a constraint is solved. - -Here are the results: -Baumgarte - this is the cheapest algorithm but it has some stability problems, -especially with the bridge. The chain links separate easily close to the root -and they jitter as they struggle to pull together. This is one of the most common -methods in the field. The big drawback is that the position correction artificially -affects the momentum, thus leading to instabilities and false bounce. I used a -bias factor of 0.2. A larger bias factor makes the bridge less stable, a smaller -factor makes joints and contacts more spongy. - -Pseudo Velocities - the is more stable than the Baumgarte method. The bridge is -stable. However, joints still separate with large angular velocities. Drag the -simple pendulum in a circle quickly and the joint will separate. The chain separates -easily and does not recover. I used a bias factor of 0.2. A larger value lead to -the bridge collapsing when a heavy cube drops on it. - -Modified NGS - this algorithm is better in some ways than Baumgarte and Pseudo -Velocities, but in other ways it is worse. The bridge and chain are much more -stable, but the simple pendulum goes unstable at high angular velocities. - -Full NGS - stable in all tests. The joints display good stiffness. The bridge -still sags, but this is better than infinite forces. - -Recommendations -Pseudo Velocities are not really worthwhile because the bridge and chain cannot -recover from joint separation. In other cases the benefit over Baumgarte is small. - -Modified NGS is not a robust method for the revolute joint due to the violent -instability seen in the simple pendulum. Perhaps it is viable with other constraint -types, especially scalar constraints where the effective mass is a scalar. - -This leaves Baumgarte and Full NGS. Baumgarte has small, but manageable instabilities -and is very fast. I don't think we can escape Baumgarte, especially in highly -demanding cases where high constraint fidelity is not needed. - -Full NGS is robust and easy on the eyes. I recommend this as an option for -higher fidelity simulation and certainly for suspension bridges and long chains. -Full NGS might be a good choice for ragdolls, especially motorized ragdolls where -joint separation can be problematic. The number of NGS iterations can be reduced -for better performance without harming robustness much. - -Each joint in a can be handled differently in the position solver. So I recommend -a system where the user can select the algorithm on a per joint basis. I would -probably default to the slower Full NGS and let the user select the faster -Baumgarte method in performance critical scenarios. -*/ - -/* -Cache Performance - -The Box2D solvers are dominated by cache misses. Data structures are designed -to increase the number of cache hits. Much of misses are due to random access -to body data. The constraint structures are iterated over linearly, which leads -to few cache misses. - -The bodies are not accessed during iteration. Instead read only data, such as -the mass values are stored with the constraints. The mutable data are the constraint -impulses and the bodies velocities/positions. The impulses are held inside the -constraint structures. The body velocities/positions are held in compact, temporary -arrays to increase the number of cache hits. Linear and angular velocity are -stored in a single array since multiple arrays lead to multiple misses. -*/ - -/* -2D Rotation - -R = [cos(theta) -sin(theta)] - [sin(theta) cos(theta) ] - -thetaDot = omega - -Let q1 = cos(theta), q2 = sin(theta). -R = [q1 -q2] - [q2 q1] - -q1Dot = -thetaDot * q2 -q2Dot = thetaDot * q1 - -q1_new = q1_old - dt * w * q2 -q2_new = q2_old + dt * w * q1 -then normalize. - -This might be faster than computing sin+cos. -However, we can compute sin+cos of the same angle fast. -*/ - -b2Island::b2Island( - int32 bodyCapacity, - int32 contactCapacity, - int32 jointCapacity, - b2StackAllocator* allocator, - b2ContactListener* listener) -{ - m_bodyCapacity = bodyCapacity; - m_contactCapacity = contactCapacity; - m_jointCapacity = jointCapacity; - m_bodyCount = 0; - m_contactCount = 0; - m_jointCount = 0; - - m_allocator = allocator; - m_listener = listener; - - m_bodies = (b2Body**)m_allocator->Allocate(bodyCapacity * sizeof(b2Body*)); - m_contacts = (b2Contact**)m_allocator->Allocate(contactCapacity * sizeof(b2Contact*)); - m_joints = (b2Joint**)m_allocator->Allocate(jointCapacity * sizeof(b2Joint*)); - - m_velocities = (b2Velocity*)m_allocator->Allocate(m_bodyCapacity * sizeof(b2Velocity)); - m_positions = (b2Position*)m_allocator->Allocate(m_bodyCapacity * sizeof(b2Position)); -} - -b2Island::~b2Island() -{ - // Warning: the order should reverse the constructor order. - m_allocator->Free(m_positions); - m_allocator->Free(m_velocities); - m_allocator->Free(m_joints); - m_allocator->Free(m_contacts); - m_allocator->Free(m_bodies); -} - -void b2Island::Solve(b2Profile* profile, const b2TimeStep& step, const b2Vec2& gravity, bool allowSleep) -{ - b2Timer timer; - - float h = step.dt; - - // Integrate velocities and apply damping. Initialize the body state. - for (int32 i = 0; i < m_bodyCount; ++i) - { - b2Body* b = m_bodies[i]; - - b2Vec2 c = b->m_sweep.c; - float a = b->m_sweep.a; - b2Vec2 v = b->m_linearVelocity; - float w = b->m_angularVelocity; - - // Store positions for continuous collision. - b->m_sweep.c0 = b->m_sweep.c; - b->m_sweep.a0 = b->m_sweep.a; - - if (b->m_type == b2_dynamicBody) - { - // Integrate velocities. - v += h * b->m_invMass * (b->m_gravityScale * b->m_mass * gravity + b->m_force); - w += h * b->m_invI * b->m_torque; - - // Apply damping. - // ODE: dv/dt + c * v = 0 - // Solution: v(t) = v0 * exp(-c * t) - // Time step: v(t + dt) = v0 * exp(-c * (t + dt)) = v0 * exp(-c * t) * exp(-c * dt) = v * exp(-c * dt) - // v2 = exp(-c * dt) * v1 - // Pade approximation: - // v2 = v1 * 1 / (1 + c * dt) - v *= 1.0f / (1.0f + h * b->m_linearDamping); - w *= 1.0f / (1.0f + h * b->m_angularDamping); - } - - m_positions[i].c = c; - m_positions[i].a = a; - m_velocities[i].v = v; - m_velocities[i].w = w; - } - - timer.Reset(); - - // Solver data - b2SolverData solverData; - solverData.step = step; - solverData.positions = m_positions; - solverData.velocities = m_velocities; - - // Initialize velocity constraints. - b2ContactSolverDef contactSolverDef; - contactSolverDef.step = step; - contactSolverDef.contacts = m_contacts; - contactSolverDef.count = m_contactCount; - contactSolverDef.positions = m_positions; - contactSolverDef.velocities = m_velocities; - contactSolverDef.allocator = m_allocator; - - b2ContactSolver contactSolver(&contactSolverDef); - contactSolver.InitializeVelocityConstraints(); - - if (step.warmStarting) - { - contactSolver.WarmStart(); - } - - for (int32 i = 0; i < m_jointCount; ++i) - { - m_joints[i]->InitVelocityConstraints(solverData); - } - - profile->solveInit = timer.GetMilliseconds(); - - // Solve velocity constraints - timer.Reset(); - for (int32 i = 0; i < step.velocityIterations; ++i) - { - for (int32 j = 0; j < m_jointCount; ++j) - { - m_joints[j]->SolveVelocityConstraints(solverData); - } - - contactSolver.SolveVelocityConstraints(); - } - - // Store impulses for warm starting - contactSolver.StoreImpulses(); - profile->solveVelocity = timer.GetMilliseconds(); - - // Integrate positions - for (int32 i = 0; i < m_bodyCount; ++i) - { - b2Vec2 c = m_positions[i].c; - float a = m_positions[i].a; - b2Vec2 v = m_velocities[i].v; - float w = m_velocities[i].w; - - // Check for large velocities - b2Vec2 translation = h * v; - if (b2Dot(translation, translation) > b2_maxTranslationSquared) - { - float ratio = b2_maxTranslation / translation.Length(); - v *= ratio; - } - - float rotation = h * w; - if (rotation * rotation > b2_maxRotationSquared) - { - float ratio = b2_maxRotation / b2Abs(rotation); - w *= ratio; - } - - // Integrate - c += h * v; - a += h * w; - - m_positions[i].c = c; - m_positions[i].a = a; - m_velocities[i].v = v; - m_velocities[i].w = w; - } - - // Solve position constraints - timer.Reset(); - bool positionSolved = false; - for (int32 i = 0; i < step.positionIterations; ++i) - { - bool contactsOkay = contactSolver.SolvePositionConstraints(); - - bool jointsOkay = true; - for (int32 j = 0; j < m_jointCount; ++j) - { - bool jointOkay = m_joints[j]->SolvePositionConstraints(solverData); - jointsOkay = jointsOkay && jointOkay; - } - - if (contactsOkay && jointsOkay) - { - // Exit early if the position errors are small. - positionSolved = true; - break; - } - } - - // Copy state buffers back to the bodies - for (int32 i = 0; i < m_bodyCount; ++i) - { - b2Body* body = m_bodies[i]; - body->m_sweep.c = m_positions[i].c; - body->m_sweep.a = m_positions[i].a; - body->m_linearVelocity = m_velocities[i].v; - body->m_angularVelocity = m_velocities[i].w; - body->SynchronizeTransform(); - } - - profile->solvePosition = timer.GetMilliseconds(); - - Report(contactSolver.m_velocityConstraints); - - if (allowSleep) - { - float minSleepTime = b2_maxFloat; - - const float linTolSqr = b2_linearSleepTolerance * b2_linearSleepTolerance; - const float angTolSqr = b2_angularSleepTolerance * b2_angularSleepTolerance; - - for (int32 i = 0; i < m_bodyCount; ++i) - { - b2Body* b = m_bodies[i]; - if (b->GetType() == b2_staticBody) - { - continue; - } - - if ((b->m_flags & b2Body::e_autoSleepFlag) == 0 || - b->m_angularVelocity * b->m_angularVelocity > angTolSqr || - b2Dot(b->m_linearVelocity, b->m_linearVelocity) > linTolSqr) - { - b->m_sleepTime = 0.0f; - minSleepTime = 0.0f; - } - else - { - b->m_sleepTime += h; - minSleepTime = b2Min(minSleepTime, b->m_sleepTime); - } - } - - if (minSleepTime >= b2_timeToSleep && positionSolved) - { - for (int32 i = 0; i < m_bodyCount; ++i) - { - b2Body* b = m_bodies[i]; - b->SetAwake(false); - } - } - } -} - -void b2Island::SolveTOI(const b2TimeStep& subStep, int32 toiIndexA, int32 toiIndexB) -{ - b2Assert(toiIndexA < m_bodyCount); - b2Assert(toiIndexB < m_bodyCount); - - // Initialize the body state. - for (int32 i = 0; i < m_bodyCount; ++i) - { - b2Body* b = m_bodies[i]; - m_positions[i].c = b->m_sweep.c; - m_positions[i].a = b->m_sweep.a; - m_velocities[i].v = b->m_linearVelocity; - m_velocities[i].w = b->m_angularVelocity; - } - - b2ContactSolverDef contactSolverDef; - contactSolverDef.contacts = m_contacts; - contactSolverDef.count = m_contactCount; - contactSolverDef.allocator = m_allocator; - contactSolverDef.step = subStep; - contactSolverDef.positions = m_positions; - contactSolverDef.velocities = m_velocities; - b2ContactSolver contactSolver(&contactSolverDef); - - // Solve position constraints. - for (int32 i = 0; i < subStep.positionIterations; ++i) - { - bool contactsOkay = contactSolver.SolveTOIPositionConstraints(toiIndexA, toiIndexB); - if (contactsOkay) - { - break; - } - } - -#if 0 - // Is the new position really safe? - for (int32 i = 0; i < m_contactCount; ++i) - { - b2Contact* c = m_contacts[i]; - b2Fixture* fA = c->GetFixtureA(); - b2Fixture* fB = c->GetFixtureB(); - - b2Body* bA = fA->GetBody(); - b2Body* bB = fB->GetBody(); - - int32 indexA = c->GetChildIndexA(); - int32 indexB = c->GetChildIndexB(); - - b2DistanceInput input; - input.proxyA.Set(fA->GetShape(), indexA); - input.proxyB.Set(fB->GetShape(), indexB); - input.transformA = bA->GetTransform(); - input.transformB = bB->GetTransform(); - input.useRadii = false; - - b2DistanceOutput output; - b2SimplexCache cache; - cache.count = 0; - b2Distance(&output, &cache, &input); - - if (output.distance == 0 || cache.count == 3) - { - cache.count += 0; - } - } -#endif - - // Leap of faith to new safe state. - m_bodies[toiIndexA]->m_sweep.c0 = m_positions[toiIndexA].c; - m_bodies[toiIndexA]->m_sweep.a0 = m_positions[toiIndexA].a; - m_bodies[toiIndexB]->m_sweep.c0 = m_positions[toiIndexB].c; - m_bodies[toiIndexB]->m_sweep.a0 = m_positions[toiIndexB].a; - - // No warm starting is needed for TOI events because warm - // starting impulses were applied in the discrete solver. - contactSolver.InitializeVelocityConstraints(); - - // Solve velocity constraints. - for (int32 i = 0; i < subStep.velocityIterations; ++i) - { - contactSolver.SolveVelocityConstraints(); - } - - // Don't store the TOI contact forces for warm starting - // because they can be quite large. - - float h = subStep.dt; - - // Integrate positions - for (int32 i = 0; i < m_bodyCount; ++i) - { - b2Vec2 c = m_positions[i].c; - float a = m_positions[i].a; - b2Vec2 v = m_velocities[i].v; - float w = m_velocities[i].w; - - // Check for large velocities - b2Vec2 translation = h * v; - if (b2Dot(translation, translation) > b2_maxTranslationSquared) - { - float ratio = b2_maxTranslation / translation.Length(); - v *= ratio; - } - - float rotation = h * w; - if (rotation * rotation > b2_maxRotationSquared) - { - float ratio = b2_maxRotation / b2Abs(rotation); - w *= ratio; - } - - // Integrate - c += h * v; - a += h * w; - - m_positions[i].c = c; - m_positions[i].a = a; - m_velocities[i].v = v; - m_velocities[i].w = w; - - // Sync bodies - b2Body* body = m_bodies[i]; - body->m_sweep.c = c; - body->m_sweep.a = a; - body->m_linearVelocity = v; - body->m_angularVelocity = w; - body->SynchronizeTransform(); - } - - Report(contactSolver.m_velocityConstraints); -} - -void b2Island::Report(const b2ContactVelocityConstraint* constraints) -{ - if (m_listener == nullptr) - { - return; - } - - for (int32 i = 0; i < m_contactCount; ++i) - { - b2Contact* c = m_contacts[i]; - - const b2ContactVelocityConstraint* vc = constraints + i; - - b2ContactImpulse impulse; - impulse.count = vc->pointCount; - for (int32 j = 0; j < vc->pointCount; ++j) - { - impulse.normalImpulses[j] = vc->points[j].normalImpulse; - impulse.tangentImpulses[j] = vc->points[j].tangentImpulse; - } - - m_listener->PostSolve(c, &impulse); - } -} diff --git a/3rdparty/box2d/src/dynamics/b2_island.h b/3rdparty/box2d/src/dynamics/b2_island.h deleted file mode 100644 index 2e28a357b047..000000000000 --- a/3rdparty/box2d/src/dynamics/b2_island.h +++ /dev/null @@ -1,97 +0,0 @@ -// MIT License - -// Copyright (c) 2019 Erin Catto - -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: - -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. - -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -#ifndef B2_ISLAND_H -#define B2_ISLAND_H - -#include "box2d/b2_body.h" -#include "box2d/b2_math.h" -#include "box2d/b2_time_step.h" - -class b2Contact; -class b2Joint; -class b2StackAllocator; -class b2ContactListener; -struct b2ContactVelocityConstraint; -struct b2Profile; - -/// This is an internal class. -class b2Island -{ -public: - b2Island(int32 bodyCapacity, int32 contactCapacity, int32 jointCapacity, - b2StackAllocator* allocator, b2ContactListener* listener); - ~b2Island(); - - void Clear() - { - m_bodyCount = 0; - m_contactCount = 0; - m_jointCount = 0; - } - - void Solve(b2Profile* profile, const b2TimeStep& step, const b2Vec2& gravity, bool allowSleep); - - void SolveTOI(const b2TimeStep& subStep, int32 toiIndexA, int32 toiIndexB); - - void Add(b2Body* body) - { - b2Assert(m_bodyCount < m_bodyCapacity); - body->m_islandIndex = m_bodyCount; - m_bodies[m_bodyCount] = body; - ++m_bodyCount; - } - - void Add(b2Contact* contact) - { - b2Assert(m_contactCount < m_contactCapacity); - m_contacts[m_contactCount++] = contact; - } - - void Add(b2Joint* joint) - { - b2Assert(m_jointCount < m_jointCapacity); - m_joints[m_jointCount++] = joint; - } - - void Report(const b2ContactVelocityConstraint* constraints); - - b2StackAllocator* m_allocator; - b2ContactListener* m_listener; - - b2Body** m_bodies; - b2Contact** m_contacts; - b2Joint** m_joints; - - b2Position* m_positions; - b2Velocity* m_velocities; - - int32 m_bodyCount; - int32 m_jointCount; - int32 m_contactCount; - - int32 m_bodyCapacity; - int32 m_contactCapacity; - int32 m_jointCapacity; -}; - -#endif diff --git a/3rdparty/box2d/src/dynamics/b2_joint.cpp b/3rdparty/box2d/src/dynamics/b2_joint.cpp deleted file mode 100644 index 41addbbb2dda..000000000000 --- a/3rdparty/box2d/src/dynamics/b2_joint.cpp +++ /dev/null @@ -1,301 +0,0 @@ -// MIT License - -// Copyright (c) 2019 Erin Catto - -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: - -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. - -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -#include "box2d/b2_block_allocator.h" -#include "box2d/b2_body.h" -#include "box2d/b2_distance_joint.h" -#include "box2d/b2_draw.h" -#include "box2d/b2_friction_joint.h" -#include "box2d/b2_gear_joint.h" -#include "box2d/b2_motor_joint.h" -#include "box2d/b2_mouse_joint.h" -#include "box2d/b2_prismatic_joint.h" -#include "box2d/b2_pulley_joint.h" -#include "box2d/b2_revolute_joint.h" -#include "box2d/b2_weld_joint.h" -#include "box2d/b2_wheel_joint.h" -#include "box2d/b2_world.h" - -#include - -void b2LinearStiffness(float& stiffness, float& damping, - float frequencyHertz, float dampingRatio, - const b2Body* bodyA, const b2Body* bodyB) -{ - float massA = bodyA->GetMass(); - float massB = bodyB->GetMass(); - float mass; - if (massA > 0.0f && massB > 0.0f) - { - mass = massA * massB / (massA + massB); - } - else if (massA > 0.0f) - { - mass = massA; - } - else - { - mass = massB; - } - - float omega = 2.0f * b2_pi * frequencyHertz; - stiffness = mass * omega * omega; - damping = 2.0f * mass * dampingRatio * omega; -} - -void b2AngularStiffness(float& stiffness, float& damping, - float frequencyHertz, float dampingRatio, - const b2Body* bodyA, const b2Body* bodyB) -{ - float IA = bodyA->GetInertia(); - float IB = bodyB->GetInertia(); - float I; - if (IA > 0.0f && IB > 0.0f) - { - I = IA * IB / (IA + IB); - } - else if (IA > 0.0f) - { - I = IA; - } - else - { - I = IB; - } - - float omega = 2.0f * b2_pi * frequencyHertz; - stiffness = I * omega * omega; - damping = 2.0f * I * dampingRatio * omega; -} - -b2Joint* b2Joint::Create(const b2JointDef* def, b2BlockAllocator* allocator) -{ - b2Joint* joint = nullptr; - - switch (def->type) - { - case e_distanceJoint: - { - void* mem = allocator->Allocate(sizeof(b2DistanceJoint)); - joint = new (mem) b2DistanceJoint(static_cast(def)); - } - break; - - case e_mouseJoint: - { - void* mem = allocator->Allocate(sizeof(b2MouseJoint)); - joint = new (mem) b2MouseJoint(static_cast(def)); - } - break; - - case e_prismaticJoint: - { - void* mem = allocator->Allocate(sizeof(b2PrismaticJoint)); - joint = new (mem) b2PrismaticJoint(static_cast(def)); - } - break; - - case e_revoluteJoint: - { - void* mem = allocator->Allocate(sizeof(b2RevoluteJoint)); - joint = new (mem) b2RevoluteJoint(static_cast(def)); - } - break; - - case e_pulleyJoint: - { - void* mem = allocator->Allocate(sizeof(b2PulleyJoint)); - joint = new (mem) b2PulleyJoint(static_cast(def)); - } - break; - - case e_gearJoint: - { - void* mem = allocator->Allocate(sizeof(b2GearJoint)); - joint = new (mem) b2GearJoint(static_cast(def)); - } - break; - - case e_wheelJoint: - { - void* mem = allocator->Allocate(sizeof(b2WheelJoint)); - joint = new (mem) b2WheelJoint(static_cast(def)); - } - break; - - case e_weldJoint: - { - void* mem = allocator->Allocate(sizeof(b2WeldJoint)); - joint = new (mem) b2WeldJoint(static_cast(def)); - } - break; - - case e_frictionJoint: - { - void* mem = allocator->Allocate(sizeof(b2FrictionJoint)); - joint = new (mem) b2FrictionJoint(static_cast(def)); - } - break; - - case e_motorJoint: - { - void* mem = allocator->Allocate(sizeof(b2MotorJoint)); - joint = new (mem) b2MotorJoint(static_cast(def)); - } - break; - - default: - b2Assert(false); - break; - } - - return joint; -} - -void b2Joint::Destroy(b2Joint* joint, b2BlockAllocator* allocator) -{ - joint->~b2Joint(); - switch (joint->m_type) - { - case e_distanceJoint: - allocator->Free(joint, sizeof(b2DistanceJoint)); - break; - - case e_mouseJoint: - allocator->Free(joint, sizeof(b2MouseJoint)); - break; - - case e_prismaticJoint: - allocator->Free(joint, sizeof(b2PrismaticJoint)); - break; - - case e_revoluteJoint: - allocator->Free(joint, sizeof(b2RevoluteJoint)); - break; - - case e_pulleyJoint: - allocator->Free(joint, sizeof(b2PulleyJoint)); - break; - - case e_gearJoint: - allocator->Free(joint, sizeof(b2GearJoint)); - break; - - case e_wheelJoint: - allocator->Free(joint, sizeof(b2WheelJoint)); - break; - - case e_weldJoint: - allocator->Free(joint, sizeof(b2WeldJoint)); - break; - - case e_frictionJoint: - allocator->Free(joint, sizeof(b2FrictionJoint)); - break; - - case e_motorJoint: - allocator->Free(joint, sizeof(b2MotorJoint)); - break; - - default: - b2Assert(false); - break; - } -} - -b2Joint::b2Joint(const b2JointDef* def) -{ - b2Assert(def->bodyA != def->bodyB); - - m_type = def->type; - m_prev = nullptr; - m_next = nullptr; - m_bodyA = def->bodyA; - m_bodyB = def->bodyB; - m_index = 0; - m_collideConnected = def->collideConnected; - m_islandFlag = false; - m_userData = def->userData; - - m_edgeA.joint = nullptr; - m_edgeA.other = nullptr; - m_edgeA.prev = nullptr; - m_edgeA.next = nullptr; - - m_edgeB.joint = nullptr; - m_edgeB.other = nullptr; - m_edgeB.prev = nullptr; - m_edgeB.next = nullptr; -} - -bool b2Joint::IsEnabled() const -{ - return m_bodyA->IsEnabled() && m_bodyB->IsEnabled(); -} - -void b2Joint::Draw(b2Draw* draw) const -{ - const b2Transform& xf1 = m_bodyA->GetTransform(); - const b2Transform& xf2 = m_bodyB->GetTransform(); - b2Vec2 x1 = xf1.p; - b2Vec2 x2 = xf2.p; - b2Vec2 p1 = GetAnchorA(); - b2Vec2 p2 = GetAnchorB(); - - b2Color color(0.5f, 0.8f, 0.8f); - - switch (m_type) - { - case e_distanceJoint: - draw->DrawSegment(p1, p2, color); - break; - - case e_pulleyJoint: - { - b2PulleyJoint* pulley = (b2PulleyJoint*)this; - b2Vec2 s1 = pulley->GetGroundAnchorA(); - b2Vec2 s2 = pulley->GetGroundAnchorB(); - draw->DrawSegment(s1, p1, color); - draw->DrawSegment(s2, p2, color); - draw->DrawSegment(s1, s2, color); - } - break; - - case e_mouseJoint: - { - b2Color c; - c.Set(0.0f, 1.0f, 0.0f); - draw->DrawPoint(p1, 4.0f, c); - draw->DrawPoint(p2, 4.0f, c); - - c.Set(0.8f, 0.8f, 0.8f); - draw->DrawSegment(p1, p2, c); - - } - break; - - default: - draw->DrawSegment(x1, p1, color); - draw->DrawSegment(p1, p2, color); - draw->DrawSegment(x2, p2, color); - } -} diff --git a/3rdparty/box2d/src/dynamics/b2_motor_joint.cpp b/3rdparty/box2d/src/dynamics/b2_motor_joint.cpp deleted file mode 100644 index 6e0b07538099..000000000000 --- a/3rdparty/box2d/src/dynamics/b2_motor_joint.cpp +++ /dev/null @@ -1,311 +0,0 @@ -// MIT License - -// Copyright (c) 2019 Erin Catto - -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: - -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. - -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -#include "box2d/b2_body.h" -#include "box2d/b2_motor_joint.h" -#include "box2d/b2_time_step.h" - -// Point-to-point constraint -// Cdot = v2 - v1 -// = v2 + cross(w2, r2) - v1 - cross(w1, r1) -// J = [-I -r1_skew I r2_skew ] -// Identity used: -// w k % (rx i + ry j) = w * (-ry i + rx j) -// -// r1 = offset - c1 -// r2 = -c2 - -// Angle constraint -// Cdot = w2 - w1 -// J = [0 0 -1 0 0 1] -// K = invI1 + invI2 - -void b2MotorJointDef::Initialize(b2Body* bA, b2Body* bB) -{ - bodyA = bA; - bodyB = bB; - b2Vec2 xB = bodyB->GetPosition(); - linearOffset = bodyA->GetLocalPoint(xB); - - float angleA = bodyA->GetAngle(); - float angleB = bodyB->GetAngle(); - angularOffset = angleB - angleA; -} - -b2MotorJoint::b2MotorJoint(const b2MotorJointDef* def) -: b2Joint(def) -{ - m_linearOffset = def->linearOffset; - m_angularOffset = def->angularOffset; - - m_linearImpulse.SetZero(); - m_angularImpulse = 0.0f; - - m_maxForce = def->maxForce; - m_maxTorque = def->maxTorque; - m_correctionFactor = def->correctionFactor; -} - -void b2MotorJoint::InitVelocityConstraints(const b2SolverData& data) -{ - m_indexA = m_bodyA->m_islandIndex; - m_indexB = m_bodyB->m_islandIndex; - m_localCenterA = m_bodyA->m_sweep.localCenter; - m_localCenterB = m_bodyB->m_sweep.localCenter; - m_invMassA = m_bodyA->m_invMass; - m_invMassB = m_bodyB->m_invMass; - m_invIA = m_bodyA->m_invI; - m_invIB = m_bodyB->m_invI; - - b2Vec2 cA = data.positions[m_indexA].c; - float aA = data.positions[m_indexA].a; - b2Vec2 vA = data.velocities[m_indexA].v; - float wA = data.velocities[m_indexA].w; - - b2Vec2 cB = data.positions[m_indexB].c; - float aB = data.positions[m_indexB].a; - b2Vec2 vB = data.velocities[m_indexB].v; - float wB = data.velocities[m_indexB].w; - - b2Rot qA(aA), qB(aB); - - // Compute the effective mass matrix. - m_rA = b2Mul(qA, m_linearOffset - m_localCenterA); - m_rB = b2Mul(qB, -m_localCenterB); - - // J = [-I -r1_skew I r2_skew] - // r_skew = [-ry; rx] - - // Matlab - // K = [ mA+r1y^2*iA+mB+r2y^2*iB, -r1y*iA*r1x-r2y*iB*r2x, -r1y*iA-r2y*iB] - // [ -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB, r1x*iA+r2x*iB] - // [ -r1y*iA-r2y*iB, r1x*iA+r2x*iB, iA+iB] - - float mA = m_invMassA, mB = m_invMassB; - float iA = m_invIA, iB = m_invIB; - - // Upper 2 by 2 of K for point to point - b2Mat22 K; - K.ex.x = mA + mB + iA * m_rA.y * m_rA.y + iB * m_rB.y * m_rB.y; - K.ex.y = -iA * m_rA.x * m_rA.y - iB * m_rB.x * m_rB.y; - K.ey.x = K.ex.y; - K.ey.y = mA + mB + iA * m_rA.x * m_rA.x + iB * m_rB.x * m_rB.x; - - m_linearMass = K.GetInverse(); - - m_angularMass = iA + iB; - if (m_angularMass > 0.0f) - { - m_angularMass = 1.0f / m_angularMass; - } - - m_linearError = cB + m_rB - cA - m_rA; - m_angularError = aB - aA - m_angularOffset; - - if (data.step.warmStarting) - { - // Scale impulses to support a variable time step. - m_linearImpulse *= data.step.dtRatio; - m_angularImpulse *= data.step.dtRatio; - - b2Vec2 P(m_linearImpulse.x, m_linearImpulse.y); - vA -= mA * P; - wA -= iA * (b2Cross(m_rA, P) + m_angularImpulse); - vB += mB * P; - wB += iB * (b2Cross(m_rB, P) + m_angularImpulse); - } - else - { - m_linearImpulse.SetZero(); - m_angularImpulse = 0.0f; - } - - data.velocities[m_indexA].v = vA; - data.velocities[m_indexA].w = wA; - data.velocities[m_indexB].v = vB; - data.velocities[m_indexB].w = wB; -} - -void b2MotorJoint::SolveVelocityConstraints(const b2SolverData& data) -{ - b2Vec2 vA = data.velocities[m_indexA].v; - float wA = data.velocities[m_indexA].w; - b2Vec2 vB = data.velocities[m_indexB].v; - float wB = data.velocities[m_indexB].w; - - float mA = m_invMassA, mB = m_invMassB; - float iA = m_invIA, iB = m_invIB; - - float h = data.step.dt; - float inv_h = data.step.inv_dt; - - // Solve angular friction - { - float Cdot = wB - wA + inv_h * m_correctionFactor * m_angularError; - float impulse = -m_angularMass * Cdot; - - float oldImpulse = m_angularImpulse; - float maxImpulse = h * m_maxTorque; - m_angularImpulse = b2Clamp(m_angularImpulse + impulse, -maxImpulse, maxImpulse); - impulse = m_angularImpulse - oldImpulse; - - wA -= iA * impulse; - wB += iB * impulse; - } - - // Solve linear friction - { - b2Vec2 Cdot = vB + b2Cross(wB, m_rB) - vA - b2Cross(wA, m_rA) + inv_h * m_correctionFactor * m_linearError; - - b2Vec2 impulse = -b2Mul(m_linearMass, Cdot); - b2Vec2 oldImpulse = m_linearImpulse; - m_linearImpulse += impulse; - - float maxImpulse = h * m_maxForce; - - if (m_linearImpulse.LengthSquared() > maxImpulse * maxImpulse) - { - m_linearImpulse.Normalize(); - m_linearImpulse *= maxImpulse; - } - - impulse = m_linearImpulse - oldImpulse; - - vA -= mA * impulse; - wA -= iA * b2Cross(m_rA, impulse); - - vB += mB * impulse; - wB += iB * b2Cross(m_rB, impulse); - } - - data.velocities[m_indexA].v = vA; - data.velocities[m_indexA].w = wA; - data.velocities[m_indexB].v = vB; - data.velocities[m_indexB].w = wB; -} - -bool b2MotorJoint::SolvePositionConstraints(const b2SolverData& data) -{ - B2_NOT_USED(data); - - return true; -} - -b2Vec2 b2MotorJoint::GetAnchorA() const -{ - return m_bodyA->GetPosition(); -} - -b2Vec2 b2MotorJoint::GetAnchorB() const -{ - return m_bodyB->GetPosition(); -} - -b2Vec2 b2MotorJoint::GetReactionForce(float inv_dt) const -{ - return inv_dt * m_linearImpulse; -} - -float b2MotorJoint::GetReactionTorque(float inv_dt) const -{ - return inv_dt * m_angularImpulse; -} - -void b2MotorJoint::SetMaxForce(float force) -{ - b2Assert(b2IsValid(force) && force >= 0.0f); - m_maxForce = force; -} - -float b2MotorJoint::GetMaxForce() const -{ - return m_maxForce; -} - -void b2MotorJoint::SetMaxTorque(float torque) -{ - b2Assert(b2IsValid(torque) && torque >= 0.0f); - m_maxTorque = torque; -} - -float b2MotorJoint::GetMaxTorque() const -{ - return m_maxTorque; -} - -void b2MotorJoint::SetCorrectionFactor(float factor) -{ - b2Assert(b2IsValid(factor) && 0.0f <= factor && factor <= 1.0f); - m_correctionFactor = factor; -} - -float b2MotorJoint::GetCorrectionFactor() const -{ - return m_correctionFactor; -} - -void b2MotorJoint::SetLinearOffset(const b2Vec2& linearOffset) -{ - if (linearOffset.x != m_linearOffset.x || linearOffset.y != m_linearOffset.y) - { - m_bodyA->SetAwake(true); - m_bodyB->SetAwake(true); - m_linearOffset = linearOffset; - } -} - -const b2Vec2& b2MotorJoint::GetLinearOffset() const -{ - return m_linearOffset; -} - -void b2MotorJoint::SetAngularOffset(float angularOffset) -{ - if (angularOffset != m_angularOffset) - { - m_bodyA->SetAwake(true); - m_bodyB->SetAwake(true); - m_angularOffset = angularOffset; - } -} - -float b2MotorJoint::GetAngularOffset() const -{ - return m_angularOffset; -} - -void b2MotorJoint::Dump() -{ - int32 indexA = m_bodyA->m_islandIndex; - int32 indexB = m_bodyB->m_islandIndex; - - b2Dump(" b2MotorJointDef jd;\n"); - b2Dump(" jd.bodyA = bodies[%d];\n", indexA); - b2Dump(" jd.bodyB = bodies[%d];\n", indexB); - b2Dump(" jd.collideConnected = bool(%d);\n", m_collideConnected); - b2Dump(" jd.linearOffset.Set(%.9g, %.9g);\n", m_linearOffset.x, m_linearOffset.y); - b2Dump(" jd.angularOffset = %.9g;\n", m_angularOffset); - b2Dump(" jd.maxForce = %.9g;\n", m_maxForce); - b2Dump(" jd.maxTorque = %.9g;\n", m_maxTorque); - b2Dump(" jd.correctionFactor = %.9g;\n", m_correctionFactor); - b2Dump(" joints[%d] = m_world->CreateJoint(&jd);\n", m_index); -} diff --git a/3rdparty/box2d/src/dynamics/b2_mouse_joint.cpp b/3rdparty/box2d/src/dynamics/b2_mouse_joint.cpp deleted file mode 100644 index dbc214fae749..000000000000 --- a/3rdparty/box2d/src/dynamics/b2_mouse_joint.cpp +++ /dev/null @@ -1,190 +0,0 @@ -// MIT License - -// Copyright (c) 2019 Erin Catto - -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: - -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. - -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -#include "box2d/b2_body.h" -#include "box2d/b2_mouse_joint.h" -#include "box2d/b2_time_step.h" - -// p = attached point, m = mouse point -// C = p - m -// Cdot = v -// = v + cross(w, r) -// J = [I r_skew] -// Identity used: -// w k % (rx i + ry j) = w * (-ry i + rx j) - -b2MouseJoint::b2MouseJoint(const b2MouseJointDef* def) -: b2Joint(def) -{ - m_targetA = def->target; - m_localAnchorB = b2MulT(m_bodyB->GetTransform(), m_targetA); - m_maxForce = def->maxForce; - m_stiffness = def->stiffness; - m_damping = def->damping; - - m_impulse.SetZero(); - m_beta = 0.0f; - m_gamma = 0.0f; -} - -void b2MouseJoint::SetTarget(const b2Vec2& target) -{ - if (target != m_targetA) - { - m_bodyB->SetAwake(true); - m_targetA = target; - } -} - -const b2Vec2& b2MouseJoint::GetTarget() const -{ - return m_targetA; -} - -void b2MouseJoint::SetMaxForce(float force) -{ - m_maxForce = force; -} - -float b2MouseJoint::GetMaxForce() const -{ - return m_maxForce; -} - -void b2MouseJoint::InitVelocityConstraints(const b2SolverData& data) -{ - m_indexB = m_bodyB->m_islandIndex; - m_localCenterB = m_bodyB->m_sweep.localCenter; - m_invMassB = m_bodyB->m_invMass; - m_invIB = m_bodyB->m_invI; - - b2Vec2 cB = data.positions[m_indexB].c; - float aB = data.positions[m_indexB].a; - b2Vec2 vB = data.velocities[m_indexB].v; - float wB = data.velocities[m_indexB].w; - - b2Rot qB(aB); - - float d = m_damping; - float k = m_stiffness; - - // magic formulas - // gamma has units of inverse mass. - // beta has units of inverse time. - float h = data.step.dt; - m_gamma = h * (d + h * k); - if (m_gamma != 0.0f) - { - m_gamma = 1.0f / m_gamma; - } - m_beta = h * k * m_gamma; - - // Compute the effective mass matrix. - m_rB = b2Mul(qB, m_localAnchorB - m_localCenterB); - - // K = [(1/m1 + 1/m2) * eye(2) - skew(r1) * invI1 * skew(r1) - skew(r2) * invI2 * skew(r2)] - // = [1/m1+1/m2 0 ] + invI1 * [r1.y*r1.y -r1.x*r1.y] + invI2 * [r1.y*r1.y -r1.x*r1.y] - // [ 0 1/m1+1/m2] [-r1.x*r1.y r1.x*r1.x] [-r1.x*r1.y r1.x*r1.x] - b2Mat22 K; - K.ex.x = m_invMassB + m_invIB * m_rB.y * m_rB.y + m_gamma; - K.ex.y = -m_invIB * m_rB.x * m_rB.y; - K.ey.x = K.ex.y; - K.ey.y = m_invMassB + m_invIB * m_rB.x * m_rB.x + m_gamma; - - m_mass = K.GetInverse(); - - m_C = cB + m_rB - m_targetA; - m_C *= m_beta; - - // Cheat with some damping - wB *= b2Max(0.0f, 1.0f - 0.02f * (60.0f * data.step.dt)); - - if (data.step.warmStarting) - { - m_impulse *= data.step.dtRatio; - vB += m_invMassB * m_impulse; - wB += m_invIB * b2Cross(m_rB, m_impulse); - } - else - { - m_impulse.SetZero(); - } - - data.velocities[m_indexB].v = vB; - data.velocities[m_indexB].w = wB; -} - -void b2MouseJoint::SolveVelocityConstraints(const b2SolverData& data) -{ - b2Vec2 vB = data.velocities[m_indexB].v; - float wB = data.velocities[m_indexB].w; - - // Cdot = v + cross(w, r) - b2Vec2 Cdot = vB + b2Cross(wB, m_rB); - b2Vec2 impulse = b2Mul(m_mass, -(Cdot + m_C + m_gamma * m_impulse)); - - b2Vec2 oldImpulse = m_impulse; - m_impulse += impulse; - float maxImpulse = data.step.dt * m_maxForce; - if (m_impulse.LengthSquared() > maxImpulse * maxImpulse) - { - m_impulse *= maxImpulse / m_impulse.Length(); - } - impulse = m_impulse - oldImpulse; - - vB += m_invMassB * impulse; - wB += m_invIB * b2Cross(m_rB, impulse); - - data.velocities[m_indexB].v = vB; - data.velocities[m_indexB].w = wB; -} - -bool b2MouseJoint::SolvePositionConstraints(const b2SolverData& data) -{ - B2_NOT_USED(data); - return true; -} - -b2Vec2 b2MouseJoint::GetAnchorA() const -{ - return m_targetA; -} - -b2Vec2 b2MouseJoint::GetAnchorB() const -{ - return m_bodyB->GetWorldPoint(m_localAnchorB); -} - -b2Vec2 b2MouseJoint::GetReactionForce(float inv_dt) const -{ - return inv_dt * m_impulse; -} - -float b2MouseJoint::GetReactionTorque(float inv_dt) const -{ - return inv_dt * 0.0f; -} - -void b2MouseJoint::ShiftOrigin(const b2Vec2& newOrigin) -{ - m_targetA -= newOrigin; -} diff --git a/3rdparty/box2d/src/dynamics/b2_polygon_circle_contact.cpp b/3rdparty/box2d/src/dynamics/b2_polygon_circle_contact.cpp deleted file mode 100644 index e4f34f582f74..000000000000 --- a/3rdparty/box2d/src/dynamics/b2_polygon_circle_contact.cpp +++ /dev/null @@ -1,54 +0,0 @@ -// MIT License - -// Copyright (c) 2019 Erin Catto - -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: - -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. - -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -#include "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/3rdparty/box2d/src/dynamics/b2_polygon_circle_contact.h b/3rdparty/box2d/src/dynamics/b2_polygon_circle_contact.h deleted file mode 100644 index 6ae542510cb4..000000000000 --- a/3rdparty/box2d/src/dynamics/b2_polygon_circle_contact.h +++ /dev/null @@ -1,42 +0,0 @@ -// MIT License - -// Copyright (c) 2019 Erin Catto - -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: - -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. - -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -#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/3rdparty/box2d/src/dynamics/b2_polygon_contact.cpp b/3rdparty/box2d/src/dynamics/b2_polygon_contact.cpp deleted file mode 100644 index e92a9e806318..000000000000 --- a/3rdparty/box2d/src/dynamics/b2_polygon_contact.cpp +++ /dev/null @@ -1,57 +0,0 @@ -// MIT License - -// Copyright (c) 2019 Erin Catto - -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: - -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. - -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -#include "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/3rdparty/box2d/src/dynamics/b2_polygon_contact.h b/3rdparty/box2d/src/dynamics/b2_polygon_contact.h deleted file mode 100644 index 0516cb0ec623..000000000000 --- a/3rdparty/box2d/src/dynamics/b2_polygon_contact.h +++ /dev/null @@ -1,43 +0,0 @@ -// MIT License - -// Copyright (c) 2019 Erin Catto - -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: - -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. - -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -#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/3rdparty/box2d/src/dynamics/b2_prismatic_joint.cpp b/3rdparty/box2d/src/dynamics/b2_prismatic_joint.cpp deleted file mode 100644 index 00e77690eb6b..000000000000 --- a/3rdparty/box2d/src/dynamics/b2_prismatic_joint.cpp +++ /dev/null @@ -1,643 +0,0 @@ -// MIT License - -// Copyright (c) 2019 Erin Catto - -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: - -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. - -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -#include "box2d/b2_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/3rdparty/box2d/src/dynamics/b2_pulley_joint.cpp b/3rdparty/box2d/src/dynamics/b2_pulley_joint.cpp deleted file mode 100644 index 099e57e49d91..000000000000 --- a/3rdparty/box2d/src/dynamics/b2_pulley_joint.cpp +++ /dev/null @@ -1,352 +0,0 @@ -// MIT License - -// Copyright (c) 2019 Erin Catto - -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: - -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. - -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -#include "box2d/b2_body.h" -#include "box2d/b2_pulley_joint.h" -#include "box2d/b2_time_step.h" - -// Pulley: -// length1 = norm(p1 - s1) -// length2 = norm(p2 - s2) -// C0 = (length1 + ratio * length2)_initial -// C = C0 - (length1 + ratio * length2) -// u1 = (p1 - s1) / norm(p1 - s1) -// u2 = (p2 - s2) / norm(p2 - s2) -// Cdot = -dot(u1, v1 + cross(w1, r1)) - ratio * dot(u2, v2 + cross(w2, r2)) -// J = -[u1 cross(r1, u1) ratio * u2 ratio * cross(r2, u2)] -// K = J * invM * JT -// = invMass1 + invI1 * cross(r1, u1)^2 + ratio^2 * (invMass2 + invI2 * cross(r2, u2)^2) - -void b2PulleyJointDef::Initialize(b2Body* bA, b2Body* bB, - const b2Vec2& groundA, const b2Vec2& groundB, - const b2Vec2& anchorA, const b2Vec2& anchorB, - float r) -{ - bodyA = bA; - bodyB = bB; - groundAnchorA = groundA; - groundAnchorB = groundB; - localAnchorA = bodyA->GetLocalPoint(anchorA); - localAnchorB = bodyB->GetLocalPoint(anchorB); - b2Vec2 dA = anchorA - groundA; - lengthA = dA.Length(); - b2Vec2 dB = anchorB - groundB; - lengthB = dB.Length(); - ratio = r; - b2Assert(ratio > b2_epsilon); -} - -b2PulleyJoint::b2PulleyJoint(const b2PulleyJointDef* def) -: b2Joint(def) -{ - m_groundAnchorA = def->groundAnchorA; - m_groundAnchorB = def->groundAnchorB; - m_localAnchorA = def->localAnchorA; - m_localAnchorB = def->localAnchorB; - - m_lengthA = def->lengthA; - m_lengthB = def->lengthB; - - b2Assert(def->ratio != 0.0f); - m_ratio = def->ratio; - - m_constant = def->lengthA + m_ratio * def->lengthB; - - m_impulse = 0.0f; -} - -void b2PulleyJoint::InitVelocityConstraints(const b2SolverData& data) -{ - m_indexA = m_bodyA->m_islandIndex; - m_indexB = m_bodyB->m_islandIndex; - m_localCenterA = m_bodyA->m_sweep.localCenter; - m_localCenterB = m_bodyB->m_sweep.localCenter; - m_invMassA = m_bodyA->m_invMass; - m_invMassB = m_bodyB->m_invMass; - m_invIA = m_bodyA->m_invI; - m_invIB = m_bodyB->m_invI; - - b2Vec2 cA = data.positions[m_indexA].c; - float aA = data.positions[m_indexA].a; - b2Vec2 vA = data.velocities[m_indexA].v; - float wA = data.velocities[m_indexA].w; - - b2Vec2 cB = data.positions[m_indexB].c; - float aB = data.positions[m_indexB].a; - b2Vec2 vB = data.velocities[m_indexB].v; - float wB = data.velocities[m_indexB].w; - - b2Rot qA(aA), qB(aB); - - m_rA = b2Mul(qA, m_localAnchorA - m_localCenterA); - m_rB = b2Mul(qB, m_localAnchorB - m_localCenterB); - - // Get the pulley axes. - m_uA = cA + m_rA - m_groundAnchorA; - m_uB = cB + m_rB - m_groundAnchorB; - - float lengthA = m_uA.Length(); - float lengthB = m_uB.Length(); - - if (lengthA > 10.0f * b2_linearSlop) - { - m_uA *= 1.0f / lengthA; - } - else - { - m_uA.SetZero(); - } - - if (lengthB > 10.0f * b2_linearSlop) - { - m_uB *= 1.0f / lengthB; - } - else - { - m_uB.SetZero(); - } - - // Compute effective mass. - float ruA = b2Cross(m_rA, m_uA); - float ruB = b2Cross(m_rB, m_uB); - - float mA = m_invMassA + m_invIA * ruA * ruA; - float mB = m_invMassB + m_invIB * ruB * ruB; - - m_mass = mA + m_ratio * m_ratio * mB; - - if (m_mass > 0.0f) - { - m_mass = 1.0f / m_mass; - } - - if (data.step.warmStarting) - { - // Scale impulses to support variable time steps. - m_impulse *= data.step.dtRatio; - - // Warm starting. - b2Vec2 PA = -(m_impulse) * m_uA; - b2Vec2 PB = (-m_ratio * m_impulse) * m_uB; - - vA += m_invMassA * PA; - wA += m_invIA * b2Cross(m_rA, PA); - vB += m_invMassB * PB; - wB += m_invIB * b2Cross(m_rB, PB); - } - else - { - m_impulse = 0.0f; - } - - data.velocities[m_indexA].v = vA; - data.velocities[m_indexA].w = wA; - data.velocities[m_indexB].v = vB; - data.velocities[m_indexB].w = wB; -} - -void b2PulleyJoint::SolveVelocityConstraints(const b2SolverData& data) -{ - b2Vec2 vA = data.velocities[m_indexA].v; - float wA = data.velocities[m_indexA].w; - b2Vec2 vB = data.velocities[m_indexB].v; - float wB = data.velocities[m_indexB].w; - - b2Vec2 vpA = vA + b2Cross(wA, m_rA); - b2Vec2 vpB = vB + b2Cross(wB, m_rB); - - float Cdot = -b2Dot(m_uA, vpA) - m_ratio * b2Dot(m_uB, vpB); - float impulse = -m_mass * Cdot; - m_impulse += impulse; - - b2Vec2 PA = -impulse * m_uA; - b2Vec2 PB = -m_ratio * impulse * m_uB; - vA += m_invMassA * PA; - wA += m_invIA * b2Cross(m_rA, PA); - vB += m_invMassB * PB; - wB += m_invIB * b2Cross(m_rB, PB); - - data.velocities[m_indexA].v = vA; - data.velocities[m_indexA].w = wA; - data.velocities[m_indexB].v = vB; - data.velocities[m_indexB].w = wB; -} - -bool b2PulleyJoint::SolvePositionConstraints(const b2SolverData& data) -{ - b2Vec2 cA = data.positions[m_indexA].c; - float aA = data.positions[m_indexA].a; - b2Vec2 cB = data.positions[m_indexB].c; - float aB = data.positions[m_indexB].a; - - b2Rot qA(aA), qB(aB); - - b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_localCenterA); - b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_localCenterB); - - // Get the pulley axes. - b2Vec2 uA = cA + rA - m_groundAnchorA; - b2Vec2 uB = cB + rB - m_groundAnchorB; - - float lengthA = uA.Length(); - float lengthB = uB.Length(); - - if (lengthA > 10.0f * b2_linearSlop) - { - uA *= 1.0f / lengthA; - } - else - { - uA.SetZero(); - } - - if (lengthB > 10.0f * b2_linearSlop) - { - uB *= 1.0f / lengthB; - } - else - { - uB.SetZero(); - } - - // Compute effective mass. - float ruA = b2Cross(rA, uA); - float ruB = b2Cross(rB, uB); - - float mA = m_invMassA + m_invIA * ruA * ruA; - float mB = m_invMassB + m_invIB * ruB * ruB; - - float mass = mA + m_ratio * m_ratio * mB; - - if (mass > 0.0f) - { - mass = 1.0f / mass; - } - - float C = m_constant - lengthA - m_ratio * lengthB; - float linearError = b2Abs(C); - - float impulse = -mass * C; - - b2Vec2 PA = -impulse * uA; - b2Vec2 PB = -m_ratio * impulse * uB; - - cA += m_invMassA * PA; - aA += m_invIA * b2Cross(rA, PA); - cB += m_invMassB * PB; - aB += m_invIB * b2Cross(rB, PB); - - data.positions[m_indexA].c = cA; - data.positions[m_indexA].a = aA; - data.positions[m_indexB].c = cB; - data.positions[m_indexB].a = aB; - - return linearError < b2_linearSlop; -} - -b2Vec2 b2PulleyJoint::GetAnchorA() const -{ - return m_bodyA->GetWorldPoint(m_localAnchorA); -} - -b2Vec2 b2PulleyJoint::GetAnchorB() const -{ - return m_bodyB->GetWorldPoint(m_localAnchorB); -} - -b2Vec2 b2PulleyJoint::GetReactionForce(float inv_dt) const -{ - b2Vec2 P = m_impulse * m_uB; - return inv_dt * P; -} - -float b2PulleyJoint::GetReactionTorque(float inv_dt) const -{ - B2_NOT_USED(inv_dt); - return 0.0f; -} - -b2Vec2 b2PulleyJoint::GetGroundAnchorA() const -{ - return m_groundAnchorA; -} - -b2Vec2 b2PulleyJoint::GetGroundAnchorB() const -{ - return m_groundAnchorB; -} - -float b2PulleyJoint::GetLengthA() const -{ - return m_lengthA; -} - -float b2PulleyJoint::GetLengthB() const -{ - return m_lengthB; -} - -float b2PulleyJoint::GetRatio() const -{ - return m_ratio; -} - -float b2PulleyJoint::GetCurrentLengthA() const -{ - b2Vec2 p = m_bodyA->GetWorldPoint(m_localAnchorA); - b2Vec2 s = m_groundAnchorA; - b2Vec2 d = p - s; - return d.Length(); -} - -float b2PulleyJoint::GetCurrentLengthB() const -{ - b2Vec2 p = m_bodyB->GetWorldPoint(m_localAnchorB); - b2Vec2 s = m_groundAnchorB; - b2Vec2 d = p - s; - return d.Length(); -} - -void b2PulleyJoint::Dump() -{ - int32 indexA = m_bodyA->m_islandIndex; - int32 indexB = m_bodyB->m_islandIndex; - - b2Dump(" b2PulleyJointDef jd;\n"); - b2Dump(" jd.bodyA = bodies[%d];\n", indexA); - b2Dump(" jd.bodyB = bodies[%d];\n", indexB); - b2Dump(" jd.collideConnected = bool(%d);\n", m_collideConnected); - b2Dump(" jd.groundAnchorA.Set(%.9g, %.9g);\n", m_groundAnchorA.x, m_groundAnchorA.y); - b2Dump(" jd.groundAnchorB.Set(%.9g, %.9g);\n", m_groundAnchorB.x, m_groundAnchorB.y); - b2Dump(" jd.localAnchorA.Set(%.9g, %.9g);\n", m_localAnchorA.x, m_localAnchorA.y); - b2Dump(" jd.localAnchorB.Set(%.9g, %.9g);\n", m_localAnchorB.x, m_localAnchorB.y); - b2Dump(" jd.lengthA = %.9g;\n", m_lengthA); - b2Dump(" jd.lengthB = %.9g;\n", m_lengthB); - b2Dump(" jd.ratio = %.9g;\n", m_ratio); - b2Dump(" joints[%d] = m_world->CreateJoint(&jd);\n", m_index); -} - -void b2PulleyJoint::ShiftOrigin(const b2Vec2& newOrigin) -{ - m_groundAnchorA -= newOrigin; - m_groundAnchorB -= newOrigin; -} diff --git a/3rdparty/box2d/src/dynamics/b2_revolute_joint.cpp b/3rdparty/box2d/src/dynamics/b2_revolute_joint.cpp deleted file mode 100644 index f7cc4cc29afe..000000000000 --- a/3rdparty/box2d/src/dynamics/b2_revolute_joint.cpp +++ /dev/null @@ -1,501 +0,0 @@ -// MIT License - -// Copyright (c) 2019 Erin Catto - -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: - -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. - -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -#include "box2d/b2_body.h" -#include "box2d/b2_draw.h" -#include "box2d/b2_revolute_joint.h" -#include "box2d/b2_time_step.h" - -// Point-to-point constraint -// C = p2 - p1 -// Cdot = v2 - v1 -// = v2 + cross(w2, r2) - v1 - cross(w1, r1) -// J = [-I -r1_skew I r2_skew ] -// Identity used: -// w k % (rx i + ry j) = w * (-ry i + rx j) - -// Motor constraint -// Cdot = w2 - w1 -// J = [0 0 -1 0 0 1] -// K = invI1 + invI2 - -void b2RevoluteJointDef::Initialize(b2Body* bA, b2Body* bB, const b2Vec2& anchor) -{ - bodyA = bA; - bodyB = bB; - localAnchorA = bodyA->GetLocalPoint(anchor); - localAnchorB = bodyB->GetLocalPoint(anchor); - referenceAngle = bodyB->GetAngle() - bodyA->GetAngle(); -} - -b2RevoluteJoint::b2RevoluteJoint(const b2RevoluteJointDef* def) -: b2Joint(def) -{ - m_localAnchorA = def->localAnchorA; - m_localAnchorB = def->localAnchorB; - m_referenceAngle = def->referenceAngle; - - m_impulse.SetZero(); - m_axialMass = 0.0f; - m_motorImpulse = 0.0f; - m_lowerImpulse = 0.0f; - m_upperImpulse = 0.0f; - - m_lowerAngle = def->lowerAngle; - m_upperAngle = def->upperAngle; - m_maxMotorTorque = def->maxMotorTorque; - m_motorSpeed = def->motorSpeed; - m_enableLimit = def->enableLimit; - m_enableMotor = def->enableMotor; - - m_angle = 0.0f; -} - -void b2RevoluteJoint::InitVelocityConstraints(const b2SolverData& data) -{ - m_indexA = m_bodyA->m_islandIndex; - m_indexB = m_bodyB->m_islandIndex; - m_localCenterA = m_bodyA->m_sweep.localCenter; - m_localCenterB = m_bodyB->m_sweep.localCenter; - m_invMassA = m_bodyA->m_invMass; - m_invMassB = m_bodyB->m_invMass; - m_invIA = m_bodyA->m_invI; - m_invIB = m_bodyB->m_invI; - - float aA = data.positions[m_indexA].a; - b2Vec2 vA = data.velocities[m_indexA].v; - float wA = data.velocities[m_indexA].w; - - float aB = data.positions[m_indexB].a; - b2Vec2 vB = data.velocities[m_indexB].v; - float wB = data.velocities[m_indexB].w; - - b2Rot qA(aA), qB(aB); - - m_rA = b2Mul(qA, m_localAnchorA - m_localCenterA); - m_rB = b2Mul(qB, m_localAnchorB - m_localCenterB); - - // J = [-I -r1_skew I r2_skew] - // r_skew = [-ry; rx] - - // Matlab - // K = [ mA+r1y^2*iA+mB+r2y^2*iB, -r1y*iA*r1x-r2y*iB*r2x] - // [ -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB] - - float mA = m_invMassA, mB = m_invMassB; - float iA = m_invIA, iB = m_invIB; - - m_K.ex.x = mA + mB + m_rA.y * m_rA.y * iA + m_rB.y * m_rB.y * iB; - m_K.ey.x = -m_rA.y * m_rA.x * iA - m_rB.y * m_rB.x * iB; - m_K.ex.y = m_K.ey.x; - m_K.ey.y = mA + mB + m_rA.x * m_rA.x * iA + m_rB.x * m_rB.x * iB; - - m_axialMass = iA + iB; - bool fixedRotation; - if (m_axialMass > 0.0f) - { - m_axialMass = 1.0f / m_axialMass; - fixedRotation = false; - } - else - { - fixedRotation = true; - } - - m_angle = aB - aA - m_referenceAngle; - if (m_enableLimit == false || fixedRotation) - { - m_lowerImpulse = 0.0f; - m_upperImpulse = 0.0f; - } - - if (m_enableMotor == false || fixedRotation) - { - m_motorImpulse = 0.0f; - } - - if (data.step.warmStarting) - { - // Scale impulses to support a variable time step. - m_impulse *= data.step.dtRatio; - m_motorImpulse *= data.step.dtRatio; - m_lowerImpulse *= data.step.dtRatio; - m_upperImpulse *= data.step.dtRatio; - - float axialImpulse = m_motorImpulse + m_lowerImpulse - m_upperImpulse; - b2Vec2 P(m_impulse.x, m_impulse.y); - - vA -= mA * P; - wA -= iA * (b2Cross(m_rA, P) + axialImpulse); - - vB += mB * P; - wB += iB * (b2Cross(m_rB, P) + axialImpulse); - } - else - { - m_impulse.SetZero(); - m_motorImpulse = 0.0f; - m_lowerImpulse = 0.0f; - m_upperImpulse = 0.0f; - } - - data.velocities[m_indexA].v = vA; - data.velocities[m_indexA].w = wA; - data.velocities[m_indexB].v = vB; - data.velocities[m_indexB].w = wB; -} - -void b2RevoluteJoint::SolveVelocityConstraints(const b2SolverData& data) -{ - b2Vec2 vA = data.velocities[m_indexA].v; - float wA = data.velocities[m_indexA].w; - b2Vec2 vB = data.velocities[m_indexB].v; - float wB = data.velocities[m_indexB].w; - - float mA = m_invMassA, mB = m_invMassB; - float iA = m_invIA, iB = m_invIB; - - bool fixedRotation = (iA + iB == 0.0f); - - // Solve motor constraint. - if (m_enableMotor && fixedRotation == false) - { - float Cdot = wB - wA - m_motorSpeed; - float impulse = -m_axialMass * Cdot; - float oldImpulse = m_motorImpulse; - float maxImpulse = data.step.dt * m_maxMotorTorque; - m_motorImpulse = b2Clamp(m_motorImpulse + impulse, -maxImpulse, maxImpulse); - impulse = m_motorImpulse - oldImpulse; - - wA -= iA * impulse; - wB += iB * impulse; - } - - if (m_enableLimit && fixedRotation == false) - { - // Lower limit - { - float C = m_angle - m_lowerAngle; - float Cdot = wB - wA; - float impulse = -m_axialMass * (Cdot + b2Max(C, 0.0f) * data.step.inv_dt); - float oldImpulse = m_lowerImpulse; - m_lowerImpulse = b2Max(m_lowerImpulse + impulse, 0.0f); - impulse = m_lowerImpulse - oldImpulse; - - wA -= iA * impulse; - wB += iB * impulse; - } - - // Upper limit - // Note: signs are flipped to keep C positive when the constraint is satisfied. - // This also keeps the impulse positive when the limit is active. - { - float C = m_upperAngle - m_angle; - float Cdot = wA - wB; - float impulse = -m_axialMass * (Cdot + b2Max(C, 0.0f) * data.step.inv_dt); - float oldImpulse = m_upperImpulse; - m_upperImpulse = b2Max(m_upperImpulse + impulse, 0.0f); - impulse = m_upperImpulse - oldImpulse; - - wA += iA * impulse; - wB -= iB * impulse; - } - } - - // Solve point-to-point constraint - { - b2Vec2 Cdot = vB + b2Cross(wB, m_rB) - vA - b2Cross(wA, m_rA); - b2Vec2 impulse = m_K.Solve(-Cdot); - - m_impulse.x += impulse.x; - m_impulse.y += impulse.y; - - vA -= mA * impulse; - wA -= iA * b2Cross(m_rA, impulse); - - vB += mB * impulse; - wB += iB * b2Cross(m_rB, impulse); - } - - data.velocities[m_indexA].v = vA; - data.velocities[m_indexA].w = wA; - data.velocities[m_indexB].v = vB; - data.velocities[m_indexB].w = wB; -} - -bool b2RevoluteJoint::SolvePositionConstraints(const b2SolverData& data) -{ - b2Vec2 cA = data.positions[m_indexA].c; - float aA = data.positions[m_indexA].a; - b2Vec2 cB = data.positions[m_indexB].c; - float aB = data.positions[m_indexB].a; - - b2Rot qA(aA), qB(aB); - - float angularError = 0.0f; - float positionError = 0.0f; - - bool fixedRotation = (m_invIA + m_invIB == 0.0f); - - // Solve angular limit constraint - if (m_enableLimit && fixedRotation == false) - { - float angle = aB - aA - m_referenceAngle; - float C = 0.0f; - - if (b2Abs(m_upperAngle - m_lowerAngle) < 2.0f * b2_angularSlop) - { - // Prevent large angular corrections - C = b2Clamp(angle - m_lowerAngle, -b2_maxAngularCorrection, b2_maxAngularCorrection); - } - else if (angle <= m_lowerAngle) - { - // Prevent large angular corrections and allow some slop. - C = b2Clamp(angle - m_lowerAngle + b2_angularSlop, -b2_maxAngularCorrection, 0.0f); - } - else if (angle >= m_upperAngle) - { - // Prevent large angular corrections and allow some slop. - C = b2Clamp(angle - m_upperAngle - b2_angularSlop, 0.0f, b2_maxAngularCorrection); - } - - float limitImpulse = -m_axialMass * C; - aA -= m_invIA * limitImpulse; - aB += m_invIB * limitImpulse; - angularError = b2Abs(C); - } - - // Solve point-to-point constraint. - { - qA.Set(aA); - qB.Set(aB); - b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_localCenterA); - b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_localCenterB); - - b2Vec2 C = cB + rB - cA - rA; - positionError = C.Length(); - - float mA = m_invMassA, mB = m_invMassB; - float iA = m_invIA, iB = m_invIB; - - b2Mat22 K; - K.ex.x = mA + mB + iA * rA.y * rA.y + iB * rB.y * rB.y; - K.ex.y = -iA * rA.x * rA.y - iB * rB.x * rB.y; - K.ey.x = K.ex.y; - K.ey.y = mA + mB + iA * rA.x * rA.x + iB * rB.x * rB.x; - - b2Vec2 impulse = -K.Solve(C); - - cA -= mA * impulse; - aA -= iA * b2Cross(rA, impulse); - - cB += mB * impulse; - aB += iB * b2Cross(rB, impulse); - } - - data.positions[m_indexA].c = cA; - data.positions[m_indexA].a = aA; - data.positions[m_indexB].c = cB; - data.positions[m_indexB].a = aB; - - return positionError <= b2_linearSlop && angularError <= b2_angularSlop; -} - -b2Vec2 b2RevoluteJoint::GetAnchorA() const -{ - return m_bodyA->GetWorldPoint(m_localAnchorA); -} - -b2Vec2 b2RevoluteJoint::GetAnchorB() const -{ - return m_bodyB->GetWorldPoint(m_localAnchorB); -} - -b2Vec2 b2RevoluteJoint::GetReactionForce(float inv_dt) const -{ - b2Vec2 P(m_impulse.x, m_impulse.y); - return inv_dt * P; -} - -float b2RevoluteJoint::GetReactionTorque(float inv_dt) const -{ - return inv_dt * (m_motorImpulse + m_lowerImpulse - m_upperImpulse); -} - -float b2RevoluteJoint::GetJointAngle() const -{ - b2Body* bA = m_bodyA; - b2Body* bB = m_bodyB; - return bB->m_sweep.a - bA->m_sweep.a - m_referenceAngle; -} - -float b2RevoluteJoint::GetJointSpeed() const -{ - b2Body* bA = m_bodyA; - b2Body* bB = m_bodyB; - return bB->m_angularVelocity - bA->m_angularVelocity; -} - -bool b2RevoluteJoint::IsMotorEnabled() const -{ - return m_enableMotor; -} - -void b2RevoluteJoint::EnableMotor(bool flag) -{ - if (flag != m_enableMotor) - { - m_bodyA->SetAwake(true); - m_bodyB->SetAwake(true); - m_enableMotor = flag; - } -} - -float b2RevoluteJoint::GetMotorTorque(float inv_dt) const -{ - return inv_dt * m_motorImpulse; -} - -void b2RevoluteJoint::SetMotorSpeed(float speed) -{ - if (speed != m_motorSpeed) - { - m_bodyA->SetAwake(true); - m_bodyB->SetAwake(true); - m_motorSpeed = speed; - } -} - -void b2RevoluteJoint::SetMaxMotorTorque(float torque) -{ - if (torque != m_maxMotorTorque) - { - m_bodyA->SetAwake(true); - m_bodyB->SetAwake(true); - m_maxMotorTorque = torque; - } -} - -bool b2RevoluteJoint::IsLimitEnabled() const -{ - return m_enableLimit; -} - -void b2RevoluteJoint::EnableLimit(bool flag) -{ - if (flag != m_enableLimit) - { - m_bodyA->SetAwake(true); - m_bodyB->SetAwake(true); - m_enableLimit = flag; - m_lowerImpulse = 0.0f; - m_upperImpulse = 0.0f; - } -} - -float b2RevoluteJoint::GetLowerLimit() const -{ - return m_lowerAngle; -} - -float b2RevoluteJoint::GetUpperLimit() const -{ - return m_upperAngle; -} - -void b2RevoluteJoint::SetLimits(float lower, float upper) -{ - b2Assert(lower <= upper); - - if (lower != m_lowerAngle || upper != m_upperAngle) - { - m_bodyA->SetAwake(true); - m_bodyB->SetAwake(true); - m_lowerImpulse = 0.0f; - m_upperImpulse = 0.0f; - m_lowerAngle = lower; - m_upperAngle = upper; - } -} - -void b2RevoluteJoint::Dump() -{ - int32 indexA = m_bodyA->m_islandIndex; - int32 indexB = m_bodyB->m_islandIndex; - - b2Dump(" b2RevoluteJointDef jd;\n"); - b2Dump(" jd.bodyA = bodies[%d];\n", indexA); - b2Dump(" jd.bodyB = bodies[%d];\n", indexB); - b2Dump(" jd.collideConnected = bool(%d);\n", m_collideConnected); - b2Dump(" jd.localAnchorA.Set(%.9g, %.9g);\n", m_localAnchorA.x, m_localAnchorA.y); - b2Dump(" jd.localAnchorB.Set(%.9g, %.9g);\n", m_localAnchorB.x, m_localAnchorB.y); - b2Dump(" jd.referenceAngle = %.9g;\n", m_referenceAngle); - b2Dump(" jd.enableLimit = bool(%d);\n", m_enableLimit); - b2Dump(" jd.lowerAngle = %.9g;\n", m_lowerAngle); - b2Dump(" jd.upperAngle = %.9g;\n", m_upperAngle); - b2Dump(" jd.enableMotor = bool(%d);\n", m_enableMotor); - b2Dump(" jd.motorSpeed = %.9g;\n", m_motorSpeed); - b2Dump(" jd.maxMotorTorque = %.9g;\n", m_maxMotorTorque); - b2Dump(" joints[%d] = m_world->CreateJoint(&jd);\n", m_index); -} - -/// -void b2RevoluteJoint::Draw(b2Draw* draw) const -{ - const b2Transform& xfA = m_bodyA->GetTransform(); - const b2Transform& xfB = m_bodyB->GetTransform(); - b2Vec2 pA = b2Mul(xfA, m_localAnchorA); - b2Vec2 pB = b2Mul(xfB, m_localAnchorB); - - b2Color c1(0.7f, 0.7f, 0.7f); - b2Color c2(0.3f, 0.9f, 0.3f); - b2Color c3(0.9f, 0.3f, 0.3f); - b2Color c4(0.3f, 0.3f, 0.9f); - b2Color c5(0.4f, 0.4f, 0.4f); - - draw->DrawPoint(pA, 5.0f, c4); - draw->DrawPoint(pB, 5.0f, c5); - - float aA = m_bodyA->GetAngle(); - float aB = m_bodyB->GetAngle(); - float angle = aB - aA - m_referenceAngle; - - const float L = 0.5f; - - b2Vec2 r = L * b2Vec2(cosf(angle), sinf(angle)); - draw->DrawSegment(pB, pB + r, c1); - draw->DrawCircle(pB, L, c1); - - if (m_enableLimit) - { - b2Vec2 rlo = L * b2Vec2(cosf(m_lowerAngle), sinf(m_lowerAngle)); - b2Vec2 rhi = L * b2Vec2(cosf(m_upperAngle), sinf(m_upperAngle)); - - draw->DrawSegment(pB, pB + rlo, c2); - draw->DrawSegment(pB, pB + rhi, c3); - } - - b2Color color(0.5f, 0.8f, 0.8f); - draw->DrawSegment(xfA.p, pA, color); - draw->DrawSegment(pA, pB, color); - draw->DrawSegment(xfB.p, pB, color); -} diff --git a/3rdparty/box2d/src/dynamics/b2_weld_joint.cpp b/3rdparty/box2d/src/dynamics/b2_weld_joint.cpp deleted file mode 100644 index df3ee0a3eaf1..000000000000 --- a/3rdparty/box2d/src/dynamics/b2_weld_joint.cpp +++ /dev/null @@ -1,344 +0,0 @@ -// MIT License - -// Copyright (c) 2019 Erin Catto - -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: - -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. - -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -#include "box2d/b2_body.h" -#include "box2d/b2_time_step.h" -#include "box2d/b2_weld_joint.h" - -// Point-to-point constraint -// C = p2 - p1 -// Cdot = v2 - v1 -// = v2 + cross(w2, r2) - v1 - cross(w1, r1) -// J = [-I -r1_skew I r2_skew ] -// Identity used: -// w k % (rx i + ry j) = w * (-ry i + rx j) - -// Angle constraint -// C = angle2 - angle1 - referenceAngle -// Cdot = w2 - w1 -// J = [0 0 -1 0 0 1] -// K = invI1 + invI2 - -void b2WeldJointDef::Initialize(b2Body* bA, b2Body* bB, const b2Vec2& anchor) -{ - bodyA = bA; - bodyB = bB; - localAnchorA = bodyA->GetLocalPoint(anchor); - localAnchorB = bodyB->GetLocalPoint(anchor); - referenceAngle = bodyB->GetAngle() - bodyA->GetAngle(); -} - -b2WeldJoint::b2WeldJoint(const b2WeldJointDef* def) -: b2Joint(def) -{ - m_localAnchorA = def->localAnchorA; - m_localAnchorB = def->localAnchorB; - m_referenceAngle = def->referenceAngle; - m_stiffness = def->stiffness; - m_damping = def->damping; - - m_impulse.SetZero(); -} - -void b2WeldJoint::InitVelocityConstraints(const b2SolverData& data) -{ - m_indexA = m_bodyA->m_islandIndex; - m_indexB = m_bodyB->m_islandIndex; - m_localCenterA = m_bodyA->m_sweep.localCenter; - m_localCenterB = m_bodyB->m_sweep.localCenter; - m_invMassA = m_bodyA->m_invMass; - m_invMassB = m_bodyB->m_invMass; - m_invIA = m_bodyA->m_invI; - m_invIB = m_bodyB->m_invI; - - float aA = data.positions[m_indexA].a; - b2Vec2 vA = data.velocities[m_indexA].v; - float wA = data.velocities[m_indexA].w; - - float aB = data.positions[m_indexB].a; - b2Vec2 vB = data.velocities[m_indexB].v; - float wB = data.velocities[m_indexB].w; - - b2Rot qA(aA), qB(aB); - - m_rA = b2Mul(qA, m_localAnchorA - m_localCenterA); - m_rB = b2Mul(qB, m_localAnchorB - m_localCenterB); - - // J = [-I -r1_skew I r2_skew] - // [ 0 -1 0 1] - // r_skew = [-ry; rx] - - // Matlab - // K = [ mA+r1y^2*iA+mB+r2y^2*iB, -r1y*iA*r1x-r2y*iB*r2x, -r1y*iA-r2y*iB] - // [ -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB, r1x*iA+r2x*iB] - // [ -r1y*iA-r2y*iB, r1x*iA+r2x*iB, iA+iB] - - float mA = m_invMassA, mB = m_invMassB; - float iA = m_invIA, iB = m_invIB; - - b2Mat33 K; - K.ex.x = mA + mB + m_rA.y * m_rA.y * iA + m_rB.y * m_rB.y * iB; - K.ey.x = -m_rA.y * m_rA.x * iA - m_rB.y * m_rB.x * iB; - K.ez.x = -m_rA.y * iA - m_rB.y * iB; - K.ex.y = K.ey.x; - K.ey.y = mA + mB + m_rA.x * m_rA.x * iA + m_rB.x * m_rB.x * iB; - K.ez.y = m_rA.x * iA + m_rB.x * iB; - K.ex.z = K.ez.x; - K.ey.z = K.ez.y; - K.ez.z = iA + iB; - - if (m_stiffness > 0.0f) - { - K.GetInverse22(&m_mass); - - float invM = iA + iB; - - float C = aB - aA - m_referenceAngle; - - // Damping coefficient - float d = m_damping; - - // Spring stiffness - float k = m_stiffness; - - // magic formulas - float h = data.step.dt; - m_gamma = h * (d + h * k); - m_gamma = m_gamma != 0.0f ? 1.0f / m_gamma : 0.0f; - m_bias = C * h * k * m_gamma; - - invM += m_gamma; - m_mass.ez.z = invM != 0.0f ? 1.0f / invM : 0.0f; - } - else if (K.ez.z == 0.0f) - { - K.GetInverse22(&m_mass); - m_gamma = 0.0f; - m_bias = 0.0f; - } - else - { - K.GetSymInverse33(&m_mass); - m_gamma = 0.0f; - m_bias = 0.0f; - } - - if (data.step.warmStarting) - { - // Scale impulses to support a variable time step. - m_impulse *= data.step.dtRatio; - - b2Vec2 P(m_impulse.x, m_impulse.y); - - vA -= mA * P; - wA -= iA * (b2Cross(m_rA, P) + m_impulse.z); - - vB += mB * P; - wB += iB * (b2Cross(m_rB, P) + m_impulse.z); - } - else - { - m_impulse.SetZero(); - } - - data.velocities[m_indexA].v = vA; - data.velocities[m_indexA].w = wA; - data.velocities[m_indexB].v = vB; - data.velocities[m_indexB].w = wB; -} - -void b2WeldJoint::SolveVelocityConstraints(const b2SolverData& data) -{ - b2Vec2 vA = data.velocities[m_indexA].v; - float wA = data.velocities[m_indexA].w; - b2Vec2 vB = data.velocities[m_indexB].v; - float wB = data.velocities[m_indexB].w; - - float mA = m_invMassA, mB = m_invMassB; - float iA = m_invIA, iB = m_invIB; - - if (m_stiffness > 0.0f) - { - float Cdot2 = wB - wA; - - float impulse2 = -m_mass.ez.z * (Cdot2 + m_bias + m_gamma * m_impulse.z); - m_impulse.z += impulse2; - - wA -= iA * impulse2; - wB += iB * impulse2; - - b2Vec2 Cdot1 = vB + b2Cross(wB, m_rB) - vA - b2Cross(wA, m_rA); - - b2Vec2 impulse1 = -b2Mul22(m_mass, Cdot1); - m_impulse.x += impulse1.x; - m_impulse.y += impulse1.y; - - b2Vec2 P = impulse1; - - vA -= mA * P; - wA -= iA * b2Cross(m_rA, P); - - vB += mB * P; - wB += iB * b2Cross(m_rB, P); - } - else - { - b2Vec2 Cdot1 = vB + b2Cross(wB, m_rB) - vA - b2Cross(wA, m_rA); - float Cdot2 = wB - wA; - b2Vec3 Cdot(Cdot1.x, Cdot1.y, Cdot2); - - b2Vec3 impulse = -b2Mul(m_mass, Cdot); - m_impulse += impulse; - - b2Vec2 P(impulse.x, impulse.y); - - vA -= mA * P; - wA -= iA * (b2Cross(m_rA, P) + impulse.z); - - vB += mB * P; - wB += iB * (b2Cross(m_rB, P) + impulse.z); - } - - data.velocities[m_indexA].v = vA; - data.velocities[m_indexA].w = wA; - data.velocities[m_indexB].v = vB; - data.velocities[m_indexB].w = wB; -} - -bool b2WeldJoint::SolvePositionConstraints(const b2SolverData& data) -{ - b2Vec2 cA = data.positions[m_indexA].c; - float aA = data.positions[m_indexA].a; - b2Vec2 cB = data.positions[m_indexB].c; - float aB = data.positions[m_indexB].a; - - b2Rot qA(aA), qB(aB); - - float mA = m_invMassA, mB = m_invMassB; - float iA = m_invIA, iB = m_invIB; - - b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_localCenterA); - b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_localCenterB); - - float positionError, angularError; - - b2Mat33 K; - K.ex.x = mA + mB + rA.y * rA.y * iA + rB.y * rB.y * iB; - K.ey.x = -rA.y * rA.x * iA - rB.y * rB.x * iB; - K.ez.x = -rA.y * iA - rB.y * iB; - K.ex.y = K.ey.x; - K.ey.y = mA + mB + rA.x * rA.x * iA + rB.x * rB.x * iB; - K.ez.y = rA.x * iA + rB.x * iB; - K.ex.z = K.ez.x; - K.ey.z = K.ez.y; - K.ez.z = iA + iB; - - if (m_stiffness > 0.0f) - { - b2Vec2 C1 = cB + rB - cA - rA; - - positionError = C1.Length(); - angularError = 0.0f; - - b2Vec2 P = -K.Solve22(C1); - - cA -= mA * P; - aA -= iA * b2Cross(rA, P); - - cB += mB * P; - aB += iB * b2Cross(rB, P); - } - else - { - b2Vec2 C1 = cB + rB - cA - rA; - float C2 = aB - aA - m_referenceAngle; - - positionError = C1.Length(); - angularError = b2Abs(C2); - - b2Vec3 C(C1.x, C1.y, C2); - - b2Vec3 impulse; - if (K.ez.z > 0.0f) - { - impulse = -K.Solve33(C); - } - else - { - b2Vec2 impulse2 = -K.Solve22(C1); - impulse.Set(impulse2.x, impulse2.y, 0.0f); - } - - b2Vec2 P(impulse.x, impulse.y); - - cA -= mA * P; - aA -= iA * (b2Cross(rA, P) + impulse.z); - - cB += mB * P; - aB += iB * (b2Cross(rB, P) + impulse.z); - } - - data.positions[m_indexA].c = cA; - data.positions[m_indexA].a = aA; - data.positions[m_indexB].c = cB; - data.positions[m_indexB].a = aB; - - return positionError <= b2_linearSlop && angularError <= b2_angularSlop; -} - -b2Vec2 b2WeldJoint::GetAnchorA() const -{ - return m_bodyA->GetWorldPoint(m_localAnchorA); -} - -b2Vec2 b2WeldJoint::GetAnchorB() const -{ - return m_bodyB->GetWorldPoint(m_localAnchorB); -} - -b2Vec2 b2WeldJoint::GetReactionForce(float inv_dt) const -{ - b2Vec2 P(m_impulse.x, m_impulse.y); - return inv_dt * P; -} - -float b2WeldJoint::GetReactionTorque(float inv_dt) const -{ - return inv_dt * m_impulse.z; -} - -void b2WeldJoint::Dump() -{ - int32 indexA = m_bodyA->m_islandIndex; - int32 indexB = m_bodyB->m_islandIndex; - - b2Dump(" b2WeldJointDef jd;\n"); - b2Dump(" jd.bodyA = bodies[%d];\n", indexA); - b2Dump(" jd.bodyB = bodies[%d];\n", indexB); - b2Dump(" jd.collideConnected = bool(%d);\n", m_collideConnected); - b2Dump(" jd.localAnchorA.Set(%.9g, %.9g);\n", m_localAnchorA.x, m_localAnchorA.y); - b2Dump(" jd.localAnchorB.Set(%.9g, %.9g);\n", m_localAnchorB.x, m_localAnchorB.y); - b2Dump(" jd.referenceAngle = %.9g;\n", m_referenceAngle); - b2Dump(" jd.stiffness = %.9g;\n", m_stiffness); - b2Dump(" jd.damping = %.9g;\n", m_damping); - b2Dump(" joints[%d] = m_world->CreateJoint(&jd);\n", m_index); -} diff --git a/3rdparty/box2d/src/dynamics/b2_wheel_joint.cpp b/3rdparty/box2d/src/dynamics/b2_wheel_joint.cpp deleted file mode 100644 index c23b98460973..000000000000 --- a/3rdparty/box2d/src/dynamics/b2_wheel_joint.cpp +++ /dev/null @@ -1,672 +0,0 @@ -// MIT License - -// Copyright (c) 2019 Erin Catto - -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: - -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. - -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -#include "box2d/b2_body.h" -#include "box2d/b2_draw.h" -#include "box2d/b2_wheel_joint.h" -#include "box2d/b2_time_step.h" - -// Linear constraint (point-to-line) -// d = pB - pA = xB + rB - xA - rA -// C = dot(ay, d) -// Cdot = dot(d, cross(wA, ay)) + dot(ay, vB + cross(wB, rB) - vA - cross(wA, rA)) -// = -dot(ay, vA) - dot(cross(d + rA, ay), wA) + dot(ay, vB) + dot(cross(rB, ay), vB) -// J = [-ay, -cross(d + rA, ay), ay, cross(rB, ay)] - -// Spring linear constraint -// C = dot(ax, d) -// Cdot = = -dot(ax, vA) - dot(cross(d + rA, ax), wA) + dot(ax, vB) + dot(cross(rB, ax), vB) -// J = [-ax -cross(d+rA, ax) ax cross(rB, ax)] - -// Motor rotational constraint -// Cdot = wB - wA -// J = [0 0 -1 0 0 1] - -void b2WheelJointDef::Initialize(b2Body* bA, b2Body* bB, const b2Vec2& anchor, const b2Vec2& axis) -{ - bodyA = bA; - bodyB = bB; - localAnchorA = bodyA->GetLocalPoint(anchor); - localAnchorB = bodyB->GetLocalPoint(anchor); - localAxisA = bodyA->GetLocalVector(axis); -} - -b2WheelJoint::b2WheelJoint(const b2WheelJointDef* def) -: b2Joint(def) -{ - m_localAnchorA = def->localAnchorA; - m_localAnchorB = def->localAnchorB; - m_localXAxisA = def->localAxisA; - m_localYAxisA = b2Cross(1.0f, m_localXAxisA); - - m_mass = 0.0f; - m_impulse = 0.0f; - m_motorMass = 0.0f; - m_motorImpulse = 0.0f; - m_springMass = 0.0f; - m_springImpulse = 0.0f; - - m_axialMass = 0.0f; - m_lowerImpulse = 0.0f; - m_upperImpulse = 0.0f; - m_lowerTranslation = def->lowerTranslation; - m_upperTranslation = def->upperTranslation; - m_enableLimit = def->enableLimit; - - m_maxMotorTorque = def->maxMotorTorque; - m_motorSpeed = def->motorSpeed; - m_enableMotor = def->enableMotor; - - m_bias = 0.0f; - m_gamma = 0.0f; - - m_ax.SetZero(); - m_ay.SetZero(); - - m_stiffness = def->stiffness; - m_damping = def->damping; -} - -void b2WheelJoint::InitVelocityConstraints(const b2SolverData& data) -{ - m_indexA = m_bodyA->m_islandIndex; - m_indexB = m_bodyB->m_islandIndex; - m_localCenterA = m_bodyA->m_sweep.localCenter; - m_localCenterB = m_bodyB->m_sweep.localCenter; - m_invMassA = m_bodyA->m_invMass; - m_invMassB = m_bodyB->m_invMass; - m_invIA = m_bodyA->m_invI; - m_invIB = m_bodyB->m_invI; - - float mA = m_invMassA, mB = m_invMassB; - float iA = m_invIA, iB = m_invIB; - - b2Vec2 cA = data.positions[m_indexA].c; - float aA = data.positions[m_indexA].a; - b2Vec2 vA = data.velocities[m_indexA].v; - float wA = data.velocities[m_indexA].w; - - b2Vec2 cB = data.positions[m_indexB].c; - float aB = data.positions[m_indexB].a; - b2Vec2 vB = data.velocities[m_indexB].v; - float wB = data.velocities[m_indexB].w; - - b2Rot qA(aA), qB(aB); - - // Compute the effective masses. - b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_localCenterA); - b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_localCenterB); - b2Vec2 d = cB + rB - cA - rA; - - // Point to line constraint - { - m_ay = b2Mul(qA, m_localYAxisA); - m_sAy = b2Cross(d + rA, m_ay); - m_sBy = b2Cross(rB, m_ay); - - m_mass = mA + mB + iA * m_sAy * m_sAy + iB * m_sBy * m_sBy; - - if (m_mass > 0.0f) - { - m_mass = 1.0f / m_mass; - } - } - - // Spring constraint - m_ax = b2Mul(qA, m_localXAxisA); - m_sAx = b2Cross(d + rA, m_ax); - m_sBx = b2Cross(rB, m_ax); - - const float invMass = mA + mB + iA * m_sAx * m_sAx + iB * m_sBx * m_sBx; - if (invMass > 0.0f) - { - m_axialMass = 1.0f / invMass; - } - else - { - m_axialMass = 0.0f; - } - - m_springMass = 0.0f; - m_bias = 0.0f; - m_gamma = 0.0f; - - if (m_stiffness > 0.0f && invMass > 0.0f) - { - m_springMass = 1.0f / invMass; - - float C = b2Dot(d, m_ax); - - // magic formulas - float h = data.step.dt; - m_gamma = h * (m_damping + h * m_stiffness); - if (m_gamma > 0.0f) - { - m_gamma = 1.0f / m_gamma; - } - - m_bias = C * h * m_stiffness * m_gamma; - - m_springMass = invMass + m_gamma; - if (m_springMass > 0.0f) - { - m_springMass = 1.0f / m_springMass; - } - } - else - { - m_springImpulse = 0.0f; - } - - if (m_enableLimit) - { - m_translation = b2Dot(m_ax, d); - } - else - { - m_lowerImpulse = 0.0f; - m_upperImpulse = 0.0f; - } - - if (m_enableMotor) - { - m_motorMass = iA + iB; - if (m_motorMass > 0.0f) - { - m_motorMass = 1.0f / m_motorMass; - } - } - else - { - m_motorMass = 0.0f; - m_motorImpulse = 0.0f; - } - - if (data.step.warmStarting) - { - // Account for variable time step. - m_impulse *= data.step.dtRatio; - m_springImpulse *= data.step.dtRatio; - m_motorImpulse *= data.step.dtRatio; - - float axialImpulse = m_springImpulse + m_lowerImpulse - m_upperImpulse; - b2Vec2 P = m_impulse * m_ay + axialImpulse * m_ax; - float LA = m_impulse * m_sAy + axialImpulse * m_sAx + m_motorImpulse; - float LB = m_impulse * m_sBy + axialImpulse * m_sBx + m_motorImpulse; - - vA -= m_invMassA * P; - wA -= m_invIA * LA; - - vB += m_invMassB * P; - wB += m_invIB * LB; - } - else - { - m_impulse = 0.0f; - m_springImpulse = 0.0f; - m_motorImpulse = 0.0f; - m_lowerImpulse = 0.0f; - m_upperImpulse = 0.0f; - } - - data.velocities[m_indexA].v = vA; - data.velocities[m_indexA].w = wA; - data.velocities[m_indexB].v = vB; - data.velocities[m_indexB].w = wB; -} - -void b2WheelJoint::SolveVelocityConstraints(const b2SolverData& data) -{ - float mA = m_invMassA, mB = m_invMassB; - float iA = m_invIA, iB = m_invIB; - - b2Vec2 vA = data.velocities[m_indexA].v; - float wA = data.velocities[m_indexA].w; - b2Vec2 vB = data.velocities[m_indexB].v; - float wB = data.velocities[m_indexB].w; - - // Solve spring constraint - { - float Cdot = b2Dot(m_ax, vB - vA) + m_sBx * wB - m_sAx * wA; - float impulse = -m_springMass * (Cdot + m_bias + m_gamma * m_springImpulse); - m_springImpulse += impulse; - - b2Vec2 P = impulse * m_ax; - float LA = impulse * m_sAx; - float LB = impulse * m_sBx; - - vA -= mA * P; - wA -= iA * LA; - - vB += mB * P; - wB += iB * LB; - } - - // Solve rotational motor constraint - { - float Cdot = wB - wA - m_motorSpeed; - float impulse = -m_motorMass * Cdot; - - float oldImpulse = m_motorImpulse; - float maxImpulse = data.step.dt * m_maxMotorTorque; - m_motorImpulse = b2Clamp(m_motorImpulse + impulse, -maxImpulse, maxImpulse); - impulse = m_motorImpulse - oldImpulse; - - wA -= iA * impulse; - wB += iB * impulse; - } - - if (m_enableLimit) - { - // Lower limit - { - float C = m_translation - m_lowerTranslation; - float Cdot = b2Dot(m_ax, vB - vA) + m_sBx * wB - m_sAx * wA; - float impulse = -m_axialMass * (Cdot + b2Max(C, 0.0f) * data.step.inv_dt); - float oldImpulse = m_lowerImpulse; - m_lowerImpulse = b2Max(m_lowerImpulse + impulse, 0.0f); - impulse = m_lowerImpulse - oldImpulse; - - b2Vec2 P = impulse * m_ax; - float LA = impulse * m_sAx; - float LB = impulse * m_sBx; - - vA -= mA * P; - wA -= iA * LA; - vB += mB * P; - wB += iB * LB; - } - - // Upper limit - // Note: signs are flipped to keep C positive when the constraint is satisfied. - // This also keeps the impulse positive when the limit is active. - { - float C = m_upperTranslation - m_translation; - float Cdot = b2Dot(m_ax, vA - vB) + m_sAx * wA - m_sBx * wB; - float impulse = -m_axialMass * (Cdot + b2Max(C, 0.0f) * data.step.inv_dt); - float oldImpulse = m_upperImpulse; - m_upperImpulse = b2Max(m_upperImpulse + impulse, 0.0f); - impulse = m_upperImpulse - oldImpulse; - - b2Vec2 P = impulse * m_ax; - float LA = impulse * m_sAx; - float LB = impulse * m_sBx; - - vA += mA * P; - wA += iA * LA; - vB -= mB * P; - wB -= iB * LB; - } - } - - // Solve point to line constraint - { - float Cdot = b2Dot(m_ay, vB - vA) + m_sBy * wB - m_sAy * wA; - float impulse = -m_mass * Cdot; - m_impulse += impulse; - - b2Vec2 P = impulse * m_ay; - float LA = impulse * m_sAy; - float LB = impulse * m_sBy; - - vA -= mA * P; - wA -= iA * LA; - - vB += mB * P; - wB += iB * LB; - } - - data.velocities[m_indexA].v = vA; - data.velocities[m_indexA].w = wA; - data.velocities[m_indexB].v = vB; - data.velocities[m_indexB].w = wB; -} - -bool b2WheelJoint::SolvePositionConstraints(const b2SolverData& data) -{ - b2Vec2 cA = data.positions[m_indexA].c; - float aA = data.positions[m_indexA].a; - b2Vec2 cB = data.positions[m_indexB].c; - float aB = data.positions[m_indexB].a; - - float linearError = 0.0f; - - if (m_enableLimit) - { - b2Rot qA(aA), qB(aB); - - b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_localCenterA); - b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_localCenterB); - b2Vec2 d = (cB - cA) + rB - rA; - - b2Vec2 ax = b2Mul(qA, m_localXAxisA); - float sAx = b2Cross(d + rA, m_ax); - float sBx = b2Cross(rB, m_ax); - - float C = 0.0f; - float translation = b2Dot(ax, d); - if (b2Abs(m_upperTranslation - m_lowerTranslation) < 2.0f * b2_linearSlop) - { - C = translation; - } - else if (translation <= m_lowerTranslation) - { - C = b2Min(translation - m_lowerTranslation, 0.0f); - } - else if (translation >= m_upperTranslation) - { - C = b2Max(translation - m_upperTranslation, 0.0f); - } - - if (C != 0.0f) - { - - float invMass = m_invMassA + m_invMassB + m_invIA * sAx * sAx + m_invIB * sBx * sBx; - float impulse = 0.0f; - if (invMass != 0.0f) - { - impulse = -C / invMass; - } - - b2Vec2 P = impulse * ax; - float LA = impulse * sAx; - float LB = impulse * sBx; - - cA -= m_invMassA * P; - aA -= m_invIA * LA; - cB += m_invMassB * P; - aB += m_invIB * LB; - - linearError = b2Abs(C); - } - } - - // Solve perpendicular constraint - { - b2Rot qA(aA), qB(aB); - - b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_localCenterA); - b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_localCenterB); - b2Vec2 d = (cB - cA) + rB - rA; - - b2Vec2 ay = b2Mul(qA, m_localYAxisA); - - float sAy = b2Cross(d + rA, ay); - float sBy = b2Cross(rB, ay); - - float C = b2Dot(d, ay); - - float invMass = m_invMassA + m_invMassB + m_invIA * m_sAy * m_sAy + m_invIB * m_sBy * m_sBy; - - float impulse = 0.0f; - if (invMass != 0.0f) - { - impulse = - C / invMass; - } - - b2Vec2 P = impulse * ay; - float LA = impulse * sAy; - float LB = impulse * sBy; - - cA -= m_invMassA * P; - aA -= m_invIA * LA; - cB += m_invMassB * P; - aB += m_invIB * LB; - - linearError = b2Max(linearError, b2Abs(C)); - } - - data.positions[m_indexA].c = cA; - data.positions[m_indexA].a = aA; - data.positions[m_indexB].c = cB; - data.positions[m_indexB].a = aB; - - return linearError <= b2_linearSlop; -} - -b2Vec2 b2WheelJoint::GetAnchorA() const -{ - return m_bodyA->GetWorldPoint(m_localAnchorA); -} - -b2Vec2 b2WheelJoint::GetAnchorB() const -{ - return m_bodyB->GetWorldPoint(m_localAnchorB); -} - -b2Vec2 b2WheelJoint::GetReactionForce(float inv_dt) const -{ - return inv_dt * (m_impulse * m_ay + (m_springImpulse + m_lowerImpulse - m_upperImpulse) * m_ax); -} - -float b2WheelJoint::GetReactionTorque(float inv_dt) const -{ - return inv_dt * m_motorImpulse; -} - -float b2WheelJoint::GetJointTranslation() const -{ - b2Body* bA = m_bodyA; - b2Body* bB = m_bodyB; - - b2Vec2 pA = bA->GetWorldPoint(m_localAnchorA); - b2Vec2 pB = bB->GetWorldPoint(m_localAnchorB); - b2Vec2 d = pB - pA; - b2Vec2 axis = bA->GetWorldVector(m_localXAxisA); - - float translation = b2Dot(d, axis); - return translation; -} - -float b2WheelJoint::GetJointLinearSpeed() const -{ - b2Body* bA = m_bodyA; - b2Body* bB = m_bodyB; - - b2Vec2 rA = b2Mul(bA->m_xf.q, m_localAnchorA - bA->m_sweep.localCenter); - b2Vec2 rB = b2Mul(bB->m_xf.q, m_localAnchorB - bB->m_sweep.localCenter); - b2Vec2 p1 = bA->m_sweep.c + rA; - b2Vec2 p2 = bB->m_sweep.c + rB; - b2Vec2 d = p2 - p1; - b2Vec2 axis = b2Mul(bA->m_xf.q, m_localXAxisA); - - b2Vec2 vA = bA->m_linearVelocity; - b2Vec2 vB = bB->m_linearVelocity; - float wA = bA->m_angularVelocity; - float wB = bB->m_angularVelocity; - - float speed = b2Dot(d, b2Cross(wA, axis)) + b2Dot(axis, vB + b2Cross(wB, rB) - vA - b2Cross(wA, rA)); - return speed; -} - -float b2WheelJoint::GetJointAngle() const -{ - b2Body* bA = m_bodyA; - b2Body* bB = m_bodyB; - return bB->m_sweep.a - bA->m_sweep.a; -} - -float b2WheelJoint::GetJointAngularSpeed() const -{ - float wA = m_bodyA->m_angularVelocity; - float wB = m_bodyB->m_angularVelocity; - return wB - wA; -} - -bool b2WheelJoint::IsLimitEnabled() const -{ - return m_enableLimit; -} - -void b2WheelJoint::EnableLimit(bool flag) -{ - if (flag != m_enableLimit) - { - m_bodyA->SetAwake(true); - m_bodyB->SetAwake(true); - m_enableLimit = flag; - m_lowerImpulse = 0.0f; - m_upperImpulse = 0.0f; - } -} - -float b2WheelJoint::GetLowerLimit() const -{ - return m_lowerTranslation; -} - -float b2WheelJoint::GetUpperLimit() const -{ - return m_upperTranslation; -} - -void b2WheelJoint::SetLimits(float lower, float upper) -{ - b2Assert(lower <= upper); - if (lower != m_lowerTranslation || upper != m_upperTranslation) - { - m_bodyA->SetAwake(true); - m_bodyB->SetAwake(true); - m_lowerTranslation = lower; - m_upperTranslation = upper; - m_lowerImpulse = 0.0f; - m_upperImpulse = 0.0f; - } -} - -bool b2WheelJoint::IsMotorEnabled() const -{ - return m_enableMotor; -} - -void b2WheelJoint::EnableMotor(bool flag) -{ - if (flag != m_enableMotor) - { - m_bodyA->SetAwake(true); - m_bodyB->SetAwake(true); - m_enableMotor = flag; - } -} - -void b2WheelJoint::SetMotorSpeed(float speed) -{ - if (speed != m_motorSpeed) - { - m_bodyA->SetAwake(true); - m_bodyB->SetAwake(true); - m_motorSpeed = speed; - } -} - -void b2WheelJoint::SetMaxMotorTorque(float torque) -{ - if (torque != m_maxMotorTorque) - { - m_bodyA->SetAwake(true); - m_bodyB->SetAwake(true); - m_maxMotorTorque = torque; - } -} - -float b2WheelJoint::GetMotorTorque(float inv_dt) const -{ - return inv_dt * m_motorImpulse; -} - -void b2WheelJoint::SetStiffness(float stiffness) -{ - m_stiffness = stiffness; -} - -float b2WheelJoint::GetStiffness() const -{ - return m_stiffness; -} - -void b2WheelJoint::SetDamping(float damping) -{ - m_damping = damping; -} - -float b2WheelJoint::GetDamping() const -{ - return m_damping; -} - -void b2WheelJoint::Dump() -{ - // FLT_DECIMAL_DIG == 9 - - int32 indexA = m_bodyA->m_islandIndex; - int32 indexB = m_bodyB->m_islandIndex; - - b2Dump(" b2WheelJointDef jd;\n"); - b2Dump(" jd.bodyA = bodies[%d];\n", indexA); - b2Dump(" jd.bodyB = bodies[%d];\n", indexB); - b2Dump(" jd.collideConnected = bool(%d);\n", m_collideConnected); - b2Dump(" jd.localAnchorA.Set(%.9g, %.9g);\n", m_localAnchorA.x, m_localAnchorA.y); - b2Dump(" jd.localAnchorB.Set(%.9g, %.9g);\n", m_localAnchorB.x, m_localAnchorB.y); - b2Dump(" jd.localAxisA.Set(%.9g, %.9g);\n", m_localXAxisA.x, m_localXAxisA.y); - b2Dump(" jd.enableMotor = bool(%d);\n", m_enableMotor); - b2Dump(" jd.motorSpeed = %.9g;\n", m_motorSpeed); - b2Dump(" jd.maxMotorTorque = %.9g;\n", m_maxMotorTorque); - b2Dump(" jd.stiffness = %.9g;\n", m_stiffness); - b2Dump(" jd.damping = %.9g;\n", m_damping); - b2Dump(" joints[%d] = m_world->CreateJoint(&jd);\n", m_index); -} - -/// -void b2WheelJoint::Draw(b2Draw* draw) const -{ - const b2Transform& xfA = m_bodyA->GetTransform(); - const b2Transform& xfB = m_bodyB->GetTransform(); - b2Vec2 pA = b2Mul(xfA, m_localAnchorA); - b2Vec2 pB = b2Mul(xfB, m_localAnchorB); - - b2Vec2 axis = b2Mul(xfA.q, m_localXAxisA); - - b2Color c1(0.7f, 0.7f, 0.7f); - b2Color c2(0.3f, 0.9f, 0.3f); - b2Color c3(0.9f, 0.3f, 0.3f); - b2Color c4(0.3f, 0.3f, 0.9f); - b2Color c5(0.4f, 0.4f, 0.4f); - - draw->DrawSegment(pA, pB, c5); - - if (m_enableLimit) - { - b2Vec2 lower = pA + m_lowerTranslation * axis; - b2Vec2 upper = pA + m_upperTranslation * axis; - b2Vec2 perp = b2Mul(xfA.q, m_localYAxisA); - draw->DrawSegment(lower, upper, c1); - draw->DrawSegment(lower - 0.5f * perp, lower + 0.5f * perp, c2); - draw->DrawSegment(upper - 0.5f * perp, upper + 0.5f * perp, c3); - } - else - { - draw->DrawSegment(pA - 1.0f * axis, pA + 1.0f * axis, c1); - } - - draw->DrawPoint(pA, 5.0f, c1); - draw->DrawPoint(pB, 5.0f, c4); -} diff --git a/3rdparty/box2d/src/dynamics/b2_world.cpp b/3rdparty/box2d/src/dynamics/b2_world.cpp deleted file mode 100644 index 78ec084c800b..000000000000 --- a/3rdparty/box2d/src/dynamics/b2_world.cpp +++ /dev/null @@ -1,1322 +0,0 @@ -// MIT License - -// Copyright (c) 2019 Erin Catto - -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: - -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. - -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -#include "b2_contact_solver.h" -#include "b2_island.h" - -#include "box2d/b2_body.h" -#include "box2d/b2_broad_phase.h" -#include "box2d/b2_chain_shape.h" -#include "box2d/b2_circle_shape.h" -#include "box2d/b2_collision.h" -#include "box2d/b2_contact.h" -#include "box2d/b2_draw.h" -#include "box2d/b2_edge_shape.h" -#include "box2d/b2_fixture.h" -#include "box2d/b2_polygon_shape.h" -#include "box2d/b2_pulley_joint.h" -#include "box2d/b2_time_of_impact.h" -#include "box2d/b2_timer.h" -#include "box2d/b2_world.h" - -#include - -b2World::b2World(const b2Vec2& gravity) -{ - m_destructionListener = nullptr; - m_debugDraw = nullptr; - - m_bodyList = nullptr; - m_jointList = nullptr; - - m_bodyCount = 0; - m_jointCount = 0; - - m_warmStarting = true; - m_continuousPhysics = true; - m_subStepping = false; - - m_stepComplete = true; - - m_allowSleep = true; - m_gravity = gravity; - - m_newContacts = false; - m_locked = false; - m_clearForces = true; - - m_inv_dt0 = 0.0f; - - m_contactManager.m_allocator = &m_blockAllocator; - - memset(&m_profile, 0, sizeof(b2Profile)); -} - -b2World::~b2World() -{ - // Some shapes allocate using b2Alloc. - b2Body* b = m_bodyList; - while (b) - { - b2Body* bNext = b->m_next; - - b2Fixture* f = b->m_fixtureList; - while (f) - { - b2Fixture* fNext = f->m_next; - f->m_proxyCount = 0; - f->Destroy(&m_blockAllocator); - f = fNext; - } - - b = bNext; - } -} - -void b2World::SetDestructionListener(b2DestructionListener* listener) -{ - m_destructionListener = listener; -} - -void b2World::SetContactFilter(b2ContactFilter* filter) -{ - m_contactManager.m_contactFilter = filter; -} - -void b2World::SetContactListener(b2ContactListener* listener) -{ - m_contactManager.m_contactListener = listener; -} - -void b2World::SetDebugDraw(b2Draw* debugDraw) -{ - m_debugDraw = debugDraw; -} - -b2Body* b2World::CreateBody(const b2BodyDef* def) -{ - b2Assert(IsLocked() == false); - if (IsLocked()) - { - return nullptr; - } - - void* mem = m_blockAllocator.Allocate(sizeof(b2Body)); - b2Body* b = new (mem) b2Body(def, this); - - // Add to world doubly linked list. - b->m_prev = nullptr; - b->m_next = m_bodyList; - if (m_bodyList) - { - m_bodyList->m_prev = b; - } - m_bodyList = b; - ++m_bodyCount; - - return b; -} - -void b2World::DestroyBody(b2Body* b) -{ - b2Assert(m_bodyCount > 0); - b2Assert(IsLocked() == false); - if (IsLocked()) - { - return; - } - - // Delete the attached joints. - b2JointEdge* je = b->m_jointList; - while (je) - { - b2JointEdge* je0 = je; - je = je->next; - - if (m_destructionListener) - { - m_destructionListener->SayGoodbye(je0->joint); - } - - DestroyJoint(je0->joint); - - b->m_jointList = je; - } - b->m_jointList = nullptr; - - // Delete the attached contacts. - b2ContactEdge* ce = b->m_contactList; - while (ce) - { - b2ContactEdge* ce0 = ce; - ce = ce->next; - m_contactManager.Destroy(ce0->contact); - } - b->m_contactList = nullptr; - - // Delete the attached fixtures. This destroys broad-phase proxies. - b2Fixture* f = b->m_fixtureList; - while (f) - { - b2Fixture* f0 = f; - f = f->m_next; - - if (m_destructionListener) - { - m_destructionListener->SayGoodbye(f0); - } - - f0->DestroyProxies(&m_contactManager.m_broadPhase); - f0->Destroy(&m_blockAllocator); - f0->~b2Fixture(); - m_blockAllocator.Free(f0, sizeof(b2Fixture)); - - b->m_fixtureList = f; - b->m_fixtureCount -= 1; - } - b->m_fixtureList = nullptr; - b->m_fixtureCount = 0; - - // Remove world body list. - if (b->m_prev) - { - b->m_prev->m_next = b->m_next; - } - - if (b->m_next) - { - b->m_next->m_prev = b->m_prev; - } - - if (b == m_bodyList) - { - m_bodyList = b->m_next; - } - - --m_bodyCount; - b->~b2Body(); - m_blockAllocator.Free(b, sizeof(b2Body)); -} - -b2Joint* b2World::CreateJoint(const b2JointDef* def) -{ - b2Assert(IsLocked() == false); - if (IsLocked()) - { - return nullptr; - } - - b2Joint* j = b2Joint::Create(def, &m_blockAllocator); - - // Connect to the world list. - j->m_prev = nullptr; - j->m_next = m_jointList; - if (m_jointList) - { - m_jointList->m_prev = j; - } - m_jointList = j; - ++m_jointCount; - - // Connect to the bodies' doubly linked lists. - j->m_edgeA.joint = j; - j->m_edgeA.other = j->m_bodyB; - j->m_edgeA.prev = nullptr; - j->m_edgeA.next = j->m_bodyA->m_jointList; - if (j->m_bodyA->m_jointList) j->m_bodyA->m_jointList->prev = &j->m_edgeA; - j->m_bodyA->m_jointList = &j->m_edgeA; - - j->m_edgeB.joint = j; - j->m_edgeB.other = j->m_bodyA; - j->m_edgeB.prev = nullptr; - j->m_edgeB.next = j->m_bodyB->m_jointList; - if (j->m_bodyB->m_jointList) j->m_bodyB->m_jointList->prev = &j->m_edgeB; - j->m_bodyB->m_jointList = &j->m_edgeB; - - b2Body* bodyA = def->bodyA; - b2Body* bodyB = def->bodyB; - - // If the joint prevents collisions, then flag any contacts for filtering. - if (def->collideConnected == false) - { - b2ContactEdge* edge = bodyB->GetContactList(); - while (edge) - { - if (edge->other == bodyA) - { - // Flag the contact for filtering at the next time step (where either - // body is awake). - edge->contact->FlagForFiltering(); - } - - edge = edge->next; - } - } - - // Note: creating a joint doesn't wake the bodies. - - return j; -} - -void b2World::DestroyJoint(b2Joint* j) -{ - b2Assert(IsLocked() == false); - if (IsLocked()) - { - return; - } - - bool collideConnected = j->m_collideConnected; - - // Remove from the doubly linked list. - if (j->m_prev) - { - j->m_prev->m_next = j->m_next; - } - - if (j->m_next) - { - j->m_next->m_prev = j->m_prev; - } - - if (j == m_jointList) - { - m_jointList = j->m_next; - } - - // Disconnect from island graph. - b2Body* bodyA = j->m_bodyA; - b2Body* bodyB = j->m_bodyB; - - // Wake up connected bodies. - bodyA->SetAwake(true); - bodyB->SetAwake(true); - - // Remove from body 1. - if (j->m_edgeA.prev) - { - j->m_edgeA.prev->next = j->m_edgeA.next; - } - - if (j->m_edgeA.next) - { - j->m_edgeA.next->prev = j->m_edgeA.prev; - } - - if (&j->m_edgeA == bodyA->m_jointList) - { - bodyA->m_jointList = j->m_edgeA.next; - } - - j->m_edgeA.prev = nullptr; - j->m_edgeA.next = nullptr; - - // Remove from body 2 - if (j->m_edgeB.prev) - { - j->m_edgeB.prev->next = j->m_edgeB.next; - } - - if (j->m_edgeB.next) - { - j->m_edgeB.next->prev = j->m_edgeB.prev; - } - - if (&j->m_edgeB == bodyB->m_jointList) - { - bodyB->m_jointList = j->m_edgeB.next; - } - - j->m_edgeB.prev = nullptr; - j->m_edgeB.next = nullptr; - - b2Joint::Destroy(j, &m_blockAllocator); - - b2Assert(m_jointCount > 0); - --m_jointCount; - - // If the joint prevents collisions, then flag any contacts for filtering. - if (collideConnected == false) - { - b2ContactEdge* edge = bodyB->GetContactList(); - while (edge) - { - if (edge->other == bodyA) - { - // Flag the contact for filtering at the next time step (where either - // body is awake). - edge->contact->FlagForFiltering(); - } - - edge = edge->next; - } - } -} - -// -void b2World::SetAllowSleeping(bool flag) -{ - if (flag == m_allowSleep) - { - return; - } - - m_allowSleep = flag; - if (m_allowSleep == false) - { - for (b2Body* b = m_bodyList; b; b = b->m_next) - { - b->SetAwake(true); - } - } -} - -// Find islands, integrate and solve constraints, solve position constraints -void b2World::Solve(const b2TimeStep& step) -{ - m_profile.solveInit = 0.0f; - m_profile.solveVelocity = 0.0f; - m_profile.solvePosition = 0.0f; - - // Size the island for the worst case. - b2Island island(m_bodyCount, - m_contactManager.m_contactCount, - m_jointCount, - &m_stackAllocator, - m_contactManager.m_contactListener); - - // Clear all the island flags. - for (b2Body* b = m_bodyList; b; b = b->m_next) - { - b->m_flags &= ~b2Body::e_islandFlag; - } - for (b2Contact* c = m_contactManager.m_contactList; c; c = c->m_next) - { - c->m_flags &= ~b2Contact::e_islandFlag; - } - for (b2Joint* j = m_jointList; j; j = j->m_next) - { - j->m_islandFlag = false; - } - - // Build and simulate all awake islands. - int32 stackSize = m_bodyCount; - b2Body** stack = (b2Body**)m_stackAllocator.Allocate(stackSize * sizeof(b2Body*)); - for (b2Body* seed = m_bodyList; seed; seed = seed->m_next) - { - if (seed->m_flags & b2Body::e_islandFlag) - { - continue; - } - - if (seed->IsAwake() == false || seed->IsEnabled() == false) - { - continue; - } - - // The seed can be dynamic or kinematic. - if (seed->GetType() == b2_staticBody) - { - continue; - } - - // Reset island and stack. - island.Clear(); - int32 stackCount = 0; - stack[stackCount++] = seed; - seed->m_flags |= b2Body::e_islandFlag; - - // Perform a depth first search (DFS) on the constraint graph. - while (stackCount > 0) - { - // Grab the next body off the stack and add it to the island. - b2Body* b = stack[--stackCount]; - b2Assert(b->IsEnabled() == true); - island.Add(b); - - // To keep islands as small as possible, we don't - // propagate islands across static bodies. - if (b->GetType() == b2_staticBody) - { - continue; - } - - // Make sure the body is awake (without resetting sleep timer). - b->m_flags |= b2Body::e_awakeFlag; - - // Search all contacts connected to this body. - for (b2ContactEdge* ce = b->m_contactList; ce; ce = ce->next) - { - b2Contact* contact = ce->contact; - - // Has this contact already been added to an island? - if (contact->m_flags & b2Contact::e_islandFlag) - { - continue; - } - - // Is this contact solid and touching? - if (contact->IsEnabled() == false || - contact->IsTouching() == false) - { - continue; - } - - // Skip sensors. - bool sensorA = contact->m_fixtureA->m_isSensor; - bool sensorB = contact->m_fixtureB->m_isSensor; - if (sensorA || sensorB) - { - continue; - } - - island.Add(contact); - contact->m_flags |= b2Contact::e_islandFlag; - - b2Body* other = ce->other; - - // Was the other body already added to this island? - if (other->m_flags & b2Body::e_islandFlag) - { - continue; - } - - b2Assert(stackCount < stackSize); - stack[stackCount++] = other; - other->m_flags |= b2Body::e_islandFlag; - } - - // Search all joints connect to this body. - for (b2JointEdge* je = b->m_jointList; je; je = je->next) - { - if (je->joint->m_islandFlag == true) - { - continue; - } - - b2Body* other = je->other; - - // Don't simulate joints connected to disabled bodies. - if (other->IsEnabled() == false) - { - continue; - } - - island.Add(je->joint); - je->joint->m_islandFlag = true; - - if (other->m_flags & b2Body::e_islandFlag) - { - continue; - } - - b2Assert(stackCount < stackSize); - stack[stackCount++] = other; - other->m_flags |= b2Body::e_islandFlag; - } - } - - b2Profile profile; - island.Solve(&profile, step, m_gravity, m_allowSleep); - m_profile.solveInit += profile.solveInit; - m_profile.solveVelocity += profile.solveVelocity; - m_profile.solvePosition += profile.solvePosition; - - // Post solve cleanup. - for (int32 i = 0; i < island.m_bodyCount; ++i) - { - // Allow static bodies to participate in other islands. - b2Body* b = island.m_bodies[i]; - if (b->GetType() == b2_staticBody) - { - b->m_flags &= ~b2Body::e_islandFlag; - } - } - } - - m_stackAllocator.Free(stack); - - { - b2Timer timer; - // Synchronize fixtures, check for out of range bodies. - for (b2Body* b = m_bodyList; b; b = b->GetNext()) - { - // If a body was not in an island then it did not move. - if ((b->m_flags & b2Body::e_islandFlag) == 0) - { - continue; - } - - if (b->GetType() == b2_staticBody) - { - continue; - } - - // Update fixtures (for broad-phase). - b->SynchronizeFixtures(); - } - - // Look for new contacts. - m_contactManager.FindNewContacts(); - m_profile.broadphase = timer.GetMilliseconds(); - } -} - -// Find TOI contacts and solve them. -void b2World::SolveTOI(const b2TimeStep& step) -{ - b2Island island(2 * b2_maxTOIContacts, b2_maxTOIContacts, 0, &m_stackAllocator, m_contactManager.m_contactListener); - - if (m_stepComplete) - { - for (b2Body* b = m_bodyList; b; b = b->m_next) - { - b->m_flags &= ~b2Body::e_islandFlag; - b->m_sweep.alpha0 = 0.0f; - } - - for (b2Contact* c = m_contactManager.m_contactList; c; c = c->m_next) - { - // Invalidate TOI - c->m_flags &= ~(b2Contact::e_toiFlag | b2Contact::e_islandFlag); - c->m_toiCount = 0; - c->m_toi = 1.0f; - } - } - - // Find TOI events and solve them. - for (;;) - { - // Find the first TOI. - b2Contact* minContact = nullptr; - float minAlpha = 1.0f; - - for (b2Contact* c = m_contactManager.m_contactList; c; c = c->m_next) - { - // Is this contact disabled? - if (c->IsEnabled() == false) - { - continue; - } - - // Prevent excessive sub-stepping. - if (c->m_toiCount > b2_maxSubSteps) - { - continue; - } - - float alpha = 1.0f; - if (c->m_flags & b2Contact::e_toiFlag) - { - // This contact has a valid cached TOI. - alpha = c->m_toi; - } - else - { - b2Fixture* fA = c->GetFixtureA(); - b2Fixture* fB = c->GetFixtureB(); - - // Is there a sensor? - if (fA->IsSensor() || fB->IsSensor()) - { - continue; - } - - b2Body* bA = fA->GetBody(); - b2Body* bB = fB->GetBody(); - - b2BodyType typeA = bA->m_type; - b2BodyType typeB = bB->m_type; - b2Assert(typeA == b2_dynamicBody || typeB == b2_dynamicBody); - - bool activeA = bA->IsAwake() && typeA != b2_staticBody; - bool activeB = bB->IsAwake() && typeB != b2_staticBody; - - // Is at least one body active (awake and dynamic or kinematic)? - if (activeA == false && activeB == false) - { - continue; - } - - bool collideA = bA->IsBullet() || typeA != b2_dynamicBody; - bool collideB = bB->IsBullet() || typeB != b2_dynamicBody; - - // Are these two non-bullet dynamic bodies? - if (collideA == false && collideB == false) - { - continue; - } - - // Compute the TOI for this contact. - // Put the sweeps onto the same time interval. - float alpha0 = bA->m_sweep.alpha0; - - if (bA->m_sweep.alpha0 < bB->m_sweep.alpha0) - { - alpha0 = bB->m_sweep.alpha0; - bA->m_sweep.Advance(alpha0); - } - else if (bB->m_sweep.alpha0 < bA->m_sweep.alpha0) - { - alpha0 = bA->m_sweep.alpha0; - bB->m_sweep.Advance(alpha0); - } - - b2Assert(alpha0 < 1.0f); - - int32 indexA = c->GetChildIndexA(); - int32 indexB = c->GetChildIndexB(); - - // Compute the time of impact in interval [0, minTOI] - b2TOIInput input; - input.proxyA.Set(fA->GetShape(), indexA); - input.proxyB.Set(fB->GetShape(), indexB); - input.sweepA = bA->m_sweep; - input.sweepB = bB->m_sweep; - input.tMax = 1.0f; - - b2TOIOutput output; - b2TimeOfImpact(&output, &input); - - // Beta is the fraction of the remaining portion of the . - float beta = output.t; - if (output.state == b2TOIOutput::e_touching) - { - alpha = b2Min(alpha0 + (1.0f - alpha0) * beta, 1.0f); - } - else - { - alpha = 1.0f; - } - - c->m_toi = alpha; - c->m_flags |= b2Contact::e_toiFlag; - } - - if (alpha < minAlpha) - { - // This is the minimum TOI found so far. - minContact = c; - minAlpha = alpha; - } - } - - if (minContact == nullptr || 1.0f - 10.0f * b2_epsilon < minAlpha) - { - // No more TOI events. Done! - m_stepComplete = true; - break; - } - - // Advance the bodies to the TOI. - b2Fixture* fA = minContact->GetFixtureA(); - b2Fixture* fB = minContact->GetFixtureB(); - b2Body* bA = fA->GetBody(); - b2Body* bB = fB->GetBody(); - - b2Sweep backup1 = bA->m_sweep; - b2Sweep backup2 = bB->m_sweep; - - bA->Advance(minAlpha); - bB->Advance(minAlpha); - - // The TOI contact likely has some new contact points. - minContact->Update(m_contactManager.m_contactListener); - minContact->m_flags &= ~b2Contact::e_toiFlag; - ++minContact->m_toiCount; - - // Is the contact solid? - if (minContact->IsEnabled() == false || minContact->IsTouching() == false) - { - // Restore the sweeps. - minContact->SetEnabled(false); - bA->m_sweep = backup1; - bB->m_sweep = backup2; - bA->SynchronizeTransform(); - bB->SynchronizeTransform(); - continue; - } - - bA->SetAwake(true); - bB->SetAwake(true); - - // Build the island - island.Clear(); - island.Add(bA); - island.Add(bB); - island.Add(minContact); - - bA->m_flags |= b2Body::e_islandFlag; - bB->m_flags |= b2Body::e_islandFlag; - minContact->m_flags |= b2Contact::e_islandFlag; - - // Get contacts on bodyA and bodyB. - b2Body* bodies[2] = {bA, bB}; - for (int32 i = 0; i < 2; ++i) - { - b2Body* body = bodies[i]; - if (body->m_type == b2_dynamicBody) - { - for (b2ContactEdge* ce = body->m_contactList; ce; ce = ce->next) - { - if (island.m_bodyCount == island.m_bodyCapacity) - { - break; - } - - if (island.m_contactCount == island.m_contactCapacity) - { - break; - } - - b2Contact* contact = ce->contact; - - // Has this contact already been added to the island? - if (contact->m_flags & b2Contact::e_islandFlag) - { - continue; - } - - // Only add static, kinematic, or bullet bodies. - b2Body* other = ce->other; - if (other->m_type == b2_dynamicBody && - body->IsBullet() == false && other->IsBullet() == false) - { - continue; - } - - // Skip sensors. - bool sensorA = contact->m_fixtureA->m_isSensor; - bool sensorB = contact->m_fixtureB->m_isSensor; - if (sensorA || sensorB) - { - continue; - } - - // Tentatively advance the body to the TOI. - b2Sweep backup = other->m_sweep; - if ((other->m_flags & b2Body::e_islandFlag) == 0) - { - other->Advance(minAlpha); - } - - // Update the contact points - contact->Update(m_contactManager.m_contactListener); - - // Was the contact disabled by the user? - if (contact->IsEnabled() == false) - { - other->m_sweep = backup; - other->SynchronizeTransform(); - continue; - } - - // Are there contact points? - if (contact->IsTouching() == false) - { - other->m_sweep = backup; - other->SynchronizeTransform(); - continue; - } - - // Add the contact to the island - contact->m_flags |= b2Contact::e_islandFlag; - island.Add(contact); - - // Has the other body already been added to the island? - if (other->m_flags & b2Body::e_islandFlag) - { - continue; - } - - // Add the other body to the island. - other->m_flags |= b2Body::e_islandFlag; - - if (other->m_type != b2_staticBody) - { - other->SetAwake(true); - } - - island.Add(other); - } - } - } - - b2TimeStep subStep; - subStep.dt = (1.0f - minAlpha) * step.dt; - subStep.inv_dt = 1.0f / subStep.dt; - subStep.dtRatio = 1.0f; - subStep.positionIterations = 20; - subStep.velocityIterations = step.velocityIterations; - subStep.warmStarting = false; - island.SolveTOI(subStep, bA->m_islandIndex, bB->m_islandIndex); - - // Reset island flags and synchronize broad-phase proxies. - for (int32 i = 0; i < island.m_bodyCount; ++i) - { - b2Body* body = island.m_bodies[i]; - body->m_flags &= ~b2Body::e_islandFlag; - - if (body->m_type != b2_dynamicBody) - { - continue; - } - - body->SynchronizeFixtures(); - - // Invalidate all contact TOIs on this displaced body. - for (b2ContactEdge* ce = body->m_contactList; ce; ce = ce->next) - { - ce->contact->m_flags &= ~(b2Contact::e_toiFlag | b2Contact::e_islandFlag); - } - } - - // Commit fixture proxy movements to the broad-phase so that new contacts are created. - // Also, some contacts can be destroyed. - m_contactManager.FindNewContacts(); - - if (m_subStepping) - { - m_stepComplete = false; - break; - } - } -} - -void b2World::Step(float dt, int32 velocityIterations, int32 positionIterations) -{ - b2Timer stepTimer; - - // If new fixtures were added, we need to find the new contacts. - if (m_newContacts) - { - m_contactManager.FindNewContacts(); - m_newContacts = false; - } - - m_locked = true; - - b2TimeStep step; - step.dt = dt; - step.velocityIterations = velocityIterations; - step.positionIterations = positionIterations; - if (dt > 0.0f) - { - step.inv_dt = 1.0f / dt; - } - else - { - step.inv_dt = 0.0f; - } - - step.dtRatio = m_inv_dt0 * dt; - - step.warmStarting = m_warmStarting; - - // Update contacts. This is where some contacts are destroyed. - { - b2Timer timer; - m_contactManager.Collide(); - m_profile.collide = timer.GetMilliseconds(); - } - - // Integrate velocities, solve velocity constraints, and integrate positions. - if (m_stepComplete && step.dt > 0.0f) - { - b2Timer timer; - Solve(step); - m_profile.solve = timer.GetMilliseconds(); - } - - // Handle TOI events. - if (m_continuousPhysics && step.dt > 0.0f) - { - b2Timer timer; - SolveTOI(step); - m_profile.solveTOI = timer.GetMilliseconds(); - } - - if (step.dt > 0.0f) - { - m_inv_dt0 = step.inv_dt; - } - - if (m_clearForces) - { - ClearForces(); - } - - m_locked = false; - - m_profile.step = stepTimer.GetMilliseconds(); -} - -void b2World::ClearForces() -{ - for (b2Body* body = m_bodyList; body; body = body->GetNext()) - { - body->m_force.SetZero(); - body->m_torque = 0.0f; - } -} - -struct b2WorldQueryWrapper -{ - bool QueryCallback(int32 proxyId) - { - b2FixtureProxy* proxy = (b2FixtureProxy*)broadPhase->GetUserData(proxyId); - return callback->ReportFixture(proxy->fixture); - } - - const b2BroadPhase* broadPhase; - b2QueryCallback* callback; -}; - -void b2World::QueryAABB(b2QueryCallback* callback, const b2AABB& aabb) const -{ - b2WorldQueryWrapper wrapper; - wrapper.broadPhase = &m_contactManager.m_broadPhase; - wrapper.callback = callback; - m_contactManager.m_broadPhase.Query(&wrapper, aabb); -} - -struct b2WorldRayCastWrapper -{ - float RayCastCallback(const b2RayCastInput& input, int32 proxyId) - { - void* userData = broadPhase->GetUserData(proxyId); - b2FixtureProxy* proxy = (b2FixtureProxy*)userData; - b2Fixture* fixture = proxy->fixture; - int32 index = proxy->childIndex; - b2RayCastOutput output; - bool hit = fixture->RayCast(&output, input, index); - - if (hit) - { - float fraction = output.fraction; - b2Vec2 point = (1.0f - fraction) * input.p1 + fraction * input.p2; - return callback->ReportFixture(fixture, point, output.normal, fraction); - } - - return input.maxFraction; - } - - const b2BroadPhase* broadPhase; - b2RayCastCallback* callback; -}; - -void b2World::RayCast(b2RayCastCallback* callback, const b2Vec2& point1, const b2Vec2& point2) const -{ - b2WorldRayCastWrapper wrapper; - wrapper.broadPhase = &m_contactManager.m_broadPhase; - wrapper.callback = callback; - b2RayCastInput input; - input.maxFraction = 1.0f; - input.p1 = point1; - input.p2 = point2; - m_contactManager.m_broadPhase.RayCast(&wrapper, input); -} - -void b2World::DrawShape(b2Fixture* fixture, const b2Transform& xf, const b2Color& color) -{ - switch (fixture->GetType()) - { - case b2Shape::e_circle: - { - b2CircleShape* circle = (b2CircleShape*)fixture->GetShape(); - - b2Vec2 center = b2Mul(xf, circle->m_p); - float radius = circle->m_radius; - b2Vec2 axis = b2Mul(xf.q, b2Vec2(1.0f, 0.0f)); - - m_debugDraw->DrawSolidCircle(center, radius, axis, color); - } - break; - - case b2Shape::e_edge: - { - b2EdgeShape* edge = (b2EdgeShape*)fixture->GetShape(); - b2Vec2 v1 = b2Mul(xf, edge->m_vertex1); - b2Vec2 v2 = b2Mul(xf, edge->m_vertex2); - m_debugDraw->DrawSegment(v1, v2, color); - - if (edge->m_oneSided == false) - { - m_debugDraw->DrawPoint(v1, 4.0f, color); - m_debugDraw->DrawPoint(v2, 4.0f, color); - } - } - break; - - case b2Shape::e_chain: - { - b2ChainShape* chain = (b2ChainShape*)fixture->GetShape(); - int32 count = chain->m_count; - const b2Vec2* vertices = chain->m_vertices; - - b2Vec2 v1 = b2Mul(xf, vertices[0]); - for (int32 i = 1; i < count; ++i) - { - b2Vec2 v2 = b2Mul(xf, vertices[i]); - m_debugDraw->DrawSegment(v1, v2, color); - v1 = v2; - } - } - break; - - case b2Shape::e_polygon: - { - b2PolygonShape* poly = (b2PolygonShape*)fixture->GetShape(); - int32 vertexCount = poly->m_count; - b2Assert(vertexCount <= b2_maxPolygonVertices); - b2Vec2 vertices[b2_maxPolygonVertices]; - - for (int32 i = 0; i < vertexCount; ++i) - { - vertices[i] = b2Mul(xf, poly->m_vertices[i]); - } - - m_debugDraw->DrawSolidPolygon(vertices, vertexCount, color); - } - break; - - default: - break; - } -} - -void b2World::DebugDraw() -{ - if (m_debugDraw == nullptr) - { - return; - } - - uint32 flags = m_debugDraw->GetFlags(); - - if (flags & b2Draw::e_shapeBit) - { - for (b2Body* b = m_bodyList; b; b = b->GetNext()) - { - const b2Transform& xf = b->GetTransform(); - for (b2Fixture* f = b->GetFixtureList(); f; f = f->GetNext()) - { - if (b->GetType() == b2_dynamicBody && b->m_mass == 0.0f) - { - // Bad body - DrawShape(f, xf, b2Color(1.0f, 0.0f, 0.0f)); - } - else if (b->IsEnabled() == false) - { - DrawShape(f, xf, b2Color(0.5f, 0.5f, 0.3f)); - } - else if (b->GetType() == b2_staticBody) - { - DrawShape(f, xf, b2Color(0.5f, 0.9f, 0.5f)); - } - else if (b->GetType() == b2_kinematicBody) - { - DrawShape(f, xf, b2Color(0.5f, 0.5f, 0.9f)); - } - else if (b->IsAwake() == false) - { - DrawShape(f, xf, b2Color(0.6f, 0.6f, 0.6f)); - } - else - { - DrawShape(f, xf, b2Color(0.9f, 0.7f, 0.7f)); - } - } - } - } - - if (flags & b2Draw::e_jointBit) - { - for (b2Joint* j = m_jointList; j; j = j->GetNext()) - { - j->Draw(m_debugDraw); - } - } - - if (flags & b2Draw::e_pairBit) - { - b2Color color(0.3f, 0.9f, 0.9f); - for (b2Contact* c = m_contactManager.m_contactList; c; c = c->GetNext()) - { - b2Fixture* fixtureA = c->GetFixtureA(); - b2Fixture* fixtureB = c->GetFixtureB(); - int32 indexA = c->GetChildIndexA(); - int32 indexB = c->GetChildIndexB(); - b2Vec2 cA = fixtureA->GetAABB(indexA).GetCenter(); - b2Vec2 cB = fixtureB->GetAABB(indexB).GetCenter(); - - m_debugDraw->DrawSegment(cA, cB, color); - } - } - - if (flags & b2Draw::e_aabbBit) - { - b2Color color(0.9f, 0.3f, 0.9f); - b2BroadPhase* bp = &m_contactManager.m_broadPhase; - - for (b2Body* b = m_bodyList; b; b = b->GetNext()) - { - if (b->IsEnabled() == false) - { - continue; - } - - for (b2Fixture* f = b->GetFixtureList(); f; f = f->GetNext()) - { - for (int32 i = 0; i < f->m_proxyCount; ++i) - { - b2FixtureProxy* proxy = f->m_proxies + i; - b2AABB aabb = bp->GetFatAABB(proxy->proxyId); - b2Vec2 vs[4]; - vs[0].Set(aabb.lowerBound.x, aabb.lowerBound.y); - vs[1].Set(aabb.upperBound.x, aabb.lowerBound.y); - vs[2].Set(aabb.upperBound.x, aabb.upperBound.y); - vs[3].Set(aabb.lowerBound.x, aabb.upperBound.y); - - m_debugDraw->DrawPolygon(vs, 4, color); - } - } - } - } - - if (flags & b2Draw::e_centerOfMassBit) - { - for (b2Body* b = m_bodyList; b; b = b->GetNext()) - { - b2Transform xf = b->GetTransform(); - xf.p = b->GetWorldCenter(); - m_debugDraw->DrawTransform(xf); - } - } -} - -int32 b2World::GetProxyCount() const -{ - return m_contactManager.m_broadPhase.GetProxyCount(); -} - -int32 b2World::GetTreeHeight() const -{ - return m_contactManager.m_broadPhase.GetTreeHeight(); -} - -int32 b2World::GetTreeBalance() const -{ - return m_contactManager.m_broadPhase.GetTreeBalance(); -} - -float b2World::GetTreeQuality() const -{ - return m_contactManager.m_broadPhase.GetTreeQuality(); -} - -void b2World::ShiftOrigin(const b2Vec2& newOrigin) -{ - b2Assert(m_locked == false); - if (m_locked) - { - return; - } - - for (b2Body* b = m_bodyList; b; b = b->m_next) - { - b->m_xf.p -= newOrigin; - b->m_sweep.c0 -= newOrigin; - b->m_sweep.c -= newOrigin; - } - - for (b2Joint* j = m_jointList; j; j = j->m_next) - { - j->ShiftOrigin(newOrigin); - } - - m_contactManager.m_broadPhase.ShiftOrigin(newOrigin); -} - -void b2World::Dump() -{ - if (m_locked) - { - return; - } - - b2OpenDump("box2d_dump.inl"); - - b2Dump("b2Vec2 g(%.9g, %.9g);\n", m_gravity.x, m_gravity.y); - b2Dump("m_world->SetGravity(g);\n"); - - b2Dump("b2Body** bodies = (b2Body**)b2Alloc(%d * sizeof(b2Body*));\n", m_bodyCount); - b2Dump("b2Joint** joints = (b2Joint**)b2Alloc(%d * sizeof(b2Joint*));\n", m_jointCount); - - int32 i = 0; - for (b2Body* b = m_bodyList; b; b = b->m_next) - { - b->m_islandIndex = i; - b->Dump(); - ++i; - } - - i = 0; - for (b2Joint* j = m_jointList; j; j = j->m_next) - { - j->m_index = i; - ++i; - } - - // First pass on joints, skip gear joints. - for (b2Joint* j = m_jointList; j; j = j->m_next) - { - if (j->m_type == e_gearJoint) - { - continue; - } - - b2Dump("{\n"); - j->Dump(); - b2Dump("}\n"); - } - - // Second pass on joints, only gear joints. - for (b2Joint* j = m_jointList; j; j = j->m_next) - { - if (j->m_type != e_gearJoint) - { - continue; - } - - b2Dump("{\n"); - j->Dump(); - b2Dump("}\n"); - } - - b2Dump("b2Free(joints);\n"); - b2Dump("b2Free(bodies);\n"); - b2Dump("joints = nullptr;\n"); - b2Dump("bodies = nullptr;\n"); - - b2CloseDump(); -} diff --git a/3rdparty/box2d/src/dynamics/b2_world_callbacks.cpp b/3rdparty/box2d/src/dynamics/b2_world_callbacks.cpp deleted file mode 100644 index e1583e221b3b..000000000000 --- a/3rdparty/box2d/src/dynamics/b2_world_callbacks.cpp +++ /dev/null @@ -1,40 +0,0 @@ -// MIT License - -// Copyright (c) 2019 Erin Catto - -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: - -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. - -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -#include "box2d/b2_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/3rdparty/box2d/src/geometry.c b/3rdparty/box2d/src/geometry.c new file mode 100644 index 000000000000..94d33d3689ab --- /dev/null +++ b/3rdparty/box2d/src/geometry.c @@ -0,0 +1,893 @@ +// SPDX-FileCopyrightText: 2023 Erin Catto +// SPDX-License-Identifier: MIT + +#include "aabb.h" +#include "core.h" +#include "shape.h" + +#include "box2d/collision.h" +#include "box2d/math_functions.h" + +#include +#include + +_Static_assert( b2_maxPolygonVertices > 2, "must be 3 or more" ); + +bool b2IsValidRay( const b2RayCastInput* input ) +{ + bool isValid = b2Vec2_IsValid( input->origin ) && b2Vec2_IsValid( input->translation ) && b2IsValid( input->maxFraction ) && + 0.0f <= input->maxFraction && input->maxFraction < b2_huge; + return isValid; +} + +static b2Vec2 b2ComputePolygonCentroid( const b2Vec2* vertices, int32_t count ) +{ + b2Vec2 center = { 0.0f, 0.0f }; + float area = 0.0f; + + // Get a reference point for forming triangles. + // Use the first vertex to reduce round-off errors. + b2Vec2 origin = vertices[0]; + + const float inv3 = 1.0f / 3.0f; + + for ( int32_t i = 1; i < count - 1; ++i ) + { + // Triangle edges + b2Vec2 e1 = b2Sub( vertices[i], origin ); + b2Vec2 e2 = b2Sub( vertices[i + 1], origin ); + float a = 0.5f * b2Cross( e1, e2 ); + + // Area weighted centroid + center = b2MulAdd( center, a * inv3, b2Add( e1, e2 ) ); + area += a; + } + + B2_ASSERT( area > FLT_EPSILON ); + float invArea = 1.0f / area; + center.x *= invArea; + center.y *= invArea; + + // Restore offset + center = b2Add( origin, center ); + + return center; +} + +b2Polygon b2MakePolygon( const b2Hull* hull, float radius ) +{ + B2_ASSERT( b2ValidateHull( hull ) ); + + if ( hull->count < 3 ) + { + // Handle a bad hull when assertions are disabled + return b2MakeSquare( 0.5f ); + } + + b2Polygon shape = { 0 }; + shape.count = hull->count; + shape.radius = radius; + + // Copy vertices + for ( int32_t i = 0; i < shape.count; ++i ) + { + shape.vertices[i] = hull->points[i]; + } + + // Compute normals. Ensure the edges have non-zero length. + for ( int32_t i = 0; i < shape.count; ++i ) + { + int32_t i1 = i; + int32_t i2 = i + 1 < shape.count ? i + 1 : 0; + b2Vec2 edge = b2Sub( shape.vertices[i2], shape.vertices[i1] ); + B2_ASSERT( b2Dot( edge, edge ) > FLT_EPSILON * FLT_EPSILON ); + shape.normals[i] = b2Normalize( b2CrossVS( edge, 1.0f ) ); + } + + shape.centroid = b2ComputePolygonCentroid( shape.vertices, shape.count ); + + return shape; +} + +b2Polygon b2MakeOffsetPolygon( const b2Hull* hull, float radius, b2Transform transform ) +{ + B2_ASSERT( b2ValidateHull( hull ) ); + + if ( hull->count < 3 ) + { + // Handle a bad hull when assertions are disabled + return b2MakeSquare( 0.5f ); + } + + b2Polygon shape = { 0 }; + shape.count = hull->count; + shape.radius = radius; + + // Copy vertices + for ( int32_t i = 0; i < shape.count; ++i ) + { + shape.vertices[i] = b2TransformPoint( transform, hull->points[i] ); + } + + // Compute normals. Ensure the edges have non-zero length. + for ( int32_t i = 0; i < shape.count; ++i ) + { + int32_t i1 = i; + int32_t i2 = i + 1 < shape.count ? i + 1 : 0; + b2Vec2 edge = b2Sub( shape.vertices[i2], shape.vertices[i1] ); + B2_ASSERT( b2Dot( edge, edge ) > FLT_EPSILON * FLT_EPSILON ); + shape.normals[i] = b2Normalize( b2CrossVS( edge, 1.0f ) ); + } + + shape.centroid = b2ComputePolygonCentroid( shape.vertices, shape.count ); + + return shape; +} + +b2Polygon b2MakeSquare( float h ) +{ + return b2MakeBox( h, h ); +} + +b2Polygon b2MakeBox( float hx, float hy ) +{ + B2_ASSERT( b2IsValid( hx ) && hx > 0.0f ); + B2_ASSERT( b2IsValid( hy ) && hy > 0.0f ); + + b2Polygon shape = { 0 }; + shape.count = 4; + shape.vertices[0] = ( b2Vec2 ){ -hx, -hy }; + shape.vertices[1] = ( b2Vec2 ){ hx, -hy }; + shape.vertices[2] = ( b2Vec2 ){ hx, hy }; + shape.vertices[3] = ( b2Vec2 ){ -hx, hy }; + shape.normals[0] = ( b2Vec2 ){ 0.0f, -1.0f }; + shape.normals[1] = ( b2Vec2 ){ 1.0f, 0.0f }; + shape.normals[2] = ( b2Vec2 ){ 0.0f, 1.0f }; + shape.normals[3] = ( b2Vec2 ){ -1.0f, 0.0f }; + shape.radius = 0.0f; + shape.centroid = b2Vec2_zero; + return shape; +} + +b2Polygon b2MakeRoundedBox( float hx, float hy, float radius ) +{ + b2Polygon shape = b2MakeBox( hx, hy ); + shape.radius = radius; + return shape; +} + +b2Polygon b2MakeOffsetBox( float hx, float hy, b2Vec2 center, b2Rot rotation ) +{ + b2Transform xf; + xf.p = center; + xf.q = rotation; + + b2Polygon shape = { 0 }; + shape.count = 4; + shape.vertices[0] = b2TransformPoint( xf, ( b2Vec2 ){ -hx, -hy } ); + shape.vertices[1] = b2TransformPoint( xf, ( b2Vec2 ){ hx, -hy } ); + shape.vertices[2] = b2TransformPoint( xf, ( b2Vec2 ){ hx, hy } ); + shape.vertices[3] = b2TransformPoint( xf, ( b2Vec2 ){ -hx, hy } ); + shape.normals[0] = b2RotateVector( xf.q, ( b2Vec2 ){ 0.0f, -1.0f } ); + shape.normals[1] = b2RotateVector( xf.q, ( b2Vec2 ){ 1.0f, 0.0f } ); + shape.normals[2] = b2RotateVector( xf.q, ( b2Vec2 ){ 0.0f, 1.0f } ); + shape.normals[3] = b2RotateVector( xf.q, ( b2Vec2 ){ -1.0f, 0.0f } ); + shape.radius = 0.0f; + shape.centroid = center; + return shape; +} + +b2Polygon b2TransformPolygon( b2Transform transform, const b2Polygon* polygon ) +{ + b2Polygon p = *polygon; + + for ( int i = 0; i < p.count; ++i ) + { + p.vertices[i] = b2TransformPoint( transform, p.vertices[i] ); + p.normals[i] = b2RotateVector( transform.q, p.normals[i] ); + } + + p.centroid = b2TransformPoint( transform, p.centroid ); + + return p; +} + +b2MassData b2ComputeCircleMass( const b2Circle* shape, float density ) +{ + float rr = shape->radius * shape->radius; + + b2MassData massData; + massData.mass = density * b2_pi * rr; + massData.center = shape->center; + + // inertia about the local origin + massData.rotationalInertia = massData.mass * ( 0.5f * rr + b2Dot( shape->center, shape->center ) ); + + return massData; +} + +b2MassData b2ComputeCapsuleMass( const b2Capsule* shape, float density ) +{ + float radius = shape->radius; + float rr = radius * radius; + b2Vec2 p1 = shape->center1; + b2Vec2 p2 = shape->center2; + float length = b2Length( b2Sub( p2, p1 ) ); + float ll = length * length; + + float circleMass = density * ( b2_pi * radius * radius ); + float boxMass = density * ( 2.0f * radius * length ); + + b2MassData massData; + massData.mass = circleMass + boxMass; + massData.center.x = 0.5f * ( p1.x + p2.x ); + massData.center.y = 0.5f * ( p1.y + p2.y ); + + // two offset half circles, both halves add up to full circle and each half is offset by half length + // semi-circle centroid = 4 r / 3 pi + // Need to apply parallel-axis theorem twice: + // 1. shift semi-circle centroid to origin + // 2. shift semi-circle to box end + // m * ((h + lc)^2 - lc^2) = m * (h^2 + 2 * h * lc) + // See: https://en.wikipedia.org/wiki/Parallel_axis_theorem + // I verified this formula by computing the convex hull of a 128 vertex capsule + + // half circle centroid + float lc = 4.0f * radius / ( 3.0f * b2_pi ); + + // half length of rectangular portion of capsule + float h = 0.5f * length; + + float circleInertia = circleMass * ( 0.5f * rr + h * h + 2.0f * h * lc ); + float boxInertia = boxMass * ( 4.0f * rr + ll ) / 12.0f; + massData.rotationalInertia = circleInertia + boxInertia; + + // inertia about the local origin + massData.rotationalInertia += massData.mass * b2Dot( massData.center, massData.center ); + + return massData; +} + +b2MassData b2ComputePolygonMass( const b2Polygon* shape, float density ) +{ + // Polygon mass, centroid, and inertia. + // Let rho be the polygon density in mass per unit area. + // Then: + // mass = rho * int(dA) + // centroid.x = (1/mass) * rho * int(x * dA) + // centroid.y = (1/mass) * rho * int(y * dA) + // I = rho * int((x*x + y*y) * dA) + // + // We can compute these integrals by summing all the integrals + // for each triangle of the polygon. To evaluate the integral + // for a single triangle, we make a change of variables to + // the (u,v) coordinates of the triangle: + // x = x0 + e1x * u + e2x * v + // y = y0 + e1y * u + e2y * v + // where 0 <= u && 0 <= v && u + v <= 1. + // + // We integrate u from [0,1-v] and then v from [0,1]. + // We also need to use the Jacobian of the transformation: + // D = cross(e1, e2) + // + // Simplification: triangle centroid = (1/3) * (p1 + p2 + p3) + // + // The rest of the derivation is handled by computer algebra. + + B2_ASSERT( shape->count > 0 ); + + if ( shape->count == 1 ) + { + b2Circle circle; + circle.center = shape->vertices[0]; + circle.radius = shape->radius; + return b2ComputeCircleMass( &circle, density ); + } + + if ( shape->count == 2 ) + { + b2Capsule capsule; + capsule.center1 = shape->vertices[0]; + capsule.center2 = shape->vertices[1]; + capsule.radius = shape->radius; + return b2ComputeCapsuleMass( &capsule, density ); + } + + b2Vec2 vertices[b2_maxPolygonVertices] = { 0 }; + int32_t count = shape->count; + float radius = shape->radius; + + if ( radius > 0.0f ) + { + // Approximate mass of rounded polygons by pushing out the vertices. + float sqrt2 = 1.412f; + for ( int32_t i = 0; i < count; ++i ) + { + int32_t j = i == 0 ? count - 1 : i - 1; + b2Vec2 n1 = shape->normals[j]; + b2Vec2 n2 = shape->normals[i]; + + b2Vec2 mid = b2Normalize( b2Add( n1, n2 ) ); + vertices[i] = b2MulAdd( shape->vertices[i], sqrt2 * radius, mid ); + } + } + else + { + for ( int32_t i = 0; i < count; ++i ) + { + vertices[i] = shape->vertices[i]; + } + } + + b2Vec2 center = { 0.0f, 0.0f }; + float area = 0.0f; + float rotationalInertia = 0.0f; + + // Get a reference point for forming triangles. + // Use the first vertex to reduce round-off errors. + b2Vec2 r = vertices[0]; + + const float inv3 = 1.0f / 3.0f; + + for ( int32_t i = 1; i < count - 1; ++i ) + { + // Triangle edges + b2Vec2 e1 = b2Sub( vertices[i], r ); + b2Vec2 e2 = b2Sub( vertices[i + 1], r ); + + float D = b2Cross( e1, e2 ); + + float triangleArea = 0.5f * D; + area += triangleArea; + + // Area weighted centroid, r at origin + center = b2MulAdd( center, triangleArea * inv3, b2Add( e1, e2 ) ); + + float ex1 = e1.x, ey1 = e1.y; + float ex2 = e2.x, ey2 = e2.y; + + float intx2 = ex1 * ex1 + ex2 * ex1 + ex2 * ex2; + float inty2 = ey1 * ey1 + ey2 * ey1 + ey2 * ey2; + + rotationalInertia += ( 0.25f * inv3 * D ) * ( intx2 + inty2 ); + } + + b2MassData massData; + + // Total mass + massData.mass = density * area; + + // Center of mass, shift back from origin at r + B2_ASSERT( area > FLT_EPSILON ); + float invArea = 1.0f / area; + center.x *= invArea; + center.y *= invArea; + massData.center = b2Add( r, center ); + + // Inertia tensor relative to the local origin (point s). + massData.rotationalInertia = density * rotationalInertia; + + // Shift to center of mass then to original body origin. + massData.rotationalInertia += massData.mass * ( b2Dot( massData.center, massData.center ) - b2Dot( center, center ) ); + + return massData; +} + +b2AABB b2ComputeCircleAABB( const b2Circle* shape, b2Transform xf ) +{ + b2Vec2 p = b2TransformPoint( xf, shape->center ); + float r = shape->radius; + + b2AABB aabb = { { p.x - r, p.y - r }, { p.x + r, p.y + r } }; + return aabb; +} + +b2AABB b2ComputeCapsuleAABB( const b2Capsule* shape, b2Transform xf ) +{ + b2Vec2 v1 = b2TransformPoint( xf, shape->center1 ); + b2Vec2 v2 = b2TransformPoint( xf, shape->center2 ); + + b2Vec2 r = { shape->radius, shape->radius }; + b2Vec2 lower = b2Sub( b2Min( v1, v2 ), r ); + b2Vec2 upper = b2Add( b2Max( v1, v2 ), r ); + + b2AABB aabb = { lower, upper }; + return aabb; +} + +b2AABB b2ComputePolygonAABB( const b2Polygon* shape, b2Transform xf ) +{ + B2_ASSERT( shape->count > 0 ); + b2Vec2 lower = b2TransformPoint( xf, shape->vertices[0] ); + b2Vec2 upper = lower; + + for ( int32_t i = 1; i < shape->count; ++i ) + { + b2Vec2 v = b2TransformPoint( xf, shape->vertices[i] ); + lower = b2Min( lower, v ); + upper = b2Max( upper, v ); + } + + b2Vec2 r = { shape->radius, shape->radius }; + lower = b2Sub( lower, r ); + upper = b2Add( upper, r ); + + b2AABB aabb = { lower, upper }; + return aabb; +} + +b2AABB b2ComputeSegmentAABB( const b2Segment* shape, b2Transform xf ) +{ + b2Vec2 v1 = b2TransformPoint( xf, shape->point1 ); + b2Vec2 v2 = b2TransformPoint( xf, shape->point2 ); + + b2Vec2 lower = b2Min( v1, v2 ); + b2Vec2 upper = b2Max( v1, v2 ); + + b2AABB aabb = { lower, upper }; + return aabb; +} + +bool b2PointInCircle( b2Vec2 point, const b2Circle* shape ) +{ + b2Vec2 center = shape->center; + return b2DistanceSquared( point, center ) <= shape->radius * shape->radius; +} + +bool b2PointInCapsule( b2Vec2 point, const b2Capsule* shape ) +{ + float rr = shape->radius * shape->radius; + b2Vec2 p1 = shape->center1; + b2Vec2 p2 = shape->center2; + + b2Vec2 d = b2Sub( p2, p1 ); + float dd = b2Dot( d, d ); + if ( dd == 0.0f ) + { + // Capsule is really a circle + return b2DistanceSquared( point, p1 ) <= rr; + } + + // Get closest point on capsule segment + // c = p1 + t * d + // dot(point - c, d) = 0 + // dot(point - p1 - t * d, d) = 0 + // t = dot(point - p1, d) / dot(d, d) + float t = b2Dot( b2Sub( point, p1 ), d ) / dd; + t = b2ClampFloat( t, 0.0f, 1.0f ); + b2Vec2 c = b2MulAdd( p1, t, d ); + + // Is query point within radius around closest point? + return b2DistanceSquared( point, c ) <= rr; +} + +bool b2PointInPolygon( b2Vec2 point, const b2Polygon* shape ) +{ + b2DistanceInput input = { 0 }; + input.proxyA = b2MakeProxy( shape->vertices, shape->count, 0.0f ); + input.proxyB = b2MakeProxy( &point, 1, 0.0f ); + input.transformA = b2Transform_identity; + input.transformB = b2Transform_identity; + input.useRadii = false; + + b2DistanceCache cache = { 0 }; + b2DistanceOutput output = b2ShapeDistance( &cache, &input, NULL, 0 ); + + return output.distance <= shape->radius; +} + +// Precision Improvements for Ray / Sphere Intersection - Ray Tracing Gems 2019 +// http://www.codercorner.com/blog/?p=321 +b2CastOutput b2RayCastCircle( const b2RayCastInput* input, const b2Circle* shape ) +{ + B2_ASSERT( b2IsValidRay( input ) ); + + b2Vec2 p = shape->center; + + b2CastOutput output = { 0 }; + + // Shift ray so circle center is the origin + b2Vec2 s = b2Sub( input->origin, p ); + float length; + b2Vec2 d = b2GetLengthAndNormalize( &length, input->translation ); + if ( length == 0.0f ) + { + // zero length ray + return output; + } + + // Find closest point on ray to origin + + // solve: dot(s + t * d, d) = 0 + float t = -b2Dot( s, d ); + + // c is the closest point on the line to the origin + b2Vec2 c = b2MulAdd( s, t, d ); + + float cc = b2Dot( c, c ); + float r = shape->radius; + float rr = r * r; + + if ( cc > rr ) + { + // closest point is outside the circle + return output; + } + + // Pythagorus + float h = sqrtf( rr - cc ); + + float fraction = t - h; + + if ( fraction < 0.0f || input->maxFraction * length < fraction ) + { + // outside the range of the ray segment + return output; + } + + b2Vec2 hitPoint = b2MulAdd( s, fraction, d ); + + output.fraction = fraction / length; + output.normal = b2Normalize( hitPoint ); + output.point = b2MulAdd( p, shape->radius, output.normal ); + output.hit = true; + + return output; +} + +b2CastOutput b2RayCastCapsule( const b2RayCastInput* input, const b2Capsule* shape ) +{ + B2_ASSERT( b2IsValidRay( input ) ); + + b2CastOutput output = { 0 }; + + b2Vec2 v1 = shape->center1; + b2Vec2 v2 = shape->center2; + + b2Vec2 e = b2Sub( v2, v1 ); + + float capsuleLength; + b2Vec2 a = b2GetLengthAndNormalize( &capsuleLength, e ); + + if ( capsuleLength < FLT_EPSILON ) + { + // Capsule is really a circle + b2Circle circle = { v1, shape->radius }; + return b2RayCastCircle( input, &circle ); + } + + b2Vec2 p1 = input->origin; + b2Vec2 d = input->translation; + + // Ray from capsule start to ray start + b2Vec2 q = b2Sub( p1, v1 ); + float qa = b2Dot( q, a ); + + // Vector to ray start that is perpendicular to capsule axis + b2Vec2 qp = b2MulAdd( q, -qa, a ); + + float radius = shape->radius; + + // Does the ray start within the infinite length capsule? + if ( b2Dot( qp, qp ) < radius * radius ) + { + if ( qa < 0.0f ) + { + // start point behind capsule segment + b2Circle circle = { v1, shape->radius }; + return b2RayCastCircle( input, &circle ); + } + + if ( qa > 1.0f ) + { + // start point ahead of capsule segment + b2Circle circle = { v2, shape->radius }; + return b2RayCastCircle( input, &circle ); + } + + // ray starts inside capsule -> no hit + return output; + } + + // Perpendicular to capsule axis, pointing right + b2Vec2 n = { a.y, -a.x }; + + float rayLength; + b2Vec2 u = b2GetLengthAndNormalize( &rayLength, d ); + + // Intersect ray with infinite length capsule + // v1 + radius * n + s1 * a = p1 + s2 * u + // v1 - radius * n + s1 * a = p1 + s2 * u + + // s1 * a - s2 * u = b + // b = q - radius * ap + // or + // b = q + radius * ap + + // Cramer's rule [a -u] + float den = -a.x * u.y + u.x * a.y; + if ( -FLT_EPSILON < den && den < FLT_EPSILON ) + { + // Ray is parallel to capsule and outside infinite length capsule + return output; + } + + b2Vec2 b1 = b2MulSub( q, radius, n ); + b2Vec2 b2 = b2MulAdd( q, radius, n ); + + float invDen = 1.0f / den; + + // Cramer's rule [a b1] + float s21 = ( a.x * b1.y - b1.x * a.y ) * invDen; + + // Cramer's rule [a b2] + float s22 = ( a.x * b2.y - b2.x * a.y ) * invDen; + + float s2; + b2Vec2 b; + if ( s21 < s22 ) + { + s2 = s21; + b = b1; + } + else + { + s2 = s22; + b = b2; + n = b2Neg( n ); + } + + if ( s2 < 0.0f || input->maxFraction * rayLength < s2 ) + { + return output; + } + + // Cramer's rule [b -u] + float s1 = ( -b.x * u.y + u.x * b.y ) * invDen; + + if ( s1 < 0.0f ) + { + // ray passes behind capsule segment + b2Circle circle = { v1, shape->radius }; + return b2RayCastCircle( input, &circle ); + } + else if ( capsuleLength < s1 ) + { + // ray passes ahead of capsule segment + b2Circle circle = { v2, shape->radius }; + return b2RayCastCircle( input, &circle ); + } + else + { + // ray hits capsule side + output.fraction = s2 / rayLength; + output.point = b2Add( b2Lerp( v1, v2, s1 / capsuleLength ), b2MulSV( shape->radius, n ) ); + output.normal = n; + output.hit = true; + return output; + } +} + +// Ray vs line segment +b2CastOutput b2RayCastSegment( const b2RayCastInput* input, const b2Segment* shape, bool oneSided ) +{ + if ( oneSided ) + { + // Skip left-side collision + float offset = b2Cross( b2Sub( input->origin, shape->point1 ), b2Sub( shape->point2, shape->point1 ) ); + if ( offset < 0.0f ) + { + b2CastOutput output = { 0 }; + return output; + } + } + + // Put the ray into the edge's frame of reference. + b2Vec2 p1 = input->origin; + b2Vec2 d = input->translation; + + b2Vec2 v1 = shape->point1; + b2Vec2 v2 = shape->point2; + b2Vec2 e = b2Sub( v2, v1 ); + + b2CastOutput output = { 0 }; + + float length; + b2Vec2 eUnit = b2GetLengthAndNormalize( &length, e ); + if ( length == 0.0f ) + { + return output; + } + + // Normal points to the right, looking from v1 towards v2 + b2Vec2 normal = b2RightPerp( eUnit ); + + // Intersect ray with infinite segment using normal + // Similar to intersecting a ray with an infinite plane + // p = p1 + t * d + // dot(normal, p - v1) = 0 + // dot(normal, p1 - v1) + t * dot(normal, d) = 0 + float numerator = b2Dot( normal, b2Sub( v1, p1 ) ); + float denominator = b2Dot( normal, d ); + + if ( denominator == 0.0f ) + { + // parallel + return output; + } + + float t = numerator / denominator; + if ( t < 0.0f || input->maxFraction < t ) + { + // out of ray range + return output; + } + + // Intersection point on infinite segment + b2Vec2 p = b2MulAdd( p1, t, d ); + + // Compute position of p along segment + // p = v1 + s * e + // s = dot(p - v1, e) / dot(e, e) + + float s = b2Dot( b2Sub( p, v1 ), eUnit ); + if ( s < 0.0f || length < s ) + { + // out of segment range + return output; + } + + if ( numerator > 0.0f ) + { + normal = b2Neg( normal ); + } + + output.fraction = t; + output.point = b2MulAdd( p1, t, d ); + output.normal = normal; + output.hit = true; + + return output; +} + +b2CastOutput b2RayCastPolygon( const b2RayCastInput* input, const b2Polygon* shape ) +{ + B2_ASSERT( b2IsValidRay( input ) ); + + if ( shape->radius == 0.0f ) + { + // Put the ray into the polygon's frame of reference. + b2Vec2 p1 = input->origin; + b2Vec2 d = input->translation; + + float lower = 0.0f, upper = input->maxFraction; + + int32_t index = -1; + + b2CastOutput output = { 0 }; + + for ( int32_t i = 0; i < shape->count; ++i ) + { + // p = p1 + a * d + // dot(normal, p - v) = 0 + // dot(normal, p1 - v) + a * dot(normal, d) = 0 + float numerator = b2Dot( shape->normals[i], b2Sub( shape->vertices[i], p1 ) ); + float denominator = b2Dot( shape->normals[i], d ); + + if ( denominator == 0.0f ) + { + if ( numerator < 0.0f ) + { + return output; + } + } + else + { + // Note: we want this predicate without division: + // lower < numerator / denominator, where denominator < 0 + // Since denominator < 0, we have to flip the inequality: + // lower < numerator / denominator <==> denominator * lower > numerator. + if ( denominator < 0.0f && numerator < lower * denominator ) + { + // Increase lower. + // The segment enters this half-space. + lower = numerator / denominator; + index = i; + } + else if ( denominator > 0.0f && numerator < upper * denominator ) + { + // Decrease upper. + // The segment exits this half-space. + upper = numerator / denominator; + } + } + + // The use of epsilon here causes the B2_ASSERT on lower to trip + // in some cases. Apparently the use of epsilon was to make edge + // shapes work, but now those are handled separately. + // if (upper < lower - b2_epsilon) + if ( upper < lower ) + { + return output; + } + } + + B2_ASSERT( 0.0f <= lower && lower <= input->maxFraction ); + + if ( index >= 0 ) + { + output.fraction = lower; + output.normal = shape->normals[index]; + output.point = b2MulAdd( p1, lower, d ); + output.hit = true; + } + + return output; + } + + // TODO_ERIN this is not working for ray vs box (zero radii) + b2ShapeCastPairInput castInput; + castInput.proxyA = b2MakeProxy( shape->vertices, shape->count, shape->radius ); + castInput.proxyB = b2MakeProxy( &input->origin, 1, 0.0f ); + castInput.transformA = b2Transform_identity; + castInput.transformB = b2Transform_identity; + castInput.translationB = input->translation; + castInput.maxFraction = input->maxFraction; + return b2ShapeCast( &castInput ); +} + +b2CastOutput b2ShapeCastCircle( const b2ShapeCastInput* input, const b2Circle* shape ) +{ + b2ShapeCastPairInput pairInput; + pairInput.proxyA = b2MakeProxy( &shape->center, 1, shape->radius ); + pairInput.proxyB = b2MakeProxy( input->points, input->count, input->radius ); + pairInput.transformA = b2Transform_identity; + pairInput.transformB = b2Transform_identity; + pairInput.translationB = input->translation; + pairInput.maxFraction = input->maxFraction; + + b2CastOutput output = b2ShapeCast( &pairInput ); + return output; +} + +b2CastOutput b2ShapeCastCapsule( const b2ShapeCastInput* input, const b2Capsule* shape ) +{ + b2ShapeCastPairInput pairInput; + pairInput.proxyA = b2MakeProxy( &shape->center1, 2, shape->radius ); + pairInput.proxyB = b2MakeProxy( input->points, input->count, input->radius ); + pairInput.transformA = b2Transform_identity; + pairInput.transformB = b2Transform_identity; + pairInput.translationB = input->translation; + pairInput.maxFraction = input->maxFraction; + + b2CastOutput output = b2ShapeCast( &pairInput ); + return output; +} + +b2CastOutput b2ShapeCastSegment( const b2ShapeCastInput* input, const b2Segment* shape ) +{ + b2ShapeCastPairInput pairInput; + pairInput.proxyA = b2MakeProxy( &shape->point1, 2, 0.0f ); + pairInput.proxyB = b2MakeProxy( input->points, input->count, input->radius ); + pairInput.transformA = b2Transform_identity; + pairInput.transformB = b2Transform_identity; + pairInput.translationB = input->translation; + pairInput.maxFraction = input->maxFraction; + + b2CastOutput output = b2ShapeCast( &pairInput ); + return output; +} + +b2CastOutput b2ShapeCastPolygon( const b2ShapeCastInput* input, const b2Polygon* shape ) +{ + b2ShapeCastPairInput pairInput; + pairInput.proxyA = b2MakeProxy( shape->vertices, shape->count, shape->radius ); + pairInput.proxyB = b2MakeProxy( input->points, input->count, input->radius ); + pairInput.transformA = b2Transform_identity; + pairInput.transformB = b2Transform_identity; + pairInput.translationB = input->translation; + pairInput.maxFraction = input->maxFraction; + + b2CastOutput output = b2ShapeCast( &pairInput ); + return output; +} diff --git a/3rdparty/box2d/src/hull.c b/3rdparty/box2d/src/hull.c new file mode 100644 index 000000000000..41fd76de1581 --- /dev/null +++ b/3rdparty/box2d/src/hull.c @@ -0,0 +1,327 @@ +// SPDX-FileCopyrightText: 2023 Erin Catto +// SPDX-License-Identifier: MIT + +#include "core.h" + +#include "box2d/collision.h" +#include "box2d/math_functions.h" + +#include + +// quickhull recursion +static b2Hull b2RecurseHull( b2Vec2 p1, b2Vec2 p2, b2Vec2* ps, int count ) +{ + b2Hull hull; + hull.count = 0; + + if ( count == 0 ) + { + return hull; + } + + // create an edge vector pointing from p1 to p2 + b2Vec2 e = b2Normalize( b2Sub( p2, p1 ) ); + + // discard points left of e and find point furthest to the right of e + b2Vec2 rightPoints[b2_maxPolygonVertices]; + int rightCount = 0; + + int bestIndex = 0; + float bestDistance = b2Cross( b2Sub( ps[bestIndex], p1 ), e ); + if ( bestDistance > 0.0f ) + { + rightPoints[rightCount++] = ps[bestIndex]; + } + + for ( int i = 1; i < count; ++i ) + { + float distance = b2Cross( b2Sub( ps[i], p1 ), e ); + if ( distance > bestDistance ) + { + bestIndex = i; + bestDistance = distance; + } + + if ( distance > 0.0f ) + { + rightPoints[rightCount++] = ps[i]; + } + } + + if ( bestDistance < 2.0f * b2_linearSlop ) + { + return hull; + } + + b2Vec2 bestPoint = ps[bestIndex]; + + // compute hull to the right of p1-bestPoint + b2Hull hull1 = b2RecurseHull( p1, bestPoint, rightPoints, rightCount ); + + // compute hull to the right of bestPoint-p2 + b2Hull hull2 = b2RecurseHull( bestPoint, p2, rightPoints, rightCount ); + + // stitch together hulls + for ( int i = 0; i < hull1.count; ++i ) + { + hull.points[hull.count++] = hull1.points[i]; + } + + hull.points[hull.count++] = bestPoint; + + for ( int i = 0; i < hull2.count; ++i ) + { + hull.points[hull.count++] = hull2.points[i]; + } + + B2_ASSERT( hull.count < b2_maxPolygonVertices ); + + return hull; +} + +// quickhull algorithm +// - merges vertices based on b2_linearSlop +// - removes collinear points using b2_linearSlop +// - returns an empty hull if it fails +b2Hull b2ComputeHull( const b2Vec2* points, int count ) +{ + b2Hull hull; + hull.count = 0; + + if ( count < 3 || count > b2_maxPolygonVertices ) + { + // check your data + return hull; + } + + count = b2MinFloat( count, b2_maxPolygonVertices ); + + b2AABB aabb = { { FLT_MAX, FLT_MAX }, { -FLT_MAX, -FLT_MAX } }; + + // Perform aggressive point welding. First point always remains. + // Also compute the bounding box for later. + b2Vec2 ps[b2_maxPolygonVertices]; + int n = 0; + const float linearSlop = b2_linearSlop; + const float tolSqr = 16.0f * linearSlop * linearSlop; + for ( int i = 0; i < count; ++i ) + { + aabb.lowerBound = b2Min( aabb.lowerBound, points[i] ); + aabb.upperBound = b2Max( aabb.upperBound, points[i] ); + + b2Vec2 vi = points[i]; + + bool unique = true; + for ( int j = 0; j < i; ++j ) + { + b2Vec2 vj = points[j]; + + float distSqr = b2DistanceSquared( vi, vj ); + if ( distSqr < tolSqr ) + { + unique = false; + break; + } + } + + if ( unique ) + { + ps[n++] = vi; + } + } + + if ( n < 3 ) + { + // all points very close together, check your data and check your scale + return hull; + } + + // Find an extreme point as the first point on the hull + b2Vec2 c = b2AABB_Center( aabb ); + int f1 = 0; + float dsq1 = b2DistanceSquared( c, ps[f1] ); + for ( int i = 1; i < n; ++i ) + { + float dsq = b2DistanceSquared( c, ps[i] ); + if ( dsq > dsq1 ) + { + f1 = i; + dsq1 = dsq; + } + } + + // remove p1 from working set + b2Vec2 p1 = ps[f1]; + ps[f1] = ps[n - 1]; + n = n - 1; + + int f2 = 0; + float dsq2 = b2DistanceSquared( p1, ps[f2] ); + for ( int i = 1; i < n; ++i ) + { + float dsq = b2DistanceSquared( p1, ps[i] ); + if ( dsq > dsq2 ) + { + f2 = i; + dsq2 = dsq; + } + } + + // remove p2 from working set + b2Vec2 p2 = ps[f2]; + ps[f2] = ps[n - 1]; + n = n - 1; + + // split the points into points that are left and right of the line p1-p2. + b2Vec2 rightPoints[b2_maxPolygonVertices - 2]; + int rightCount = 0; + + b2Vec2 leftPoints[b2_maxPolygonVertices - 2]; + int leftCount = 0; + + b2Vec2 e = b2Normalize( b2Sub( p2, p1 ) ); + + for ( int i = 0; i < n; ++i ) + { + float d = b2Cross( b2Sub( ps[i], p1 ), e ); + + // slop used here to skip points that are very close to the line p1-p2 + if ( d >= 2.0f * linearSlop ) + { + rightPoints[rightCount++] = ps[i]; + } + else if ( d <= -2.0f * linearSlop ) + { + leftPoints[leftCount++] = ps[i]; + } + } + + // compute hulls on right and left + b2Hull hull1 = b2RecurseHull( p1, p2, rightPoints, rightCount ); + b2Hull hull2 = b2RecurseHull( p2, p1, leftPoints, leftCount ); + + if ( hull1.count == 0 && hull2.count == 0 ) + { + // all points collinear + return hull; + } + + // stitch hulls together, preserving CCW winding order + hull.points[hull.count++] = p1; + + for ( int i = 0; i < hull1.count; ++i ) + { + hull.points[hull.count++] = hull1.points[i]; + } + + hull.points[hull.count++] = p2; + + for ( int i = 0; i < hull2.count; ++i ) + { + hull.points[hull.count++] = hull2.points[i]; + } + + B2_ASSERT( hull.count <= b2_maxPolygonVertices ); + + // merge collinear + bool searching = true; + while ( searching && hull.count > 2 ) + { + searching = false; + + for ( int i = 0; i < hull.count; ++i ) + { + int i1 = i; + int i2 = ( i + 1 ) % hull.count; + int i3 = ( i + 2 ) % hull.count; + + b2Vec2 s1 = hull.points[i1]; + b2Vec2 s2 = hull.points[i2]; + b2Vec2 s3 = hull.points[i3]; + + // unit edge vector for s1-s3 + b2Vec2 r = b2Normalize( b2Sub( s3, s1 ) ); + + float distance = b2Cross( b2Sub( s2, s1 ), r ); + if ( distance <= 2.0f * linearSlop ) + { + // remove midpoint from hull + for ( int j = i2; j < hull.count - 1; ++j ) + { + hull.points[j] = hull.points[j + 1]; + } + hull.count -= 1; + + // continue searching for collinear points + searching = true; + + break; + } + } + } + + if ( hull.count < 3 ) + { + // all points collinear, shouldn't be reached since this was validated above + hull.count = 0; + } + + return hull; +} + +bool b2ValidateHull( const b2Hull* hull ) +{ + if ( hull->count < 3 || b2_maxPolygonVertices < hull->count ) + { + return false; + } + + // test that every point is behind every edge + for ( int i = 0; i < hull->count; ++i ) + { + // create an edge vector + int i1 = i; + int i2 = i < hull->count - 1 ? i1 + 1 : 0; + b2Vec2 p = hull->points[i1]; + b2Vec2 e = b2Normalize( b2Sub( hull->points[i2], p ) ); + + for ( int j = 0; j < hull->count; ++j ) + { + // skip points that subtend the current edge + if ( j == i1 || j == i2 ) + { + continue; + } + + float distance = b2Cross( b2Sub( hull->points[j], p ), e ); + if ( distance >= 0.0f ) + { + return false; + } + } + } + + // test for collinear points + const float linearSlop = b2_linearSlop; + for ( int i = 0; i < hull->count; ++i ) + { + int i1 = i; + int i2 = ( i + 1 ) % hull->count; + int i3 = ( i + 2 ) % hull->count; + + b2Vec2 p1 = hull->points[i1]; + b2Vec2 p2 = hull->points[i2]; + b2Vec2 p3 = hull->points[i3]; + + b2Vec2 e = b2Normalize( b2Sub( p3, p1 ) ); + + float distance = b2Cross( b2Sub( p2, p1 ), e ); + if ( distance <= linearSlop ) + { + // p1-p2-p3 are collinear + return false; + } + } + + return true; +} diff --git a/3rdparty/box2d/src/id_pool.c b/3rdparty/box2d/src/id_pool.c new file mode 100644 index 000000000000..2cbb06ec4916 --- /dev/null +++ b/3rdparty/box2d/src/id_pool.c @@ -0,0 +1,73 @@ +// SPDX-FileCopyrightText: 2023 Erin Catto +// SPDX-License-Identifier: MIT + +#include "id_pool.h" + +#include "array.h" + +b2IdPool b2CreateIdPool() +{ + b2IdPool pool = { 0 }; + pool.freeArray = b2IntArray_Create( 32 ); + return pool; +} + +void b2DestroyIdPool( b2IdPool* pool ) +{ + b2IntArray_Destroy( &pool->freeArray ); + *pool = ( b2IdPool ){ 0 }; +} + +int b2AllocId( b2IdPool* pool ) +{ + int count = pool->freeArray.count; + if (count > 0 ) + { + int id = b2IntArray_Pop(&pool->freeArray); + return id; + } + + int id = pool->nextIndex; + pool->nextIndex += 1; + return id; +} + +void b2FreeId( b2IdPool* pool, int id ) +{ + B2_ASSERT( pool->nextIndex > 0 ); + B2_ASSERT( 0 <= id && id < pool->nextIndex ); + + if ( id == pool->nextIndex ) + { + pool->nextIndex -= 1; + return; + } + + b2IntArray_Push( &pool->freeArray, id ); +} + +#if B2_VALIDATE + +void b2ValidateFreeId( b2IdPool* pool, int id ) +{ + int freeCount = pool->freeArray.count; + for ( int i = 0; i < freeCount; ++i ) + { + if ( pool->freeArray.data[i] == id ) + { + return; + } + } + + B2_ASSERT( 0 ); +} + +#else + +void b2ValidateFreeId( b2IdPool* pool, int id ) +{ + B2_MAYBE_UNUSED( pool ); + B2_MAYBE_UNUSED( id ); +} + +#endif diff --git a/3rdparty/box2d/src/id_pool.h b/3rdparty/box2d/src/id_pool.h new file mode 100644 index 000000000000..6a72080db22f --- /dev/null +++ b/3rdparty/box2d/src/id_pool.h @@ -0,0 +1,34 @@ +// SPDX-FileCopyrightText: 2023 Erin Catto +// SPDX-License-Identifier: MIT + +#pragma once + +#include "array.h" + +typedef struct b2IdPool +{ + b2IntArray freeArray; + int nextIndex; +} b2IdPool; + +b2IdPool b2CreateIdPool(); +void b2DestroyIdPool( b2IdPool* pool ); + +int b2AllocId( b2IdPool* pool ); +void b2FreeId( b2IdPool* pool, int id ); +void b2ValidateFreeId( b2IdPool* pool, int id ); + +static inline int b2GetIdCount( b2IdPool* pool ) +{ + return pool->nextIndex - pool->freeArray.count; +} + +static inline int b2GetIdCapacity( b2IdPool* pool ) +{ + return pool->nextIndex; +} + +static inline int b2GetIdBytes( b2IdPool* pool ) +{ + return b2IntArray_ByteCount(&pool->freeArray); +} diff --git a/3rdparty/box2d/src/island.c b/3rdparty/box2d/src/island.c new file mode 100644 index 000000000000..4f1b322ffe4d --- /dev/null +++ b/3rdparty/box2d/src/island.c @@ -0,0 +1,979 @@ +// SPDX-FileCopyrightText: 2023 Erin Catto +// SPDX-License-Identifier: MIT + +#include "island.h" + +#include "body.h" +#include "contact.h" +#include "core.h" +#include "joint.h" +#include "solver_set.h" +#include "world.h" + +#include + +B2_ARRAY_SOURCE( b2Island, b2Island ); +B2_ARRAY_SOURCE( b2IslandSim, b2IslandSim ); + +b2Island* b2CreateIsland( b2World* world, int setIndex ) +{ + B2_ASSERT( setIndex == b2_awakeSet || setIndex >= b2_firstSleepingSet ); + + int islandId = b2AllocId( &world->islandIdPool ); + + if ( islandId == world->islands.count ) + { + b2Island emptyIsland = { 0 }; + b2IslandArray_Push( &world->islands, emptyIsland ); + } + else + { + B2_ASSERT( world->islands.data[islandId].setIndex == B2_NULL_INDEX ); + } + + b2SolverSet* set = b2SolverSetArray_Get( &world->solverSets, setIndex ); + + b2Island* island = b2IslandArray_Get( &world->islands, islandId ); + island->setIndex = setIndex; + island->localIndex = set->islandSims.count; + island->islandId = islandId; + island->headBody = B2_NULL_INDEX; + island->tailBody = B2_NULL_INDEX; + island->bodyCount = 0; + island->headContact = B2_NULL_INDEX; + island->tailContact = B2_NULL_INDEX; + island->contactCount = 0; + island->headJoint = B2_NULL_INDEX; + island->tailJoint = B2_NULL_INDEX; + island->jointCount = 0; + island->parentIsland = B2_NULL_INDEX; + island->constraintRemoveCount = 0; + + b2IslandSim* islandSim = b2IslandSimArray_Add( &set->islandSims ); + islandSim->islandId = islandId; + + return island; +} + +void b2DestroyIsland( b2World* world, int islandId ) +{ + // assume island is empty + b2Island* island = b2IslandArray_Get( &world->islands, islandId ); + b2SolverSet* set = b2SolverSetArray_Get( &world->solverSets, island->setIndex ); + int movedIndex = b2IslandSimArray_RemoveSwap( &set->islandSims, island->localIndex ); + if ( movedIndex != B2_NULL_INDEX ) + { + // Fix index on moved element + b2IslandSim* movedElement = set->islandSims.data + island->localIndex; + int movedId = movedElement->islandId; + b2Island* movedIsland = b2IslandArray_Get( &world->islands, movedId ); + B2_ASSERT( movedIsland->localIndex == movedIndex ); + movedIsland->localIndex = island->localIndex; + } + + // Free island and id (preserve island revision) + island->islandId = B2_NULL_INDEX; + island->setIndex = B2_NULL_INDEX; + island->localIndex = B2_NULL_INDEX; + b2FreeId( &world->islandIdPool, islandId ); +} + +static void b2AddContactToIsland( b2World* world, int islandId, b2Contact* contact ) +{ + B2_ASSERT( contact->islandId == B2_NULL_INDEX ); + B2_ASSERT( contact->islandPrev == B2_NULL_INDEX ); + B2_ASSERT( contact->islandNext == B2_NULL_INDEX ); + + b2Island* island = b2IslandArray_Get( &world->islands, islandId ); + + if ( island->headContact != B2_NULL_INDEX ) + { + contact->islandNext = island->headContact; + b2Contact* headContact = b2ContactArray_Get( &world->contacts, island->headContact); + headContact->islandPrev = contact->contactId; + } + + island->headContact = contact->contactId; + if ( island->tailContact == B2_NULL_INDEX ) + { + island->tailContact = island->headContact; + } + + island->contactCount += 1; + contact->islandId = islandId; + + b2ValidateIsland( world, islandId ); +} + +// Link a contact into an island. +// This performs union-find and path compression to join islands. +// https://en.wikipedia.org/wiki/Disjoint-set_data_structure +void b2LinkContact( b2World* world, b2Contact* contact ) +{ + B2_ASSERT( ( contact->flags & b2_contactTouchingFlag ) != 0 && ( contact->flags & b2_contactSensorFlag ) == 0 ); + + int bodyIdA = contact->edges[0].bodyId; + int bodyIdB = contact->edges[1].bodyId; + + b2Body* bodyA = b2BodyArray_Get( &world->bodies, bodyIdA ); + b2Body* bodyB = b2BodyArray_Get( &world->bodies, bodyIdB ); + + B2_ASSERT( bodyA->setIndex != b2_disabledSet && bodyB->setIndex != b2_disabledSet ); + B2_ASSERT( bodyA->setIndex != b2_staticSet || bodyB->setIndex != b2_staticSet ); + + // Wake bodyB if bodyA is awake and bodyB is sleeping + if ( bodyA->setIndex == b2_awakeSet && bodyB->setIndex >= b2_firstSleepingSet ) + { + b2WakeSolverSet( world, bodyB->setIndex ); + } + + // Wake bodyA if bodyB is awake and bodyA is sleeping + if ( bodyB->setIndex == b2_awakeSet && bodyA->setIndex >= b2_firstSleepingSet ) + { + b2WakeSolverSet( world, bodyA->setIndex ); + } + + int islandIdA = bodyA->islandId; + int islandIdB = bodyB->islandId; + + // Static bodies have null island indices. + B2_ASSERT( bodyA->setIndex != b2_staticSet || islandIdA == B2_NULL_INDEX ); + B2_ASSERT( bodyB->setIndex != b2_staticSet || islandIdB == B2_NULL_INDEX ); + B2_ASSERT( islandIdA != B2_NULL_INDEX || islandIdB != B2_NULL_INDEX ); + + if ( islandIdA == islandIdB ) + { + // Contact in same island + b2AddContactToIsland( world, islandIdA, contact ); + return; + } + + // Union-find root of islandA + b2Island* islandA = NULL; + if ( islandIdA != B2_NULL_INDEX ) + { + islandA = b2IslandArray_Get( &world->islands, islandIdA ); + int parentId = islandA->parentIsland; + while ( parentId != B2_NULL_INDEX ) + { + b2Island* parent = b2IslandArray_Get( &world->islands, parentId ); + if ( parent->parentIsland != B2_NULL_INDEX ) + { + // path compression + islandA->parentIsland = parent->parentIsland; + } + + islandA = parent; + islandIdA = parentId; + parentId = islandA->parentIsland; + } + } + + // Union-find root of islandB + b2Island* islandB = NULL; + if ( islandIdB != B2_NULL_INDEX ) + { + islandB = b2IslandArray_Get( &world->islands, islandIdB ); + int parentId = islandB->parentIsland; + while ( islandB->parentIsland != B2_NULL_INDEX ) + { + b2Island* parent = b2IslandArray_Get( &world->islands, parentId ); + if ( parent->parentIsland != B2_NULL_INDEX ) + { + // path compression + islandB->parentIsland = parent->parentIsland; + } + + islandB = parent; + islandIdB = parentId; + parentId = islandB->parentIsland; + } + } + + B2_ASSERT( islandA != NULL || islandB != NULL ); + + // Union-Find link island roots + if ( islandA != islandB && islandA != NULL && islandB != NULL ) + { + B2_ASSERT( islandA != islandB ); + B2_ASSERT( islandB->parentIsland == B2_NULL_INDEX ); + islandB->parentIsland = islandIdA; + } + + if ( islandA != NULL ) + { + b2AddContactToIsland( world, islandIdA, contact ); + } + else + { + b2AddContactToIsland( world, islandIdB, contact ); + } +} + +// This is called when a contact no longer has contact points or when a contact is destroyed. +void b2UnlinkContact( b2World* world, b2Contact* contact ) +{ + B2_ASSERT( ( contact->flags & b2_contactSensorFlag ) == 0 ); + B2_ASSERT( contact->islandId != B2_NULL_INDEX ); + + // remove from island + int islandId = contact->islandId; + b2Island* island = b2IslandArray_Get( &world->islands, islandId ); + + if ( contact->islandPrev != B2_NULL_INDEX ) + { + b2Contact* prevContact = b2ContactArray_Get( &world->contacts, contact->islandPrev); + B2_ASSERT( prevContact->islandNext == contact->contactId ); + prevContact->islandNext = contact->islandNext; + } + + if ( contact->islandNext != B2_NULL_INDEX ) + { + b2Contact* nextContact = b2ContactArray_Get( &world->contacts, contact->islandNext ); + B2_ASSERT( nextContact->islandPrev == contact->contactId ); + nextContact->islandPrev = contact->islandPrev; + } + + if ( island->headContact == contact->contactId ) + { + island->headContact = contact->islandNext; + } + + if ( island->tailContact == contact->contactId ) + { + island->tailContact = contact->islandPrev; + } + + B2_ASSERT( island->contactCount > 0 ); + island->contactCount -= 1; + island->constraintRemoveCount += 1; + + contact->islandId = B2_NULL_INDEX; + contact->islandPrev = B2_NULL_INDEX; + contact->islandNext = B2_NULL_INDEX; + + b2ValidateIsland( world, islandId ); +} + +static void b2AddJointToIsland( b2World* world, int islandId, b2Joint* joint ) +{ + B2_ASSERT( joint->islandId == B2_NULL_INDEX ); + B2_ASSERT( joint->islandPrev == B2_NULL_INDEX ); + B2_ASSERT( joint->islandNext == B2_NULL_INDEX ); + + b2Island* island = b2IslandArray_Get( &world->islands, islandId ); + + if ( island->headJoint != B2_NULL_INDEX ) + { + joint->islandNext = island->headJoint; + b2Joint* headJoint = b2JointArray_Get( &world->joints, island->headJoint ); + headJoint->islandPrev = joint->jointId; + } + + island->headJoint = joint->jointId; + if ( island->tailJoint == B2_NULL_INDEX ) + { + island->tailJoint = island->headJoint; + } + + island->jointCount += 1; + joint->islandId = islandId; + + b2ValidateIsland( world, islandId ); +} + +void b2LinkJoint( b2World* world, b2Joint* joint, bool mergeIslands ) +{ + b2Body* bodyA = b2BodyArray_Get( &world->bodies, joint->edges[0].bodyId ); + b2Body* bodyB = b2BodyArray_Get( &world->bodies, joint->edges[1].bodyId ); + + if ( bodyA->setIndex == b2_awakeSet && bodyB->setIndex >= b2_firstSleepingSet ) + { + b2WakeSolverSet( world, bodyB->setIndex ); + } + else if ( bodyB->setIndex == b2_awakeSet && bodyA->setIndex >= b2_firstSleepingSet ) + { + b2WakeSolverSet( world, bodyA->setIndex ); + } + + int islandIdA = bodyA->islandId; + int islandIdB = bodyB->islandId; + + B2_ASSERT( islandIdA != B2_NULL_INDEX || islandIdB != B2_NULL_INDEX ); + + if ( islandIdA == islandIdB ) + { + // Joint in same island + b2AddJointToIsland( world, islandIdA, joint ); + return; + } + + // Union-find root of islandA + b2Island* islandA = NULL; + if ( islandIdA != B2_NULL_INDEX ) + { + islandA = b2IslandArray_Get( &world->islands, islandIdA ); + while ( islandA->parentIsland != B2_NULL_INDEX ) + { + b2Island* parent = b2IslandArray_Get( &world->islands, islandA->parentIsland ); + if ( parent->parentIsland != B2_NULL_INDEX ) + { + // path compression + islandA->parentIsland = parent->parentIsland; + } + + islandIdA = islandA->parentIsland; + islandA = parent; + } + } + + // Union-find root of islandB + b2Island* islandB = NULL; + if ( islandIdB != B2_NULL_INDEX ) + { + islandB = b2IslandArray_Get( &world->islands, islandIdB ); + while ( islandB->parentIsland != B2_NULL_INDEX ) + { + b2Island* parent = b2IslandArray_Get( &world->islands, islandB->parentIsland ); + if ( parent->parentIsland != B2_NULL_INDEX ) + { + // path compression + islandB->parentIsland = parent->parentIsland; + } + + islandIdB = islandB->parentIsland; + islandB = parent; + } + } + + B2_ASSERT( islandA != NULL || islandB != NULL ); + + // Union-Find link island roots + if ( islandA != islandB && islandA != NULL && islandB != NULL ) + { + B2_ASSERT( islandA != islandB ); + B2_ASSERT( islandB->parentIsland == B2_NULL_INDEX ); + islandB->parentIsland = islandIdA; + } + + if ( islandA != NULL ) + { + b2AddJointToIsland( world, islandIdA, joint ); + } + else + { + b2AddJointToIsland( world, islandIdB, joint ); + } + + // Joints need to have islands merged immediately when they are created + // to keep the island graph valid. + // However, when a body type is being changed the merge can be deferred until + // all joints are linked. + if (mergeIslands) + { + b2MergeAwakeIslands( world ); + } +} + +void b2UnlinkJoint( b2World* world, b2Joint* joint ) +{ + B2_ASSERT( joint->islandId != B2_NULL_INDEX ); + + // remove from island + int islandId = joint->islandId; + b2Island* island = b2IslandArray_Get( &world->islands, islandId ); + + if ( joint->islandPrev != B2_NULL_INDEX ) + { + b2Joint* prevJoint = b2JointArray_Get( &world->joints, joint->islandPrev ); + B2_ASSERT( prevJoint->islandNext == joint->jointId ); + prevJoint->islandNext = joint->islandNext; + } + + if ( joint->islandNext != B2_NULL_INDEX ) + { + b2Joint* nextJoint = b2JointArray_Get( &world->joints, joint->islandNext ); + B2_ASSERT( nextJoint->islandPrev == joint->jointId ); + nextJoint->islandPrev = joint->islandPrev; + } + + if ( island->headJoint == joint->jointId ) + { + island->headJoint = joint->islandNext; + } + + if ( island->tailJoint == joint->jointId ) + { + island->tailJoint = joint->islandPrev; + } + + B2_ASSERT( island->jointCount > 0 ); + island->jointCount -= 1; + island->constraintRemoveCount += 1; + + joint->islandId = B2_NULL_INDEX; + joint->islandPrev = B2_NULL_INDEX; + joint->islandNext = B2_NULL_INDEX; + + b2ValidateIsland( world, islandId ); +} + +// Merge an island into its root island. +// todo we can assume all islands are awake here +static void b2MergeIsland( b2World* world, b2Island* island ) +{ + B2_ASSERT( island->parentIsland != B2_NULL_INDEX ); + + int rootId = island->parentIsland; + b2Island* rootIsland = b2IslandArray_Get( &world->islands, rootId ); + B2_ASSERT( rootIsland->parentIsland == B2_NULL_INDEX ); + + // remap island indices + int bodyId = island->headBody; + while ( bodyId != B2_NULL_INDEX ) + { + b2Body* body = b2BodyArray_Get( &world->bodies, bodyId ); + body->islandId = rootId; + bodyId = body->islandNext; + } + + int contactId = island->headContact; + while ( contactId != B2_NULL_INDEX ) + { + b2Contact* contact = b2ContactArray_Get( &world->contacts, contactId ); + contact->islandId = rootId; + contactId = contact->islandNext; + } + + int jointId = island->headJoint; + while ( jointId != B2_NULL_INDEX ) + { + b2Joint* joint = b2JointArray_Get( &world->joints, jointId ); + joint->islandId = rootId; + jointId = joint->islandNext; + } + + // connect body lists + B2_ASSERT( rootIsland->tailBody != B2_NULL_INDEX ); + b2Body* tailBody = b2BodyArray_Get( &world->bodies, rootIsland->tailBody ); + B2_ASSERT( tailBody->islandNext == B2_NULL_INDEX ); + tailBody->islandNext = island->headBody; + + B2_ASSERT( island->headBody != B2_NULL_INDEX ); + b2Body* headBody = b2BodyArray_Get( &world->bodies, island->headBody ); + B2_ASSERT( headBody->islandPrev == B2_NULL_INDEX ); + headBody->islandPrev = rootIsland->tailBody; + + rootIsland->tailBody = island->tailBody; + rootIsland->bodyCount += island->bodyCount; + + // connect contact lists + if ( rootIsland->headContact == B2_NULL_INDEX ) + { + // Root island has no contacts + B2_ASSERT( rootIsland->tailContact == B2_NULL_INDEX && rootIsland->contactCount == 0 ); + rootIsland->headContact = island->headContact; + rootIsland->tailContact = island->tailContact; + rootIsland->contactCount = island->contactCount; + } + else if ( island->headContact != B2_NULL_INDEX ) + { + // Both islands have contacts + B2_ASSERT( island->tailContact != B2_NULL_INDEX && island->contactCount > 0 ); + B2_ASSERT( rootIsland->tailContact != B2_NULL_INDEX && rootIsland->contactCount > 0 ); + + b2Contact* tailContact = b2ContactArray_Get( &world->contacts, rootIsland->tailContact ); + B2_ASSERT( tailContact->islandNext == B2_NULL_INDEX ); + tailContact->islandNext = island->headContact; + + b2Contact* headContact = b2ContactArray_Get( &world->contacts, island->headContact ); + B2_ASSERT( headContact->islandPrev == B2_NULL_INDEX ); + headContact->islandPrev = rootIsland->tailContact; + + rootIsland->tailContact = island->tailContact; + rootIsland->contactCount += island->contactCount; + } + + if ( rootIsland->headJoint == B2_NULL_INDEX ) + { + // Root island has no joints + B2_ASSERT( rootIsland->tailJoint == B2_NULL_INDEX && rootIsland->jointCount == 0 ); + rootIsland->headJoint = island->headJoint; + rootIsland->tailJoint = island->tailJoint; + rootIsland->jointCount = island->jointCount; + } + else if ( island->headJoint != B2_NULL_INDEX ) + { + // Both islands have joints + B2_ASSERT( island->tailJoint != B2_NULL_INDEX && island->jointCount > 0 ); + B2_ASSERT( rootIsland->tailJoint != B2_NULL_INDEX && rootIsland->jointCount > 0 ); + + b2Joint* tailJoint = b2JointArray_Get( &world->joints, rootIsland->tailJoint ); + B2_ASSERT( tailJoint->islandNext == B2_NULL_INDEX ); + tailJoint->islandNext = island->headJoint; + + b2Joint* headJoint = b2JointArray_Get( &world->joints, island->headJoint ); + B2_ASSERT( headJoint->islandPrev == B2_NULL_INDEX ); + headJoint->islandPrev = rootIsland->tailJoint; + + rootIsland->tailJoint = island->tailJoint; + rootIsland->jointCount += island->jointCount; + } + + // Track removed constraints + rootIsland->constraintRemoveCount += island->constraintRemoveCount; + + b2ValidateIsland( world, rootId ); +} + +// Iterate over all awake islands and merge any that need merging +// Islands that get merged into a root island will be removed from the awake island array +// and returned to the pool. +// todo this might be faster if b2IslandSim held the connectivity data +void b2MergeAwakeIslands( b2World* world ) +{ + b2TracyCZoneNC( merge_islands, "Merge Islands", b2_colorMediumTurquoise, true ); + + b2SolverSet* awakeSet = b2SolverSetArray_Get( &world->solverSets, b2_awakeSet ); + b2IslandSim* islandSims = awakeSet->islandSims.data; + int awakeIslandCount = awakeSet->islandSims.count; + + // Step 1: Ensure every child island points to its root island. This avoids merging a child island with + // a parent island that has already been merged with a grand-parent island. + for ( int i = 0; i < awakeIslandCount; ++i ) + { + int islandId = islandSims[i].islandId; + + b2Island* island = b2IslandArray_Get( &world->islands, islandId ); + + // find the root island + int rootId = islandId; + b2Island* rootIsland = island; + while ( rootIsland->parentIsland != B2_NULL_INDEX ) + { + b2Island* parent = b2IslandArray_Get( &world->islands, rootIsland->parentIsland ); + if ( parent->parentIsland != B2_NULL_INDEX ) + { + // path compression + rootIsland->parentIsland = parent->parentIsland; + } + + rootId = rootIsland->parentIsland; + rootIsland = parent; + } + + if ( rootIsland != island ) + { + island->parentIsland = rootId; + } + } + + // Step 2: merge every awake island into its parent (which must be a root island) + // Reverse to support removal from awake array. + for ( int i = awakeIslandCount - 1; i >= 0; --i ) + { + int islandId = islandSims[i].islandId; + b2Island* island = b2IslandArray_Get( &world->islands, islandId ); + + if ( island->parentIsland == B2_NULL_INDEX ) + { + continue; + } + + b2MergeIsland( world, island ); + + // this call does a remove swap from the end of the island sim array + b2DestroyIsland( world, islandId ); + } + + b2ValidateConnectivity( world ); + + b2TracyCZoneEnd( merge_islands ); +} + +#define B2_CONTACT_REMOVE_THRESHOLD 1 + +void b2SplitIsland( b2World* world, int baseId ) +{ + b2Island* baseIsland = b2IslandArray_Get( &world->islands, baseId ); + int setIndex = baseIsland->setIndex; + + if ( setIndex != b2_awakeSet ) + { + // can only split awake island + return; + } + + if ( baseIsland->constraintRemoveCount == 0 ) + { + // this island doesn't need to be split + return; + } + + b2ValidateIsland( world, baseId ); + + int bodyCount = baseIsland->bodyCount; + + b2Body* bodies = world->bodies.data; + b2StackAllocator* alloc = &world->stackAllocator; + + // No lock is needed because I ensure the allocator is not used while this task is active. + int* stack = b2AllocateStackItem( alloc, bodyCount * sizeof( int ), "island stack" ); + int* bodyIds = b2AllocateStackItem( alloc, bodyCount * sizeof( int ), "body ids" ); + + // Build array containing all body indices from base island. These + // serve as seed bodies for the depth first search (DFS). + int index = 0; + int nextBody = baseIsland->headBody; + while ( nextBody != B2_NULL_INDEX ) + { + bodyIds[index++] = nextBody; + b2Body* body = bodies + nextBody; + + // Clear visitation mark + body->isMarked = false; + + nextBody = body->islandNext; + } + B2_ASSERT( index == bodyCount ); + + // Clear contact island flags. Only need to consider contacts + // already in the base island. + int nextContactId = baseIsland->headContact; + while ( nextContactId != B2_NULL_INDEX ) + { + b2Contact* contact = b2ContactArray_Get( &world->contacts, nextContactId ); + contact->isMarked = false; + nextContactId = contact->islandNext; + } + + // Clear joint island flags. + int nextJoint = baseIsland->headJoint; + while ( nextJoint != B2_NULL_INDEX ) + { + b2Joint* joint = b2JointArray_Get( &world->joints, nextJoint ); + joint->isMarked = false; + nextJoint = joint->islandNext; + } + + // Done with the base split island. + b2DestroyIsland( world, baseId ); + + // Each island is found as a depth first search starting from a seed body + for ( int i = 0; i < bodyCount; ++i ) + { + int seedIndex = bodyIds[i]; + b2Body* seed = bodies + seedIndex; + B2_ASSERT( seed->setIndex == setIndex ); + + if ( seed->isMarked == true ) + { + // The body has already been visited + continue; + } + + int stackCount = 0; + stack[stackCount++] = seedIndex; + seed->isMarked = true; + + // Create new island + // No lock needed because only a single island can split per time step. No islands are being used during the constraint + // solve. However, islands are touched during body finalization. + b2Island* island = b2CreateIsland( world, setIndex ); + + int islandId = island->islandId; + + // Perform a depth first search (DFS) on the constraint graph. + while ( stackCount > 0 ) + { + // Grab the next body off the stack and add it to the island. + int bodyId = stack[--stackCount]; + b2Body* body = bodies + bodyId; + B2_ASSERT( body->setIndex == b2_awakeSet ); + B2_ASSERT( body->isMarked == true ); + + // Add body to island + body->islandId = islandId; + if ( island->tailBody != B2_NULL_INDEX ) + { + bodies[island->tailBody].islandNext = bodyId; + } + body->islandPrev = island->tailBody; + body->islandNext = B2_NULL_INDEX; + island->tailBody = bodyId; + + if ( island->headBody == B2_NULL_INDEX ) + { + island->headBody = bodyId; + } + + island->bodyCount += 1; + + // Search all contacts connected to this body. + int contactKey = body->headContactKey; + while ( contactKey != B2_NULL_INDEX ) + { + int contactId = contactKey >> 1; + int edgeIndex = contactKey & 1; + + b2Contact* contact = b2ContactArray_Get( &world->contacts, contactId ); + B2_ASSERT( contact->contactId == contactId ); + + // Next key + contactKey = contact->edges[edgeIndex].nextKey; + + // Has this contact already been added to this island? + if ( contact->isMarked ) + { + continue; + } + + // Skip sensors + if ( contact->flags & b2_contactSensorFlag ) + { + continue; + } + + // Is this contact enabled and touching? + if ( ( contact->flags & b2_contactTouchingFlag ) == 0 ) + { + continue; + } + + contact->isMarked = true; + + int otherEdgeIndex = edgeIndex ^ 1; + int otherBodyId = contact->edges[otherEdgeIndex].bodyId; + b2Body* otherBody = bodies + otherBodyId; + + // Maybe add other body to stack + if ( otherBody->isMarked == false && otherBody->setIndex != b2_staticSet ) + { + B2_ASSERT( stackCount < bodyCount ); + stack[stackCount++] = otherBodyId; + otherBody->isMarked = true; + } + + // Add contact to island + contact->islandId = islandId; + if ( island->tailContact != B2_NULL_INDEX ) + { + b2Contact* tailContact = b2ContactArray_Get( &world->contacts, island->tailContact ); + tailContact->islandNext = contactId; + } + contact->islandPrev = island->tailContact; + contact->islandNext = B2_NULL_INDEX; + island->tailContact = contactId; + + if ( island->headContact == B2_NULL_INDEX ) + { + island->headContact = contactId; + } + + island->contactCount += 1; + } + + // Search all joints connect to this body. + int jointKey = body->headJointKey; + while ( jointKey != B2_NULL_INDEX ) + { + int jointId = jointKey >> 1; + int edgeIndex = jointKey & 1; + + b2Joint* joint = b2JointArray_Get( &world->joints, jointId ); + B2_ASSERT( joint->jointId == jointId ); + + // Next key + jointKey = joint->edges[edgeIndex].nextKey; + + // Has this joint already been added to this island? + if ( joint->isMarked ) + { + continue; + } + + joint->isMarked = true; + + int otherEdgeIndex = edgeIndex ^ 1; + int otherBodyId = joint->edges[otherEdgeIndex].bodyId; + b2Body* otherBody = bodies + otherBodyId; + + // Don't simulate joints connected to disabled bodies. + if ( otherBody->setIndex == b2_disabledSet ) + { + continue; + } + + // Maybe add other body to stack + if ( otherBody->isMarked == false && otherBody->setIndex == b2_awakeSet ) + { + B2_ASSERT( stackCount < bodyCount ); + stack[stackCount++] = otherBodyId; + otherBody->isMarked = true; + } + + // Add joint to island + joint->islandId = islandId; + if ( island->tailJoint != B2_NULL_INDEX ) + { + b2Joint* tailJoint = b2JointArray_Get( &world->joints, island->tailJoint ); + tailJoint->islandNext = jointId; + } + joint->islandPrev = island->tailJoint; + joint->islandNext = B2_NULL_INDEX; + island->tailJoint = jointId; + + if ( island->headJoint == B2_NULL_INDEX ) + { + island->headJoint = jointId; + } + + island->jointCount += 1; + } + } + + b2ValidateIsland( world, islandId ); + } + + b2FreeStackItem( alloc, bodyIds ); + b2FreeStackItem( alloc, stack ); +} + +// Split an island because some contacts and/or joints have been removed. +// This is called during the constraint solve while islands are not being touched. This uses DFS and touches a lot of memory, +// so it can be quite slow. +// Note: contacts/joints connected to static bodies must belong to an island but don't affect island connectivity +// Note: static bodies are never in an island +// Note: this task interacts with some allocators without locks under the assumption that no other tasks +// are interacting with these data structures. +void b2SplitIslandTask( int startIndex, int endIndex, uint32_t threadIndex, void* context ) +{ + b2TracyCZoneNC( split, "Split Island", b2_colorHoneydew, true ); + + B2_MAYBE_UNUSED( startIndex ); + B2_MAYBE_UNUSED( endIndex ); + B2_MAYBE_UNUSED( threadIndex ); + + b2Timer timer = b2CreateTimer(); + b2World* world = context; + + B2_ASSERT( world->splitIslandId != B2_NULL_INDEX ); + + b2SplitIsland( world, world->splitIslandId ); + + world->profile.splitIslands += b2GetMilliseconds( &timer ); + b2TracyCZoneEnd( split ); +} + +#if B2_VALIDATE +void b2ValidateIsland( b2World* world, int islandId ) +{ + b2Island* island = b2IslandArray_Get( &world->islands, islandId ); + B2_ASSERT( island->islandId == islandId ); + B2_ASSERT( island->setIndex != B2_NULL_INDEX ); + B2_ASSERT( island->headBody != B2_NULL_INDEX ); + + { + B2_ASSERT( island->tailBody != B2_NULL_INDEX ); + B2_ASSERT( island->bodyCount > 0 ); + if ( island->bodyCount > 1 ) + { + B2_ASSERT( island->tailBody != island->headBody ); + } + B2_ASSERT( island->bodyCount <= b2GetIdCount( &world->bodyIdPool ) ); + + int count = 0; + int bodyId = island->headBody; + while ( bodyId != B2_NULL_INDEX ) + { + b2Body* body = b2BodyArray_Get(&world->bodies, bodyId); + B2_ASSERT( body->islandId == islandId ); + B2_ASSERT( body->setIndex == island->setIndex ); + count += 1; + + if ( count == island->bodyCount ) + { + B2_ASSERT( bodyId == island->tailBody ); + } + + bodyId = body->islandNext; + } + B2_ASSERT( count == island->bodyCount ); + } + + if ( island->headContact != B2_NULL_INDEX ) + { + B2_ASSERT( island->tailContact != B2_NULL_INDEX ); + B2_ASSERT( island->contactCount > 0 ); + if ( island->contactCount > 1 ) + { + B2_ASSERT( island->tailContact != island->headContact ); + } + B2_ASSERT( island->contactCount <= b2GetIdCount( &world->contactIdPool ) ); + + int count = 0; + int contactId = island->headContact; + while ( contactId != B2_NULL_INDEX ) + { + b2Contact* contact = b2ContactArray_Get( &world->contacts, contactId ); + B2_ASSERT( contact->setIndex == island->setIndex ); + B2_ASSERT( contact->islandId == islandId ); + count += 1; + + if ( count == island->contactCount ) + { + B2_ASSERT( contactId == island->tailContact ); + } + + contactId = contact->islandNext; + } + B2_ASSERT( count == island->contactCount ); + } + else + { + B2_ASSERT( island->tailContact == B2_NULL_INDEX ); + B2_ASSERT( island->contactCount == 0 ); + } + + if ( island->headJoint != B2_NULL_INDEX ) + { + B2_ASSERT( island->tailJoint != B2_NULL_INDEX ); + B2_ASSERT( island->jointCount > 0 ); + if ( island->jointCount > 1 ) + { + B2_ASSERT( island->tailJoint != island->headJoint ); + } + B2_ASSERT( island->jointCount <= b2GetIdCount( &world->jointIdPool ) ); + + int count = 0; + int jointId = island->headJoint; + while ( jointId != B2_NULL_INDEX ) + { + b2Joint* joint = b2JointArray_Get( &world->joints, jointId ); + B2_ASSERT( joint->setIndex == island->setIndex ); + count += 1; + + if ( count == island->jointCount ) + { + B2_ASSERT( jointId == island->tailJoint ); + } + + jointId = joint->islandNext; + } + B2_ASSERT( count == island->jointCount ); + } + else + { + B2_ASSERT( island->tailJoint == B2_NULL_INDEX ); + B2_ASSERT( island->jointCount == 0 ); + } +} + +#else + +void b2ValidateIsland( b2World* world, int islandId ) +{ + B2_MAYBE_UNUSED( world ); + B2_MAYBE_UNUSED( islandId ); +} +#endif diff --git a/3rdparty/box2d/src/island.h b/3rdparty/box2d/src/island.h new file mode 100644 index 000000000000..c4334641394c --- /dev/null +++ b/3rdparty/box2d/src/island.h @@ -0,0 +1,90 @@ +// SPDX-FileCopyrightText: 2023 Erin Catto +// SPDX-License-Identifier: MIT + +#pragma once + +#include "array.h" + +#include +#include + +typedef struct b2Body b2Body; +typedef struct b2Contact b2Contact; +typedef struct b2Joint b2Joint; +typedef struct b2StepContext b2StepContext; +typedef struct b2World b2World; + +// Deterministic solver +// +// Collide all awake contacts +// Use bit array to emit start/stop touching events in defined order, per thread. Try using contact index, assuming contacts are +// created in a deterministic order. bit-wise OR together bit arrays and issue changes: +// - start touching: merge islands - temporary linked list - mark root island dirty - wake all - largest island is root +// - stop touching: increment constraintRemoveCount + +// Persistent island for awake bodies, joints, and contacts +// https://en.wikipedia.org/wiki/Component_(graph_theory) +// https://en.wikipedia.org/wiki/Dynamic_connectivity +// map from int to solver set and index +typedef struct b2Island +{ + // index of solver set stored in b2World + // may be B2_NULL_INDEX + int setIndex; + + // island index within set + // may be B2_NULL_INDEX + int localIndex; + + int islandId; + + int headBody; + int tailBody; + int bodyCount; + + int headContact; + int tailContact; + int contactCount; + + int headJoint; + int tailJoint; + int jointCount; + + // Union find + int parentIsland; + + // Keeps track of how many contacts have been removed from this island. + // This is used to determine if an island is a candidate for splitting. + int constraintRemoveCount; +} b2Island; + +typedef struct b2IslandSim +{ + int islandId; + +} b2IslandSim; + +b2Island* b2CreateIsland( b2World* world, int setIndex ); +void b2DestroyIsland( b2World* world, int islandId ); + +// Link contacts into the island graph when it starts having contact points +void b2LinkContact( b2World* world, b2Contact* contact ); + +// Unlink contact from the island graph when it stops having contact points +void b2UnlinkContact( b2World* world, b2Contact* contact ); + +// Link a joint into the island graph when it is created +void b2LinkJoint( b2World* world, b2Joint* joint, bool mergeIslands ); + +// Unlink a joint from the island graph when it is destroyed +void b2UnlinkJoint( b2World* world, b2Joint* joint ); + +void b2MergeAwakeIslands( b2World* world ); + +void b2SplitIsland( b2World* world, int baseId ); +void b2SplitIslandTask( int startIndex, int endIndex, uint32_t threadIndex, void* context ); + +void b2ValidateIsland( b2World* world, int islandId ); + +B2_ARRAY_INLINE( b2Island, b2Island ); +B2_ARRAY_INLINE( b2IslandSim, b2IslandSim ); diff --git a/3rdparty/box2d/src/joint.c b/3rdparty/box2d/src/joint.c new file mode 100644 index 000000000000..d1af86f83b75 --- /dev/null +++ b/3rdparty/box2d/src/joint.c @@ -0,0 +1,1241 @@ +// SPDX-FileCopyrightText: 2023 Erin Catto +// SPDX-License-Identifier: MIT + +#include "joint.h" + +#include "body.h" +#include "contact.h" +#include "core.h" +#include "island.h" +#include "shape.h" +#include "solver.h" +#include "solver_set.h" +#include "world.h" + +// needed for dll export +#include "box2d/box2d.h" + +#include + +B2_ARRAY_SOURCE( b2Joint, b2Joint ); +B2_ARRAY_SOURCE( b2JointSim, b2JointSim ); + +b2DistanceJointDef b2DefaultDistanceJointDef( void ) +{ + b2DistanceJointDef def = { 0 }; + def.length = 1.0f; + def.maxLength = b2_huge; + def.internalValue = B2_SECRET_COOKIE; + return def; +} + +b2MotorJointDef b2DefaultMotorJointDef( void ) +{ + b2MotorJointDef def = { 0 }; + def.maxForce = 1.0f; + def.maxTorque = 1.0f; + def.correctionFactor = 0.3f; + def.internalValue = B2_SECRET_COOKIE; + return def; +} + +b2MouseJointDef b2DefaultMouseJointDef( void ) +{ + b2MouseJointDef def = { 0 }; + def.hertz = 4.0f; + def.dampingRatio = 1.0f; + def.maxForce = 1.0f; + def.internalValue = B2_SECRET_COOKIE; + return def; +} + +b2PrismaticJointDef b2DefaultPrismaticJointDef( void ) +{ + b2PrismaticJointDef def = { 0 }; + def.localAxisA = ( b2Vec2 ){ 1.0f, 0.0f }; + def.internalValue = B2_SECRET_COOKIE; + return def; +} + +b2RevoluteJointDef b2DefaultRevoluteJointDef( void ) +{ + b2RevoluteJointDef def = { 0 }; + def.drawSize = 0.25f; + def.internalValue = B2_SECRET_COOKIE; + return def; +} + +b2WeldJointDef b2DefaultWeldJointDef( void ) +{ + b2WeldJointDef def = { 0 }; + def.internalValue = B2_SECRET_COOKIE; + return def; +} + +b2WheelJointDef b2DefaultWheelJointDef( void ) +{ + b2WheelJointDef def = { 0 }; + def.localAxisA.y = 1.0f; + def.enableSpring = true; + def.hertz = 1.0f; + def.dampingRatio = 0.7f; + def.internalValue = B2_SECRET_COOKIE; + return def; +} + +static b2Joint* b2GetJointFullId( b2World* world, b2JointId jointId ) +{ + int id = jointId.index1 - 1; + b2Joint* joint = b2JointArray_Get( &world->joints, id ); + B2_ASSERT( joint->jointId == id && joint->revision == jointId.revision ); + return joint; +} + +b2JointSim* b2GetJointSim( b2World* world, b2Joint* joint ) +{ + if ( joint->setIndex == b2_awakeSet ) + { + B2_ASSERT( 0 <= joint->colorIndex && joint->colorIndex < b2_graphColorCount ); + b2GraphColor* color = world->constraintGraph.colors + joint->colorIndex; + return b2JointSimArray_Get( &color->jointSims, joint->localIndex ); + } + + b2SolverSet* set = b2SolverSetArray_Get( &world->solverSets, joint->setIndex ); + return b2JointSimArray_Get( &set->jointSims, joint->localIndex ); +} + +b2JointSim* b2GetJointSimCheckType( b2JointId jointId, b2JointType type ) +{ + B2_MAYBE_UNUSED( type ); + + b2World* world = b2GetWorld( jointId.world0 ); + B2_ASSERT( world->locked == false ); + if ( world->locked ) + { + return NULL; + } + + b2Joint* joint = b2GetJointFullId( world, jointId ); + B2_ASSERT( joint->type == type ); + b2JointSim* jointSim = b2GetJointSim( world, joint ); + B2_ASSERT( jointSim->type == type ); + return jointSim; +} + +typedef struct b2JointPair +{ + b2Joint* joint; + b2JointSim* jointSim; +} b2JointPair; + +static b2JointPair b2CreateJoint( b2World* world, b2Body* bodyA, b2Body* bodyB, void* userData, float drawSize, b2JointType type, + bool collideConnected ) +{ + int bodyIdA = bodyA->id; + int bodyIdB = bodyB->id; + int maxSetIndex = b2MaxInt( bodyA->setIndex, bodyB->setIndex ); + + // Create joint id and joint + int jointId = b2AllocId( &world->jointIdPool ); + if ( jointId == world->joints.count ) + { + b2JointArray_Push( &world->joints, ( b2Joint ){ 0 } ); + } + + b2Joint* joint = b2JointArray_Get( &world->joints, jointId ); + joint->jointId = jointId; + joint->userData = userData; + joint->revision += 1; + joint->setIndex = B2_NULL_INDEX; + joint->colorIndex = B2_NULL_INDEX; + joint->localIndex = B2_NULL_INDEX; + joint->islandId = B2_NULL_INDEX; + joint->islandPrev = B2_NULL_INDEX; + joint->islandNext = B2_NULL_INDEX; + joint->drawSize = drawSize; + joint->type = type; + joint->collideConnected = collideConnected; + joint->isMarked = false; + + // Doubly linked list on bodyA + joint->edges[0].bodyId = bodyIdA; + joint->edges[0].prevKey = B2_NULL_INDEX; + joint->edges[0].nextKey = bodyA->headJointKey; + + int keyA = ( jointId << 1 ) | 0; + if ( bodyA->headJointKey != B2_NULL_INDEX ) + { + b2Joint* jointA = b2JointArray_Get( &world->joints, bodyA->headJointKey >> 1 ); + b2JointEdge* edgeA = jointA->edges + ( bodyA->headJointKey & 1 ); + edgeA->prevKey = keyA; + } + bodyA->headJointKey = keyA; + bodyA->jointCount += 1; + + // Doubly linked list on bodyB + joint->edges[1].bodyId = bodyIdB; + joint->edges[1].prevKey = B2_NULL_INDEX; + joint->edges[1].nextKey = bodyB->headJointKey; + + int keyB = ( jointId << 1 ) | 1; + if ( bodyB->headJointKey != B2_NULL_INDEX ) + { + b2Joint* jointB = b2JointArray_Get( &world->joints, bodyB->headJointKey >> 1 ); + b2JointEdge* edgeB = jointB->edges + ( bodyB->headJointKey & 1 ); + edgeB->prevKey = keyB; + } + bodyB->headJointKey = keyB; + bodyB->jointCount += 1; + + b2JointSim* jointSim; + + if ( bodyA->setIndex == b2_disabledSet || bodyB->setIndex == b2_disabledSet ) + { + // if either body is disabled, create in disabled set + b2SolverSet* set = b2SolverSetArray_Get( &world->solverSets, b2_disabledSet ); + joint->setIndex = b2_disabledSet; + joint->localIndex = set->jointSims.count; + + jointSim = b2JointSimArray_Add( &set->jointSims ); + jointSim->jointId = jointId; + jointSim->bodyIdA = bodyIdA; + jointSim->bodyIdB = bodyIdB; + } + else if ( bodyA->setIndex == b2_staticSet && bodyB->setIndex == b2_staticSet ) + { + // joint is connecting static bodies + b2SolverSet* set = b2SolverSetArray_Get( &world->solverSets, b2_staticSet ); + joint->setIndex = b2_staticSet; + joint->localIndex = set->jointSims.count; + + jointSim = b2JointSimArray_Add( &set->jointSims ); + jointSim->jointId = jointId; + jointSim->bodyIdA = bodyIdA; + jointSim->bodyIdB = bodyIdB; + } + else if ( bodyA->setIndex == b2_awakeSet || bodyB->setIndex == b2_awakeSet ) + { + // if either body is sleeping, wake it + if ( maxSetIndex >= b2_firstSleepingSet ) + { + b2WakeSolverSet( world, maxSetIndex ); + } + + joint->setIndex = b2_awakeSet; + + jointSim = b2CreateJointInGraph( world, joint ); + jointSim->jointId = jointId; + jointSim->bodyIdA = bodyIdA; + jointSim->bodyIdB = bodyIdB; + } + else + { + // joint connected between sleeping and/or static bodies + B2_ASSERT( bodyA->setIndex >= b2_firstSleepingSet || bodyB->setIndex >= b2_firstSleepingSet ); + B2_ASSERT( bodyA->setIndex != b2_staticSet || bodyB->setIndex != b2_staticSet ); + + // joint should go into the sleeping set (not static set) + int setIndex = maxSetIndex; + + b2SolverSet* set = b2SolverSetArray_Get( &world->solverSets, setIndex ); + joint->setIndex = setIndex; + joint->localIndex = set->jointSims.count; + jointSim = b2JointSimArray_Add( &set->jointSims ); + jointSim->jointId = jointId; + jointSim->bodyIdA = bodyIdA; + jointSim->bodyIdB = bodyIdB; + + if ( bodyA->setIndex != bodyB->setIndex && bodyA->setIndex >= b2_firstSleepingSet && + bodyB->setIndex >= b2_firstSleepingSet ) + { + // merge sleeping sets + b2MergeSolverSets( world, bodyA->setIndex, bodyB->setIndex ); + B2_ASSERT( bodyA->setIndex == bodyB->setIndex ); + + // fix potentially invalid set index + setIndex = bodyA->setIndex; + + b2SolverSet* mergedSet = b2SolverSetArray_Get( &world->solverSets, setIndex ); + + // Careful! The joint sim pointer was orphaned by the set merge. + jointSim = b2JointSimArray_Get( &mergedSet->jointSims, joint->localIndex ); + } + + B2_ASSERT( joint->setIndex == setIndex ); + } + + B2_ASSERT( jointSim->jointId == jointId ); + B2_ASSERT( jointSim->bodyIdA == bodyIdA ); + B2_ASSERT( jointSim->bodyIdB == bodyIdB ); + + if ( joint->setIndex > b2_disabledSet ) + { + // Add edge to island graph + bool mergeIslands = true; + b2LinkJoint( world, joint, mergeIslands ); + } + + b2ValidateSolverSets( world ); + + return ( b2JointPair ){ joint, jointSim }; +} + +static void b2DestroyContactsBetweenBodies( b2World* world, b2Body* bodyA, b2Body* bodyB ) +{ + int contactKey; + int otherBodyId; + + // use the smaller of the two contact lists + if ( bodyA->contactCount < bodyB->contactCount ) + { + contactKey = bodyA->headContactKey; + otherBodyId = bodyB->id; + } + else + { + contactKey = bodyB->headContactKey; + otherBodyId = bodyA->id; + } + + // no need to wake bodies when a joint removes collision between them + bool wakeBodies = false; + + // destroy the contacts + while ( contactKey != B2_NULL_INDEX ) + { + int contactId = contactKey >> 1; + int edgeIndex = contactKey & 1; + + b2Contact* contact = b2ContactArray_Get( &world->contacts, contactId ); + contactKey = contact->edges[edgeIndex].nextKey; + + int otherEdgeIndex = edgeIndex ^ 1; + if ( contact->edges[otherEdgeIndex].bodyId == otherBodyId ) + { + // Careful, this removes the contact from the current doubly linked list + b2DestroyContact( world, contact, wakeBodies ); + } + } + + b2ValidateSolverSets( world ); +} + +b2JointId b2CreateDistanceJoint( b2WorldId worldId, const b2DistanceJointDef* def ) +{ + b2CheckDef( def ); + b2World* world = b2GetWorldFromId( worldId ); + + B2_ASSERT( world->locked == false ); + + if ( world->locked ) + { + return ( b2JointId ){ 0 }; + } + + B2_ASSERT( b2Body_IsValid( def->bodyIdA ) ); + B2_ASSERT( b2Body_IsValid( def->bodyIdB ) ); + B2_ASSERT( b2IsValid( def->length ) && def->length > 0.0f ); + + b2Body* bodyA = b2GetBodyFullId( world, def->bodyIdA ); + b2Body* bodyB = b2GetBodyFullId( world, def->bodyIdB ); + + b2JointPair pair = b2CreateJoint( world, bodyA, bodyB, def->userData, 1.0f, b2_distanceJoint, def->collideConnected ); + + b2JointSim* joint = pair.jointSim; + joint->type = b2_distanceJoint; + joint->localOriginAnchorA = def->localAnchorA; + joint->localOriginAnchorB = def->localAnchorB; + + b2DistanceJoint empty = { 0 }; + joint->distanceJoint = empty; + joint->distanceJoint.length = b2MaxFloat( def->length, b2_linearSlop ); + joint->distanceJoint.hertz = def->hertz; + joint->distanceJoint.dampingRatio = def->dampingRatio; + joint->distanceJoint.minLength = b2MaxFloat( def->minLength, b2_linearSlop ); + joint->distanceJoint.maxLength = b2MaxFloat( def->minLength, def->maxLength ); + joint->distanceJoint.maxMotorForce = def->maxMotorForce; + joint->distanceJoint.motorSpeed = def->motorSpeed; + joint->distanceJoint.enableSpring = def->enableSpring; + joint->distanceJoint.enableLimit = def->enableLimit; + joint->distanceJoint.enableMotor = def->enableMotor; + joint->distanceJoint.impulse = 0.0f; + joint->distanceJoint.lowerImpulse = 0.0f; + joint->distanceJoint.upperImpulse = 0.0f; + joint->distanceJoint.motorImpulse = 0.0f; + + // If the joint prevents collisions, then destroy all contacts between attached bodies + if ( def->collideConnected == false ) + { + b2DestroyContactsBetweenBodies( world, bodyA, bodyB ); + } + + b2JointId jointId = { joint->jointId + 1, world->worldId, pair.joint->revision }; + return jointId; +} + +b2JointId b2CreateMotorJoint( b2WorldId worldId, const b2MotorJointDef* def ) +{ + b2CheckDef( def ); + b2World* world = b2GetWorldFromId( worldId ); + + B2_ASSERT( world->locked == false ); + + if ( world->locked ) + { + return ( b2JointId ){ 0 }; + } + + b2Body* bodyA = b2GetBodyFullId( world, def->bodyIdA ); + b2Body* bodyB = b2GetBodyFullId( world, def->bodyIdB ); + + b2JointPair pair = b2CreateJoint( world, bodyA, bodyB, def->userData, 1.0f, b2_motorJoint, def->collideConnected ); + b2JointSim* joint = pair.jointSim; + + joint->type = b2_motorJoint; + joint->localOriginAnchorA = ( b2Vec2 ){ 0.0f, 0.0f }; + joint->localOriginAnchorB = ( b2Vec2 ){ 0.0f, 0.0f }; + joint->motorJoint = ( b2MotorJoint ){ 0 }; + joint->motorJoint.linearOffset = def->linearOffset; + joint->motorJoint.angularOffset = def->angularOffset; + joint->motorJoint.maxForce = def->maxForce; + joint->motorJoint.maxTorque = def->maxTorque; + joint->motorJoint.correctionFactor = b2ClampFloat( def->correctionFactor, 0.0f, 1.0f ); + + // If the joint prevents collisions, then destroy all contacts between attached bodies + if ( def->collideConnected == false ) + { + b2DestroyContactsBetweenBodies( world, bodyA, bodyB ); + } + + b2JointId jointId = { joint->jointId + 1, world->worldId, pair.joint->revision }; + return jointId; +} + +b2JointId b2CreateMouseJoint( b2WorldId worldId, const b2MouseJointDef* def ) +{ + b2CheckDef( def ); + b2World* world = b2GetWorldFromId( worldId ); + + B2_ASSERT( world->locked == false ); + + if ( world->locked ) + { + return ( b2JointId ){ 0 }; + } + + b2Body* bodyA = b2GetBodyFullId( world, def->bodyIdA ); + b2Body* bodyB = b2GetBodyFullId( world, def->bodyIdB ); + + b2Transform transformA = b2GetBodyTransformQuick( world, bodyA ); + b2Transform transformB = b2GetBodyTransformQuick( world, bodyB ); + + b2JointPair pair = b2CreateJoint( world, bodyA, bodyB, def->userData, 1.0f, b2_mouseJoint, def->collideConnected ); + + b2JointSim* joint = pair.jointSim; + joint->type = b2_mouseJoint; + joint->localOriginAnchorA = b2InvTransformPoint( transformA, def->target ); + joint->localOriginAnchorB = b2InvTransformPoint( transformB, def->target ); + + b2MouseJoint empty = { 0 }; + joint->mouseJoint = empty; + joint->mouseJoint.targetA = def->target; + joint->mouseJoint.hertz = def->hertz; + joint->mouseJoint.dampingRatio = def->dampingRatio; + joint->mouseJoint.maxForce = def->maxForce; + + b2JointId jointId = { joint->jointId + 1, world->worldId, pair.joint->revision }; + return jointId; +} + +b2JointId b2CreateRevoluteJoint( b2WorldId worldId, const b2RevoluteJointDef* def ) +{ + b2CheckDef( def ); + b2World* world = b2GetWorldFromId( worldId ); + + B2_ASSERT( world->locked == false ); + + if ( world->locked ) + { + return ( b2JointId ){ 0 }; + } + + b2Body* bodyA = b2GetBodyFullId( world, def->bodyIdA ); + b2Body* bodyB = b2GetBodyFullId( world, def->bodyIdB ); + + b2JointPair pair = + b2CreateJoint( world, bodyA, bodyB, def->userData, def->drawSize, b2_revoluteJoint, def->collideConnected ); + + b2JointSim* joint = pair.jointSim; + joint->type = b2_revoluteJoint; + joint->localOriginAnchorA = def->localAnchorA; + joint->localOriginAnchorB = def->localAnchorB; + + b2RevoluteJoint empty = { 0 }; + joint->revoluteJoint = empty; + + joint->revoluteJoint.referenceAngle = b2ClampFloat( def->referenceAngle, -b2_pi, b2_pi ); + joint->revoluteJoint.linearImpulse = b2Vec2_zero; + joint->revoluteJoint.axialMass = 0.0f; + joint->revoluteJoint.springImpulse = 0.0f; + joint->revoluteJoint.motorImpulse = 0.0f; + joint->revoluteJoint.lowerImpulse = 0.0f; + joint->revoluteJoint.upperImpulse = 0.0f; + joint->revoluteJoint.hertz = def->hertz; + joint->revoluteJoint.dampingRatio = def->dampingRatio; + joint->revoluteJoint.lowerAngle = b2MinFloat( def->lowerAngle, def->upperAngle ); + joint->revoluteJoint.upperAngle = b2MaxFloat( def->lowerAngle, def->upperAngle ); + joint->revoluteJoint.lowerAngle = b2ClampFloat( joint->revoluteJoint.lowerAngle, -b2_pi, b2_pi ); + joint->revoluteJoint.upperAngle = b2ClampFloat( joint->revoluteJoint.upperAngle, -b2_pi, b2_pi ); + joint->revoluteJoint.maxMotorTorque = def->maxMotorTorque; + joint->revoluteJoint.motorSpeed = def->motorSpeed; + joint->revoluteJoint.enableSpring = def->enableSpring; + joint->revoluteJoint.enableLimit = def->enableLimit; + joint->revoluteJoint.enableMotor = def->enableMotor; + + // If the joint prevents collisions, then destroy all contacts between attached bodies + if ( def->collideConnected == false ) + { + b2DestroyContactsBetweenBodies( world, bodyA, bodyB ); + } + + b2JointId jointId = { joint->jointId + 1, world->worldId, pair.joint->revision }; + return jointId; +} + +b2JointId b2CreatePrismaticJoint( b2WorldId worldId, const b2PrismaticJointDef* def ) +{ + b2CheckDef( def ); + b2World* world = b2GetWorldFromId( worldId ); + + B2_ASSERT( world->locked == false ); + + if ( world->locked ) + { + return ( b2JointId ){ 0 }; + } + + b2Body* bodyA = b2GetBodyFullId( world, def->bodyIdA ); + b2Body* bodyB = b2GetBodyFullId( world, def->bodyIdB ); + + b2JointPair pair = b2CreateJoint( world, bodyA, bodyB, def->userData, 1.0f, b2_prismaticJoint, def->collideConnected ); + + b2JointSim* joint = pair.jointSim; + joint->type = b2_prismaticJoint; + joint->localOriginAnchorA = def->localAnchorA; + joint->localOriginAnchorB = def->localAnchorB; + + b2PrismaticJoint empty = { 0 }; + joint->prismaticJoint = empty; + + joint->prismaticJoint.localAxisA = b2Normalize( def->localAxisA ); + joint->prismaticJoint.referenceAngle = def->referenceAngle; + joint->prismaticJoint.impulse = b2Vec2_zero; + joint->prismaticJoint.axialMass = 0.0f; + joint->prismaticJoint.springImpulse = 0.0f; + joint->prismaticJoint.motorImpulse = 0.0f; + joint->prismaticJoint.lowerImpulse = 0.0f; + joint->prismaticJoint.upperImpulse = 0.0f; + joint->prismaticJoint.hertz = def->hertz; + joint->prismaticJoint.dampingRatio = def->dampingRatio; + joint->prismaticJoint.lowerTranslation = def->lowerTranslation; + joint->prismaticJoint.upperTranslation = def->upperTranslation; + joint->prismaticJoint.maxMotorForce = def->maxMotorForce; + joint->prismaticJoint.motorSpeed = def->motorSpeed; + joint->prismaticJoint.enableSpring = def->enableSpring; + joint->prismaticJoint.enableLimit = def->enableLimit; + joint->prismaticJoint.enableMotor = def->enableMotor; + + // If the joint prevents collisions, then destroy all contacts between attached bodies + if ( def->collideConnected == false ) + { + b2DestroyContactsBetweenBodies( world, bodyA, bodyB ); + } + + b2JointId jointId = { joint->jointId + 1, world->worldId, pair.joint->revision }; + return jointId; +} + +b2JointId b2CreateWeldJoint( b2WorldId worldId, const b2WeldJointDef* def ) +{ + b2CheckDef( def ); + b2World* world = b2GetWorldFromId( worldId ); + + B2_ASSERT( world->locked == false ); + + if ( world->locked ) + { + return ( b2JointId ){ 0 }; + } + + b2Body* bodyA = b2GetBodyFullId( world, def->bodyIdA ); + b2Body* bodyB = b2GetBodyFullId( world, def->bodyIdB ); + + b2JointPair pair = b2CreateJoint( world, bodyA, bodyB, def->userData, 1.0f, b2_weldJoint, def->collideConnected ); + + b2JointSim* joint = pair.jointSim; + joint->type = b2_weldJoint; + joint->localOriginAnchorA = def->localAnchorA; + joint->localOriginAnchorB = def->localAnchorB; + + b2WeldJoint empty = { 0 }; + joint->weldJoint = empty; + joint->weldJoint.referenceAngle = def->referenceAngle; + joint->weldJoint.linearHertz = def->linearHertz; + joint->weldJoint.linearDampingRatio = def->linearDampingRatio; + joint->weldJoint.angularHertz = def->angularHertz; + joint->weldJoint.angularDampingRatio = def->angularDampingRatio; + joint->weldJoint.linearImpulse = b2Vec2_zero; + joint->weldJoint.angularImpulse = 0.0f; + + // If the joint prevents collisions, then destroy all contacts between attached bodies + if ( def->collideConnected == false ) + { + b2DestroyContactsBetweenBodies( world, bodyA, bodyB ); + } + + b2JointId jointId = { joint->jointId + 1, world->worldId, pair.joint->revision }; + return jointId; +} + +b2JointId b2CreateWheelJoint( b2WorldId worldId, const b2WheelJointDef* def ) +{ + b2CheckDef( def ); + b2World* world = b2GetWorldFromId( worldId ); + + B2_ASSERT( world->locked == false ); + + if ( world->locked ) + { + return ( b2JointId ){ 0 }; + } + + b2Body* bodyA = b2GetBodyFullId( world, def->bodyIdA ); + b2Body* bodyB = b2GetBodyFullId( world, def->bodyIdB ); + + b2JointPair pair = b2CreateJoint( world, bodyA, bodyB, def->userData, 1.0f, b2_wheelJoint, def->collideConnected ); + + b2JointSim* joint = pair.jointSim; + joint->type = b2_wheelJoint; + joint->localOriginAnchorA = def->localAnchorA; + joint->localOriginAnchorB = def->localAnchorB; + + joint->wheelJoint = ( b2WheelJoint ){ 0 }; + joint->wheelJoint.localAxisA = b2Normalize( def->localAxisA ); + joint->wheelJoint.perpMass = 0.0f; + joint->wheelJoint.axialMass = 0.0f; + joint->wheelJoint.motorImpulse = 0.0f; + joint->wheelJoint.lowerImpulse = 0.0f; + joint->wheelJoint.upperImpulse = 0.0f; + joint->wheelJoint.lowerTranslation = def->lowerTranslation; + joint->wheelJoint.upperTranslation = def->upperTranslation; + joint->wheelJoint.maxMotorTorque = def->maxMotorTorque; + joint->wheelJoint.motorSpeed = def->motorSpeed; + joint->wheelJoint.hertz = def->hertz; + joint->wheelJoint.dampingRatio = def->dampingRatio; + joint->wheelJoint.enableSpring = def->enableSpring; + joint->wheelJoint.enableLimit = def->enableLimit; + joint->wheelJoint.enableMotor = def->enableMotor; + + // If the joint prevents collisions, then destroy all contacts between attached bodies + if ( def->collideConnected == false ) + { + b2DestroyContactsBetweenBodies( world, bodyA, bodyB ); + } + + b2JointId jointId = { joint->jointId + 1, world->worldId, pair.joint->revision }; + return jointId; +} + +void b2DestroyJointInternal( b2World* world, b2Joint* joint, bool wakeBodies ) +{ + int jointId = joint->jointId; + + b2JointEdge* edgeA = joint->edges + 0; + b2JointEdge* edgeB = joint->edges + 1; + + int idA = edgeA->bodyId; + int idB = edgeB->bodyId; + b2Body* bodyA = b2BodyArray_Get( &world->bodies, idA ); + b2Body* bodyB = b2BodyArray_Get( &world->bodies, idB ); + + // Remove from body A + if ( edgeA->prevKey != B2_NULL_INDEX ) + { + b2Joint* prevJoint = b2JointArray_Get( &world->joints, edgeA->prevKey >> 1 ); + b2JointEdge* prevEdge = prevJoint->edges + ( edgeA->prevKey & 1 ); + prevEdge->nextKey = edgeA->nextKey; + } + + if ( edgeA->nextKey != B2_NULL_INDEX ) + { + b2Joint* nextJoint = b2JointArray_Get( &world->joints, edgeA->nextKey >> 1 ); + b2JointEdge* nextEdge = nextJoint->edges + ( edgeA->nextKey & 1 ); + nextEdge->prevKey = edgeA->prevKey; + } + + int edgeKeyA = ( jointId << 1 ) | 0; + if ( bodyA->headJointKey == edgeKeyA ) + { + bodyA->headJointKey = edgeA->nextKey; + } + + bodyA->jointCount -= 1; + + // Remove from body B + if ( edgeB->prevKey != B2_NULL_INDEX ) + { + b2Joint* prevJoint = b2JointArray_Get( &world->joints, edgeB->prevKey >> 1 ); + b2JointEdge* prevEdge = prevJoint->edges + ( edgeB->prevKey & 1 ); + prevEdge->nextKey = edgeB->nextKey; + } + + if ( edgeB->nextKey != B2_NULL_INDEX ) + { + b2Joint* nextJoint = b2JointArray_Get( &world->joints, edgeB->nextKey >> 1 ); + b2JointEdge* nextEdge = nextJoint->edges + ( edgeB->nextKey & 1 ); + nextEdge->prevKey = edgeB->prevKey; + } + + int edgeKeyB = ( jointId << 1 ) | 1; + if ( bodyB->headJointKey == edgeKeyB ) + { + bodyB->headJointKey = edgeB->nextKey; + } + + bodyB->jointCount -= 1; + + if ( joint->islandId != B2_NULL_INDEX ) + { + B2_ASSERT( joint->setIndex > b2_disabledSet ); + b2UnlinkJoint( world, joint ); + } + else + { + B2_ASSERT( joint->setIndex <= b2_disabledSet ); + } + + // Remove joint from solver set that owns it + int setIndex = joint->setIndex; + int localIndex = joint->localIndex; + + if ( setIndex == b2_awakeSet ) + { + b2RemoveJointFromGraph( world, joint->edges[0].bodyId, joint->edges[1].bodyId, joint->colorIndex, localIndex ); + } + else + { + b2SolverSet* set = b2SolverSetArray_Get( &world->solverSets, setIndex ); + int movedIndex = b2JointSimArray_RemoveSwap( &set->jointSims, localIndex ); + if ( movedIndex != B2_NULL_INDEX ) + { + // Fix moved joint + b2JointSim* movedJointSim = set->jointSims.data + localIndex; + int movedId = movedJointSim->jointId; + b2Joint* movedJoint = b2JointArray_Get( &world->joints, movedId ); + B2_ASSERT( movedJoint->localIndex == movedIndex ); + movedJoint->localIndex = localIndex; + } + } + + // Free joint and id (preserve joint revision) + joint->setIndex = B2_NULL_INDEX; + joint->localIndex = B2_NULL_INDEX; + joint->colorIndex = B2_NULL_INDEX; + joint->jointId = B2_NULL_INDEX; + b2FreeId( &world->jointIdPool, jointId ); + + if ( wakeBodies ) + { + b2WakeBody( world, bodyA ); + b2WakeBody( world, bodyB ); + } + + b2ValidateSolverSets( world ); +} + +void b2DestroyJoint( b2JointId jointId ) +{ + b2World* world = b2GetWorld( jointId.world0 ); + B2_ASSERT( world->locked == false ); + + if ( world->locked ) + { + return; + } + + b2Joint* joint = b2GetJointFullId( world, jointId ); + + b2DestroyJointInternal( world, joint, true ); +} + +b2JointType b2Joint_GetType( b2JointId jointId ) +{ + b2World* world = b2GetWorld( jointId.world0 ); + b2Joint* joint = b2GetJointFullId( world, jointId ); + return joint->type; +} + +b2BodyId b2Joint_GetBodyA( b2JointId jointId ) +{ + b2World* world = b2GetWorld( jointId.world0 ); + b2Joint* joint = b2GetJointFullId( world, jointId ); + return b2MakeBodyId( world, joint->edges[0].bodyId ); +} + +b2BodyId b2Joint_GetBodyB( b2JointId jointId ) +{ + b2World* world = b2GetWorld( jointId.world0 ); + b2Joint* joint = b2GetJointFullId( world, jointId ); + return b2MakeBodyId( world, joint->edges[1].bodyId ); +} + +b2WorldId b2Joint_GetWorld( b2JointId jointId ) +{ + b2World* world = b2GetWorld( jointId.world0 ); + return ( b2WorldId ){ jointId.world0 + 1, world->revision }; +} + +b2Vec2 b2Joint_GetLocalAnchorA( b2JointId jointId ) +{ + b2World* world = b2GetWorld( jointId.world0 ); + b2Joint* joint = b2GetJointFullId( world, jointId ); + b2JointSim* jointSim = b2GetJointSim( world, joint ); + return jointSim->localOriginAnchorA; +} + +b2Vec2 b2Joint_GetLocalAnchorB( b2JointId jointId ) +{ + b2World* world = b2GetWorld( jointId.world0 ); + b2Joint* joint = b2GetJointFullId( world, jointId ); + b2JointSim* jointSim = b2GetJointSim( world, joint ); + return jointSim->localOriginAnchorB; +} + +void b2Joint_SetCollideConnected( b2JointId jointId, bool shouldCollide ) +{ + b2World* world = b2GetWorldLocked( jointId.world0 ); + if ( world == NULL ) + { + return; + } + + b2Joint* joint = b2GetJointFullId( world, jointId ); + if ( joint->collideConnected == shouldCollide ) + { + return; + } + + joint->collideConnected = shouldCollide; + + b2Body* bodyA = b2BodyArray_Get( &world->bodies, joint->edges[0].bodyId ); + b2Body* bodyB = b2BodyArray_Get( &world->bodies, joint->edges[1].bodyId ); + + if ( shouldCollide ) + { + // need to tell the broad-phase to look for new pairs for one of the + // two bodies. Pick the one with the fewest shapes. + int shapeCountA = bodyA->shapeCount; + int shapeCountB = bodyB->shapeCount; + + int shapeId = shapeCountA < shapeCountB ? bodyA->headShapeId : bodyB->headShapeId; + while ( shapeId != B2_NULL_INDEX ) + { + b2Shape* shape = b2ShapeArray_Get( &world->shapes, shapeId ); + + if ( shape->proxyKey != B2_NULL_INDEX ) + { + b2BufferMove( &world->broadPhase, shape->proxyKey ); + } + + shapeId = shape->nextShapeId; + } + } + else + { + b2DestroyContactsBetweenBodies( world, bodyA, bodyB ); + } +} + +bool b2Joint_GetCollideConnected( b2JointId jointId ) +{ + b2World* world = b2GetWorld( jointId.world0 ); + b2Joint* joint = b2GetJointFullId( world, jointId ); + return joint->collideConnected; +} + +void b2Joint_SetUserData( b2JointId jointId, void* userData ) +{ + b2World* world = b2GetWorld( jointId.world0 ); + b2Joint* joint = b2GetJointFullId( world, jointId ); + joint->userData = userData; +} + +void* b2Joint_GetUserData( b2JointId jointId ) +{ + b2World* world = b2GetWorld( jointId.world0 ); + b2Joint* joint = b2GetJointFullId( world, jointId ); + return joint->userData; +} + +void b2Joint_WakeBodies( b2JointId jointId ) +{ + b2World* world = b2GetWorldLocked( jointId.world0 ); + if ( world == NULL ) + { + return; + } + + b2Joint* joint = b2GetJointFullId( world, jointId ); + b2Body* bodyA = b2BodyArray_Get( &world->bodies, joint->edges[0].bodyId ); + b2Body* bodyB = b2BodyArray_Get( &world->bodies, joint->edges[1].bodyId ); + + b2WakeBody( world, bodyA ); + b2WakeBody( world, bodyB ); +} + +extern b2Vec2 b2GetDistanceJointForce( b2World* world, b2JointSim* base ); +extern b2Vec2 b2GetMotorJointForce( b2World* world, b2JointSim* base ); +extern b2Vec2 b2GetMouseJointForce( b2World* world, b2JointSim* base ); +extern b2Vec2 b2GetPrismaticJointForce( b2World* world, b2JointSim* base ); +extern b2Vec2 b2GetRevoluteJointForce( b2World* world, b2JointSim* base ); +extern b2Vec2 b2GetWeldJointForce( b2World* world, b2JointSim* base ); +extern b2Vec2 b2GetWheelJointForce( b2World* world, b2JointSim* base ); + +b2Vec2 b2Joint_GetConstraintForce( b2JointId jointId ) +{ + b2World* world = b2GetWorld( jointId.world0 ); + b2Joint* joint = b2GetJointFullId( world, jointId ); + b2JointSim* base = b2GetJointSim( world, joint ); + + switch ( joint->type ) + { + case b2_distanceJoint: + return b2GetDistanceJointForce( world, base ); + + case b2_motorJoint: + return b2GetMotorJointForce( world, base ); + + case b2_mouseJoint: + return b2GetMouseJointForce( world, base ); + + case b2_prismaticJoint: + return b2GetPrismaticJointForce( world, base ); + + case b2_revoluteJoint: + return b2GetRevoluteJointForce( world, base ); + + case b2_weldJoint: + return b2GetWeldJointForce( world, base ); + + case b2_wheelJoint: + return b2GetWheelJointForce( world, base ); + + default: + B2_ASSERT( false ); + return b2Vec2_zero; + } +} + +extern float b2GetMotorJointTorque( b2World* world, b2JointSim* base ); +extern float b2GetMouseJointTorque( b2World* world, b2JointSim* base ); +extern float b2GetPrismaticJointTorque( b2World* world, b2JointSim* base ); +extern float b2GetRevoluteJointTorque( b2World* world, b2JointSim* base ); +extern float b2GetWeldJointTorque( b2World* world, b2JointSim* base ); +extern float b2GetWheelJointTorque( b2World* world, b2JointSim* base ); + +float b2Joint_GetConstraintTorque( b2JointId jointId ) +{ + b2World* world = b2GetWorld( jointId.world0 ); + b2Joint* joint = b2GetJointFullId( world, jointId ); + b2JointSim* base = b2GetJointSim( world, joint ); + + switch ( joint->type ) + { + case b2_distanceJoint: + return 0.0f; + + case b2_motorJoint: + return b2GetMotorJointTorque( world, base ); + + case b2_mouseJoint: + return b2GetMouseJointTorque( world, base ); + + case b2_prismaticJoint: + return b2GetPrismaticJointTorque( world, base ); + + case b2_revoluteJoint: + return b2GetRevoluteJointTorque( world, base ); + + case b2_weldJoint: + return b2GetWeldJointTorque( world, base ); + + case b2_wheelJoint: + return b2GetWheelJointTorque( world, base ); + + default: + B2_ASSERT( false ); + return 0.0f; + } +} + +extern void b2PrepareDistanceJoint( b2JointSim* base, b2StepContext* context ); +extern void b2PrepareMotorJoint( b2JointSim* base, b2StepContext* context ); +extern void b2PrepareMouseJoint( b2JointSim* base, b2StepContext* context ); +extern void b2PreparePrismaticJoint( b2JointSim* base, b2StepContext* context ); +extern void b2PrepareRevoluteJoint( b2JointSim* base, b2StepContext* context ); +extern void b2PrepareWeldJoint( b2JointSim* base, b2StepContext* context ); +extern void b2PrepareWheelJoint( b2JointSim* base, b2StepContext* context ); + +void b2PrepareJoint( b2JointSim* joint, b2StepContext* context ) +{ + switch ( joint->type ) + { + case b2_distanceJoint: + b2PrepareDistanceJoint( joint, context ); + break; + + case b2_motorJoint: + b2PrepareMotorJoint( joint, context ); + break; + + case b2_mouseJoint: + b2PrepareMouseJoint( joint, context ); + break; + + case b2_prismaticJoint: + b2PreparePrismaticJoint( joint, context ); + break; + + case b2_revoluteJoint: + b2PrepareRevoluteJoint( joint, context ); + break; + + case b2_weldJoint: + b2PrepareWeldJoint( joint, context ); + break; + + case b2_wheelJoint: + b2PrepareWheelJoint( joint, context ); + break; + + default: + B2_ASSERT( false ); + } +} + +extern void b2WarmStartDistanceJoint( b2JointSim* base, b2StepContext* context ); +extern void b2WarmStartMotorJoint( b2JointSim* base, b2StepContext* context ); +extern void b2WarmStartMouseJoint( b2JointSim* base, b2StepContext* context ); +extern void b2WarmStartPrismaticJoint( b2JointSim* base, b2StepContext* context ); +extern void b2WarmStartRevoluteJoint( b2JointSim* base, b2StepContext* context ); +extern void b2WarmStartWeldJoint( b2JointSim* base, b2StepContext* context ); +extern void b2WarmStartWheelJoint( b2JointSim* base, b2StepContext* context ); + +void b2WarmStartJoint( b2JointSim* joint, b2StepContext* context ) +{ + switch ( joint->type ) + { + case b2_distanceJoint: + b2WarmStartDistanceJoint( joint, context ); + break; + + case b2_motorJoint: + b2WarmStartMotorJoint( joint, context ); + break; + + case b2_mouseJoint: + b2WarmStartMouseJoint( joint, context ); + break; + + case b2_prismaticJoint: + b2WarmStartPrismaticJoint( joint, context ); + break; + + case b2_revoluteJoint: + b2WarmStartRevoluteJoint( joint, context ); + break; + + case b2_weldJoint: + b2WarmStartWeldJoint( joint, context ); + break; + + case b2_wheelJoint: + b2WarmStartWheelJoint( joint, context ); + break; + + default: + B2_ASSERT( false ); + } +} + +extern void b2SolveDistanceJoint( b2JointSim* base, b2StepContext* context, bool useBias ); +extern void b2SolveMotorJoint( b2JointSim* base, b2StepContext* context, bool useBias ); +extern void b2SolveMouseJoint( b2JointSim* base, b2StepContext* context ); +extern void b2SolvePrismaticJoint( b2JointSim* base, b2StepContext* context, bool useBias ); +extern void b2SolveRevoluteJoint( b2JointSim* base, b2StepContext* context, bool useBias ); +extern void b2SolveWeldJoint( b2JointSim* base, b2StepContext* context, bool useBias ); +extern void b2SolveWheelJoint( b2JointSim* base, b2StepContext* context, bool useBias ); + +void b2SolveJoint( b2JointSim* joint, b2StepContext* context, bool useBias ) +{ + switch ( joint->type ) + { + case b2_distanceJoint: + b2SolveDistanceJoint( joint, context, useBias ); + break; + + case b2_motorJoint: + b2SolveMotorJoint( joint, context, useBias ); + break; + + case b2_mouseJoint: + b2SolveMouseJoint( joint, context ); + break; + + case b2_prismaticJoint: + b2SolvePrismaticJoint( joint, context, useBias ); + break; + + case b2_revoluteJoint: + b2SolveRevoluteJoint( joint, context, useBias ); + break; + + case b2_weldJoint: + b2SolveWeldJoint( joint, context, useBias ); + break; + + case b2_wheelJoint: + b2SolveWheelJoint( joint, context, useBias ); + break; + + default: + B2_ASSERT( false ); + } +} + +void b2PrepareOverflowJoints( b2StepContext* context ) +{ + b2TracyCZoneNC( prepare_joints, "PrepJoints", b2_colorOldLace, true ); + + b2ConstraintGraph* graph = context->graph; + b2JointSim* joints = graph->colors[b2_overflowIndex].jointSims.data; + int jointCount = graph->colors[b2_overflowIndex].jointSims.count; + + for ( int i = 0; i < jointCount; ++i ) + { + b2JointSim* joint = joints + i; + b2PrepareJoint( joint, context ); + } + + b2TracyCZoneEnd( prepare_joints ); +} + +void b2WarmStartOverflowJoints( b2StepContext* context ) +{ + b2TracyCZoneNC( prepare_joints, "PrepJoints", b2_colorOldLace, true ); + + b2ConstraintGraph* graph = context->graph; + b2JointSim* joints = graph->colors[b2_overflowIndex].jointSims.data; + int jointCount = graph->colors[b2_overflowIndex].jointSims.count; + + for ( int i = 0; i < jointCount; ++i ) + { + b2JointSim* joint = joints + i; + b2WarmStartJoint( joint, context ); + } + + b2TracyCZoneEnd( prepare_joints ); +} + +void b2SolveOverflowJoints( b2StepContext* context, bool useBias ) +{ + b2TracyCZoneNC( solve_joints, "SolveJoints", b2_colorLemonChiffon, true ); + + b2ConstraintGraph* graph = context->graph; + b2JointSim* joints = graph->colors[b2_overflowIndex].jointSims.data; + int jointCount = graph->colors[b2_overflowIndex].jointSims.count; + + for ( int i = 0; i < jointCount; ++i ) + { + b2JointSim* joint = joints + i; + b2SolveJoint( joint, context, useBias ); + } + + b2TracyCZoneEnd( solve_joints ); +} + +extern void b2DrawDistanceJoint( b2DebugDraw* draw, b2JointSim* base, b2Transform transformA, b2Transform transformB ); +extern void b2DrawPrismaticJoint( b2DebugDraw* draw, b2JointSim* base, b2Transform transformA, b2Transform transformB ); +extern void b2DrawRevoluteJoint( b2DebugDraw* draw, b2JointSim* base, b2Transform transformA, b2Transform transformB, + float drawSize ); +extern void b2DrawWheelJoint( b2DebugDraw* draw, b2JointSim* base, b2Transform transformA, b2Transform transformB ); + +void b2DrawJoint( b2DebugDraw* draw, b2World* world, b2Joint* joint ) +{ + b2Body* bodyA = b2BodyArray_Get( &world->bodies, joint->edges[0].bodyId ); + b2Body* bodyB = b2BodyArray_Get( &world->bodies, joint->edges[1].bodyId ); + if ( bodyA->setIndex == b2_disabledSet || bodyB->setIndex == b2_disabledSet ) + { + return; + } + + b2JointSim* jointSim = b2GetJointSim( world, joint ); + + b2Transform transformA = b2GetBodyTransformQuick( world, bodyA ); + b2Transform transformB = b2GetBodyTransformQuick( world, bodyB ); + b2Vec2 pA = b2TransformPoint( transformA, jointSim->localOriginAnchorA ); + b2Vec2 pB = b2TransformPoint( transformB, jointSim->localOriginAnchorB ); + + b2HexColor color = b2_colorDarkSeaGreen; + + switch ( joint->type ) + { + case b2_distanceJoint: + b2DrawDistanceJoint( draw, jointSim, transformA, transformB ); + break; + + case b2_mouseJoint: + { + b2Vec2 target = jointSim->mouseJoint.targetA; + + b2HexColor c1 = b2_colorGreen; + draw->DrawPoint( target, 4.0f, c1, draw->context ); + draw->DrawPoint( pB, 4.0f, c1, draw->context ); + + b2HexColor c2 = b2_colorGray8; + draw->DrawSegment( target, pB, c2, draw->context ); + } + break; + + case b2_prismaticJoint: + b2DrawPrismaticJoint( draw, jointSim, transformA, transformB ); + break; + + case b2_revoluteJoint: + b2DrawRevoluteJoint( draw, jointSim, transformA, transformB, joint->drawSize ); + break; + + case b2_wheelJoint: + b2DrawWheelJoint( draw, jointSim, transformA, transformB ); + break; + + default: + draw->DrawSegment( transformA.p, pA, color, draw->context ); + draw->DrawSegment( pA, pB, color, draw->context ); + draw->DrawSegment( transformB.p, pB, color, draw->context ); + } + + if ( draw->drawGraphColors ) + { + b2HexColor colors[b2_graphColorCount] = { b2_colorRed, b2_colorOrange, b2_colorYellow, b2_colorGreen, + b2_colorCyan, b2_colorBlue, b2_colorViolet, b2_colorPink, + b2_colorChocolate, b2_colorGoldenrod, b2_colorCoral, b2_colorBlack }; + + int colorIndex = joint->colorIndex; + if ( colorIndex != B2_NULL_INDEX ) + { + b2Vec2 p = b2Lerp( pA, pB, 0.5f ); + draw->DrawPoint( p, 5.0f, colors[colorIndex], draw->context ); + } + } +} diff --git a/3rdparty/box2d/src/joint.h b/3rdparty/box2d/src/joint.h new file mode 100644 index 000000000000..80ac04bfb6bb --- /dev/null +++ b/3rdparty/box2d/src/joint.h @@ -0,0 +1,289 @@ +// SPDX-FileCopyrightText: 2023 Erin Catto +// SPDX-License-Identifier: MIT +#pragma once + +#include "array.h" +#include "solver.h" + +#include "box2d/types.h" + +typedef struct b2DebugDraw b2DebugDraw; +typedef struct b2StepContext b2StepContext; +typedef struct b2World b2World; + +/// A joint edge is used to connect bodies and joints together +/// in a joint graph where each body is a node and each joint +/// is an edge. A joint edge belongs to a doubly linked list +/// maintained in each attached body. Each joint has two joint +/// nodes, one for each attached body. +typedef struct b2JointEdge +{ + int bodyId; + int prevKey; + int nextKey; +} b2JointEdge; + +// Map from b2JointId to b2Joint in the solver sets +typedef struct b2Joint +{ + void* userData; + + // index of simulation set stored in b2World + // B2_NULL_INDEX when slot is free + int setIndex; + + // index into the constraint graph color array, may be B2_NULL_INDEX for sleeping/disabled joints + // B2_NULL_INDEX when slot is free + int colorIndex; + + // joint index within set or graph color + // B2_NULL_INDEX when slot is free + int localIndex; + + b2JointEdge edges[2]; + + int jointId; + int islandId; + int islandPrev; + int islandNext; + + // This is monotonically advanced when a body is allocated in this slot + // Used to check for invalid b2JointId + int revision; + + float drawSize; + + b2JointType type; + bool isMarked; + bool collideConnected; + +} b2Joint; + +typedef struct b2DistanceJoint +{ + float length; + float hertz; + float dampingRatio; + float minLength; + float maxLength; + + float maxMotorForce; + float motorSpeed; + + float impulse; + float lowerImpulse; + float upperImpulse; + float motorImpulse; + + int indexA; + int indexB; + b2Vec2 anchorA; + b2Vec2 anchorB; + b2Vec2 deltaCenter; + b2Softness distanceSoftness; + float axialMass; + + bool enableSpring; + bool enableLimit; + bool enableMotor; +} b2DistanceJoint; + +typedef struct b2MotorJoint +{ + b2Vec2 linearOffset; + float angularOffset; + b2Vec2 linearImpulse; + float angularImpulse; + float maxForce; + float maxTorque; + float correctionFactor; + + int indexA; + int indexB; + b2Vec2 anchorA; + b2Vec2 anchorB; + b2Vec2 deltaCenter; + float deltaAngle; + b2Mat22 linearMass; + float angularMass; +} b2MotorJoint; + +typedef struct b2MouseJoint +{ + b2Vec2 targetA; + float hertz; + float dampingRatio; + float maxForce; + + b2Vec2 linearImpulse; + float angularImpulse; + + b2Softness linearSoftness; + b2Softness angularSoftness; + int indexB; + b2Vec2 anchorB; + b2Vec2 deltaCenter; + b2Mat22 linearMass; +} b2MouseJoint; + +typedef struct b2PrismaticJoint +{ + b2Vec2 localAxisA; + b2Vec2 impulse; + float springImpulse; + float motorImpulse; + float lowerImpulse; + float upperImpulse; + float hertz; + float dampingRatio; + float maxMotorForce; + float motorSpeed; + float referenceAngle; + float lowerTranslation; + float upperTranslation; + + int indexA; + int indexB; + b2Vec2 anchorA; + b2Vec2 anchorB; + b2Vec2 axisA; + b2Vec2 deltaCenter; + float deltaAngle; + float axialMass; + b2Softness springSoftness; + + bool enableSpring; + bool enableLimit; + bool enableMotor; +} b2PrismaticJoint; + +typedef struct b2RevoluteJoint +{ + b2Vec2 linearImpulse; + float springImpulse; + float motorImpulse; + float lowerImpulse; + float upperImpulse; + float hertz; + float dampingRatio; + float maxMotorTorque; + float motorSpeed; + float referenceAngle; + float lowerAngle; + float upperAngle; + + int indexA; + int indexB; + b2Vec2 anchorA; + b2Vec2 anchorB; + b2Vec2 deltaCenter; + float deltaAngle; + float axialMass; + b2Softness springSoftness; + + bool enableSpring; + bool enableMotor; + bool enableLimit; +} b2RevoluteJoint; + +typedef struct b2WeldJoint +{ + float referenceAngle; + float linearHertz; + float linearDampingRatio; + float angularHertz; + float angularDampingRatio; + + b2Softness linearSoftness; + b2Softness angularSoftness; + b2Vec2 linearImpulse; + float angularImpulse; + + int indexA; + int indexB; + b2Vec2 anchorA; + b2Vec2 anchorB; + b2Vec2 deltaCenter; + float deltaAngle; + float axialMass; +} b2WeldJoint; + +typedef struct b2WheelJoint +{ + b2Vec2 localAxisA; + float perpImpulse; + float motorImpulse; + float springImpulse; + float lowerImpulse; + float upperImpulse; + float maxMotorTorque; + float motorSpeed; + float lowerTranslation; + float upperTranslation; + float hertz; + float dampingRatio; + + int indexA; + int indexB; + b2Vec2 anchorA; + b2Vec2 anchorB; + b2Vec2 axisA; + b2Vec2 deltaCenter; + float perpMass; + float motorMass; + float axialMass; + b2Softness springSoftness; + + bool enableSpring; + bool enableMotor; + bool enableLimit; +} b2WheelJoint; + +/// The base joint class. Joints are used to constraint two bodies together in +/// various fashions. Some joints also feature limits and motors. +typedef struct b2JointSim +{ + int jointId; + + int bodyIdA; + int bodyIdB; + + b2JointType type; + + // Anchors relative to body origin + b2Vec2 localOriginAnchorA; + b2Vec2 localOriginAnchorB; + + float invMassA, invMassB; + float invIA, invIB; + + union + { + b2DistanceJoint distanceJoint; + b2MotorJoint motorJoint; + b2MouseJoint mouseJoint; + b2RevoluteJoint revoluteJoint; + b2PrismaticJoint prismaticJoint; + b2WeldJoint weldJoint; + b2WheelJoint wheelJoint; + }; +} b2JointSim; + +void b2DestroyJointInternal( b2World* world, b2Joint* joint, bool wakeBodies ); + +b2JointSim* b2GetJointSim( b2World* world, b2Joint* joint ); +b2JointSim* b2GetJointSimCheckType( b2JointId jointId, b2JointType type ); + +void b2PrepareJoint( b2JointSim* joint, b2StepContext* context ); +void b2WarmStartJoint( b2JointSim* joint, b2StepContext* context ); +void b2SolveJoint( b2JointSim* joint, b2StepContext* context, bool useBias ); + +void b2PrepareOverflowJoints( b2StepContext* context ); +void b2WarmStartOverflowJoints( b2StepContext* context ); +void b2SolveOverflowJoints( b2StepContext* context, bool useBias ); + +void b2DrawJoint( b2DebugDraw* draw, b2World* world, b2Joint* joint ); + +// Define inline functions for arrays +B2_ARRAY_INLINE( b2Joint, b2Joint ); +B2_ARRAY_INLINE( b2JointSim, b2JointSim ); diff --git a/3rdparty/box2d/src/manifold.c b/3rdparty/box2d/src/manifold.c new file mode 100644 index 000000000000..b66785d2850d --- /dev/null +++ b/3rdparty/box2d/src/manifold.c @@ -0,0 +1,1601 @@ +// SPDX-FileCopyrightText: 2023 Erin Catto +// SPDX-License-Identifier: MIT + +#include "core.h" + +#include "box2d/collision.h" +#include "box2d/math_functions.h" + +#include +#include + +#define B2_MAKE_ID( A, B ) ( (uint8_t)( A ) << 8 | (uint8_t)( B ) ) + +static b2Polygon b2MakeCapsule( b2Vec2 p1, b2Vec2 p2, float radius ) +{ + b2Polygon shape = { 0 }; + shape.vertices[0] = p1; + shape.vertices[1] = p2; + shape.centroid = b2Lerp( p1, p2, 0.5f ); + + b2Vec2 d = b2Sub( p2, p1 ); + B2_ASSERT( b2LengthSquared( d ) > FLT_EPSILON ); + b2Vec2 axis = b2Normalize( d ); + b2Vec2 normal = b2RightPerp( axis ); + + shape.normals[0] = normal; + shape.normals[1] = b2Neg( normal ); + shape.count = 2; + shape.radius = radius; + + return shape; +} + +// point = qA * localAnchorA + pA +// localAnchorB = qBc * (point - pB) +// anchorB = point - pB = qA * localAnchorA + pA - pB +// = anchorA + (pA - pB) +b2Manifold b2CollideCircles( const b2Circle* circleA, b2Transform xfA, const b2Circle* circleB, b2Transform xfB ) +{ + b2Manifold manifold = { 0 }; + + b2Transform xf = b2InvMulTransforms( xfA, xfB ); + + b2Vec2 pointA = circleA->center; + b2Vec2 pointB = b2TransformPoint( xf, circleB->center ); + + float distance; + b2Vec2 normal = b2GetLengthAndNormalize( &distance, b2Sub( pointB, pointA ) ); + + float radiusA = circleA->radius; + float radiusB = circleB->radius; + + float separation = distance - radiusA - radiusB; + if ( separation > b2_speculativeDistance ) + { + return manifold; + } + + b2Vec2 cA = b2MulAdd( pointA, radiusA, normal ); + b2Vec2 cB = b2MulAdd( pointB, -radiusB, normal ); + b2Vec2 contactPointA = b2Lerp( cA, cB, 0.5f ); + + manifold.normal = b2RotateVector( xfA.q, normal ); + b2ManifoldPoint* mp = manifold.points + 0; + mp->anchorA = b2RotateVector( xfA.q, contactPointA ); + mp->anchorB = b2Add( mp->anchorA, b2Sub( xfA.p, xfB.p ) ); + mp->point = b2Add( xfA.p, mp->anchorA ); + mp->separation = separation; + mp->id = 0; + manifold.pointCount = 1; + return manifold; +} + +/// Compute the collision manifold between a capsule and circle +b2Manifold b2CollideCapsuleAndCircle( const b2Capsule* capsuleA, b2Transform xfA, const b2Circle* circleB, b2Transform xfB ) +{ + b2Manifold manifold = { 0 }; + + b2Transform xf = b2InvMulTransforms( xfA, xfB ); + + // Compute circle position in the frame of the capsule. + b2Vec2 pB = b2TransformPoint( xf, circleB->center ); + + // Compute closest point + b2Vec2 p1 = capsuleA->center1; + b2Vec2 p2 = capsuleA->center2; + + b2Vec2 e = b2Sub( p2, p1 ); + + // dot(p - pA, e) = 0 + // dot(p - (p1 + s1 * e), e) = 0 + // s1 = dot(p - p1, e) + b2Vec2 pA; + float s1 = b2Dot( b2Sub( pB, p1 ), e ); + float s2 = b2Dot( b2Sub( p2, pB ), e ); + if ( s1 < 0.0f ) + { + // p1 region + pA = p1; + } + else if ( s2 < 0.0f ) + { + // p2 region + pA = p2; + } + else + { + // circle colliding with segment interior + float s = s1 / b2Dot( e, e ); + pA = b2MulAdd( p1, s, e ); + } + + float distance; + b2Vec2 normal = b2GetLengthAndNormalize( &distance, b2Sub( pB, pA ) ); + + float radiusA = capsuleA->radius; + float radiusB = circleB->radius; + float separation = distance - radiusA - radiusB; + if ( separation > b2_speculativeDistance ) + { + return manifold; + } + + b2Vec2 cA = b2MulAdd( pA, radiusA, normal ); + b2Vec2 cB = b2MulAdd( pB, -radiusB, normal ); + b2Vec2 contactPointA = b2Lerp( cA, cB, 0.5f ); + + manifold.normal = b2RotateVector( xfA.q, normal ); + b2ManifoldPoint* mp = manifold.points + 0; + mp->anchorA = b2RotateVector( xfA.q, contactPointA ); + mp->anchorB = b2Add( mp->anchorA, b2Sub( xfA.p, xfB.p ) ); + mp->point = b2Add( xfA.p, mp->anchorA ); + mp->separation = separation; + mp->id = 0; + manifold.pointCount = 1; + return manifold; +} + +b2Manifold b2CollidePolygonAndCircle( const b2Polygon* polygonA, b2Transform xfA, const b2Circle* circleB, b2Transform xfB ) +{ + b2Manifold manifold = { 0 }; + const float speculativeDistance = b2_speculativeDistance; + + b2Transform xf = b2InvMulTransforms( xfA, xfB ); + + // Compute circle position in the frame of the polygon. + b2Vec2 c = b2TransformPoint( xf, circleB->center ); + float radiusA = polygonA->radius; + float radiusB = circleB->radius; + float radius = radiusA + radiusB; + + // Find the min separating edge. + int32_t normalIndex = 0; + float separation = -FLT_MAX; + int32_t vertexCount = polygonA->count; + const b2Vec2* vertices = polygonA->vertices; + const b2Vec2* normals = polygonA->normals; + + for ( int32_t i = 0; i < vertexCount; ++i ) + { + float s = b2Dot( normals[i], b2Sub( c, vertices[i] ) ); + if ( s > separation ) + { + separation = s; + normalIndex = i; + } + } + + if ( separation > radius + speculativeDistance ) + { + return manifold; + } + + // Vertices of the reference edge. + int32_t vertIndex1 = normalIndex; + int32_t vertIndex2 = vertIndex1 + 1 < vertexCount ? vertIndex1 + 1 : 0; + b2Vec2 v1 = vertices[vertIndex1]; + b2Vec2 v2 = vertices[vertIndex2]; + + // Compute barycentric coordinates + float u1 = b2Dot( b2Sub( c, v1 ), b2Sub( v2, v1 ) ); + float u2 = b2Dot( b2Sub( c, v2 ), b2Sub( v1, v2 ) ); + + if ( u1 < 0.0f && separation > FLT_EPSILON ) + { + // Circle center is closest to v1 and safely outside the polygon + b2Vec2 normal = b2Normalize( b2Sub( c, v1 ) ); + separation = b2Dot( b2Sub( c, v1 ), normal ); + if ( separation > radius + speculativeDistance ) + { + return manifold; + } + + b2Vec2 cA = b2MulAdd( v1, radiusA, normal ); + b2Vec2 cB = b2MulSub( c, radiusB, normal ); + b2Vec2 contactPointA = b2Lerp( cA, cB, 0.5f ); + + manifold.normal = b2RotateVector( xfA.q, normal ); + b2ManifoldPoint* mp = manifold.points + 0; + mp->anchorA = b2RotateVector( xfA.q, contactPointA ); + mp->anchorB = b2Add( mp->anchorA, b2Sub( xfA.p, xfB.p ) ); + mp->point = b2Add( xfA.p, mp->anchorA ); + mp->separation = b2Dot( b2Sub( cB, cA ), normal ); + mp->id = 0; + manifold.pointCount = 1; + } + else if ( u2 < 0.0f && separation > FLT_EPSILON ) + { + // Circle center is closest to v2 and safely outside the polygon + b2Vec2 normal = b2Normalize( b2Sub( c, v2 ) ); + separation = b2Dot( b2Sub( c, v2 ), normal ); + if ( separation > radius + speculativeDistance ) + { + return manifold; + } + + b2Vec2 cA = b2MulAdd( v2, radiusA, normal ); + b2Vec2 cB = b2MulSub( c, radiusB, normal ); + b2Vec2 contactPointA = b2Lerp( cA, cB, 0.5f ); + + manifold.normal = b2RotateVector( xfA.q, normal ); + b2ManifoldPoint* mp = manifold.points + 0; + mp->anchorA = b2RotateVector( xfA.q, contactPointA ); + mp->anchorB = b2Add( mp->anchorA, b2Sub( xfA.p, xfB.p ) ); + mp->point = b2Add( xfA.p, mp->anchorA ); + mp->separation = b2Dot( b2Sub( cB, cA ), normal ); + mp->id = 0; + manifold.pointCount = 1; + } + else + { + // Circle center is between v1 and v2. Center may be inside polygon + b2Vec2 normal = normals[normalIndex]; + manifold.normal = b2RotateVector( xfA.q, normal ); + + // cA is the projection of the circle center onto to the reference edge + b2Vec2 cA = b2MulAdd( c, radiusA - b2Dot( b2Sub( c, v1 ), normal ), normal ); + + // cB is the deepest point on the circle with respect to the reference edge + b2Vec2 cB = b2MulSub( c, radiusB, normal ); + + b2Vec2 contactPointA = b2Lerp( cA, cB, 0.5f ); + + // The contact point is the midpoint in world space + b2ManifoldPoint* mp = manifold.points + 0; + mp->anchorA = b2RotateVector( xfA.q, contactPointA ); + mp->anchorB = b2Add( mp->anchorA, b2Sub( xfA.p, xfB.p ) ); + mp->point = b2Add( xfA.p, mp->anchorA ); + mp->separation = separation - radius; + mp->id = 0; + manifold.pointCount = 1; + } + + return manifold; +} + +// Follows Ericson 5.1.9 Closest Points of Two Line Segments +// Adds some logic to support clipping to get two contact points +b2Manifold b2CollideCapsules( const b2Capsule* capsuleA, b2Transform xfA, const b2Capsule* capsuleB, b2Transform xfB ) +{ + b2Vec2 origin = capsuleA->center1; + + // Shift polyA to origin + // pw = q * pb + p + // pw = q * (pbs + origin) + p + // pw = q * pbs + (p + q * origin) + b2Transform sfA = { b2Add( xfA.p, b2RotateVector( xfA.q, origin ) ), xfA.q }; + b2Transform xf = b2InvMulTransforms( sfA, xfB ); + + b2Vec2 p1 = b2Vec2_zero; + b2Vec2 q1 = b2Sub( capsuleA->center2, origin ); + + b2Vec2 p2 = b2TransformPoint( xf, capsuleB->center1 ); + b2Vec2 q2 = b2TransformPoint( xf, capsuleB->center2 ); + + b2Vec2 d1 = b2Sub( q1, p1 ); + b2Vec2 d2 = b2Sub( q2, p2 ); + + float dd1 = b2Dot( d1, d1 ); + float dd2 = b2Dot( d2, d2 ); + + const float epsSqr = FLT_EPSILON * FLT_EPSILON; + B2_ASSERT( dd1 > epsSqr && dd2 > epsSqr ); + + b2Vec2 r = b2Sub( p1, p2 ); + float rd1 = b2Dot( r, d1 ); + float rd2 = b2Dot( r, d2 ); + + float d12 = b2Dot( d1, d2 ); + + float denom = dd1 * dd2 - d12 * d12; + + // Fraction on segment 1 + float f1 = 0.0f; + if ( denom != 0.0f ) + { + // not parallel + f1 = b2ClampFloat( ( d12 * rd2 - rd1 * dd2 ) / denom, 0.0f, 1.0f ); + } + + // Compute point on segment 2 closest to p1 + f1 * d1 + float f2 = ( d12 * f1 + rd2 ) / dd2; + + // Clamping of segment 2 requires a do over on segment 1 + if ( f2 < 0.0f ) + { + f2 = 0.0f; + f1 = b2ClampFloat( -rd1 / dd1, 0.0f, 1.0f ); + } + else if ( f2 > 1.0f ) + { + f2 = 1.0f; + f1 = b2ClampFloat( ( d12 - rd1 ) / dd1, 0.0f, 1.0f ); + } + + b2Vec2 closest1 = b2MulAdd( p1, f1, d1 ); + b2Vec2 closest2 = b2MulAdd( p2, f2, d2 ); + float distanceSquared = b2DistanceSquared( closest1, closest2 ); + + b2Manifold manifold = { 0 }; + float radiusA = capsuleA->radius; + float radiusB = capsuleB->radius; + float radius = radiusA + radiusB; + float maxDistance = radius + b2_speculativeDistance; + + if ( distanceSquared > maxDistance * maxDistance ) + { + return manifold; + } + + float distance = sqrt( distanceSquared ); + + float length1, length2; + b2Vec2 u1 = b2GetLengthAndNormalize( &length1, d1 ); + b2Vec2 u2 = b2GetLengthAndNormalize( &length2, d2 ); + + // Does segment B project outside segment A? + float fp2 = b2Dot( b2Sub( p2, p1 ), u1 ); + float fq2 = b2Dot( b2Sub( q2, p1 ), u1 ); + bool outsideA = ( fp2 <= 0.0f && fq2 <= 0.0f ) || ( fp2 >= length1 && fq2 >= length1 ); + + // Does segment A project outside segment B? + float fp1 = b2Dot( b2Sub( p1, p2 ), u2 ); + float fq1 = b2Dot( b2Sub( q1, p2 ), u2 ); + bool outsideB = ( fp1 <= 0.0f && fq1 <= 0.0f ) || ( fp1 >= length2 && fq1 >= length2 ); + + if ( outsideA == false && outsideB == false) + { + // attempt to clip + // this may yield contact points with excessive separation + // in that case the algorithm falls back to single point collision + + // find reference edge using SAT + b2Vec2 normalA; + float separationA; + + { + normalA = b2LeftPerp( u1 ); + float ss1 = b2Dot( b2Sub( p2, p1 ), normalA ); + float ss2 = b2Dot( b2Sub( q2, p1 ), normalA ); + float s1p = ss1 < ss2 ? ss1 : ss2; + float s1n = -ss1 < -ss2 ? -ss1 : -ss2; + + if ( s1p > s1n ) + { + separationA = s1p; + } + else + { + separationA = s1n; + normalA = b2Neg( normalA ); + } + } + + b2Vec2 normalB; + float separationB; + { + normalB = b2LeftPerp( u2 ); + float ss1 = b2Dot( b2Sub( p1, p2 ), normalB ); + float ss2 = b2Dot( b2Sub( q1, p2 ), normalB ); + float s1p = ss1 < ss2 ? ss1 : ss2; + float s1n = -ss1 < -ss2 ? -ss1 : -ss2; + + if ( s1p > s1n ) + { + separationB = s1p; + } + else + { + separationB = s1n; + normalB = b2Neg( normalB ); + } + } + + if ( separationA >= separationB ) + { + manifold.normal = normalA; + + b2Vec2 cp = p2; + b2Vec2 cq = q2; + + // clip to p1 + if ( fp2 < 0.0f && fq2 > 0.0f ) + { + cp = b2Lerp( p2, q2, ( 0.0f - fp2 ) / ( fq2 - fp2 ) ); + } + else if ( fq2 < 0.0f && fp2 > 0.0f ) + { + cq = b2Lerp( q2, p2, ( 0.0f - fq2 ) / ( fp2 - fq2 ) ); + } + + // clip to q1 + if ( fp2 > length1 && fq2 < length1 ) + { + cp = b2Lerp( p2, q2, ( fp2 - length1 ) / ( fp2 - fq2 ) ); + } + else if ( fq2 > length1 && fp2 < length1 ) + { + cq = b2Lerp( q2, p2, ( fq2 - length1 ) / ( fq2 - fp2 ) ); + } + + float sp = b2Dot( b2Sub( cp, p1 ), normalA ); + float sq = b2Dot( b2Sub( cq, p1 ), normalA ); + + if ( sp <= distance + b2_linearSlop || sq <= distance + b2_linearSlop ) + { + b2ManifoldPoint* mp; + mp = manifold.points + 0; + mp->anchorA = b2MulAdd( cp, 0.5f * ( radiusA - radiusB - sp ), normalA ); + mp->separation = sp - radius; + mp->id = B2_MAKE_ID( 0, 0 ); + + mp = manifold.points + 1; + mp->anchorA = b2MulAdd( cq, 0.5f * ( radiusA - radiusB - sq ), normalA ); + mp->separation = sq - radius; + mp->id = B2_MAKE_ID( 0, 1 ); + manifold.pointCount = 2; + } + } + else + { + // normal always points from A to B + manifold.normal = b2Neg( normalB ); + + b2Vec2 cp = p1; + b2Vec2 cq = q1; + + // clip to p2 + if ( fp1 < 0.0f && fq1 > 0.0f ) + { + cp = b2Lerp( p1, q1, ( 0.0f - fp1 ) / ( fq1 - fp1 ) ); + } + else if ( fq1 < 0.0f && fp1 > 0.0f ) + { + cq = b2Lerp( q1, p1, ( 0.0f - fq1 ) / ( fp1 - fq1 ) ); + } + + // clip to q2 + if ( fp1 > length2 && fq1 < length2 ) + { + cp = b2Lerp( p1, q1, ( fp1 - length2 ) / ( fp1 - fq1 ) ); + } + else if ( fq1 > length2 && fp1 < length2 ) + { + cq = b2Lerp( q1, p1, ( fq1 - length2 ) / ( fq1 - fp1 ) ); + } + + float sp = b2Dot( b2Sub( cp, p2 ), normalB ); + float sq = b2Dot( b2Sub( cq, p2 ), normalB ); + + if ( sp <= distance + b2_linearSlop || sq <= distance + b2_linearSlop ) + { + b2ManifoldPoint* mp; + mp = manifold.points + 0; + mp->anchorA = b2MulAdd( cp, 0.5f * ( radiusB - radiusA - sp ), normalB ); + mp->separation = sp - radius; + mp->id = B2_MAKE_ID( 0, 0 ); + mp = manifold.points + 1; + mp->anchorA = b2MulAdd( cq, 0.5f * ( radiusB - radiusA - sq ), normalB ); + mp->separation = sq - radius; + mp->id = B2_MAKE_ID( 1, 0 ); + manifold.pointCount = 2; + } + } + } + + if (manifold.pointCount == 0) + { + // single point collision + b2Vec2 normal = b2Sub( closest2, closest1 ); + if ( b2Dot( normal, normal ) > epsSqr ) + { + normal = b2Normalize( normal ); + } + else + { + normal = b2LeftPerp( u1 ); + } + + b2Vec2 c1 = b2MulAdd( closest1, radiusA, normal ); + b2Vec2 c2 = b2MulAdd( closest2, -radiusB, normal ); + + int i1 = f1 == 0.0f ? 0 : 1; + int i2 = f2 == 0.0f ? 0 : 1; + + manifold.normal = normal; + manifold.points[0].anchorA = b2Lerp( c1, c2, 0.5f ); + manifold.points[0].separation = sqrtf( distanceSquared ) - radius; + manifold.points[0].id = B2_MAKE_ID( i1, i2 ); + manifold.pointCount = 1; + } + + // Convert manifold to world space + if ( manifold.pointCount > 0 ) + { + manifold.normal = b2RotateVector( xfA.q, manifold.normal ); + for ( int i = 0; i < manifold.pointCount; ++i ) + { + b2ManifoldPoint* mp = manifold.points + i; + + // anchor points relative to shape origin in world space + mp->anchorA = b2RotateVector( xfA.q, b2Add( mp->anchorA, origin ) ); + mp->anchorB = b2Add( mp->anchorA, b2Sub( xfA.p, xfB.p ) ); + mp->point = b2Add( xfA.p, mp->anchorA ); + } + } + + return manifold; +} + +b2Manifold b2CollideSegmentAndCapsule( const b2Segment* segmentA, b2Transform xfA, const b2Capsule* capsuleB, b2Transform xfB ) +{ + b2Capsule capsuleA = { segmentA->point1, segmentA->point2, 0.0f }; + return b2CollideCapsules( &capsuleA, xfA, capsuleB, xfB ); +} + +b2Manifold b2CollidePolygonAndCapsule( const b2Polygon* polygonA, b2Transform xfA, const b2Capsule* capsuleB, b2Transform xfB ) +{ + b2Polygon polyB = b2MakeCapsule( capsuleB->center1, capsuleB->center2, capsuleB->radius ); + return b2CollidePolygons( polygonA, xfA, &polyB, xfB ); +} + +// Polygon clipper used to compute contact points when there are potentially two contact points. +static b2Manifold b2ClipPolygons( const b2Polygon* polyA, const b2Polygon* polyB, int32_t edgeA, int32_t edgeB, bool flip ) +{ + b2Manifold manifold = { 0 }; + + // reference polygon + const b2Polygon* poly1; + int32_t i11, i12; + + // incident polygon + const b2Polygon* poly2; + int32_t i21, i22; + + if ( flip ) + { + poly1 = polyB; + poly2 = polyA; + i11 = edgeB; + i12 = edgeB + 1 < polyB->count ? edgeB + 1 : 0; + i21 = edgeA; + i22 = edgeA + 1 < polyA->count ? edgeA + 1 : 0; + } + else + { + poly1 = polyA; + poly2 = polyB; + i11 = edgeA; + i12 = edgeA + 1 < polyA->count ? edgeA + 1 : 0; + i21 = edgeB; + i22 = edgeB + 1 < polyB->count ? edgeB + 1 : 0; + } + + b2Vec2 normal = poly1->normals[i11]; + + // Reference edge vertices + b2Vec2 v11 = poly1->vertices[i11]; + b2Vec2 v12 = poly1->vertices[i12]; + + // Incident edge vertices + b2Vec2 v21 = poly2->vertices[i21]; + b2Vec2 v22 = poly2->vertices[i22]; + + b2Vec2 tangent = b2CrossSV( 1.0f, normal ); + + float lower1 = 0.0f; + float upper1 = b2Dot( b2Sub( v12, v11 ), tangent ); + + // Incident edge points opposite of tangent due to CCW winding + float upper2 = b2Dot( b2Sub( v21, v11 ), tangent ); + float lower2 = b2Dot( b2Sub( v22, v11 ), tangent ); + + // This check can fail slightly due to mismatch with GJK code. + // Perhaps fall back to a single point here? Otherwise we get two coincident points. + // if (upper2 < lower1 || upper1 < lower2) + //{ + // // numeric failure + // B2_ASSERT(false); + // return manifold; + //} + + b2Vec2 vLower; + if ( lower2 < lower1 && upper2 - lower2 > FLT_EPSILON ) + { + vLower = b2Lerp( v22, v21, ( lower1 - lower2 ) / ( upper2 - lower2 ) ); + } + else + { + vLower = v22; + } + + b2Vec2 vUpper; + if ( upper2 > upper1 && upper2 - lower2 > FLT_EPSILON ) + { + vUpper = b2Lerp( v22, v21, ( upper1 - lower2 ) / ( upper2 - lower2 ) ); + } + else + { + vUpper = v21; + } + + // todo vLower can be very close to vUpper, reduce to one point? + + float separationLower = b2Dot( b2Sub( vLower, v11 ), normal ); + float separationUpper = b2Dot( b2Sub( vUpper, v11 ), normal ); + + float r1 = poly1->radius; + float r2 = poly2->radius; + + // Put contact points at midpoint, accounting for radii + vLower = b2MulAdd( vLower, 0.5f * ( r1 - r2 - separationLower ), normal ); + vUpper = b2MulAdd( vUpper, 0.5f * ( r1 - r2 - separationUpper ), normal ); + + float radius = r1 + r2; + + if ( flip == false ) + { + manifold.normal = normal; + b2ManifoldPoint* cp = manifold.points + 0; + + { + cp->anchorA = vLower; + cp->separation = separationLower - radius; + cp->id = B2_MAKE_ID( i11, i22 ); + manifold.pointCount += 1; + cp += 1; + } + + { + cp->anchorA = vUpper; + cp->separation = separationUpper - radius; + cp->id = B2_MAKE_ID( i12, i21 ); + manifold.pointCount += 1; + } + } + else + { + manifold.normal = b2Neg( normal ); + b2ManifoldPoint* cp = manifold.points + 0; + + { + cp->anchorA = vUpper; + cp->separation = separationUpper - radius; + cp->id = B2_MAKE_ID( i21, i12 ); + manifold.pointCount += 1; + cp += 1; + } + + { + cp->anchorA = vLower; + cp->separation = separationLower - radius; + cp->id = B2_MAKE_ID( i22, i11 ); + manifold.pointCount += 1; + } + } + + return manifold; +} + +// Find the max separation between poly1 and poly2 using edge normals from poly1. +static float b2FindMaxSeparation( int32_t* edgeIndex, const b2Polygon* poly1, const b2Polygon* poly2 ) +{ + int32_t count1 = poly1->count; + int32_t count2 = poly2->count; + const b2Vec2* n1s = poly1->normals; + const b2Vec2* v1s = poly1->vertices; + const b2Vec2* v2s = poly2->vertices; + + int32_t bestIndex = 0; + float maxSeparation = -FLT_MAX; + for ( int32_t i = 0; i < count1; ++i ) + { + // Get poly1 normal in frame2. + b2Vec2 n = n1s[i]; + b2Vec2 v1 = v1s[i]; + + // Find the deepest point for normal i. + float si = FLT_MAX; + for ( int32_t j = 0; j < count2; ++j ) + { + float sij = b2Dot( n, b2Sub( v2s[j], v1 ) ); + if ( sij < si ) + { + si = sij; + } + } + + if ( si > maxSeparation ) + { + maxSeparation = si; + bestIndex = i; + } + } + + *edgeIndex = bestIndex; + return maxSeparation; +} + +// Due to speculation, every polygon is rounded +// Algorithm: +// +// compute edge separation using the separating axis test (SAT) +// if (separation > speculation_distance) +// return +// find reference and incident edge +// if separation >= 0.1f * b2_linearSlop +// compute closest points between reference and incident edge +// if vertices are closest +// single vertex-vertex contact +// else +// clip edges +// end +// else +// clip edges +// end + +b2Manifold b2CollidePolygons( const b2Polygon* polygonA, b2Transform xfA, const b2Polygon* polygonB, b2Transform xfB ) +{ + b2Vec2 origin = polygonA->vertices[0]; + + // Shift polyA to origin + // pw = q * pb + p + // pw = q * (pbs + origin) + p + // pw = q * pbs + (p + q * origin) + b2Transform sfA = { b2Add( xfA.p, b2RotateVector( xfA.q, origin ) ), xfA.q }; + b2Transform xf = b2InvMulTransforms( sfA, xfB ); + + b2Polygon localPolyA; + localPolyA.count = polygonA->count; + localPolyA.radius = polygonA->radius; + localPolyA.vertices[0] = b2Vec2_zero; + localPolyA.normals[0] = polygonA->normals[0]; + for ( int i = 1; i < localPolyA.count; ++i ) + { + localPolyA.vertices[i] = b2Sub( polygonA->vertices[i], origin ); + localPolyA.normals[i] = polygonA->normals[i]; + } + + // Put polyB in polyA's frame to reduce round-off error + b2Polygon localPolyB; + localPolyB.count = polygonB->count; + localPolyB.radius = polygonB->radius; + for ( int i = 0; i < localPolyB.count; ++i ) + { + localPolyB.vertices[i] = b2TransformPoint( xf, polygonB->vertices[i] ); + localPolyB.normals[i] = b2RotateVector( xf.q, polygonB->normals[i] ); + } + + int edgeA = 0; + float separationA = b2FindMaxSeparation( &edgeA, &localPolyA, &localPolyB ); + + int edgeB = 0; + float separationB = b2FindMaxSeparation( &edgeB, &localPolyB, &localPolyA ); + + float radius = localPolyA.radius + localPolyB.radius; + + if ( separationA > b2_speculativeDistance + radius || separationB > b2_speculativeDistance + radius ) + { + return ( b2Manifold ){ 0 }; + } + + // Find incident edge + bool flip; + if ( separationA >= separationB ) + { + flip = false; + + b2Vec2 searchDirection = localPolyA.normals[edgeA]; + + // Find the incident edge on polyB + int count = localPolyB.count; + const b2Vec2* normals = localPolyB.normals; + edgeB = 0; + float minDot = FLT_MAX; + for ( int i = 0; i < count; ++i ) + { + float dot = b2Dot( searchDirection, normals[i] ); + if ( dot < minDot ) + { + minDot = dot; + edgeB = i; + } + } + } + else + { + flip = true; + + b2Vec2 searchDirection = localPolyB.normals[edgeB]; + + // Find the incident edge on polyA + int count = localPolyA.count; + const b2Vec2* normals = localPolyA.normals; + edgeA = 0; + float minDot = FLT_MAX; + for ( int i = 0; i < count; ++i ) + { + float dot = b2Dot( searchDirection, normals[i] ); + if ( dot < minDot ) + { + minDot = dot; + edgeA = i; + } + } + } + + b2Manifold manifold = { 0 }; + + // Using slop here to ensure vertex-vertex normal vectors can be safely normalized + // todo this means edge clipping needs to handle slightly non-overlapping edges. + if ( separationA > 0.1f * b2_linearSlop || separationB > 0.1f * b2_linearSlop ) + { + // Polygons are disjoint. Find closest points between reference edge and incident edge + // Reference edge on polygon A + int i11 = edgeA; + int i12 = edgeA + 1 < localPolyA.count ? edgeA + 1 : 0; + int i21 = edgeB; + int i22 = edgeB + 1 < localPolyB.count ? edgeB + 1 : 0; + + b2Vec2 v11 = localPolyA.vertices[i11]; + b2Vec2 v12 = localPolyA.vertices[i12]; + b2Vec2 v21 = localPolyB.vertices[i21]; + b2Vec2 v22 = localPolyB.vertices[i22]; + + b2SegmentDistanceResult result = b2SegmentDistance( v11, v12, v21, v22 ); + + if ( result.fraction1 == 0.0f && result.fraction2 == 0.0f ) + { + // v11 - v21 + b2Vec2 normal = b2Sub( v21, v11 ); + B2_ASSERT( result.distanceSquared > 0.0f ); + float distance = sqrtf( result.distanceSquared ); + if ( distance > b2_speculativeDistance + radius ) + { + return manifold; + } + float invDistance = 1.0f / distance; + normal.x *= invDistance; + normal.y *= invDistance; + + b2Vec2 c1 = b2MulAdd( v11, localPolyA.radius, normal ); + b2Vec2 c2 = b2MulAdd( v21, -localPolyB.radius, normal ); + + manifold.normal = normal; + manifold.points[0].anchorA = b2Lerp( c1, c2, 0.5f ); + manifold.points[0].separation = distance - radius; + manifold.points[0].id = B2_MAKE_ID( i11, i21 ); + manifold.pointCount = 1; + } + else if ( result.fraction1 == 0.0f && result.fraction2 == 1.0f ) + { + // v11 - v22 + b2Vec2 normal = b2Sub( v22, v11 ); + B2_ASSERT( result.distanceSquared > 0.0f ); + float distance = sqrtf( result.distanceSquared ); + if ( distance > b2_speculativeDistance + radius ) + { + return manifold; + } + float invDistance = 1.0f / distance; + normal.x *= invDistance; + normal.y *= invDistance; + + b2Vec2 c1 = b2MulAdd( v11, localPolyA.radius, normal ); + b2Vec2 c2 = b2MulAdd( v22, -localPolyB.radius, normal ); + + manifold.normal = normal; + manifold.points[0].anchorA = b2Lerp( c1, c2, 0.5f ); + manifold.points[0].separation = distance - radius; + manifold.points[0].id = B2_MAKE_ID( i11, i22 ); + manifold.pointCount = 1; + } + else if ( result.fraction1 == 1.0f && result.fraction2 == 0.0f ) + { + // v12 - v21 + b2Vec2 normal = b2Sub( v21, v12 ); + B2_ASSERT( result.distanceSquared > 0.0f ); + float distance = sqrtf( result.distanceSquared ); + if ( distance > b2_speculativeDistance + radius ) + { + return manifold; + } + float invDistance = 1.0f / distance; + normal.x *= invDistance; + normal.y *= invDistance; + + b2Vec2 c1 = b2MulAdd( v12, localPolyA.radius, normal ); + b2Vec2 c2 = b2MulAdd( v21, -localPolyB.radius, normal ); + + manifold.normal = normal; + manifold.points[0].anchorA = b2Lerp( c1, c2, 0.5f ); + manifold.points[0].separation = distance - radius; + manifold.points[0].id = B2_MAKE_ID( i12, i21 ); + manifold.pointCount = 1; + } + else if ( result.fraction1 == 1.0f && result.fraction2 == 1.0f ) + { + // v12 - v22 + b2Vec2 normal = b2Sub( v22, v12 ); + B2_ASSERT( result.distanceSquared > 0.0f ); + float distance = sqrtf( result.distanceSquared ); + if ( distance > b2_speculativeDistance + radius ) + { + return manifold; + } + float invDistance = 1.0f / distance; + normal.x *= invDistance; + normal.y *= invDistance; + + b2Vec2 c1 = b2MulAdd( v12, localPolyA.radius, normal ); + b2Vec2 c2 = b2MulAdd( v22, -localPolyB.radius, normal ); + + manifold.normal = normal; + manifold.points[0].anchorA = b2Lerp( c1, c2, 0.5f ); + manifold.points[0].separation = distance - radius; + manifold.points[0].id = B2_MAKE_ID( i12, i22 ); + manifold.pointCount = 1; + } + else + { + // Edge region + manifold = b2ClipPolygons( &localPolyA, &localPolyB, edgeA, edgeB, flip ); + } + } + else + { + // Polygons overlap + manifold = b2ClipPolygons( &localPolyA, &localPolyB, edgeA, edgeB, flip ); + } + + // Convert manifold to world space + if ( manifold.pointCount > 0 ) + { + manifold.normal = b2RotateVector( xfA.q, manifold.normal ); + for ( int i = 0; i < manifold.pointCount; ++i ) + { + b2ManifoldPoint* mp = manifold.points + i; + + // anchor points relative to shape origin in world space + mp->anchorA = b2RotateVector( xfA.q, b2Add( mp->anchorA, origin ) ); + mp->anchorB = b2Add( mp->anchorA, b2Sub( xfA.p, xfB.p ) ); + mp->point = b2Add( xfA.p, mp->anchorA ); + } + } + + return manifold; +} + +b2Manifold b2CollideSegmentAndCircle( const b2Segment* segmentA, b2Transform xfA, const b2Circle* circleB, b2Transform xfB ) +{ + b2Capsule capsuleA = { segmentA->point1, segmentA->point2, 0.0f }; + return b2CollideCapsuleAndCircle( &capsuleA, xfA, circleB, xfB ); +} + +b2Manifold b2CollideSegmentAndPolygon( const b2Segment* segmentA, b2Transform xfA, const b2Polygon* polygonB, b2Transform xfB ) +{ + b2Polygon polygonA = b2MakeCapsule( segmentA->point1, segmentA->point2, 0.0f ); + return b2CollidePolygons( &polygonA, xfA, polygonB, xfB ); +} + +b2Manifold b2CollideChainSegmentAndCircle( const b2ChainSegment* segmentA, b2Transform xfA, const b2Circle* circleB, + b2Transform xfB ) +{ + b2Manifold manifold = { 0 }; + + b2Transform xf = b2InvMulTransforms( xfA, xfB ); + + // Compute circle in frame of segment + b2Vec2 pB = b2TransformPoint( xf, circleB->center ); + + b2Vec2 p1 = segmentA->segment.point1; + b2Vec2 p2 = segmentA->segment.point2; + b2Vec2 e = b2Sub( p2, p1 ); + + // Normal points to the right + float offset = b2Dot( b2RightPerp( e ), b2Sub( pB, p1 ) ); + if ( offset < 0.0f ) + { + // collision is one-sided + return manifold; + } + + // Barycentric coordinates + float u = b2Dot( e, b2Sub( p2, pB ) ); + float v = b2Dot( e, b2Sub( pB, p1 ) ); + + b2Vec2 pA; + + if ( v <= 0.0f ) + { + // Behind point1? + // Is pB in the Voronoi region of the previous edge? + b2Vec2 prevEdge = b2Sub( p1, segmentA->ghost1 ); + float uPrev = b2Dot( prevEdge, b2Sub( pB, p1 ) ); + if ( uPrev <= 0.0f ) + { + return manifold; + } + + pA = p1; + } + else if ( u <= 0.0f ) + { + // Ahead of point2? + b2Vec2 nextEdge = b2Sub( segmentA->ghost2, p2 ); + float vNext = b2Dot( nextEdge, b2Sub( pB, p2 ) ); + + // Is pB in the Voronoi region of the next edge? + if ( vNext > 0.0f ) + { + return manifold; + } + + pA = p2; + } + else + { + float ee = b2Dot( e, e ); + pA = ( b2Vec2 ){ u * p1.x + v * p2.x, u * p1.y + v * p2.y }; + pA = ee > 0.0f ? b2MulSV( 1.0f / ee, pA ) : p1; + } + + float distance; + b2Vec2 normal = b2GetLengthAndNormalize( &distance, b2Sub( pB, pA ) ); + + float radius = circleB->radius; + float separation = distance - radius; + if ( separation > b2_speculativeDistance ) + { + return manifold; + } + + b2Vec2 cA = pA; + b2Vec2 cB = b2MulAdd( pB, -radius, normal ); + b2Vec2 contactPointA = b2Lerp( cA, cB, 0.5f ); + + manifold.normal = b2RotateVector( xfA.q, normal ); + + b2ManifoldPoint* mp = manifold.points + 0; + mp->anchorA = b2RotateVector( xfA.q, contactPointA ); + mp->anchorB = b2Add( mp->anchorA, b2Sub( xfA.p, xfB.p ) ); + mp->point = b2Add( xfA.p, mp->anchorA ); + mp->separation = separation; + mp->id = 0; + manifold.pointCount = 1; + return manifold; +} + +b2Manifold b2CollideChainSegmentAndCapsule( const b2ChainSegment* segmentA, b2Transform xfA, const b2Capsule* capsuleB, + b2Transform xfB, b2DistanceCache* cache ) +{ + b2Polygon polyB = b2MakeCapsule( capsuleB->center1, capsuleB->center2, capsuleB->radius ); + return b2CollideChainSegmentAndPolygon( segmentA, xfA, &polyB, xfB, cache ); +} + +static b2Manifold b2ClipSegments( b2Vec2 a1, b2Vec2 a2, b2Vec2 b1, b2Vec2 b2, b2Vec2 normal, float ra, float rb, uint16_t id1, + uint16_t id2 ) +{ + b2Manifold manifold = { 0 }; + + b2Vec2 tangent = b2LeftPerp( normal ); + + // Barycentric coordinates of each point relative to a1 along tangent + float lower1 = 0.0f; + float upper1 = b2Dot( b2Sub( a2, a1 ), tangent ); + + // Incident edge points opposite of tangent due to CCW winding + float upper2 = b2Dot( b2Sub( b1, a1 ), tangent ); + float lower2 = b2Dot( b2Sub( b2, a1 ), tangent ); + + // Do segments overlap? + if ( upper2 < lower1 || upper1 < lower2 ) + { + return manifold; + } + + b2Vec2 vLower; + if ( lower2 < lower1 && upper2 - lower2 > FLT_EPSILON ) + { + vLower = b2Lerp( b2, b1, ( lower1 - lower2 ) / ( upper2 - lower2 ) ); + } + else + { + vLower = b2; + } + + b2Vec2 vUpper; + if ( upper2 > upper1 && upper2 - lower2 > FLT_EPSILON ) + { + vUpper = b2Lerp( b2, b1, ( upper1 - lower2 ) / ( upper2 - lower2 ) ); + } + else + { + vUpper = b1; + } + + // todo vLower can be very close to vUpper, reduce to one point? + + float separationLower = b2Dot( b2Sub( vLower, a1 ), normal ); + float separationUpper = b2Dot( b2Sub( vUpper, a1 ), normal ); + + // Put contact points at midpoint, accounting for radii + vLower = b2MulAdd( vLower, 0.5f * ( ra - rb - separationLower ), normal ); + vUpper = b2MulAdd( vUpper, 0.5f * ( ra - rb - separationUpper ), normal ); + + float radius = ra + rb; + + manifold.normal = normal; + { + b2ManifoldPoint* cp = manifold.points + 0; + cp->anchorA = vLower; + cp->separation = separationLower - radius; + cp->id = id1; + } + + { + b2ManifoldPoint* cp = manifold.points + 1; + cp->anchorA = vUpper; + cp->separation = separationUpper - radius; + cp->id = id2; + } + + manifold.pointCount = 2; + + return manifold; +} + +enum b2NormalType +{ + // This means the normal points in a direction that is non-smooth relative to a convex vertex and should be skipped + b2_normalSkip, + + // This means the normal points in a direction that is smooth relative to a convex vertex and should be used for collision + b2_normalAdmit, + + // This means the normal is in a region of a concave vertex and should be snapped to the segment normal + b2_normalSnap +}; + +struct b2ChainSegmentParams +{ + b2Vec2 edge1; + b2Vec2 normal0; + b2Vec2 normal2; + bool convex1; + bool convex2; +}; + +// Evaluate Gauss map +// See https://box2d.org/posts/2020/06/ghost-collisions/ +static enum b2NormalType b2ClassifyNormal( struct b2ChainSegmentParams params, b2Vec2 normal ) +{ + const float sinTol = 0.01f; + + if ( b2Dot( normal, params.edge1 ) <= 0.0f ) + { + // Normal points towards the segment tail + if ( params.convex1 ) + { + if ( b2Cross( normal, params.normal0 ) > sinTol ) + { + return b2_normalSkip; + } + + return b2_normalAdmit; + } + else + { + return b2_normalSnap; + } + } + else + { + // Normal points towards segment head + if ( params.convex2 ) + { + if ( b2Cross( params.normal2, normal ) > sinTol ) + { + return b2_normalSkip; + } + + return b2_normalAdmit; + } + else + { + return b2_normalSnap; + } + } +} + +b2Manifold b2CollideChainSegmentAndPolygon( const b2ChainSegment* segmentA, b2Transform xfA, const b2Polygon* polygonB, + b2Transform xfB, b2DistanceCache* cache ) +{ + b2Manifold manifold = { 0 }; + + b2Transform xf = b2InvMulTransforms( xfA, xfB ); + + b2Vec2 centroidB = b2TransformPoint( xf, polygonB->centroid ); + float radiusB = polygonB->radius; + + b2Vec2 p1 = segmentA->segment.point1; + b2Vec2 p2 = segmentA->segment.point2; + + b2Vec2 edge1 = b2Normalize( b2Sub( p2, p1 ) ); + + struct b2ChainSegmentParams smoothParams = { 0 }; + smoothParams.edge1 = edge1; + + const float convexTol = 0.01f; + b2Vec2 edge0 = b2Normalize( b2Sub( p1, segmentA->ghost1 ) ); + smoothParams.normal0 = b2RightPerp( edge0 ); + smoothParams.convex1 = b2Cross( edge0, edge1 ) >= convexTol; + + b2Vec2 edge2 = b2Normalize( b2Sub( segmentA->ghost2, p2 ) ); + smoothParams.normal2 = b2RightPerp( edge2 ); + smoothParams.convex2 = b2Cross( edge1, edge2 ) >= convexTol; + + // Normal points to the right + b2Vec2 normal1 = b2RightPerp( edge1 ); + bool behind1 = b2Dot( normal1, b2Sub( centroidB, p1 ) ) < 0.0f; + bool behind0 = true; + bool behind2 = true; + if ( smoothParams.convex1 ) + { + behind0 = b2Dot( smoothParams.normal0, b2Sub( centroidB, p1 ) ) < 0.0f; + } + + if ( smoothParams.convex2 ) + { + behind2 = b2Dot( smoothParams.normal2, b2Sub( centroidB, p2 ) ) < 0.0f; + } + + if ( behind1 && behind0 && behind2 ) + { + // one-sided collision + return manifold; + } + + // Get polygonB in frameA + int32_t count = polygonB->count; + b2Vec2 vertices[b2_maxPolygonVertices]; + b2Vec2 normals[b2_maxPolygonVertices]; + for ( int32_t i = 0; i < count; ++i ) + { + vertices[i] = b2TransformPoint( xf, polygonB->vertices[i] ); + normals[i] = b2RotateVector( xf.q, polygonB->normals[i] ); + } + + // Distance doesn't work correctly with partial polygons + b2DistanceInput input; + input.proxyA = b2MakeProxy( &segmentA->segment.point1, 2, 0.0f ); + input.proxyB = b2MakeProxy( vertices, count, 0.0f ); + input.transformA = b2Transform_identity; + input.transformB = b2Transform_identity; + input.useRadii = false; + + b2DistanceOutput output = b2ShapeDistance( cache, &input, NULL, 0 ); + + if ( output.distance > radiusB + b2_speculativeDistance ) + { + return manifold; + } + + // Snap concave normals for partial polygon + b2Vec2 n0 = smoothParams.convex1 ? smoothParams.normal0 : normal1; + b2Vec2 n2 = smoothParams.convex2 ? smoothParams.normal2 : normal1; + + // Index of incident vertex on polygon + int32_t incidentIndex = -1; + int32_t incidentNormal = -1; + + if ( behind1 == false && output.distance > 0.1f * b2_linearSlop ) + { + // The closest features may be two vertices or an edge and a vertex even when there should + // be face contact + + if ( cache->count == 1 ) + { + // vertex-vertex collision + b2Vec2 pA = output.pointA; + b2Vec2 pB = output.pointB; + + b2Vec2 normal = b2Normalize( b2Sub( pB, pA ) ); + + enum b2NormalType type = b2ClassifyNormal( smoothParams, normal ); + if ( type == b2_normalSkip ) + { + return manifold; + } + + if ( type == b2_normalAdmit ) + { + manifold.normal = b2RotateVector( xfA.q, normal ); + b2ManifoldPoint* cp = manifold.points + 0; + cp->anchorA = b2RotateVector( xfA.q, pA ); + cp->anchorB = b2Add( cp->anchorA, b2Sub( xfA.p, xfB.p ) ); + cp->point = b2Add( xfA.p, cp->anchorA ); + cp->separation = output.distance - radiusB; + cp->id = B2_MAKE_ID( cache->indexA[0], cache->indexB[0] ); + manifold.pointCount = 1; + return manifold; + } + + // fall through b2_normalSnap + incidentIndex = cache->indexB[0]; + } + else + { + // vertex-edge collision + B2_ASSERT( cache->count == 2 ); + + int32_t ia1 = cache->indexA[0]; + int32_t ia2 = cache->indexA[1]; + int32_t ib1 = cache->indexB[0]; + int32_t ib2 = cache->indexB[1]; + + if ( ia1 == ia2 ) + { + // 1 point on A, expect 2 points on B + B2_ASSERT( ib1 != ib2 ); + + // Find polygon normal most aligned with vector between closest points. + // This effectively sorts ib1 and ib2 + b2Vec2 normalB = b2Sub( output.pointA, output.pointB ); + float dot1 = b2Dot( normalB, normals[ib1] ); + float dot2 = b2Dot( normalB, normals[ib2] ); + int32_t ib = dot1 > dot2 ? ib1 : ib2; + + // Use accurate normal + normalB = normals[ib]; + + enum b2NormalType type = b2ClassifyNormal( smoothParams, b2Neg( normalB ) ); + if ( type == b2_normalSkip ) + { + return manifold; + } + + if ( type == b2_normalAdmit ) + { + // Get polygon edge associated with normal + ib1 = ib; + ib2 = ib < count - 1 ? ib + 1 : 0; + + b2Vec2 b1 = vertices[ib1]; + b2Vec2 b2 = vertices[ib2]; + + // Find incident segment vertex + dot1 = b2Dot( normalB, b2Sub( p1, b1 ) ); + dot2 = b2Dot( normalB, b2Sub( p2, b1 ) ); + + if ( dot1 < dot2 ) + { + if ( b2Dot( n0, normalB ) < b2Dot( normal1, normalB ) ) + { + // Neighbor is incident + return manifold; + } + } + else + { + if ( b2Dot( n2, normalB ) < b2Dot( normal1, normalB ) ) + { + // Neighbor is incident + return manifold; + } + } + + manifold = + b2ClipSegments( b1, b2, p1, p2, normalB, radiusB, 0.0f, B2_MAKE_ID( ib1, 1 ), B2_MAKE_ID( ib2, 0 ) ); + manifold.normal = b2RotateVector( xfA.q, b2Neg( normalB ) ); + manifold.points[0].anchorA = b2RotateVector( xfA.q, manifold.points[0].anchorA ); + manifold.points[1].anchorA = b2RotateVector( xfA.q, manifold.points[1].anchorA ); + b2Vec2 pAB = b2Sub( xfA.p, xfB.p ); + manifold.points[0].anchorB = b2Add( manifold.points[0].anchorA, pAB ); + manifold.points[1].anchorB = b2Add( manifold.points[1].anchorA, pAB ); + manifold.points[0].point = b2Add( xfA.p, manifold.points[0].anchorA ); + manifold.points[1].point = b2Add( xfA.p, manifold.points[1].anchorA ); + return manifold; + } + + // fall through b2_normalSnap + incidentNormal = ib; + } + else + { + // Get index of incident polygonB vertex + float dot1 = b2Dot( normal1, b2Sub( vertices[ib1], p1 ) ); + float dot2 = b2Dot( normal1, b2Sub( vertices[ib2], p2 ) ); + incidentIndex = dot1 < dot2 ? ib1 : ib2; + } + } + } + else + { + // SAT edge normal + float edgeSeparation = FLT_MAX; + + for ( int32_t i = 0; i < count; ++i ) + { + float s = b2Dot( normal1, b2Sub( vertices[i], p1 ) ); + if ( s < edgeSeparation ) + { + edgeSeparation = s; + incidentIndex = i; + } + } + + // Check convex neighbor for edge separation + if ( smoothParams.convex1 ) + { + float s0 = FLT_MAX; + + for ( int32_t i = 0; i < count; ++i ) + { + float s = b2Dot( smoothParams.normal0, b2Sub( vertices[i], p1 ) ); + if ( s < s0 ) + { + s0 = s; + } + } + + if ( s0 > edgeSeparation ) + { + edgeSeparation = s0; + + // Indicate neighbor owns edge separation + incidentIndex = -1; + } + } + + // Check convex neighbor for edge separation + if ( smoothParams.convex2 ) + { + float s2 = FLT_MAX; + + for ( int32_t i = 0; i < count; ++i ) + { + float s = b2Dot( smoothParams.normal2, b2Sub( vertices[i], p2 ) ); + if ( s < s2 ) + { + s2 = s; + } + } + + if ( s2 > edgeSeparation ) + { + edgeSeparation = s2; + + // Indicate neighbor owns edge separation + incidentIndex = -1; + } + } + + // SAT polygon normals + float polygonSeparation = -FLT_MAX; + int32_t referenceIndex = -1; + + for ( int32_t i = 0; i < count; ++i ) + { + b2Vec2 n = normals[i]; + + enum b2NormalType type = b2ClassifyNormal( smoothParams, b2Neg( n ) ); + if ( type != b2_normalAdmit ) + { + continue; + } + + // Check the infinite sides of the partial polygon + // if ((smoothParams.convex1 && b2Cross(n0, n) > 0.0f) || (smoothParams.convex2 && b2Cross(n, n2) > 0.0f)) + //{ + // continue; + //} + + b2Vec2 p = vertices[i]; + float s = b2MinFloat( b2Dot( n, b2Sub( p2, p ) ), b2Dot( n, b2Sub( p1, p ) ) ); + + if ( s > polygonSeparation ) + { + polygonSeparation = s; + referenceIndex = i; + } + } + + if ( polygonSeparation > edgeSeparation ) + { + int32_t ia1 = referenceIndex; + int32_t ia2 = ia1 < count - 1 ? ia1 + 1 : 0; + b2Vec2 a1 = vertices[ia1]; + b2Vec2 a2 = vertices[ia2]; + + b2Vec2 n = normals[ia1]; + + float dot1 = b2Dot( n, b2Sub( p1, a1 ) ); + float dot2 = b2Dot( n, b2Sub( p2, a1 ) ); + + if ( dot1 < dot2 ) + { + if ( b2Dot( n0, n ) < b2Dot( normal1, n ) ) + { + // Neighbor is incident + return manifold; + } + } + else + { + if ( b2Dot( n2, n ) < b2Dot( normal1, n ) ) + { + // Neighbor is incident + return manifold; + } + } + + manifold = b2ClipSegments( a1, a2, p1, p2, normals[ia1], radiusB, 0.0f, B2_MAKE_ID( ia1, 1 ), B2_MAKE_ID( ia2, 0 ) ); + manifold.normal = b2RotateVector( xfA.q, b2Neg( normals[ia1] ) ); + manifold.points[0].anchorA = b2RotateVector( xfA.q, manifold.points[0].anchorA ); + manifold.points[1].anchorA = b2RotateVector( xfA.q, manifold.points[1].anchorA ); + b2Vec2 pAB = b2Sub( xfA.p, xfB.p ); + manifold.points[0].anchorB = b2Add( manifold.points[0].anchorA, pAB ); + manifold.points[1].anchorB = b2Add( manifold.points[1].anchorA, pAB ); + manifold.points[0].point = b2Add( xfA.p, manifold.points[0].anchorA ); + manifold.points[1].point = b2Add( xfA.p, manifold.points[1].anchorA ); + return manifold; + } + + if ( incidentIndex == -1 ) + { + // neighboring segment is the separating axis + return manifold; + } + + // fall through segment normal axis + } + + B2_ASSERT( incidentNormal != -1 || incidentIndex != -1 ); + + // Segment normal + + // Find incident polygon normal: normal adjacent to deepest vertex that is most anti-parallel to segment normal + b2Vec2 b1, b2; + int32_t ib1, ib2; + + if ( incidentNormal != -1 ) + { + ib1 = incidentNormal; + ib2 = ib1 < count - 1 ? ib1 + 1 : 0; + b1 = vertices[ib1]; + b2 = vertices[ib2]; + } + else + { + int32_t i2 = incidentIndex; + int32_t i1 = i2 > 0 ? i2 - 1 : count - 1; + float d1 = b2Dot( normal1, normals[i1] ); + float d2 = b2Dot( normal1, normals[i2] ); + if ( d1 < d2 ) + { + ib1 = i1, ib2 = i2; + b1 = vertices[ib1]; + b2 = vertices[ib2]; + } + else + { + ib1 = i2, ib2 = i2 < count - 1 ? i2 + 1 : 0; + b1 = vertices[ib1]; + b2 = vertices[ib2]; + } + } + + manifold = b2ClipSegments( p1, p2, b1, b2, normal1, 0.0f, radiusB, B2_MAKE_ID( 0, ib2 ), B2_MAKE_ID( 1, ib1 ) ); + manifold.normal = b2RotateVector( xfA.q, manifold.normal ); + manifold.points[0].anchorA = b2RotateVector( xfA.q, manifold.points[0].anchorA ); + manifold.points[1].anchorA = b2RotateVector( xfA.q, manifold.points[1].anchorA ); + b2Vec2 pAB = b2Sub( xfA.p, xfB.p ); + manifold.points[0].anchorB = b2Add( manifold.points[0].anchorA, pAB ); + manifold.points[1].anchorB = b2Add( manifold.points[1].anchorA, pAB ); + manifold.points[0].point = b2Add( xfA.p, manifold.points[0].anchorA ); + manifold.points[1].point = b2Add( xfA.p, manifold.points[1].anchorA ); + + return manifold; +} diff --git a/3rdparty/box2d/src/math_functions.c b/3rdparty/box2d/src/math_functions.c new file mode 100644 index 000000000000..039ace249ba0 --- /dev/null +++ b/3rdparty/box2d/src/math_functions.c @@ -0,0 +1,147 @@ +// SPDX-FileCopyrightText: 2023 Erin Catto +// SPDX-License-Identifier: MIT + +#include "box2d/math_functions.h" + +#include "core.h" + +#include + +bool b2IsValid( float a ) +{ + if ( isnan( a ) ) + { + return false; + } + + if ( isinf( a ) ) + { + return false; + } + + return true; +} + +bool b2Vec2_IsValid( b2Vec2 v ) +{ + if ( isnan( v.x ) || isnan( v.y ) ) + { + return false; + } + + if ( isinf( v.x ) || isinf( v.y ) ) + { + return false; + } + + return true; +} + +bool b2Rot_IsValid( b2Rot q ) +{ + if ( isnan( q.s ) || isnan( q.c ) ) + { + return false; + } + + if ( isinf( q.s ) || isinf( q.c ) ) + { + return false; + } + + return b2IsNormalized( q ); +} + +// https://mazzo.li/posts/vectorized-atan2.html +static inline float b2Atan( float x ) +{ + float a1 = 0.99997726f; + float a3 = -0.33262347f; + float a5 = 0.19354346f; + float a7 = -0.11643287f; + float a9 = 0.05265332f; + float a11 = -0.01172120f; + + float x2 = x * x; + return x * ( a1 + x2 * ( a3 + x2 * ( a5 + x2 * ( a7 + x2 * ( a9 + x2 * a11 ) ) ) ) ); +} + +// I tested atan2f and got different results on Apple Clang (Arm) than MSVC (x64). +float b2Atan2( float y, float x ) +{ + float pi = b2_pi; + float halfPi = 0.5f * b2_pi; + + bool swap = b2AbsFloat( x ) < b2AbsFloat( y ); + float atanInput = ( swap ? x : y ) / ( swap ? y : x ); + + // Approximate atan + float res = b2Atan( atanInput ); + + // If swapped, adjust atan output + res = swap ? ( atanInput >= 0.0f ? halfPi : -halfPi ) - res : res; + // Adjust quadrants + if ( x >= 0.0f && y >= 0.0f ) + { + } // 1st quadrant + else if ( x < 0.0f && y >= 0.0f ) + { + res = pi + res; + } // 2nd quadrant + else if ( x < 0.0f && y < 0.0f ) + { + res = -pi + res; + } // 3rd quadrant + else if ( x >= 0.0f && y < 0.0f ) + { + } // 4th quadrant + + return res; +} + +// Approximate cosine and sine for determinism. In my testing cosf and sinf produced +// the same results on x64 and ARM using MSVC, GCC, and Clang. However, I don't trust +// this result. +// https://en.wikipedia.org/wiki/Bh%C4%81skara_I%27s_sine_approximation_formula +b2CosSin b2ComputeCosSin( float angle ) +{ + // return ( b2CosSin ){ cosf( angle ), sinf( angle ) }; + + float x = b2UnwindLargeAngle( angle ); + float pi2 = b2_pi * b2_pi; + + b2Rot q; + + // cosine needs angle in [-pi/2, pi/2] + if (x < -0.5f * b2_pi) + { + float y = x + b2_pi; + float y2 = y * y; + q.c = -( pi2 - 4.0f * y2 ) / ( pi2 + y2 ); + } + else if (x > 0.5f * b2_pi) + { + float y = x - b2_pi; + float y2 = y * y; + q.c = -( pi2 - 4.0f * y2 ) / ( pi2 + y2 ); + } + else + { + float y2 = x * x; + q.c = ( pi2 - 4.0f * y2 ) / ( pi2 + y2 ); + } + + // sine needs angle in [0, pi] + if (x < 0.0f) + { + float y = x + b2_pi; + q.s = -16.0f * y * ( b2_pi - y ) / ( 5.0f * pi2 - 4.0f * y * ( b2_pi - y ) ); + } + else + { + q.s = 16.0f * x * ( b2_pi - x ) / ( 5.0f * pi2 - 4.0f * x * ( b2_pi - x ) ); + } + + q = b2NormalizeRot( q ); + return ( b2CosSin ){ q.c, q.s }; +} diff --git a/3rdparty/box2d/src/motor_joint.c b/3rdparty/box2d/src/motor_joint.c new file mode 100644 index 000000000000..b41f685a80a8 --- /dev/null +++ b/3rdparty/box2d/src/motor_joint.c @@ -0,0 +1,282 @@ +// SPDX-FileCopyrightText: 2023 Erin Catto +// SPDX-License-Identifier: MIT + +#include "body.h" +#include "core.h" +#include "joint.h" +#include "solver.h" +#include "solver_set.h" +#include "world.h" + +// needed for dll export +#include "box2d/box2d.h" + +void b2MotorJoint_SetLinearOffset( b2JointId jointId, b2Vec2 linearOffset ) +{ + b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_motorJoint ); + joint->motorJoint.linearOffset = linearOffset; +} + +b2Vec2 b2MotorJoint_GetLinearOffset( b2JointId jointId ) +{ + b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_motorJoint ); + return joint->motorJoint.linearOffset; +} + +void b2MotorJoint_SetAngularOffset( b2JointId jointId, float angularOffset ) +{ + b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_motorJoint ); + joint->motorJoint.angularOffset = b2ClampFloat( angularOffset, -b2_pi, b2_pi ); +} + +float b2MotorJoint_GetAngularOffset( b2JointId jointId ) +{ + b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_motorJoint ); + return joint->motorJoint.angularOffset; +} + +void b2MotorJoint_SetMaxForce( b2JointId jointId, float maxForce ) +{ + b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_motorJoint ); + joint->motorJoint.maxForce = b2MaxFloat( 0.0f, maxForce ); +} + +float b2MotorJoint_GetMaxForce( b2JointId jointId ) +{ + b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_motorJoint ); + return joint->motorJoint.maxForce; +} + +void b2MotorJoint_SetMaxTorque( b2JointId jointId, float maxTorque ) +{ + b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_motorJoint ); + joint->motorJoint.maxTorque = b2MaxFloat( 0.0f, maxTorque ); +} + +float b2MotorJoint_GetMaxTorque( b2JointId jointId ) +{ + b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_motorJoint ); + return joint->motorJoint.maxTorque; +} + +void b2MotorJoint_SetCorrectionFactor( b2JointId jointId, float correctionFactor ) +{ + b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_motorJoint ); + joint->motorJoint.correctionFactor = b2ClampFloat( correctionFactor, 0.0f, 1.0f ); +} + +float b2MotorJoint_GetCorrectionFactor( b2JointId jointId ) +{ + b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_motorJoint ); + return joint->motorJoint.correctionFactor; +} + +b2Vec2 b2GetMotorJointForce( b2World* world, b2JointSim* base ) +{ + b2Vec2 force = b2MulSV( world->inv_h, base->motorJoint.linearImpulse ); + return force; +} + +float b2GetMotorJointTorque( b2World* world, b2JointSim* base ) +{ + return world->inv_h * base->motorJoint.angularImpulse; +} + +// Point-to-point constraint +// C = p2 - p1 +// Cdot = v2 - v1 +// = v2 + cross(w2, r2) - v1 - cross(w1, r1) +// J = [-I -r1_skew I r2_skew ] +// Identity used: +// w k % (rx i + ry j) = w * (-ry i + rx j) + +// Angle constraint +// C = angle2 - angle1 - referenceAngle +// Cdot = w2 - w1 +// J = [0 0 -1 0 0 1] +// K = invI1 + invI2 + +void b2PrepareMotorJoint( b2JointSim* base, b2StepContext* context ) +{ + B2_ASSERT( base->type == b2_motorJoint ); + + // chase body id to the solver set where the body lives + int idA = base->bodyIdA; + int idB = base->bodyIdB; + + b2World* world = context->world; + + b2Body* bodyA = b2BodyArray_Get( &world->bodies, idA ); + b2Body* bodyB = b2BodyArray_Get( &world->bodies, idB ); + + B2_ASSERT( bodyA->setIndex == b2_awakeSet || bodyB->setIndex == b2_awakeSet ); + + b2SolverSet* setA = b2SolverSetArray_Get( &world->solverSets, bodyA->setIndex ); + b2SolverSet* setB = b2SolverSetArray_Get( &world->solverSets, bodyB->setIndex ); + + int localIndexA = bodyA->localIndex; + int localIndexB = bodyB->localIndex; + + b2BodySim* bodySimA = b2BodySimArray_Get( &setA->bodySims, localIndexA ); + b2BodySim* bodySimB = b2BodySimArray_Get( &setB->bodySims, localIndexB ); + + float mA = bodySimA->invMass; + float iA = bodySimA->invInertia; + float mB = bodySimB->invMass; + float iB = bodySimB->invInertia; + + base->invMassA = mA; + base->invMassB = mB; + base->invIA = iA; + base->invIB = iB; + + b2MotorJoint* joint = &base->motorJoint; + joint->indexA = bodyA->setIndex == b2_awakeSet ? localIndexA : B2_NULL_INDEX; + joint->indexB = bodyB->setIndex == b2_awakeSet ? localIndexB : B2_NULL_INDEX; + + joint->anchorA = b2RotateVector( bodySimA->transform.q, b2Sub( base->localOriginAnchorA, bodySimA->localCenter ) ); + joint->anchorB = b2RotateVector( bodySimB->transform.q, b2Sub( base->localOriginAnchorB, bodySimB->localCenter ) ); + joint->deltaCenter = b2Sub( b2Sub( bodySimB->center, bodySimA->center ), joint->linearOffset ); + joint->deltaAngle = b2RelativeAngle( bodySimB->transform.q, bodySimA->transform.q ) - joint->angularOffset; + joint->deltaAngle = b2UnwindAngle( joint->deltaAngle ); + + b2Vec2 rA = joint->anchorA; + b2Vec2 rB = joint->anchorB; + + b2Mat22 K; + K.cx.x = mA + mB + rA.y * rA.y * iA + rB.y * rB.y * iB; + K.cx.y = -rA.y * rA.x * iA - rB.y * rB.x * iB; + K.cy.x = K.cx.y; + K.cy.y = mA + mB + rA.x * rA.x * iA + rB.x * rB.x * iB; + joint->linearMass = b2GetInverse22( K ); + + float ka = iA + iB; + joint->angularMass = ka > 0.0f ? 1.0f / ka : 0.0f; + + if ( context->enableWarmStarting == false ) + { + joint->linearImpulse = b2Vec2_zero; + joint->angularImpulse = 0.0f; + } +} + +void b2WarmStartMotorJoint( b2JointSim* base, b2StepContext* context ) +{ + float mA = base->invMassA; + float mB = base->invMassB; + float iA = base->invIA; + float iB = base->invIB; + + b2MotorJoint* joint = &base->motorJoint; + + // dummy state for static bodies + b2BodyState dummyState = b2_identityBodyState; + + b2BodyState* bodyA = joint->indexA == B2_NULL_INDEX ? &dummyState : context->states + joint->indexA; + b2BodyState* bodyB = joint->indexB == B2_NULL_INDEX ? &dummyState : context->states + joint->indexB; + + b2Vec2 rA = b2RotateVector( bodyA->deltaRotation, joint->anchorA ); + b2Vec2 rB = b2RotateVector( bodyB->deltaRotation, joint->anchorB ); + + bodyA->linearVelocity = b2MulSub( bodyA->linearVelocity, mA, joint->linearImpulse ); + bodyA->angularVelocity -= iA * ( b2Cross( rA, joint->linearImpulse ) + joint->angularImpulse ); + bodyB->linearVelocity = b2MulAdd( bodyB->linearVelocity, mB, joint->linearImpulse ); + bodyB->angularVelocity += iB * ( b2Cross( rB, joint->linearImpulse ) + joint->angularImpulse ); +} + +void b2SolveMotorJoint( b2JointSim* base, b2StepContext* context, bool useBias ) +{ + B2_ASSERT( base->type == b2_motorJoint ); + + float mA = base->invMassA; + float mB = base->invMassB; + float iA = base->invIA; + float iB = base->invIB; + + // dummy state for static bodies + b2BodyState dummyState = b2_identityBodyState; + + b2MotorJoint* joint = &base->motorJoint; + b2BodyState* bodyA = joint->indexA == B2_NULL_INDEX ? &dummyState : context->states + joint->indexA; + b2BodyState* bodyB = joint->indexB == B2_NULL_INDEX ? &dummyState : context->states + joint->indexB; + + b2Vec2 vA = bodyA->linearVelocity; + float wA = bodyA->angularVelocity; + b2Vec2 vB = bodyB->linearVelocity; + float wB = bodyB->angularVelocity; + + // angular constraint + { + float angularSeperation = b2RelativeAngle( bodyB->deltaRotation, bodyA->deltaRotation ) + joint->deltaAngle; + angularSeperation = b2UnwindAngle( angularSeperation ); + + float angularBias = context->inv_h * joint->correctionFactor * angularSeperation; + + float Cdot = wB - wA; + float impulse = -joint->angularMass * ( Cdot + angularBias ); + + float oldImpulse = joint->angularImpulse; + float maxImpulse = context->h * joint->maxTorque; + joint->angularImpulse = b2ClampFloat( joint->angularImpulse + impulse, -maxImpulse, maxImpulse ); + impulse = joint->angularImpulse - oldImpulse; + + wA -= iA * impulse; + wB += iB * impulse; + } + + // linear constraint + { + b2Vec2 rA = b2RotateVector( bodyA->deltaRotation, joint->anchorA ); + b2Vec2 rB = b2RotateVector( bodyB->deltaRotation, joint->anchorB ); + + b2Vec2 ds = b2Add( b2Sub( bodyB->deltaPosition, bodyA->deltaPosition ), b2Sub( rB, rA ) ); + b2Vec2 linearSeparation = b2Add( joint->deltaCenter, ds ); + b2Vec2 linearBias = b2MulSV( context->inv_h * joint->correctionFactor, linearSeparation ); + + b2Vec2 Cdot = b2Sub( b2Add( vB, b2CrossSV( wB, rB ) ), b2Add( vA, b2CrossSV( wA, rA ) ) ); + b2Vec2 b = b2MulMV( joint->linearMass, b2Add( Cdot, linearBias ) ); + b2Vec2 impulse = { -b.x, -b.y }; + + b2Vec2 oldImpulse = joint->linearImpulse; + float maxImpulse = context->h * joint->maxForce; + joint->linearImpulse = b2Add( joint->linearImpulse, impulse ); + + if ( b2LengthSquared( joint->linearImpulse ) > maxImpulse * maxImpulse ) + { + joint->linearImpulse = b2Normalize( joint->linearImpulse ); + joint->linearImpulse.x *= maxImpulse; + joint->linearImpulse.y *= maxImpulse; + } + + impulse = b2Sub( joint->linearImpulse, oldImpulse ); + + vA = b2MulSub( vA, mA, impulse ); + wA -= iA * b2Cross( rA, impulse ); + vB = b2MulAdd( vB, mB, impulse ); + wB += iB * b2Cross( rB, impulse ); + } + + bodyA->linearVelocity = vA; + bodyA->angularVelocity = wA; + bodyB->linearVelocity = vB; + bodyB->angularVelocity = wB; +} + +#if 0 +void b2DumpMotorJoint() +{ + int32 indexA = m_bodyA->m_islandIndex; + int32 indexB = m_bodyB->m_islandIndex; + + b2Dump(" b2MotorJointDef jd;\n"); + b2Dump(" jd.bodyA = sims[%d];\n", indexA); + b2Dump(" jd.bodyB = sims[%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); +} +#endif diff --git a/3rdparty/box2d/src/mouse_joint.c b/3rdparty/box2d/src/mouse_joint.c new file mode 100644 index 000000000000..4bbf6ef76455 --- /dev/null +++ b/3rdparty/box2d/src/mouse_joint.c @@ -0,0 +1,214 @@ +// SPDX-FileCopyrightText: 2023 Erin Catto +// SPDX-License-Identifier: MIT + +#include "body.h" +#include "core.h" +#include "joint.h" +#include "solver.h" +#include "solver_set.h" +#include "world.h" + +// needed for dll export +#include "box2d/box2d.h" + +void b2MouseJoint_SetTarget( b2JointId jointId, b2Vec2 target ) +{ + B2_ASSERT( b2Vec2_IsValid( target ) ); + b2JointSim* base = b2GetJointSimCheckType( jointId, b2_mouseJoint ); + base->mouseJoint.targetA = target; +} + +b2Vec2 b2MouseJoint_GetTarget( b2JointId jointId ) +{ + b2JointSim* base = b2GetJointSimCheckType( jointId, b2_mouseJoint ); + return base->mouseJoint.targetA; +} + +void b2MouseJoint_SetSpringHertz( b2JointId jointId, float hertz ) +{ + B2_ASSERT( b2IsValid( hertz ) && hertz >= 0.0f ); + b2JointSim* base = b2GetJointSimCheckType( jointId, b2_mouseJoint ); + base->mouseJoint.hertz = hertz; +} + +float b2MouseJoint_GetSpringHertz( b2JointId jointId ) +{ + b2JointSim* base = b2GetJointSimCheckType( jointId, b2_mouseJoint ); + return base->mouseJoint.hertz; +} + +void b2MouseJoint_SetSpringDampingRatio( b2JointId jointId, float dampingRatio ) +{ + B2_ASSERT( b2IsValid( dampingRatio ) && dampingRatio >= 0.0f ); + b2JointSim* base = b2GetJointSimCheckType( jointId, b2_mouseJoint ); + base->mouseJoint.dampingRatio = dampingRatio; +} + +float b2MouseJoint_GetSpringDampingRatio( b2JointId jointId ) +{ + b2JointSim* base = b2GetJointSimCheckType( jointId, b2_mouseJoint ); + return base->mouseJoint.dampingRatio; +} + +void b2MouseJoint_SetMaxForce( b2JointId jointId, float maxForce ) +{ + B2_ASSERT( b2IsValid( maxForce ) && maxForce >= 0.0f ); + b2JointSim* base = b2GetJointSimCheckType( jointId, b2_mouseJoint ); + base->mouseJoint.maxForce = maxForce; +} + +float b2MouseJoint_GetMaxForce( b2JointId jointId ) +{ + b2JointSim* base = b2GetJointSimCheckType( jointId, b2_mouseJoint ); + return base->mouseJoint.maxForce; +} + +b2Vec2 b2GetMouseJointForce( b2World* world, b2JointSim* base ) +{ + b2Vec2 force = b2MulSV( world->inv_h, base->mouseJoint.linearImpulse ); + return force; +} + +float b2GetMouseJointTorque( b2World* world, b2JointSim* base ) +{ + return world->inv_h * base->mouseJoint.angularImpulse; +} + +void b2PrepareMouseJoint( b2JointSim* base, b2StepContext* context ) +{ + B2_ASSERT( base->type == b2_mouseJoint ); + + // chase body id to the solver set where the body lives + int idB = base->bodyIdB; + + b2World* world = context->world; + + b2Body* bodyB = b2BodyArray_Get( &world->bodies, idB ); + + B2_ASSERT( bodyB->setIndex == b2_awakeSet ); + b2SolverSet* setB = b2SolverSetArray_Get( &world->solverSets, bodyB->setIndex ); + + int localIndexB = bodyB->localIndex; + b2BodySim* bodySimB = b2BodySimArray_Get( &setB->bodySims, localIndexB ); + + base->invMassB = bodySimB->invMass; + base->invIB = bodySimB->invInertia; + + b2MouseJoint* joint = &base->mouseJoint; + joint->indexB = bodyB->setIndex == b2_awakeSet ? localIndexB : B2_NULL_INDEX; + joint->anchorB = b2RotateVector( bodySimB->transform.q, b2Sub( base->localOriginAnchorB, bodySimB->localCenter ) ); + + joint->linearSoftness = b2MakeSoft( joint->hertz, joint->dampingRatio, context->h ); + + float angularHertz = 0.5f; + float angularDampingRatio = 0.1f; + joint->angularSoftness = b2MakeSoft( angularHertz, angularDampingRatio, context->h ); + + b2Vec2 rB = joint->anchorB; + float mB = bodySimB->invMass; + float iB = bodySimB->invInertia; + + // K = [(1/m1 + 1/m2) * eye(2) - skew(r1) * invI1 * skew(r1) - skew(r2) * invI2 * skew(r2)] + // = [1/m1+1/m2 0 ] + invI1 * [r1.y*r1.y -r1.x*r1.y] + invI2 * [r1.y*r1.y -r1.x*r1.y] + // [ 0 1/m1+1/m2] [-r1.x*r1.y r1.x*r1.x] [-r1.x*r1.y r1.x*r1.x] + b2Mat22 K; + K.cx.x = mB + iB * rB.y * rB.y; + K.cx.y = -iB * rB.x * rB.y; + K.cy.x = K.cx.y; + K.cy.y = mB + iB * rB.x * rB.x; + + joint->linearMass = b2GetInverse22( K ); + joint->deltaCenter = b2Sub( bodySimB->center, joint->targetA ); + + if ( context->enableWarmStarting == false ) + { + joint->linearImpulse = b2Vec2_zero; + joint->angularImpulse = 0.0f; + } +} + +void b2WarmStartMouseJoint( b2JointSim* base, b2StepContext* context ) +{ + B2_ASSERT( base->type == b2_mouseJoint ); + + float mB = base->invMassB; + float iB = base->invIB; + + b2MouseJoint* joint = &base->mouseJoint; + + b2BodyState* stateB = context->states + joint->indexB; + b2Vec2 vB = stateB->linearVelocity; + float wB = stateB->angularVelocity; + + b2Rot dqB = stateB->deltaRotation; + b2Vec2 rB = b2RotateVector( dqB, joint->anchorB ); + + vB = b2MulAdd( vB, mB, joint->linearImpulse ); + wB += iB * ( b2Cross( rB, joint->linearImpulse ) + joint->angularImpulse ); + + stateB->linearVelocity = vB; + stateB->angularVelocity = wB; +} + +void b2SolveMouseJoint( b2JointSim* base, b2StepContext* context ) +{ + float mB = base->invMassB; + float iB = base->invIB; + + b2MouseJoint* joint = &base->mouseJoint; + b2BodyState* stateB = context->states + joint->indexB; + + b2Vec2 vB = stateB->linearVelocity; + float wB = stateB->angularVelocity; + + // Softness with no bias to reduce rotation speed + { + float massScale = joint->angularSoftness.massScale; + float impulseScale = joint->angularSoftness.impulseScale; + + float impulse = iB > 0.0f ? -wB / iB : 0.0f; + impulse = massScale * impulse - impulseScale * joint->angularImpulse; + joint->angularImpulse += impulse; + + wB += iB * impulse; + } + + float maxImpulse = joint->maxForce * context->h; + + { + b2Rot dqB = stateB->deltaRotation; + b2Vec2 rB = b2RotateVector( dqB, joint->anchorB ); + b2Vec2 Cdot = b2Add( vB, b2CrossSV( wB, rB ) ); + + b2Vec2 separation = b2Add( b2Add( stateB->deltaPosition, rB ), joint->deltaCenter ); + b2Vec2 bias = b2MulSV( joint->linearSoftness.biasRate, separation ); + + float massScale = joint->linearSoftness.massScale; + float impulseScale = joint->linearSoftness.impulseScale; + + b2Vec2 b = b2MulMV( joint->linearMass, b2Add( Cdot, bias ) ); + + b2Vec2 impulse; + impulse.x = -massScale * b.x - impulseScale * joint->linearImpulse.x; + impulse.y = -massScale * b.y - impulseScale * joint->linearImpulse.y; + + b2Vec2 oldImpulse = joint->linearImpulse; + joint->linearImpulse.x += impulse.x; + joint->linearImpulse.y += impulse.y; + + float mag = b2Length( joint->linearImpulse ); + if ( mag > maxImpulse ) + { + joint->linearImpulse = b2MulSV( maxImpulse, b2Normalize( joint->linearImpulse ) ); + } + + impulse.x = joint->linearImpulse.x - oldImpulse.x; + impulse.y = joint->linearImpulse.y - oldImpulse.y; + + vB = b2MulAdd( vB, mB, impulse ); + wB += iB * b2Cross( rB, impulse ); + } + + stateB->linearVelocity = vB; + stateB->angularVelocity = wB; +} diff --git a/3rdparty/box2d/src/prismatic_joint.c b/3rdparty/box2d/src/prismatic_joint.c new file mode 100644 index 000000000000..ba6836600ce7 --- /dev/null +++ b/3rdparty/box2d/src/prismatic_joint.c @@ -0,0 +1,600 @@ +// SPDX-FileCopyrightText: 2023 Erin Catto +// SPDX-License-Identifier: MIT + +#include "body.h" +#include "core.h" +#include "joint.h" +#include "solver.h" +#include "solver_set.h" +#include "world.h" + +// needed for dll export +#include "box2d/box2d.h" + +#include + +void b2PrismaticJoint_EnableSpring( b2JointId jointId, bool enableSpring ) +{ + b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_prismaticJoint ); + if ( enableSpring != joint->prismaticJoint.enableSpring ) + { + joint->prismaticJoint.enableSpring = enableSpring; + joint->prismaticJoint.springImpulse = 0.0f; + } +} + +bool b2PrismaticJoint_IsSpringEnabled( b2JointId jointId ) +{ + b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_prismaticJoint ); + return joint->prismaticJoint.enableSpring; +} + +void b2PrismaticJoint_SetSpringHertz( b2JointId jointId, float hertz ) +{ + b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_prismaticJoint ); + joint->prismaticJoint.hertz = hertz; +} + +float b2PrismaticJoint_GetSpringHertz( b2JointId jointId ) +{ + b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_prismaticJoint ); + return joint->prismaticJoint.hertz; +} + +void b2PrismaticJoint_SetSpringDampingRatio( b2JointId jointId, float dampingRatio ) +{ + b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_prismaticJoint ); + joint->prismaticJoint.dampingRatio = dampingRatio; +} + +float b2PrismaticJoint_GetSpringDampingRatio( b2JointId jointId ) +{ + b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_prismaticJoint ); + return joint->prismaticJoint.dampingRatio; +} + +void b2PrismaticJoint_EnableLimit( b2JointId jointId, bool enableLimit ) +{ + b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_prismaticJoint ); + if ( enableLimit != joint->prismaticJoint.enableLimit ) + { + joint->prismaticJoint.enableLimit = enableLimit; + joint->prismaticJoint.lowerImpulse = 0.0f; + joint->prismaticJoint.upperImpulse = 0.0f; + } +} + +bool b2PrismaticJoint_IsLimitEnabled( b2JointId jointId ) +{ + b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_prismaticJoint ); + return joint->prismaticJoint.enableLimit; +} + +float b2PrismaticJoint_GetLowerLimit( b2JointId jointId ) +{ + b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_prismaticJoint ); + return joint->prismaticJoint.lowerTranslation; +} + +float b2PrismaticJoint_GetUpperLimit( b2JointId jointId ) +{ + b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_prismaticJoint ); + return joint->prismaticJoint.upperTranslation; +} + +void b2PrismaticJoint_SetLimits( b2JointId jointId, float lower, float upper ) +{ + b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_prismaticJoint ); + if ( lower != joint->prismaticJoint.lowerTranslation || upper != joint->prismaticJoint.upperTranslation ) + { + joint->prismaticJoint.lowerTranslation = b2MinFloat( lower, upper ); + joint->prismaticJoint.upperTranslation = b2MaxFloat( lower, upper ); + joint->prismaticJoint.lowerImpulse = 0.0f; + joint->prismaticJoint.upperImpulse = 0.0f; + } +} + +void b2PrismaticJoint_EnableMotor( b2JointId jointId, bool enableMotor ) +{ + b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_prismaticJoint ); + if ( enableMotor != joint->prismaticJoint.enableMotor ) + { + joint->prismaticJoint.enableMotor = enableMotor; + joint->prismaticJoint.motorImpulse = 0.0f; + } +} + +bool b2PrismaticJoint_IsMotorEnabled( b2JointId jointId ) +{ + b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_prismaticJoint ); + return joint->prismaticJoint.enableMotor; +} + +void b2PrismaticJoint_SetMotorSpeed( b2JointId jointId, float motorSpeed ) +{ + b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_prismaticJoint ); + joint->prismaticJoint.motorSpeed = motorSpeed; +} + +float b2PrismaticJoint_GetMotorSpeed( b2JointId jointId ) +{ + b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_prismaticJoint ); + return joint->prismaticJoint.motorSpeed; +} + +float b2PrismaticJoint_GetMotorForce( b2JointId jointId ) +{ + b2World* world = b2GetWorld( jointId.world0 ); + b2JointSim* base = b2GetJointSimCheckType( jointId, b2_prismaticJoint ); + return world->inv_h * base->prismaticJoint.motorImpulse; +} + +void b2PrismaticJoint_SetMaxMotorForce( b2JointId jointId, float force ) +{ + b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_prismaticJoint ); + joint->prismaticJoint.maxMotorForce = force; +} + +float b2PrismaticJoint_GetMaxMotorForce( b2JointId jointId ) +{ + b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_prismaticJoint ); + return joint->prismaticJoint.maxMotorForce; +} + +b2Vec2 b2GetPrismaticJointForce( b2World* world, b2JointSim* base ) +{ + int idA = base->bodyIdA; + b2Transform transformA = b2GetBodyTransform( world, idA ); + + b2PrismaticJoint* joint = &base->prismaticJoint; + + b2Vec2 axisA = b2RotateVector( transformA.q, joint->localAxisA ); + b2Vec2 perpA = b2LeftPerp( axisA ); + + float inv_h = world->inv_h; + float perpForce = inv_h * joint->impulse.x; + float axialForce = inv_h * ( joint->motorImpulse + joint->lowerImpulse - joint->upperImpulse ); + + b2Vec2 force = b2Add( b2MulSV( perpForce, perpA ), b2MulSV( axialForce, axisA ) ); + return force; +} + +float b2GetPrismaticJointTorque( b2World* world, b2JointSim* base ) +{ + return world->inv_h * base->prismaticJoint.impulse.y; +} + +// 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 b2PreparePrismaticJoint( b2JointSim* base, b2StepContext* context ) +{ + B2_ASSERT( base->type == b2_prismaticJoint ); + + // chase body id to the solver set where the body lives + int idA = base->bodyIdA; + int idB = base->bodyIdB; + + b2World* world = context->world; + + b2Body* bodyA = b2BodyArray_Get( &world->bodies, idA ); + b2Body* bodyB = b2BodyArray_Get( &world->bodies, idB ); + + B2_ASSERT( bodyA->setIndex == b2_awakeSet || bodyB->setIndex == b2_awakeSet ); + b2SolverSet* setA = b2SolverSetArray_Get( &world->solverSets, bodyA->setIndex ); + b2SolverSet* setB = b2SolverSetArray_Get( &world->solverSets, bodyB->setIndex ); + + int localIndexA = bodyA->localIndex; + int localIndexB = bodyB->localIndex; + + b2BodySim* bodySimA = b2BodySimArray_Get( &setA->bodySims, localIndexA ); + b2BodySim* bodySimB = b2BodySimArray_Get( &setB->bodySims, localIndexB ); + + float mA = bodySimA->invMass; + float iA = bodySimA->invInertia; + float mB = bodySimB->invMass; + float iB = bodySimB->invInertia; + + base->invMassA = mA; + base->invMassB = mB; + base->invIA = iA; + base->invIB = iB; + + b2PrismaticJoint* joint = &base->prismaticJoint; + joint->indexA = bodyA->setIndex == b2_awakeSet ? localIndexA : B2_NULL_INDEX; + joint->indexB = bodyB->setIndex == b2_awakeSet ? localIndexB : B2_NULL_INDEX; + + b2Rot qA = bodySimA->transform.q; + b2Rot qB = bodySimB->transform.q; + + joint->anchorA = b2RotateVector( qA, b2Sub( base->localOriginAnchorA, bodySimA->localCenter ) ); + joint->anchorB = b2RotateVector( qB, b2Sub( base->localOriginAnchorB, bodySimB->localCenter ) ); + joint->axisA = b2RotateVector( qA, joint->localAxisA ); + joint->deltaCenter = b2Sub( bodySimB->center, bodySimA->center ); + joint->deltaAngle = b2RelativeAngle( qB, qA ) - joint->referenceAngle; + + b2Vec2 rA = joint->anchorA; + b2Vec2 rB = joint->anchorB; + + b2Vec2 d = b2Add( joint->deltaCenter, b2Sub( rB, rA ) ); + float a1 = b2Cross( b2Add( d, rA ), joint->axisA ); + float a2 = b2Cross( rB, joint->axisA ); + + // effective masses + float k = mA + mB + iA * a1 * a1 + iB * a2 * a2; + joint->axialMass = k > 0.0f ? 1.0f / k : 0.0f; + + joint->springSoftness = b2MakeSoft( joint->hertz, joint->dampingRatio, context->h ); + + if ( context->enableWarmStarting == false ) + { + joint->impulse = b2Vec2_zero; + joint->springImpulse = 0.0f; + joint->motorImpulse = 0.0f; + joint->lowerImpulse = 0.0f; + joint->upperImpulse = 0.0f; + } +} + +void b2WarmStartPrismaticJoint( b2JointSim* base, b2StepContext* context ) +{ + B2_ASSERT( base->type == b2_prismaticJoint ); + + float mA = base->invMassA; + float mB = base->invMassB; + float iA = base->invIA; + float iB = base->invIB; + + // dummy state for static bodies + b2BodyState dummyState = b2_identityBodyState; + + b2PrismaticJoint* joint = &base->prismaticJoint; + + b2BodyState* stateA = joint->indexA == B2_NULL_INDEX ? &dummyState : context->states + joint->indexA; + b2BodyState* stateB = joint->indexB == B2_NULL_INDEX ? &dummyState : context->states + joint->indexB; + + b2Vec2 rA = b2RotateVector( stateA->deltaRotation, joint->anchorA ); + b2Vec2 rB = b2RotateVector( stateB->deltaRotation, joint->anchorB ); + + b2Vec2 d = b2Add( b2Add( b2Sub( stateB->deltaPosition, stateA->deltaPosition ), joint->deltaCenter ), b2Sub( rB, rA ) ); + b2Vec2 axisA = b2RotateVector( stateA->deltaRotation, joint->axisA ); + + // impulse is applied at anchor point on body B + float a1 = b2Cross( b2Add( d, rA ), axisA ); + float a2 = b2Cross( rB, axisA ); + float axialImpulse = joint->springImpulse + joint->motorImpulse + joint->lowerImpulse - joint->upperImpulse; + + // perpendicular constraint + b2Vec2 perpA = b2LeftPerp( axisA ); + float s1 = b2Cross( b2Add( d, rA ), perpA ); + float s2 = b2Cross( rB, perpA ); + float perpImpulse = joint->impulse.x; + float angleImpulse = joint->impulse.y; + + b2Vec2 P = b2Add( b2MulSV( axialImpulse, axisA ), b2MulSV( perpImpulse, perpA ) ); + float LA = axialImpulse * a1 + perpImpulse * s1 + angleImpulse; + float LB = axialImpulse * a2 + perpImpulse * s2 + angleImpulse; + + stateA->linearVelocity = b2MulSub( stateA->linearVelocity, mA, P ); + stateA->angularVelocity -= iA * LA; + stateB->linearVelocity = b2MulAdd( stateB->linearVelocity, mB, P ); + stateB->angularVelocity += iB * LB; +} + +void b2SolvePrismaticJoint( b2JointSim* base, b2StepContext* context, bool useBias ) +{ + B2_ASSERT( base->type == b2_prismaticJoint ); + + float mA = base->invMassA; + float mB = base->invMassB; + float iA = base->invIA; + float iB = base->invIB; + + // dummy state for static bodies + b2BodyState dummyState = b2_identityBodyState; + + b2PrismaticJoint* joint = &base->prismaticJoint; + + b2BodyState* stateA = joint->indexA == B2_NULL_INDEX ? &dummyState : context->states + joint->indexA; + b2BodyState* stateB = joint->indexB == B2_NULL_INDEX ? &dummyState : context->states + joint->indexB; + + b2Vec2 vA = stateA->linearVelocity; + float wA = stateA->angularVelocity; + b2Vec2 vB = stateB->linearVelocity; + float wB = stateB->angularVelocity; + + // current anchors + b2Vec2 rA = b2RotateVector( stateA->deltaRotation, joint->anchorA ); + b2Vec2 rB = b2RotateVector( stateB->deltaRotation, joint->anchorB ); + + b2Vec2 d = b2Add( b2Add( b2Sub( stateB->deltaPosition, stateA->deltaPosition ), joint->deltaCenter ), b2Sub( rB, rA ) ); + b2Vec2 axisA = b2RotateVector( stateA->deltaRotation, joint->axisA ); + float translation = b2Dot( axisA, d ); + + // These scalars are for torques generated by axial forces + float a1 = b2Cross( b2Add( d, rA ), axisA ); + float a2 = b2Cross( rB, axisA ); + + // spring constraint + if ( joint->enableSpring ) + { + // This is a real spring and should be applied even during relax + float C = translation; + float bias = joint->springSoftness.biasRate * C; + float massScale = joint->springSoftness.massScale; + float impulseScale = joint->springSoftness.impulseScale; + + float Cdot = b2Dot( axisA, b2Sub( vB, vA ) ) + a2 * wB - a1 * wA; + float impulse = -massScale * joint->axialMass * ( Cdot + bias ) - impulseScale * joint->springImpulse; + joint->springImpulse += impulse; + + b2Vec2 P = b2MulSV( impulse, axisA ); + float LA = impulse * a1; + float LB = impulse * a2; + + vA = b2MulSub( vA, mA, P ); + wA -= iA * LA; + vB = b2MulAdd( vB, mB, P ); + wB += iB * LB; + } + + // Solve motor constraint + if ( joint->enableMotor ) + { + float Cdot = b2Dot( axisA, b2Sub( vB, vA ) ) + a2 * wB - a1 * wA; + float impulse = joint->axialMass * ( joint->motorSpeed - Cdot ); + float oldImpulse = joint->motorImpulse; + float maxImpulse = context->h * joint->maxMotorForce; + joint->motorImpulse = b2ClampFloat( joint->motorImpulse + impulse, -maxImpulse, maxImpulse ); + impulse = joint->motorImpulse - oldImpulse; + + b2Vec2 P = b2MulSV( impulse, axisA ); + float LA = impulse * a1; + float LB = impulse * a2; + + vA = b2MulSub( vA, mA, P ); + wA -= iA * LA; + vB = b2MulAdd( vB, mB, P ); + wB += iB * LB; + } + + if ( joint->enableLimit ) + { + // Lower limit + { + float C = translation - joint->lowerTranslation; + float bias = 0.0f; + float massScale = 1.0f; + float impulseScale = 0.0f; + + if ( C > 0.0f ) + { + // speculation + bias = C * context->inv_h; + } + else if ( useBias ) + { + bias = context->jointSoftness.biasRate * C; + massScale = context->jointSoftness.massScale; + impulseScale = context->jointSoftness.impulseScale; + } + + float oldImpulse = joint->lowerImpulse; + float Cdot = b2Dot( axisA, b2Sub( vB, vA ) ) + a2 * wB - a1 * wA; + float impulse = -joint->axialMass * massScale * ( Cdot + bias ) - impulseScale * oldImpulse; + joint->lowerImpulse = b2MaxFloat( oldImpulse + impulse, 0.0f ); + impulse = joint->lowerImpulse - oldImpulse; + + b2Vec2 P = b2MulSV( impulse, axisA ); + float LA = impulse * a1; + float LB = impulse * a2; + + vA = b2MulSub( vA, mA, P ); + wA -= iA * LA; + vB = b2MulAdd( 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. + { + // sign flipped + float C = joint->upperTranslation - translation; + float bias = 0.0f; + float massScale = 1.0f; + float impulseScale = 0.0f; + + if ( C > 0.0f ) + { + // speculation + bias = C * context->inv_h; + } + else if ( useBias ) + { + bias = context->jointSoftness.biasRate * C; + massScale = context->jointSoftness.massScale; + impulseScale = context->jointSoftness.impulseScale; + } + + float oldImpulse = joint->upperImpulse; + // sign flipped + float Cdot = b2Dot( axisA, b2Sub( vA, vB ) ) + a1 * wA - a2 * wB; + float impulse = -joint->axialMass * massScale * ( Cdot + bias ) - impulseScale * oldImpulse; + joint->upperImpulse = b2MaxFloat( oldImpulse + impulse, 0.0f ); + impulse = joint->upperImpulse - oldImpulse; + + b2Vec2 P = b2MulSV( impulse, axisA ); + float LA = impulse * a1; + float LB = impulse * a2; + + // sign flipped + vA = b2MulAdd( vA, mA, P ); + wA += iA * LA; + vB = b2MulSub( vB, mB, P ); + wB -= iB * LB; + } + } + + // Solve the prismatic constraint in block form + { + b2Vec2 perpA = b2LeftPerp( axisA ); + + // These scalars are for torques generated by the perpendicular constraint force + float s1 = b2Cross( b2Add( d, rA ), perpA ); + float s2 = b2Cross( rB, perpA ); + + b2Vec2 Cdot; + Cdot.x = b2Dot( perpA, b2Sub( vB, vA ) ) + s2 * wB - s1 * wA; + Cdot.y = wB - wA; + + b2Vec2 bias = b2Vec2_zero; + float massScale = 1.0f; + float impulseScale = 0.0f; + if ( useBias ) + { + b2Vec2 C; + C.x = b2Dot( perpA, d ); + C.y = b2RelativeAngle( stateB->deltaRotation, stateA->deltaRotation ) + joint->deltaAngle; + + bias = b2MulSV( context->jointSoftness.biasRate, C ); + massScale = context->jointSoftness.massScale; + impulseScale = context->jointSoftness.impulseScale; + } + + float k11 = mA + mB + iA * s1 * s1 + iB * s2 * s2; + float k12 = iA * s1 + iB * s2; + float k22 = iA + iB; + if ( k22 == 0.0f ) + { + // For bodies with fixed rotation. + k22 = 1.0f; + } + + b2Mat22 K = { { k11, k12 }, { k12, k22 } }; + + b2Vec2 b = b2Solve22( K, b2Add( Cdot, bias ) ); + b2Vec2 impulse; + impulse.x = -massScale * b.x - impulseScale * joint->impulse.x; + impulse.y = -massScale * b.y - impulseScale * joint->impulse.y; + + joint->impulse.x += impulse.x; + joint->impulse.y += impulse.y; + + b2Vec2 P = b2MulSV( impulse.x, perpA ); + float LA = impulse.x * s1 + impulse.y; + float LB = impulse.x * s2 + impulse.y; + + vA = b2MulSub( vA, mA, P ); + wA -= iA * LA; + vB = b2MulAdd( vB, mB, P ); + wB += iB * LB; + } + + stateA->linearVelocity = vA; + stateA->angularVelocity = wA; + stateB->linearVelocity = vB; + stateB->angularVelocity = wB; +} + +#if 0 +void b2PrismaticJoint::Dump() +{ + int32 indexA = joint->bodyA->joint->islandIndex; + int32 indexB = joint->bodyB->joint->islandIndex; + + b2Dump(" b2PrismaticJointDef jd;\n"); + b2Dump(" jd.bodyA = sims[%d];\n", indexA); + b2Dump(" jd.bodyB = sims[%d];\n", indexB); + b2Dump(" jd.collideConnected = bool(%d);\n", joint->collideConnected); + b2Dump(" jd.localAnchorA.Set(%.9g, %.9g);\n", joint->localAnchorA.x, joint->localAnchorA.y); + b2Dump(" jd.localAnchorB.Set(%.9g, %.9g);\n", joint->localAnchorB.x, joint->localAnchorB.y); + b2Dump(" jd.referenceAngle = %.9g;\n", joint->referenceAngle); + b2Dump(" jd.enableLimit = bool(%d);\n", joint->enableLimit); + b2Dump(" jd.lowerAngle = %.9g;\n", joint->lowerAngle); + b2Dump(" jd.upperAngle = %.9g;\n", joint->upperAngle); + b2Dump(" jd.enableMotor = bool(%d);\n", joint->enableMotor); + b2Dump(" jd.motorSpeed = %.9g;\n", joint->motorSpeed); + b2Dump(" jd.maxMotorTorque = %.9g;\n", joint->maxMotorTorque); + b2Dump(" joints[%d] = joint->world->CreateJoint(&jd);\n", joint->index); +} +#endif + +void b2DrawPrismaticJoint( b2DebugDraw* draw, b2JointSim* base, b2Transform transformA, b2Transform transformB ) +{ + B2_ASSERT( base->type == b2_prismaticJoint ); + + b2PrismaticJoint* joint = &base->prismaticJoint; + + b2Vec2 pA = b2TransformPoint( transformA, base->localOriginAnchorA ); + b2Vec2 pB = b2TransformPoint( transformB, base->localOriginAnchorB ); + + b2Vec2 axis = b2RotateVector( transformA.q, joint->localAxisA ); + + b2HexColor c1 = b2_colorGray7; + b2HexColor c2 = b2_colorGreen; + b2HexColor c3 = b2_colorRed; + b2HexColor c4 = b2_colorBlue; + b2HexColor c5 = b2_colorGray4; + + draw->DrawSegment( pA, pB, c5, draw->context ); + + if ( joint->enableLimit ) + { + b2Vec2 lower = b2MulAdd( pA, joint->lowerTranslation, axis ); + b2Vec2 upper = b2MulAdd( pA, joint->upperTranslation, axis ); + b2Vec2 perp = b2LeftPerp( axis ); + draw->DrawSegment( lower, upper, c1, draw->context ); + draw->DrawSegment( b2MulSub( lower, 0.1f, perp ), b2MulAdd( lower, 0.1f, perp ), c2, draw->context ); + draw->DrawSegment( b2MulSub( upper, 0.1f, perp ), b2MulAdd( upper, 0.1f, perp ), c3, draw->context ); + } + else + { + draw->DrawSegment( b2MulSub( pA, 1.0f, axis ), b2MulAdd( pA, 1.0f, axis ), c1, draw->context ); + } + + draw->DrawPoint( pA, 5.0f, c1, draw->context ); + draw->DrawPoint( pB, 5.0f, c4, draw->context ); +} diff --git a/3rdparty/box2d/src/revolute_joint.c b/3rdparty/box2d/src/revolute_joint.c new file mode 100644 index 000000000000..98b8eb09c87d --- /dev/null +++ b/3rdparty/box2d/src/revolute_joint.c @@ -0,0 +1,547 @@ +// SPDX-FileCopyrightText: 2023 Erin Catto +// SPDX-License-Identifier: MIT + +#if defined( _MSC_VER ) && !defined( _CRT_SECURE_NO_WARNINGS ) +#define _CRT_SECURE_NO_WARNINGS +#endif + +#include "body.h" +#include "core.h" +#include "joint.h" +#include "solver.h" +#include "solver_set.h" +#include "world.h" + +// needed for dll export +#include "box2d/box2d.h" + +#include + +void b2RevoluteJoint_EnableSpring( b2JointId jointId, bool enableSpring ) +{ + b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_revoluteJoint ); + if ( enableSpring != joint->revoluteJoint.enableSpring ) + { + joint->revoluteJoint.enableSpring = enableSpring; + joint->revoluteJoint.springImpulse = 0.0f; + } +} + +bool b2RevoluteJoint_IsSpringEnabled( b2JointId jointId ) +{ + b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_revoluteJoint ); + return joint->revoluteJoint.enableSpring; +} + +void b2RevoluteJoint_SetSpringHertz( b2JointId jointId, float hertz ) +{ + b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_revoluteJoint ); + joint->revoluteJoint.hertz = hertz; +} + +float b2RevoluteJoint_GetSpringHertz( b2JointId jointId ) +{ + b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_revoluteJoint ); + return joint->revoluteJoint.hertz; +} + +void b2RevoluteJoint_SetSpringDampingRatio( b2JointId jointId, float dampingRatio ) +{ + b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_revoluteJoint ); + joint->revoluteJoint.dampingRatio = dampingRatio; +} + +float b2RevoluteJoint_GetSpringDampingRatio( b2JointId jointId ) +{ + b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_revoluteJoint ); + return joint->revoluteJoint.dampingRatio; +} + +float b2RevoluteJoint_GetAngle( b2JointId jointId ) +{ + b2World* world = b2GetWorld( jointId.world0 ); + b2JointSim* jointSim = b2GetJointSimCheckType( jointId, b2_revoluteJoint ); + b2Transform transformA = b2GetBodyTransform( world, jointSim->bodyIdA ); + b2Transform transformB = b2GetBodyTransform( world, jointSim->bodyIdB ); + + float angle = b2RelativeAngle( transformB.q, transformA.q ) - jointSim->revoluteJoint.referenceAngle; + angle = b2UnwindAngle( angle ); + return angle; +} + +void b2RevoluteJoint_EnableLimit( b2JointId jointId, bool enableLimit ) +{ + b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_revoluteJoint ); + if ( enableLimit != joint->revoluteJoint.enableLimit ) + { + joint->revoluteJoint.enableLimit = enableLimit; + joint->revoluteJoint.lowerImpulse = 0.0f; + joint->revoluteJoint.upperImpulse = 0.0f; + } +} + +bool b2RevoluteJoint_IsLimitEnabled( b2JointId jointId ) +{ + b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_revoluteJoint ); + return joint->revoluteJoint.enableLimit; +} + +float b2RevoluteJoint_GetLowerLimit( b2JointId jointId ) +{ + b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_revoluteJoint ); + return joint->revoluteJoint.lowerAngle; +} + +float b2RevoluteJoint_GetUpperLimit( b2JointId jointId ) +{ + b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_revoluteJoint ); + return joint->revoluteJoint.upperAngle; +} + +void b2RevoluteJoint_SetLimits( b2JointId jointId, float lower, float upper ) +{ + b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_revoluteJoint ); + if ( lower != joint->revoluteJoint.lowerAngle || upper != joint->revoluteJoint.upperAngle ) + { + joint->revoluteJoint.lowerAngle = b2MinFloat( lower, upper ); + joint->revoluteJoint.upperAngle = b2MaxFloat( lower, upper ); + joint->revoluteJoint.lowerImpulse = 0.0f; + joint->revoluteJoint.upperImpulse = 0.0f; + } +} + +void b2RevoluteJoint_EnableMotor( b2JointId jointId, bool enableMotor ) +{ + b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_revoluteJoint ); + if ( enableMotor != joint->revoluteJoint.enableMotor ) + { + joint->revoluteJoint.enableMotor = enableMotor; + joint->revoluteJoint.motorImpulse = 0.0f; + } +} + +bool b2RevoluteJoint_IsMotorEnabled( b2JointId jointId ) +{ + b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_revoluteJoint ); + return joint->revoluteJoint.enableMotor; +} + +void b2RevoluteJoint_SetMotorSpeed( b2JointId jointId, float motorSpeed ) +{ + b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_revoluteJoint ); + joint->revoluteJoint.motorSpeed = motorSpeed; +} + +float b2RevoluteJoint_GetMotorSpeed( b2JointId jointId ) +{ + b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_revoluteJoint ); + return joint->revoluteJoint.motorSpeed; +} + +float b2RevoluteJoint_GetMotorTorque( b2JointId jointId ) +{ + b2World* world = b2GetWorld( jointId.world0 ); + b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_revoluteJoint ); + return world->inv_h * joint->revoluteJoint.motorImpulse; +} + +void b2RevoluteJoint_SetMaxMotorTorque( b2JointId jointId, float torque ) +{ + b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_revoluteJoint ); + joint->revoluteJoint.maxMotorTorque = torque; +} + +float b2RevoluteJoint_GetMaxMotorTorque( b2JointId jointId ) +{ + b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_revoluteJoint ); + return joint->revoluteJoint.maxMotorTorque; +} + +b2Vec2 b2GetRevoluteJointForce( b2World* world, b2JointSim* base ) +{ + b2Vec2 force = b2MulSV( world->inv_h, base->revoluteJoint.linearImpulse ); + return force; +} + +float b2GetRevoluteJointTorque( b2World* world, b2JointSim* base ) +{ + const b2RevoluteJoint* revolute = &base->revoluteJoint; + float torque = world->inv_h * ( revolute->motorImpulse + revolute->lowerImpulse - revolute->upperImpulse ); + return torque; +} + +// 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 + +// Body State +// The solver operates on the body state. The body state array does not hold static bodies. Static bodies are shared +// across worker threads. It would be okay to read their states, but writing to them would cause cache thrashing across +// workers, even if the values don't change. +// This causes some trouble when computing anchors. I rotate the anchors using the body rotation every sub-step. For static +// bodies the anchor doesn't rotate. Body A or B could be static and this can lead to lots of branching. This branching +// should be minimized. +// +// Solution 1: +// Use delta rotations. This means anchors need to be prepared in world space. The delta rotation for static bodies will be +// identity. Base separation and angles need to be computed. Manifolds will be behind a frame, but that is probably best if bodies +// move fast. +// +// Solution 2: +// Use full rotation. The anchors for static bodies will be in world space while the anchors for dynamic bodies will be in local +// space. Potentially confusing and bug prone. + +void b2PrepareRevoluteJoint( b2JointSim* base, b2StepContext* context ) +{ + B2_ASSERT( base->type == b2_revoluteJoint ); + + // chase body id to the solver set where the body lives + int idA = base->bodyIdA; + int idB = base->bodyIdB; + + b2World* world = context->world; + + b2Body* bodyA = b2BodyArray_Get(&world->bodies, idA); + b2Body* bodyB = b2BodyArray_Get(&world->bodies, idB); + + B2_ASSERT( bodyA->setIndex == b2_awakeSet || bodyB->setIndex == b2_awakeSet ); + b2SolverSet* setA = b2SolverSetArray_Get( &world->solverSets, bodyA->setIndex ); + b2SolverSet* setB = b2SolverSetArray_Get( &world->solverSets, bodyB->setIndex ); + + int localIndexA = bodyA->localIndex; + int localIndexB = bodyB->localIndex; + + b2BodySim* bodySimA = b2BodySimArray_Get( &setA->bodySims, localIndexA ); + b2BodySim* bodySimB = b2BodySimArray_Get( &setB->bodySims, localIndexB ); + + float mA = bodySimA->invMass; + float iA = bodySimA->invInertia; + float mB = bodySimB->invMass; + float iB = bodySimB->invInertia; + + base->invMassA = mA; + base->invMassB = mB; + base->invIA = iA; + base->invIB = iB; + + b2RevoluteJoint* joint = &base->revoluteJoint; + + joint->indexA = bodyA->setIndex == b2_awakeSet ? localIndexA : B2_NULL_INDEX; + joint->indexB = bodyB->setIndex == b2_awakeSet ? localIndexB : B2_NULL_INDEX; + + // initial anchors in world space + joint->anchorA = b2RotateVector( bodySimA->transform.q, b2Sub( base->localOriginAnchorA, bodySimA->localCenter ) ); + joint->anchorB = b2RotateVector( bodySimB->transform.q, b2Sub( base->localOriginAnchorB, bodySimB->localCenter ) ); + joint->deltaCenter = b2Sub( bodySimB->center, bodySimA->center ); + joint->deltaAngle = b2RelativeAngle( bodySimB->transform.q, bodySimA->transform.q ) - joint->referenceAngle; + joint->deltaAngle = b2UnwindAngle( joint->deltaAngle ); + + float k = iA + iB; + joint->axialMass = k > 0.0f ? 1.0f / k : 0.0f; + + joint->springSoftness = b2MakeSoft( joint->hertz, joint->dampingRatio, context->h ); + + if ( context->enableWarmStarting == false ) + { + joint->linearImpulse = b2Vec2_zero; + joint->springImpulse = 0.0f; + joint->motorImpulse = 0.0f; + joint->lowerImpulse = 0.0f; + joint->upperImpulse = 0.0f; + } +} + +void b2WarmStartRevoluteJoint( b2JointSim* base, b2StepContext* context ) +{ + B2_ASSERT( base->type == b2_revoluteJoint ); + + float mA = base->invMassA; + float mB = base->invMassB; + float iA = base->invIA; + float iB = base->invIB; + + // dummy state for static bodies + b2BodyState dummyState = b2_identityBodyState; + + b2RevoluteJoint* joint = &base->revoluteJoint; + b2BodyState* stateA = joint->indexA == B2_NULL_INDEX ? &dummyState : context->states + joint->indexA; + b2BodyState* stateB = joint->indexB == B2_NULL_INDEX ? &dummyState : context->states + joint->indexB; + + b2Vec2 rA = b2RotateVector( stateA->deltaRotation, joint->anchorA ); + b2Vec2 rB = b2RotateVector( stateB->deltaRotation, joint->anchorB ); + + float axialImpulse = joint->springImpulse + joint->motorImpulse + joint->lowerImpulse - joint->upperImpulse; + + stateA->linearVelocity = b2MulSub( stateA->linearVelocity, mA, joint->linearImpulse ); + stateA->angularVelocity -= iA * ( b2Cross( rA, joint->linearImpulse ) + axialImpulse ); + + stateB->linearVelocity = b2MulAdd( stateB->linearVelocity, mB, joint->linearImpulse ); + stateB->angularVelocity += iB * ( b2Cross( rB, joint->linearImpulse ) + axialImpulse ); +} + +void b2SolveRevoluteJoint( b2JointSim* base, b2StepContext* context, bool useBias ) +{ + B2_ASSERT( base->type == b2_revoluteJoint ); + + float mA = base->invMassA; + float mB = base->invMassB; + float iA = base->invIA; + float iB = base->invIB; + + // dummy state for static bodies + b2BodyState dummyState = b2_identityBodyState; + + b2RevoluteJoint* joint = &base->revoluteJoint; + + b2BodyState* stateA = joint->indexA == B2_NULL_INDEX ? &dummyState : context->states + joint->indexA; + b2BodyState* stateB = joint->indexB == B2_NULL_INDEX ? &dummyState : context->states + joint->indexB; + + b2Vec2 vA = stateA->linearVelocity; + float wA = stateA->angularVelocity; + b2Vec2 vB = stateB->linearVelocity; + float wB = stateB->angularVelocity; + + bool fixedRotation = ( iA + iB == 0.0f ); + // const float maxBias = context->maxBiasVelocity; + + // Solve spring. + if ( joint->enableSpring && fixedRotation == false ) + { + float C = b2RelativeAngle( stateB->deltaRotation, stateA->deltaRotation ) + joint->deltaAngle; + float bias = joint->springSoftness.biasRate * C; + float massScale = joint->springSoftness.massScale; + float impulseScale = joint->springSoftness.impulseScale; + + float Cdot = wB - wA; + float impulse = -massScale * joint->axialMass * ( Cdot + bias ) - impulseScale * joint->springImpulse; + joint->springImpulse += impulse; + + wA -= iA * impulse; + wB += iB * impulse; + } + + // Solve motor constraint. + if ( joint->enableMotor && fixedRotation == false ) + { + float Cdot = wB - wA - joint->motorSpeed; + float impulse = -joint->axialMass * Cdot; + float oldImpulse = joint->motorImpulse; + float maxImpulse = context->h * joint->maxMotorTorque; + joint->motorImpulse = b2ClampFloat( joint->motorImpulse + impulse, -maxImpulse, maxImpulse ); + impulse = joint->motorImpulse - oldImpulse; + + wA -= iA * impulse; + wB += iB * impulse; + } + + if ( joint->enableLimit && fixedRotation == false ) + { + float jointAngle = b2RelativeAngle( stateB->deltaRotation, stateA->deltaRotation ) + joint->deltaAngle; + jointAngle = b2UnwindAngle( jointAngle ); + + // Lower limit + { + float C = jointAngle - joint->lowerAngle; + float bias = 0.0f; + float massScale = 1.0f; + float impulseScale = 0.0f; + if ( C > 0.0f ) + { + // speculation + bias = C * context->inv_h; + } + else if ( useBias ) + { + bias = context->jointSoftness.biasRate * C; + massScale = context->jointSoftness.massScale; + impulseScale = context->jointSoftness.impulseScale; + } + + float Cdot = wB - wA; + float impulse = -massScale * joint->axialMass * ( Cdot + bias ) - impulseScale * joint->lowerImpulse; + float oldImpulse = joint->lowerImpulse; + joint->lowerImpulse = b2MaxFloat( joint->lowerImpulse + impulse, 0.0f ); + impulse = joint->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 = joint->upperAngle - jointAngle; + float bias = 0.0f; + float massScale = 1.0f; + float impulseScale = 0.0f; + if ( C > 0.0f ) + { + // speculation + bias = C * context->inv_h; + } + else if ( useBias ) + { + bias = context->jointSoftness.biasRate * C; + massScale = context->jointSoftness.massScale; + impulseScale = context->jointSoftness.impulseScale; + } + + // sign flipped on Cdot + float Cdot = wA - wB; + float impulse = -massScale * joint->axialMass * ( Cdot + bias ) - impulseScale * joint->lowerImpulse; + float oldImpulse = joint->upperImpulse; + joint->upperImpulse = b2MaxFloat( joint->upperImpulse + impulse, 0.0f ); + impulse = joint->upperImpulse - oldImpulse; + + // sign flipped on applied impulse + wA += iA * impulse; + wB -= iB * impulse; + } + } + + // Solve point-to-point constraint + { + // J = [-I -r1_skew I r2_skew] + // r_skew = [-ry; rx] + // 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] + + // current anchors + b2Vec2 rA = b2RotateVector( stateA->deltaRotation, joint->anchorA ); + b2Vec2 rB = b2RotateVector( stateB->deltaRotation, joint->anchorB ); + + b2Vec2 Cdot = b2Sub( b2Add( vB, b2CrossSV( wB, rB ) ), b2Add( vA, b2CrossSV( wA, rA ) ) ); + + b2Vec2 bias = b2Vec2_zero; + float massScale = 1.0f; + float impulseScale = 0.0f; + if ( useBias ) + { + b2Vec2 dcA = stateA->deltaPosition; + b2Vec2 dcB = stateB->deltaPosition; + + b2Vec2 separation = b2Add( b2Add( b2Sub( dcB, dcA ), b2Sub( rB, rA ) ), joint->deltaCenter ); + bias = b2MulSV( context->jointSoftness.biasRate, separation ); + massScale = context->jointSoftness.massScale; + impulseScale = context->jointSoftness.impulseScale; + } + + b2Mat22 K; + K.cx.x = mA + mB + rA.y * rA.y * iA + rB.y * rB.y * iB; + K.cy.x = -rA.y * rA.x * iA - rB.y * rB.x * iB; + K.cx.y = K.cy.x; + K.cy.y = mA + mB + rA.x * rA.x * iA + rB.x * rB.x * iB; + b2Vec2 b = b2Solve22( K, b2Add( Cdot, bias ) ); + + b2Vec2 impulse; + impulse.x = -massScale * b.x - impulseScale * joint->linearImpulse.x; + impulse.y = -massScale * b.y - impulseScale * joint->linearImpulse.y; + joint->linearImpulse.x += impulse.x; + joint->linearImpulse.y += impulse.y; + + vA = b2MulSub( vA, mA, impulse ); + wA -= iA * b2Cross( rA, impulse ); + vB = b2MulAdd( vB, mB, impulse ); + wB += iB * b2Cross( rB, impulse ); + } + + stateA->linearVelocity = vA; + stateA->angularVelocity = wA; + stateB->linearVelocity = vB; + stateB->angularVelocity = wB; +} + +#if 0 +void b2RevoluteJoint::Dump() +{ + int32 indexA = joint->bodyA->joint->islandIndex; + int32 indexB = joint->bodyB->joint->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", joint->collideConnected); + b2Dump(" jd.localAnchorA.Set(%.9g, %.9g);\n", joint->localAnchorA.x, joint->localAnchorA.y); + b2Dump(" jd.localAnchorB.Set(%.9g, %.9g);\n", joint->localAnchorB.x, joint->localAnchorB.y); + b2Dump(" jd.referenceAngle = %.9g;\n", joint->referenceAngle); + b2Dump(" jd.enableLimit = bool(%d);\n", joint->enableLimit); + b2Dump(" jd.lowerAngle = %.9g;\n", joint->lowerAngle); + b2Dump(" jd.upperAngle = %.9g;\n", joint->upperAngle); + b2Dump(" jd.enableMotor = bool(%d);\n", joint->enableMotor); + b2Dump(" jd.motorSpeed = %.9g;\n", joint->motorSpeed); + b2Dump(" jd.maxMotorTorque = %.9g;\n", joint->maxMotorTorque); + b2Dump(" joints[%d] = joint->world->CreateJoint(&jd);\n", joint->index); +} +#endif + +void b2DrawRevoluteJoint( b2DebugDraw* draw, b2JointSim* base, b2Transform transformA, b2Transform transformB, float drawSize ) +{ + B2_ASSERT( base->type == b2_revoluteJoint ); + + b2RevoluteJoint* joint = &base->revoluteJoint; + + b2Vec2 pA = b2TransformPoint( transformA, base->localOriginAnchorA ); + b2Vec2 pB = b2TransformPoint( transformB, base->localOriginAnchorB ); + + b2HexColor c1 = b2_colorGray7; + b2HexColor c2 = b2_colorGreen; + b2HexColor c3 = b2_colorRed; + + const float L = drawSize; + // draw->DrawPoint(pA, 3.0f, b2_colorGray40, draw->context); + // draw->DrawPoint(pB, 3.0f, b2_colorLightBlue, draw->context); + draw->DrawCircle( pB, L, c1, draw->context ); + + float angle = b2RelativeAngle( transformB.q, transformA.q ); + + b2Rot rot = b2MakeRot( angle ); + b2Vec2 r = { L * rot.c, L * rot.s }; + b2Vec2 pC = b2Add( pB, r ); + draw->DrawSegment( pB, pC, c1, draw->context ); + + if ( draw->drawJointExtras ) + { + float jointAngle = b2UnwindAngle( angle - joint->referenceAngle ); + char buffer[32]; + snprintf( buffer, 32, " %.1f deg", 180.0f * jointAngle / b2_pi ); + draw->DrawString( pC, buffer, draw->context ); + } + + float lowerAngle = joint->lowerAngle + joint->referenceAngle; + float upperAngle = joint->upperAngle + joint->referenceAngle; + + if ( joint->enableLimit ) + { + b2Rot rotLo = b2MakeRot( lowerAngle ); + b2Vec2 rlo = { L * rotLo.c, L * rotLo.s }; + + b2Rot rotHi = b2MakeRot( upperAngle ); + b2Vec2 rhi = { L * rotHi.c, L * rotHi.s }; + + draw->DrawSegment( pB, b2Add( pB, rlo ), c2, draw->context ); + draw->DrawSegment( pB, b2Add( pB, rhi ), c3, draw->context ); + + b2Rot rotRef = b2MakeRot( joint->referenceAngle ); + b2Vec2 ref = ( b2Vec2 ){ L * rotRef.c, L * rotRef.s }; + draw->DrawSegment( pB, b2Add( pB, ref ), b2_colorBlue, draw->context ); + } + + b2HexColor color = b2_colorGold; + draw->DrawSegment( transformA.p, pA, color, draw->context ); + draw->DrawSegment( pA, pB, color, draw->context ); + draw->DrawSegment( transformB.p, pB, color, draw->context ); + + // char buffer[32]; + // sprintf(buffer, "%.1f", b2Length(joint->impulse)); + // draw->DrawString(pA, buffer, draw->context); +} diff --git a/3rdparty/box2d/src/rope/b2_rope.cpp b/3rdparty/box2d/src/rope/b2_rope.cpp deleted file mode 100644 index 71fbe79ee9f7..000000000000 --- a/3rdparty/box2d/src/rope/b2_rope.cpp +++ /dev/null @@ -1,809 +0,0 @@ -// MIT License - -// Copyright (c) 2019 Erin Catto - -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: - -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. - -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -#include "box2d/b2_draw.h" -#include "box2d/b2_rope.h" - -#include - -struct b2RopeStretch -{ - int32 i1, i2; - float invMass1, invMass2; - float L; - float lambda; - float spring; - float damper; -}; - -struct b2RopeBend -{ - int32 i1, i2, i3; - float invMass1, invMass2, invMass3; - float invEffectiveMass; - float lambda; - float L1, L2; - float alpha1, alpha2; - float spring; - float damper; -}; - -b2Rope::b2Rope() -{ - m_position.SetZero(); - m_count = 0; - m_stretchCount = 0; - m_bendCount = 0; - m_stretchConstraints = nullptr; - m_bendConstraints = nullptr; - m_bindPositions = nullptr; - m_ps = nullptr; - m_p0s = nullptr; - m_vs = nullptr; - m_invMasses = nullptr; - m_gravity.SetZero(); -} - -b2Rope::~b2Rope() -{ - b2Free(m_stretchConstraints); - b2Free(m_bendConstraints); - b2Free(m_bindPositions); - b2Free(m_ps); - b2Free(m_p0s); - b2Free(m_vs); - b2Free(m_invMasses); -} - -void b2Rope::Create(const b2RopeDef& def) -{ - b2Assert(def.count >= 3); - m_position = def.position; - m_count = def.count; - m_bindPositions = (b2Vec2*)b2Alloc(m_count * sizeof(b2Vec2)); - m_ps = (b2Vec2*)b2Alloc(m_count * sizeof(b2Vec2)); - m_p0s = (b2Vec2*)b2Alloc(m_count * sizeof(b2Vec2)); - m_vs = (b2Vec2*)b2Alloc(m_count * sizeof(b2Vec2)); - m_invMasses = (float*)b2Alloc(m_count * sizeof(float)); - - for (int32 i = 0; i < m_count; ++i) - { - m_bindPositions[i] = def.vertices[i]; - m_ps[i] = def.vertices[i] + m_position; - m_p0s[i] = def.vertices[i] + m_position; - m_vs[i].SetZero(); - - float m = def.masses[i]; - if (m > 0.0f) - { - m_invMasses[i] = 1.0f / m; - } - else - { - m_invMasses[i] = 0.0f; - } - } - - m_stretchCount = m_count - 1; - m_bendCount = m_count - 2; - - m_stretchConstraints = (b2RopeStretch*)b2Alloc(m_stretchCount * sizeof(b2RopeStretch)); - m_bendConstraints = (b2RopeBend*)b2Alloc(m_bendCount * sizeof(b2RopeBend)); - - for (int32 i = 0; i < m_stretchCount; ++i) - { - b2RopeStretch& c = m_stretchConstraints[i]; - - b2Vec2 p1 = m_ps[i]; - b2Vec2 p2 = m_ps[i+1]; - - c.i1 = i; - c.i2 = i + 1; - c.L = b2Distance(p1, p2); - c.invMass1 = m_invMasses[i]; - c.invMass2 = m_invMasses[i + 1]; - c.lambda = 0.0f; - c.damper = 0.0f; - c.spring = 0.0f; - } - - for (int32 i = 0; i < m_bendCount; ++i) - { - b2RopeBend& c = m_bendConstraints[i]; - - b2Vec2 p1 = m_ps[i]; - b2Vec2 p2 = m_ps[i + 1]; - b2Vec2 p3 = m_ps[i + 2]; - - c.i1 = i; - c.i2 = i + 1; - c.i3 = i + 2; - c.invMass1 = m_invMasses[i]; - c.invMass2 = m_invMasses[i + 1]; - c.invMass3 = m_invMasses[i + 2]; - c.invEffectiveMass = 0.0f; - c.L1 = b2Distance(p1, p2); - c.L2 = b2Distance(p2, p3); - c.lambda = 0.0f; - - // Pre-compute effective mass (TODO use flattened config) - b2Vec2 e1 = p2 - p1; - b2Vec2 e2 = p3 - p2; - float L1sqr = e1.LengthSquared(); - float L2sqr = e2.LengthSquared(); - - if (L1sqr * L2sqr == 0.0f) - { - continue; - } - - b2Vec2 Jd1 = (-1.0f / L1sqr) * e1.Skew(); - b2Vec2 Jd2 = (1.0f / L2sqr) * e2.Skew(); - - b2Vec2 J1 = -Jd1; - b2Vec2 J2 = Jd1 - Jd2; - b2Vec2 J3 = Jd2; - - c.invEffectiveMass = c.invMass1 * b2Dot(J1, J1) + c.invMass2 * b2Dot(J2, J2) + c.invMass3 * b2Dot(J3, J3); - - b2Vec2 r = p3 - p1; - - float rr = r.LengthSquared(); - if (rr == 0.0f) - { - continue; - } - - // a1 = h2 / (h1 + h2) - // a2 = h1 / (h1 + h2) - c.alpha1 = b2Dot(e2, r) / rr; - c.alpha2 = b2Dot(e1, r) / rr; - } - - m_gravity = def.gravity; - - SetTuning(def.tuning); -} - -void b2Rope::SetTuning(const b2RopeTuning& tuning) -{ - m_tuning = tuning; - - // Pre-compute spring and damper values based on tuning - - const float bendOmega = 2.0f * b2_pi * m_tuning.bendHertz; - - for (int32 i = 0; i < m_bendCount; ++i) - { - b2RopeBend& c = m_bendConstraints[i]; - - float L1sqr = c.L1 * c.L1; - float L2sqr = c.L2 * c.L2; - - if (L1sqr * L2sqr == 0.0f) - { - c.spring = 0.0f; - c.damper = 0.0f; - continue; - } - - // Flatten the triangle formed by the two edges - float J2 = 1.0f / c.L1 + 1.0f / c.L2; - float sum = c.invMass1 / L1sqr + c.invMass2 * J2 * J2 + c.invMass3 / L2sqr; - if (sum == 0.0f) - { - c.spring = 0.0f; - c.damper = 0.0f; - continue; - } - - float mass = 1.0f / sum; - - c.spring = mass * bendOmega * bendOmega; - c.damper = 2.0f * mass * m_tuning.bendDamping * bendOmega; - } - - const float stretchOmega = 2.0f * b2_pi * m_tuning.stretchHertz; - - for (int32 i = 0; i < m_stretchCount; ++i) - { - b2RopeStretch& c = m_stretchConstraints[i]; - - float sum = c.invMass1 + c.invMass2; - if (sum == 0.0f) - { - continue; - } - - float mass = 1.0f / sum; - - c.spring = mass * stretchOmega * stretchOmega; - c.damper = 2.0f * mass * m_tuning.stretchDamping * stretchOmega; - } -} - -void b2Rope::Step(float dt, int32 iterations, const b2Vec2& position) -{ - if (dt == 0.0f) - { - return; - } - - const float inv_dt = 1.0f / dt; - float d = expf(- dt * m_tuning.damping); - - // Apply gravity and damping - for (int32 i = 0; i < m_count; ++i) - { - if (m_invMasses[i] > 0.0f) - { - m_vs[i] *= d; - m_vs[i] += dt * m_gravity; - } - else - { - m_vs[i] = inv_dt * (m_bindPositions[i] + position - m_p0s[i]); - } - } - - // Apply bending spring - if (m_tuning.bendingModel == b2_springAngleBendingModel) - { - ApplyBendForces(dt); - } - - for (int32 i = 0; i < m_bendCount; ++i) - { - m_bendConstraints[i].lambda = 0.0f; - } - - for (int32 i = 0; i < m_stretchCount; ++i) - { - m_stretchConstraints[i].lambda = 0.0f; - } - - // Update position - for (int32 i = 0; i < m_count; ++i) - { - m_ps[i] += dt * m_vs[i]; - } - - // Solve constraints - for (int32 i = 0; i < iterations; ++i) - { - if (m_tuning.bendingModel == b2_pbdAngleBendingModel) - { - SolveBend_PBD_Angle(); - } - else if (m_tuning.bendingModel == b2_xpbdAngleBendingModel) - { - SolveBend_XPBD_Angle(dt); - } - else if (m_tuning.bendingModel == b2_pbdDistanceBendingModel) - { - SolveBend_PBD_Distance(); - } - else if (m_tuning.bendingModel == b2_pbdHeightBendingModel) - { - SolveBend_PBD_Height(); - } - else if (m_tuning.bendingModel == b2_pbdTriangleBendingModel) - { - SolveBend_PBD_Triangle(); - } - - if (m_tuning.stretchingModel == b2_pbdStretchingModel) - { - SolveStretch_PBD(); - } - else if (m_tuning.stretchingModel == b2_xpbdStretchingModel) - { - SolveStretch_XPBD(dt); - } - } - - // Constrain velocity - for (int32 i = 0; i < m_count; ++i) - { - m_vs[i] = inv_dt * (m_ps[i] - m_p0s[i]); - m_p0s[i] = m_ps[i]; - } -} - -void b2Rope::Reset(const b2Vec2& position) -{ - m_position = position; - - for (int32 i = 0; i < m_count; ++i) - { - m_ps[i] = m_bindPositions[i] + m_position; - m_p0s[i] = m_bindPositions[i] + m_position; - m_vs[i].SetZero(); - } - - for (int32 i = 0; i < m_bendCount; ++i) - { - m_bendConstraints[i].lambda = 0.0f; - } - - for (int32 i = 0; i < m_stretchCount; ++i) - { - m_stretchConstraints[i].lambda = 0.0f; - } -} - -void b2Rope::SolveStretch_PBD() -{ - const float stiffness = m_tuning.stretchStiffness; - - for (int32 i = 0; i < m_stretchCount; ++i) - { - const b2RopeStretch& c = m_stretchConstraints[i]; - - b2Vec2 p1 = m_ps[c.i1]; - b2Vec2 p2 = m_ps[c.i2]; - - b2Vec2 d = p2 - p1; - float L = d.Normalize(); - - float sum = c.invMass1 + c.invMass2; - if (sum == 0.0f) - { - continue; - } - - float s1 = c.invMass1 / sum; - float s2 = c.invMass2 / sum; - - p1 -= stiffness * s1 * (c.L - L) * d; - p2 += stiffness * s2 * (c.L - L) * d; - - m_ps[c.i1] = p1; - m_ps[c.i2] = p2; - } -} - -void b2Rope::SolveStretch_XPBD(float dt) -{ - b2Assert(dt > 0.0f); - - for (int32 i = 0; i < m_stretchCount; ++i) - { - b2RopeStretch& c = m_stretchConstraints[i]; - - b2Vec2 p1 = m_ps[c.i1]; - b2Vec2 p2 = m_ps[c.i2]; - - b2Vec2 dp1 = p1 - m_p0s[c.i1]; - b2Vec2 dp2 = p2 - m_p0s[c.i2]; - - b2Vec2 u = p2 - p1; - float L = u.Normalize(); - - b2Vec2 J1 = -u; - b2Vec2 J2 = u; - - float sum = c.invMass1 + c.invMass2; - if (sum == 0.0f) - { - continue; - } - - const float alpha = 1.0f / (c.spring * dt * dt); // 1 / kg - const float beta = dt * dt * c.damper; // kg * s - const float sigma = alpha * beta / dt; // non-dimensional - float C = L - c.L; - - // This is using the initial velocities - float Cdot = b2Dot(J1, dp1) + b2Dot(J2, dp2); - - float B = C + alpha * c.lambda + sigma * Cdot; - float sum2 = (1.0f + sigma) * sum + alpha; - - float impulse = -B / sum2; - - p1 += (c.invMass1 * impulse) * J1; - p2 += (c.invMass2 * impulse) * J2; - - m_ps[c.i1] = p1; - m_ps[c.i2] = p2; - c.lambda += impulse; - } -} - -void b2Rope::SolveBend_PBD_Angle() -{ - const float stiffness = m_tuning.bendStiffness; - - for (int32 i = 0; i < m_bendCount; ++i) - { - const b2RopeBend& c = m_bendConstraints[i]; - - b2Vec2 p1 = m_ps[c.i1]; - b2Vec2 p2 = m_ps[c.i2]; - b2Vec2 p3 = m_ps[c.i3]; - - b2Vec2 d1 = p2 - p1; - b2Vec2 d2 = p3 - p2; - float a = b2Cross(d1, d2); - float b = b2Dot(d1, d2); - - float angle = b2Atan2(a, b); - - float L1sqr, L2sqr; - - if (m_tuning.isometric) - { - L1sqr = c.L1 * c.L1; - L2sqr = c.L2 * c.L2; - } - else - { - L1sqr = d1.LengthSquared(); - L2sqr = d2.LengthSquared(); - } - - if (L1sqr * L2sqr == 0.0f) - { - continue; - } - - b2Vec2 Jd1 = (-1.0f / L1sqr) * d1.Skew(); - b2Vec2 Jd2 = (1.0f / L2sqr) * d2.Skew(); - - b2Vec2 J1 = -Jd1; - b2Vec2 J2 = Jd1 - Jd2; - b2Vec2 J3 = Jd2; - - float sum; - if (m_tuning.fixedEffectiveMass) - { - sum = c.invEffectiveMass; - } - else - { - sum = c.invMass1 * b2Dot(J1, J1) + c.invMass2 * b2Dot(J2, J2) + c.invMass3 * b2Dot(J3, J3); - } - - if (sum == 0.0f) - { - sum = c.invEffectiveMass; - } - - float impulse = -stiffness * angle / sum; - - p1 += (c.invMass1 * impulse) * J1; - p2 += (c.invMass2 * impulse) * J2; - p3 += (c.invMass3 * impulse) * J3; - - m_ps[c.i1] = p1; - m_ps[c.i2] = p2; - m_ps[c.i3] = p3; - } -} - -void b2Rope::SolveBend_XPBD_Angle(float dt) -{ - b2Assert(dt > 0.0f); - - for (int32 i = 0; i < m_bendCount; ++i) - { - b2RopeBend& c = m_bendConstraints[i]; - - b2Vec2 p1 = m_ps[c.i1]; - b2Vec2 p2 = m_ps[c.i2]; - b2Vec2 p3 = m_ps[c.i3]; - - b2Vec2 dp1 = p1 - m_p0s[c.i1]; - b2Vec2 dp2 = p2 - m_p0s[c.i2]; - b2Vec2 dp3 = p3 - m_p0s[c.i3]; - - b2Vec2 d1 = p2 - p1; - b2Vec2 d2 = p3 - p2; - - float L1sqr, L2sqr; - - if (m_tuning.isometric) - { - L1sqr = c.L1 * c.L1; - L2sqr = c.L2 * c.L2; - } - else - { - L1sqr = d1.LengthSquared(); - L2sqr = d2.LengthSquared(); - } - - if (L1sqr * L2sqr == 0.0f) - { - continue; - } - - float a = b2Cross(d1, d2); - float b = b2Dot(d1, d2); - - float angle = b2Atan2(a, b); - - b2Vec2 Jd1 = (-1.0f / L1sqr) * d1.Skew(); - b2Vec2 Jd2 = (1.0f / L2sqr) * d2.Skew(); - - b2Vec2 J1 = -Jd1; - b2Vec2 J2 = Jd1 - Jd2; - b2Vec2 J3 = Jd2; - - float sum; - if (m_tuning.fixedEffectiveMass) - { - sum = c.invEffectiveMass; - } - else - { - sum = c.invMass1 * b2Dot(J1, J1) + c.invMass2 * b2Dot(J2, J2) + c.invMass3 * b2Dot(J3, J3); - } - - if (sum == 0.0f) - { - continue; - } - - const float alpha = 1.0f / (c.spring * dt * dt); - const float beta = dt * dt * c.damper; - const float sigma = alpha * beta / dt; - float C = angle; - - // This is using the initial velocities - float Cdot = b2Dot(J1, dp1) + b2Dot(J2, dp2) + b2Dot(J3, dp3); - - float B = C + alpha * c.lambda + sigma * Cdot; - float sum2 = (1.0f + sigma) * sum + alpha; - - float impulse = -B / sum2; - - p1 += (c.invMass1 * impulse) * J1; - p2 += (c.invMass2 * impulse) * J2; - p3 += (c.invMass3 * impulse) * J3; - - m_ps[c.i1] = p1; - m_ps[c.i2] = p2; - m_ps[c.i3] = p3; - c.lambda += impulse; - } -} - -void b2Rope::ApplyBendForces(float dt) -{ - // omega = 2 * pi * hz - const float omega = 2.0f * b2_pi * m_tuning.bendHertz; - - for (int32 i = 0; i < m_bendCount; ++i) - { - const b2RopeBend& c = m_bendConstraints[i]; - - b2Vec2 p1 = m_ps[c.i1]; - b2Vec2 p2 = m_ps[c.i2]; - b2Vec2 p3 = m_ps[c.i3]; - - b2Vec2 v1 = m_vs[c.i1]; - b2Vec2 v2 = m_vs[c.i2]; - b2Vec2 v3 = m_vs[c.i3]; - - b2Vec2 d1 = p2 - p1; - b2Vec2 d2 = p3 - p2; - - float L1sqr, L2sqr; - - if (m_tuning.isometric) - { - L1sqr = c.L1 * c.L1; - L2sqr = c.L2 * c.L2; - } - else - { - L1sqr = d1.LengthSquared(); - L2sqr = d2.LengthSquared(); - } - - if (L1sqr * L2sqr == 0.0f) - { - continue; - } - - float a = b2Cross(d1, d2); - float b = b2Dot(d1, d2); - - float angle = b2Atan2(a, b); - - b2Vec2 Jd1 = (-1.0f / L1sqr) * d1.Skew(); - b2Vec2 Jd2 = (1.0f / L2sqr) * d2.Skew(); - - b2Vec2 J1 = -Jd1; - b2Vec2 J2 = Jd1 - Jd2; - b2Vec2 J3 = Jd2; - - float sum; - if (m_tuning.fixedEffectiveMass) - { - sum = c.invEffectiveMass; - } - else - { - sum = c.invMass1 * b2Dot(J1, J1) + c.invMass2 * b2Dot(J2, J2) + c.invMass3 * b2Dot(J3, J3); - } - - if (sum == 0.0f) - { - continue; - } - - float mass = 1.0f / sum; - - const float spring = mass * omega * omega; - const float damper = 2.0f * mass * m_tuning.bendDamping * omega; - - float C = angle; - float Cdot = b2Dot(J1, v1) + b2Dot(J2, v2) + b2Dot(J3, v3); - - float impulse = -dt * (spring * C + damper * Cdot); - - m_vs[c.i1] += (c.invMass1 * impulse) * J1; - m_vs[c.i2] += (c.invMass2 * impulse) * J2; - m_vs[c.i3] += (c.invMass3 * impulse) * J3; - } -} - -void b2Rope::SolveBend_PBD_Distance() -{ - const float stiffness = m_tuning.bendStiffness; - - for (int32 i = 0; i < m_bendCount; ++i) - { - const b2RopeBend& c = m_bendConstraints[i]; - - int32 i1 = c.i1; - int32 i2 = c.i3; - - b2Vec2 p1 = m_ps[i1]; - b2Vec2 p2 = m_ps[i2]; - - b2Vec2 d = p2 - p1; - float L = d.Normalize(); - - float sum = c.invMass1 + c.invMass3; - if (sum == 0.0f) - { - continue; - } - - float s1 = c.invMass1 / sum; - float s2 = c.invMass3 / sum; - - p1 -= stiffness * s1 * (c.L1 + c.L2 - L) * d; - p2 += stiffness * s2 * (c.L1 + c.L2 - L) * d; - - m_ps[i1] = p1; - m_ps[i2] = p2; - } -} - -// Constraint based implementation of: -// P. Volino: Simple Linear Bending Stiffness in Particle Systems -void b2Rope::SolveBend_PBD_Height() -{ - const float stiffness = m_tuning.bendStiffness; - - for (int32 i = 0; i < m_bendCount; ++i) - { - const b2RopeBend& c = m_bendConstraints[i]; - - b2Vec2 p1 = m_ps[c.i1]; - b2Vec2 p2 = m_ps[c.i2]; - b2Vec2 p3 = m_ps[c.i3]; - - // Barycentric coordinates are held constant - b2Vec2 d = c.alpha1 * p1 + c.alpha2 * p3 - p2; - float dLen = d.Length(); - - if (dLen == 0.0f) - { - continue; - } - - b2Vec2 dHat = (1.0f / dLen) * d; - - b2Vec2 J1 = c.alpha1 * dHat; - b2Vec2 J2 = -dHat; - b2Vec2 J3 = c.alpha2 * dHat; - - float sum = c.invMass1 * c.alpha1 * c.alpha1 + c.invMass2 + c.invMass3 * c.alpha2 * c.alpha2; - - if (sum == 0.0f) - { - continue; - } - - float C = dLen; - float mass = 1.0f / sum; - float impulse = -stiffness * mass * C; - - p1 += (c.invMass1 * impulse) * J1; - p2 += (c.invMass2 * impulse) * J2; - p3 += (c.invMass3 * impulse) * J3; - - m_ps[c.i1] = p1; - m_ps[c.i2] = p2; - m_ps[c.i3] = p3; - } -} - -// M. Kelager: A Triangle Bending Constraint Model for PBD -void b2Rope::SolveBend_PBD_Triangle() -{ - const float stiffness = m_tuning.bendStiffness; - - for (int32 i = 0; i < m_bendCount; ++i) - { - const b2RopeBend& c = m_bendConstraints[i]; - - b2Vec2 b0 = m_ps[c.i1]; - b2Vec2 v = m_ps[c.i2]; - b2Vec2 b1 = m_ps[c.i3]; - - float wb0 = c.invMass1; - float wv = c.invMass2; - float wb1 = c.invMass3; - - float W = wb0 + wb1 + 2.0f * wv; - float invW = stiffness / W; - - b2Vec2 d = v - (1.0f / 3.0f) * (b0 + v + b1); - - b2Vec2 db0 = 2.0f * wb0 * invW * d; - b2Vec2 dv = -4.0f * wv * invW * d; - b2Vec2 db1 = 2.0f * wb1 * invW * d; - - b0 += db0; - v += dv; - b1 += db1; - - m_ps[c.i1] = b0; - m_ps[c.i2] = v; - m_ps[c.i3] = b1; - } -} - -void b2Rope::Draw(b2Draw* draw) const -{ - b2Color c(0.4f, 0.5f, 0.7f); - b2Color pg(0.1f, 0.8f, 0.1f); - b2Color pd(0.7f, 0.2f, 0.4f); - - for (int32 i = 0; i < m_count - 1; ++i) - { - draw->DrawSegment(m_ps[i], m_ps[i+1], c); - - const b2Color& pc = m_invMasses[i] > 0.0f ? pd : pg; - draw->DrawPoint(m_ps[i], 5.0f, pc); - } - - const b2Color& pc = m_invMasses[m_count - 1] > 0.0f ? pd : pg; - draw->DrawPoint(m_ps[m_count - 1], 5.0f, pc); -} diff --git a/3rdparty/box2d/src/shape.c b/3rdparty/box2d/src/shape.c new file mode 100644 index 000000000000..9d4cb8bace01 --- /dev/null +++ b/3rdparty/box2d/src/shape.c @@ -0,0 +1,1370 @@ +// SPDX-FileCopyrightText: 2023 Erin Catto +// SPDX-License-Identifier: MIT + +#include "shape.h" + +#include "body.h" +#include "broad_phase.h" +#include "contact.h" +#include "world.h" + +// needed for dll export +#include "box2d/box2d.h" + +#include + +B2_ARRAY_SOURCE( b2ChainShape, b2ChainShape ); +B2_ARRAY_SOURCE( b2Shape, b2Shape ); + +static b2Shape* b2GetShape( b2World* world, b2ShapeId shapeId ) +{ + int id = shapeId.index1 - 1; + b2Shape* shape = b2ShapeArray_Get( &world->shapes, id ); + B2_ASSERT( shape->id == id && shape->revision == shapeId.revision ); + return shape; +} + +static b2ChainShape* b2GetChainShape( b2World* world, b2ChainId chainId ) +{ + int id = chainId.index1 - 1; + b2ChainShape* chain = b2ChainShapeArray_Get( &world->chainShapes, id ); + B2_ASSERT( chain->id == id && chain->revision == chainId.revision ); + return chain; +} + +static void b2UpdateShapeAABBs( b2Shape* shape, b2Transform transform, b2BodyType proxyType ) +{ + // Compute a bounding box with a speculative margin + const float speculativeDistance = b2_speculativeDistance; + const float aabbMargin = b2_aabbMargin; + + b2AABB aabb = b2ComputeShapeAABB( shape, transform ); + aabb.lowerBound.x -= speculativeDistance; + aabb.lowerBound.y -= speculativeDistance; + aabb.upperBound.x += speculativeDistance; + aabb.upperBound.y += speculativeDistance; + shape->aabb = aabb; + + // Smaller margin for static bodies. Cannot be zero due to TOI tolerance. + float margin = proxyType == b2_staticBody ? speculativeDistance : aabbMargin; + b2AABB fatAABB; + fatAABB.lowerBound.x = aabb.lowerBound.x - margin; + fatAABB.lowerBound.y = aabb.lowerBound.y - margin; + fatAABB.upperBound.x = aabb.upperBound.x + margin; + fatAABB.upperBound.y = aabb.upperBound.y + margin; + shape->fatAABB = fatAABB; +} + +static b2Shape* b2CreateShapeInternal( b2World* world, b2Body* body, b2Transform transform, const b2ShapeDef* def, + const void* geometry, b2ShapeType shapeType ) +{ + B2_ASSERT( b2IsValid( def->density ) && def->density >= 0.0f ); + B2_ASSERT( b2IsValid( def->friction ) && def->friction >= 0.0f ); + B2_ASSERT( b2IsValid( def->restitution ) && def->restitution >= 0.0f ); + + int shapeId = b2AllocId( &world->shapeIdPool ); + + if ( shapeId == world->shapes.count ) + { + b2ShapeArray_Push( &world->shapes, ( b2Shape ){ 0 } ); + } + else + { + B2_ASSERT( world->shapes.data[shapeId].id == B2_NULL_INDEX ); + } + + b2Shape* shape = b2ShapeArray_Get( &world->shapes, shapeId ); + + switch ( shapeType ) + { + case b2_capsuleShape: + shape->capsule = *(const b2Capsule*)geometry; + break; + + case b2_circleShape: + shape->circle = *(const b2Circle*)geometry; + break; + + case b2_polygonShape: + shape->polygon = *(const b2Polygon*)geometry; + break; + + case b2_segmentShape: + shape->segment = *(const b2Segment*)geometry; + break; + + case b2_chainSegmentShape: + shape->chainSegment = *(const b2ChainSegment*)geometry; + break; + + default: + B2_ASSERT( false ); + break; + } + + shape->id = shapeId; + shape->bodyId = body->id; + shape->type = shapeType; + shape->density = def->density; + shape->friction = def->friction; + shape->restitution = def->restitution; + shape->filter = def->filter; + shape->userData = def->userData; + shape->customColor = def->customColor; + shape->isSensor = def->isSensor; + shape->enlargedAABB = false; + shape->enableSensorEvents = def->enableSensorEvents; + shape->enableContactEvents = def->enableContactEvents; + shape->enableHitEvents = def->enableHitEvents; + shape->enablePreSolveEvents = def->enablePreSolveEvents; + shape->isFast = false; + shape->proxyKey = B2_NULL_INDEX; + shape->localCentroid = b2GetShapeCentroid( shape ); + shape->aabb = ( b2AABB ){ b2Vec2_zero, b2Vec2_zero }; + shape->fatAABB = ( b2AABB ){ b2Vec2_zero, b2Vec2_zero }; + shape->revision += 1; + + if ( body->setIndex != b2_disabledSet ) + { + b2BodyType proxyType = body->type; + b2CreateShapeProxy( shape, &world->broadPhase, proxyType, transform, def->forceContactCreation || def->isSensor ); + } + + // Add to shape doubly linked list + if ( body->headShapeId != B2_NULL_INDEX ) + { + b2Shape* headShape = b2ShapeArray_Get( &world->shapes, body->headShapeId ); + headShape->prevShapeId = shapeId; + } + + shape->prevShapeId = B2_NULL_INDEX; + shape->nextShapeId = body->headShapeId; + body->headShapeId = shapeId; + body->shapeCount += 1; + + b2ValidateSolverSets( world ); + + return shape; +} + +b2ShapeId b2CreateShape( b2BodyId bodyId, const b2ShapeDef* def, const void* geometry, b2ShapeType shapeType ) +{ + b2CheckDef( def ); + B2_ASSERT( b2IsValid( def->density ) && def->density >= 0.0f ); + B2_ASSERT( b2IsValid( def->friction ) && def->friction >= 0.0f ); + B2_ASSERT( b2IsValid( def->restitution ) && def->restitution >= 0.0f ); + + b2World* world = b2GetWorldLocked( bodyId.world0 ); + if ( world == NULL ) + { + return ( b2ShapeId ){ 0 }; + } + + b2Body* body = b2GetBodyFullId( world, bodyId ); + b2Transform transform = b2GetBodyTransformQuick( world, body ); + + b2Shape* shape = b2CreateShapeInternal( world, body, transform, def, geometry, shapeType ); + + if ( body->automaticMass == true ) + { + b2UpdateBodyMassData( world, body ); + } + + b2ValidateSolverSets( world ); + + b2ShapeId id = { shape->id + 1, bodyId.world0, shape->revision }; + return id; +} + +b2ShapeId b2CreateCircleShape( b2BodyId bodyId, const b2ShapeDef* def, const b2Circle* circle ) +{ + return b2CreateShape( bodyId, def, circle, b2_circleShape ); +} + +b2ShapeId b2CreateCapsuleShape( b2BodyId bodyId, const b2ShapeDef* def, const b2Capsule* capsule ) +{ + float lengthSqr = b2DistanceSquared( capsule->center1, capsule->center2 ); + if ( lengthSqr <= b2_linearSlop * b2_linearSlop ) + { + b2Circle circle = { b2Lerp( capsule->center1, capsule->center2, 0.5f ), capsule->radius }; + return b2CreateShape( bodyId, def, &circle, b2_circleShape ); + } + + return b2CreateShape( bodyId, def, capsule, b2_capsuleShape ); +} + +b2ShapeId b2CreatePolygonShape( b2BodyId bodyId, const b2ShapeDef* def, const b2Polygon* polygon ) +{ + B2_ASSERT( b2IsValid( polygon->radius ) && polygon->radius >= 0.0f ); + return b2CreateShape( bodyId, def, polygon, b2_polygonShape ); +} + +b2ShapeId b2CreateSegmentShape( b2BodyId bodyId, const b2ShapeDef* def, const b2Segment* segment ) +{ + float lengthSqr = b2DistanceSquared( segment->point1, segment->point2 ); + if ( lengthSqr <= b2_linearSlop * b2_linearSlop ) + { + B2_ASSERT( false ); + return b2_nullShapeId; + } + + return b2CreateShape( bodyId, def, segment, b2_segmentShape ); +} + +// Destroy a shape on a body. This doesn't need to be called when destroying a body. +void b2DestroyShapeInternal( b2World* world, b2Shape* shape, b2Body* body, bool wakeBodies ) +{ + int shapeId = shape->id; + + // Remove the shape from the body's doubly linked list. + if ( shape->prevShapeId != B2_NULL_INDEX ) + { + b2Shape* prevShape = b2ShapeArray_Get( &world->shapes, shape->prevShapeId ); + prevShape->nextShapeId = shape->nextShapeId; + } + + if ( shape->nextShapeId != B2_NULL_INDEX ) + { + b2Shape* nextShape = b2ShapeArray_Get( &world->shapes, shape->nextShapeId ); + nextShape->prevShapeId = shape->prevShapeId; + } + + if ( shapeId == body->headShapeId ) + { + body->headShapeId = shape->nextShapeId; + } + + body->shapeCount -= 1; + + // Remove from broad-phase. + b2DestroyShapeProxy( shape, &world->broadPhase ); + + // Destroy any contacts associated with the shape. + int contactKey = body->headContactKey; + while ( contactKey != B2_NULL_INDEX ) + { + int contactId = contactKey >> 1; + int edgeIndex = contactKey & 1; + + b2Contact* contact = b2ContactArray_Get( &world->contacts, contactId ); + contactKey = contact->edges[edgeIndex].nextKey; + + if ( contact->shapeIdA == shapeId || contact->shapeIdB == shapeId ) + { + b2DestroyContact( world, contact, wakeBodies ); + } + } + + // Return shape to free list. + b2FreeId( &world->shapeIdPool, shapeId ); + shape->id = B2_NULL_INDEX; + + b2ValidateSolverSets( world ); +} + +void b2DestroyShape( b2ShapeId shapeId ) +{ + b2World* world = b2GetWorldLocked( shapeId.world0 ); + + b2Shape* shape = b2GetShape( world, shapeId ); + + // need to wake bodies because this might be a static body + bool wakeBodies = true; + + b2Body* body = b2BodyArray_Get( &world->bodies, shape->bodyId ); + b2DestroyShapeInternal( world, shape, body, wakeBodies ); + + if ( body->automaticMass == true ) + { + b2UpdateBodyMassData( world, body ); + } +} + +b2ChainId b2CreateChain( b2BodyId bodyId, const b2ChainDef* def ) +{ + b2CheckDef( def ); + B2_ASSERT( b2IsValid( def->friction ) && def->friction >= 0.0f ); + B2_ASSERT( b2IsValid( def->restitution ) && def->restitution >= 0.0f ); + B2_ASSERT( def->count >= 4 ); + + b2World* world = b2GetWorldLocked( bodyId.world0 ); + if ( world == NULL ) + { + return ( b2ChainId ){ 0 }; + } + + b2Body* body = b2GetBodyFullId( world, bodyId ); + b2Transform transform = b2GetBodyTransformQuick( world, body ); + + int chainId = b2AllocId( &world->chainIdPool ); + + if ( chainId == world->chainShapes.count ) + { + b2ChainShapeArray_Push( &world->chainShapes, ( b2ChainShape ){ 0 } ); + } + else + { + B2_ASSERT( world->chainShapes.data[chainId].id == B2_NULL_INDEX ); + } + + b2ChainShape* chainShape = b2ChainShapeArray_Get( &world->chainShapes, chainId ); + + chainShape->id = chainId; + chainShape->bodyId = body->id; + chainShape->nextChainId = body->headChainId; + chainShape->revision += 1; + body->headChainId = chainId; + + b2ShapeDef shapeDef = b2DefaultShapeDef(); + shapeDef.userData = def->userData; + shapeDef.restitution = def->restitution; + shapeDef.friction = def->friction; + shapeDef.filter = def->filter; + shapeDef.enableContactEvents = false; + shapeDef.enableHitEvents = false; + shapeDef.enableSensorEvents = false; + + int n = def->count; + const b2Vec2* points = def->points; + + if ( def->isLoop ) + { + chainShape->count = n; + chainShape->shapeIndices = b2Alloc( n * sizeof( int ) ); + + b2ChainSegment chainSegment; + + int prevIndex = n - 1; + for ( int i = 0; i < n - 2; ++i ) + { + chainSegment.ghost1 = points[prevIndex]; + chainSegment.segment.point1 = points[i]; + chainSegment.segment.point2 = points[i + 1]; + chainSegment.ghost2 = points[i + 2]; + chainSegment.chainId = chainId; + prevIndex = i; + + b2Shape* shape = b2CreateShapeInternal( world, body, transform, &shapeDef, &chainSegment, b2_chainSegmentShape ); + chainShape->shapeIndices[i] = shape->id; + } + + { + chainSegment.ghost1 = points[n - 3]; + chainSegment.segment.point1 = points[n - 2]; + chainSegment.segment.point2 = points[n - 1]; + chainSegment.ghost2 = points[0]; + chainSegment.chainId = chainId; + b2Shape* shape = b2CreateShapeInternal( world, body, transform, &shapeDef, &chainSegment, b2_chainSegmentShape ); + chainShape->shapeIndices[n - 2] = shape->id; + } + + { + chainSegment.ghost1 = points[n - 2]; + chainSegment.segment.point1 = points[n - 1]; + chainSegment.segment.point2 = points[0]; + chainSegment.ghost2 = points[1]; + chainSegment.chainId = chainId; + b2Shape* shape = b2CreateShapeInternal( world, body, transform, &shapeDef, &chainSegment, b2_chainSegmentShape ); + chainShape->shapeIndices[n - 1] = shape->id; + } + } + else + { + chainShape->count = n - 3; + chainShape->shapeIndices = b2Alloc( n * sizeof( int ) ); + + b2ChainSegment chainSegment; + + for ( int i = 0; i < n - 3; ++i ) + { + chainSegment.ghost1 = points[i]; + chainSegment.segment.point1 = points[i + 1]; + chainSegment.segment.point2 = points[i + 2]; + chainSegment.ghost2 = points[i + 3]; + chainSegment.chainId = chainId; + + b2Shape* shape = b2CreateShapeInternal( world, body, transform, &shapeDef, &chainSegment, b2_chainSegmentShape ); + chainShape->shapeIndices[i] = shape->id; + } + } + + b2ChainId id = { chainId + 1, world->worldId, chainShape->revision }; + return id; +} + +void b2DestroyChain( b2ChainId chainId ) +{ + b2World* world = b2GetWorldLocked( chainId.world0 ); + + b2ChainShape* chain = b2GetChainShape(world, chainId); + bool wakeBodies = true; + + b2Body* body = b2BodyArray_Get( &world->bodies, chain->bodyId ); + + // Remove the chain from the body's singly linked list. + int* chainIdPtr = &body->headChainId; + bool found = false; + while ( *chainIdPtr != B2_NULL_INDEX ) + { + if ( *chainIdPtr == chain->id ) + { + *chainIdPtr = chain->nextChainId; + found = true; + break; + } + + chainIdPtr = &( world->chainShapes.data[*chainIdPtr].nextChainId ); + } + + B2_ASSERT( found == true ); + if ( found == false ) + { + return; + } + + int count = chain->count; + for ( int i = 0; i < count; ++i ) + { + int shapeId = chain->shapeIndices[i]; + b2Shape* shape = b2ShapeArray_Get( &world->shapes, shapeId ); + b2DestroyShapeInternal( world, shape, body, wakeBodies ); + } + + b2Free( chain->shapeIndices, chain->count * sizeof( int ) ); + chain->shapeIndices = NULL; + + // Return chain to free list. + b2FreeId( &world->chainIdPool, chain->id ); + chain->id = B2_NULL_INDEX; + + b2ValidateSolverSets( world ); +} + +b2WorldId b2Chain_GetWorld(b2ChainId chainId) +{ + b2World* world = b2GetWorld( chainId.world0 ); + return ( b2WorldId ){ chainId.world0 + 1, world->revision }; +} + +int b2Chain_GetSegmentCount(b2ChainId chainId) +{ + b2World* world = b2GetWorldLocked( chainId.world0 ); + b2ChainShape* chain = b2GetChainShape( world, chainId ); + return chain->count; +} + +int b2Chain_GetSegments(b2ChainId chainId, b2ShapeId* segmentArray, int capacity) +{ + b2World* world = b2GetWorldLocked( chainId.world0 ); + b2ChainShape* chain = b2GetChainShape( world, chainId ); + + int count = b2MinInt(chain->count, capacity); + for ( int i = 0; i < count; ++i ) + { + int shapeId = chain->shapeIndices[i]; + b2Shape* shape = b2ShapeArray_Get( &world->shapes, shapeId ); + segmentArray[i] = ( b2ShapeId ){ shapeId + 1, chainId.world0, shape->revision }; + } + + return count; +} + +b2AABB b2ComputeShapeAABB( const b2Shape* shape, b2Transform xf ) +{ + switch ( shape->type ) + { + case b2_capsuleShape: + return b2ComputeCapsuleAABB( &shape->capsule, xf ); + case b2_circleShape: + return b2ComputeCircleAABB( &shape->circle, xf ); + case b2_polygonShape: + return b2ComputePolygonAABB( &shape->polygon, xf ); + case b2_segmentShape: + return b2ComputeSegmentAABB( &shape->segment, xf ); + case b2_chainSegmentShape: + return b2ComputeSegmentAABB( &shape->chainSegment.segment, xf ); + default: + { + B2_ASSERT( false ); + b2AABB empty = { xf.p, xf.p }; + return empty; + } + } +} + +b2Vec2 b2GetShapeCentroid( const b2Shape* shape ) +{ + switch ( shape->type ) + { + case b2_capsuleShape: + return b2Lerp( shape->capsule.center1, shape->capsule.center2, 0.5f ); + case b2_circleShape: + return shape->circle.center; + case b2_polygonShape: + return shape->polygon.centroid; + case b2_segmentShape: + return b2Lerp( shape->segment.point1, shape->segment.point2, 0.5f ); + case b2_chainSegmentShape: + return b2Lerp( shape->chainSegment.segment.point1, shape->chainSegment.segment.point2, 0.5f ); + default: + return b2Vec2_zero; + } +} + +// todo maybe compute this on shape creation +float b2GetShapePerimeter( const b2Shape* shape ) +{ + switch ( shape->type ) + { + case b2_capsuleShape: + return 2.0f * b2Length( b2Sub( shape->capsule.center1, shape->capsule.center2 ) ) + + 2.0f * b2_pi * shape->capsule.radius; + case b2_circleShape: + return 2.0f * b2_pi * shape->circle.radius; + case b2_polygonShape: + { + const b2Vec2* points = shape->polygon.vertices; + int count = shape->polygon.count; + float perimeter = 2.0f * b2_pi * shape->polygon.radius; + B2_ASSERT( count > 0 ); + b2Vec2 prev = points[count - 1]; + for ( int i = 0; i < count; ++i ) + { + b2Vec2 next = points[i]; + perimeter += b2Length( b2Sub( next, prev ) ); + prev = next; + } + + return perimeter; + } + case b2_segmentShape: + return 2.0f * b2Length( b2Sub( shape->segment.point1, shape->segment.point2 ) ); + case b2_chainSegmentShape: + return 2.0f * b2Length( b2Sub( shape->chainSegment.segment.point1, shape->chainSegment.segment.point2 ) ); + default: + return 0.0f; + } +} + +b2MassData b2ComputeShapeMass( const b2Shape* shape ) +{ + switch ( shape->type ) + { + case b2_capsuleShape: + return b2ComputeCapsuleMass( &shape->capsule, shape->density ); + case b2_circleShape: + return b2ComputeCircleMass( &shape->circle, shape->density ); + case b2_polygonShape: + return b2ComputePolygonMass( &shape->polygon, shape->density ); + default: + { + return ( b2MassData ){ 0 }; + } + } +} + +b2ShapeExtent b2ComputeShapeExtent( const b2Shape* shape, b2Vec2 localCenter ) +{ + b2ShapeExtent extent = { 0 }; + + switch ( shape->type ) + { + case b2_capsuleShape: + { + float radius = shape->capsule.radius; + extent.minExtent = radius; + b2Vec2 c1 = b2Sub( shape->capsule.center1, localCenter ); + b2Vec2 c2 = b2Sub( shape->capsule.center2, localCenter ); + extent.maxExtent = sqrtf( b2MaxFloat( b2LengthSquared( c1 ), b2LengthSquared( c2 ) ) ) + radius; + } + break; + + case b2_circleShape: + { + float radius = shape->circle.radius; + extent.minExtent = radius; + extent.maxExtent = b2Length( b2Sub( shape->circle.center, localCenter ) ) + radius; + } + break; + + case b2_polygonShape: + { + const b2Polygon* poly = &shape->polygon; + float minExtent = b2_huge; + float maxExtentSqr = 0.0f; + int count = poly->count; + for ( int i = 0; i < count; ++i ) + { + b2Vec2 v = poly->vertices[i]; + float planeOffset = b2Dot( poly->normals[i], b2Sub( v, poly->centroid ) ); + minExtent = b2MinFloat( minExtent, planeOffset ); + + float distanceSqr = b2LengthSquared( b2Sub( v, localCenter ) ); + maxExtentSqr = b2MaxFloat( maxExtentSqr, distanceSqr ); + } + + extent.minExtent = minExtent + poly->radius; + extent.maxExtent = sqrtf( maxExtentSqr ) + poly->radius; + } + break; + + case b2_segmentShape: + { + extent.minExtent = 0.0f; + b2Vec2 c1 = b2Sub( shape->segment.point1, localCenter ); + b2Vec2 c2 = b2Sub( shape->segment.point2, localCenter ); + extent.maxExtent = sqrtf( b2MaxFloat( b2LengthSquared( c1 ), b2LengthSquared( c2 ) ) ); + } + break; + + case b2_chainSegmentShape: + { + extent.minExtent = 0.0f; + b2Vec2 c1 = b2Sub( shape->chainSegment.segment.point1, localCenter ); + b2Vec2 c2 = b2Sub( shape->chainSegment.segment.point2, localCenter ); + extent.maxExtent = sqrtf( b2MaxFloat( b2LengthSquared( c1 ), b2LengthSquared( c2 ) ) ); + } + break; + + default: + break; + } + + return extent; +} + +b2CastOutput b2RayCastShape( const b2RayCastInput* input, const b2Shape* shape, b2Transform transform ) +{ + b2RayCastInput localInput = *input; + localInput.origin = b2InvTransformPoint( transform, input->origin ); + localInput.translation = b2InvRotateVector( transform.q, input->translation ); + + b2CastOutput output = { 0 }; + switch ( shape->type ) + { + case b2_capsuleShape: + output = b2RayCastCapsule( &localInput, &shape->capsule ); + break; + case b2_circleShape: + output = b2RayCastCircle( &localInput, &shape->circle ); + break; + case b2_polygonShape: + output = b2RayCastPolygon( &localInput, &shape->polygon ); + break; + case b2_segmentShape: + output = b2RayCastSegment( &localInput, &shape->segment, false ); + break; + case b2_chainSegmentShape: + output = b2RayCastSegment( &localInput, &shape->chainSegment.segment, true ); + break; + default: + return output; + } + + output.point = b2TransformPoint( transform, output.point ); + output.normal = b2RotateVector( transform.q, output.normal ); + return output; +} + +b2CastOutput b2ShapeCastShape( const b2ShapeCastInput* input, const b2Shape* shape, b2Transform transform ) +{ + b2ShapeCastInput localInput = *input; + + for ( int i = 0; i < localInput.count; ++i ) + { + localInput.points[i] = b2InvTransformPoint( transform, input->points[i] ); + } + + localInput.translation = b2InvRotateVector( transform.q, input->translation ); + + b2CastOutput output = { 0 }; + switch ( shape->type ) + { + case b2_capsuleShape: + output = b2ShapeCastCapsule( &localInput, &shape->capsule ); + break; + case b2_circleShape: + output = b2ShapeCastCircle( &localInput, &shape->circle ); + break; + case b2_polygonShape: + output = b2ShapeCastPolygon( &localInput, &shape->polygon ); + break; + case b2_segmentShape: + output = b2ShapeCastSegment( &localInput, &shape->segment ); + break; + case b2_chainSegmentShape: + output = b2ShapeCastSegment( &localInput, &shape->chainSegment.segment ); + break; + default: + return output; + } + + output.point = b2TransformPoint( transform, output.point ); + output.normal = b2RotateVector( transform.q, output.normal ); + return output; +} + +void b2CreateShapeProxy( b2Shape* shape, b2BroadPhase* bp, b2BodyType type, b2Transform transform, bool forcePairCreation ) +{ + B2_ASSERT( shape->proxyKey == B2_NULL_INDEX ); + + b2UpdateShapeAABBs( shape, transform, type ); + + // Create proxies in the broad-phase. + shape->proxyKey = + b2BroadPhase_CreateProxy( bp, type, shape->fatAABB, shape->filter.categoryBits, shape->id, forcePairCreation ); + B2_ASSERT( B2_PROXY_TYPE( shape->proxyKey ) < b2_bodyTypeCount ); +} + +void b2DestroyShapeProxy( b2Shape* shape, b2BroadPhase* bp ) +{ + if ( shape->proxyKey != B2_NULL_INDEX ) + { + b2BroadPhase_DestroyProxy( bp, shape->proxyKey ); + shape->proxyKey = B2_NULL_INDEX; + } +} + +b2DistanceProxy b2MakeShapeDistanceProxy( const b2Shape* shape ) +{ + switch ( shape->type ) + { + case b2_capsuleShape: + return b2MakeProxy( &shape->capsule.center1, 2, shape->capsule.radius ); + case b2_circleShape: + return b2MakeProxy( &shape->circle.center, 1, shape->circle.radius ); + case b2_polygonShape: + return b2MakeProxy( shape->polygon.vertices, shape->polygon.count, shape->polygon.radius ); + case b2_segmentShape: + return b2MakeProxy( &shape->segment.point1, 2, 0.0f ); + case b2_chainSegmentShape: + return b2MakeProxy( &shape->chainSegment.segment.point1, 2, 0.0f ); + default: + { + B2_ASSERT( false ); + b2DistanceProxy empty = { 0 }; + return empty; + } + } +} + +b2BodyId b2Shape_GetBody( b2ShapeId shapeId ) +{ + b2World* world = b2GetWorld( shapeId.world0 ); + b2Shape* shape = b2GetShape( world, shapeId ); + return b2MakeBodyId( world, shape->bodyId ); +} + +b2WorldId b2Shape_GetWorld(b2ShapeId shapeId) +{ + b2World* world = b2GetWorld( shapeId.world0 ); + return ( b2WorldId ){ shapeId.world0 + 1, world->revision }; +} + +void b2Shape_SetUserData( b2ShapeId shapeId, void* userData ) +{ + b2World* world = b2GetWorld( shapeId.world0 ); + b2Shape* shape = b2GetShape( world, shapeId ); + shape->userData = userData; +} + +void* b2Shape_GetUserData( b2ShapeId shapeId ) +{ + b2World* world = b2GetWorld( shapeId.world0 ); + b2Shape* shape = b2GetShape( world, shapeId ); + return shape->userData; +} + +bool b2Shape_IsSensor( b2ShapeId shapeId ) +{ + b2World* world = b2GetWorld( shapeId.world0 ); + b2Shape* shape = b2GetShape( world, shapeId ); + return shape->isSensor; +} + +bool b2Shape_TestPoint( b2ShapeId shapeId, b2Vec2 point ) +{ + b2World* world = b2GetWorld( shapeId.world0 ); + b2Shape* shape = b2GetShape( world, shapeId ); + + b2Transform transform = b2GetBodyTransform( world, shape->bodyId ); + b2Vec2 localPoint = b2InvTransformPoint( transform, point ); + + switch ( shape->type ) + { + case b2_capsuleShape: + return b2PointInCapsule( localPoint, &shape->capsule ); + + case b2_circleShape: + return b2PointInCircle( localPoint, &shape->circle ); + + case b2_polygonShape: + return b2PointInPolygon( localPoint, &shape->polygon ); + + default: + return false; + } +} + +// todo_erin untested +b2CastOutput b2Shape_RayCast( b2ShapeId shapeId, const b2RayCastInput* input ) +{ + b2World* world = b2GetWorld( shapeId.world0 ); + b2Shape* shape = b2GetShape( world, shapeId ); + + b2Transform transform = b2GetBodyTransform( world, shape->bodyId ); + + // input in local coordinates + b2RayCastInput localInput; + localInput.origin = b2InvTransformPoint( transform, input->origin ); + localInput.translation = b2InvRotateVector( transform.q, input->translation ); + localInput.maxFraction = input->maxFraction; + + b2CastOutput output = { 0 }; + switch ( shape->type ) + { + case b2_capsuleShape: + output = b2RayCastCapsule( &localInput, &shape->capsule ); + break; + + case b2_circleShape: + output = b2RayCastCircle( &localInput, &shape->circle ); + break; + + case b2_segmentShape: + output = b2RayCastSegment( &localInput, &shape->segment, false ); + break; + + case b2_polygonShape: + output = b2RayCastPolygon( &localInput, &shape->polygon ); + break; + + case b2_chainSegmentShape: + output = b2RayCastSegment( &localInput, &shape->chainSegment.segment, true ); + break; + + default: + B2_ASSERT( false ); + return output; + } + + if ( output.hit ) + { + // convert to world coordinates + output.normal = b2RotateVector( transform.q, output.normal ); + output.point = b2TransformPoint( transform, output.point ); + } + + return output; +} + +void b2Shape_SetDensity( b2ShapeId shapeId, float density ) +{ + B2_ASSERT( b2IsValid( density ) && density >= 0.0f ); + + b2World* world = b2GetWorldLocked( shapeId.world0 ); + if ( world == NULL ) + { + return; + } + + b2Shape* shape = b2GetShape( world, shapeId ); + if ( density == shape->density ) + { + // early return to avoid expensive function + return; + } + + shape->density = density; +} + +float b2Shape_GetDensity( b2ShapeId shapeId ) +{ + b2World* world = b2GetWorld( shapeId.world0 ); + b2Shape* shape = b2GetShape( world, shapeId ); + return shape->density; +} + +void b2Shape_SetFriction( b2ShapeId shapeId, float friction ) +{ + B2_ASSERT( b2IsValid( friction ) && friction >= 0.0f ); + + b2World* world = b2GetWorld( shapeId.world0 ); + B2_ASSERT( world->locked == false ); + if ( world->locked ) + { + return; + } + + b2Shape* shape = b2GetShape( world, shapeId ); + shape->friction = friction; +} + +float b2Shape_GetFriction( b2ShapeId shapeId ) +{ + b2World* world = b2GetWorld( shapeId.world0 ); + b2Shape* shape = b2GetShape( world, shapeId ); + return shape->friction; +} + +void b2Shape_SetRestitution( b2ShapeId shapeId, float restitution ) +{ + B2_ASSERT( b2IsValid( restitution ) && restitution >= 0.0f ); + + b2World* world = b2GetWorld( shapeId.world0 ); + B2_ASSERT( world->locked == false ); + if ( world->locked ) + { + return; + } + + b2Shape* shape = b2GetShape( world, shapeId ); + shape->restitution = restitution; +} + +float b2Shape_GetRestitution( b2ShapeId shapeId ) +{ + b2World* world = b2GetWorld( shapeId.world0 ); + b2Shape* shape = b2GetShape( world, shapeId ); + return shape->restitution; +} + +b2Filter b2Shape_GetFilter( b2ShapeId shapeId ) +{ + b2World* world = b2GetWorld( shapeId.world0 ); + b2Shape* shape = b2GetShape( world, shapeId ); + return shape->filter; +} + +static void b2ResetProxy( b2World* world, b2Shape* shape, bool wakeBodies, bool destroyProxy ) +{ + b2Body* body = b2BodyArray_Get( &world->bodies, shape->bodyId ); + + int shapeId = shape->id; + + // destroy all contacts associated with this shape + int contactKey = body->headContactKey; + while ( contactKey != B2_NULL_INDEX ) + { + int contactId = contactKey >> 1; + int edgeIndex = contactKey & 1; + + b2Contact* contact = b2ContactArray_Get( &world->contacts, contactId ); + contactKey = contact->edges[edgeIndex].nextKey; + + if ( contact->shapeIdA == shapeId || contact->shapeIdB == shapeId ) + { + b2DestroyContact( world, contact, wakeBodies ); + } + } + + b2Transform transform = b2GetBodyTransformQuick( world, body ); + if ( shape->proxyKey != B2_NULL_INDEX ) + { + b2BodyType proxyType = B2_PROXY_TYPE( shape->proxyKey ); + b2UpdateShapeAABBs( shape, transform, proxyType ); + + if ( destroyProxy ) + { + b2BroadPhase_DestroyProxy( &world->broadPhase, shape->proxyKey ); + + bool forcePairCreation = true; + shape->proxyKey = b2BroadPhase_CreateProxy( &world->broadPhase, proxyType, shape->fatAABB, shape->filter.categoryBits, + shapeId, forcePairCreation ); + } + else + { + b2BroadPhase_MoveProxy( &world->broadPhase, shape->proxyKey, shape->fatAABB ); + } + } + else + { + b2BodyType proxyType = body->type; + b2UpdateShapeAABBs( shape, transform, proxyType ); + } + + b2ValidateSolverSets( world ); +} + +void b2Shape_SetFilter( b2ShapeId shapeId, b2Filter filter ) +{ + b2World* world = b2GetWorldLocked( shapeId.world0 ); + if ( world == NULL ) + { + return; + } + + b2Shape* shape = b2GetShape( world, shapeId ); + if ( filter.maskBits == shape->filter.maskBits && filter.categoryBits == shape->filter.categoryBits && + filter.groupIndex == shape->filter.groupIndex ) + { + return; + } + + // If the category bits change, I need to destroy the proxy because it affects the tree sorting. + bool destroyProxy = filter.categoryBits == shape->filter.categoryBits; + + shape->filter = filter; + + // need to wake bodies because a filter change may destroy contacts + bool wakeBodies = true; + b2ResetProxy( world, shape, wakeBodies, destroyProxy ); +} + +void b2Shape_EnableSensorEvents( b2ShapeId shapeId, bool flag ) +{ + b2World* world = b2GetWorldLocked( shapeId.world0 ); + if ( world == NULL ) + { + return; + } + + b2Shape* shape = b2GetShape( world, shapeId ); + shape->enableSensorEvents = flag; +} + +bool b2Shape_AreSensorEventsEnabled( b2ShapeId shapeId ) +{ + b2World* world = b2GetWorld( shapeId.world0 ); + b2Shape* shape = b2GetShape( world, shapeId ); + return shape->enableSensorEvents; +} + +void b2Shape_EnableContactEvents( b2ShapeId shapeId, bool flag ) +{ + b2World* world = b2GetWorldLocked( shapeId.world0 ); + if ( world == NULL ) + { + return; + } + + b2Shape* shape = b2GetShape( world, shapeId ); + shape->enableContactEvents = flag; +} + +bool b2Shape_AreContactEventsEnabled( b2ShapeId shapeId ) +{ + b2World* world = b2GetWorld( shapeId.world0 ); + b2Shape* shape = b2GetShape( world, shapeId ); + return shape->enableContactEvents; +} + +void b2Shape_EnablePreSolveEvents( b2ShapeId shapeId, bool flag ) +{ + b2World* world = b2GetWorldLocked( shapeId.world0 ); + if ( world == NULL ) + { + return; + } + + b2Shape* shape = b2GetShape( world, shapeId ); + shape->enablePreSolveEvents = flag; +} + +bool b2Shape_ArePreSolveEventsEnabled( b2ShapeId shapeId ) +{ + b2World* world = b2GetWorld( shapeId.world0 ); + b2Shape* shape = b2GetShape( world, shapeId ); + return shape->enablePreSolveEvents; +} + +void b2Shape_EnableHitEvents( b2ShapeId shapeId, bool flag ) +{ + b2World* world = b2GetWorldLocked( shapeId.world0 ); + if ( world == NULL ) + { + return; + } + + b2Shape* shape = b2GetShape( world, shapeId ); + shape->enableHitEvents = flag; +} + +bool b2Shape_AreHitEventsEnabled( b2ShapeId shapeId ) +{ + b2World* world = b2GetWorld( shapeId.world0 ); + b2Shape* shape = b2GetShape( world, shapeId ); + return shape->enableHitEvents; +} + +b2ShapeType b2Shape_GetType( b2ShapeId shapeId ) +{ + b2World* world = b2GetWorld( shapeId.world0 ); + b2Shape* shape = b2GetShape( world, shapeId ); + return shape->type; +} + +b2Circle b2Shape_GetCircle( b2ShapeId shapeId ) +{ + b2World* world = b2GetWorld( shapeId.world0 ); + b2Shape* shape = b2GetShape( world, shapeId ); + B2_ASSERT( shape->type == b2_circleShape ); + return shape->circle; +} + +b2Segment b2Shape_GetSegment( b2ShapeId shapeId ) +{ + b2World* world = b2GetWorld( shapeId.world0 ); + b2Shape* shape = b2GetShape( world, shapeId ); + B2_ASSERT( shape->type == b2_segmentShape ); + return shape->segment; +} + +b2ChainSegment b2Shape_GetChainSegment( b2ShapeId shapeId ) +{ + b2World* world = b2GetWorld( shapeId.world0 ); + b2Shape* shape = b2GetShape( world, shapeId ); + B2_ASSERT( shape->type == b2_chainSegmentShape ); + return shape->chainSegment; +} + +b2Capsule b2Shape_GetCapsule( b2ShapeId shapeId ) +{ + b2World* world = b2GetWorld( shapeId.world0 ); + b2Shape* shape = b2GetShape( world, shapeId ); + B2_ASSERT( shape->type == b2_capsuleShape ); + return shape->capsule; +} + +b2Polygon b2Shape_GetPolygon( b2ShapeId shapeId ) +{ + b2World* world = b2GetWorld( shapeId.world0 ); + b2Shape* shape = b2GetShape( world, shapeId ); + B2_ASSERT( shape->type == b2_polygonShape ); + return shape->polygon; +} + +void b2Shape_SetCircle( b2ShapeId shapeId, const b2Circle* circle ) +{ + b2World* world = b2GetWorldLocked( shapeId.world0 ); + if ( world == NULL ) + { + return; + } + + b2Shape* shape = b2GetShape( world, shapeId ); + shape->circle = *circle; + shape->type = b2_circleShape; + + // need to wake bodies so they can react to the shape change + bool wakeBodies = true; + bool destroyProxy = true; + b2ResetProxy( world, shape, wakeBodies, destroyProxy ); +} + +void b2Shape_SetCapsule( b2ShapeId shapeId, const b2Capsule* capsule ) +{ + b2World* world = b2GetWorldLocked( shapeId.world0 ); + if ( world == NULL ) + { + return; + } + + b2Shape* shape = b2GetShape( world, shapeId ); + shape->capsule = *capsule; + shape->type = b2_capsuleShape; + + // need to wake bodies so they can react to the shape change + bool wakeBodies = true; + bool destroyProxy = true; + b2ResetProxy( world, shape, wakeBodies, destroyProxy ); +} + +void b2Shape_SetSegment( b2ShapeId shapeId, const b2Segment* segment ) +{ + b2World* world = b2GetWorldLocked( shapeId.world0 ); + if ( world == NULL ) + { + return; + } + + b2Shape* shape = b2GetShape( world, shapeId ); + shape->segment = *segment; + shape->type = b2_segmentShape; + + // need to wake bodies so they can react to the shape change + bool wakeBodies = true; + bool destroyProxy = true; + b2ResetProxy( world, shape, wakeBodies, destroyProxy ); +} + +void b2Shape_SetPolygon( b2ShapeId shapeId, const b2Polygon* polygon ) +{ + b2World* world = b2GetWorldLocked( shapeId.world0 ); + if ( world == NULL ) + { + return; + } + + b2Shape* shape = b2GetShape( world, shapeId ); + shape->polygon = *polygon; + shape->type = b2_polygonShape; + + // need to wake bodies so they can react to the shape change + bool wakeBodies = true; + bool destroyProxy = true; + b2ResetProxy( world, shape, wakeBodies, destroyProxy ); +} + +b2ChainId b2Shape_GetParentChain( b2ShapeId shapeId ) +{ + b2World* world = b2GetWorld( shapeId.world0 ); + b2Shape* shape = b2GetShape( world, shapeId ); + if ( shape->type == b2_chainSegmentShape ) + { + int chainId = shape->chainSegment.chainId; + if ( chainId != B2_NULL_INDEX ) + { + b2ChainShape* chain = b2ChainShapeArray_Get(&world->chainShapes, chainId); + b2ChainId id = { chainId + 1, shapeId.world0, chain->revision }; + return id; + } + } + + return ( b2ChainId ){ 0 }; +} + +void b2Chain_SetFriction( b2ChainId chainId, float friction ) +{ + b2World* world = b2GetWorldLocked( chainId.world0 ); + if ( world == NULL ) + { + return; + } + + b2ChainShape* chainShape = b2GetChainShape( world, chainId ); + + int count = chainShape->count; + + for ( int i = 0; i < count; ++i ) + { + int shapeId = chainShape->shapeIndices[i]; + b2Shape* shape = b2ShapeArray_Get( &world->shapes, shapeId ); + shape->friction = friction; + } +} + +void b2Chain_SetRestitution( b2ChainId chainId, float restitution ) +{ + b2World* world = b2GetWorldLocked( chainId.world0 ); + if ( world == NULL ) + { + return; + } + + b2ChainShape* chainShape = b2GetChainShape( world, chainId ); + + int count = chainShape->count; + + for ( int i = 0; i < count; ++i ) + { + int shapeId = chainShape->shapeIndices[i]; + b2Shape* shape = b2ShapeArray_Get( &world->shapes, shapeId ); + shape->restitution = restitution; + } +} + +int b2Shape_GetContactCapacity( b2ShapeId shapeId ) +{ + b2World* world = b2GetWorldLocked( shapeId.world0 ); + if ( world == NULL ) + { + return 0; + } + + b2Shape* shape = b2GetShape( world, shapeId ); + if ( shape->isSensor ) + { + return 0; + } + + b2Body* body = b2BodyArray_Get( &world->bodies, shape->bodyId ); + + // Conservative and fast + return body->contactCount; +} + +// todo sample needed +int b2Shape_GetContactData( b2ShapeId shapeId, b2ContactData* contactData, int capacity ) +{ + b2World* world = b2GetWorldLocked( shapeId.world0 ); + if ( world == NULL ) + { + return 0; + } + + b2Shape* shape = b2GetShape( world, shapeId ); + if ( shape->isSensor ) + { + return 0; + } + + b2Body* body = b2BodyArray_Get( &world->bodies, shape->bodyId ); + int contactKey = body->headContactKey; + int index = 0; + while ( contactKey != B2_NULL_INDEX && index < capacity ) + { + int contactId = contactKey >> 1; + int edgeIndex = contactKey & 1; + + b2Contact* contact = b2ContactArray_Get( &world->contacts, contactId ); + + // Does contact involve this shape and is it touching? + if ( ( contact->shapeIdA == shapeId.index1 - 1 || contact->shapeIdB == shapeId.index1 - 1 ) && + ( contact->flags & b2_contactTouchingFlag ) != 0 ) + { + b2Shape* shapeA = world->shapes.data + contact->shapeIdA; + b2Shape* shapeB = world->shapes.data + contact->shapeIdB; + + contactData[index].shapeIdA = ( b2ShapeId ){ shapeA->id + 1, shapeId.world0, shapeA->revision }; + contactData[index].shapeIdB = ( b2ShapeId ){ shapeB->id + 1, shapeId.world0, shapeB->revision }; + + b2ContactSim* contactSim = b2GetContactSim( world, contact ); + contactData[index].manifold = contactSim->manifold; + index += 1; + } + + contactKey = contact->edges[edgeIndex].nextKey; + } + + B2_ASSERT( index <= capacity ); + + return index; +} + +b2AABB b2Shape_GetAABB( b2ShapeId shapeId ) +{ + b2World* world = b2GetWorld( shapeId.world0 ); + if ( world == NULL ) + { + return ( b2AABB ){ 0 }; + } + + b2Shape* shape = b2GetShape( world, shapeId ); + return shape->aabb; +} + +b2Vec2 b2Shape_GetClosestPoint( b2ShapeId shapeId, b2Vec2 target ) +{ + b2World* world = b2GetWorld( shapeId.world0 ); + if ( world == NULL ) + { + return ( b2Vec2 ){ 0 }; + } + + b2Shape* shape = b2GetShape( world, shapeId ); + b2Body* body = b2BodyArray_Get( &world->bodies, shape->bodyId ); + b2Transform transform = b2GetBodyTransformQuick( world, body ); + + b2DistanceInput input; + input.proxyA = b2MakeShapeDistanceProxy( shape ); + input.proxyB = b2MakeProxy( &target, 1, 0.0f ); + input.transformA = transform; + input.transformB = b2Transform_identity; + input.useRadii = true; + + b2DistanceCache cache = { 0 }; + b2DistanceOutput output = b2ShapeDistance( &cache, &input, NULL, 0 ); + + return output.pointA; +} diff --git a/3rdparty/box2d/src/shape.h b/3rdparty/box2d/src/shape.h new file mode 100644 index 000000000000..e5efe65e4d6e --- /dev/null +++ b/3rdparty/box2d/src/shape.h @@ -0,0 +1,85 @@ +// SPDX-FileCopyrightText: 2023 Erin Catto +// SPDX-License-Identifier: MIT + +#pragma once + +#include "world.h" + +#include "box2d/types.h" + +typedef struct b2BroadPhase b2BroadPhase; +typedef struct b2World b2World; + +typedef struct b2Shape +{ + int id; + int bodyId; + int prevShapeId; + int nextShapeId; + b2ShapeType type; + float density; + float friction; + float restitution; + + b2AABB aabb; + b2AABB fatAABB; + b2Vec2 localCentroid; + int proxyKey; + + b2Filter filter; + void* userData; + uint32_t customColor; + + union + { + b2Capsule capsule; + b2Circle circle; + b2Polygon polygon; + b2Segment segment; + b2ChainSegment chainSegment; + }; + + uint16_t revision; + bool isSensor; + bool enableSensorEvents; + bool enableContactEvents; + bool enableHitEvents; + bool enablePreSolveEvents; + bool enlargedAABB; + bool isFast; +} b2Shape; + +typedef struct b2ChainShape +{ + int id; + int bodyId; + int nextChainId; + int* shapeIndices; + int count; + uint16_t revision; +} b2ChainShape; + +typedef struct b2ShapeExtent +{ + float minExtent; + float maxExtent; +} b2ShapeExtent; + +void b2CreateShapeProxy( b2Shape* shape, b2BroadPhase* bp, b2BodyType type, b2Transform transform, bool forcePairCreation ); +void b2DestroyShapeProxy( b2Shape* shape, b2BroadPhase* bp ); + +b2MassData b2ComputeShapeMass( const b2Shape* shape ); +b2ShapeExtent b2ComputeShapeExtent( const b2Shape* shape, b2Vec2 localCenter ); +b2AABB b2ComputeShapeAABB( const b2Shape* shape, b2Transform transform ); +b2Vec2 b2GetShapeCentroid( const b2Shape* shape ); +float b2GetShapePerimeter( const b2Shape* shape ); + +b2DistanceProxy b2MakeShapeDistanceProxy( const b2Shape* shape ); + +b2CastOutput b2RayCastShape( const b2RayCastInput* input, const b2Shape* shape, b2Transform transform ); +b2CastOutput b2ShapeCastShape( const b2ShapeCastInput* input, const b2Shape* shape, b2Transform transform ); + +b2Transform b2GetOwnerTransform( b2World* world, b2Shape* shape ); + +B2_ARRAY_INLINE( b2ChainShape, b2ChainShape ); +B2_ARRAY_INLINE( b2Shape, b2Shape ); diff --git a/3rdparty/box2d/src/solver.c b/3rdparty/box2d/src/solver.c new file mode 100644 index 000000000000..28a4c2b256d7 --- /dev/null +++ b/3rdparty/box2d/src/solver.c @@ -0,0 +1,2045 @@ +// SPDX-FileCopyrightText: 2023 Erin Catto +// SPDX-License-Identifier: MIT + +#include "solver.h" + +#include "array.h" +#include "bitset.h" +#include "body.h" +#include "contact.h" +#include "contact_solver.h" +#include "core.h" +#include "ctz.h" +#include "island.h" +#include "joint.h" +#include "shape.h" +#include "solver_set.h" +#include "stack_allocator.h" +#include "world.h" + +#include +#include +#include +#include + +#if ( defined( __GNUC__ ) || defined( __clang__ ) ) && ( defined( __i386__ ) || defined( __x86_64__ ) ) +static inline void b2Pause( void ) +{ + __asm__ __volatile__( "pause\n" ); +} +#elif ( defined( __arm__ ) && defined( __ARM_ARCH ) && __ARM_ARCH >= 7 ) || defined( __aarch64__ ) +static inline void b2Pause( void ) +{ + __asm__ __volatile__( "yield" ::: "memory" ); +} +#elif defined( _MSC_VER ) && ( defined( _M_IX86 ) || defined( _M_X64 ) ) +//#include +static inline void b2Pause( void ) +{ + _mm_pause(); +} +#elif defined( _MSC_VER ) && ( defined( _M_ARM ) || defined( _M_ARM64 ) ) +static inline void b2Pause( void ) +{ + __yield(); +} +#else +static inline void b2Pause( void ) +{ +} +#endif + +typedef struct b2WorkerContext +{ + b2StepContext* context; + int workerIndex; + void* userTask; +} b2WorkerContext; + +// Integrate velocities and apply damping +static void b2IntegrateVelocitiesTask( int startIndex, int endIndex, b2StepContext* context ) +{ + b2TracyCZoneNC( integrate_velocity, "IntVel", b2_colorDeepPink, true ); + + b2BodyState* states = context->states; + b2BodySim* sims = context->sims; + + b2Vec2 gravity = context->world->gravity; + float h = context->h; + float maxLinearSpeed = context->maxLinearVelocity; + float maxAngularSpeed = b2_maxRotation * context->inv_dt; + float maxLinearSpeedSquared = maxLinearSpeed * maxLinearSpeed; + float maxAngularSpeedSquared = maxAngularSpeed * maxAngularSpeed; + + for ( int i = startIndex; i < endIndex; ++i ) + { + b2BodySim* sim = sims + i; + b2BodyState* state = states + i; + + b2Vec2 v = state->linearVelocity; + float w = state->angularVelocity; + + // Apply forces, torque, gravity, and damping + // Apply damping. + // Differential equation: dv/dt + c * v = 0 + // Solution: v(t) = v0 * exp(-c * t) + // Time step: v(t + dt) = v0 * exp(-c * (t + dt)) = v0 * exp(-c * t) * exp(-c * dt) = v(t) * exp(-c * dt) + // v2 = exp(-c * dt) * v1 + // Pade approximation: + // v2 = v1 * 1 / (1 + c * dt) + float linearDamping = 1.0f / ( 1.0f + h * sim->linearDamping ); + float angularDamping = 1.0f / ( 1.0f + h * sim->angularDamping ); + b2Vec2 linearVelocityDelta = b2MulSV( h * sim->invMass, b2MulAdd( sim->force, sim->mass * sim->gravityScale, gravity ) ); + float angularVelocityDelta = h * sim->invInertia * sim->torque; + + v = b2MulAdd( linearVelocityDelta, linearDamping, v ); + w = angularVelocityDelta + angularDamping * w; + + // Clamp to max linear speed + if ( b2Dot( v, v ) > maxLinearSpeedSquared ) + { + float ratio = maxLinearSpeed / b2Length( v ); + v = b2MulSV( ratio, v ); + sim->isSpeedCapped = true; + } + + // Clamp to max angular speed + if ( w * w > maxAngularSpeedSquared && sim->allowFastRotation == false ) + { + float ratio = maxAngularSpeed / b2AbsFloat( w ); + w *= ratio; + sim->isSpeedCapped = true; + } + + state->linearVelocity = v; + state->angularVelocity = w; + } + + b2TracyCZoneEnd( integrate_velocity ); +} + +static void b2PrepareJointsTask( int startIndex, int endIndex, b2StepContext* context ) +{ + b2TracyCZoneNC( prepare_joints, "PrepJoints", b2_colorOldLace, true ); + + b2JointSim** joints = context->joints; + + for ( int i = startIndex; i < endIndex; ++i ) + { + b2JointSim* joint = joints[i]; + b2PrepareJoint( joint, context ); + } + + b2TracyCZoneEnd( prepare_joints ); +} + +static void b2WarmStartJointsTask( int startIndex, int endIndex, b2StepContext* context, int colorIndex ) +{ + b2TracyCZoneNC( warm_joints, "WarmJoints", b2_colorGold, true ); + + b2GraphColor* color = context->graph->colors + colorIndex; + b2JointSim* joints = color->jointSims.data; + B2_ASSERT( 0 <= startIndex && startIndex < color->jointSims.count ); + B2_ASSERT( startIndex <= endIndex && endIndex <= color->jointSims.count ); + + for ( int i = startIndex; i < endIndex; ++i ) + { + b2JointSim* joint = joints + i; + b2WarmStartJoint( joint, context ); + } + + b2TracyCZoneEnd( warm_joints ); +} + +static void b2SolveJointsTask( int startIndex, int endIndex, b2StepContext* context, int colorIndex, bool useBias ) +{ + b2TracyCZoneNC( solve_joints, "SolveJoints", b2_colorLemonChiffon, true ); + + b2GraphColor* color = context->graph->colors + colorIndex; + b2JointSim* joints = color->jointSims.data; + B2_ASSERT( 0 <= startIndex && startIndex < color->jointSims.count ); + B2_ASSERT( startIndex <= endIndex && endIndex <= color->jointSims.count ); + + for ( int i = startIndex; i < endIndex; ++i ) + { + b2JointSim* joint = joints + i; + b2SolveJoint( joint, context, useBias ); + } + + b2TracyCZoneEnd( solve_joints ); +} + +static void b2IntegratePositionsTask( int startIndex, int endIndex, b2StepContext* context ) +{ + b2TracyCZoneNC( integrate_positions, "IntPos", b2_colorDarkSeaGreen, true ); + + b2BodyState* states = context->states; + float h = context->h; + + B2_ASSERT( startIndex <= endIndex ); + + for ( int i = startIndex; i < endIndex; ++i ) + { + b2BodyState* state = states + i; + state->deltaRotation = b2IntegrateRotation( state->deltaRotation, h * state->angularVelocity ); + state->deltaPosition = b2MulAdd( state->deltaPosition, h, state->linearVelocity ); + } + + b2TracyCZoneEnd( integrate_positions ); +} + +static void b2FinalizeBodiesTask( int startIndex, int endIndex, uint32_t threadIndex, void* context ) +{ + b2TracyCZoneNC( finalize_bodies, "FinalizeBodies", b2_colorViolet, true ); + + b2StepContext* stepContext = context; + b2World* world = stepContext->world; + bool enableSleep = world->enableSleep; + b2BodyState* states = stepContext->states; + b2BodySim* sims = stepContext->sims; + b2Body* bodies = world->bodies.data; + float timeStep = stepContext->dt; + float invTimeStep = stepContext->inv_dt; + + uint16_t worldId = world->worldId; + + // The body move event array has should already have the correct size + B2_ASSERT( endIndex <= world->bodyMoveEvents.count ); + b2BodyMoveEvent* moveEvents = world->bodyMoveEvents.data; + + b2BitSet* enlargedSimBitSet = &world->taskContexts.data[threadIndex].enlargedSimBitSet; + b2BitSet* awakeIslandBitSet = &world->taskContexts.data[threadIndex].awakeIslandBitSet; + b2TaskContext* taskContext = world->taskContexts.data + threadIndex; + + bool enableContinuous = world->enableContinuous; + + const float speculativeDistance = b2_speculativeDistance; + const float aabbMargin = b2_aabbMargin; + + B2_ASSERT( startIndex <= endIndex ); + + for ( int simIndex = startIndex; simIndex < endIndex; ++simIndex ) + { + b2BodyState* state = states + simIndex; + b2BodySim* sim = sims + simIndex; + + b2Vec2 v = state->linearVelocity; + float w = state->angularVelocity; + + B2_ASSERT( b2Vec2_IsValid( v ) ); + B2_ASSERT( b2IsValid( w ) ); + + sim->center = b2Add( sim->center, state->deltaPosition ); + sim->transform.q = b2NormalizeRot( b2MulRot( state->deltaRotation, sim->transform.q ) ); + + // Use the velocity of the farthest point on the body to account for rotation. + float maxVelocity = b2Length( v ) + b2AbsFloat( w ) * sim->maxExtent; + + // Sleep needs to observe position correction as well as true velocity. + float maxDeltaPosition = b2Length( state->deltaPosition ) + b2AbsFloat( state->deltaRotation.s ) * sim->maxExtent; + + // Position correction is not as important for sleep as true velocity. + float positionSleepFactor = 0.5f; + + float sleepVelocity = b2MaxFloat( maxVelocity, positionSleepFactor * invTimeStep * maxDeltaPosition ); + + // reset state deltas + state->deltaPosition = b2Vec2_zero; + state->deltaRotation = b2Rot_identity; + + sim->transform.p = b2Sub( sim->center, b2RotateVector( sim->transform.q, sim->localCenter ) ); + + // cache miss here, however I need the shape list below + b2Body* body = bodies + sim->bodyId; + body->bodyMoveIndex = simIndex; + moveEvents[simIndex].transform = sim->transform; + moveEvents[simIndex].bodyId = ( b2BodyId ){ sim->bodyId + 1, worldId, body->revision }; + moveEvents[simIndex].userData = body->userData; + moveEvents[simIndex].fellAsleep = false; + + // reset applied force and torque + sim->force = b2Vec2_zero; + sim->torque = 0.0f; + + body->isSpeedCapped = sim->isSpeedCapped; + sim->isSpeedCapped = false; + + sim->isFast = false; + + if ( enableSleep == false || body->enableSleep == false || sleepVelocity > body->sleepThreshold ) + { + // Body is not sleepy + body->sleepTime = 0.0f; + + const float saftetyFactor = 0.5f; + if ( body->type == b2_dynamicBody && enableContinuous && maxVelocity * timeStep > saftetyFactor * sim->minExtent ) + { + // Store in fast array for the continuous collision stage + // This is deterministic because the order of TOI sweeps doesn't matter + if ( sim->isBullet ) + { + int bulletIndex = atomic_fetch_add( &stepContext->bulletBodyCount, 1 ); + stepContext->bulletBodies[bulletIndex] = simIndex; + } + else + { + int fastIndex = atomic_fetch_add( &stepContext->fastBodyCount, 1 ); + stepContext->fastBodies[fastIndex] = simIndex; + } + + sim->isFast = true; + } + else + { + // Body is safe to advance + sim->center0 = sim->center; + sim->rotation0 = sim->transform.q; + } + } + else + { + // Body is safe to advance and is falling asleep + sim->center0 = sim->center; + sim->rotation0 = sim->transform.q; + body->sleepTime += timeStep; + } + + // Any single body in an island can keep it awake + b2Island* island = b2IslandArray_Get( &world->islands, body->islandId ); + if ( body->sleepTime < b2_timeToSleep ) + { + // keep island awake + int islandIndex = island->localIndex; + b2SetBit( awakeIslandBitSet, islandIndex ); + } + else if ( island->constraintRemoveCount > 0 ) + { + // body wants to sleep but its island needs splitting first + if ( body->sleepTime > taskContext->splitSleepTime ) + { + // pick the sleepiest candidate + taskContext->splitIslandId = body->islandId; + taskContext->splitSleepTime = body->sleepTime; + } + } + + // Update shapes AABBs + b2Transform transform = sim->transform; + bool isFast = sim->isFast; + int shapeId = body->headShapeId; + while ( shapeId != B2_NULL_INDEX ) + { + b2Shape* shape = b2ShapeArray_Get( &world->shapes, shapeId ); + + B2_ASSERT( shape->isFast == false ); + + if ( isFast ) + { + // The AABB is updated after continuous collision. + // Add to moved shapes regardless of AABB changes. + shape->isFast = true; + + // Bit-set to keep the move array sorted + b2SetBit( enlargedSimBitSet, simIndex ); + } + else + { + b2AABB aabb = b2ComputeShapeAABB( shape, transform ); + aabb.lowerBound.x -= speculativeDistance; + aabb.lowerBound.y -= speculativeDistance; + aabb.upperBound.x += speculativeDistance; + aabb.upperBound.y += speculativeDistance; + shape->aabb = aabb; + + B2_ASSERT( shape->enlargedAABB == false ); + + if ( b2AABB_Contains( shape->fatAABB, aabb ) == false ) + { + b2AABB fatAABB; + fatAABB.lowerBound.x = aabb.lowerBound.x - aabbMargin; + fatAABB.lowerBound.y = aabb.lowerBound.y - aabbMargin; + fatAABB.upperBound.x = aabb.upperBound.x + aabbMargin; + fatAABB.upperBound.y = aabb.upperBound.y + aabbMargin; + shape->fatAABB = fatAABB; + + shape->enlargedAABB = true; + + // Bit-set to keep the move array sorted + b2SetBit( enlargedSimBitSet, simIndex ); + } + } + + shapeId = shape->nextShapeId; + } + } + + b2TracyCZoneEnd( finalize_bodies ); +} + +/* + typedef enum b2SolverStageType +{ + b2_stagePrepareJoints, + b2_stagePrepareContacts, + b2_stageIntegrateVelocities, + b2_stageWarmStart, + b2_stageSolve, + b2_stageIntegratePositions, + b2_stageRelax, + b2_stageRestitution, + b2_stageStoreImpulses +} b2SolverStageType; + +typedef enum b2SolverBlockType +{ + b2_bodyBlock, + b2_jointBlock, + b2_contactBlock, + b2_graphJointBlock, + b2_graphContactBlock +} b2SolverBlockType; +*/ + +static void b2ExecuteBlock( b2SolverStage* stage, b2StepContext* context, b2SolverBlock* block ) +{ + b2SolverStageType stageType = stage->type; + b2SolverBlockType blockType = block->blockType; + int startIndex = block->startIndex; + int endIndex = startIndex + block->count; + + switch ( stageType ) + { + case b2_stagePrepareJoints: + b2PrepareJointsTask( startIndex, endIndex, context ); + break; + + case b2_stagePrepareContacts: + b2PrepareContactsTask( startIndex, endIndex, context ); + break; + + case b2_stageIntegrateVelocities: + b2IntegrateVelocitiesTask( startIndex, endIndex, context ); + break; + + case b2_stageWarmStart: + if ( context->world->enableWarmStarting ) + { + if ( blockType == b2_graphContactBlock ) + { + b2WarmStartContactsTask( startIndex, endIndex, context, stage->colorIndex ); + } + else if ( blockType == b2_graphJointBlock ) + { + b2WarmStartJointsTask( startIndex, endIndex, context, stage->colorIndex ); + } + } + break; + + case b2_stageSolve: + if ( blockType == b2_graphContactBlock ) + { + b2SolveContactsTask( startIndex, endIndex, context, stage->colorIndex, true ); + } + else if ( blockType == b2_graphJointBlock ) + { + b2SolveJointsTask( startIndex, endIndex, context, stage->colorIndex, true ); + } + break; + + case b2_stageIntegratePositions: + b2IntegratePositionsTask( startIndex, endIndex, context ); + break; + + case b2_stageRelax: + if ( blockType == b2_graphContactBlock ) + { + b2SolveContactsTask( startIndex, endIndex, context, stage->colorIndex, false ); + } + else if ( blockType == b2_graphJointBlock ) + { + b2SolveJointsTask( startIndex, endIndex, context, stage->colorIndex, false ); + } + break; + + case b2_stageRestitution: + if ( blockType == b2_graphContactBlock ) + { + b2ApplyRestitutionTask( startIndex, endIndex, context, stage->colorIndex ); + } + break; + + case b2_stageStoreImpulses: + b2StoreImpulsesTask( startIndex, endIndex, context ); + break; + } +} + +static inline int GetWorkerStartIndex( int workerIndex, int blockCount, int workerCount ) +{ + if ( blockCount <= workerCount ) + { + return workerIndex < blockCount ? workerIndex : B2_NULL_INDEX; + } + + int blocksPerWorker = blockCount / workerCount; + int remainder = blockCount - blocksPerWorker * workerCount; + return blocksPerWorker * workerIndex + b2MinInt( remainder, workerIndex ); +} + +static void b2ExecuteStage( b2SolverStage* stage, b2StepContext* context, int previousSyncIndex, int syncIndex, int workerIndex ) +{ + int completedCount = 0; + b2SolverBlock* blocks = stage->blocks; + int blockCount = stage->blockCount; + + int expectedSyncIndex = previousSyncIndex; + + int startIndex = GetWorkerStartIndex( workerIndex, blockCount, context->workerCount ); + if ( startIndex == B2_NULL_INDEX ) + { + return; + } + + B2_ASSERT( 0 <= startIndex && startIndex < blockCount ); + + int blockIndex = startIndex; + + // Caution: this can change expectedSyncIndex + while ( atomic_compare_exchange_strong( &blocks[blockIndex].syncIndex, &expectedSyncIndex, syncIndex ) == true ) + { + B2_ASSERT( stage->type != b2_stagePrepareContacts || syncIndex < 2 ); + + B2_ASSERT( completedCount < blockCount ); + + b2ExecuteBlock( stage, context, blocks + blockIndex ); + + completedCount += 1; + blockIndex += 1; + if ( blockIndex >= blockCount ) + { + // Keep looking for work + blockIndex = 0; + } + + expectedSyncIndex = previousSyncIndex; + } + + // Search backwards for blocks + blockIndex = startIndex - 1; + while ( true ) + { + if ( blockIndex < 0 ) + { + blockIndex = blockCount - 1; + } + + expectedSyncIndex = previousSyncIndex; + + // Caution: this can change expectedSyncIndex + if ( atomic_compare_exchange_strong( &blocks[blockIndex].syncIndex, &expectedSyncIndex, syncIndex ) == false ) + { + break; + } + + b2ExecuteBlock( stage, context, blocks + blockIndex ); + completedCount += 1; + blockIndex -= 1; + } + + (void)atomic_fetch_add( &stage->completionCount, completedCount ); +} + +static void b2ExecuteMainStage( b2SolverStage* stage, b2StepContext* context, uint32_t syncBits ) +{ + int blockCount = stage->blockCount; + if ( blockCount == 0 ) + { + return; + } + + if ( blockCount == 1 ) + { + b2ExecuteBlock( stage, context, stage->blocks ); + } + else + { + atomic_store( &context->atomicSyncBits, syncBits ); + + int syncIndex = ( syncBits >> 16 ) & 0xFFFF; + B2_ASSERT( syncIndex > 0 ); + int previousSyncIndex = syncIndex - 1; + + b2ExecuteStage( stage, context, previousSyncIndex, syncIndex, 0 ); + + // todo consider using the cycle counter as well + while ( atomic_load( &stage->completionCount ) != blockCount ) + { + b2Pause(); + } + + atomic_store( &stage->completionCount, 0 ); + } +} + +// This should not use the thread index because thread 0 can be called twice by enkiTS. +void b2SolverTask( int startIndex, int endIndex, uint32_t threadIndexDontUse, void* taskContext ) +{ + B2_MAYBE_UNUSED( startIndex ); + B2_MAYBE_UNUSED( endIndex ); + B2_MAYBE_UNUSED( threadIndexDontUse ); + + b2WorkerContext* workerContext = taskContext; + int workerIndex = workerContext->workerIndex; + b2StepContext* context = workerContext->context; + int activeColorCount = context->activeColorCount; + b2SolverStage* stages = context->stages; + b2Profile* profile = &context->world->profile; + + if ( workerIndex == 0 ) + { + // Main thread synchronizes the workers and does work itself. + // + // Stages are re-used by loops so that I don't need more stages for large iteration counts. + // The sync indices grow monotonically for the body/graph/constraint groupings because they share solver blocks. + // The stage index and sync indices are combined in to sync bits for atomic synchronization. + // The workers need to compute the previous sync index for a given stage so that CAS works correctly. This + // setup makes this easy to do. + + /* + b2_stagePrepareJoints, + b2_stagePrepareContacts, + b2_stageIntegrateVelocities, + b2_stageWarmStart, + b2_stageSolve, + b2_stageIntegratePositions, + b2_stageRelax, + b2_stageRestitution, + b2_stageStoreImpulses + */ + + b2Timer timer = b2CreateTimer(); + + int bodySyncIndex = 1; + int stageIndex = 0; + + // This stage loops over all awake joints + uint32_t jointSyncIndex = 1; + uint32_t syncBits = ( jointSyncIndex << 16 ) | stageIndex; + B2_ASSERT( stages[stageIndex].type == b2_stagePrepareJoints ); + b2ExecuteMainStage( stages + stageIndex, context, syncBits ); + stageIndex += 1; + jointSyncIndex += 1; + + // This stage loops over all contact constraints + uint32_t contactSyncIndex = 1; + syncBits = ( contactSyncIndex << 16 ) | stageIndex; + B2_ASSERT( stages[stageIndex].type == b2_stagePrepareContacts ); + b2ExecuteMainStage( stages + stageIndex, context, syncBits ); + stageIndex += 1; + contactSyncIndex += 1; + + int graphSyncIndex = 1; + + // Single-threaded overflow work. These constraints don't fit in the graph coloring. + b2PrepareOverflowJoints( context ); + b2PrepareOverflowContacts( context ); + + profile->prepareConstraints += b2GetMillisecondsAndReset( &timer ); + + int subStepCount = context->subStepCount; + for ( int i = 0; i < subStepCount; ++i ) + { + // stage index restarted each iteration + // syncBits still increases monotonically because the upper bits increase each iteration + int iterStageIndex = stageIndex; + + // integrate velocities + syncBits = ( bodySyncIndex << 16 ) | iterStageIndex; + B2_ASSERT( stages[iterStageIndex].type == b2_stageIntegrateVelocities ); + b2ExecuteMainStage( stages + iterStageIndex, context, syncBits ); + iterStageIndex += 1; + bodySyncIndex += 1; + + profile->integrateVelocities += b2GetMillisecondsAndReset( &timer ); + + // warm start constraints + b2WarmStartOverflowJoints( context ); + b2WarmStartOverflowContacts( context ); + + for ( int colorIndex = 0; colorIndex < activeColorCount; ++colorIndex ) + { + syncBits = ( graphSyncIndex << 16 ) | iterStageIndex; + B2_ASSERT( stages[iterStageIndex].type == b2_stageWarmStart ); + b2ExecuteMainStage( stages + iterStageIndex, context, syncBits ); + iterStageIndex += 1; + } + graphSyncIndex += 1; + + profile->warmStart += b2GetMillisecondsAndReset( &timer ); + + // solve constraints + bool useBias = true; + b2SolveOverflowJoints( context, useBias ); + b2SolveOverflowContacts( context, useBias ); + + for ( int colorIndex = 0; colorIndex < activeColorCount; ++colorIndex ) + { + syncBits = ( graphSyncIndex << 16 ) | iterStageIndex; + B2_ASSERT( stages[iterStageIndex].type == b2_stageSolve ); + b2ExecuteMainStage( stages + iterStageIndex, context, syncBits ); + iterStageIndex += 1; + } + graphSyncIndex += 1; + + profile->solveVelocities += b2GetMillisecondsAndReset( &timer ); + + // integrate positions + B2_ASSERT( stages[iterStageIndex].type == b2_stageIntegratePositions ); + syncBits = ( bodySyncIndex << 16 ) | iterStageIndex; + b2ExecuteMainStage( stages + iterStageIndex, context, syncBits ); + iterStageIndex += 1; + bodySyncIndex += 1; + + profile->integratePositions += b2GetMillisecondsAndReset( &timer ); + + // relax constraints + useBias = false; + b2SolveOverflowJoints( context, useBias ); + b2SolveOverflowContacts( context, useBias ); + + for ( int colorIndex = 0; colorIndex < activeColorCount; ++colorIndex ) + { + syncBits = ( graphSyncIndex << 16 ) | iterStageIndex; + B2_ASSERT( stages[iterStageIndex].type == b2_stageRelax ); + b2ExecuteMainStage( stages + iterStageIndex, context, syncBits ); + iterStageIndex += 1; + } + graphSyncIndex += 1; + + profile->relaxVelocities += b2GetMillisecondsAndReset( &timer ); + } + + // advance the stage according to the sub-stepping tasks just completed + // integrate velocities / warm start / solve / integrate positions / relax + stageIndex += 1 + activeColorCount + activeColorCount + 1 + activeColorCount; + + // Restitution + { + b2ApplyOverflowRestitution( context ); + + int iterStageIndex = stageIndex; + for ( int colorIndex = 0; colorIndex < activeColorCount; ++colorIndex ) + { + syncBits = ( graphSyncIndex << 16 ) | iterStageIndex; + B2_ASSERT( stages[iterStageIndex].type == b2_stageRestitution ); + b2ExecuteMainStage( stages + iterStageIndex, context, syncBits ); + iterStageIndex += 1; + } + // graphSyncIndex += 1; + stageIndex += activeColorCount; + } + + profile->applyRestitution += b2GetMillisecondsAndReset( &timer ); + + b2StoreOverflowImpulses( context ); + + syncBits = ( contactSyncIndex << 16 ) | stageIndex; + B2_ASSERT( stages[stageIndex].type == b2_stageStoreImpulses ); + b2ExecuteMainStage( stages + stageIndex, context, syncBits ); + + profile->storeImpulses += b2GetMillisecondsAndReset( &timer ); + + // Signal workers to finish + atomic_store( &context->atomicSyncBits, UINT_MAX ); + + B2_ASSERT( stageIndex + 1 == context->stageCount ); + return; + } + + // Worker spins and waits for work + uint32_t lastSyncBits = 0; + // uint64_t maxSpinTime = 10; + while ( true ) + { + // Spin until main thread bumps changes the sync bits. This can waste significant time overall, but it is necessary for + // parallel simulation with graph coloring. + uint32_t syncBits; + int spinCount = 0; + while ( ( syncBits = atomic_load( &context->atomicSyncBits ) ) == lastSyncBits ) + { + if ( spinCount > 5 ) + { + b2Yield(); + spinCount = 0; + } + else + { + // Using the cycle counter helps to account for variation in mm_pause timing across different + // CPUs. However, this is X64 only. + // uint64_t prev = __rdtsc(); + // do + //{ + // b2Pause(); + //} + // while ((__rdtsc() - prev) < maxSpinTime); + // maxSpinTime += 10; + b2Pause(); + b2Pause(); + spinCount += 1; + } + } + + if ( syncBits == UINT_MAX ) + { + // sentinel hit + break; + } + + int stageIndex = syncBits & 0xFFFF; + B2_ASSERT( stageIndex < context->stageCount ); + + int syncIndex = ( syncBits >> 16 ) & 0xFFFF; + B2_ASSERT( syncIndex > 0 ); + + int previousSyncIndex = syncIndex - 1; + + b2SolverStage* stage = stages + stageIndex; + b2ExecuteStage( stage, context, previousSyncIndex, syncIndex, workerIndex ); + + lastSyncBits = syncBits; + } +} + +struct b2ContinuousContext +{ + b2World* world; + b2BodySim* fastBodySim; + b2Shape* fastShape; + b2Vec2 centroid1, centroid2; + b2Sweep sweep; + float fraction; +}; + +// todo this may lead to pauses for scenarios where pre-solve would disable collision +static bool b2ContinuousQueryCallback( int proxyId, int shapeId, void* context ) +{ + B2_MAYBE_UNUSED( proxyId ); + + struct b2ContinuousContext* continuousContext = context; + b2Shape* fastShape = continuousContext->fastShape; + b2BodySim* fastBodySim = continuousContext->fastBodySim; + + // Skip same shape + if ( shapeId == fastShape->id ) + { + return true; + } + + b2World* world = continuousContext->world; + + b2Shape* shape = b2ShapeArray_Get( &world->shapes, shapeId ); + + // Skip same body + if ( shape->bodyId == fastShape->bodyId ) + { + return true; + } + + // Skip filtered shapes + bool canCollide = b2ShouldShapesCollide( fastShape->filter, shape->filter ); + if ( canCollide == false ) + { + return true; + } + + // Skip sensors + if ( shape->isSensor == true ) + { + return true; + } + + b2Body* body = b2BodyArray_Get( &world->bodies, shape->bodyId ); + b2BodySim* bodySim = b2GetBodySim( world, body ); + B2_ASSERT( body->type == b2_staticBody || fastBodySim->isBullet ); + + // Skip bullets + if ( bodySim->isBullet ) + { + return true; + } + + // Skip filtered bodies + b2Body* fastBody = b2BodyArray_Get( &world->bodies, fastBodySim->bodyId ); + canCollide = b2ShouldBodiesCollide( world, fastBody, body ); + if ( canCollide == false ) + { + return true; + } + + // Custom user filtering + b2CustomFilterFcn* customFilterFcn = world->customFilterFcn; + if ( customFilterFcn != NULL ) + { + b2ShapeId idA = { shape->id + 1, world->worldId, shape->revision }; + b2ShapeId idB = { fastShape->id + 1, world->worldId, fastShape->revision }; + canCollide = customFilterFcn( idA, idB, world->customFilterContext ); + if ( canCollide == false ) + { + return true; + } + } + + // Prevent pausing on chain segment junctions + if ( shape->type == b2_chainSegmentShape ) + { + b2Transform transform = bodySim->transform; + b2Vec2 p1 = b2TransformPoint( transform, shape->chainSegment.segment.point1 ); + b2Vec2 p2 = b2TransformPoint( transform, shape->chainSegment.segment.point2 ); + b2Vec2 e = b2Sub( p2, p1 ); + b2Vec2 c1 = continuousContext->centroid1; + b2Vec2 c2 = continuousContext->centroid2; + float offset1 = b2Cross( b2Sub( c1, p1 ), e ); + float offset2 = b2Cross( b2Sub( c2, p1 ), e ); + + if ( offset1 < 0.0f || offset2 > 0.0f ) + { + // Started behind or finished in front + return true; + } + } + + b2TOIInput input; + input.proxyA = b2MakeShapeDistanceProxy( shape ); + input.proxyB = b2MakeShapeDistanceProxy( fastShape ); + input.sweepA = b2MakeSweep( bodySim ); + input.sweepB = continuousContext->sweep; + input.tMax = continuousContext->fraction; + + b2TOIOutput output = b2TimeOfImpact( &input ); + if ( 0.0f < output.t && output.t < continuousContext->fraction ) + { + continuousContext->fraction = output.t; + } + else if ( 0.0f == output.t ) + { + // fallback to TOI of a small circle around the fast shape centroid + b2Vec2 centroid = b2GetShapeCentroid( fastShape ); + input.proxyB = b2MakeProxy( ¢roid, 1, b2_speculativeDistance ); + output = b2TimeOfImpact( &input ); + if ( 0.0f < output.t && output.t < continuousContext->fraction ) + { + continuousContext->fraction = output.t; + } + } + + return true; +} + +// Continuous collision of dynamic versus static +static void b2SolveContinuous( b2World* world, int bodySimIndex ) +{ + b2SolverSet* awakeSet = b2SolverSetArray_Get( &world->solverSets, b2_awakeSet ); + b2BodySim* fastBodySim = b2BodySimArray_Get( &awakeSet->bodySims, bodySimIndex ); + B2_ASSERT( fastBodySim->isFast ); + + b2Sweep sweep = b2MakeSweep( fastBodySim ); + + b2Transform xf1; + xf1.q = sweep.q1; + xf1.p = b2Sub( sweep.c1, b2RotateVector( sweep.q1, sweep.localCenter ) ); + + b2Transform xf2; + xf2.q = sweep.q2; + xf2.p = b2Sub( sweep.c2, b2RotateVector( sweep.q2, sweep.localCenter ) ); + + b2DynamicTree* staticTree = world->broadPhase.trees + b2_staticBody; + b2DynamicTree* kinematicTree = world->broadPhase.trees + b2_kinematicBody; + b2DynamicTree* dynamicTree = world->broadPhase.trees + b2_dynamicBody; + + struct b2ContinuousContext context; + context.world = world; + context.sweep = sweep; + context.fastBodySim = fastBodySim; + context.fraction = 1.0f; + + bool isBullet = fastBodySim->isBullet; + + b2Body* fastBody = b2BodyArray_Get( &world->bodies, fastBodySim->bodyId ); + int shapeId = fastBody->headShapeId; + while ( shapeId != B2_NULL_INDEX ) + { + b2Shape* fastShape = b2ShapeArray_Get( &world->shapes, shapeId ); + B2_ASSERT( fastShape->isFast == true ); + + shapeId = fastShape->nextShapeId; + + // Clear flag (keep set on body) + fastShape->isFast = false; + + context.fastShape = fastShape; + context.centroid1 = b2TransformPoint( xf1, fastShape->localCentroid ); + context.centroid2 = b2TransformPoint( xf2, fastShape->localCentroid ); + + b2AABB box1 = fastShape->aabb; + b2AABB box2 = b2ComputeShapeAABB( fastShape, xf2 ); + b2AABB box = b2AABB_Union( box1, box2 ); + + // Store this for later + fastShape->aabb = box2; + + // No continuous collision for sensors + if ( fastShape->isSensor ) + { + continue; + } + + b2DynamicTree_Query( staticTree, box, b2_defaultMaskBits, b2ContinuousQueryCallback, &context ); + + if ( isBullet ) + { + b2DynamicTree_Query( kinematicTree, box, b2_defaultMaskBits, b2ContinuousQueryCallback, &context ); + b2DynamicTree_Query( dynamicTree, box, b2_defaultMaskBits, b2ContinuousQueryCallback, &context ); + } + } + + const float speculativeDistance = b2_speculativeDistance; + const float aabbMargin = b2_aabbMargin; + + if ( context.fraction < 1.0f ) + { + // Handle time of impact event + b2Rot q = b2NLerp( sweep.q1, sweep.q2, context.fraction ); + b2Vec2 c = b2Lerp( sweep.c1, sweep.c2, context.fraction ); + b2Vec2 origin = b2Sub( c, b2RotateVector( q, sweep.localCenter ) ); + + // Advance body + b2Transform transform = { origin, q }; + fastBodySim->transform = transform; + fastBodySim->center = c; + fastBodySim->rotation0 = q; + fastBodySim->center0 = c; + + // Prepare AABBs for broad-phase + shapeId = fastBody->headShapeId; + while ( shapeId != B2_NULL_INDEX ) + { + b2Shape* shape = b2ShapeArray_Get( &world->shapes, shapeId ); + + // Must recompute aabb at the interpolated transform + b2AABB aabb = b2ComputeShapeAABB( shape, transform ); + aabb.lowerBound.x -= speculativeDistance; + aabb.lowerBound.y -= speculativeDistance; + aabb.upperBound.x += speculativeDistance; + aabb.upperBound.y += speculativeDistance; + shape->aabb = aabb; + + if ( b2AABB_Contains( shape->fatAABB, aabb ) == false ) + { + b2AABB fatAABB; + fatAABB.lowerBound.x = aabb.lowerBound.x - aabbMargin; + fatAABB.lowerBound.y = aabb.lowerBound.y - aabbMargin; + fatAABB.upperBound.x = aabb.upperBound.x + aabbMargin; + fatAABB.upperBound.y = aabb.upperBound.y + aabbMargin; + shape->fatAABB = fatAABB; + + shape->enlargedAABB = true; + fastBodySim->enlargeAABB = true; + } + + shapeId = shape->nextShapeId; + } + } + else + { + // No time of impact event + + // Advance body + fastBodySim->rotation0 = fastBodySim->transform.q; + fastBodySim->center0 = fastBodySim->center; + + // Prepare AABBs for broad-phase + shapeId = fastBody->headShapeId; + while ( shapeId != B2_NULL_INDEX ) + { + b2Shape* shape = b2ShapeArray_Get( &world->shapes, shapeId ); + + // shape->aabb is still valid + + if ( b2AABB_Contains( shape->fatAABB, shape->aabb ) == false ) + { + b2AABB fatAABB; + fatAABB.lowerBound.x = shape->aabb.lowerBound.x - aabbMargin; + fatAABB.lowerBound.y = shape->aabb.lowerBound.y - aabbMargin; + fatAABB.upperBound.x = shape->aabb.upperBound.x + aabbMargin; + fatAABB.upperBound.y = shape->aabb.upperBound.y + aabbMargin; + shape->fatAABB = fatAABB; + + shape->enlargedAABB = true; + fastBodySim->enlargeAABB = true; + } + + shapeId = shape->nextShapeId; + } + } +} + +static void b2FastBodyTask( int startIndex, int endIndex, uint32_t threadIndex, void* taskContext ) +{ + B2_MAYBE_UNUSED( threadIndex ); + + b2TracyCZoneNC( fast_body_task, "Fast Body Task", b2_colorCyan, true ); + + b2StepContext* stepContext = taskContext; + + B2_ASSERT( startIndex <= endIndex ); + + for ( int i = startIndex; i < endIndex; ++i ) + { + int simIndex = stepContext->fastBodies[i]; + b2SolveContinuous( stepContext->world, simIndex ); + } + + b2TracyCZoneEnd( fast_body_task ); +} + +static void b2BulletBodyTask( int startIndex, int endIndex, uint32_t threadIndex, void* taskContext ) +{ + B2_MAYBE_UNUSED( threadIndex ); + + b2TracyCZoneNC( bullet_body_task, "Bullet Body Task", b2_colorLightSkyBlue, true ); + + b2StepContext* stepContext = taskContext; + + B2_ASSERT( startIndex <= endIndex ); + + for ( int i = startIndex; i < endIndex; ++i ) + { + int simIndex = stepContext->bulletBodies[i]; + b2SolveContinuous( stepContext->world, simIndex ); + } + + b2TracyCZoneEnd( bullet_body_task ); +} + +#if B2_SIMD_WIDTH == 8 +#define B2_SIMD_SHIFT 3 +#else +#define B2_SIMD_SHIFT 2 +#endif + +// Solve with graph coloring +void b2Solve( b2World* world, b2StepContext* stepContext ) +{ + b2Timer timer = b2CreateTimer(); + + world->stepIndex += 1; + + b2MergeAwakeIslands( world ); + + world->profile.buildIslands = b2GetMillisecondsAndReset( &timer ); + + b2SolverSet* awakeSet = b2SolverSetArray_Get( &world->solverSets, b2_awakeSet ); + int awakeBodyCount = awakeSet->bodySims.count; + if ( awakeBodyCount == 0 ) + { + // Nothing to simulate, however I must still finish the broad-phase rebuild. + if ( world->userTreeTask != NULL ) + { + world->finishTaskFcn( world->userTreeTask, world->userTaskContext ); + world->userTreeTask = NULL; + world->activeTaskCount -= 1; + } + + b2ValidateNoEnlarged( &world->broadPhase ); + return; + } + + b2TracyCZoneNC( solve, "Solve", b2_colorMistyRose, true ); + + // Prepare buffers for continuous collision (fast bodies) + stepContext->fastBodyCount = 0; + stepContext->fastBodies = b2AllocateStackItem( &world->stackAllocator, awakeBodyCount * sizeof( int ), "fast bodies" ); + stepContext->bulletBodyCount = 0; + stepContext->bulletBodies = b2AllocateStackItem( &world->stackAllocator, awakeBodyCount * sizeof( int ), "bullet bodies" ); + + b2TracyCZoneNC( graph_solver, "Graph", b2_colorSeaGreen, true ); + + // Solve constraints using graph coloring + { + b2TracyCZoneNC( prepare_stages, "Prepare Stages", b2_colorDarkOrange, true ); + + b2ConstraintGraph* graph = &world->constraintGraph; + b2GraphColor* colors = graph->colors; + + stepContext->sims = awakeSet->bodySims.data; + stepContext->states = awakeSet->bodyStates.data; + + // count contacts, joints, and colors + int awakeContactCount = 0; + int awakeJointCount = 0; + int activeColorCount = 0; + for ( int i = 0; i < b2_graphColorCount - 1; ++i ) + { + int perColorContactCount = colors[i].contactSims.count; + int perColorJointCount = colors[i].jointSims.count; + int occupancyCount = perColorContactCount + perColorJointCount; + activeColorCount += occupancyCount > 0 ? 1 : 0; + awakeContactCount += perColorContactCount; + awakeJointCount += perColorJointCount; + } + + // Deal with void** + { + b2BodyMoveEventArray_Resize( &world->bodyMoveEvents, awakeBodyCount ); + } + + // Each worker receives at most M blocks of work. The workers may receive less than there is not sufficient work. + // Each block of work has a minimum number of elements (block size). This in turn may limit number of blocks. + // If there are many elements then the block size is increased so there are still at most M blocks of work per worker. + // M is a tunable number that has two goals: + // 1. keep M small to reduce overhead + // 2. keep M large enough for other workers to be able to steal work + // The block size is a power of two to make math efficient. + + int workerCount = world->workerCount; + const int blocksPerWorker = 4; + const int maxBlockCount = blocksPerWorker * workerCount; + + // Configure blocks for tasks that parallel-for bodies + int bodyBlockSize = 1 << 5; + int bodyBlockCount; + if ( awakeBodyCount > bodyBlockSize * maxBlockCount ) + { + // Too many blocks, increase block size + bodyBlockSize = awakeBodyCount / maxBlockCount; + bodyBlockCount = maxBlockCount; + } + else + { + bodyBlockCount = ( ( awakeBodyCount - 1 ) >> 5 ) + 1; + } + + // Configure blocks for tasks parallel-for each active graph color + // The blocks are a mix of SIMD contact blocks and joint blocks + int activeColorIndices[b2_graphColorCount]; + + int colorContactCounts[b2_graphColorCount]; + int colorContactBlockSizes[b2_graphColorCount]; + int colorContactBlockCounts[b2_graphColorCount]; + + int colorJointCounts[b2_graphColorCount]; + int colorJointBlockSizes[b2_graphColorCount]; + int colorJointBlockCounts[b2_graphColorCount]; + + int graphBlockCount = 0; + + // c is the active color index + int simdContactCount = 0; + int c = 0; + for ( int i = 0; i < b2_graphColorCount - 1; ++i ) + { + int colorContactCount = colors[i].contactSims.count; + int colorJointCount = colors[i].jointSims.count; + + if ( colorContactCount + colorJointCount > 0 ) + { + activeColorIndices[c] = i; + + // 4/8-way SIMD + int colorContactCountSIMD = colorContactCount > 0 ? ( ( colorContactCount - 1 ) >> B2_SIMD_SHIFT ) + 1 : 0; + + colorContactCounts[c] = colorContactCountSIMD; + + // determine the number of contact work blocks for this color + if ( colorContactCountSIMD > blocksPerWorker * maxBlockCount ) + { + // too many contact blocks + colorContactBlockSizes[c] = colorContactCountSIMD / maxBlockCount; + colorContactBlockCounts[c] = maxBlockCount; + } + else if ( colorContactCountSIMD > 0 ) + { + // dividing by blocksPerWorker (4) + colorContactBlockSizes[c] = blocksPerWorker; + colorContactBlockCounts[c] = ( ( colorContactCountSIMD - 1 ) >> 2 ) + 1; + } + else + { + // no contacts in this color + colorContactBlockSizes[c] = 0; + colorContactBlockCounts[c] = 0; + } + + colorJointCounts[c] = colorJointCount; + + // determine number of joint work blocks for this color + if ( colorJointCount > blocksPerWorker * maxBlockCount ) + { + // too many joint blocks + colorJointBlockSizes[c] = colorJointCount / maxBlockCount; + colorJointBlockCounts[c] = maxBlockCount; + } + else if ( colorJointCount > 0 ) + { + // dividing by blocksPerWorker (4) + colorJointBlockSizes[c] = blocksPerWorker; + colorJointBlockCounts[c] = ( ( colorJointCount - 1 ) >> 2 ) + 1; + } + else + { + colorJointBlockSizes[c] = 0; + colorJointBlockCounts[c] = 0; + } + + graphBlockCount += colorContactBlockCounts[c] + colorJointBlockCounts[c]; + simdContactCount += colorContactCountSIMD; + c += 1; + } + } + activeColorCount = c; + + // Gather contact pointers for easy parallel-for traversal. Some may be NULL due to SIMD remainders. + b2ContactSim** contacts = b2AllocateStackItem( + &world->stackAllocator, B2_SIMD_WIDTH * simdContactCount * sizeof( b2ContactSim* ), "contact pointers" ); + + // Gather joint pointers for easy parallel-for traversal. + b2JointSim** joints = + b2AllocateStackItem( &world->stackAllocator, awakeJointCount * sizeof( b2JointSim* ), "joint pointers" ); + + int simdConstraintSize = b2GetContactConstraintSIMDByteCount(); + b2ContactConstraintSIMD* simdContactConstraints = + b2AllocateStackItem( &world->stackAllocator, simdContactCount * simdConstraintSize, "contact constraint" ); + + int overflowContactCount = colors[b2_overflowIndex].contactSims.count; + b2ContactConstraint* overflowContactConstraints = b2AllocateStackItem( + &world->stackAllocator, overflowContactCount * sizeof( b2ContactConstraint ), "overflow contact constraint" ); + + graph->colors[b2_overflowIndex].overflowConstraints = overflowContactConstraints; + + // Distribute transient constraints to each graph color and build flat arrays of contact and joint pointers + { + int contactBase = 0; + int jointBase = 0; + for ( int i = 0; i < activeColorCount; ++i ) + { + int j = activeColorIndices[i]; + b2GraphColor* color = colors + j; + + int colorContactCount = color->contactSims.count; + + if ( colorContactCount == 0 ) + { + color->simdConstraints = NULL; + } + else + { + color->simdConstraints = + (b2ContactConstraintSIMD*)( (uint8_t*)simdContactConstraints + contactBase * simdConstraintSize ); + + for ( int k = 0; k < colorContactCount; ++k ) + { + contacts[B2_SIMD_WIDTH * contactBase + k] = color->contactSims.data + k; + } + + // remainder + int colorContactCountSIMD = ( ( colorContactCount - 1 ) >> B2_SIMD_SHIFT ) + 1; + for ( int k = colorContactCount; k < B2_SIMD_WIDTH * colorContactCountSIMD; ++k ) + { + contacts[B2_SIMD_WIDTH * contactBase + k] = NULL; + } + + contactBase += colorContactCountSIMD; + } + + int colorJointCount = color->jointSims.count; + for ( int k = 0; k < colorJointCount; ++k ) + { + joints[jointBase + k] = color->jointSims.data + k; + } + jointBase += colorJointCount; + } + + B2_ASSERT( contactBase == simdContactCount ); + B2_ASSERT( jointBase == awakeJointCount ); + } + + // Define work blocks for preparing contacts and storing contact impulses + int contactBlockSize = blocksPerWorker; + int contactBlockCount = simdContactCount > 0 ? ( ( simdContactCount - 1 ) >> 2 ) + 1 : 0; + if ( simdContactCount > contactBlockSize * maxBlockCount ) + { + // Too many blocks, increase block size + contactBlockSize = simdContactCount / maxBlockCount; + contactBlockCount = maxBlockCount; + } + + // Define work blocks for preparing joints + int jointBlockSize = blocksPerWorker; + int jointBlockCount = awakeJointCount > 0 ? ( ( awakeJointCount - 1 ) >> 2 ) + 1 : 0; + if ( awakeJointCount > jointBlockSize * maxBlockCount ) + { + // Too many blocks, increase block size + jointBlockSize = awakeJointCount / maxBlockCount; + jointBlockCount = maxBlockCount; + } + + int stageCount = 0; + + // b2_stagePrepareJoints + stageCount += 1; + // b2_stagePrepareContacts + stageCount += 1; + // b2_stageIntegrateVelocities + stageCount += 1; + // b2_stageWarmStart + stageCount += activeColorCount; + // b2_stageSolve + stageCount += activeColorCount; + // b2_stageIntegratePositions + stageCount += 1; + // b2_stageRelax + stageCount += activeColorCount; + // b2_stageRestitution + stageCount += activeColorCount; + // b2_stageStoreImpulses + stageCount += 1; + + b2SolverStage* stages = b2AllocateStackItem( &world->stackAllocator, stageCount * sizeof( b2SolverStage ), "stages" ); + b2SolverBlock* bodyBlocks = + b2AllocateStackItem( &world->stackAllocator, bodyBlockCount * sizeof( b2SolverBlock ), "body blocks" ); + b2SolverBlock* contactBlocks = + b2AllocateStackItem( &world->stackAllocator, contactBlockCount * sizeof( b2SolverBlock ), "contact blocks" ); + b2SolverBlock* jointBlocks = + b2AllocateStackItem( &world->stackAllocator, jointBlockCount * sizeof( b2SolverBlock ), "joint blocks" ); + b2SolverBlock* graphBlocks = + b2AllocateStackItem( &world->stackAllocator, graphBlockCount * sizeof( b2SolverBlock ), "graph blocks" ); + + // Split an awake island. This modifies: + // - stack allocator + // - world island array and solver set + // - island indices on bodies, contacts, and joints + // I'm squeezing this task in here because it may be expensive and this is a safe place to put it. + // Note: cannot split islands in parallel with FinalizeBodies + void* splitIslandTask = NULL; + if ( world->splitIslandId != B2_NULL_INDEX ) + { + splitIslandTask = world->enqueueTaskFcn( &b2SplitIslandTask, 1, 1, world, world->userTaskContext ); + world->taskCount += 1; + world->activeTaskCount += splitIslandTask == NULL ? 0 : 1; + } + + // Prepare body work blocks + for ( int i = 0; i < bodyBlockCount; ++i ) + { + b2SolverBlock* block = bodyBlocks + i; + block->startIndex = i * bodyBlockSize; + block->count = (int16_t)bodyBlockSize; + block->blockType = b2_bodyBlock; + block->syncIndex = 0; + } + bodyBlocks[bodyBlockCount - 1].count = (int16_t)( awakeBodyCount - ( bodyBlockCount - 1 ) * bodyBlockSize ); + + // Prepare joint work blocks + for ( int i = 0; i < jointBlockCount; ++i ) + { + b2SolverBlock* block = jointBlocks + i; + block->startIndex = i * jointBlockSize; + block->count = (int16_t)jointBlockSize; + block->blockType = b2_jointBlock; + block->syncIndex = 0; + } + + if ( jointBlockCount > 0 ) + { + jointBlocks[jointBlockCount - 1].count = (int16_t)( awakeJointCount - ( jointBlockCount - 1 ) * jointBlockSize ); + } + + // Prepare contact work blocks + for ( int i = 0; i < contactBlockCount; ++i ) + { + b2SolverBlock* block = contactBlocks + i; + block->startIndex = i * contactBlockSize; + block->count = (int16_t)contactBlockSize; + block->blockType = b2_contactBlock; + block->syncIndex = 0; + } + + if ( contactBlockCount > 0 ) + { + contactBlocks[contactBlockCount - 1].count = + (int16_t)( simdContactCount - ( contactBlockCount - 1 ) * contactBlockSize ); + } + + // Prepare graph work blocks + b2SolverBlock* graphColorBlocks[b2_graphColorCount]; + b2SolverBlock* baseGraphBlock = graphBlocks; + + for ( int i = 0; i < activeColorCount; ++i ) + { + graphColorBlocks[i] = baseGraphBlock; + + int colorJointBlockCount = colorJointBlockCounts[i]; + int colorJointBlockSize = colorJointBlockSizes[i]; + for ( int j = 0; j < colorJointBlockCount; ++j ) + { + b2SolverBlock* block = baseGraphBlock + j; + block->startIndex = j * colorJointBlockSize; + block->count = (int16_t)colorJointBlockSize; + block->blockType = b2_graphJointBlock; + block->syncIndex = 0; + } + + if ( colorJointBlockCount > 0 ) + { + baseGraphBlock[colorJointBlockCount - 1].count = + (int16_t)( colorJointCounts[i] - ( colorJointBlockCount - 1 ) * colorJointBlockSize ); + baseGraphBlock += colorJointBlockCount; + } + + int colorContactBlockCount = colorContactBlockCounts[i]; + int colorContactBlockSize = colorContactBlockSizes[i]; + for ( int j = 0; j < colorContactBlockCount; ++j ) + { + b2SolverBlock* block = baseGraphBlock + j; + block->startIndex = j * colorContactBlockSize; + block->count = (int16_t)colorContactBlockSize; + block->blockType = b2_graphContactBlock; + block->syncIndex = 0; + } + + if ( colorContactBlockCount > 0 ) + { + baseGraphBlock[colorContactBlockCount - 1].count = + (int16_t)( colorContactCounts[i] - ( colorContactBlockCount - 1 ) * colorContactBlockSize ); + baseGraphBlock += colorContactBlockCount; + } + } + + ptrdiff_t blockDiff = baseGraphBlock - graphBlocks; + B2_ASSERT( blockDiff == graphBlockCount ); + + b2SolverStage* stage = stages; + + // Prepare joints + stage->type = b2_stagePrepareJoints; + stage->blocks = jointBlocks; + stage->blockCount = jointBlockCount; + stage->colorIndex = -1; + stage->completionCount = 0; + stage += 1; + + // Prepare contacts + stage->type = b2_stagePrepareContacts; + stage->blocks = contactBlocks; + stage->blockCount = contactBlockCount; + stage->colorIndex = -1; + stage->completionCount = 0; + stage += 1; + + // Integrate velocities + stage->type = b2_stageIntegrateVelocities; + stage->blocks = bodyBlocks; + stage->blockCount = bodyBlockCount; + stage->colorIndex = -1; + stage->completionCount = 0; + stage += 1; + + // Warm start + for ( int i = 0; i < activeColorCount; ++i ) + { + stage->type = b2_stageWarmStart; + stage->blocks = graphColorBlocks[i]; + stage->blockCount = colorJointBlockCounts[i] + colorContactBlockCounts[i]; + stage->colorIndex = activeColorIndices[i]; + stage->completionCount = 0; + stage += 1; + } + + // Solve graph + for ( int i = 0; i < activeColorCount; ++i ) + { + stage->type = b2_stageSolve; + stage->blocks = graphColorBlocks[i]; + stage->blockCount = colorJointBlockCounts[i] + colorContactBlockCounts[i]; + stage->colorIndex = activeColorIndices[i]; + stage->completionCount = 0; + stage += 1; + } + + // Integrate positions + stage->type = b2_stageIntegratePositions; + stage->blocks = bodyBlocks; + stage->blockCount = bodyBlockCount; + stage->colorIndex = -1; + stage->completionCount = 0; + stage += 1; + + // Relax constraints + for ( int i = 0; i < activeColorCount; ++i ) + { + stage->type = b2_stageRelax; + stage->blocks = graphColorBlocks[i]; + stage->blockCount = colorJointBlockCounts[i] + colorContactBlockCounts[i]; + stage->colorIndex = activeColorIndices[i]; + stage->completionCount = 0; + stage += 1; + } + + // Restitution + // Note: joint blocks mixed in, could have joint limit restitution + for ( int i = 0; i < activeColorCount; ++i ) + { + stage->type = b2_stageRestitution; + stage->blocks = graphColorBlocks[i]; + stage->blockCount = colorJointBlockCounts[i] + colorContactBlockCounts[i]; + stage->colorIndex = activeColorIndices[i]; + stage->completionCount = 0; + stage += 1; + } + + // Store impulses + stage->type = b2_stageStoreImpulses; + stage->blocks = contactBlocks; + stage->blockCount = contactBlockCount; + stage->colorIndex = -1; + stage->completionCount = 0; + stage += 1; + + B2_ASSERT( (int)( stage - stages ) == stageCount ); + + B2_ASSERT( workerCount <= b2_maxWorkers ); + b2WorkerContext workerContext[b2_maxWorkers]; + + stepContext->graph = graph; + stepContext->joints = joints; + stepContext->contacts = contacts; + stepContext->simdContactConstraints = simdContactConstraints; + stepContext->activeColorCount = activeColorCount; + stepContext->workerCount = workerCount; + stepContext->stageCount = stageCount; + stepContext->stages = stages; + stepContext->atomicSyncBits = 0; + + world->profile.prepareTasks = b2GetMillisecondsAndReset( &timer ); + + b2TracyCZoneEnd( prepare_stages ); + + // Must use worker index because thread 0 can be assigned multiple tasks by enkiTS + for ( int i = 0; i < workerCount; ++i ) + { + workerContext[i].context = stepContext; + workerContext[i].workerIndex = i; + workerContext[i].userTask = world->enqueueTaskFcn( b2SolverTask, 1, 1, workerContext + i, world->userTaskContext ); + world->taskCount += 1; + world->activeTaskCount += workerContext[i].userTask == NULL ? 0 : 1; + } + + // Finish island split + if ( splitIslandTask != NULL ) + { + world->finishTaskFcn( splitIslandTask, world->userTaskContext ); + world->activeTaskCount -= 1; + } + world->splitIslandId = B2_NULL_INDEX; + + // Finish constraint solve + for ( int i = 0; i < workerCount; ++i ) + { + if ( workerContext[i].userTask != NULL ) + { + world->finishTaskFcn( workerContext[i].userTask, world->userTaskContext ); + world->activeTaskCount -= 1; + } + } + + world->profile.solverTasks = b2GetMillisecondsAndReset( &timer ); + + // Prepare contact, enlarged body, and island bit sets used in body finalization. + int awakeIslandCount = awakeSet->islandSims.count; + for ( int i = 0; i < world->workerCount; ++i ) + { + b2TaskContext* taskContext = world->taskContexts.data + i; + b2SetBitCountAndClear( &taskContext->enlargedSimBitSet, awakeBodyCount ); + b2SetBitCountAndClear( &taskContext->awakeIslandBitSet, awakeIslandCount ); + taskContext->splitIslandId = B2_NULL_INDEX; + taskContext->splitSleepTime = 0.0f; + } + + // Finalize bodies. Must happen after the constraint solver and after island splitting. + void* finalizeBodiesTask = + world->enqueueTaskFcn( b2FinalizeBodiesTask, awakeBodyCount, 64, stepContext, world->userTaskContext ); + world->taskCount += 1; + if ( finalizeBodiesTask != NULL ) + { + world->finishTaskFcn( finalizeBodiesTask, world->userTaskContext ); + } + + world->profile.finalizeBodies = b2GetMillisecondsAndReset( &timer ); + + b2FreeStackItem( &world->stackAllocator, graphBlocks ); + b2FreeStackItem( &world->stackAllocator, jointBlocks ); + b2FreeStackItem( &world->stackAllocator, contactBlocks ); + b2FreeStackItem( &world->stackAllocator, bodyBlocks ); + b2FreeStackItem( &world->stackAllocator, stages ); + b2FreeStackItem( &world->stackAllocator, overflowContactConstraints ); + b2FreeStackItem( &world->stackAllocator, simdContactConstraints ); + b2FreeStackItem( &world->stackAllocator, joints ); + b2FreeStackItem( &world->stackAllocator, contacts ); + } + + b2TracyCZoneEnd( graph_solver ); + world->profile.solveConstraints = b2GetMillisecondsAndReset( &timer ); + + // Report hit events + // todo perhaps optimize this with a bitset + { + b2TracyCZoneNC( hit_events, "Hit", b2_colorVioletRed, true ); + + B2_ASSERT( world->contactHitEvents.count == 0 ); + + float threshold = world->hitEventThreshold; + b2GraphColor* colors = world->constraintGraph.colors; + for ( int i = 0; i < b2_graphColorCount; ++i ) + { + b2GraphColor* color = colors + i; + int contactCount = color->contactSims.count; + b2ContactSim* contactSims = color->contactSims.data; + for ( int j = 0; j < contactCount; ++j ) + { + b2ContactSim* contactSim = contactSims + j; + if ( ( contactSim->simFlags & b2_simEnableHitEvent ) == 0 ) + { + continue; + } + + b2ContactHitEvent event = { 0 }; + event.approachSpeed = threshold; + + bool hit = false; + int pointCount = contactSim->manifold.pointCount; + for ( int k = 0; k < pointCount; ++k ) + { + b2ManifoldPoint* mp = contactSim->manifold.points + k; + float approachSpeed = -mp->normalVelocity; + if ( approachSpeed > event.approachSpeed && mp->normalImpulse > 0.0f ) + { + event.approachSpeed = approachSpeed; + event.point = mp->point; + hit = true; + } + } + + if ( hit == true ) + { + event.normal = contactSim->manifold.normal; + + b2Shape* shapeA = b2ShapeArray_Get( &world->shapes, contactSim->shapeIdA ); + b2Shape* shapeB = b2ShapeArray_Get( &world->shapes, contactSim->shapeIdB ); + + event.shapeIdA = ( b2ShapeId ){ shapeA->id + 1, world->worldId, shapeA->revision }; + event.shapeIdB = ( b2ShapeId ){ shapeB->id + 1, world->worldId, shapeB->revision }; + + b2ContactHitEventArray_Push( &world->contactHitEvents, event ); + } + } + } + + b2TracyCZoneEnd( hit_events ); + } + + world->profile.hitEvents = b2GetMillisecondsAndReset( &timer ); + + // Finish the user tree task that was queued earlier in the time step. This must be complete before touching the broad-phase. + if ( world->userTreeTask != NULL ) + { + world->finishTaskFcn( world->userTreeTask, world->userTaskContext ); + world->userTreeTask = NULL; + world->activeTaskCount -= 1; + } + + b2ValidateNoEnlarged( &world->broadPhase ); + + b2TracyCZoneNC( broad_phase, "Broadphase", b2_colorPurple, true ); + + b2TracyCZoneNC( enlarge_proxies, "Enlarge Proxies", b2_colorDarkTurquoise, true ); + + // Gather bits for all sim bodies that have enlarged AABBs + b2BitSet* simBitSet = &world->taskContexts.data[0].enlargedSimBitSet; + for ( int i = 1; i < world->workerCount; ++i ) + { + b2InPlaceUnion( simBitSet, &world->taskContexts.data[i].enlargedSimBitSet ); + } + + // Enlarge broad-phase proxies and build move array + // Apply shape AABB changes to broad-phase. This also create the move array which must be + // in deterministic order. I'm tracking sim bodies because the number of shape ids can be huge. + { + b2BroadPhase* broadPhase = &world->broadPhase; + uint32_t wordCount = simBitSet->blockCount; + uint64_t* bits = simBitSet->bits; + + // Fast array access is important here + b2Body* bodyArray = world->bodies.data; + b2BodySim* bodySimArray = awakeSet->bodySims.data; + b2Shape* shapeArray = world->shapes.data; + + for ( uint32_t k = 0; k < wordCount; ++k ) + { + uint64_t word = bits[k]; + while ( word != 0 ) + { + uint32_t ctz = b2CTZ64( word ); + uint32_t bodySimIndex = 64 * k + ctz; + + b2BodySim* bodySim = bodySimArray + bodySimIndex; + b2Body* body = bodyArray + bodySim->bodyId; + + int shapeId = body->headShapeId; + while ( shapeId != B2_NULL_INDEX ) + { + b2Shape* shape = shapeArray + shapeId; + + if ( shape->enlargedAABB ) + { + B2_ASSERT( shape->isFast == false ); + + b2BroadPhase_EnlargeProxy( broadPhase, shape->proxyKey, shape->fatAABB ); + shape->enlargedAABB = false; + } + else if ( shape->isFast ) + { + // Shape is fast. It's aabb will be enlarged in continuous collision. + b2BufferMove( broadPhase, shape->proxyKey ); + } + + shapeId = shape->nextShapeId; + } + + // Clear the smallest set bit + word = word & ( word - 1 ); + } + } + } + + b2TracyCZoneEnd( enlarge_proxies ); + + b2ValidateBroadphase( &world->broadPhase ); + + world->profile.broadphase = b2GetMillisecondsAndReset( &timer ); + + b2TracyCZoneEnd( broad_phase ); + + b2TracyCZoneNC( continuous_collision, "Continuous", b2_colorDarkGoldenrod, true ); + + // Parallel continuous collision + if ( stepContext->fastBodyCount > 0 ) + { + // fast bodies + int minRange = 8; + void* userFastBodyTask = + world->enqueueTaskFcn( &b2FastBodyTask, stepContext->fastBodyCount, minRange, stepContext, world->userTaskContext ); + world->taskCount += 1; + if ( userFastBodyTask != NULL ) + { + world->finishTaskFcn( userFastBodyTask, world->userTaskContext ); + } + } + + // Serially enlarge broad-phase proxies for fast shapes + // Doing this here so that bullet shapes see them + { + b2BroadPhase* broadPhase = &world->broadPhase; + b2DynamicTree* dynamicTree = broadPhase->trees + b2_dynamicBody; + + // Fast array access is important here + b2Body* bodyArray = world->bodies.data; + b2BodySim* bodySimArray = awakeSet->bodySims.data; + b2Shape* shapeArray = world->shapes.data; + + int* fastBodySimIndices = stepContext->fastBodies; + int fastBodyCount = stepContext->fastBodyCount; + + // This loop has non-deterministic order but it shouldn't affect the result + for ( int i = 0; i < fastBodyCount; ++i ) + { + b2BodySim* fastBodySim = bodySimArray + fastBodySimIndices[i]; + if ( fastBodySim->enlargeAABB == false ) + { + continue; + } + + // clear flag + fastBodySim->enlargeAABB = false; + + int bodyId = fastBodySim->bodyId; + + B2_ASSERT( 0 <= bodyId && bodyId < world->bodies.count ); + b2Body* fastBody = bodyArray + bodyId; + + int shapeId = fastBody->headShapeId; + while ( shapeId != B2_NULL_INDEX ) + { + b2Shape* shape = shapeArray + shapeId; + if ( shape->enlargedAABB == false ) + { + shapeId = shape->nextShapeId; + continue; + } + + // clear flag + shape->enlargedAABB = false; + + int proxyKey = shape->proxyKey; + int proxyId = B2_PROXY_ID( proxyKey ); + B2_ASSERT( B2_PROXY_TYPE( proxyKey ) == b2_dynamicBody ); + + // all fast shapes should already be in the move buffer + B2_ASSERT( b2ContainsKey( &broadPhase->moveSet, proxyKey + 1 ) ); + + b2DynamicTree_EnlargeProxy( dynamicTree, proxyId, shape->fatAABB ); + + shapeId = shape->nextShapeId; + } + } + } + + if ( stepContext->bulletBodyCount > 0 ) + { + // bullet bodies + int minRange = 8; + void* userBulletBodyTask = world->enqueueTaskFcn( &b2BulletBodyTask, stepContext->bulletBodyCount, minRange, stepContext, + world->userTaskContext ); + world->taskCount += 1; + if ( userBulletBodyTask != NULL ) + { + world->finishTaskFcn( userBulletBodyTask, world->userTaskContext ); + } + } + + // Serially enlarge broad-phase proxies for bullet shapes + { + b2BroadPhase* broadPhase = &world->broadPhase; + b2DynamicTree* dynamicTree = broadPhase->trees + b2_dynamicBody; + + // Fast array access is important here + b2Body* bodyArray = world->bodies.data; + b2BodySim* bodySimArray = awakeSet->bodySims.data; + b2Shape* shapeArray = world->shapes.data; + + // Serially enlarge broad-phase proxies for bullet shapes + int* bulletBodySimIndices = stepContext->bulletBodies; + int bulletBodyCount = stepContext->bulletBodyCount; + + // This loop has non-deterministic order but it shouldn't affect the result + for ( int i = 0; i < bulletBodyCount; ++i ) + { + b2BodySim* bulletBodySim = bodySimArray + bulletBodySimIndices[i]; + if ( bulletBodySim->enlargeAABB == false ) + { + continue; + } + + // clear flag + bulletBodySim->enlargeAABB = false; + + int bodyId = bulletBodySim->bodyId; + B2_ASSERT( 0 <= bodyId && bodyId < world->bodies.count ); + b2Body* bulletBody = bodyArray + bodyId; + + int shapeId = bulletBody->headShapeId; + while ( shapeId != B2_NULL_INDEX ) + { + b2Shape* shape = shapeArray + shapeId; + if ( shape->enlargedAABB == false ) + { + shapeId = shape->nextShapeId; + continue; + } + + // clear flag + shape->enlargedAABB = false; + + int proxyKey = shape->proxyKey; + int proxyId = B2_PROXY_ID( proxyKey ); + B2_ASSERT( B2_PROXY_TYPE( proxyKey ) == b2_dynamicBody ); + + // all fast shapes should already be in the move buffer + B2_ASSERT( b2ContainsKey( &broadPhase->moveSet, proxyKey + 1 ) ); + + b2DynamicTree_EnlargeProxy( dynamicTree, proxyId, shape->fatAABB ); + + shapeId = shape->nextShapeId; + } + } + } + + b2TracyCZoneEnd( continuous_collision ); + + b2FreeStackItem( &world->stackAllocator, stepContext->bulletBodies ); + stepContext->bulletBodies = NULL; + stepContext->bulletBodyCount = 0; + + b2FreeStackItem( &world->stackAllocator, stepContext->fastBodies ); + stepContext->fastBodies = NULL; + stepContext->fastBodyCount = 0; + + world->profile.continuous = b2GetMillisecondsAndReset( &timer ); + + // Island sleeping + // This must be done last because putting islands to sleep invalidates the enlarged body bits. + if ( world->enableSleep == true ) + { + b2TracyCZoneNC( sleep_islands, "Island Sleep", b2_colorGainsboro, true ); + + // Collect split island candidate for the next time step. No need to split if sleeping is disabled. + B2_ASSERT( world->splitIslandId == B2_NULL_INDEX ); + float splitSleepTimer = 0.0f; + for ( int i = 0; i < world->workerCount; ++i ) + { + b2TaskContext* taskContext = world->taskContexts.data + i; + if ( taskContext->splitIslandId != B2_NULL_INDEX && taskContext->splitSleepTime >= splitSleepTimer ) + { + B2_ASSERT( taskContext->splitSleepTime > 0.0f ); + + // Tie breaking for determinism. Largest island id wins. Needed due to work stealing. + if ( taskContext->splitSleepTime == splitSleepTimer && taskContext->splitIslandId < world->splitIslandId ) + { + continue; + } + + world->splitIslandId = taskContext->splitIslandId; + splitSleepTimer = taskContext->splitSleepTime; + } + } + + b2BitSet* awakeIslandBitSet = &world->taskContexts.data[0].awakeIslandBitSet; + for ( int i = 1; i < world->workerCount; ++i ) + { + b2InPlaceUnion( awakeIslandBitSet, &world->taskContexts.data[i].awakeIslandBitSet ); + } + + // Need to process in reverse because this moves islands to sleeping solver sets. + b2IslandSim* islands = awakeSet->islandSims.data; + int count = awakeSet->islandSims.count; + for ( int islandIndex = count - 1; islandIndex >= 0; islandIndex -= 1 ) + { + if ( b2GetBit( awakeIslandBitSet, islandIndex ) == true ) + { + // this island is still awake + continue; + } + + b2IslandSim* island = islands + islandIndex; + int islandId = island->islandId; + + b2TrySleepIsland( world, islandId ); + } + + b2ValidateSolverSets( world ); + + b2TracyCZoneEnd( sleep_islands ); + } + + world->profile.sleepIslands = b2GetMillisecondsAndReset( &timer ); + b2TracyCZoneEnd( solve ); +} diff --git a/3rdparty/box2d/src/solver.h b/3rdparty/box2d/src/solver.h new file mode 100644 index 000000000000..7d5cedd2226d --- /dev/null +++ b/3rdparty/box2d/src/solver.h @@ -0,0 +1,157 @@ +// SPDX-FileCopyrightText: 2023 Erin Catto +// SPDX-License-Identifier: MIT + +#pragma once + +#include "box2d/math_functions.h" + +#include +#include + +typedef struct b2BodySim b2BodySim; +typedef struct b2BodyState b2BodyState; +typedef struct b2ContactSim b2ContactSim; +typedef struct b2JointSim b2JointSim; +typedef struct b2World b2World; + +typedef struct b2Softness +{ + float biasRate; + float massScale; + float impulseScale; +} b2Softness; + +typedef enum b2SolverStageType +{ + b2_stagePrepareJoints, + b2_stagePrepareContacts, + b2_stageIntegrateVelocities, + b2_stageWarmStart, + b2_stageSolve, + b2_stageIntegratePositions, + b2_stageRelax, + b2_stageRestitution, + b2_stageStoreImpulses +} b2SolverStageType; + +typedef enum b2SolverBlockType +{ + b2_bodyBlock, + b2_jointBlock, + b2_contactBlock, + b2_graphJointBlock, + b2_graphContactBlock +} b2SolverBlockType; + +// Each block of work has a sync index that gets incremented when a worker claims the block. This ensures only a single worker +// claims a block, yet lets work be distributed dynamically across multiple workers (work stealing). This also reduces contention +// on a single block index atomic. For non-iterative stages the sync index is simply set to one. For iterative stages (solver +// iteration) the same block of work is executed once per iteration and the atomic sync index is shared across iterations, so it +// increases monotonically. +typedef struct b2SolverBlock +{ + int startIndex; + int16_t count; + int16_t blockType; // b2SolverBlockType + // todo consider false sharing of this atomic + _Atomic int syncIndex; +} b2SolverBlock; + +// Each stage must be completed before going to the next stage. +// Non-iterative stages use a stage instance once while iterative stages re-use the same instance each iteration. +typedef struct b2SolverStage +{ + b2SolverStageType type; + b2SolverBlock* blocks; + int blockCount; + int colorIndex; + // todo consider false sharing of this atomic + _Atomic int completionCount; +} b2SolverStage; + +// Context for a time step. Recreated each time step. +typedef struct b2StepContext +{ + // time step + float dt; + + // inverse time step (0 if dt == 0). + float inv_dt; + + // sub-step + float h; + float inv_h; + + int subStepCount; + + b2Softness jointSoftness; + b2Softness contactSoftness; + b2Softness staticSoftness; + + float restitutionThreshold; + float maxLinearVelocity; + + struct b2World* world; + struct b2ConstraintGraph* graph; + + // shortcut to body states from awake set + b2BodyState* states; + + // shortcut to body sims from awake set + b2BodySim* sims; + + // array of all shape ids for shapes that have enlarged AABBs + int* enlargedShapes; + int enlargedShapeCount; + + // Array of fast bodies that need continuous collision handling + int* fastBodies; + _Atomic int fastBodyCount; + + // Array of bullet bodies that need continuous collision handling + int* bulletBodies; + _Atomic int bulletBodyCount; + + // joint pointers for simplified parallel-for access. + b2JointSim** joints; + + // contact pointers for simplified parallel-for access. + // - parallel-for collide with no gaps + // - parallel-for prepare and store contacts with NULL gaps for SIMD remainders + // despite being an array of pointers, these are contiguous sub-arrays corresponding + // to constraint graph colors + b2ContactSim** contacts; + + struct b2ContactConstraintSIMD* simdContactConstraints; + int activeColorCount; + int workerCount; + + b2SolverStage* stages; + int stageCount; + bool enableWarmStarting; + + // todo padding to prevent false sharing + char dummy1[64]; + + // sync index (16-bits) | stage type (16-bits) + _Atomic unsigned int atomicSyncBits; + + char dummy2[64]; + +} b2StepContext; + +static inline b2Softness b2MakeSoft( float hertz, float zeta, float h ) +{ + if ( hertz == 0.0f ) + { + return ( b2Softness ){ 0.0f, 1.0f, 0.0f }; + } + + float omega = 2.0f * b2_pi * hertz; + float a1 = 2.0f * zeta + h * omega; + float a2 = h * omega * a1; + float a3 = 1.0f / ( 1.0f + a2 ); + return ( b2Softness ){ omega / a1, a2 * a3, a3 }; +} + +void b2Solve( b2World* world, b2StepContext* stepContext ); diff --git a/3rdparty/box2d/src/solver_set.c b/3rdparty/box2d/src/solver_set.c new file mode 100644 index 000000000000..4f59b08cb3d6 --- /dev/null +++ b/3rdparty/box2d/src/solver_set.c @@ -0,0 +1,613 @@ +// SPDX-FileCopyrightText: 2023 Erin Catto +// SPDX-License-Identifier: MIT + +#include "solver_set.h" + +#include "body.h" +#include "constraint_graph.h" +#include "contact.h" +#include "core.h" +#include "island.h" +#include "joint.h" +#include "world.h" + +#include + +B2_ARRAY_SOURCE( b2SolverSet, b2SolverSet ); + +void b2DestroySolverSet( b2World* world, int setIndex ) +{ + b2SolverSet* set = b2SolverSetArray_Get( &world->solverSets, setIndex ); + b2BodySimArray_Destroy( &set->bodySims ); + b2BodyStateArray_Destroy( &set->bodyStates ); + b2ContactSimArray_Destroy( &set->contactSims ); + b2JointSimArray_Destroy( &set->jointSims ); + b2IslandSimArray_Destroy( &set->islandSims ); + b2FreeId( &world->solverSetIdPool, setIndex ); + *set = ( b2SolverSet ){ 0 }; + set->setIndex = B2_NULL_INDEX; +} + +// Wake a solver set. Does not merge islands. +// Contacts can be in several places: +// 1. non-touching contacts in the disabled set +// 2. non-touching contacts already in the awake set +// 3. touching contacts in the sleeping set +// This handles contact types 1 and 3. Type 2 doesn't need any action. +void b2WakeSolverSet( b2World* world, int setIndex ) +{ + B2_ASSERT( setIndex >= b2_firstSleepingSet ); + b2SolverSet* set = b2SolverSetArray_Get( &world->solverSets, setIndex ); + b2SolverSet* awakeSet = b2SolverSetArray_Get( &world->solverSets, b2_awakeSet ); + b2SolverSet* disabledSet = b2SolverSetArray_Get( &world->solverSets, b2_disabledSet ); + + b2Body* bodies = world->bodies.data; + + int bodyCount = set->bodySims.count; + for ( int i = 0; i < bodyCount; ++i ) + { + b2BodySim* simSrc = set->bodySims.data + i; + + b2Body* body = bodies + simSrc->bodyId; + B2_ASSERT( body->setIndex == setIndex ); + body->setIndex = b2_awakeSet; + body->localIndex = awakeSet->bodySims.count; + + // Reset sleep timer + body->sleepTime = 0.0f; + + b2BodySim* simDst = b2BodySimArray_Add( &awakeSet->bodySims ); + memcpy( simDst, simSrc, sizeof( b2BodySim ) ); + + b2BodyState* state = b2BodyStateArray_Add( &awakeSet->bodyStates ); + *state = b2_identityBodyState; + + // move non-touching contacts from disabled set to awake set + int contactKey = body->headContactKey; + while ( contactKey != B2_NULL_INDEX ) + { + int edgeIndex = contactKey & 1; + int contactId = contactKey >> 1; + + b2Contact* contact = b2ContactArray_Get( &world->contacts, contactId ); + + contactKey = contact->edges[edgeIndex].nextKey; + + if ( contact->setIndex != b2_disabledSet ) + { + B2_ASSERT( contact->setIndex == b2_awakeSet || contact->setIndex == setIndex ); + continue; + } + + int localIndex = contact->localIndex; + b2ContactSim* contactSim = b2ContactSimArray_Get( &disabledSet->contactSims, localIndex ); + + B2_ASSERT( ( contact->flags & b2_contactTouchingFlag ) == 0 && contactSim->manifold.pointCount == 0 ); + + contact->setIndex = b2_awakeSet; + contact->localIndex = awakeSet->contactSims.count; + b2ContactSim* awakeContactSim = b2ContactSimArray_Add( &awakeSet->contactSims ); + memcpy( awakeContactSim, contactSim, sizeof( b2ContactSim ) ); + + int movedLocalIndex = b2ContactSimArray_RemoveSwap( &disabledSet->contactSims, localIndex ); + if ( movedLocalIndex != B2_NULL_INDEX ) + { + // fix moved element + b2ContactSim* movedContactSim = disabledSet->contactSims.data + localIndex; + b2Contact* movedContact = b2ContactArray_Get( &world->contacts, movedContactSim->contactId ); + B2_ASSERT( movedContact->localIndex == movedLocalIndex ); + movedContact->localIndex = localIndex; + } + } + } + + // transfer touching contacts from sleeping set to contact graph + { + int contactCount = set->contactSims.count; + for ( int i = 0; i < contactCount; ++i ) + { + b2ContactSim* contactSim = set->contactSims.data + i; + b2Contact* contact = b2ContactArray_Get( &world->contacts, contactSim->contactId ); + B2_ASSERT( contact->flags & b2_contactTouchingFlag ); + B2_ASSERT( contactSim->simFlags & b2_simTouchingFlag ); + B2_ASSERT( contactSim->manifold.pointCount > 0 ); + B2_ASSERT( contact->setIndex == setIndex ); + b2AddContactToGraph( world, contactSim, contact ); + contact->setIndex = b2_awakeSet; + } + } + + // transfer joints from sleeping set to awake set + { + int jointCount = set->jointSims.count; + for ( int i = 0; i < jointCount; ++i ) + { + b2JointSim* jointSim = set->jointSims.data + i; + b2Joint* joint = b2JointArray_Get( &world->joints, +jointSim->jointId ); + B2_ASSERT( joint->setIndex == setIndex ); + b2AddJointToGraph( world, jointSim, joint ); + joint->setIndex = b2_awakeSet; + } + } + + // transfer island from sleeping set to awake set + // Usually a sleeping set has only one island, but it is possible + // that joints are created between sleeping islands and they + // are moved to the same sleeping set. + { + int islandCount = set->islandSims.count; + for ( int i = 0; i < islandCount; ++i ) + { + b2IslandSim* islandSrc = set->islandSims.data + i; + b2Island* island = b2IslandArray_Get( &world->islands, islandSrc->islandId ); + island->setIndex = b2_awakeSet; + island->localIndex = awakeSet->islandSims.count; + b2IslandSim* islandDst = b2IslandSimArray_Add( &awakeSet->islandSims ); + memcpy( islandDst, islandSrc, sizeof( b2IslandSim ) ); + } + } + + // destroy the sleeping set + b2DestroySolverSet( world, setIndex ); + + b2ValidateSolverSets( world ); +} + +void b2TrySleepIsland( b2World* world, int islandId ) +{ + b2Island* island = b2IslandArray_Get( &world->islands, islandId ); + B2_ASSERT( island->setIndex == b2_awakeSet ); + + // cannot put an island to sleep while it has a pending split + if ( island->constraintRemoveCount > 0 ) + { + return; + } + + // island is sleeping + // - create new sleeping solver set + // - move island to sleeping solver set + // - identify non-touching contacts that should move to sleeping solver set or disabled set + // - remove old island + // - fix island + int sleepSetId = b2AllocId( &world->solverSetIdPool ); + if ( sleepSetId == world->solverSets.count ) + { + b2SolverSet set = { 0 }; + set.setIndex = B2_NULL_INDEX; + b2SolverSetArray_Push( &world->solverSets, set ); + } + + b2SolverSet* sleepSet = b2SolverSetArray_Get( &world->solverSets, sleepSetId ); + *sleepSet = ( b2SolverSet ){ 0 }; + + // grab awake set after creating the sleep set because the solver set array may have been resized + b2SolverSet* awakeSet = b2SolverSetArray_Get( &world->solverSets, b2_awakeSet ); + B2_ASSERT( 0 <= island->localIndex && island->localIndex < awakeSet->islandSims.count ); + + sleepSet->setIndex = sleepSetId; + sleepSet->bodySims = b2BodySimArray_Create( island->bodyCount ); + sleepSet->contactSims = b2ContactSimArray_Create( island->contactCount ); + sleepSet->jointSims = b2JointSimArray_Create( island->jointCount ); + + // move awake bodies to sleeping set + // this shuffles around bodies in the awake set + { + b2SolverSet* disabledSet = b2SolverSetArray_Get( &world->solverSets, b2_disabledSet ); + int bodyId = island->headBody; + while ( bodyId != B2_NULL_INDEX ) + { + b2Body* body = b2BodyArray_Get( &world->bodies, bodyId ); + B2_ASSERT( body->setIndex == b2_awakeSet ); + B2_ASSERT( body->islandId == islandId ); + + // Update the body move event to indicate this body fell asleep + // It could happen the body is forced asleep before it ever moves. + if ( body->bodyMoveIndex != B2_NULL_INDEX ) + { + b2BodyMoveEvent* moveEvent = b2BodyMoveEventArray_Get( &world->bodyMoveEvents, body->bodyMoveIndex ); + B2_ASSERT( moveEvent->bodyId.index1 - 1 == bodyId ); + B2_ASSERT( moveEvent->bodyId.revision == body->revision ); + moveEvent->fellAsleep = true; + body->bodyMoveIndex = B2_NULL_INDEX; + } + + int awakeBodyIndex = body->localIndex; + b2BodySim* awakeSim = b2BodySimArray_Get( &awakeSet->bodySims, awakeBodyIndex ); + + // move body sim to sleep set + int sleepBodyIndex = sleepSet->bodySims.count; + b2BodySim* sleepBodySim = b2BodySimArray_Add( &sleepSet->bodySims ); + memcpy( sleepBodySim, awakeSim, sizeof( b2BodySim ) ); + + int movedIndex = b2BodySimArray_RemoveSwap( &awakeSet->bodySims, awakeBodyIndex ); + if ( movedIndex != B2_NULL_INDEX ) + { + // fix local index on moved element + b2BodySim* movedSim = awakeSet->bodySims.data + awakeBodyIndex; + int movedId = movedSim->bodyId; + b2Body* movedBody = b2BodyArray_Get( &world->bodies, movedId ); + B2_ASSERT( movedBody->localIndex == movedIndex ); + movedBody->localIndex = awakeBodyIndex; + } + + // destroy state, no need to clone + b2BodyStateArray_RemoveSwap( &awakeSet->bodyStates, awakeBodyIndex ); + + body->setIndex = sleepSetId; + body->localIndex = sleepBodyIndex; + + // Move non-touching contacts to the disabled set. + // Non-touching contacts may exist between sleeping islands and there is no clear ownership. + int contactKey = body->headContactKey; + while ( contactKey != B2_NULL_INDEX ) + { + int contactId = contactKey >> 1; + int edgeIndex = contactKey & 1; + + b2Contact* contact = b2ContactArray_Get( &world->contacts, contactId ); + + B2_ASSERT( contact->setIndex == b2_awakeSet || contact->setIndex == b2_disabledSet ); + contactKey = contact->edges[edgeIndex].nextKey; + + if ( contact->setIndex == b2_disabledSet ) + { + // already moved to disabled set by another body in the island + continue; + } + + if ( contact->colorIndex != B2_NULL_INDEX ) + { + // contact is touching and will be moved separately + B2_ASSERT( ( contact->flags & b2_contactTouchingFlag ) != 0 ); + continue; + } + + // the other body may still be awake, it still may go to sleep and then it will be responsible + // for moving this contact to the disabled set. + int otherEdgeIndex = edgeIndex ^ 1; + int otherBodyId = contact->edges[otherEdgeIndex].bodyId; + b2Body* otherBody = b2BodyArray_Get( &world->bodies, otherBodyId ); + if ( otherBody->setIndex == b2_awakeSet ) + { + continue; + } + + int localIndex = contact->localIndex; + b2ContactSim* contactSim = b2ContactSimArray_Get( &awakeSet->contactSims, localIndex ); + + B2_ASSERT( contactSim->manifold.pointCount == 0 ); + B2_ASSERT( ( contact->flags & b2_contactTouchingFlag ) == 0 || ( contact->flags & b2_contactSensorFlag ) != 0 ); + + // move the non-touching contact to the disabled set + contact->setIndex = b2_disabledSet; + contact->localIndex = disabledSet->contactSims.count; + b2ContactSim* disabledContactSim = b2ContactSimArray_Add( &disabledSet->contactSims ); + memcpy( disabledContactSim, contactSim, sizeof( b2ContactSim ) ); + + int movedLocalIndex = b2ContactSimArray_RemoveSwap( &awakeSet->contactSims, localIndex ); + if ( movedLocalIndex != B2_NULL_INDEX ) + { + // fix moved element + b2ContactSim* movedContactSim = awakeSet->contactSims.data + localIndex; + b2Contact* movedContact = b2ContactArray_Get( &world->contacts, movedContactSim->contactId ); + B2_ASSERT( movedContact->localIndex == movedLocalIndex ); + movedContact->localIndex = localIndex; + } + } + + bodyId = body->islandNext; + } + } + + // move touching contacts + // this shuffles contacts in the awake set + { + int contactId = island->headContact; + while ( contactId != B2_NULL_INDEX ) + { + b2Contact* contact = b2ContactArray_Get( &world->contacts, contactId ); + B2_ASSERT( contact->setIndex == b2_awakeSet ); + B2_ASSERT( contact->islandId == islandId ); + int colorIndex = contact->colorIndex; + B2_ASSERT( 0 <= colorIndex && colorIndex < b2_graphColorCount ); + + b2GraphColor* color = world->constraintGraph.colors + colorIndex; + + // Remove bodies from graph coloring associated with this constraint + if ( colorIndex != b2_overflowIndex ) + { + // might clear a bit for a static body, but this has no effect + b2ClearBit( &color->bodySet, contact->edges[0].bodyId ); + b2ClearBit( &color->bodySet, contact->edges[1].bodyId ); + } + + int localIndex = contact->localIndex; + b2ContactSim* awakeContactSim = b2ContactSimArray_Get( &color->contactSims, localIndex ); + + int sleepContactIndex = sleepSet->contactSims.count; + b2ContactSim* sleepContactSim = b2ContactSimArray_Add( &sleepSet->contactSims ); + memcpy( sleepContactSim, awakeContactSim, sizeof( b2ContactSim ) ); + + int movedLocalIndex = b2ContactSimArray_RemoveSwap( &color->contactSims, localIndex ); + if ( movedLocalIndex != B2_NULL_INDEX ) + { + // fix moved element + b2ContactSim* movedContactSim = color->contactSims.data + localIndex; + b2Contact* movedContact = b2ContactArray_Get( &world->contacts, movedContactSim->contactId ); + B2_ASSERT( movedContact->localIndex == movedLocalIndex ); + movedContact->localIndex = localIndex; + } + + contact->setIndex = sleepSetId; + contact->colorIndex = B2_NULL_INDEX; + contact->localIndex = sleepContactIndex; + + contactId = contact->islandNext; + } + } + + // move joints + // this shuffles joints in the awake set + { + int jointId = island->headJoint; + while ( jointId != B2_NULL_INDEX ) + { + b2Joint* joint = b2JointArray_Get( &world->joints, jointId ); + B2_ASSERT( joint->setIndex == b2_awakeSet ); + B2_ASSERT( joint->islandId == islandId ); + int colorIndex = joint->colorIndex; + int localIndex = joint->localIndex; + + B2_ASSERT( 0 <= colorIndex && colorIndex < b2_graphColorCount ); + + b2GraphColor* color = world->constraintGraph.colors + colorIndex; + + b2JointSim* awakeJointSim = b2JointSimArray_Get( &color->jointSims, localIndex ); + + if ( colorIndex != b2_overflowIndex ) + { + // might clear a bit for a static body, but this has no effect + b2ClearBit( &color->bodySet, joint->edges[0].bodyId ); + b2ClearBit( &color->bodySet, joint->edges[1].bodyId ); + } + + int sleepJointIndex = sleepSet->jointSims.count; + b2JointSim* sleepJointSim = b2JointSimArray_Add( &sleepSet->jointSims ); + memcpy( sleepJointSim, awakeJointSim, sizeof( b2JointSim ) ); + + int movedIndex = b2JointSimArray_RemoveSwap( &color->jointSims, localIndex ); + if ( movedIndex != B2_NULL_INDEX ) + { + // fix moved element + b2JointSim* movedJointSim = color->jointSims.data + localIndex; + int movedId = movedJointSim->jointId; + b2Joint* movedJoint = b2JointArray_Get( &world->joints, movedId ); + B2_ASSERT( movedJoint->localIndex == movedIndex ); + movedJoint->localIndex = localIndex; + } + + joint->setIndex = sleepSetId; + joint->colorIndex = B2_NULL_INDEX; + joint->localIndex = sleepJointIndex; + + jointId = joint->islandNext; + } + } + + // move island struct + { + B2_ASSERT( island->setIndex == b2_awakeSet ); + + int islandIndex = island->localIndex; + b2IslandSim* sleepIsland = b2IslandSimArray_Add( &sleepSet->islandSims ); + sleepIsland->islandId = islandId; + + int movedIslandIndex = b2IslandSimArray_RemoveSwap( &awakeSet->islandSims, islandIndex ); + if ( movedIslandIndex != B2_NULL_INDEX ) + { + // fix index on moved element + b2IslandSim* movedIslandSim = awakeSet->islandSims.data + islandIndex; + int movedIslandId = movedIslandSim->islandId; + b2Island* movedIsland = b2IslandArray_Get( &world->islands, movedIslandId ); + B2_ASSERT( movedIsland->localIndex == movedIslandIndex ); + movedIsland->localIndex = islandIndex; + } + + island->setIndex = sleepSetId; + island->localIndex = 0; + } + + b2ValidateSolverSets( world ); +} + +// This is called when joints are created between sets. I want to allow the sets +// to continue sleeping if both are asleep. Otherwise one set is waked. +// Islands will get merge when the set is waked. +void b2MergeSolverSets( b2World* world, int setId1, int setId2 ) +{ + B2_ASSERT( setId1 >= b2_firstSleepingSet ); + B2_ASSERT( setId2 >= b2_firstSleepingSet ); + b2SolverSet* set1 = b2SolverSetArray_Get( &world->solverSets, setId1 ); + b2SolverSet* set2 = b2SolverSetArray_Get( &world->solverSets, setId2 ); + + // Move the fewest number of bodies + if ( set1->bodySims.count < set2->bodySims.count ) + { + b2SolverSet* tempSet = set1; + set1 = set2; + set2 = tempSet; + + int tempId = setId1; + setId1 = setId2; + setId2 = tempId; + } + + // transfer bodies + { + b2Body* bodies = world->bodies.data; + int bodyCount = set2->bodySims.count; + for ( int i = 0; i < bodyCount; ++i ) + { + b2BodySim* simSrc = set2->bodySims.data + i; + + b2Body* body = bodies + simSrc->bodyId; + B2_ASSERT( body->setIndex == setId2 ); + body->setIndex = setId1; + body->localIndex = set1->bodySims.count; + + b2BodySim* simDst = b2BodySimArray_Add( &set1->bodySims ); + memcpy( simDst, simSrc, sizeof( b2BodySim ) ); + } + } + + // transfer contacts + { + int contactCount = set2->contactSims.count; + for ( int i = 0; i < contactCount; ++i ) + { + b2ContactSim* contactSrc = set2->contactSims.data + i; + + b2Contact* contact = b2ContactArray_Get( &world->contacts, contactSrc->contactId ); + B2_ASSERT( contact->setIndex == setId2 ); + contact->setIndex = setId1; + contact->localIndex = set1->contactSims.count; + + b2ContactSim* contactDst = b2ContactSimArray_Add( &set1->contactSims ); + memcpy( contactDst, contactSrc, sizeof( b2ContactSim ) ); + } + } + + // transfer joints + { + int jointCount = set2->jointSims.count; + for ( int i = 0; i < jointCount; ++i ) + { + b2JointSim* jointSrc = set2->jointSims.data + i; + + b2Joint* joint = b2JointArray_Get( &world->joints, jointSrc->jointId ); + B2_ASSERT( joint->setIndex == setId2 ); + joint->setIndex = setId1; + joint->localIndex = set1->jointSims.count; + + b2JointSim* jointDst = b2JointSimArray_Add( &set1->jointSims ); + memcpy( jointDst, jointSrc, sizeof( b2JointSim ) ); + } + } + + // transfer islands + { + int islandCount = set2->islandSims.count; + for ( int i = 0; i < islandCount; ++i ) + { + b2IslandSim* islandSrc = set2->islandSims.data + i; + int islandId = islandSrc->islandId; + + b2Island* island = b2IslandArray_Get( &world->islands, islandId ); + island->setIndex = setId1; + island->localIndex = set1->islandSims.count; + + b2IslandSim* islandDst = b2IslandSimArray_Add( &set1->islandSims ); + memcpy( islandDst, islandSrc, sizeof( b2IslandSim ) ); + } + } + + // destroy the merged set + b2DestroySolverSet( world, setId2 ); + + b2ValidateSolverSets( world ); +} + +void b2TransferBody( b2World* world, b2SolverSet* targetSet, b2SolverSet* sourceSet, b2Body* body ) +{ + B2_ASSERT( targetSet != sourceSet ); + + int sourceIndex = body->localIndex; + b2BodySim* sourceSim = b2BodySimArray_Get( &sourceSet->bodySims, sourceIndex ); + + int targetIndex = targetSet->bodySims.count; + b2BodySim* targetSim = b2BodySimArray_Add( &targetSet->bodySims ); + memcpy( targetSim, sourceSim, sizeof( b2BodySim ) ); + + // Remove body sim from solver set that owns it + int movedIndex = b2BodySimArray_RemoveSwap( &sourceSet->bodySims, sourceIndex ); + if ( movedIndex != B2_NULL_INDEX ) + { + // Fix moved body index + b2BodySim* movedSim = sourceSet->bodySims.data + sourceIndex; + int movedId = movedSim->bodyId; + b2Body* movedBody = b2BodyArray_Get( &world->bodies, movedId ); + B2_ASSERT( movedBody->localIndex == movedIndex ); + movedBody->localIndex = sourceIndex; + } + + if ( sourceSet->setIndex == b2_awakeSet ) + { + b2BodyStateArray_RemoveSwap( &sourceSet->bodyStates, sourceIndex ); + } + else if ( targetSet->setIndex == b2_awakeSet ) + { + b2BodyState* state = b2BodyStateArray_Add( &targetSet->bodyStates ); + *state = b2_identityBodyState; + } + + body->setIndex = targetSet->setIndex; + body->localIndex = targetIndex; +} + +void b2TransferJoint( b2World* world, b2SolverSet* targetSet, b2SolverSet* sourceSet, b2Joint* joint ) +{ + B2_ASSERT( targetSet != sourceSet ); + + int localIndex = joint->localIndex; + int colorIndex = joint->colorIndex; + + // Retrieve source. + b2JointSim* sourceSim; + if ( sourceSet->setIndex == b2_awakeSet ) + { + B2_ASSERT( 0 <= colorIndex && colorIndex < b2_graphColorCount ); + b2GraphColor* color = world->constraintGraph.colors + colorIndex; + + sourceSim = b2JointSimArray_Get( &color->jointSims, localIndex ); + } + else + { + B2_ASSERT( colorIndex == B2_NULL_INDEX ); + sourceSim = b2JointSimArray_Get( &sourceSet->jointSims, +localIndex ); + } + + // Create target and copy. Fix joint. + if ( targetSet->setIndex == b2_awakeSet ) + { + b2AddJointToGraph( world, sourceSim, joint ); + joint->setIndex = b2_awakeSet; + } + else + { + joint->setIndex = targetSet->setIndex; + joint->localIndex = targetSet->jointSims.count; + joint->colorIndex = B2_NULL_INDEX; + + b2JointSim* targetSim = b2JointSimArray_Add( &targetSet->jointSims ); + memcpy( targetSim, sourceSim, sizeof( b2JointSim ) ); + } + + // Destroy source. + if ( sourceSet->setIndex == b2_awakeSet ) + { + b2RemoveJointFromGraph( world, joint->edges[0].bodyId, joint->edges[1].bodyId, colorIndex, localIndex ); + } + else + { + int movedIndex = b2JointSimArray_RemoveSwap( &sourceSet->jointSims, localIndex ); + if ( movedIndex != B2_NULL_INDEX ) + { + // fix swapped element + b2JointSim* movedJointSim = sourceSet->jointSims.data + localIndex; + int movedId = movedJointSim->jointId; + b2Joint* movedJoint = b2JointArray_Get( &world->joints, movedId ); + movedJoint->localIndex = localIndex; + } + } +} diff --git a/3rdparty/box2d/src/solver_set.h b/3rdparty/box2d/src/solver_set.h new file mode 100644 index 000000000000..1df615b8d2e7 --- /dev/null +++ b/3rdparty/box2d/src/solver_set.h @@ -0,0 +1,57 @@ +// SPDX-FileCopyrightText: 2023 Erin Catto +// SPDX-License-Identifier: MIT + +#pragma once + +#include "array.h" + +typedef struct b2Body b2Body; +typedef struct b2Joint b2Joint; +typedef struct b2World b2World; + +// This holds solver set data. The following sets are used: +// - static set for all static bodies (no contacts or joints) +// - active set for all active bodies with body states (no contacts or joints) +// - disabled set for disabled bodies and their joints +// - all further sets are sleeping island sets along with their contacts and joints +// The purpose of solver sets is to achieve high memory locality. +// https://www.youtube.com/watch?v=nZNd5FjSquk +typedef struct b2SolverSet +{ + // Body array. Empty for unused set. + b2BodySimArray bodySims; + + // Body state only exists for active set + b2BodyStateArray bodyStates; + + // This holds sleeping/disabled joints. Empty for static/active set. + b2JointSimArray jointSims; + + // This holds all contacts for sleeping sets. + // This holds non-touching contacts for the awake set. + b2ContactSimArray contactSims; + + // The awake set has an array of islands. Sleeping sets normally have a single islands. However, joints + // created between sleeping sets causes the sets to merge, leaving them with multiple islands. These sleeping + // islands will be naturally merged with the set is woken. + // The static and disabled sets have no islands. + // Islands live in the solver sets to limit the number of islands that need to be considered for sleeping. + b2IslandSimArray islandSims; + + // Aligns with b2World::solverSetIdPool. Used to create a stable id for body/contact/joint/islands. + int setIndex; +} b2SolverSet; + +void b2DestroySolverSet( b2World* world, int setIndex ); + +void b2WakeSolverSet( b2World* world, int setIndex ); +void b2TrySleepIsland( b2World* world, int islandId ); + +// Merge set 2 into set 1 then destroy set 2. +// Warning: any pointers into these sets will be orphaned. +void b2MergeSolverSets( b2World* world, int setIndex1, int setIndex2 ); + +void b2TransferBody( b2World* world, b2SolverSet* targetSet, b2SolverSet* sourceSet, b2Body* body ); +void b2TransferJoint( b2World* world, b2SolverSet* targetSet, b2SolverSet* sourceSet, b2Joint* joint ); + +B2_ARRAY_INLINE( b2SolverSet, b2SolverSet ); diff --git a/3rdparty/box2d/src/stack_allocator.c b/3rdparty/box2d/src/stack_allocator.c new file mode 100644 index 000000000000..2b6b408c1f72 --- /dev/null +++ b/3rdparty/box2d/src/stack_allocator.c @@ -0,0 +1,121 @@ +// SPDX-FileCopyrightText: 2023 Erin Catto +// SPDX-License-Identifier: MIT + +#include "stack_allocator.h" + +#include "array.h" +#include "core.h" + +#include +#include + +typedef struct b2StackEntry +{ + char* data; + const char* name; + int size; + bool usedMalloc; +} b2StackEntry; + +B2_ARRAY_INLINE( b2StackEntry, b2StackEntry ); +B2_ARRAY_SOURCE( b2StackEntry, b2StackEntry ); + +b2StackAllocator b2CreateStackAllocator( int capacity ) +{ + B2_ASSERT( capacity >= 0 ); + b2StackAllocator allocator = { 0 }; + allocator.capacity = capacity; + allocator.data = b2Alloc( capacity ); + allocator.allocation = 0; + allocator.maxAllocation = 0; + allocator.index = 0; + allocator.entries = b2StackEntryArray_Create( 32 ); + return allocator; +} + +void b2DestroyStackAllocator( b2StackAllocator* allocator ) +{ + b2StackEntryArray_Destroy( &allocator->entries ); + b2Free( allocator->data, allocator->capacity ); +} + +void* b2AllocateStackItem( b2StackAllocator* alloc, int size, const char* name ) +{ + // ensure allocation is 32 byte aligned to support 256-bit SIMD + int size32 = ( ( size - 1 ) | 0x1F ) + 1; + + b2StackEntry entry; + entry.size = size32; + entry.name = name; + if ( alloc->index + size32 > alloc->capacity ) + { + // fall back to the heap (undesirable) + entry.data = b2Alloc( size32 ); + entry.usedMalloc = true; + + B2_ASSERT( ( (uintptr_t)entry.data & 0x1F ) == 0 ); + } + else + { + entry.data = alloc->data + alloc->index; + entry.usedMalloc = false; + alloc->index += size32; + + B2_ASSERT( ( (uintptr_t)entry.data & 0x1F ) == 0 ); + } + + alloc->allocation += size32; + if ( alloc->allocation > alloc->maxAllocation ) + { + alloc->maxAllocation = alloc->allocation; + } + + b2StackEntryArray_Push( &alloc->entries, entry ); + return entry.data; +} + +void b2FreeStackItem( b2StackAllocator* alloc, void* mem ) +{ + int entryCount = alloc->entries.count; + B2_ASSERT( entryCount > 0 ); + b2StackEntry* entry = alloc->entries.data + ( entryCount - 1 ); + B2_ASSERT( mem == entry->data ); + if ( entry->usedMalloc ) + { + b2Free( mem, entry->size ); + } + else + { + alloc->index -= entry->size; + } + alloc->allocation -= entry->size; + b2StackEntryArray_Pop( &alloc->entries ); +} + +void b2GrowStack( b2StackAllocator* alloc ) +{ + // Stack must not be in use + B2_ASSERT( alloc->allocation == 0 ); + + if ( alloc->maxAllocation > alloc->capacity ) + { + b2Free( alloc->data, alloc->capacity ); + alloc->capacity = alloc->maxAllocation + alloc->maxAllocation / 2; + alloc->data = b2Alloc( alloc->capacity ); + } +} + +int b2GetStackCapacity( b2StackAllocator* alloc ) +{ + return alloc->capacity; +} + +int b2GetStackAllocation( b2StackAllocator* alloc ) +{ + return alloc->allocation; +} + +int b2GetMaxStackAllocation( b2StackAllocator* alloc ) +{ + return alloc->maxAllocation; +} diff --git a/3rdparty/box2d/src/stack_allocator.h b/3rdparty/box2d/src/stack_allocator.h new file mode 100644 index 000000000000..e6d6149a2403 --- /dev/null +++ b/3rdparty/box2d/src/stack_allocator.h @@ -0,0 +1,38 @@ +// SPDX-FileCopyrightText: 2023 Erin Catto +// SPDX-License-Identifier: MIT + +#pragma once + +#include "array.h" + +B2_ARRAY_DECLARE( b2StackEntry, b2StackEntry ); + +// This is a stack-like arena allocator used for fast per step allocations. +// You must nest allocate/free pairs. The code will B2_ASSERT +// if you try to interleave multiple allocate/free pairs. +// This allocator uses the heap if space is insufficient. +// I could remove the need to free entries individually. +typedef struct b2StackAllocator +{ + char* data; + int capacity; + int index; + + int allocation; + int maxAllocation; + + b2StackEntryArray entries; +} b2StackAllocator; + +b2StackAllocator b2CreateStackAllocator( int capacity ); +void b2DestroyStackAllocator( b2StackAllocator* allocator ); + +void* b2AllocateStackItem( b2StackAllocator* alloc, int size, const char* name ); +void b2FreeStackItem( b2StackAllocator* alloc, void* mem ); + +// Grow the stack based on usage +void b2GrowStack( b2StackAllocator* alloc ); + +int b2GetStackCapacity( b2StackAllocator* alloc ); +int b2GetStackAllocation( b2StackAllocator* alloc ); +int b2GetMaxStackAllocation( b2StackAllocator* alloc ); diff --git a/3rdparty/box2d/src/table.c b/3rdparty/box2d/src/table.c new file mode 100644 index 000000000000..c13d9525d8e3 --- /dev/null +++ b/3rdparty/box2d/src/table.c @@ -0,0 +1,231 @@ +// SPDX-FileCopyrightText: 2023 Erin Catto +// SPDX-License-Identifier: MIT + +#include "table.h" + +#include "core.h" +#include "ctz.h" + +#include +#include +#include + +#if B2_DEBUG +_Atomic int g_probeCount; +#endif + +// todo compare with https://github.com/skeeto/scratch/blob/master/set32/set32.h + +b2HashSet b2CreateSet( int32_t capacity ) +{ + b2HashSet set = { 0 }; + + // Capacity must be a power of 2 + if ( capacity > 16 ) + { + set.capacity = b2RoundUpPowerOf2( capacity ); + } + else + { + set.capacity = 16; + } + + set.count = 0; + set.items = b2Alloc( capacity * sizeof( b2SetItem ) ); + memset( set.items, 0, capacity * sizeof( b2SetItem ) ); + + return set; +} + +void b2DestroySet( b2HashSet* set ) +{ + b2Free( set->items, set->capacity * sizeof( b2SetItem ) ); + set->items = NULL; + set->count = 0; + set->capacity = 0; +} + +void b2ClearSet( b2HashSet* set ) +{ + set->count = 0; + memset( set->items, 0, set->capacity * sizeof( b2SetItem ) ); +} + +// I need a good hash because the keys are built from pairs of increasing integers. +// A simple hash like hash = (integer1 XOR integer2) has many collisions. +// https://lemire.me/blog/2018/08/15/fast-strongly-universal-64-bit-hashing-everywhere/ +// https://preshing.com/20130107/this-hash-set-is-faster-than-a-judy-array/ +// TODO_ERIN try: https://www.jandrewrogers.com/2019/02/12/fast-perfect-hashing/ +static inline uint32_t b2KeyHash( uint64_t key ) +{ + uint64_t h = key; + h ^= h >> 33; + h *= 0xff51afd7ed558ccdL; + h ^= h >> 33; + h *= 0xc4ceb9fe1a85ec53L; + h ^= h >> 33; + + return (uint32_t)h; +} + +int32_t b2FindSlot( const b2HashSet* set, uint64_t key, uint32_t hash ) +{ + uint32_t capacity = set->capacity; + int32_t index = hash & ( capacity - 1 ); + const b2SetItem* items = set->items; + while ( items[index].hash != 0 && items[index].key != key ) + { +#if B2_DEBUG + atomic_fetch_add( &g_probeCount, 1 ); +#endif + index = ( index + 1 ) & ( capacity - 1 ); + } + + return index; +} + +static void b2AddKeyHaveCapacity( b2HashSet* set, uint64_t key, uint32_t hash ) +{ + int32_t index = b2FindSlot( set, key, hash ); + b2SetItem* items = set->items; + B2_ASSERT( items[index].hash == 0 ); + + items[index].key = key; + items[index].hash = hash; + set->count += 1; +} + +static void b2GrowTable( b2HashSet* set ) +{ + uint32_t oldCount = set->count; + B2_MAYBE_UNUSED( oldCount ); + + uint32_t oldCapacity = set->capacity; + b2SetItem* oldItems = set->items; + + set->count = 0; + // Capacity must be a power of 2 + set->capacity = 2 * oldCapacity; + set->items = b2Alloc( set->capacity * sizeof( b2SetItem ) ); + memset( set->items, 0, set->capacity * sizeof( b2SetItem ) ); + + // Transfer items into new array + for ( uint32_t i = 0; i < oldCapacity; ++i ) + { + b2SetItem* item = oldItems + i; + if ( item->hash == 0 ) + { + // this item was empty + continue; + } + + b2AddKeyHaveCapacity( set, item->key, item->hash ); + } + + B2_ASSERT( set->count == oldCount ); + + b2Free( oldItems, oldCapacity * sizeof( b2SetItem ) ); +} + +bool b2ContainsKey( const b2HashSet* set, uint64_t key ) +{ + // key of zero is a sentinel + B2_ASSERT( key != 0 ); + uint32_t hash = b2KeyHash( key ); + int32_t index = b2FindSlot( set, key, hash ); + return set->items[index].key == key; +} + +int b2GetHashSetBytes( b2HashSet* set ) +{ + return set->capacity * (int)sizeof( b2SetItem ); +} + +bool b2AddKey( b2HashSet* set, uint64_t key ) +{ + // key of zero is a sentinel + B2_ASSERT( key != 0 ); + + uint32_t hash = b2KeyHash( key ); + B2_ASSERT( hash != 0 ); + + int32_t index = b2FindSlot( set, key, hash ); + if ( set->items[index].hash != 0 ) + { + // Already in set + B2_ASSERT( set->items[index].hash == hash && set->items[index].key == key ); + return true; + } + + if ( 2 * set->count >= set->capacity ) + { + b2GrowTable( set ); + } + + b2AddKeyHaveCapacity( set, key, hash ); + return false; +} + +// See https://en.wikipedia.org/wiki/Open_addressing +bool b2RemoveKey( b2HashSet* set, uint64_t key ) +{ + uint32_t hash = b2KeyHash( key ); + int32_t i = b2FindSlot( set, key, hash ); + b2SetItem* items = set->items; + if ( items[i].hash == 0 ) + { + // Not in set + return false; + } + + // Mark item i as unoccupied + items[i].key = 0; + items[i].hash = 0; + + B2_ASSERT( set->count > 0 ); + set->count -= 1; + + // Attempt to fill item i + int32_t j = i; + uint32_t capacity = set->capacity; + for ( ;; ) + { + j = ( j + 1 ) & ( capacity - 1 ); + if ( items[j].hash == 0 ) + { + break; + } + + // k is the first item for the hash of j + int32_t k = items[j].hash & ( capacity - 1 ); + + // determine if k lies cyclically in (i,j] + // i <= j: | i..k..j | + // i > j: |.k..j i....| or |....j i..k.| + if ( i <= j ) + { + if ( i < k && k <= j ) + { + continue; + } + } + else + { + if ( i < k || k <= j ) + { + continue; + } + } + + // Move j into i + items[i] = items[j]; + + // Mark item j as unoccupied + items[j].key = 0; + items[j].hash = 0; + + i = j; + } + + return true; +} diff --git a/3rdparty/box2d/src/table.h b/3rdparty/box2d/src/table.h new file mode 100644 index 000000000000..04949949b748 --- /dev/null +++ b/3rdparty/box2d/src/table.h @@ -0,0 +1,37 @@ +// SPDX-FileCopyrightText: 2023 Erin Catto +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +#define B2_SHAPE_PAIR_KEY( K1, K2 ) K1 < K2 ? (uint64_t)K1 << 32 | (uint64_t)K2 : (uint64_t)K2 << 32 | (uint64_t)K1 + +typedef struct b2SetItem +{ + uint64_t key; + uint32_t hash; +} b2SetItem; + +typedef struct b2HashSet +{ + b2SetItem* items; + uint32_t capacity; + uint32_t count; +} b2HashSet; + +b2HashSet b2CreateSet( int32_t capacity ); +void b2DestroySet( b2HashSet* set ); + +void b2ClearSet( b2HashSet* set ); + +// Returns true if key was already in set +bool b2AddKey( b2HashSet* set, uint64_t key ); + +// Returns true if the key was found +bool b2RemoveKey( b2HashSet* set, uint64_t key ); + +bool b2ContainsKey( const b2HashSet* set, uint64_t key ); + +int b2GetHashSetBytes( b2HashSet* set ); diff --git a/3rdparty/box2d/src/timer.c b/3rdparty/box2d/src/timer.c new file mode 100644 index 000000000000..0aecd57baac2 --- /dev/null +++ b/3rdparty/box2d/src/timer.c @@ -0,0 +1,202 @@ +// SPDX-FileCopyrightText: 2023 Erin Catto +// SPDX-License-Identifier: MIT + +#include "box2d/base.h" + +#include + +#if defined( _WIN32 ) + +#ifndef WIN32_LEAN_AND_MEAN +#define WIN32_LEAN_AND_MEAN +#endif + +#include + +static double s_invFrequency = 0.0; + +b2Timer b2CreateTimer( void ) +{ + 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 ); + b2Timer timer; + timer.start = largeInteger.QuadPart; + return timer; +} + +int64_t b2GetTicks( b2Timer* timer ) +{ + LARGE_INTEGER largeInteger; + QueryPerformanceCounter( &largeInteger ); + int64_t ticks = largeInteger.QuadPart; + int64_t count = ticks - timer->start; + timer->start = ticks; + return count; +} + +float b2GetMilliseconds( const b2Timer* timer ) +{ + LARGE_INTEGER largeInteger; + QueryPerformanceCounter( &largeInteger ); + int64_t count = largeInteger.QuadPart; + float ms = (float)( s_invFrequency * ( count - timer->start ) ); + return ms; +} + +float b2GetMillisecondsAndReset( b2Timer* timer ) +{ + LARGE_INTEGER largeInteger; + QueryPerformanceCounter( &largeInteger ); + int64_t count = largeInteger.QuadPart; + float ms = (float)( s_invFrequency * ( count - timer->start ) ); + timer->start = count; + return ms; +} + +void b2SleepMilliseconds( int milliseconds ) +{ + // also SwitchToThread() + Sleep( (DWORD)milliseconds ); +} + +void b2Yield() +{ + SwitchToThread(); +} + +#elif defined( __linux__ ) || defined( __APPLE__ ) + +#include +#include +#include + +b2Timer b2CreateTimer( void ) +{ + b2Timer timer; + struct timeval t; + gettimeofday( &t, 0 ); + timer.start_sec = t.tv_sec; + timer.start_usec = t.tv_usec; + return timer; +} + +float b2GetMilliseconds( const b2Timer* timer ) +{ + struct timeval t; + gettimeofday( &t, 0 ); + time_t start_sec = timer->start_sec; + suseconds_t start_usec = (suseconds_t)timer->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 ); +} + +float b2GetMillisecondsAndReset( b2Timer* timer ) +{ + struct timeval t; + gettimeofday( &t, 0 ); + time_t start_sec = timer->start_sec; + suseconds_t start_usec = (suseconds_t)timer->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; + } + + timer->start_sec = t.tv_sec; + timer->start_usec = t.tv_usec; + + return 1000.0f * ( t.tv_sec - start_sec ) + 0.001f * ( t.tv_usec - start_usec ); +} + +void b2SleepMilliseconds( int milliseconds ) +{ + struct timespec ts; + ts.tv_sec = milliseconds / 1000; + ts.tv_nsec = ( milliseconds % 1000 ) * 1000000; + nanosleep( &ts, NULL ); +} + +void b2Yield() +{ + sched_yield(); +} + +#else + +b2Timer b2CreateTimer( void ) +{ + b2Timer timer = { 0 }; + return timer; +} + +float b2GetMilliseconds( const b2Timer* timer ) +{ + ( (void)( timer ) ); + return 0.0f; +} + +float b2GetMillisecondsAndReset( b2Timer* timer ) +{ + ( (void)( timer ) ); + return 0.0f; +} + +void b2SleepMilliseconds( int milliseconds ) +{ + ( (void)( milliseconds ) ); +} + +void b2Yield() +{ +} + +#endif + +// djb2 hash +// https://en.wikipedia.org/wiki/List_of_hash_functions +uint32_t b2Hash( uint32_t hash, const uint8_t* data, int count ) +{ + uint32_t result = hash; + for ( size_t i = 0; i < count; i++ ) + { + result = ( result << 5 ) + result + data[i]; + } + + return result; +} diff --git a/3rdparty/box2d/src/types.c b/3rdparty/box2d/src/types.c new file mode 100644 index 000000000000..69614cf2eff7 --- /dev/null +++ b/3rdparty/box2d/src/types.c @@ -0,0 +1,164 @@ +// SPDX-FileCopyrightText: 2023 Erin Catto +// SPDX-License-Identifier: MIT + +#include "box2d/types.h" + +#include "core.h" + +b2WorldDef b2DefaultWorldDef( void ) +{ + b2WorldDef def = { 0 }; + def.gravity.x = 0.0f; + def.gravity.y = -10.0f; + def.hitEventThreshold = 1.0f * b2_lengthUnitsPerMeter; + def.restitutionThreshold = 1.0f * b2_lengthUnitsPerMeter; + def.contactPushoutVelocity = 3.0f * b2_lengthUnitsPerMeter; + def.contactHertz = 30.0; + def.contactDampingRatio = 10.0f; + def.jointHertz = 60.0; + def.jointDampingRatio = 2.0f; + // 400 meters per second, faster than the speed of sound + def.maximumLinearVelocity = 400.0f * b2_lengthUnitsPerMeter; + def.enableSleep = true; + def.enableContinuous = true; + def.internalValue = B2_SECRET_COOKIE; + return def; +} + +b2BodyDef b2DefaultBodyDef( void ) +{ + b2BodyDef def = { 0 }; + def.type = b2_staticBody; + def.rotation = b2Rot_identity; + def.sleepThreshold = 0.05f * b2_lengthUnitsPerMeter; + def.gravityScale = 1.0f; + def.enableSleep = true; + def.isAwake = true; + def.isEnabled = true; + def.automaticMass = true; + def.internalValue = B2_SECRET_COOKIE; + return def; +} + +b2Filter b2DefaultFilter( void ) +{ + b2Filter filter = { 0x0001ULL, UINT64_MAX, 0 }; + return filter; +} + +b2QueryFilter b2DefaultQueryFilter( void ) +{ + b2QueryFilter filter = { 0x0001ULL, UINT64_MAX }; + return filter; +} + +b2ShapeDef b2DefaultShapeDef( void ) +{ + b2ShapeDef def = { 0 }; + def.friction = 0.6f; + def.density = 1.0f; + def.filter = b2DefaultFilter(); + def.enableSensorEvents = true; + def.enableContactEvents = true; + def.internalValue = B2_SECRET_COOKIE; + return def; +} + +b2ChainDef b2DefaultChainDef( void ) +{ + b2ChainDef def = { 0 }; + def.friction = 0.6f; + def.filter = b2DefaultFilter(); + def.internalValue = B2_SECRET_COOKIE; + return def; +} + +static void b2EmptyDrawPolygon( const b2Vec2* vertices, int vertexCount, b2HexColor color, void* context ) +{ + B2_MAYBE_UNUSED( vertices ); + B2_MAYBE_UNUSED( vertexCount ); + B2_MAYBE_UNUSED( color ); + B2_MAYBE_UNUSED( context ); +} + +static void b2EmptyDrawSolidPolygon( b2Transform transform, const b2Vec2* vertices, int vertexCount, float radius, b2HexColor color, + void* context ) +{ + B2_MAYBE_UNUSED( transform ); + B2_MAYBE_UNUSED( vertices ); + B2_MAYBE_UNUSED( vertexCount ); + B2_MAYBE_UNUSED( radius ); + B2_MAYBE_UNUSED( color ); + B2_MAYBE_UNUSED( context ); +} + +static void b2EmptyDrawCircle( b2Vec2 center, float radius, b2HexColor color, void* context ) +{ + B2_MAYBE_UNUSED( center ); + B2_MAYBE_UNUSED( radius ); + B2_MAYBE_UNUSED( color ); + B2_MAYBE_UNUSED( context ); +} + +static void b2EmptyDrawSolidCircle( b2Transform transform, float radius, b2HexColor color, void* context ) +{ + B2_MAYBE_UNUSED( transform ); + B2_MAYBE_UNUSED( radius ); + B2_MAYBE_UNUSED( color ); + B2_MAYBE_UNUSED( context ); +} + +static void b2EmptyDrawSolidCapsule( b2Vec2 p1, b2Vec2 p2, float radius, b2HexColor color, void* context ) +{ + B2_MAYBE_UNUSED( p1 ); + B2_MAYBE_UNUSED( p2 ); + B2_MAYBE_UNUSED( radius ); + B2_MAYBE_UNUSED( color ); + B2_MAYBE_UNUSED( context ); +} + +static void b2EmptyDrawSegment( b2Vec2 p1, b2Vec2 p2, b2HexColor color, void* context ) +{ + B2_MAYBE_UNUSED( p1 ); + B2_MAYBE_UNUSED( p2 ); + B2_MAYBE_UNUSED( color ); + B2_MAYBE_UNUSED( context ); +} + +static void b2EmptyDrawTransform( b2Transform transform, void* context ) +{ + B2_MAYBE_UNUSED( transform ); + B2_MAYBE_UNUSED( context ); +} + +static void b2EmptyDrawPoint( b2Vec2 p, float size, b2HexColor color, void* context ) +{ + B2_MAYBE_UNUSED( p ); + B2_MAYBE_UNUSED( size ); + B2_MAYBE_UNUSED( color ); + B2_MAYBE_UNUSED( context ); +} + +static void b2EmptyDrawString( b2Vec2 p, const char* s, void* context ) +{ + B2_MAYBE_UNUSED( p ); + B2_MAYBE_UNUSED( s ); + B2_MAYBE_UNUSED( context ); +} + +b2DebugDraw b2DefaultDebugDraw(void) +{ + b2DebugDraw draw = { 0 }; + + // These allow the user to skip some implementations and not hit null exceptions. + draw.DrawPolygon = b2EmptyDrawPolygon; + draw.DrawSolidPolygon = b2EmptyDrawSolidPolygon; + draw.DrawCircle = b2EmptyDrawCircle; + draw.DrawSolidCircle = b2EmptyDrawSolidCircle; + draw.DrawSolidCapsule = b2EmptyDrawSolidCapsule; + draw.DrawSegment = b2EmptyDrawSegment; + draw.DrawTransform = b2EmptyDrawTransform; + draw.DrawPoint = b2EmptyDrawPoint; + draw.DrawString = b2EmptyDrawString; + return draw; +} diff --git a/3rdparty/box2d/src/weld_joint.c b/3rdparty/box2d/src/weld_joint.c new file mode 100644 index 000000000000..ea8b5a0965bc --- /dev/null +++ b/3rdparty/box2d/src/weld_joint.c @@ -0,0 +1,298 @@ +// SPDX-FileCopyrightText: 2023 Erin Catto +// SPDX-License-Identifier: MIT + +#include "body.h" +#include "core.h" +#include "joint.h" +#include "solver.h" +#include "solver_set.h" +#include "world.h" + +// needed for dll export +#include "box2d/box2d.h" + +void b2WeldJoint_SetLinearHertz( b2JointId jointId, float hertz ) +{ + B2_ASSERT( b2IsValid( hertz ) && hertz >= 0.0f ); + b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_weldJoint ); + joint->weldJoint.linearHertz = hertz; +} + +float b2WeldJoint_GetLinearHertz( b2JointId jointId ) +{ + b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_weldJoint ); + return joint->weldJoint.linearHertz; +} + +void b2WeldJoint_SetLinearDampingRatio( b2JointId jointId, float dampingRatio ) +{ + B2_ASSERT( b2IsValid( dampingRatio ) && dampingRatio >= 0.0f ); + b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_weldJoint ); + joint->weldJoint.linearDampingRatio = dampingRatio; +} + +float b2WeldJoint_GetLinearDampingRatio( b2JointId jointId ) +{ + b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_weldJoint ); + return joint->weldJoint.linearDampingRatio; +} + +void b2WeldJoint_SetAngularHertz( b2JointId jointId, float hertz ) +{ + B2_ASSERT( b2IsValid( hertz ) && hertz >= 0.0f ); + b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_weldJoint ); + joint->weldJoint.angularHertz = hertz; +} + +float b2WeldJoint_GetAngularHertz( b2JointId jointId ) +{ + b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_weldJoint ); + return joint->weldJoint.angularHertz; +} + +void b2WeldJoint_SetAngularDampingRatio( b2JointId jointId, float dampingRatio ) +{ + B2_ASSERT( b2IsValid( dampingRatio ) && dampingRatio >= 0.0f ); + b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_weldJoint ); + joint->weldJoint.angularDampingRatio = dampingRatio; +} + +float b2WeldJoint_GetAngularDampingRatio( b2JointId jointId ) +{ + b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_weldJoint ); + return joint->weldJoint.angularDampingRatio; +} + +b2Vec2 b2GetWeldJointForce( b2World* world, b2JointSim* base ) +{ + b2Vec2 force = b2MulSV( world->inv_h, base->weldJoint.linearImpulse ); + return force; +} + +float b2GetWeldJointTorque( b2World* world, b2JointSim* base ) +{ + return world->inv_h * base->weldJoint.angularImpulse; +} + +// Point-to-point constraint +// C = p2 - p1 +// Cdot = v2 - v1 +// = v2 + cross(w2, r2) - v1 - cross(w1, r1) +// J = [-I -r1_skew I r2_skew ] +// Identity used: +// w k % (rx i + ry j) = w * (-ry i + rx j) + +// Angle constraint +// C = angle2 - angle1 - referenceAngle +// Cdot = w2 - w1 +// J = [0 0 -1 0 0 1] +// K = invI1 + invI2 + +void b2PrepareWeldJoint( b2JointSim* base, b2StepContext* context ) +{ + B2_ASSERT( base->type == b2_weldJoint ); + + // chase body id to the solver set where the body lives + int idA = base->bodyIdA; + int idB = base->bodyIdB; + + b2World* world = context->world; + + b2Body* bodyA = b2BodyArray_Get( &world->bodies, idA ); + b2Body* bodyB = b2BodyArray_Get( &world->bodies, idB ); + + B2_ASSERT( bodyA->setIndex == b2_awakeSet || bodyB->setIndex == b2_awakeSet ); + b2SolverSet* setA = b2SolverSetArray_Get( &world->solverSets, bodyA->setIndex ); + b2SolverSet* setB = b2SolverSetArray_Get( &world->solverSets, bodyB->setIndex ); + + int localIndexA = bodyA->localIndex; + int localIndexB = bodyB->localIndex; + + b2BodySim* bodySimA = b2BodySimArray_Get( &setA->bodySims, localIndexA ); + b2BodySim* bodySimB = b2BodySimArray_Get( &setB->bodySims, localIndexB ); + + float mA = bodySimA->invMass; + float iA = bodySimA->invInertia; + float mB = bodySimB->invMass; + float iB = bodySimB->invInertia; + + base->invMassA = mA; + base->invMassB = mB; + base->invIA = iA; + base->invIB = iB; + + b2WeldJoint* joint = &base->weldJoint; + joint->indexA = bodyA->setIndex == b2_awakeSet ? localIndexA : B2_NULL_INDEX; + joint->indexB = bodyB->setIndex == b2_awakeSet ? localIndexB : B2_NULL_INDEX; + + b2Rot qA = bodySimA->transform.q; + b2Rot qB = bodySimB->transform.q; + + joint->anchorA = b2RotateVector( qA, b2Sub( base->localOriginAnchorA, bodySimA->localCenter ) ); + joint->anchorB = b2RotateVector( qB, b2Sub( base->localOriginAnchorB, bodySimB->localCenter ) ); + joint->deltaCenter = b2Sub( bodySimB->center, bodySimA->center ); + joint->deltaAngle = b2RelativeAngle( qB, qA ) - joint->referenceAngle; + + float ka = iA + iB; + joint->axialMass = ka > 0.0f ? 1.0f / ka : 0.0f; + + const float h = context->dt; + + if ( joint->linearHertz == 0.0f ) + { + joint->linearSoftness = context->jointSoftness; + } + else + { + joint->linearSoftness = b2MakeSoft( joint->linearHertz, joint->linearDampingRatio, context->h ); + } + + if ( joint->angularHertz == 0.0f ) + { + joint->angularSoftness = context->jointSoftness; + } + else + { + joint->angularSoftness = b2MakeSoft( joint->angularHertz, joint->angularDampingRatio, context->h ); + } + + if ( context->enableWarmStarting == false ) + { + joint->linearImpulse = b2Vec2_zero; + joint->angularImpulse = 0.0f; + } +} + +void b2WarmStartWeldJoint( b2JointSim* base, b2StepContext* context ) +{ + float mA = base->invMassA; + float mB = base->invMassB; + float iA = base->invIA; + float iB = base->invIB; + + // dummy state for static bodies + b2BodyState dummyState = b2_identityBodyState; + + b2WeldJoint* joint = &base->weldJoint; + + b2BodyState* stateA = joint->indexA == B2_NULL_INDEX ? &dummyState : context->states + joint->indexA; + b2BodyState* stateB = joint->indexB == B2_NULL_INDEX ? &dummyState : context->states + joint->indexB; + + b2Vec2 rA = b2RotateVector( stateA->deltaRotation, joint->anchorA ); + b2Vec2 rB = b2RotateVector( stateB->deltaRotation, joint->anchorB ); + + stateA->linearVelocity = b2MulSub( stateA->linearVelocity, mA, joint->linearImpulse ); + stateA->angularVelocity -= iA * ( b2Cross( rA, joint->linearImpulse ) + joint->angularImpulse ); + + stateB->linearVelocity = b2MulAdd( stateB->linearVelocity, mB, joint->linearImpulse ); + stateB->angularVelocity += iB * ( b2Cross( rB, joint->linearImpulse ) + joint->angularImpulse ); +} + +void b2SolveWeldJoint( b2JointSim* base, b2StepContext* context, bool useBias ) +{ + B2_ASSERT( base->type == b2_weldJoint ); + + float mA = base->invMassA; + float mB = base->invMassB; + float iA = base->invIA; + float iB = base->invIB; + + // dummy state for static bodies + b2BodyState dummyState = b2_identityBodyState; + + b2WeldJoint* joint = &base->weldJoint; + + b2BodyState* stateA = joint->indexA == B2_NULL_INDEX ? &dummyState : context->states + joint->indexA; + b2BodyState* stateB = joint->indexB == B2_NULL_INDEX ? &dummyState : context->states + joint->indexB; + + b2Vec2 vA = stateA->linearVelocity; + float wA = stateA->angularVelocity; + b2Vec2 vB = stateB->linearVelocity; + float wB = stateB->angularVelocity; + + // angular constraint + { + float bias = 0.0f; + float massScale = 1.0f; + float impulseScale = 0.0f; + if ( useBias || joint->angularHertz > 0.0f ) + { + float C = b2RelativeAngle( stateB->deltaRotation, stateA->deltaRotation ) + joint->deltaAngle; + bias = joint->angularSoftness.biasRate * C; + massScale = joint->angularSoftness.massScale; + impulseScale = joint->angularSoftness.impulseScale; + } + + float Cdot = wB - wA; + float impulse = -massScale * joint->axialMass * ( Cdot + bias ) - impulseScale * joint->angularImpulse; + joint->angularImpulse += impulse; + + wA -= iA * impulse; + wB += iB * impulse; + } + + // linear constraint + { + b2Vec2 rA = b2RotateVector( stateA->deltaRotation, joint->anchorA ); + b2Vec2 rB = b2RotateVector( stateB->deltaRotation, joint->anchorB ); + + b2Vec2 bias = b2Vec2_zero; + float massScale = 1.0f; + float impulseScale = 0.0f; + if ( useBias || joint->linearHertz > 0.0f ) + { + b2Vec2 dcA = stateA->deltaPosition; + b2Vec2 dcB = stateB->deltaPosition; + b2Vec2 C = b2Add( b2Add( b2Sub( dcB, dcA ), b2Sub( rB, rA ) ), joint->deltaCenter ); + + bias = b2MulSV( joint->linearSoftness.biasRate, C ); + massScale = joint->linearSoftness.massScale; + impulseScale = joint->linearSoftness.impulseScale; + } + + b2Vec2 Cdot = b2Sub( b2Add( vB, b2CrossSV( wB, rB ) ), b2Add( vA, b2CrossSV( wA, rA ) ) ); + + b2Mat22 K; + K.cx.x = mA + mB + rA.y * rA.y * iA + rB.y * rB.y * iB; + K.cy.x = -rA.y * rA.x * iA - rB.y * rB.x * iB; + K.cx.y = K.cy.x; + K.cy.y = mA + mB + rA.x * rA.x * iA + rB.x * rB.x * iB; + b2Vec2 b = b2Solve22( K, b2Add( Cdot, bias ) ); + + b2Vec2 impulse = { + -massScale * b.x - impulseScale * joint->linearImpulse.x, + -massScale * b.y - impulseScale * joint->linearImpulse.y, + }; + + joint->linearImpulse = b2Add( joint->linearImpulse, impulse ); + + vA = b2MulSub( vA, mA, impulse ); + wA -= iA * b2Cross( rA, impulse ); + vB = b2MulAdd( vB, mB, impulse ); + wB += iB * b2Cross( rB, impulse ); + } + + stateA->linearVelocity = vA; + stateA->angularVelocity = wA; + stateB->linearVelocity = vB; + stateB->angularVelocity = wB; +} + +#if 0 +void b2DumpWeldJoint() +{ + int32 indexA = m_bodyA->m_islandIndex; + int32 indexB = m_bodyB->m_islandIndex; + + b2Dump(" b2WeldJointDef jd;\n"); + b2Dump(" jd.bodyA = sims[%d];\n", indexA); + b2Dump(" jd.bodyB = sims[%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); +} +#endif diff --git a/3rdparty/box2d/src/wheel_joint.c b/3rdparty/box2d/src/wheel_joint.c new file mode 100644 index 000000000000..e4d96b401505 --- /dev/null +++ b/3rdparty/box2d/src/wheel_joint.c @@ -0,0 +1,554 @@ +// SPDX-FileCopyrightText: 2023 Erin Catto +// SPDX-License-Identifier: MIT + +#include "body.h" +#include "core.h" +#include "joint.h" +#include "solver.h" +#include "solver_set.h" +#include "world.h" + +// needed for dll export +#include "box2d/box2d.h" + +#include + +void b2WheelJoint_EnableSpring( b2JointId jointId, bool enableSpring ) +{ + b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_wheelJoint ); + + if ( enableSpring != joint->wheelJoint.enableSpring ) + { + joint->wheelJoint.enableSpring = enableSpring; + joint->wheelJoint.springImpulse = 0.0f; + } +} + +bool b2WheelJoint_IsSpringEnabled( b2JointId jointId ) +{ + b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_wheelJoint ); + return joint->wheelJoint.enableSpring; +} + +void b2WheelJoint_SetSpringHertz( b2JointId jointId, float hertz ) +{ + b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_wheelJoint ); + joint->wheelJoint.hertz = hertz; +} + +float b2WheelJoint_GetSpringHertz( b2JointId jointId ) +{ + b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_wheelJoint ); + return joint->wheelJoint.hertz; +} + +void b2WheelJoint_SetSpringDampingRatio( b2JointId jointId, float dampingRatio ) +{ + b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_wheelJoint ); + joint->wheelJoint.dampingRatio = dampingRatio; +} + +float b2WheelJoint_GetSpringDampingRatio( b2JointId jointId ) +{ + b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_wheelJoint ); + return joint->wheelJoint.dampingRatio; +} + +void b2WheelJoint_EnableLimit( b2JointId jointId, bool enableLimit ) +{ + b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_wheelJoint ); + if ( joint->wheelJoint.enableLimit != enableLimit ) + { + joint->wheelJoint.lowerImpulse = 0.0f; + joint->wheelJoint.upperImpulse = 0.0f; + joint->wheelJoint.enableLimit = enableLimit; + } +} + +bool b2WheelJoint_IsLimitEnabled( b2JointId jointId ) +{ + b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_wheelJoint ); + return joint->wheelJoint.enableLimit; +} + +float b2WheelJoint_GetLowerLimit( b2JointId jointId ) +{ + b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_wheelJoint ); + return joint->wheelJoint.lowerTranslation; +} + +float b2WheelJoint_GetUpperLimit( b2JointId jointId ) +{ + b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_wheelJoint ); + return joint->wheelJoint.upperTranslation; +} + +void b2WheelJoint_SetLimits( b2JointId jointId, float lower, float upper ) +{ + b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_wheelJoint ); + if ( lower != joint->wheelJoint.lowerTranslation || upper != joint->wheelJoint.upperTranslation ) + { + joint->wheelJoint.lowerTranslation = b2MinFloat( lower, upper ); + joint->wheelJoint.upperTranslation = b2MaxFloat( lower, upper ); + joint->wheelJoint.lowerImpulse = 0.0f; + joint->wheelJoint.upperImpulse = 0.0f; + } +} + +void b2WheelJoint_EnableMotor( b2JointId jointId, bool enableMotor ) +{ + b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_wheelJoint ); + if ( joint->wheelJoint.enableMotor != enableMotor ) + { + joint->wheelJoint.motorImpulse = 0.0f; + joint->wheelJoint.enableMotor = enableMotor; + } +} + +bool b2WheelJoint_IsMotorEnabled( b2JointId jointId ) +{ + b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_wheelJoint ); + return joint->wheelJoint.enableMotor; +} + +void b2WheelJoint_SetMotorSpeed( b2JointId jointId, float motorSpeed ) +{ + b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_wheelJoint ); + joint->wheelJoint.motorSpeed = motorSpeed; +} + +float b2WheelJoint_GetMotorSpeed( b2JointId jointId ) +{ + b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_wheelJoint ); + return joint->wheelJoint.motorSpeed; +} + +float b2WheelJoint_GetMotorTorque( b2JointId jointId ) +{ + b2World* world = b2GetWorld( jointId.world0 ); + b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_wheelJoint ); + return world->inv_h * joint->wheelJoint.motorImpulse; +} + +void b2WheelJoint_SetMaxMotorTorque( b2JointId jointId, float torque ) +{ + b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_wheelJoint ); + joint->wheelJoint.maxMotorTorque = torque; +} + +float b2WheelJoint_GetMaxMotorTorque( b2JointId jointId ) +{ + b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_wheelJoint ); + return joint->wheelJoint.maxMotorTorque; +} + +b2Vec2 b2GetWheelJointForce( b2World* world, b2JointSim* base ) +{ + b2WheelJoint* joint = &base->wheelJoint; + + // This is a frame behind + b2Vec2 axisA = joint->axisA; + b2Vec2 perpA = b2LeftPerp( axisA ); + + float perpForce = world->inv_h * joint->perpImpulse; + float axialForce = world->inv_h * ( joint->springImpulse + joint->lowerImpulse - joint->upperImpulse ); + + b2Vec2 force = b2Add( b2MulSV( perpForce, perpA ), b2MulSV( axialForce, axisA ) ); + return force; +} + +float b2GetWheelJointTorque( b2World* world, b2JointSim* base ) +{ + return world->inv_h * base->wheelJoint.motorImpulse; +} + +// 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 b2PrepareWheelJoint( b2JointSim* base, b2StepContext* context ) +{ + B2_ASSERT( base->type == b2_wheelJoint ); + + // chase body id to the solver set where the body lives + int idA = base->bodyIdA; + int idB = base->bodyIdB; + + b2World* world = context->world; + + b2Body* bodyA = b2BodyArray_Get( &world->bodies, idA ); + b2Body* bodyB = b2BodyArray_Get( &world->bodies, idB ); + + B2_ASSERT( bodyA->setIndex == b2_awakeSet || bodyB->setIndex == b2_awakeSet ); + b2SolverSet* setA = b2SolverSetArray_Get( &world->solverSets, bodyA->setIndex ); + b2SolverSet* setB = b2SolverSetArray_Get( &world->solverSets, bodyB->setIndex ); + + int localIndexA = bodyA->localIndex; + int localIndexB = bodyB->localIndex; + + b2BodySim* bodySimA = b2BodySimArray_Get( &setA->bodySims, localIndexA ); + b2BodySim* bodySimB = b2BodySimArray_Get( &setB->bodySims, localIndexB ); + + float mA = bodySimA->invMass; + float iA = bodySimA->invInertia; + float mB = bodySimB->invMass; + float iB = bodySimB->invInertia; + + base->invMassA = mA; + base->invMassB = mB; + base->invIA = iA; + base->invIB = iB; + + b2WheelJoint* joint = &base->wheelJoint; + + joint->indexA = bodyA->setIndex == b2_awakeSet ? localIndexA : B2_NULL_INDEX; + joint->indexB = bodyB->setIndex == b2_awakeSet ? localIndexB : B2_NULL_INDEX; + + b2Rot qA = bodySimA->transform.q; + b2Rot qB = bodySimB->transform.q; + + joint->anchorA = b2RotateVector( qA, b2Sub( base->localOriginAnchorA, bodySimA->localCenter ) ); + joint->anchorB = b2RotateVector( qB, b2Sub( base->localOriginAnchorB, bodySimB->localCenter ) ); + joint->axisA = b2RotateVector( qA, joint->localAxisA ); + joint->deltaCenter = b2Sub( bodySimB->center, bodySimA->center ); + + b2Vec2 rA = joint->anchorA; + b2Vec2 rB = joint->anchorB; + + b2Vec2 d = b2Add( joint->deltaCenter, b2Sub( rB, rA ) ); + b2Vec2 axisA = joint->axisA; + b2Vec2 perpA = b2LeftPerp( axisA ); + + // perpendicular constraint (keep wheel on line) + float s1 = b2Cross( b2Add( d, rA ), perpA ); + float s2 = b2Cross( rB, perpA ); + + float kp = mA + mB + iA * s1 * s1 + iB * s2 * s2; + joint->perpMass = kp > 0.0f ? 1.0f / kp : 0.0f; + + // spring constraint + float a1 = b2Cross( b2Add( d, rA ), axisA ); + float a2 = b2Cross( rB, axisA ); + + float ka = mA + mB + iA * a1 * a1 + iB * a2 * a2; + joint->axialMass = ka > 0.0f ? 1.0f / ka : 0.0f; + + joint->springSoftness = b2MakeSoft( joint->hertz, joint->dampingRatio, context->h ); + + float km = iA + iB; + joint->motorMass = km > 0.0f ? 1.0f / km : 0.0f; + + if ( context->enableWarmStarting == false ) + { + joint->perpImpulse = 0.0f; + joint->springImpulse = 0.0f; + joint->motorImpulse = 0.0f; + joint->lowerImpulse = 0.0f; + joint->upperImpulse = 0.0f; + } +} + +void b2WarmStartWheelJoint( b2JointSim* base, b2StepContext* context ) +{ + B2_ASSERT( base->type == b2_wheelJoint ); + + float mA = base->invMassA; + float mB = base->invMassB; + float iA = base->invIA; + float iB = base->invIB; + + // dummy state for static bodies + b2BodyState dummyState = b2_identityBodyState; + + b2WheelJoint* joint = &base->wheelJoint; + + b2BodyState* stateA = joint->indexA == B2_NULL_INDEX ? &dummyState : context->states + joint->indexA; + b2BodyState* stateB = joint->indexB == B2_NULL_INDEX ? &dummyState : context->states + joint->indexB; + + b2Vec2 rA = b2RotateVector( stateA->deltaRotation, joint->anchorA ); + b2Vec2 rB = b2RotateVector( stateB->deltaRotation, joint->anchorB ); + + b2Vec2 d = b2Add( b2Add( b2Sub( stateB->deltaPosition, stateA->deltaPosition ), joint->deltaCenter ), b2Sub( rB, rA ) ); + b2Vec2 axisA = b2RotateVector( stateA->deltaRotation, joint->axisA ); + b2Vec2 perpA = b2LeftPerp( axisA ); + + float a1 = b2Cross( b2Add( d, rA ), axisA ); + float a2 = b2Cross( rB, axisA ); + float s1 = b2Cross( b2Add( d, rA ), perpA ); + float s2 = b2Cross( rB, perpA ); + + float axialImpulse = joint->springImpulse + joint->lowerImpulse - joint->upperImpulse; + + b2Vec2 P = b2Add( b2MulSV( axialImpulse, axisA ), b2MulSV( joint->perpImpulse, perpA ) ); + float LA = axialImpulse * a1 + joint->perpImpulse * s1 + joint->motorImpulse; + float LB = axialImpulse * a2 + joint->perpImpulse * s2 + joint->motorImpulse; + + stateA->linearVelocity = b2MulSub( stateA->linearVelocity, mA, P ); + stateA->angularVelocity -= iA * LA; + stateB->linearVelocity = b2MulAdd( stateB->linearVelocity, mB, P ); + stateB->angularVelocity += iB * LB; +} + +void b2SolveWheelJoint( b2JointSim* base, b2StepContext* context, bool useBias ) +{ + B2_ASSERT( base->type == b2_wheelJoint ); + + float mA = base->invMassA; + float mB = base->invMassB; + float iA = base->invIA; + float iB = base->invIB; + + // dummy state for static bodies + b2BodyState dummyState = b2_identityBodyState; + + b2WheelJoint* joint = &base->wheelJoint; + + // This is a dummy body to represent a static body since static bodies don't have a solver body. + b2BodyState dummyBody = { 0 }; + + b2BodyState* stateA = joint->indexA == B2_NULL_INDEX ? &dummyState : context->states + joint->indexA; + b2BodyState* stateB = joint->indexB == B2_NULL_INDEX ? &dummyState : context->states + joint->indexB; + + b2Vec2 vA = stateA->linearVelocity; + float wA = stateA->angularVelocity; + b2Vec2 vB = stateB->linearVelocity; + float wB = stateB->angularVelocity; + + bool fixedRotation = ( iA + iB == 0.0f ); + + // current anchors + b2Vec2 rA = b2RotateVector( stateA->deltaRotation, joint->anchorA ); + b2Vec2 rB = b2RotateVector( stateB->deltaRotation, joint->anchorB ); + + b2Vec2 d = b2Add( b2Add( b2Sub( stateB->deltaPosition, stateA->deltaPosition ), joint->deltaCenter ), b2Sub( rB, rA ) ); + b2Vec2 axisA = b2RotateVector( stateA->deltaRotation, joint->axisA ); + float translation = b2Dot( axisA, d ); + + float a1 = b2Cross( b2Add( d, rA ), axisA ); + float a2 = b2Cross( rB, axisA ); + + // motor constraint + if ( joint->enableMotor && fixedRotation == false ) + { + float Cdot = wB - wA - joint->motorSpeed; + float impulse = -joint->motorMass * Cdot; + float oldImpulse = joint->motorImpulse; + float maxImpulse = context->h * joint->maxMotorTorque; + joint->motorImpulse = b2ClampFloat( joint->motorImpulse + impulse, -maxImpulse, maxImpulse ); + impulse = joint->motorImpulse - oldImpulse; + + wA -= iA * impulse; + wB += iB * impulse; + } + + // spring constraint + if ( joint->enableSpring ) + { + // This is a real spring and should be applied even during relax + float C = translation; + float bias = joint->springSoftness.biasRate * C; + float massScale = joint->springSoftness.massScale; + float impulseScale = joint->springSoftness.impulseScale; + + float Cdot = b2Dot( axisA, b2Sub( vB, vA ) ) + a2 * wB - a1 * wA; + float impulse = -massScale * joint->axialMass * ( Cdot + bias ) - impulseScale * joint->springImpulse; + joint->springImpulse += impulse; + + b2Vec2 P = b2MulSV( impulse, axisA ); + float LA = impulse * a1; + float LB = impulse * a2; + + vA = b2MulSub( vA, mA, P ); + wA -= iA * LA; + vB = b2MulAdd( vB, mB, P ); + wB += iB * LB; + } + + if ( joint->enableLimit ) + { + float translation = b2Dot( axisA, d ); + + // Lower limit + { + float C = translation - joint->lowerTranslation; + float bias = 0.0f; + float massScale = 1.0f; + float impulseScale = 0.0f; + + if ( C > 0.0f ) + { + // speculation + bias = C * context->inv_h; + } + else if ( useBias ) + { + bias = context->jointSoftness.biasRate * C; + massScale = context->jointSoftness.massScale; + impulseScale = context->jointSoftness.impulseScale; + } + + float Cdot = b2Dot( axisA, b2Sub( vB, vA ) ) + a2 * wB - a1 * wA; + float impulse = -massScale * joint->axialMass * ( Cdot + bias ) - impulseScale * joint->lowerImpulse; + float oldImpulse = joint->lowerImpulse; + joint->lowerImpulse = b2MaxFloat( oldImpulse + impulse, 0.0f ); + impulse = joint->lowerImpulse - oldImpulse; + + b2Vec2 P = b2MulSV( impulse, axisA ); + float LA = impulse * a1; + float LB = impulse * a2; + + vA = b2MulSub( vA, mA, P ); + wA -= iA * LA; + vB = b2MulAdd( 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. + { + // sign flipped + float C = joint->upperTranslation - translation; + float bias = 0.0f; + float massScale = 1.0f; + float impulseScale = 0.0f; + + if ( C > 0.0f ) + { + // speculation + bias = C * context->inv_h; + } + else if ( useBias ) + { + bias = context->jointSoftness.biasRate * C; + massScale = context->jointSoftness.massScale; + impulseScale = context->jointSoftness.impulseScale; + } + + // sign flipped on Cdot + float Cdot = b2Dot( axisA, b2Sub( vA, vB ) ) + a1 * wA - a2 * wB; + float impulse = -massScale * joint->axialMass * ( Cdot + bias ) - impulseScale * joint->upperImpulse; + float oldImpulse = joint->upperImpulse; + joint->upperImpulse = b2MaxFloat( oldImpulse + impulse, 0.0f ); + impulse = joint->upperImpulse - oldImpulse; + + b2Vec2 P = b2MulSV( impulse, axisA ); + float LA = impulse * a1; + float LB = impulse * a2; + + // sign flipped on applied impulse + vA = b2MulAdd( vA, mA, P ); + wA += iA * LA; + vB = b2MulSub( vB, mB, P ); + wB -= iB * LB; + } + } + + // point to line constraint + { + b2Vec2 perpA = b2LeftPerp( axisA ); + + float bias = 0.0f; + float massScale = 1.0f; + float impulseScale = 0.0f; + if ( useBias ) + { + float C = b2Dot( perpA, d ); + bias = context->jointSoftness.biasRate * C; + massScale = context->jointSoftness.massScale; + impulseScale = context->jointSoftness.impulseScale; + } + + float s1 = b2Cross( b2Add( d, rA ), perpA ); + float s2 = b2Cross( rB, perpA ); + float Cdot = b2Dot( perpA, b2Sub( vB, vA ) ) + s2 * wB - s1 * wA; + + float impulse = -massScale * joint->perpMass * ( Cdot + bias ) - impulseScale * joint->perpImpulse; + joint->perpImpulse += impulse; + + b2Vec2 P = b2MulSV( impulse, perpA ); + float LA = impulse * s1; + float LB = impulse * s2; + + vA = b2MulSub( vA, mA, P ); + wA -= iA * LA; + vB = b2MulAdd( vB, mB, P ); + wB += iB * LB; + } + + stateA->linearVelocity = vA; + stateA->angularVelocity = wA; + stateB->linearVelocity = vB; + stateB->angularVelocity = wB; +} + +#if 0 +void b2WheelJoint_Dump() +{ + int32 indexA = joint->bodyA->joint->islandIndex; + int32 indexB = joint->bodyB->joint->islandIndex; + + b2Dump(" b2WheelJointDef jd;\n"); + b2Dump(" jd.bodyA = sims[%d];\n", indexA); + b2Dump(" jd.bodyB = sims[%d];\n", indexB); + b2Dump(" jd.collideConnected = bool(%d);\n", joint->collideConnected); + b2Dump(" jd.localAnchorA.Set(%.9g, %.9g);\n", joint->localAnchorA.x, joint->localAnchorA.y); + b2Dump(" jd.localAnchorB.Set(%.9g, %.9g);\n", joint->localAnchorB.x, joint->localAnchorB.y); + b2Dump(" jd.referenceAngle = %.9g;\n", joint->referenceAngle); + b2Dump(" jd.enableLimit = bool(%d);\n", joint->enableLimit); + b2Dump(" jd.lowerAngle = %.9g;\n", joint->lowerAngle); + b2Dump(" jd.upperAngle = %.9g;\n", joint->upperAngle); + b2Dump(" jd.enableMotor = bool(%d);\n", joint->enableMotor); + b2Dump(" jd.motorSpeed = %.9g;\n", joint->motorSpeed); + b2Dump(" jd.maxMotorTorque = %.9g;\n", joint->maxMotorTorque); + b2Dump(" joints[%d] = joint->world->CreateJoint(&jd);\n", joint->index); +} +#endif + +void b2DrawWheelJoint( b2DebugDraw* draw, b2JointSim* base, b2Transform transformA, b2Transform transformB ) +{ + B2_ASSERT( base->type == b2_wheelJoint ); + + b2WheelJoint* joint = &base->wheelJoint; + + b2Vec2 pA = b2TransformPoint( transformA, base->localOriginAnchorA ); + b2Vec2 pB = b2TransformPoint( transformB, base->localOriginAnchorB ); + b2Vec2 axis = b2RotateVector( transformA.q, joint->localAxisA ); + + b2HexColor c1 = b2_colorGray7; + b2HexColor c2 = b2_colorGreen; + b2HexColor c3 = b2_colorRed; + b2HexColor c4 = b2_colorGray4; + b2HexColor c5 = b2_colorBlue; + + draw->DrawSegment( pA, pB, c5, draw->context ); + + if ( joint->enableLimit ) + { + b2Vec2 lower = b2MulAdd( pA, joint->lowerTranslation, axis ); + b2Vec2 upper = b2MulAdd( pA, joint->upperTranslation, axis ); + b2Vec2 perp = b2LeftPerp( axis ); + draw->DrawSegment( lower, upper, c1, draw->context ); + draw->DrawSegment( b2MulSub( lower, 0.1f, perp ), b2MulAdd( lower, 0.1f, perp ), c2, draw->context ); + draw->DrawSegment( b2MulSub( upper, 0.1f, perp ), b2MulAdd( upper, 0.1f, perp ), c3, draw->context ); + } + else + { + draw->DrawSegment( b2MulSub( pA, 1.0f, axis ), b2MulAdd( pA, 1.0f, axis ), c1, draw->context ); + } + + draw->DrawPoint( pA, 5.0f, c1, draw->context ); + draw->DrawPoint( pB, 5.0f, c4, draw->context ); +} diff --git a/3rdparty/box2d/src/world.c b/3rdparty/box2d/src/world.c new file mode 100644 index 000000000000..53d462dacc2e --- /dev/null +++ b/3rdparty/box2d/src/world.c @@ -0,0 +1,2972 @@ +// SPDX-FileCopyrightText: 2023 Erin Catto +// SPDX-License-Identifier: MIT + +#if defined( _MSC_VER ) && !defined( _CRT_SECURE_NO_WARNINGS ) +#define _CRT_SECURE_NO_WARNINGS +#endif + +#include "world.h" + +#include "aabb.h" +#include "array.h" +#include "bitset.h" +#include "body.h" +#include "broad_phase.h" +#include "constraint_graph.h" +#include "contact.h" +#include "core.h" +#include "ctz.h" +#include "island.h" +#include "joint.h" +#include "shape.h" +#include "solver.h" +#include "solver_set.h" +#include "stack_allocator.h" + +#include "box2d/box2d.h" + +#include +#include +#include + +_Static_assert( b2_maxWorlds > 0, "must be 1 or more" ); +b2World b2_worlds[b2_maxWorlds]; + +B2_ARRAY_SOURCE( b2BodyMoveEvent, b2BodyMoveEvent ); +B2_ARRAY_SOURCE( b2ContactBeginTouchEvent, b2ContactBeginTouchEvent ); +B2_ARRAY_SOURCE( b2ContactEndTouchEvent, b2ContactEndTouchEvent ); +B2_ARRAY_SOURCE( b2ContactHitEvent, b2ContactHitEvent ); +B2_ARRAY_SOURCE( b2SensorBeginTouchEvent, b2SensorBeginTouchEvent ); +B2_ARRAY_SOURCE( b2SensorEndTouchEvent, b2SensorEndTouchEvent ); +B2_ARRAY_SOURCE( b2TaskContext, b2TaskContext ); + +b2World* b2GetWorldFromId( b2WorldId id ) +{ + B2_ASSERT( 1 <= id.index1 && id.index1 <= b2_maxWorlds ); + b2World* world = b2_worlds + ( id.index1 - 1 ); + B2_ASSERT( id.index1 == world->worldId + 1 ); + B2_ASSERT( id.revision == world->revision ); + return world; +} + +b2World* b2GetWorld( int index ) +{ + B2_ASSERT( 0 <= index && index < b2_maxWorlds ); + b2World* world = b2_worlds + index; + B2_ASSERT( world->worldId == index ); + return world; +} + +b2World* b2GetWorldLocked( int index ) +{ + B2_ASSERT( 0 <= index && index < b2_maxWorlds ); + b2World* world = b2_worlds + index; + B2_ASSERT( world->worldId == index ); + if ( world->locked ) + { + B2_ASSERT( false ); + return NULL; + } + + return world; +} + +static void* b2DefaultAddTaskFcn( b2TaskCallback* task, int count, int minRange, void* taskContext, void* userContext ) +{ + B2_MAYBE_UNUSED( minRange ); + B2_MAYBE_UNUSED( userContext ); + task( 0, count, 0, taskContext ); + return NULL; +} + +static void b2DefaultFinishTaskFcn( void* userTask, void* userContext ) +{ + B2_MAYBE_UNUSED( userTask ); + B2_MAYBE_UNUSED( userContext ); +} + +b2WorldId b2CreateWorld( const b2WorldDef* def ) +{ + _Static_assert( b2_maxWorlds < UINT16_MAX, "b2_maxWorlds limit exceeded" ); + b2CheckDef( def ); + + int worldId = B2_NULL_INDEX; + for ( int i = 0; i < b2_maxWorlds; ++i ) + { + if ( b2_worlds[i].inUse == false ) + { + worldId = i; + break; + } + } + + if ( worldId == B2_NULL_INDEX ) + { + return ( b2WorldId ){ 0 }; + } + + b2InitializeContactRegisters(); + + b2World* world = b2_worlds + worldId; + uint16_t revision = world->revision; + + *world = ( b2World ){ 0 }; + + world->worldId = (uint16_t)worldId; + world->revision = revision; + world->inUse = true; + + world->stackAllocator = b2CreateStackAllocator( 2048 ); + b2CreateBroadPhase( &world->broadPhase ); + b2CreateGraph( &world->constraintGraph, 16 ); + + // pools + world->bodyIdPool = b2CreateIdPool(); + world->bodies = b2BodyArray_Create( 16 ); + world->solverSets = b2SolverSetArray_Create( 8 ); + + // add empty static, active, and disabled body sets + world->solverSetIdPool = b2CreateIdPool(); + b2SolverSet set = { 0 }; + + // static set + set.setIndex = b2AllocId( &world->solverSetIdPool ); + b2SolverSetArray_Push( &world->solverSets, set ); + B2_ASSERT( world->solverSets.data[b2_staticSet].setIndex == b2_staticSet ); + + // disabled set + set.setIndex = b2AllocId( &world->solverSetIdPool ); + b2SolverSetArray_Push( &world->solverSets, set ); + B2_ASSERT( world->solverSets.data[b2_disabledSet].setIndex == b2_disabledSet ); + + // awake set + set.setIndex = b2AllocId( &world->solverSetIdPool ); + b2SolverSetArray_Push( &world->solverSets, set ); + B2_ASSERT( world->solverSets.data[b2_awakeSet].setIndex == b2_awakeSet ); + + world->shapeIdPool = b2CreateIdPool(); + world->shapes = b2ShapeArray_Create( 16 ); + + world->chainIdPool = b2CreateIdPool(); + world->chainShapes = b2ChainShapeArray_Create( 4 ); + + world->contactIdPool = b2CreateIdPool(); + world->contacts = b2ContactArray_Create( 16 ); + + world->jointIdPool = b2CreateIdPool(); + world->joints = b2JointArray_Create( 16 ); + + world->islandIdPool = b2CreateIdPool(); + world->islands = b2IslandArray_Create( 8 ); + + world->bodyMoveEvents = b2BodyMoveEventArray_Create( 4 ); + world->sensorBeginEvents = b2SensorBeginTouchEventArray_Create( 4 ); + world->sensorEndEvents = b2SensorEndTouchEventArray_Create( 4 ); + world->contactBeginEvents = b2ContactBeginTouchEventArray_Create( 4 ); + world->contactEndEvents = b2ContactEndTouchEventArray_Create( 4 ); + world->contactHitEvents = b2ContactHitEventArray_Create( 4 ); + + world->stepIndex = 0; + world->splitIslandId = B2_NULL_INDEX; + world->activeTaskCount = 0; + world->taskCount = 0; + world->gravity = def->gravity; + world->hitEventThreshold = def->hitEventThreshold; + world->restitutionThreshold = def->restitutionThreshold; + world->maxLinearVelocity = def->maximumLinearVelocity; + world->contactPushoutVelocity = def->contactPushoutVelocity; + world->contactHertz = def->contactHertz; + world->contactDampingRatio = def->contactDampingRatio; + world->jointHertz = def->jointHertz; + world->jointDampingRatio = def->jointDampingRatio; + world->enableSleep = def->enableSleep; + world->locked = false; + world->enableWarmStarting = true; + world->enableContinuous = def->enableContinuous; + world->userTreeTask = NULL; + + if ( def->workerCount > 0 && def->enqueueTask != NULL && def->finishTask != NULL ) + { + world->workerCount = b2MinInt( def->workerCount, b2_maxWorkers ); + world->enqueueTaskFcn = def->enqueueTask; + world->finishTaskFcn = def->finishTask; + world->userTaskContext = def->userTaskContext; + } + else + { + world->workerCount = 1; + world->enqueueTaskFcn = b2DefaultAddTaskFcn; + world->finishTaskFcn = b2DefaultFinishTaskFcn; + world->userTaskContext = NULL; + } + + world->taskContexts = b2TaskContextArray_Create( world->workerCount ); + b2TaskContextArray_Resize( &world->taskContexts, world->workerCount ); + + for ( int i = 0; i < world->workerCount; ++i ) + { + world->taskContexts.data[i].contactStateBitSet = b2CreateBitSet( 1024 ); + world->taskContexts.data[i].enlargedSimBitSet = b2CreateBitSet( 256 ); + world->taskContexts.data[i].awakeIslandBitSet = b2CreateBitSet( 256 ); + } + + world->debugBodySet = b2CreateBitSet( 256 ); + world->debugJointSet = b2CreateBitSet( 256 ); + world->debugContactSet = b2CreateBitSet( 256 ); + + // add one to worldId so that 0 represents a null b2WorldId + return ( b2WorldId ){ (uint16_t)( worldId + 1 ), world->revision }; +} + +void b2DestroyWorld( b2WorldId worldId ) +{ + b2World* world = b2GetWorldFromId( worldId ); + + b2DestroyBitSet( &world->debugBodySet ); + b2DestroyBitSet( &world->debugJointSet ); + b2DestroyBitSet( &world->debugContactSet ); + + for ( int i = 0; i < world->workerCount; ++i ) + { + b2DestroyBitSet( &world->taskContexts.data[i].contactStateBitSet ); + b2DestroyBitSet( &world->taskContexts.data[i].enlargedSimBitSet ); + b2DestroyBitSet( &world->taskContexts.data[i].awakeIslandBitSet ); + } + + b2TaskContextArray_Destroy( &world->taskContexts ); + + b2BodyMoveEventArray_Destroy( &world->bodyMoveEvents ); + b2SensorBeginTouchEventArray_Destroy( &world->sensorBeginEvents ); + b2SensorEndTouchEventArray_Destroy( &world->sensorEndEvents ); + b2ContactBeginTouchEventArray_Destroy( &world->contactBeginEvents ); + b2ContactEndTouchEventArray_Destroy( &world->contactEndEvents ); + b2ContactHitEventArray_Destroy( &world->contactHitEvents ); + + int chainCapacity = world->chainShapes.count; + for ( int i = 0; i < chainCapacity; ++i ) + { + b2ChainShape* chain = world->chainShapes.data + i; + if ( chain->id != B2_NULL_INDEX ) + { + b2Free( chain->shapeIndices, chain->count * sizeof( int ) ); + } + else + { + B2_ASSERT( chain->shapeIndices == NULL ); + } + } + + b2BodyArray_Destroy( &world->bodies ); + b2ShapeArray_Destroy( &world->shapes ); + b2ChainShapeArray_Destroy( &world->chainShapes ); + b2ContactArray_Destroy( &world->contacts ); + b2JointArray_Destroy( &world->joints ); + b2IslandArray_Destroy( &world->islands ); + + // Destroy solver sets + int setCapacity = world->solverSets.count; + for ( int i = 0; i < setCapacity; ++i ) + { + b2SolverSet* set = world->solverSets.data + i; + if ( set->setIndex != B2_NULL_INDEX ) + { + b2DestroySolverSet( world, i ); + } + } + + b2SolverSetArray_Destroy( &world->solverSets ); + + b2DestroyGraph( &world->constraintGraph ); + b2DestroyBroadPhase( &world->broadPhase ); + + b2DestroyIdPool( &world->bodyIdPool ); + b2DestroyIdPool( &world->shapeIdPool ); + b2DestroyIdPool( &world->chainIdPool ); + b2DestroyIdPool( &world->contactIdPool ); + b2DestroyIdPool( &world->jointIdPool ); + b2DestroyIdPool( &world->islandIdPool ); + b2DestroyIdPool( &world->solverSetIdPool ); + + b2DestroyStackAllocator( &world->stackAllocator ); + + // Wipe world but preserve revision + uint16_t revision = world->revision; + *world = ( b2World ){ 0 }; + world->worldId = B2_NULL_INDEX; + world->revision = revision + 1; +} + +static void b2CollideTask( int startIndex, int endIndex, uint32_t threadIndex, void* context ) +{ + b2TracyCZoneNC( collide_task, "Collide Task", b2_colorDodgerBlue, true ); + + b2StepContext* stepContext = context; + b2World* world = stepContext->world; + B2_ASSERT( threadIndex < world->workerCount ); + b2TaskContext* taskContext = world->taskContexts.data + threadIndex; + b2ContactSim** contactSims = stepContext->contacts; + b2Shape* shapes = world->shapes.data; + b2Body* bodies = world->bodies.data; + + B2_ASSERT( startIndex < endIndex ); + + for ( int i = startIndex; i < endIndex; ++i ) + { + b2ContactSim* contactSim = contactSims[i]; + + int contactId = contactSim->contactId; + + b2Shape* shapeA = shapes + contactSim->shapeIdA; + b2Shape* shapeB = shapes + contactSim->shapeIdB; + + // Do proxies still overlap? + bool overlap = b2AABB_Overlaps( shapeA->fatAABB, shapeB->fatAABB ); + if ( overlap == false ) + { + contactSim->simFlags |= b2_simDisjoint; + contactSim->simFlags &= ~b2_simTouchingFlag; + b2SetBit( &taskContext->contactStateBitSet, contactId ); + } + else + { + bool wasTouching = ( contactSim->simFlags & b2_simTouchingFlag ); + + // Update contact respecting shape/body order (A,B) + b2Body* bodyA = bodies + shapeA->bodyId; + b2Body* bodyB = bodies + shapeB->bodyId; + b2BodySim* bodySimA = b2GetBodySim( world, bodyA ); + b2BodySim* bodySimB = b2GetBodySim( world, bodyB ); + + // avoid cache misses in b2PrepareContactsTask + contactSim->bodySimIndexA = bodyA->setIndex == b2_awakeSet ? bodyA->localIndex : B2_NULL_INDEX; + contactSim->invMassA = bodySimA->invMass; + contactSim->invIA = bodySimA->invInertia; + + contactSim->bodySimIndexB = bodyB->setIndex == b2_awakeSet ? bodyB->localIndex : B2_NULL_INDEX; + contactSim->invMassB = bodySimB->invMass; + contactSim->invIB = bodySimB->invInertia; + + b2Transform transformA = bodySimA->transform; + b2Transform transformB = bodySimB->transform; + + b2Vec2 centerOffsetA = b2RotateVector( transformA.q, bodySimA->localCenter ); + b2Vec2 centerOffsetB = b2RotateVector( transformB.q, bodySimB->localCenter ); + + // This updates solid contacts and sensors + bool touching = + b2UpdateContact( world, contactSim, shapeA, transformA, centerOffsetA, shapeB, transformB, centerOffsetB ); + + // State changes that affect island connectivity. Also contact and sensor events. + if ( touching == true && wasTouching == false ) + { + contactSim->simFlags |= b2_simStartedTouching; + b2SetBit( &taskContext->contactStateBitSet, contactId ); + } + else if ( touching == false && wasTouching == true ) + { + contactSim->simFlags |= b2_simStoppedTouching; + b2SetBit( &taskContext->contactStateBitSet, contactId ); + } + } + } + + b2TracyCZoneEnd( collide_task ); +} + +static void b2UpdateTreesTask( int startIndex, int endIndex, uint32_t threadIndex, void* context ) +{ + B2_MAYBE_UNUSED( startIndex ); + B2_MAYBE_UNUSED( endIndex ); + B2_MAYBE_UNUSED( threadIndex ); + + b2TracyCZoneNC( tree_task, "Rebuild Trees", b2_colorSnow, true ); + + b2World* world = context; + b2BroadPhase_RebuildTrees( &world->broadPhase ); + + b2TracyCZoneEnd( tree_task ); +} + +static void b2AddNonTouchingContact( b2World* world, b2Contact* contact, b2ContactSim* contactSim ) +{ + B2_ASSERT( contact->setIndex == b2_awakeSet ); + b2SolverSet* set = b2SolverSetArray_Get( &world->solverSets, b2_awakeSet ); + contact->colorIndex = B2_NULL_INDEX; + contact->localIndex = set->contactSims.count; + + b2ContactSim* newContactSim = b2ContactSimArray_Add( &set->contactSims ); + memcpy( newContactSim, contactSim, sizeof( b2ContactSim ) ); +} + +static void b2RemoveNonTouchingContact( b2World* world, int setIndex, int localIndex ) +{ + b2SolverSet* set = b2SolverSetArray_Get( &world->solverSets, setIndex ); + int movedIndex = b2ContactSimArray_RemoveSwap( &set->contactSims, localIndex ); + if ( movedIndex != B2_NULL_INDEX ) + { + b2ContactSim* movedContactSim = set->contactSims.data + localIndex; + b2Contact* movedContact = b2ContactArray_Get(&world->contacts, movedContactSim->contactId); + B2_ASSERT( movedContact->setIndex == setIndex ); + B2_ASSERT( movedContact->localIndex == movedIndex ); + B2_ASSERT( movedContact->colorIndex == B2_NULL_INDEX ); + movedContact->localIndex = localIndex; + } +} + +// Narrow-phase collision +static void b2Collide( b2StepContext* context ) +{ + b2World* world = context->world; + + B2_ASSERT( world->workerCount > 0 ); + + b2TracyCZoneNC( collide, "Collide", b2_colorDarkOrchid, true ); + + // Tasks that can be done in parallel with the narrow-phase + // - rebuild the collision tree for dynamic and kinematic bodies to keep their query performance good + world->userTreeTask = world->enqueueTaskFcn( &b2UpdateTreesTask, 1, 1, world, world->userTaskContext ); + world->taskCount += 1; + world->activeTaskCount += world->userTreeTask == NULL ? 0 : 1; + + // gather contacts into a single array for easier parallel-for + int contactCount = 0; + b2GraphColor* graphColors = world->constraintGraph.colors; + for ( int i = 0; i < b2_graphColorCount; ++i ) + { + contactCount += graphColors[i].contactSims.count; + } + + int nonTouchingCount = world->solverSets.data[b2_awakeSet].contactSims.count; + contactCount += nonTouchingCount; + + if ( contactCount == 0 ) + { + b2TracyCZoneEnd( collide ); + return; + } + + b2ContactSim** contactSims = b2AllocateStackItem( &world->stackAllocator, contactCount * sizeof( b2ContactSim ), "contacts" ); + + int contactIndex = 0; + for ( int i = 0; i < b2_graphColorCount; ++i ) + { + b2GraphColor* color = graphColors + i; + int count = color->contactSims.count; + b2ContactSim* base = color->contactSims.data; + for ( int j = 0; j < count; ++j ) + { + contactSims[contactIndex] = base + j; + contactIndex += 1; + } + } + + { + b2ContactSim* base = world->solverSets.data[b2_awakeSet].contactSims.data; + for ( int i = 0; i < nonTouchingCount; ++i ) + { + contactSims[contactIndex] = base + i; + contactIndex += 1; + } + } + + B2_ASSERT( contactIndex == contactCount ); + + context->contacts = contactSims; + + // Contact bit set on ids because contact pointers are unstable as they move between touching and not touching. + int contactIdCapacity = b2GetIdCapacity( &world->contactIdPool ); + for ( int i = 0; i < world->workerCount; ++i ) + { + b2SetBitCountAndClear( &world->taskContexts.data[i].contactStateBitSet, contactIdCapacity ); + } + + // Task should take at least 40us on a 4GHz CPU (10K cycles) + int minRange = 64; + void* userCollideTask = world->enqueueTaskFcn( &b2CollideTask, contactCount, minRange, context, world->userTaskContext ); + world->taskCount += 1; + if ( userCollideTask != NULL ) + { + world->finishTaskFcn( userCollideTask, world->userTaskContext ); + } + + b2FreeStackItem( &world->stackAllocator, contactSims ); + context->contacts = NULL; + contactSims = NULL; + + // Serially update contact state + b2TracyCZoneNC( contact_state, "Contact State", b2_colorCoral, true ); + + // Bitwise OR all contact bits + b2BitSet* bitSet = &world->taskContexts.data[0].contactStateBitSet; + for ( int i = 1; i < world->workerCount; ++i ) + { + b2InPlaceUnion( bitSet, &world->taskContexts.data[i].contactStateBitSet ); + } + + b2SolverSet* awakeSet = b2SolverSetArray_Get( &world->solverSets, b2_awakeSet ); + + const b2Shape* shapes = world->shapes.data; + int16_t worldId = world->worldId; + + // Process contact state changes. Iterate over set bits + for ( uint32_t k = 0; k < bitSet->blockCount; ++k ) + { + uint64_t bits = bitSet->bits[k]; + while ( bits != 0 ) + { + uint32_t ctz = b2CTZ64( bits ); + int contactId = (int)( 64 * k + ctz ); + + b2Contact* contact = b2ContactArray_Get(&world->contacts, contactId); + B2_ASSERT( contact->setIndex == b2_awakeSet ); + + int colorIndex = contact->colorIndex; + int localIndex = contact->localIndex; + + b2ContactSim* contactSim = NULL; + if ( colorIndex != B2_NULL_INDEX ) + { + // contact lives in constraint graph + B2_ASSERT( 0 <= colorIndex && colorIndex < b2_graphColorCount ); + b2GraphColor* color = graphColors + colorIndex; + contactSim = b2ContactSimArray_Get( &color->contactSims, localIndex ); + } + else + { + contactSim = b2ContactSimArray_Get( &awakeSet->contactSims, localIndex ); + } + + const b2Shape* shapeA = shapes + contact->shapeIdA; + const b2Shape* shapeB = shapes + contact->shapeIdB; + b2ShapeId shapeIdA = { shapeA->id + 1, worldId, shapeA->revision }; + b2ShapeId shapeIdB = { shapeB->id + 1, worldId, shapeB->revision }; + uint32_t flags = contact->flags; + uint32_t simFlags = contactSim->simFlags; + + if ( simFlags & b2_simDisjoint ) + { + // Was touching? + if ( ( flags & b2_contactTouchingFlag ) != 0 && ( flags & b2_contactEnableContactEvents ) != 0 ) + { + b2ContactEndTouchEvent event = { shapeIdA, shapeIdB }; + b2ContactEndTouchEventArray_Push( &world->contactEndEvents, event ); + } + + // Bounding boxes no longer overlap + contact->flags &= ~b2_contactTouchingFlag; + b2DestroyContact( world, contact, false ); + contact = NULL; + contactSim = NULL; + } + else if ( simFlags & b2_simStartedTouching ) + { + B2_ASSERT( contact->islandId == B2_NULL_INDEX ); + if ( ( flags & b2_contactSensorFlag ) != 0 ) + { + // Contact is a sensor + if ( ( flags & b2_contactEnableSensorEvents ) != 0 ) + { + if ( shapeA->isSensor ) + { + b2SensorBeginTouchEvent event = { shapeIdA, shapeIdB }; + b2SensorBeginTouchEventArray_Push( &world->sensorBeginEvents, event ); + } + + if ( shapeB->isSensor ) + { + b2SensorBeginTouchEvent event = { shapeIdB, shapeIdA }; + b2SensorBeginTouchEventArray_Push( &world->sensorBeginEvents, event ); + } + } + + contactSim->simFlags &= ~b2_simStartedTouching; + contact->flags |= b2_contactSensorTouchingFlag; + } + else + { + // Contact is solid + if ( flags & b2_contactEnableContactEvents ) + { + b2ContactBeginTouchEvent event = { shapeIdA, shapeIdB }; + b2ContactBeginTouchEventArray_Push( &world->contactBeginEvents, event ); + } + + B2_ASSERT( contactSim->manifold.pointCount > 0 ); + B2_ASSERT( contact->setIndex == b2_awakeSet ); + + // Link first because this wakes colliding bodies and ensures the body sims + // are in the correct place. + contact->flags |= b2_contactTouchingFlag; + b2LinkContact( world, contact ); + + // Make sure these didn't change + B2_ASSERT( contact->colorIndex == B2_NULL_INDEX ); + B2_ASSERT( contact->localIndex == localIndex ); + + // Contact sim pointer may have become orphaned due to awake set growth, + // so I just need to refresh it. + contactSim = b2ContactSimArray_Get( &awakeSet->contactSims, localIndex ); + + contactSim->simFlags &= ~b2_simStartedTouching; + + b2AddContactToGraph( world, contactSim, contact ); + b2RemoveNonTouchingContact( world, b2_awakeSet, localIndex ); + contactSim = NULL; + } + } + else if ( simFlags & b2_simStoppedTouching ) + { + contactSim->simFlags &= ~b2_simStoppedTouching; + + if ( ( flags & b2_contactSensorFlag ) != 0 ) + { + // Contact is a sensor + contact->flags &= ~b2_contactSensorTouchingFlag; + + if ( ( flags & b2_contactEnableSensorEvents ) != 0 ) + { + if ( shapeA->isSensor ) + { + b2SensorEndTouchEvent event = { shapeIdA, shapeIdB }; + b2SensorEndTouchEventArray_Push( &world->sensorEndEvents, event ); + } + + if ( shapeB->isSensor ) + { + b2SensorEndTouchEvent event = { shapeIdB, shapeIdA }; + b2SensorEndTouchEventArray_Push( &world->sensorEndEvents, event ); + } + } + } + else + { + // Contact is solid + contact->flags &= ~b2_contactTouchingFlag; + + if ( contact->flags & b2_contactEnableContactEvents ) + { + b2ContactEndTouchEvent event = { shapeIdA, shapeIdB }; + b2ContactEndTouchEventArray_Push( &world->contactEndEvents, event ); + } + + B2_ASSERT( contactSim->manifold.pointCount == 0 ); + + b2UnlinkContact( world, contact ); + int bodyIdA = contact->edges[0].bodyId; + int bodyIdB = contact->edges[1].bodyId; + + b2AddNonTouchingContact( world, contact, contactSim ); + b2RemoveContactFromGraph( world, bodyIdA, bodyIdB, colorIndex, localIndex ); + contact = NULL; + contactSim = NULL; + } + } + + // Clear the smallest set bit + bits = bits & ( bits - 1 ); + } + } + + b2ValidateSolverSets( world ); + b2ValidateContacts( world ); + + b2TracyCZoneEnd( contact_state ); + b2TracyCZoneEnd( collide ); +} + +void b2World_Step( b2WorldId worldId, float timeStep, int subStepCount ) +{ + b2World* world = b2GetWorldFromId( worldId ); + B2_ASSERT( world->locked == false ); + if ( world->locked ) + { + return; + } + + // Prepare to capture events + // Ensure user does not access stale data if there is an early return + b2BodyMoveEventArray_Clear( &world->bodyMoveEvents ); + b2SensorBeginTouchEventArray_Clear( &world->sensorBeginEvents ); + b2SensorEndTouchEventArray_Clear( &world->sensorEndEvents ); + b2ContactBeginTouchEventArray_Clear( &world->contactBeginEvents ); + b2ContactEndTouchEventArray_Clear( &world->contactEndEvents ); + b2ContactHitEventArray_Clear( &world->contactHitEvents ); + + world->profile = ( b2Profile ){ 0 }; + + if ( timeStep == 0.0f ) + { + // todo would be useful to still process collision while paused + return; + } + + b2TracyCZoneNC( world_step, "Step", b2_colorChartreuse, true ); + + world->locked = true; + world->activeTaskCount = 0; + world->taskCount = 0; + + b2Timer stepTimer = b2CreateTimer(); + + // Update collision pairs and create contacts + { + b2Timer timer = b2CreateTimer(); + b2UpdateBroadPhasePairs( world ); + world->profile.pairs = b2GetMilliseconds( &timer ); + } + + b2StepContext context = { 0 }; + context.world = world; + context.dt = timeStep; + context.subStepCount = b2MaxInt( 1, subStepCount ); + + if ( timeStep > 0.0f ) + { + context.inv_dt = 1.0f / timeStep; + context.h = timeStep / context.subStepCount; + context.inv_h = context.subStepCount * context.inv_dt; + } + else + { + context.inv_dt = 0.0f; + context.h = 0.0f; + context.inv_h = 0.0f; + } + + world->inv_h = context.inv_h; + + // Hertz values get reduced for large time steps + float contactHertz = b2MinFloat( world->contactHertz, 0.25f * context.inv_h ); + float jointHertz = b2MinFloat( world->jointHertz, 0.125f * context.inv_h ); + + context.contactSoftness = b2MakeSoft( contactHertz, world->contactDampingRatio, context.h ); + context.staticSoftness = b2MakeSoft( 2.0f * contactHertz, world->contactDampingRatio, context.h ); + context.jointSoftness = b2MakeSoft( jointHertz, world->jointDampingRatio, context.h ); + + context.restitutionThreshold = world->restitutionThreshold; + context.maxLinearVelocity = world->maxLinearVelocity; + context.enableWarmStarting = world->enableWarmStarting; + + // Update contacts + { + b2Timer timer = b2CreateTimer(); + b2Collide( &context ); + world->profile.collide = b2GetMilliseconds( &timer ); + } + + // Integrate velocities, solve velocity constraints, and integrate positions. + if ( context.dt > 0.0f ) + { + b2Timer timer = b2CreateTimer(); + b2Solve( world, &context ); + world->profile.solve = b2GetMilliseconds( &timer ); + } + + world->locked = false; + + world->profile.step = b2GetMilliseconds( &stepTimer ); + + B2_ASSERT( b2GetStackAllocation( &world->stackAllocator ) == 0 ); + + // Ensure stack is large enough + b2GrowStack( &world->stackAllocator ); + + // Make sure all tasks that were started were also finished + B2_ASSERT( world->activeTaskCount == 0 ); + + b2TracyCZoneEnd( world_step ); +} + +static void b2DrawShape( b2DebugDraw* draw, b2Shape* shape, b2Transform xf, b2HexColor color ) +{ + switch ( shape->type ) + { + case b2_capsuleShape: + { + b2Capsule* capsule = &shape->capsule; + b2Vec2 p1 = b2TransformPoint( xf, capsule->center1 ); + b2Vec2 p2 = b2TransformPoint( xf, capsule->center2 ); + draw->DrawSolidCapsule( p1, p2, capsule->radius, color, draw->context ); + } + break; + + case b2_circleShape: + { + b2Circle* circle = &shape->circle; + xf.p = b2TransformPoint( xf, circle->center ); + draw->DrawSolidCircle( xf, circle->radius, color, draw->context ); + } + break; + + case b2_polygonShape: + { + b2Polygon* poly = &shape->polygon; + draw->DrawSolidPolygon( xf, poly->vertices, poly->count, poly->radius, color, draw->context ); + } + break; + + case b2_segmentShape: + { + b2Segment* segment = &shape->segment; + b2Vec2 p1 = b2TransformPoint( xf, segment->point1 ); + b2Vec2 p2 = b2TransformPoint( xf, segment->point2 ); + draw->DrawSegment( p1, p2, color, draw->context ); + } + break; + + case b2_chainSegmentShape: + { + b2Segment* segment = &shape->chainSegment.segment; + b2Vec2 p1 = b2TransformPoint( xf, segment->point1 ); + b2Vec2 p2 = b2TransformPoint( xf, segment->point2 ); + draw->DrawSegment( p1, p2, color, draw->context ); + draw->DrawPoint( p2, 4.0f, color, draw->context ); + draw->DrawSegment( p1, b2Lerp( p1, p2, 0.1f ), b2_colorPaleGreen, draw->context ); + } + break; + + default: + break; + } +} + +struct DrawContext +{ + b2World* world; + b2DebugDraw* draw; +}; + +static bool DrawQueryCallback( int proxyId, int shapeId, void* context ) +{ + B2_MAYBE_UNUSED( proxyId ); + + struct DrawContext* drawContext = context; + b2World* world = drawContext->world; + b2DebugDraw* draw = drawContext->draw; + + b2Shape* shape = b2ShapeArray_Get( &world->shapes, shapeId ); + B2_ASSERT( shape->id == shapeId ); + + b2SetBit( &world->debugBodySet, shape->bodyId ); + + if ( draw->drawShapes ) + { + b2Body* body = b2BodyArray_Get( &world->bodies, shape->bodyId ); + b2BodySim* bodySim = b2GetBodySim( world, body ); + + b2HexColor color; + + if ( shape->customColor != 0 ) + { + color = shape->customColor; + } + else if ( body->type == b2_dynamicBody && bodySim->mass == 0.0f ) + { + // Bad body + color = b2_colorRed; + } + else if ( body->setIndex == b2_disabledSet ) + { + color = b2_colorSlateGray; + } + else if ( shape->isSensor ) + { + color = b2_colorWheat; + } + else if ( bodySim->isBullet && body->setIndex == b2_awakeSet ) + { + color = b2_colorTurquoise; + } + else if ( body->isSpeedCapped ) + { + color = b2_colorYellow; + } + else if ( bodySim->isFast ) + { + color = b2_colorSalmon; + } + else if ( body->type == b2_staticBody ) + { + color = b2_colorPaleGreen; + } + else if ( body->type == b2_kinematicBody ) + { + color = b2_colorRoyalBlue; + } + else if ( body->setIndex == b2_awakeSet ) + { + color = b2_colorPink; + } + else + { + color = b2_colorGray; + } + + b2DrawShape( draw, shape, bodySim->transform, color ); + } + + if ( draw->drawAABBs ) + { + b2AABB aabb = shape->fatAABB; + + b2Vec2 vs[4] = { { aabb.lowerBound.x, aabb.lowerBound.y }, + { aabb.upperBound.x, aabb.lowerBound.y }, + { aabb.upperBound.x, aabb.upperBound.y }, + { aabb.lowerBound.x, aabb.upperBound.y } }; + + draw->DrawPolygon( vs, 4, b2_colorGold, draw->context ); + } + + return true; +} + +// todo this has varying order for moving shapes, causing flicker when overlapping shapes are moving +// solution: display order by shape id modulus 3, keep 3 buckets in GLSolid* and flush in 3 passes. +static void b2DrawWithBounds( b2World* world, b2DebugDraw* draw ) +{ + B2_ASSERT( b2AABB_IsValid( draw->drawingBounds ) ); + + const float k_impulseScale = 1.0f; + const float k_axisScale = 0.3f; + b2HexColor speculativeColor = b2_colorGray3; + b2HexColor addColor = b2_colorGreen; + b2HexColor persistColor = b2_colorBlue; + b2HexColor normalColor = b2_colorGray9; + b2HexColor impulseColor = b2_colorMagenta; + b2HexColor frictionColor = b2_colorYellow; + + b2HexColor graphColors[b2_graphColorCount] = { b2_colorRed, b2_colorOrange, b2_colorYellow, b2_colorGreen, + b2_colorCyan, b2_colorBlue, b2_colorViolet, b2_colorPink, + b2_colorChocolate, b2_colorGoldenrod, b2_colorCoral, b2_colorBlack }; + + int bodyCapacity = b2GetIdCapacity( &world->bodyIdPool ); + b2SetBitCountAndClear( &world->debugBodySet, bodyCapacity ); + + int jointCapacity = b2GetIdCapacity( &world->jointIdPool ); + b2SetBitCountAndClear( &world->debugJointSet, jointCapacity ); + + int contactCapacity = b2GetIdCapacity( &world->contactIdPool ); + b2SetBitCountAndClear( &world->debugContactSet, contactCapacity ); + + struct DrawContext drawContext = { world, draw }; + + for ( int i = 0; i < b2_bodyTypeCount; ++i ) + { + b2DynamicTree_Query( world->broadPhase.trees + i, draw->drawingBounds, b2_defaultMaskBits, DrawQueryCallback, + &drawContext ); + } + + uint32_t wordCount = world->debugBodySet.blockCount; + uint64_t* bits = world->debugBodySet.bits; + for ( uint32_t k = 0; k < wordCount; ++k ) + { + uint64_t word = bits[k]; + while ( word != 0 ) + { + uint32_t ctz = b2CTZ64( word ); + uint32_t bodyId = 64 * k + ctz; + + b2Body* body = b2BodyArray_Get( &world->bodies, bodyId ); + + if ( draw->drawMass && body->type == b2_dynamicBody ) + { + b2Vec2 offset = { 0.1f, 0.1f }; + b2BodySim* bodySim = b2GetBodySim( world, body ); + + b2Transform transform = { bodySim->center, bodySim->transform.q }; + draw->DrawTransform( transform, draw->context ); + + b2Vec2 p = b2TransformPoint( transform, offset ); + + char buffer[32]; + snprintf( buffer, 32, " %.2f", bodySim->mass ); + draw->DrawString( p, buffer, draw->context ); + } + + if ( draw->drawJoints ) + { + int jointKey = body->headJointKey; + while ( jointKey != B2_NULL_INDEX ) + { + int jointId = jointKey >> 1; + int edgeIndex = jointKey & 1; + b2Joint* joint = b2JointArray_Get( &world->joints, jointId ); + + // avoid double draw + if ( b2GetBit( &world->debugJointSet, jointId ) == false ) + { + b2DrawJoint( draw, world, joint ); + b2SetBit( &world->debugJointSet, jointId ); + } + else + { + // todo testing + edgeIndex += 0; + } + + jointKey = joint->edges[edgeIndex].nextKey; + } + } + + const float linearSlop = b2_linearSlop; + if ( draw->drawContacts && body->type == b2_dynamicBody && body->setIndex == b2_awakeSet ) + { + int contactKey = body->headContactKey; + while ( contactKey != B2_NULL_INDEX ) + { + int contactId = contactKey >> 1; + int edgeIndex = contactKey & 1; + b2Contact* contact = b2ContactArray_Get(&world->contacts, contactId); + contactKey = contact->edges[edgeIndex].nextKey; + + if ( contact->setIndex != b2_awakeSet || contact->colorIndex == B2_NULL_INDEX ) + { + continue; + } + + // avoid double draw + if ( b2GetBit( &world->debugContactSet, contactId ) == false ) + { + B2_ASSERT( 0 <= contact->colorIndex && contact->colorIndex < b2_graphColorCount ); + + b2GraphColor* gc = world->constraintGraph.colors + contact->colorIndex; + b2ContactSim* contactSim = b2ContactSimArray_Get( &gc->contactSims, contact->localIndex ); + int pointCount = contactSim->manifold.pointCount; + b2Vec2 normal = contactSim->manifold.normal; + char buffer[32]; + + for ( int j = 0; j < pointCount; ++j ) + { + b2ManifoldPoint* point = contactSim->manifold.points + j; + + if ( draw->drawGraphColors ) + { + // graph color + float pointSize = contact->colorIndex == b2_overflowIndex ? 7.5f : 5.0f; + draw->DrawPoint( point->point, pointSize, graphColors[contact->colorIndex], draw->context ); + // g_draw.DrawString(point->position, "%d", point->color); + } + else if ( point->separation > linearSlop ) + { + // Speculative + draw->DrawPoint( point->point, 5.0f, speculativeColor, draw->context ); + } + else if ( point->persisted == false ) + { + // Add + draw->DrawPoint( point->point, 10.0f, addColor, draw->context ); + } + else if ( point->persisted == true ) + { + // Persist + draw->DrawPoint( point->point, 5.0f, persistColor, draw->context ); + } + + if ( draw->drawContactNormals ) + { + b2Vec2 p1 = point->point; + b2Vec2 p2 = b2MulAdd( p1, k_axisScale, normal ); + draw->DrawSegment( p1, p2, normalColor, draw->context ); + } + else if ( draw->drawContactImpulses ) + { + b2Vec2 p1 = point->point; + b2Vec2 p2 = b2MulAdd( p1, k_impulseScale * point->normalImpulse, normal ); + draw->DrawSegment( p1, p2, impulseColor, draw->context ); + snprintf( buffer, B2_ARRAY_COUNT( buffer ), "%.1f", 1000.0f * point->normalImpulse ); + draw->DrawString( p1, buffer, draw->context ); + } + + if ( draw->drawFrictionImpulses ) + { + b2Vec2 tangent = b2RightPerp( normal ); + b2Vec2 p1 = point->point; + b2Vec2 p2 = b2MulAdd( p1, k_impulseScale * point->tangentImpulse, tangent ); + draw->DrawSegment( p1, p2, frictionColor, draw->context ); + snprintf( buffer, B2_ARRAY_COUNT( buffer ), "%.1f", 1000.0f * point->tangentImpulse ); + draw->DrawString( p1, buffer, draw->context ); + } + } + + b2SetBit( &world->debugContactSet, contactId ); + } + else + { + // todo testing + edgeIndex += 0; + } + + contactKey = contact->edges[edgeIndex].nextKey; + } + } + + // Clear the smallest set bit + word = word & ( word - 1 ); + } + } +} + +void b2World_Draw( b2WorldId worldId, b2DebugDraw* draw ) +{ + b2World* world = b2GetWorldFromId( worldId ); + B2_ASSERT( world->locked == false ); + if ( world->locked ) + { + return; + } + + // todo it seems bounds drawing is fast enough for regular usage + if ( draw->useDrawingBounds ) + { + b2DrawWithBounds( world, draw ); + return; + } + + if ( draw->drawShapes ) + { + int setCount = world->solverSets.count; + for ( int setIndex = 0; setIndex < setCount; ++setIndex ) + { + b2SolverSet* set = b2SolverSetArray_Get( &world->solverSets, setIndex ); + int bodyCount = set->bodySims.count; + for ( int bodyIndex = 0; bodyIndex < bodyCount; ++bodyIndex ) + { + b2BodySim* bodySim = set->bodySims.data + bodyIndex; + b2Body* body = b2BodyArray_Get( &world->bodies, bodySim->bodyId ); + B2_ASSERT( body->setIndex == setIndex ); + + b2Transform xf = bodySim->transform; + int shapeId = body->headShapeId; + while ( shapeId != B2_NULL_INDEX ) + { + b2Shape* shape = world->shapes.data + shapeId; + b2HexColor color; + + if ( shape->customColor != 0 ) + { + color = shape->customColor; + } + else if ( body->type == b2_dynamicBody && bodySim->mass == 0.0f ) + { + // Bad body + color = b2_colorRed; + } + else if ( body->setIndex == b2_disabledSet ) + { + color = b2_colorSlateGray; + } + else if ( shape->isSensor ) + { + color = b2_colorWheat; + } + else if ( bodySim->isBullet && body->setIndex == b2_awakeSet ) + { + color = b2_colorTurquoise; + } + else if ( body->isSpeedCapped ) + { + color = b2_colorYellow; + } + else if ( bodySim->isFast ) + { + color = b2_colorSalmon; + } + else if ( body->type == b2_staticBody ) + { + color = b2_colorPaleGreen; + } + else if ( body->type == b2_kinematicBody ) + { + color = b2_colorRoyalBlue; + } + else if ( body->setIndex == b2_awakeSet ) + { + color = b2_colorPink; + } + else + { + color = b2_colorGray; + } + + b2DrawShape( draw, shape, xf, color ); + shapeId = shape->nextShapeId; + } + } + } + } + + if ( draw->drawJoints ) + { + int count = world->joints.count; + for ( int i = 0; i < count; ++i ) + { + b2Joint* joint = world->joints.data + i; + if ( joint->setIndex == B2_NULL_INDEX ) + { + continue; + } + + b2DrawJoint( draw, world, joint ); + } + } + + if ( draw->drawAABBs ) + { + b2HexColor color = b2_colorGold; + + int setCount = world->solverSets.count; + for ( int setIndex = 0; setIndex < setCount; ++setIndex ) + { + b2SolverSet* set = b2SolverSetArray_Get( &world->solverSets, setIndex ); + int bodyCount = set->bodySims.count; + for ( int bodyIndex = 0; bodyIndex < bodyCount; ++bodyIndex ) + { + b2BodySim* bodySim = set->bodySims.data + bodyIndex; + + char buffer[32]; + snprintf( buffer, 32, "%d", bodySim->bodyId ); + draw->DrawString( bodySim->center, buffer, draw->context ); + + b2Body* body = b2BodyArray_Get( &world->bodies, bodySim->bodyId ); + B2_ASSERT( body->setIndex == setIndex ); + + int shapeId = body->headShapeId; + while ( shapeId != B2_NULL_INDEX ) + { + b2Shape* shape = world->shapes.data + shapeId; + b2AABB aabb = shape->fatAABB; + + b2Vec2 vs[4] = { { aabb.lowerBound.x, aabb.lowerBound.y }, + { aabb.upperBound.x, aabb.lowerBound.y }, + { aabb.upperBound.x, aabb.upperBound.y }, + { aabb.lowerBound.x, aabb.upperBound.y } }; + + draw->DrawPolygon( vs, 4, color, draw->context ); + + shapeId = shape->nextShapeId; + } + } + } + } + + if ( draw->drawMass ) + { + b2Vec2 offset = { 0.1f, 0.1f }; + int setCount = world->solverSets.count; + for ( int setIndex = 0; setIndex < setCount; ++setIndex ) + { + b2SolverSet* set = b2SolverSetArray_Get( &world->solverSets, setIndex ); + int bodyCount = set->bodySims.count; + for ( int bodyIndex = 0; bodyIndex < bodyCount; ++bodyIndex ) + { + b2BodySim* bodySim = set->bodySims.data + bodyIndex; + + b2Transform transform = { bodySim->center, bodySim->transform.q }; + draw->DrawTransform( transform, draw->context ); + + b2Vec2 p = b2TransformPoint( transform, offset ); + + char buffer[32]; + snprintf( buffer, 32, " %.2f", bodySim->mass ); + draw->DrawString( p, buffer, draw->context ); + } + } + } + + if ( draw->drawContacts ) + { + const float k_impulseScale = 1.0f; + const float k_axisScale = 0.3f; + const float linearSlop = b2_linearSlop; + + b2HexColor speculativeColor = b2_colorGray3; + b2HexColor addColor = b2_colorGreen; + b2HexColor persistColor = b2_colorBlue; + b2HexColor normalColor = b2_colorGray9; + b2HexColor impulseColor = b2_colorMagenta; + b2HexColor frictionColor = b2_colorYellow; + + b2HexColor colors[b2_graphColorCount] = { b2_colorRed, b2_colorOrange, b2_colorYellow, b2_colorGreen, + b2_colorCyan, b2_colorBlue, b2_colorViolet, b2_colorPink, + b2_colorChocolate, b2_colorGoldenrod, b2_colorCoral, b2_colorBlack }; + + for ( int colorIndex = 0; colorIndex < b2_graphColorCount; ++colorIndex ) + { + b2GraphColor* graphColor = world->constraintGraph.colors + colorIndex; + + int contactCount = graphColor->contactSims.count; + for ( int contactIndex = 0; contactIndex < contactCount; ++contactIndex ) + { + b2ContactSim* contact = graphColor->contactSims.data + contactIndex; + int pointCount = contact->manifold.pointCount; + b2Vec2 normal = contact->manifold.normal; + char buffer[32]; + + for ( int j = 0; j < pointCount; ++j ) + { + b2ManifoldPoint* point = contact->manifold.points + j; + + if ( draw->drawGraphColors && 0 <= colorIndex && colorIndex <= b2_graphColorCount ) + { + // graph color + float pointSize = colorIndex == b2_overflowIndex ? 7.5f : 5.0f; + draw->DrawPoint( point->point, pointSize, colors[colorIndex], draw->context ); + // g_draw.DrawString(point->position, "%d", point->color); + } + else if ( point->separation > linearSlop ) + { + // Speculative + draw->DrawPoint( point->point, 5.0f, speculativeColor, draw->context ); + } + else if ( point->persisted == false ) + { + // Add + draw->DrawPoint( point->point, 10.0f, addColor, draw->context ); + } + else if ( point->persisted == true ) + { + // Persist + draw->DrawPoint( point->point, 5.0f, persistColor, draw->context ); + } + + if ( draw->drawContactNormals ) + { + b2Vec2 p1 = point->point; + b2Vec2 p2 = b2MulAdd( p1, k_axisScale, normal ); + draw->DrawSegment( p1, p2, normalColor, draw->context ); + } + else if ( draw->drawContactImpulses ) + { + b2Vec2 p1 = point->point; + b2Vec2 p2 = b2MulAdd( p1, k_impulseScale * point->normalImpulse, normal ); + draw->DrawSegment( p1, p2, impulseColor, draw->context ); + snprintf( buffer, B2_ARRAY_COUNT( buffer ), "%.2f", 1000.0f * point->normalImpulse ); + draw->DrawString( p1, buffer, draw->context ); + } + + if ( draw->drawFrictionImpulses ) + { + b2Vec2 tangent = b2RightPerp( normal ); + b2Vec2 p1 = point->point; + b2Vec2 p2 = b2MulAdd( p1, k_impulseScale * point->tangentImpulse, tangent ); + draw->DrawSegment( p1, p2, frictionColor, draw->context ); + snprintf( buffer, B2_ARRAY_COUNT( buffer ), "%.2f", point->normalImpulse ); + draw->DrawString( p1, buffer, draw->context ); + } + } + } + } + } +} + +b2BodyEvents b2World_GetBodyEvents( b2WorldId worldId ) +{ + b2World* world = b2GetWorldFromId( worldId ); + B2_ASSERT( world->locked == false ); + if ( world->locked ) + { + return ( b2BodyEvents ){ 0 }; + } + + int count = world->bodyMoveEvents.count; + b2BodyEvents events = { world->bodyMoveEvents.data, count }; + return events; +} + +b2SensorEvents b2World_GetSensorEvents( b2WorldId worldId ) +{ + b2World* world = b2GetWorldFromId( worldId ); + B2_ASSERT( world->locked == false ); + if ( world->locked ) + { + return ( b2SensorEvents ){ 0 }; + } + + int beginCount = world->sensorBeginEvents.count; + int endCount = world->sensorEndEvents.count; + + b2SensorEvents events = { world->sensorBeginEvents.data, world->sensorEndEvents.data, beginCount, endCount }; + return events; +} + +b2ContactEvents b2World_GetContactEvents( b2WorldId worldId ) +{ + b2World* world = b2GetWorldFromId( worldId ); + B2_ASSERT( world->locked == false ); + if ( world->locked ) + { + return ( b2ContactEvents ){ 0 }; + } + + int beginCount = world->contactBeginEvents.count; + int endCount = world->contactEndEvents.count; + int hitCount = world->contactHitEvents.count; + + b2ContactEvents events = { + world->contactBeginEvents.data, world->contactEndEvents.data, world->contactHitEvents.data, beginCount, endCount, hitCount, + }; + + return events; +} + +bool b2World_IsValid( b2WorldId id ) +{ + if ( id.index1 < 1 || b2_maxWorlds < id.index1 ) + { + return false; + } + + b2World* world = b2_worlds + ( id.index1 - 1 ); + + if ( world->worldId != id.index1 - 1 ) + { + // world is not allocated + return false; + } + + return id.revision == world->revision; +} + +bool b2Body_IsValid( b2BodyId id ) +{ + if ( id.world0 < 0 || b2_maxWorlds <= id.world0 ) + { + // invalid world + return false; + } + + b2World* world = b2_worlds + id.world0; + if ( world->worldId != id.world0 ) + { + // world is free + return false; + } + + if ( id.index1 < 1 || world->bodies.count < id.index1 ) + { + // invalid index + return false; + } + + b2Body* body = world->bodies.data + ( id.index1 - 1 ); + if ( body->setIndex == B2_NULL_INDEX ) + { + // this was freed + return false; + } + + B2_ASSERT( body->localIndex != B2_NULL_INDEX ); + + if ( body->revision != id.revision ) + { + // this id is orphaned + return false; + } + + return true; +} + +bool b2Shape_IsValid( b2ShapeId id ) +{ + if ( b2_maxWorlds <= id.world0 ) + { + return false; + } + + b2World* world = b2_worlds + id.world0; + if ( world->worldId != id.world0 ) + { + // world is free + return false; + } + + int shapeId = id.index1 - 1; + if ( shapeId < 0 || world->shapes.count <= shapeId ) + { + return false; + } + + b2Shape* shape = world->shapes.data + shapeId; + if ( shape->id == B2_NULL_INDEX ) + { + // shape is free + return false; + } + + B2_ASSERT( shape->id == shapeId ); + + return id.revision == shape->revision; +} + +bool b2Chain_IsValid( b2ChainId id ) +{ + if ( id.world0 < 0 || b2_maxWorlds <= id.world0 ) + { + return false; + } + + b2World* world = b2_worlds + id.world0; + if ( world->worldId != id.world0 ) + { + // world is free + return false; + } + + int chainId = id.index1 - 1; + if ( chainId < 0 || world->chainShapes.count <= chainId ) + { + return false; + } + + b2ChainShape* chain = world->chainShapes.data + chainId; + if ( chain->id == B2_NULL_INDEX ) + { + // chain is free + return false; + } + + B2_ASSERT( chain->id == chainId ); + + return id.revision == chain->revision; +} + +bool b2Joint_IsValid( b2JointId id ) +{ + if ( id.world0 < 0 || b2_maxWorlds <= id.world0 ) + { + return false; + } + + b2World* world = b2_worlds + id.world0; + if ( world->worldId != id.world0 ) + { + // world is free + return false; + } + + int jointId = id.index1 - 1; + if ( jointId < 0 || world->joints.count <= jointId ) + { + return false; + } + + b2Joint* joint = world->joints.data + jointId; + if ( joint->jointId == B2_NULL_INDEX ) + { + // joint is free + return false; + } + + B2_ASSERT( joint->jointId == jointId ); + + return id.revision == joint->revision; +} + +void b2World_EnableSleeping( b2WorldId worldId, bool flag ) +{ + b2World* world = b2GetWorldFromId( worldId ); + B2_ASSERT( world->locked == false ); + if ( world->locked ) + { + return; + } + + if ( flag == world->enableSleep ) + { + return; + } + + world->enableSleep = flag; + + if ( flag == false ) + { + int setCount = world->solverSets.count; + for ( int i = b2_firstSleepingSet; i < setCount; ++i ) + { + b2SolverSet* set = b2SolverSetArray_Get( &world->solverSets, i ); + if ( set->bodySims.count > 0 ) + { + b2WakeSolverSet( world, i ); + } + } + } +} + +bool b2World_IsSleepingEnabled( b2WorldId worldId ) +{ + b2World* world = b2GetWorldFromId( worldId ); + return world->enableSleep; +} + +void b2World_EnableWarmStarting( b2WorldId worldId, bool flag ) +{ + b2World* world = b2GetWorldFromId( worldId ); + B2_ASSERT( world->locked == false ); + if ( world->locked ) + { + return; + } + + world->enableWarmStarting = flag; +} + +bool b2World_IsWarmStartingEnabled( b2WorldId worldId ) +{ + b2World* world = b2GetWorldFromId( worldId ); + return world->enableWarmStarting; +} + +void b2World_EnableContinuous( b2WorldId worldId, bool flag ) +{ + b2World* world = b2GetWorldFromId( worldId ); + B2_ASSERT( world->locked == false ); + if ( world->locked ) + { + return; + } + + world->enableContinuous = flag; +} + +bool b2World_IsContinuousEnabled( b2WorldId worldId ) +{ + b2World* world = b2GetWorldFromId( worldId ); + return world->enableContinuous; +} + +void b2World_SetRestitutionThreshold( b2WorldId worldId, float value ) +{ + b2World* world = b2GetWorldFromId( worldId ); + B2_ASSERT( world->locked == false ); + if ( world->locked ) + { + return; + } + + world->restitutionThreshold = b2ClampFloat( value, 0.0f, FLT_MAX ); +} + +float b2World_GetRestitutionThreshold( b2WorldId worldId ) +{ + b2World* world = b2GetWorldFromId( worldId ); + return world->restitutionThreshold; +} + +void b2World_SetHitEventThreshold( b2WorldId worldId, float value ) +{ + b2World* world = b2GetWorldFromId( worldId ); + B2_ASSERT( world->locked == false ); + if ( world->locked ) + { + return; + } + + world->hitEventThreshold = b2ClampFloat( value, 0.0f, FLT_MAX ); +} + +float b2World_GetHitEventThreshold( b2WorldId worldId ) +{ + b2World* world = b2GetWorldFromId( worldId ); + return world->hitEventThreshold; +} + +void b2World_SetContactTuning( b2WorldId worldId, float hertz, float dampingRatio, float pushOut ) +{ + b2World* world = b2GetWorldFromId( worldId ); + B2_ASSERT( world->locked == false ); + if ( world->locked ) + { + return; + } + + world->contactHertz = b2ClampFloat( hertz, 0.0f, FLT_MAX ); + world->contactDampingRatio = b2ClampFloat( dampingRatio, 0.0f, FLT_MAX ); + world->contactPushoutVelocity = b2ClampFloat( pushOut, 0.0f, FLT_MAX ); +} + +b2Profile b2World_GetProfile( b2WorldId worldId ) +{ + b2World* world = b2GetWorldFromId( worldId ); + return world->profile; +} + +b2Counters b2World_GetCounters( b2WorldId worldId ) +{ + b2World* world = b2GetWorldFromId( worldId ); + b2Counters s = { 0 }; + s.bodyCount = b2GetIdCount( &world->bodyIdPool ); + s.shapeCount = b2GetIdCount( &world->shapeIdPool ); + s.contactCount = b2GetIdCount( &world->contactIdPool ); + s.jointCount = b2GetIdCount( &world->jointIdPool ); + s.islandCount = b2GetIdCount( &world->islandIdPool ); + + b2DynamicTree* staticTree = world->broadPhase.trees + b2_staticBody; + s.staticTreeHeight = b2DynamicTree_GetHeight( staticTree ); + + b2DynamicTree* dynamicTree = world->broadPhase.trees + b2_dynamicBody; + b2DynamicTree* kinematicTree = world->broadPhase.trees + b2_kinematicBody; + s.treeHeight = b2MaxInt( b2DynamicTree_GetHeight( dynamicTree ), b2DynamicTree_GetHeight( kinematicTree ) ); + + s.stackUsed = b2GetMaxStackAllocation( &world->stackAllocator ); + s.byteCount = b2GetByteCount(); + s.taskCount = world->taskCount; + + for ( int i = 0; i < b2_graphColorCount; ++i ) + { + s.colorCounts[i] = world->constraintGraph.colors[i].contactSims.count + world->constraintGraph.colors[i].jointSims.count; + } + return s; +} + +void b2World_DumpMemoryStats( b2WorldId worldId ) +{ + FILE* file = fopen( "box2d_memory.txt", "w" ); + if ( file == NULL ) + { + return; + } + + b2World* world = b2GetWorldFromId( worldId ); + + // id pools + fprintf( file, "id pools\n" ); + fprintf( file, "body ids: %d\n", b2GetIdBytes( &world->bodyIdPool ) ); + fprintf( file, "solver set ids: %d\n", b2GetIdBytes( &world->solverSetIdPool ) ); + fprintf( file, "joint ids: %d\n", b2GetIdBytes( &world->jointIdPool ) ); + fprintf( file, "contact ids: %d\n", b2GetIdBytes( &world->contactIdPool ) ); + fprintf( file, "island ids: %d\n", b2GetIdBytes( &world->islandIdPool ) ); + fprintf( file, "shape ids: %d\n", b2GetIdBytes( &world->shapeIdPool ) ); + fprintf( file, "chain ids: %d\n", b2GetIdBytes( &world->chainIdPool ) ); + fprintf( file, "\n" ); + + // world arrays + fprintf( file, "world arrays\n" ); + fprintf( file, "bodies: %d\n", b2BodyArray_ByteCount( &world->bodies ) ); + fprintf( file, "solver sets: %d\n", b2SolverSetArray_ByteCount( &world->solverSets ) ); + fprintf( file, "joints: %d\n", b2JointArray_ByteCount( &world->joints ) ); + fprintf( file, "contacts: %d\n", b2ContactArray_ByteCount( &world->contacts ) ); + fprintf( file, "islands: %d\n", b2IslandArray_ByteCount( &world->islands ) ); + fprintf( file, "shapes: %d\n", b2ShapeArray_ByteCount( &world->shapes ) ); + fprintf( file, "chains: %d\n", b2ChainShapeArray_ByteCount( &world->chainShapes ) ); + fprintf( file, "\n" ); + + // broad-phase + fprintf( file, "broad-phase\n" ); + fprintf( file, "static tree: %d\n", b2DynamicTree_GetByteCount( world->broadPhase.trees + b2_staticBody ) ); + fprintf( file, "kinematic tree: %d\n", b2DynamicTree_GetByteCount( world->broadPhase.trees + b2_kinematicBody ) ); + fprintf( file, "dynamic tree: %d\n", b2DynamicTree_GetByteCount( world->broadPhase.trees + b2_dynamicBody ) ); + b2HashSet* moveSet = &world->broadPhase.moveSet; + fprintf( file, "moveSet: %d (%d, %d)\n", b2GetHashSetBytes( moveSet ), moveSet->count, moveSet->capacity ); + fprintf( file, "moveArray: %d\n", b2IntArray_ByteCount( &world->broadPhase.moveArray ) ); + b2HashSet* pairSet = &world->broadPhase.pairSet; + fprintf( file, "pairSet: %d (%d, %d)\n", b2GetHashSetBytes( pairSet ), pairSet->count, pairSet->capacity ); + fprintf( file, "\n" ); + + // solver sets + int bodySimCapacity = 0; + int bodyStateCapacity = 0; + int jointSimCapacity = 0; + int contactSimCapacity = 0; + int islandSimCapacity = 0; + int solverSetCapacity = world->solverSets.count; + for ( int i = 0; i < solverSetCapacity; ++i ) + { + b2SolverSet* set = world->solverSets.data + i; + if ( set->setIndex == B2_NULL_INDEX ) + { + continue; + } + + bodySimCapacity += set->bodySims.capacity; + bodyStateCapacity += set->bodyStates.capacity; + jointSimCapacity += set->jointSims.capacity; + contactSimCapacity += set->contactSims.capacity; + islandSimCapacity += set->islandSims.capacity; + } + + fprintf( file, "solver sets\n" ); + fprintf( file, "body sim: %d\n", bodySimCapacity * (int)sizeof( b2BodySim ) ); + fprintf( file, "body state: %d\n", bodyStateCapacity * (int)sizeof( b2BodyState ) ); + fprintf( file, "joint sim: %d\n", jointSimCapacity * (int)sizeof( b2JointSim ) ); + fprintf( file, "contact sim: %d\n", contactSimCapacity * (int)sizeof( b2ContactSim ) ); + fprintf( file, "island sim: %d\n", islandSimCapacity * (int)sizeof( islandSimCapacity ) ); + fprintf( file, "\n" ); + + // constraint graph + int bodyBitSetBytes = 0; + contactSimCapacity = 0; + jointSimCapacity = 0; + for ( int i = 0; i < b2_graphColorCount; ++i ) + { + b2GraphColor* c = world->constraintGraph.colors + i; + bodyBitSetBytes += b2GetBitSetBytes( &c->bodySet ); + contactSimCapacity += c->contactSims.capacity; + jointSimCapacity += c->jointSims.capacity; + } + + fprintf( file, "constraint graph\n" ); + fprintf( file, "body bit sets: %d\n", bodyBitSetBytes ); + fprintf( file, "joint sim: %d\n", jointSimCapacity * (int)sizeof( b2JointSim ) ); + fprintf( file, "contact sim: %d\n", contactSimCapacity * (int)sizeof( b2ContactSim ) ); + fprintf( file, "\n" ); + + // stack allocator + fprintf( file, "stack allocator: %d\n\n", world->stackAllocator.capacity ); + + // chain shapes + // todo + + fclose( file ); +} + +typedef struct WorldQueryContext +{ + b2World* world; + b2OverlapResultFcn* fcn; + b2QueryFilter filter; + void* userContext; +} WorldQueryContext; + +static bool TreeQueryCallback( int proxyId, int shapeId, void* context ) +{ + B2_MAYBE_UNUSED( proxyId ); + + WorldQueryContext* worldContext = context; + b2World* world = worldContext->world; + + b2Shape* shape = b2ShapeArray_Get( &world->shapes, shapeId ); + + b2Filter shapeFilter = shape->filter; + b2QueryFilter queryFilter = worldContext->filter; + + if ( ( shapeFilter.categoryBits & queryFilter.maskBits ) == 0 || ( shapeFilter.maskBits & queryFilter.categoryBits ) == 0 ) + { + return true; + } + + b2ShapeId id = { shapeId + 1, world->worldId, shape->revision }; + bool result = worldContext->fcn( id, worldContext->userContext ); + return result; +} + +void b2World_OverlapAABB( b2WorldId worldId, b2AABB aabb, b2QueryFilter filter, b2OverlapResultFcn* fcn, void* context ) +{ + b2World* world = b2GetWorldFromId( worldId ); + B2_ASSERT( world->locked == false ); + if ( world->locked ) + { + return; + } + + B2_ASSERT( b2AABB_IsValid( aabb ) ); + + WorldQueryContext worldContext = { world, fcn, filter, context }; + + for ( int i = 0; i < b2_bodyTypeCount; ++i ) + { + b2DynamicTree_Query( world->broadPhase.trees + i, aabb, filter.maskBits, TreeQueryCallback, &worldContext ); + } +} + +typedef struct WorldOverlapContext +{ + b2World* world; + b2OverlapResultFcn* fcn; + b2QueryFilter filter; + b2DistanceProxy proxy; + b2Transform transform; + void* userContext; +} WorldOverlapContext; + +static bool TreeOverlapCallback( int proxyId, int shapeId, void* context ) +{ + B2_MAYBE_UNUSED( proxyId ); + + WorldOverlapContext* worldContext = context; + b2World* world = worldContext->world; + + b2Shape* shape = b2ShapeArray_Get( &world->shapes, shapeId ); + + b2Filter shapeFilter = shape->filter; + b2QueryFilter queryFilter = worldContext->filter; + + if ( ( shapeFilter.categoryBits & queryFilter.maskBits ) == 0 || ( shapeFilter.maskBits & queryFilter.categoryBits ) == 0 ) + { + return true; + } + + b2Body* body = b2BodyArray_Get( &world->bodies, shape->bodyId ); + b2Transform transform = b2GetBodyTransformQuick( world, body ); + + b2DistanceInput input; + input.proxyA = worldContext->proxy; + input.proxyB = b2MakeShapeDistanceProxy( shape ); + input.transformA = worldContext->transform; + input.transformB = transform; + input.useRadii = true; + + b2DistanceCache cache = { 0 }; + b2DistanceOutput output = b2ShapeDistance( &cache, &input, NULL, 0 ); + + if ( output.distance > 0.0f ) + { + return true; + } + + b2ShapeId id = { shape->id + 1, world->worldId, shape->revision }; + bool result = worldContext->fcn( id, worldContext->userContext ); + return result; +} + +void b2World_OverlapCircle( b2WorldId worldId, const b2Circle* circle, b2Transform transform, b2QueryFilter filter, + b2OverlapResultFcn* fcn, void* context ) +{ + b2World* world = b2GetWorldFromId( worldId ); + B2_ASSERT( world->locked == false ); + if ( world->locked ) + { + return; + } + + B2_ASSERT( b2Vec2_IsValid( transform.p ) ); + B2_ASSERT( b2Rot_IsValid( transform.q ) ); + + b2AABB aabb = b2ComputeCircleAABB( circle, transform ); + WorldOverlapContext worldContext = { + world, fcn, filter, b2MakeProxy( &circle->center, 1, circle->radius ), transform, context, + }; + + for ( int i = 0; i < b2_bodyTypeCount; ++i ) + { + b2DynamicTree_Query( world->broadPhase.trees + i, aabb, filter.maskBits, TreeOverlapCallback, &worldContext ); + } +} + +void b2World_OverlapCapsule( b2WorldId worldId, const b2Capsule* capsule, b2Transform transform, b2QueryFilter filter, + b2OverlapResultFcn* fcn, void* context ) +{ + b2World* world = b2GetWorldFromId( worldId ); + B2_ASSERT( world->locked == false ); + if ( world->locked ) + { + return; + } + + B2_ASSERT( b2Vec2_IsValid( transform.p ) ); + B2_ASSERT( b2Rot_IsValid( transform.q ) ); + + b2AABB aabb = b2ComputeCapsuleAABB( capsule, transform ); + WorldOverlapContext worldContext = { + world, fcn, filter, b2MakeProxy( &capsule->center1, 2, capsule->radius ), transform, context, + }; + + for ( int i = 0; i < b2_bodyTypeCount; ++i ) + { + b2DynamicTree_Query( world->broadPhase.trees + i, aabb, filter.maskBits, TreeOverlapCallback, &worldContext ); + } +} + +void b2World_OverlapPolygon( b2WorldId worldId, const b2Polygon* polygon, b2Transform transform, b2QueryFilter filter, + b2OverlapResultFcn* fcn, void* context ) +{ + b2World* world = b2GetWorldFromId( worldId ); + B2_ASSERT( world->locked == false ); + if ( world->locked ) + { + return; + } + + B2_ASSERT( b2Vec2_IsValid( transform.p ) ); + B2_ASSERT( b2Rot_IsValid( transform.q ) ); + + b2AABB aabb = b2ComputePolygonAABB( polygon, transform ); + WorldOverlapContext worldContext = { + world, fcn, filter, b2MakeProxy( polygon->vertices, polygon->count, polygon->radius ), transform, context, + }; + + for ( int i = 0; i < b2_bodyTypeCount; ++i ) + { + b2DynamicTree_Query( world->broadPhase.trees + i, aabb, filter.maskBits, TreeOverlapCallback, &worldContext ); + } +} + +typedef struct WorldRayCastContext +{ + b2World* world; + b2CastResultFcn* fcn; + b2QueryFilter filter; + float fraction; + void* userContext; +} WorldRayCastContext; + +static float RayCastCallback( const b2RayCastInput* input, int proxyId, int shapeId, void* context ) +{ + B2_MAYBE_UNUSED( proxyId ); + + WorldRayCastContext* worldContext = context; + b2World* world = worldContext->world; + + b2Shape* shape = b2ShapeArray_Get( &world->shapes, shapeId ); + b2Filter shapeFilter = shape->filter; + b2QueryFilter queryFilter = worldContext->filter; + + if ( ( shapeFilter.categoryBits & queryFilter.maskBits ) == 0 || ( shapeFilter.maskBits & queryFilter.categoryBits ) == 0 ) + { + return input->maxFraction; + } + + b2Body* body = b2BodyArray_Get( &world->bodies, shape->bodyId ); + b2Transform transform = b2GetBodyTransformQuick( world, body ); + b2CastOutput output = b2RayCastShape( input, shape, transform ); + + if ( output.hit ) + { + b2ShapeId id = { shapeId + 1, world->worldId, shape->revision }; + float fraction = worldContext->fcn( id, output.point, output.normal, output.fraction, worldContext->userContext ); + worldContext->fraction = fraction; + return fraction; + } + + return input->maxFraction; +} + +void b2World_CastRay( b2WorldId worldId, b2Vec2 origin, b2Vec2 translation, b2QueryFilter filter, b2CastResultFcn* fcn, + void* context ) +{ + b2World* world = b2GetWorldFromId( worldId ); + B2_ASSERT( world->locked == false ); + if ( world->locked ) + { + return; + } + + B2_ASSERT( b2Vec2_IsValid( origin ) ); + B2_ASSERT( b2Vec2_IsValid( translation ) ); + + b2RayCastInput input = { origin, translation, 1.0f }; + + WorldRayCastContext worldContext = { world, fcn, filter, 1.0f, context }; + + for ( int i = 0; i < b2_bodyTypeCount; ++i ) + { + b2DynamicTree_RayCast( world->broadPhase.trees + i, &input, filter.maskBits, RayCastCallback, &worldContext ); + + if ( worldContext.fraction == 0.0f ) + { + return; + } + + input.maxFraction = worldContext.fraction; + } +} + +// This callback finds the closest hit. This is the most common callback used in games. +static float b2RayCastClosestFcn( b2ShapeId shapeId, b2Vec2 point, b2Vec2 normal, float fraction, void* context ) +{ + b2RayResult* rayResult = (b2RayResult*)context; + rayResult->shapeId = shapeId; + rayResult->point = point; + rayResult->normal = normal; + rayResult->fraction = fraction; + rayResult->hit = true; + return fraction; +} + +b2RayResult b2World_CastRayClosest( b2WorldId worldId, b2Vec2 origin, b2Vec2 translation, b2QueryFilter filter ) +{ + b2RayResult result = { 0 }; + + b2World* world = b2GetWorldFromId( worldId ); + B2_ASSERT( world->locked == false ); + if ( world->locked ) + { + return result; + } + + B2_ASSERT( b2Vec2_IsValid( origin ) ); + B2_ASSERT( b2Vec2_IsValid( translation ) ); + + b2RayCastInput input = { origin, translation, 1.0f }; + WorldRayCastContext worldContext = { world, b2RayCastClosestFcn, filter, 1.0f, &result }; + + for ( int i = 0; i < b2_bodyTypeCount; ++i ) + { + b2DynamicTree_RayCast( world->broadPhase.trees + i, &input, filter.maskBits, RayCastCallback, &worldContext ); + + if ( worldContext.fraction == 0.0f ) + { + return result; + } + + input.maxFraction = worldContext.fraction; + } + + return result; +} + +static float ShapeCastCallback( const b2ShapeCastInput* input, int proxyId, int shapeId, void* context ) +{ + B2_MAYBE_UNUSED( proxyId ); + + WorldRayCastContext* worldContext = context; + b2World* world = worldContext->world; + + b2Shape* shape = b2ShapeArray_Get( &world->shapes, shapeId ); + b2Filter shapeFilter = shape->filter; + b2QueryFilter queryFilter = worldContext->filter; + + if ( ( shapeFilter.categoryBits & queryFilter.maskBits ) == 0 || ( shapeFilter.maskBits & queryFilter.categoryBits ) == 0 ) + { + return input->maxFraction; + } + + b2Body* body = b2BodyArray_Get( &world->bodies, shape->bodyId ); + b2Transform transform = b2GetBodyTransformQuick( world, body ); + b2CastOutput output = b2ShapeCastShape( input, shape, transform ); + + if ( output.hit ) + { + b2ShapeId id = { shapeId + 1, world->worldId, shape->revision }; + float fraction = worldContext->fcn( id, output.point, output.normal, output.fraction, worldContext->userContext ); + worldContext->fraction = fraction; + return fraction; + } + + return input->maxFraction; +} + +void b2World_CastCircle( b2WorldId worldId, const b2Circle* circle, b2Transform originTransform, b2Vec2 translation, + b2QueryFilter filter, b2CastResultFcn* fcn, void* context ) +{ + b2World* world = b2GetWorldFromId( worldId ); + B2_ASSERT( world->locked == false ); + if ( world->locked ) + { + return; + } + + B2_ASSERT( b2Vec2_IsValid( originTransform.p ) ); + B2_ASSERT( b2Rot_IsValid( originTransform.q ) ); + B2_ASSERT( b2Vec2_IsValid( translation ) ); + + b2ShapeCastInput input; + input.points[0] = b2TransformPoint( originTransform, circle->center ); + input.count = 1; + input.radius = circle->radius; + input.translation = translation; + input.maxFraction = 1.0f; + + WorldRayCastContext worldContext = { world, fcn, filter, 1.0f, context }; + + for ( int i = 0; i < b2_bodyTypeCount; ++i ) + { + b2DynamicTree_ShapeCast( world->broadPhase.trees + i, &input, filter.maskBits, ShapeCastCallback, &worldContext ); + + if ( worldContext.fraction == 0.0f ) + { + return; + } + + input.maxFraction = worldContext.fraction; + } +} + +void b2World_CastCapsule( b2WorldId worldId, const b2Capsule* capsule, b2Transform originTransform, b2Vec2 translation, + b2QueryFilter filter, b2CastResultFcn* fcn, void* context ) +{ + b2World* world = b2GetWorldFromId( worldId ); + B2_ASSERT( world->locked == false ); + if ( world->locked ) + { + return; + } + + B2_ASSERT( b2Vec2_IsValid( originTransform.p ) ); + B2_ASSERT( b2Rot_IsValid( originTransform.q ) ); + B2_ASSERT( b2Vec2_IsValid( translation ) ); + + b2ShapeCastInput input; + input.points[0] = b2TransformPoint( originTransform, capsule->center1 ); + input.points[1] = b2TransformPoint( originTransform, capsule->center2 ); + input.count = 2; + input.radius = capsule->radius; + input.translation = translation; + input.maxFraction = 1.0f; + + WorldRayCastContext worldContext = { world, fcn, filter, 1.0f, context }; + + for ( int i = 0; i < b2_bodyTypeCount; ++i ) + { + b2DynamicTree_ShapeCast( world->broadPhase.trees + i, &input, filter.maskBits, ShapeCastCallback, &worldContext ); + + if ( worldContext.fraction == 0.0f ) + { + return; + } + + input.maxFraction = worldContext.fraction; + } +} + +void b2World_CastPolygon( b2WorldId worldId, const b2Polygon* polygon, b2Transform originTransform, b2Vec2 translation, + b2QueryFilter filter, b2CastResultFcn* fcn, void* context ) +{ + b2World* world = b2GetWorldFromId( worldId ); + B2_ASSERT( world->locked == false ); + if ( world->locked ) + { + return; + } + + B2_ASSERT( b2Vec2_IsValid( originTransform.p ) ); + B2_ASSERT( b2Rot_IsValid( originTransform.q ) ); + B2_ASSERT( b2Vec2_IsValid( translation ) ); + + b2ShapeCastInput input; + for ( int i = 0; i < polygon->count; ++i ) + { + input.points[i] = b2TransformPoint( originTransform, polygon->vertices[i] ); + } + input.count = polygon->count; + input.radius = polygon->radius; + input.translation = translation; + input.maxFraction = 1.0f; + + WorldRayCastContext worldContext = { world, fcn, filter, 1.0f, context }; + + for ( int i = 0; i < b2_bodyTypeCount; ++i ) + { + b2DynamicTree_ShapeCast( world->broadPhase.trees + i, &input, filter.maskBits, ShapeCastCallback, &worldContext ); + + if ( worldContext.fraction == 0.0f ) + { + return; + } + + input.maxFraction = worldContext.fraction; + } +} + +#if 0 + +void b2World_ShiftOrigin(b2WorldId worldId, b2Vec2 newOrigin) +{ + B2_ASSERT(m_locked == false); + if (m_locked) + { + return; + } + + for (b2Body* b = m_bodyList; b; b = b->m_next) + { + b->m_xf.p -= newOrigin; + b->m_sweep.c0 -= newOrigin; + b->m_sweep.c -= newOrigin; + } + + for (b2Joint* j = m_jointList; j; j = j->m_next) + { + j->ShiftOrigin(newOrigin); + } + + m_contactManager.m_broadPhase.ShiftOrigin(newOrigin); +} + +void b2World_Dump() +{ + if (m_locked) + { + return; + } + + b2OpenDump("box2d_dump.inl"); + + b2Dump("b2Vec2 g(%.9g, %.9g);\n", m_gravity.x, m_gravity.y); + b2Dump("m_world->SetGravity(g);\n"); + + b2Dump("b2Body** sims = (b2Body**)b2Alloc(%d * sizeof(b2Body*));\n", m_bodyCount); + b2Dump("b2Joint** joints = (b2Joint**)b2Alloc(%d * sizeof(b2Joint*));\n", m_jointCount); + + int32 i = 0; + for (b2Body* b = m_bodyList; b; b = b->m_next) + { + b->m_islandIndex = i; + b->Dump(); + ++i; + } + + i = 0; + for (b2Joint* j = m_jointList; j; j = j->m_next) + { + j->m_index = i; + ++i; + } + + // First pass on joints, skip gear joints. + for (b2Joint* j = m_jointList; j; j = j->m_next) + { + if (j->m_type == e_gearJoint) + { + continue; + } + + b2Dump("{\n"); + j->Dump(); + b2Dump("}\n"); + } + + // Second pass on joints, only gear joints. + for (b2Joint* j = m_jointList; j; j = j->m_next) + { + if (j->m_type != e_gearJoint) + { + continue; + } + + b2Dump("{\n"); + j->Dump(); + b2Dump("}\n"); + } + + b2Dump("b2Free(joints);\n"); + b2Dump("b2Free(sims);\n"); + b2Dump("joints = nullptr;\n"); + b2Dump("sims = nullptr;\n"); + + b2CloseDump(); +} +#endif + +void b2World_SetCustomFilterCallback( b2WorldId worldId, b2CustomFilterFcn* fcn, void* context ) +{ + b2World* world = b2GetWorldFromId( worldId ); + world->customFilterFcn = fcn; + world->customFilterContext = context; +} + +void b2World_SetPreSolveCallback( b2WorldId worldId, b2PreSolveFcn* fcn, void* context ) +{ + b2World* world = b2GetWorldFromId( worldId ); + world->preSolveFcn = fcn; + world->preSolveContext = context; +} + +void b2World_SetGravity( b2WorldId worldId, b2Vec2 gravity ) +{ + b2World* world = b2GetWorldFromId( worldId ); + world->gravity = gravity; +} + +b2Vec2 b2World_GetGravity( b2WorldId worldId ) +{ + b2World* world = b2GetWorldFromId( worldId ); + return world->gravity; +} + +struct ExplosionContext +{ + b2World* world; + b2Vec2 position; + float radius; + float magnitude; +}; + +static bool ExplosionCallback( int proxyId, int shapeId, void* context ) +{ + B2_MAYBE_UNUSED( proxyId ); + + struct ExplosionContext* explosionContext = context; + b2World* world = explosionContext->world; + + b2Shape* shape = b2ShapeArray_Get( &world->shapes, shapeId ); + + b2Body* body = b2BodyArray_Get( &world->bodies, shape->bodyId ); + if ( body->type == b2_kinematicBody ) + { + return true; + } + + b2WakeBody( world, body ); + + if ( body->setIndex != b2_awakeSet ) + { + return true; + } + + b2Transform transform = b2GetBodyTransformQuick( world, body ); + + b2DistanceInput input; + input.proxyA = b2MakeShapeDistanceProxy( shape ); + input.proxyB = b2MakeProxy( &explosionContext->position, 1, 0.0f ); + input.transformA = transform; + input.transformB = b2Transform_identity; + input.useRadii = true; + + b2DistanceCache cache = { 0 }; + b2DistanceOutput output = b2ShapeDistance( &cache, &input, NULL, 0 ); + + if ( output.distance > explosionContext->radius ) + { + return true; + } + + b2Vec2 closestPoint = output.pointA; + + if ( output.distance == 0.0f ) + { + b2Vec2 localCentroid = b2GetShapeCentroid( shape ); + closestPoint = b2TransformPoint( transform, localCentroid ); + } + + float falloff = 0.4f; + float perimeter = b2GetShapePerimeter( shape ); + float magnitude = explosionContext->magnitude * perimeter * ( 1.0f - falloff * output.distance / explosionContext->radius ); + + b2Vec2 direction = b2Normalize( b2Sub( closestPoint, explosionContext->position ) ); + b2Vec2 impulse = b2MulSV( magnitude, direction ); + + int localIndex = body->localIndex; + b2SolverSet* set = b2SolverSetArray_Get( &world->solverSets, b2_awakeSet ); + b2BodyState* state = b2BodyStateArray_Get( &set->bodyStates, localIndex ); + b2BodySim* bodySim = b2BodySimArray_Get( &set->bodySims, localIndex ); + state->linearVelocity = b2MulAdd( state->linearVelocity, bodySim->invMass, impulse ); + state->angularVelocity += bodySim->invInertia * b2Cross( b2Sub( closestPoint, bodySim->center ), impulse ); + + return true; +} + +void b2World_Explode( b2WorldId worldId, b2Vec2 position, float radius, float magnitude ) +{ + B2_ASSERT( b2Vec2_IsValid( position ) ); + B2_ASSERT( b2IsValid( radius ) && radius > 0.0f ); + B2_ASSERT( b2IsValid( magnitude ) ); + + b2World* world = b2GetWorldFromId( worldId ); + B2_ASSERT( world->locked == false ); + if ( world->locked ) + { + return; + } + + struct ExplosionContext explosionContext = { world, position, radius, magnitude }; + + b2AABB aabb; + aabb.lowerBound.x = position.x - radius; + aabb.lowerBound.y = position.y - radius; + aabb.upperBound.x = position.x + radius; + aabb.upperBound.y = position.y + radius; + + b2DynamicTree_Query( world->broadPhase.trees + b2_dynamicBody, aabb, b2_defaultMaskBits, ExplosionCallback, + &explosionContext ); +} + +#if B2_VALIDATE +// When validating islands ids I have to compare the root island +// ids because islands are not merged until the next time step. +static int b2GetRootIslandId( b2World* world, int islandId ) +{ + if ( islandId == B2_NULL_INDEX ) + { + return B2_NULL_INDEX; + } + + b2Island* island = b2IslandArray_Get( &world->islands, islandId ); + + int rootId = islandId; + b2Island* rootIsland = island; + while ( rootIsland->parentIsland != B2_NULL_INDEX ) + { + b2Island* parent = b2IslandArray_Get( &world->islands, rootIsland->parentIsland ); + rootId = rootIsland->parentIsland; + rootIsland = parent; + } + + return rootId; +} + +// This validates island graph connectivity for each body +void b2ValidateConnectivity( b2World* world ) +{ + b2Body* bodies = world->bodies.data; + int bodyCapacity = world->bodies.count; + + for ( int bodyIndex = 0; bodyIndex < bodyCapacity; ++bodyIndex ) + { + b2Body* body = bodies + bodyIndex; + if ( body->id == B2_NULL_INDEX ) + { + b2ValidateFreeId( &world->bodyIdPool, bodyIndex ); + continue; + } + + B2_ASSERT( bodyIndex == body->id ); + + // Need to get the root island because islands are not merged until the next time step + int bodyIslandId = b2GetRootIslandId( world, body->islandId ); + int bodySetIndex = body->setIndex; + + int contactKey = body->headContactKey; + while ( contactKey != B2_NULL_INDEX ) + { + int contactId = contactKey >> 1; + int edgeIndex = contactKey & 1; + + b2Contact* contact = b2ContactArray_Get(&world->contacts, contactId); + + bool touching = ( contact->flags & b2_contactTouchingFlag ) != 0; + if ( touching && ( contact->flags & b2_contactSensorFlag ) == 0 ) + { + if ( bodySetIndex != b2_staticSet ) + { + int contactIslandId = b2GetRootIslandId( world, contact->islandId ); + B2_ASSERT( contactIslandId == bodyIslandId ); + } + } + else + { + B2_ASSERT( contact->islandId == B2_NULL_INDEX ); + } + + contactKey = contact->edges[edgeIndex].nextKey; + } + + int jointKey = body->headJointKey; + while ( jointKey != B2_NULL_INDEX ) + { + int jointId = jointKey >> 1; + int edgeIndex = jointKey & 1; + + b2Joint* joint = b2JointArray_Get( &world->joints, jointId ); + + int otherEdgeIndex = edgeIndex ^ 1; + + b2Body* otherBody = b2BodyArray_Get( &world->bodies, joint->edges[otherEdgeIndex].bodyId ); + + if ( bodySetIndex == b2_disabledSet || otherBody->setIndex == b2_disabledSet ) + { + B2_ASSERT( joint->islandId == B2_NULL_INDEX ); + } + else if ( bodySetIndex == b2_staticSet ) + { + if ( otherBody->setIndex == b2_staticSet ) + { + B2_ASSERT( joint->islandId == B2_NULL_INDEX ); + } + } + else + { + int jointIslandId = b2GetRootIslandId( world, joint->islandId ); + B2_ASSERT( jointIslandId == bodyIslandId ); + } + + jointKey = joint->edges[edgeIndex].nextKey; + } + } +} + +// Validates solver sets, but not island connectivity +void b2ValidateSolverSets( b2World* world ) +{ + B2_ASSERT( b2GetIdCapacity( &world->bodyIdPool ) == world->bodies.count ); + B2_ASSERT( b2GetIdCapacity( &world->contactIdPool ) == world->contacts.count ); + B2_ASSERT( b2GetIdCapacity( &world->jointIdPool ) == world->joints.count ); + B2_ASSERT( b2GetIdCapacity( &world->islandIdPool ) == world->islands.count ); + B2_ASSERT( b2GetIdCapacity( &world->solverSetIdPool ) == world->solverSets.count ); + + int activeSetCount = 0; + int totalBodyCount = 0; + int totalJointCount = 0; + int totalContactCount = 0; + int totalIslandCount = 0; + + // Validate all solver sets + int setCount = world->solverSets.count; + for ( int setIndex = 0; setIndex < setCount; ++setIndex ) + { + b2SolverSet* set = world->solverSets.data + setIndex; + if ( set->setIndex != B2_NULL_INDEX ) + { + activeSetCount += 1; + + if ( setIndex == b2_staticSet ) + { + B2_ASSERT( set->contactSims.count == 0 ); + B2_ASSERT( set->islandSims.count == 0 ); + B2_ASSERT( set->bodyStates.count == 0 ); + } + else if ( setIndex == b2_awakeSet ) + { + B2_ASSERT( set->bodySims.count == set->bodyStates.count ); + B2_ASSERT( set->jointSims.count == 0 ); + } + else if ( setIndex == b2_disabledSet ) + { + B2_ASSERT( set->islandSims.count == 0 ); + B2_ASSERT( set->bodyStates.count == 0 ); + } + else + { + B2_ASSERT( set->bodyStates.count == 0 ); + } + + // Validate bodies + { + b2Body* bodies = world->bodies.data; + B2_ASSERT( set->bodySims.count >= 0 ); + totalBodyCount += set->bodySims.count; + for ( int i = 0; i < set->bodySims.count; ++i ) + { + b2BodySim* bodySim = set->bodySims.data + i; + + int bodyId = bodySim->bodyId; + B2_ASSERT( 0 <= bodyId && bodyId < world->bodies.count ); + b2Body* body = bodies + bodyId; + B2_ASSERT( body->setIndex == setIndex ); + B2_ASSERT( body->localIndex == i ); + B2_ASSERT( body->revision == body->revision ); + + if ( setIndex == b2_disabledSet ) + { + B2_ASSERT( body->headContactKey == B2_NULL_INDEX ); + } + + // Validate body shapes + int prevShapeId = B2_NULL_INDEX; + int shapeId = body->headShapeId; + while ( shapeId != B2_NULL_INDEX ) + { + b2Shape* shape = b2ShapeArray_Get( &world->shapes, shapeId ); + B2_ASSERT( shape->id == shapeId ); + B2_ASSERT( shape->prevShapeId == prevShapeId ); + + if ( setIndex == b2_disabledSet ) + { + B2_ASSERT( shape->proxyKey == B2_NULL_INDEX ); + } + else if ( setIndex == b2_staticSet ) + { + B2_ASSERT( B2_PROXY_TYPE( shape->proxyKey ) == b2_staticBody ); + } + else + { + b2BodyType proxyType = B2_PROXY_TYPE( shape->proxyKey ); + B2_ASSERT( proxyType == b2_kinematicBody || proxyType == b2_dynamicBody ); + } + + prevShapeId = shapeId; + shapeId = shape->nextShapeId; + } + + // Validate body contacts + int contactKey = body->headContactKey; + while ( contactKey != B2_NULL_INDEX ) + { + int contactId = contactKey >> 1; + int edgeIndex = contactKey & 1; + + b2Contact* contact = b2ContactArray_Get(&world->contacts, contactId); + B2_ASSERT( contact->setIndex != b2_staticSet ); + B2_ASSERT( contact->edges[0].bodyId == bodyId || contact->edges[1].bodyId == bodyId ); + contactKey = contact->edges[edgeIndex].nextKey; + } + + // Validate body joints + int jointKey = body->headJointKey; + while ( jointKey != B2_NULL_INDEX ) + { + int jointId = jointKey >> 1; + int edgeIndex = jointKey & 1; + + b2Joint* joint = b2JointArray_Get( &world->joints, jointId ); + + int otherEdgeIndex = edgeIndex ^ 1; + + b2Body* otherBody = b2BodyArray_Get( &world->bodies, joint->edges[otherEdgeIndex].bodyId ); + + if ( setIndex == b2_disabledSet || otherBody->setIndex == b2_disabledSet ) + { + B2_ASSERT( joint->setIndex == b2_disabledSet ); + } + else if ( setIndex == b2_staticSet && otherBody->setIndex == b2_staticSet ) + { + B2_ASSERT( joint->setIndex == b2_staticSet ); + } + else if ( setIndex == b2_awakeSet ) + { + B2_ASSERT( joint->setIndex == b2_awakeSet ); + } + else if ( setIndex >= b2_firstSleepingSet ) + { + B2_ASSERT( joint->setIndex == setIndex ); + } + + b2JointSim* jointSim = b2GetJointSim( world, joint ); + B2_ASSERT( jointSim->jointId == jointId ); + B2_ASSERT( jointSim->bodyIdA == joint->edges[0].bodyId ); + B2_ASSERT( jointSim->bodyIdB == joint->edges[1].bodyId ); + + jointKey = joint->edges[edgeIndex].nextKey; + } + } + } + + // Validate contacts + { + B2_ASSERT( set->contactSims.count >= 0 ); + totalContactCount += set->contactSims.count; + for ( int i = 0; i < set->contactSims.count; ++i ) + { + b2ContactSim* contactSim = set->contactSims.data + i; + b2Contact* contact = b2ContactArray_Get(&world->contacts, contactSim->contactId); + if ( setIndex == b2_awakeSet ) + { + // contact should be non-touching if awake + // or it could be this contact hasn't been transferred yet + B2_ASSERT( contactSim->manifold.pointCount == 0 || + ( contactSim->simFlags & b2_simStartedTouching ) != 0 ); + } + B2_ASSERT( contact->setIndex == setIndex ); + B2_ASSERT( contact->colorIndex == B2_NULL_INDEX ); + B2_ASSERT( contact->localIndex == i ); + } + } + + // Validate joints + { + B2_ASSERT( set->jointSims.count >= 0 ); + totalJointCount += set->jointSims.count; + for ( int i = 0; i < set->jointSims.count; ++i ) + { + b2JointSim* jointSim = set->jointSims.data + i; + b2Joint* joint = b2JointArray_Get( &world->joints, jointSim->jointId ); + B2_ASSERT( joint->setIndex == setIndex ); + B2_ASSERT( joint->colorIndex == B2_NULL_INDEX ); + B2_ASSERT( joint->localIndex == i ); + } + } + + // Validate islands + { + B2_ASSERT( set->islandSims.count >= 0 ); + totalIslandCount += set->islandSims.count; + for ( int i = 0; i < set->islandSims.count; ++i ) + { + b2IslandSim* islandSim = set->islandSims.data + i; + b2Island* island = b2IslandArray_Get( &world->islands, islandSim->islandId ); + B2_ASSERT( island->setIndex == setIndex ); + B2_ASSERT( island->localIndex == i ); + } + } + } + else + { + B2_ASSERT( set->bodySims.count == 0 ); + B2_ASSERT( set->contactSims.count == 0 ); + B2_ASSERT( set->jointSims.count == 0 ); + B2_ASSERT( set->islandSims.count == 0 ); + B2_ASSERT( set->bodyStates.count == 0 ); + } + } + + int setIdCount = b2GetIdCount( &world->solverSetIdPool ); + B2_ASSERT( activeSetCount == setIdCount ); + + int bodyIdCount = b2GetIdCount( &world->bodyIdPool ); + B2_ASSERT( totalBodyCount == bodyIdCount ); + + int islandIdCount = b2GetIdCount( &world->islandIdPool ); + B2_ASSERT( totalIslandCount == islandIdCount ); + + // Validate constraint graph + for ( int colorIndex = 0; colorIndex < b2_graphColorCount; ++colorIndex ) + { + b2GraphColor* color = world->constraintGraph.colors + colorIndex; + { + B2_ASSERT( color->contactSims.count >= 0 ); + totalContactCount += color->contactSims.count; + for ( int i = 0; i < color->contactSims.count; ++i ) + { + b2ContactSim* contactSim = color->contactSims.data + i; + b2Contact* contact = b2ContactArray_Get(&world->contacts, contactSim->contactId); + // contact should be touching in the constraint graph or awaiting transfer to non-touching + B2_ASSERT( contactSim->manifold.pointCount > 0 || + ( contactSim->simFlags & ( b2_simStoppedTouching | b2_simDisjoint ) ) != 0 ); + B2_ASSERT( contact->setIndex == b2_awakeSet ); + B2_ASSERT( contact->colorIndex == colorIndex ); + B2_ASSERT( contact->localIndex == i ); + + int bodyIdA = contact->edges[0].bodyId; + int bodyIdB = contact->edges[1].bodyId; + + if ( colorIndex < b2_overflowIndex ) + { + b2Body* bodyA = b2BodyArray_Get( &world->bodies, bodyIdA ); + b2Body* bodyB = b2BodyArray_Get( &world->bodies, bodyIdB ); + B2_ASSERT( b2GetBit( &color->bodySet, bodyIdA ) == ( bodyA->type != b2_staticBody ) ); + B2_ASSERT( b2GetBit( &color->bodySet, bodyIdB ) == ( bodyB->type != b2_staticBody ) ); + } + } + } + + { + B2_ASSERT( color->jointSims.count >= 0 ); + totalJointCount += color->jointSims.count; + for ( int i = 0; i < color->jointSims.count; ++i ) + { + b2JointSim* jointSim = color->jointSims.data + i; + b2Joint* joint = b2JointArray_Get( &world->joints, jointSim->jointId ); + B2_ASSERT( joint->setIndex == b2_awakeSet ); + B2_ASSERT( joint->colorIndex == colorIndex ); + B2_ASSERT( joint->localIndex == i ); + + int bodyIdA = joint->edges[0].bodyId; + int bodyIdB = joint->edges[1].bodyId; + + if ( colorIndex < b2_overflowIndex ) + { + b2Body* bodyA = b2BodyArray_Get( &world->bodies, bodyIdA ); + b2Body* bodyB = b2BodyArray_Get( &world->bodies, bodyIdB ); + B2_ASSERT( b2GetBit( &color->bodySet, bodyIdA ) == ( bodyA->type != b2_staticBody ) ); + B2_ASSERT( b2GetBit( &color->bodySet, bodyIdB ) == ( bodyB->type != b2_staticBody ) ); + } + } + } + } + + int contactIdCount = b2GetIdCount( &world->contactIdPool ); + B2_ASSERT( totalContactCount == contactIdCount ); + B2_ASSERT( totalContactCount == world->broadPhase.pairSet.count ); + + int jointIdCount = b2GetIdCount( &world->jointIdPool ); + B2_ASSERT( totalJointCount == jointIdCount ); + +// Validate shapes +// This is very slow on compounds +#if 0 + int shapeCapacity = b2Array(world->shapeArray).count; + for (int shapeIndex = 0; shapeIndex < shapeCapacity; shapeIndex += 1) + { + b2Shape* shape = world->shapeArray + shapeIndex; + if (shape->id != shapeIndex) + { + continue; + } + + B2_ASSERT(0 <= shape->bodyId && shape->bodyId < b2Array(world->bodyArray).count); + + b2Body* body = world->bodyArray + shape->bodyId; + B2_ASSERT(0 <= body->setIndex && body->setIndex < b2Array(world->solverSetArray).count); + + b2SolverSet* set = world->solverSetArray + body->setIndex; + B2_ASSERT(0 <= body->localIndex && body->localIndex < set->sims.count); + + b2BodySim* bodySim = set->sims.data + body->localIndex; + B2_ASSERT(bodySim->bodyId == shape->bodyId); + + bool found = false; + int shapeCount = 0; + int index = body->headShapeId; + while (index != B2_NULL_INDEX) + { + b2CheckId(world->shapeArray, index); + b2Shape* s = world->shapeArray + index; + if (index == shapeIndex) + { + found = true; + } + + index = s->nextShapeId; + shapeCount += 1; + } + + B2_ASSERT(found); + B2_ASSERT(shapeCount == body->shapeCount); + } +#endif +} + +// Validate contact touching status. +void b2ValidateContacts( b2World* world ) +{ + int contactCount = world->contacts.count; + B2_ASSERT( contactCount == b2GetIdCapacity( &world->contactIdPool ) ); + int allocatedContactCount = 0; + + for ( int contactIndex = 0; contactIndex < contactCount; ++contactIndex ) + { + b2Contact* contact = b2ContactArray_Get(&world->contacts, contactIndex); + if ( contact->contactId == B2_NULL_INDEX ) + { + continue; + } + + B2_ASSERT( contact->contactId == contactIndex ); + + allocatedContactCount += 1; + + bool touching = ( contact->flags & b2_contactTouchingFlag ) != 0; + bool sensorTouching = ( contact->flags & b2_contactSensorTouchingFlag ) != 0; + bool isSensor = ( contact->flags & b2_contactSensorFlag ) != 0; + + B2_ASSERT( touching == false || sensorTouching == false ); + B2_ASSERT( touching == false || isSensor == false ); + + int setId = contact->setIndex; + + if ( setId == b2_awakeSet ) + { + // If touching and not a sensor + if ( touching && isSensor == false ) + { + B2_ASSERT( 0 <= contact->colorIndex && contact->colorIndex < b2_graphColorCount ); + } + else + { + B2_ASSERT( contact->colorIndex == B2_NULL_INDEX ); + } + } + else if ( setId >= b2_firstSleepingSet ) + { + // Only touching contacts allowed in a sleeping set + B2_ASSERT( touching == true && isSensor == false ); + } + else + { + // Sleeping and non-touching contacts or sensor contacts belong in the disabled set + B2_ASSERT( touching == false && setId == b2_disabledSet ); + } + + b2ContactSim* contactSim = b2GetContactSim( world, contact ); + B2_ASSERT( contactSim->contactId == contactIndex ); + B2_ASSERT( contactSim->bodyIdA == contact->edges[0].bodyId ); + B2_ASSERT( contactSim->bodyIdB == contact->edges[1].bodyId ); + + // Sim touching is true for solid and sensor contacts + bool simTouching = ( contactSim->simFlags & b2_simTouchingFlag ) != 0; + B2_ASSERT( touching == simTouching || sensorTouching == simTouching ); + + B2_ASSERT( 0 <= contactSim->manifold.pointCount && contactSim->manifold.pointCount <= 2 ); + } + + int contactIdCount = b2GetIdCount( &world->contactIdPool ); + B2_ASSERT( allocatedContactCount == contactIdCount ); +} + +#else + +void b2ValidateConnectivity( b2World* world ) +{ + B2_MAYBE_UNUSED( world ); +} + +void b2ValidateSolverSets( b2World* world ) +{ + B2_MAYBE_UNUSED( world ); +} + +void b2ValidateContacts( b2World* world ) +{ + B2_MAYBE_UNUSED( world ); +} + +#endif diff --git a/3rdparty/box2d/src/world.h b/3rdparty/box2d/src/world.h new file mode 100644 index 000000000000..8bba9ef5293a --- /dev/null +++ b/3rdparty/box2d/src/world.h @@ -0,0 +1,180 @@ +// SPDX-FileCopyrightText: 2023 Erin Catto +// SPDX-License-Identifier: MIT + +#pragma once + +#include "array.h" +#include "bitset.h" +#include "broad_phase.h" +#include "constraint_graph.h" +#include "id_pool.h" +#include "stack_allocator.h" + +#include "box2d/types.h" + +typedef struct b2ContactSim b2ContactSim; + +enum b2SetType +{ + b2_staticSet = 0, + b2_disabledSet = 1, + b2_awakeSet = 2, + b2_firstSleepingSet = 3, +}; + +// Per thread task storage +typedef struct b2TaskContext +{ + // These bits align with the b2ConstraintGraph::contactBlocks and signal a change in contact status + b2BitSet contactStateBitSet; + + // Used to track bodies with shapes that have enlarged AABBs. This avoids having a bit array + // that is very large when there are many static shapes. + b2BitSet enlargedSimBitSet; + + // Used to put islands to sleep + b2BitSet awakeIslandBitSet; + + // Per worker split island candidate + float splitSleepTime; + int splitIslandId; + +} b2TaskContext; + +/// The world class manages all physics entities, dynamic simulation, +/// and asynchronous queries. The world also contains efficient memory +/// management facilities. +typedef struct b2World +{ + b2StackAllocator stackAllocator; + b2BroadPhase broadPhase; + b2ConstraintGraph constraintGraph; + + // The body id pool is used to allocate and recycle body ids. Body ids + // provide a stable identifier for users, but incur caches misses when used + // to access body data. Aligns with b2Body. + b2IdPool bodyIdPool; + + // This is a sparse array that maps body ids to the body data + // stored in solver sets. As sims move within a set or across set. + // Indices come from id pool. + b2BodyArray bodies; + + // Provides free list for solver sets. + b2IdPool solverSetIdPool; + + // Solvers sets allow sims to be stored in contiguous arrays. The first + // set is all static sims. The second set is active sims. The third set is disabled + // sims. The remaining sets are sleeping islands. + b2SolverSetArray solverSets; + + // Used to create stable ids for joints + b2IdPool jointIdPool; + + // This is a sparse array that maps joint ids to the joint data stored in the constraint graph + // or in the solver sets. + b2JointArray joints; + + // Used to create stable ids for contacts + b2IdPool contactIdPool; + + // This is a sparse array that maps contact ids to the contact data stored in the constraint graph + // or in the solver sets. + b2ContactArray contacts; + + // Used to create stable ids for islands + b2IdPool islandIdPool; + + // This is a sparse array that maps island ids to the island data stored in the solver sets. + b2IslandArray islands; + + b2IdPool shapeIdPool; + b2IdPool chainIdPool; + + // These are sparse arrays that point into the pools above + b2ShapeArray shapes; + b2ChainShapeArray chainShapes; + + // Per thread storage + b2TaskContextArray taskContexts; + + b2BodyMoveEventArray bodyMoveEvents; + b2SensorBeginTouchEventArray sensorBeginEvents; + b2SensorEndTouchEventArray sensorEndEvents; + b2ContactBeginTouchEventArray contactBeginEvents; + b2ContactEndTouchEventArray contactEndEvents; + b2ContactHitEventArray contactHitEvents; + + // Used to track debug draw + b2BitSet debugBodySet; + b2BitSet debugJointSet; + b2BitSet debugContactSet; + + // Id that is incremented every time step + uint64_t stepIndex; + + // Identify islands for splitting as follows: + // - I want to split islands so smaller islands can sleep + // - when a body comes to rest and its sleep timer trips, I can look at the island and flag it for splitting + // if it has removed constraints + // - islands that have removed constraints must be put split first because I don't want to wake bodies incorrectly + // - otherwise I can use the awake islands that have bodies wanting to sleep as the splitting candidates + // - if no bodies want to sleep then there is no reason to perform island splitting + int splitIslandId; + + b2Vec2 gravity; + float hitEventThreshold; + float restitutionThreshold; + float maxLinearVelocity; + float contactPushoutVelocity; + float contactHertz; + float contactDampingRatio; + float jointHertz; + float jointDampingRatio; + + uint16_t revision; + + b2Profile profile; + + b2PreSolveFcn* preSolveFcn; + void* preSolveContext; + + b2CustomFilterFcn* customFilterFcn; + void* customFilterContext; + + int workerCount; + b2EnqueueTaskCallback* enqueueTaskFcn; + b2FinishTaskCallback* finishTaskFcn; + void* userTaskContext; + void* userTreeTask; + + // Remember type step used for reporting forces and torques + float inv_h; + + int activeTaskCount; + int taskCount; + + uint16_t worldId; + + bool enableSleep; + bool locked; + bool enableWarmStarting; + bool enableContinuous; + bool inUse; +} b2World; + +b2World* b2GetWorldFromId( b2WorldId id ); +b2World* b2GetWorld( int index ); +b2World* b2GetWorldLocked( int index ); + +void b2ValidateConnectivity( b2World* world ); +void b2ValidateSolverSets( b2World* world ); +void b2ValidateContacts( b2World* world ); + +B2_ARRAY_INLINE( b2BodyMoveEvent, b2BodyMoveEvent ); +B2_ARRAY_INLINE( b2ContactBeginTouchEvent, b2ContactBeginTouchEvent ); +B2_ARRAY_INLINE( b2ContactEndTouchEvent, b2ContactEndTouchEvent ); +B2_ARRAY_INLINE( b2ContactHitEvent, b2ContactHitEvent ); +B2_ARRAY_INLINE( b2SensorBeginTouchEvent, b2SensorBeginTouchEvent ); +B2_ARRAY_INLINE( b2SensorEndTouchEvent, b2SensorEndTouchEvent ); +B2_ARRAY_INLINE( b2TaskContext, b2TaskContext ); diff --git a/3rdparty/chipmunk/CMakeLists.txt b/3rdparty/chipmunk/CMakeLists.txt deleted file mode 100644 index 39cef51c1305..000000000000 --- a/3rdparty/chipmunk/CMakeLists.txt +++ /dev/null @@ -1,72 +0,0 @@ -cmake_minimum_required(VERSION 3.10) - -project(chipmunk) - -# to change the prefix, run cmake with the parameter: -# -D CMAKE_INSTALL_PREFIX=/my/prefix - -# to change the build type, run cmake with the parameter: -# -D CMAKE_BUILD_TYPE= -# run "cmake --help-variable CMAKE_BUILD_TYPE" for details -if(NOT CMAKE_BUILD_TYPE) - SET(CMAKE_BUILD_TYPE Release CACHE STRING - "Choose the type of build, options are: None Debug Release RelWithDebInfo MinSizeRel." - FORCE) -endif() - -# to manually select install locations of libraries and executables -# -D LIB_INSTALL_DIR mylib -# -D BIN_INSTALL_DIR newbin -set(LIB_INSTALL_DIR lib CACHE STRING "Install location of libraries") -set(BIN_INSTALL_DIR bin CACHE STRING "Install location of executables") - -# other options for the build, you can i.e. activate the shared library by passing -# -D CP_BUILD_SHARED=ON -# to cmake. Other options analog -if(ANDROID) - option(CP_BUILD_DEMOS "Build the demo applications" OFF) - option(CP_INSTALL_DEMOS "Install the demo applications" OFF) - option(CP_BUILD_SHARED "Build and install the shared library" ON) - option(CP_BUILD_STATIC "Build as static library" ON) - option(CP_INSTALL_STATIC "Install the static library" OFF) -else() - option(CP_BUILD_DEMOS "Build the demo applications" ON) - option(CP_INSTALL_DEMOS "Install the demo applications" OFF) - option(CP_BUILD_SHARED "Build and install the shared library" ON) - option(CP_BUILD_STATIC "Build as static library" ON) - option(CP_INSTALL_STATIC "Install the static library" ON) -endif() - -option(CP_USE_DOUBLES ON) -if(NOT CP_USE_DOUBLES) - add_definitions(-DCP_USE_DOUBLES=0) -endif() - -if(CMAKE_C_COMPILER_ID STREQUAL "Clang") - option(FORCE_CLANG_BLOCKS "Force enable Clang blocks" YES) -endif() - -# sanity checks... -if(CP_INSTALL_DEMOS) - set(CP_BUILD_DEMOS ON FORCE) -endif() - -# these need the static lib too -if(CP_BUILD_DEMOS OR CP_INSTALL_STATIC) - set(CP_BUILD_STATIC ON FORCE) -endif() - -if(NOT MSVC) - set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -std=gnu99") # always use gnu99 - if(FORCE_CLANG_BLOCKS) - set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -fblocks") - endif() - set(CMAKE_C_FLAGS_RELEASE "${CMAKE_C_FLAGS_RELEASE} -ffast-math") # extend release-profile with fast-math - set(CMAKE_C_FLAGS_DEBUG "${CMAKE_C_FLAGS_DEBUG} -Wall") # extend debug-profile with -Wall -endif() - -add_subdirectory(src) - -if(CP_BUILD_DEMOS) - add_subdirectory(demo) -endif() diff --git a/3rdparty/chipmunk/include/chipmunk/chipmunk.h b/3rdparty/chipmunk/include/chipmunk/chipmunk.h deleted file mode 100644 index 68359d3fa4ca..000000000000 --- a/3rdparty/chipmunk/include/chipmunk/chipmunk.h +++ /dev/null @@ -1,235 +0,0 @@ -/* Copyright (c) 2013 Scott Lembcke and Howling Moon Software - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -#ifndef CHIPMUNK_H -#define CHIPMUNK_H - -#include -#include - -#ifndef alloca - #ifdef _WIN32 - #include - #elif defined(__FreeBSD__) - /* already included in */ - #else - #include - #endif -#endif - -#ifdef _WIN32 - #define CP_EXPORT __declspec(dllexport) -#else - #define CP_EXPORT -#endif - -#ifdef __cplusplus -extern "C" { -#endif - -CP_EXPORT void cpMessage(const char *condition, const char *file, int line, int isError, int isHardError, const char *message, ...); -#ifdef NDEBUG - #define cpAssertWarn(__condition__, ...) - #define cpAssertSoft(__condition__, ...) -#else - #define cpAssertSoft(__condition__, ...) if(!(__condition__)){cpMessage(#__condition__, __FILE__, __LINE__, 1, 0, __VA_ARGS__); abort();} - #define cpAssertWarn(__condition__, ...) if(!(__condition__)) cpMessage(#__condition__, __FILE__, __LINE__, 0, 0, __VA_ARGS__) -#endif - -// Hard assertions are used in situations where the program definitely will crash anyway, and the reason is inexpensive to detect. -#define cpAssertHard(__condition__, ...) if(!(__condition__)){cpMessage(#__condition__, __FILE__, __LINE__, 1, 1, __VA_ARGS__); abort();} - -#include "chipmunk_types.h" - -/// @defgroup misc Misc -/// @{ - -/// Allocated size for various Chipmunk buffers -#ifndef CP_BUFFER_BYTES - #define CP_BUFFER_BYTES (32*1024) -#endif - -#ifndef cpcalloc - /// Chipmunk calloc() alias. - #define cpcalloc calloc -#endif - -#ifndef cprealloc - /// Chipmunk realloc() alias. - #define cprealloc realloc -#endif - -#ifndef cpfree - /// Chipmunk free() alias. - #define cpfree free -#endif - -typedef struct cpArray cpArray; -typedef struct cpHashSet cpHashSet; - -typedef struct cpBody cpBody; - -typedef struct cpShape cpShape; -typedef struct cpCircleShape cpCircleShape; -typedef struct cpSegmentShape cpSegmentShape; -typedef struct cpPolyShape cpPolyShape; - -typedef struct cpConstraint cpConstraint; -typedef struct cpPinJoint cpPinJoint; -typedef struct cpSlideJoint cpSlideJoint; -typedef struct cpPivotJoint cpPivotJoint; -typedef struct cpGrooveJoint cpGrooveJoint; -typedef struct cpDampedSpring cpDampedSpring; -typedef struct cpDampedRotarySpring cpDampedRotarySpring; -typedef struct cpRotaryLimitJoint cpRotaryLimitJoint; -typedef struct cpRatchetJoint cpRatchetJoint; -typedef struct cpGearJoint cpGearJoint; -typedef struct cpSimpleMotorJoint cpSimpleMotorJoint; - -typedef struct cpCollisionHandler cpCollisionHandler; -typedef struct cpContactPointSet cpContactPointSet; -typedef struct cpArbiter cpArbiter; - -typedef struct cpSpace cpSpace; - -#include "cpVect.h" -#include "cpBB.h" -#include "cpTransform.h" -#include "cpSpatialIndex.h" - -#include "cpArbiter.h" - -#include "cpBody.h" -#include "cpShape.h" -#include "cpPolyShape.h" - -#include "cpConstraint.h" - -#include "cpSpace.h" - -// patch me: axis link android required -#include "cpHastySpace.h" - -// Chipmunk 7.0.3 -#define CP_VERSION_MAJOR 7 -#define CP_VERSION_MINOR 0 -#define CP_VERSION_RELEASE 3 - -/// Version string. -CP_EXPORT extern const char *cpVersionString; - -/// Calculate the moment of inertia for a circle. -/// @c r1 and @c r2 are the inner and outer diameters. A solid circle has an inner diameter of 0. -CP_EXPORT cpFloat cpMomentForCircle(cpFloat m, cpFloat r1, cpFloat r2, cpVect offset); - -/// Calculate area of a hollow circle. -/// @c r1 and @c r2 are the inner and outer diameters. A solid circle has an inner diameter of 0. -CP_EXPORT cpFloat cpAreaForCircle(cpFloat r1, cpFloat r2); - -/// Calculate the moment of inertia for a line segment. -/// Beveling radius is not supported. -CP_EXPORT cpFloat cpMomentForSegment(cpFloat m, cpVect a, cpVect b, cpFloat radius); - -/// Calculate the area of a fattened (capsule shaped) line segment. -CP_EXPORT cpFloat cpAreaForSegment(cpVect a, cpVect b, cpFloat radius); - -/// Calculate the moment of inertia for a solid polygon shape assuming it's center of gravity is at it's centroid. The offset is added to each vertex. -CP_EXPORT cpFloat cpMomentForPoly(cpFloat m, int count, const cpVect *verts, cpVect offset, cpFloat radius); - -/// Calculate the signed area of a polygon. A Clockwise winding gives positive area. -/// This is probably backwards from what you expect, but matches Chipmunk's the winding for poly shapes. -CP_EXPORT cpFloat cpAreaForPoly(const int count, const cpVect *verts, cpFloat radius); - -/// Calculate the natural centroid of a polygon. -CP_EXPORT cpVect cpCentroidForPoly(const int count, const cpVect *verts); - -/// Calculate the moment of inertia for a solid box. -CP_EXPORT cpFloat cpMomentForBox(cpFloat m, cpFloat width, cpFloat height); - -/// Calculate the moment of inertia for a solid box. -CP_EXPORT cpFloat cpMomentForBox2(cpFloat m, cpBB box); - -/// Calculate the convex hull of a given set of points. Returns the count of points in the hull. -/// @c result must be a pointer to a @c cpVect array with at least @c count elements. If @c verts == @c result, then @c verts will be reduced inplace. -/// @c first is an optional pointer to an integer to store where the first vertex in the hull came from (i.e. verts[first] == result[0]) -/// @c tol is the allowed amount to shrink the hull when simplifying it. A tolerance of 0.0 creates an exact hull. -CP_EXPORT int cpConvexHull(int count, const cpVect *verts, cpVect *result, int *first, cpFloat tol); - -/// Convenience macro to work with cpConvexHull. -/// @c count and @c verts is the input array passed to cpConvexHull(). -/// @c count_var and @c verts_var are the names of the variables the macro creates to store the result. -/// The output vertex array is allocated on the stack using alloca() so it will be freed automatically, but cannot be returned from the current scope. -#define CP_CONVEX_HULL(__count__, __verts__, __count_var__, __verts_var__) \ -cpVect *__verts_var__ = (cpVect *)alloca(__count__*sizeof(cpVect)); \ -int __count_var__ = cpConvexHull(__count__, __verts__, __verts_var__, NULL, 0.0); \ - -/// Returns the closest point on the line segment ab, to the point p. -static inline cpVect -cpClosetPointOnSegment(const cpVect p, const cpVect a, const cpVect b) -{ - cpVect delta = cpvsub(a, b); - cpFloat t = cpfclamp01(cpvdot(delta, cpvsub(p, b))/cpvlengthsq(delta)); - return cpvadd(b, cpvmult(delta, t)); -} - -#if defined(__has_extension) -#if __has_extension(blocks) -// Define alternate block based alternatives for a few of the callback heavy functions. -// Collision handlers are post-step callbacks are not included to avoid memory management issues. -// If you want to use blocks for those and are aware of how to correctly manage the memory, the implementation is trivial. - -void cpSpaceEachBody_b(cpSpace *space, void (^block)(cpBody *body)); -void cpSpaceEachShape_b(cpSpace *space, void (^block)(cpShape *shape)); -void cpSpaceEachConstraint_b(cpSpace *space, void (^block)(cpConstraint *constraint)); - -void cpBodyEachShape_b(cpBody *body, void (^block)(cpShape *shape)); -void cpBodyEachConstraint_b(cpBody *body, void (^block)(cpConstraint *constraint)); -void cpBodyEachArbiter_b(cpBody *body, void (^block)(cpArbiter *arbiter)); - -typedef void (^cpSpacePointQueryBlock)(cpShape *shape, cpVect point, cpFloat distance, cpVect gradient); -void cpSpacePointQuery_b(cpSpace *space, cpVect point, cpFloat maxDistance, cpShapeFilter filter, cpSpacePointQueryBlock block); - -typedef void (^cpSpaceSegmentQueryBlock)(cpShape *shape, cpVect point, cpVect normal, cpFloat alpha); -void cpSpaceSegmentQuery_b(cpSpace *space, cpVect start, cpVect end, cpFloat radius, cpShapeFilter filter, cpSpaceSegmentQueryBlock block); - -typedef void (^cpSpaceBBQueryBlock)(cpShape *shape); -void cpSpaceBBQuery_b(cpSpace *space, cpBB bb, cpShapeFilter filter, cpSpaceBBQueryBlock block); - -typedef void (^cpSpaceShapeQueryBlock)(cpShape *shape, cpContactPointSet *points); -cpBool cpSpaceShapeQuery_b(cpSpace *space, cpShape *shape, cpSpaceShapeQueryBlock block); - -#endif -#endif - - -//@} - -#ifdef __cplusplus -} - -static inline cpVect operator *(const cpVect v, const cpFloat s){return cpvmult(v, s);} -static inline cpVect operator +(const cpVect v1, const cpVect v2){return cpvadd(v1, v2);} -static inline cpVect operator -(const cpVect v1, const cpVect v2){return cpvsub(v1, v2);} -static inline cpBool operator ==(const cpVect v1, const cpVect v2){return cpveql(v1, v2);} -static inline cpVect operator -(const cpVect v){return cpvneg(v);} - -#endif -#endif diff --git a/3rdparty/chipmunk/include/chipmunk/chipmunk_ffi.h b/3rdparty/chipmunk/include/chipmunk/chipmunk_ffi.h deleted file mode 100644 index 86e3d9fcd6fd..000000000000 --- a/3rdparty/chipmunk/include/chipmunk/chipmunk_ffi.h +++ /dev/null @@ -1,105 +0,0 @@ -/* Copyright (c) 2013 Scott Lembcke and Howling Moon Software - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -#ifdef CHIPMUNK_FFI - -// Create non static inlined copies of Chipmunk functions, useful for working with dynamic FFIs -// For many languages, it may be faster to reimplement these functions natively instead. -// Note: This file should only be included by chipmunk.c. - -#ifdef _MSC_VER - #if _MSC_VER >= 1600 - #define MAKE_REF(name) CP_EXPORT decltype(name) *_##name = name - #else - #define MAKE_REF(name) - #endif -#else - #define MAKE_REF(name) __typeof__(name) *_##name = name -#endif - -#ifdef __cplusplus -extern "C" { -#endif - -MAKE_REF(cpv); // makes a variable named _cpv that contains the function pointer for cpv() -MAKE_REF(cpveql); -MAKE_REF(cpvadd); -MAKE_REF(cpvneg); -MAKE_REF(cpvsub); -MAKE_REF(cpvmult); -MAKE_REF(cpvdot); -MAKE_REF(cpvcross); -MAKE_REF(cpvperp); -MAKE_REF(cpvrperp); -MAKE_REF(cpvproject); -MAKE_REF(cpvforangle); -MAKE_REF(cpvtoangle); -MAKE_REF(cpvrotate); -MAKE_REF(cpvunrotate); -MAKE_REF(cpvlengthsq); -MAKE_REF(cpvlength); -MAKE_REF(cpvlerp); -MAKE_REF(cpvnormalize); -MAKE_REF(cpvclamp); -MAKE_REF(cpvlerpconst); -MAKE_REF(cpvdist); -MAKE_REF(cpvdistsq); -MAKE_REF(cpvnear); - -MAKE_REF(cpfmax); -MAKE_REF(cpfmin); -MAKE_REF(cpfabs); -MAKE_REF(cpfclamp); -MAKE_REF(cpflerp); -MAKE_REF(cpflerpconst); - -MAKE_REF(cpBBNew); -MAKE_REF(cpBBNewForExtents); -MAKE_REF(cpBBNewForCircle); -MAKE_REF(cpBBIntersects); -MAKE_REF(cpBBContainsBB); -MAKE_REF(cpBBContainsVect); -MAKE_REF(cpBBMerge); -MAKE_REF(cpBBExpand); -MAKE_REF(cpBBCenter); -MAKE_REF(cpBBArea); -MAKE_REF(cpBBMergedArea); -MAKE_REF(cpBBSegmentQuery); -MAKE_REF(cpBBIntersectsSegment); -MAKE_REF(cpBBClampVect); - -MAKE_REF(cpSpatialIndexDestroy); -MAKE_REF(cpSpatialIndexCount); -MAKE_REF(cpSpatialIndexEach); -MAKE_REF(cpSpatialIndexContains); -MAKE_REF(cpSpatialIndexInsert); -MAKE_REF(cpSpatialIndexRemove); -MAKE_REF(cpSpatialIndexReindex); -MAKE_REF(cpSpatialIndexReindexObject); -MAKE_REF(cpSpatialIndexSegmentQuery); -MAKE_REF(cpSpatialIndexQuery); -MAKE_REF(cpSpatialIndexReindexQuery); - -#ifdef __cplusplus -} -#endif - -#endif diff --git a/3rdparty/chipmunk/include/chipmunk/chipmunk_private.h b/3rdparty/chipmunk/include/chipmunk/chipmunk_private.h deleted file mode 100644 index e606ba16c0e6..000000000000 --- a/3rdparty/chipmunk/include/chipmunk/chipmunk_private.h +++ /dev/null @@ -1,344 +0,0 @@ -/* Copyright (c) 2013 Scott Lembcke and Howling Moon Software - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -#ifndef CHIPMUNK_PRIVATE_H -#define CHIPMUNK_PRIVATE_H - -#include "chipmunk/chipmunk.h" -#include "chipmunk/chipmunk_structs.h" - -#define CP_HASH_COEF (3344921057ul) -#define CP_HASH_PAIR(A, B) ((cpHashValue)(A)*CP_HASH_COEF ^ (cpHashValue)(B)*CP_HASH_COEF) - -// TODO: Eww. Magic numbers. -#define MAGIC_EPSILON 1e-5 - - -//MARK: cpArray - -cpArray *cpArrayNew(int size); - -void cpArrayFree(cpArray *arr); - -void cpArrayPush(cpArray *arr, void *object); -void *cpArrayPop(cpArray *arr); -void cpArrayDeleteObj(cpArray *arr, void *obj); -cpBool cpArrayContains(cpArray *arr, void *ptr); - -void cpArrayFreeEach(cpArray *arr, void (freeFunc)(void*)); - - -//MARK: cpHashSet - -typedef cpBool (*cpHashSetEqlFunc)(const void *ptr, const void *elt); -typedef void *(*cpHashSetTransFunc)(const void *ptr, void *data); - -cpHashSet *cpHashSetNew(int size, cpHashSetEqlFunc eqlFunc); -void cpHashSetSetDefaultValue(cpHashSet *set, void *default_value); - -void cpHashSetFree(cpHashSet *set); - -int cpHashSetCount(cpHashSet *set); -const void *cpHashSetInsert(cpHashSet *set, cpHashValue hash, const void *ptr, cpHashSetTransFunc trans, void *data); -const void *cpHashSetRemove(cpHashSet *set, cpHashValue hash, const void *ptr); -const void *cpHashSetFind(cpHashSet *set, cpHashValue hash, const void *ptr); - -typedef void (*cpHashSetIteratorFunc)(void *elt, void *data); -void cpHashSetEach(cpHashSet *set, cpHashSetIteratorFunc func, void *data); - -typedef cpBool (*cpHashSetFilterFunc)(void *elt, void *data); -void cpHashSetFilter(cpHashSet *set, cpHashSetFilterFunc func, void *data); - - -//MARK: Bodies - -void cpBodyAddShape(cpBody *body, cpShape *shape); -void cpBodyRemoveShape(cpBody *body, cpShape *shape); - -//void cpBodyAccumulateMassForShape(cpBody *body, cpShape *shape); -void cpBodyAccumulateMassFromShapes(cpBody *body); - -void cpBodyRemoveConstraint(cpBody *body, cpConstraint *constraint); - - -//MARK: Spatial Index Functions - -cpSpatialIndex *cpSpatialIndexInit(cpSpatialIndex *index, cpSpatialIndexClass *klass, cpSpatialIndexBBFunc bbfunc, cpSpatialIndex *staticIndex); - - -//MARK: Arbiters - -cpArbiter* cpArbiterInit(cpArbiter *arb, cpShape *a, cpShape *b); - -static inline struct cpArbiterThread * -cpArbiterThreadForBody(cpArbiter *arb, cpBody *body) -{ - return (arb->body_a == body ? &arb->thread_a : &arb->thread_b); -} - -void cpArbiterUnthread(cpArbiter *arb); - -void cpArbiterUpdate(cpArbiter *arb, struct cpCollisionInfo *info, cpSpace *space); -void cpArbiterPreStep(cpArbiter *arb, cpFloat dt, cpFloat bias, cpFloat slop); -void cpArbiterApplyCachedImpulse(cpArbiter *arb, cpFloat dt_coef); -void cpArbiterApplyImpulse(cpArbiter *arb); - - -//MARK: Shapes/Collisions - -cpShape *cpShapeInit(cpShape *shape, const cpShapeClass *klass, cpBody *body, struct cpShapeMassInfo massInfo); - -static inline cpBool -cpShapeActive(cpShape *shape) -{ - // checks if the shape is added to a shape list. - // TODO could this just check the space now? - return (shape->prev || (shape->body && shape->body->shapeList == shape)); -} - -// Note: This function returns contact points with r1/r2 in absolute coordinates, not body relative. -struct cpCollisionInfo cpCollide(const cpShape *a, const cpShape *b, cpCollisionID id, struct cpContact *contacts); - -static inline void -CircleSegmentQuery(cpShape *shape, cpVect center, cpFloat r1, cpVect a, cpVect b, cpFloat r2, cpSegmentQueryInfo *info) -{ - cpVect da = cpvsub(a, center); - cpVect db = cpvsub(b, center); - cpFloat rsum = r1 + r2; - - cpFloat qa = cpvdot(da, da) - 2.0f*cpvdot(da, db) + cpvdot(db, db); - cpFloat qb = cpvdot(da, db) - cpvdot(da, da); - cpFloat det = qb*qb - qa*(cpvdot(da, da) - rsum*rsum); - - if(det >= 0.0f){ - cpFloat t = (-qb - cpfsqrt(det))/(qa); - if(0.0f<= t && t <= 1.0f){ - cpVect n = cpvnormalize(cpvlerp(da, db, t)); - - info->shape = shape; - info->point = cpvsub(cpvlerp(a, b, t), cpvmult(n, r2)); - info->normal = n; - info->alpha = t; - } - } -} - -static inline cpBool -cpShapeFilterReject(cpShapeFilter a, cpShapeFilter b) -{ - // Reject the collision if: - return ( - // They are in the same non-zero group. - (a.group != 0 && a.group == b.group) || - // One of the category/mask combinations fails. - (a.categories & b.mask) == 0 || - (b.categories & a.mask) == 0 - ); -} - -void cpLoopIndexes(const cpVect *verts, int count, int *start, int *end); - - -//MARK: Constraints -// TODO naming conventions here - -void cpConstraintInit(cpConstraint *constraint, const struct cpConstraintClass *klass, cpBody *a, cpBody *b); - -static inline void -cpConstraintActivateBodies(cpConstraint *constraint) -{ - cpBody *a = constraint->a; cpBodyActivate(a); - cpBody *b = constraint->b; cpBodyActivate(b); -} - -static inline cpVect -relative_velocity(cpBody *a, cpBody *b, cpVect r1, cpVect r2){ - cpVect v1_sum = cpvadd(a->v, cpvmult(cpvperp(r1), a->w)); - cpVect v2_sum = cpvadd(b->v, cpvmult(cpvperp(r2), b->w)); - - return cpvsub(v2_sum, v1_sum); -} - -static inline cpFloat -normal_relative_velocity(cpBody *a, cpBody *b, cpVect r1, cpVect r2, cpVect n){ - return cpvdot(relative_velocity(a, b, r1, r2), n); -} - -static inline void -apply_impulse(cpBody *body, cpVect j, cpVect r){ - body->v = cpvadd(body->v, cpvmult(j, body->m_inv)); - body->w += body->i_inv*cpvcross(r, j); -} - -static inline void -apply_impulses(cpBody *a , cpBody *b, cpVect r1, cpVect r2, cpVect j) -{ - apply_impulse(a, cpvneg(j), r1); - apply_impulse(b, j, r2); -} - -static inline void -apply_bias_impulse(cpBody *body, cpVect j, cpVect r) -{ - body->v_bias = cpvadd(body->v_bias, cpvmult(j, body->m_inv)); - body->w_bias += body->i_inv*cpvcross(r, j); -} - -static inline void -apply_bias_impulses(cpBody *a , cpBody *b, cpVect r1, cpVect r2, cpVect j) -{ - apply_bias_impulse(a, cpvneg(j), r1); - apply_bias_impulse(b, j, r2); -} - -static inline cpFloat -k_scalar_body(cpBody *body, cpVect r, cpVect n) -{ - cpFloat rcn = cpvcross(r, n); - return body->m_inv + body->i_inv*rcn*rcn; -} - -static inline cpFloat -k_scalar(cpBody *a, cpBody *b, cpVect r1, cpVect r2, cpVect n) -{ - cpFloat value = k_scalar_body(a, r1, n) + k_scalar_body(b, r2, n); - cpAssertSoft(value != 0.0, "Unsolvable collision or constraint."); - - return value; -} - -static inline cpMat2x2 -k_tensor(cpBody *a, cpBody *b, cpVect r1, cpVect r2) -{ - cpFloat m_sum = a->m_inv + b->m_inv; - - // start with Identity*m_sum - cpFloat k11 = m_sum, k12 = 0.0f; - cpFloat k21 = 0.0f, k22 = m_sum; - - // add the influence from r1 - cpFloat a_i_inv = a->i_inv; - cpFloat r1xsq = r1.x * r1.x * a_i_inv; - cpFloat r1ysq = r1.y * r1.y * a_i_inv; - cpFloat r1nxy = -r1.x * r1.y * a_i_inv; - k11 += r1ysq; k12 += r1nxy; - k21 += r1nxy; k22 += r1xsq; - - // add the influnce from r2 - cpFloat b_i_inv = b->i_inv; - cpFloat r2xsq = r2.x * r2.x * b_i_inv; - cpFloat r2ysq = r2.y * r2.y * b_i_inv; - cpFloat r2nxy = -r2.x * r2.y * b_i_inv; - k11 += r2ysq; k12 += r2nxy; - k21 += r2nxy; k22 += r2xsq; - - // invert - cpFloat det = k11*k22 - k12*k21; - cpAssertSoft(det != 0.0, "Unsolvable constraint."); - - cpFloat det_inv = 1.0f/det; - return cpMat2x2New( - k22*det_inv, -k12*det_inv, - -k21*det_inv, k11*det_inv - ); -} - -static inline cpFloat -bias_coef(cpFloat errorBias, cpFloat dt) -{ - return 1.0f - cpfpow(errorBias, dt); -} - - -//MARK: Spaces - -#define cpAssertSpaceUnlocked(space) \ - cpAssertHard(!space->locked, \ - "This operation cannot be done safely during a call to cpSpaceStep() or during a query. " \ - "Put these calls into a post-step callback." \ - ); - -void cpSpaceSetStaticBody(cpSpace *space, cpBody *body); - -extern cpCollisionHandler cpCollisionHandlerDoNothing; - -void cpSpaceProcessComponents(cpSpace *space, cpFloat dt); - -void cpSpacePushFreshContactBuffer(cpSpace *space); -struct cpContact *cpContactBufferGetArray(cpSpace *space); -void cpSpacePushContacts(cpSpace *space, int count); - -cpPostStepCallback *cpSpaceGetPostStepCallback(cpSpace *space, void *key); - -cpBool cpSpaceArbiterSetFilter(cpArbiter *arb, cpSpace *space); -void cpSpaceFilterArbiters(cpSpace *space, cpBody *body, cpShape *filter); - -void cpSpaceActivateBody(cpSpace *space, cpBody *body); -void cpSpaceLock(cpSpace *space); -void cpSpaceUnlock(cpSpace *space, cpBool runPostStep); - -static inline void -cpSpaceUncacheArbiter(cpSpace *space, cpArbiter *arb) -{ - const cpShape *a = arb->a, *b = arb->b; - const cpShape *shape_pair[] = {a, b}; - cpHashValue arbHashID = CP_HASH_PAIR((cpHashValue)a, (cpHashValue)b); - cpHashSetRemove(space->cachedArbiters, arbHashID, shape_pair); - cpArrayDeleteObj(space->arbiters, arb); -} - -static inline cpArray * -cpSpaceArrayForBodyType(cpSpace *space, cpBodyType type) -{ - return (type == CP_BODY_TYPE_STATIC ? space->staticBodies : space->dynamicBodies); -} - -void cpShapeUpdateFunc(cpShape *shape, void *unused); -cpCollisionID cpSpaceCollideShapes(cpShape *a, cpShape *b, cpCollisionID id, cpSpace *space); - - -//MARK: Foreach loops - -static inline cpConstraint * -cpConstraintNext(cpConstraint *node, cpBody *body) -{ - return (node->a == body ? node->next_a : node->next_b); -} - -#define CP_BODY_FOREACH_CONSTRAINT(bdy, var)\ - for(cpConstraint *var = bdy->constraintList; var; var = cpConstraintNext(var, bdy)) - -static inline cpArbiter * -cpArbiterNext(cpArbiter *node, cpBody *body) -{ - return (node->body_a == body ? node->thread_a.next : node->thread_b.next); -} - -#define CP_BODY_FOREACH_ARBITER(bdy, var)\ - for(cpArbiter *var = bdy->arbiterList; var; var = cpArbiterNext(var, bdy)) - -#define CP_BODY_FOREACH_SHAPE(body, var)\ - for(cpShape *var = body->shapeList; var; var = var->next) - -#define CP_BODY_FOREACH_COMPONENT(root, var)\ - for(cpBody *var = root; var; var = var->sleeping.next) - -#endif diff --git a/3rdparty/chipmunk/include/chipmunk/chipmunk_structs.h b/3rdparty/chipmunk/include/chipmunk/chipmunk_structs.h deleted file mode 100644 index 1485795d4e97..000000000000 --- a/3rdparty/chipmunk/include/chipmunk/chipmunk_structs.h +++ /dev/null @@ -1,450 +0,0 @@ -/* Copyright (c) 2013 Scott Lembcke and Howling Moon Software - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -// All of the struct definitions for Chipmunk should be considered part of the private API. -// However, it is very valuable to know the struct sizes for preallocating memory. - -#ifndef CHIPMUNK_STRUCTS_H -#define CHIPMUNK_STRUCTS_H - -#include "chipmunk/chipmunk.h" - -struct cpArray { - int num, max; - void **arr; -}; - -struct cpBody { - // Integration functions - cpBodyVelocityFunc velocity_func; - cpBodyPositionFunc position_func; - - // mass and it's inverse - cpFloat m; - cpFloat m_inv; - - // moment of inertia and it's inverse - cpFloat i; - cpFloat i_inv; - - // center of gravity - cpVect cog; - - // position, velocity, force - cpVect p; - cpVect v; - cpVect f; - - // Angle, angular velocity, torque (radians) - cpFloat a; - cpFloat w; - cpFloat t; - - cpTransform transform; - - cpDataPointer userData; - - // "pseudo-velocities" used for eliminating overlap. - // Erin Catto has some papers that talk about what these are. - cpVect v_bias; - cpFloat w_bias; - - cpSpace *space; - - cpShape *shapeList; - cpArbiter *arbiterList; - cpConstraint *constraintList; - - struct { - cpBody *root; - cpBody *next; - cpFloat idleTime; - } sleeping; -}; - -enum cpArbiterState { - // Arbiter is active and its the first collision. - CP_ARBITER_STATE_FIRST_COLLISION, - // Arbiter is active and its not the first collision. - CP_ARBITER_STATE_NORMAL, - // Collision has been explicitly ignored. - // Either by returning false from a begin collision handler or calling cpArbiterIgnore(). - CP_ARBITER_STATE_IGNORE, - // Collison is no longer active. A space will cache an arbiter for up to cpSpace.collisionPersistence more steps. - CP_ARBITER_STATE_CACHED, - // Collison arbiter is invalid because one of the shapes was removed. - CP_ARBITER_STATE_INVALIDATED, -}; - -struct cpArbiterThread { - struct cpArbiter *next, *prev; -}; - -struct cpContact { - cpVect r1, r2; - - cpFloat nMass, tMass; - cpFloat bounce; // TODO: look for an alternate bounce solution. - - cpFloat jnAcc, jtAcc, jBias; - cpFloat bias; - - cpHashValue hash; -}; - -struct cpCollisionInfo { - const cpShape *a, *b; - cpCollisionID id; - - cpVect n; - - int count; - // TODO Should this be a unique struct type? - struct cpContact *arr; -}; - -struct cpArbiter { - cpFloat e; - cpFloat u; - cpVect surface_vr; - - cpDataPointer data; - - const cpShape *a, *b; - cpBody *body_a, *body_b; - struct cpArbiterThread thread_a, thread_b; - - int count; - struct cpContact *contacts; - cpVect n; - - // Regular, wildcard A and wildcard B collision handlers. - cpCollisionHandler *handler, *handlerA, *handlerB; - cpBool swapped; - - cpTimestamp stamp; - enum cpArbiterState state; -}; - -struct cpShapeMassInfo { - cpFloat m; - cpFloat i; - cpVect cog; - cpFloat area; -}; - -typedef enum cpShapeType{ - CP_CIRCLE_SHAPE, - CP_SEGMENT_SHAPE, - CP_POLY_SHAPE, - CP_NUM_SHAPES -} cpShapeType; - -typedef cpBB (*cpShapeCacheDataImpl)(cpShape *shape, cpTransform transform); -typedef void (*cpShapeDestroyImpl)(cpShape *shape); -typedef void (*cpShapePointQueryImpl)(const cpShape *shape, cpVect p, cpPointQueryInfo *info); -typedef void (*cpShapeSegmentQueryImpl)(const cpShape *shape, cpVect a, cpVect b, cpFloat radius, cpSegmentQueryInfo *info); - -typedef struct cpShapeClass cpShapeClass; - -struct cpShapeClass { - cpShapeType type; - - cpShapeCacheDataImpl cacheData; - cpShapeDestroyImpl destroy; - cpShapePointQueryImpl pointQuery; - cpShapeSegmentQueryImpl segmentQuery; -}; - -struct cpShape { - const cpShapeClass *klass; - - cpSpace *space; - cpBody *body; - struct cpShapeMassInfo massInfo; - cpBB bb; - - cpBool sensor; - - cpFloat e; - cpFloat u; - cpVect surfaceV; - - cpDataPointer userData; - - cpCollisionType type; - cpShapeFilter filter; - - cpShape *next; - cpShape *prev; - - cpHashValue hashid; -}; - -struct cpCircleShape { - cpShape shape; - - cpVect c, tc; - cpFloat r; -}; - -struct cpSegmentShape { - cpShape shape; - - cpVect a, b, n; - cpVect ta, tb, tn; - cpFloat r; - - cpVect a_tangent, b_tangent; -}; - -struct cpSplittingPlane { - cpVect v0, n; -}; - -#define CP_POLY_SHAPE_INLINE_ALLOC 6 - -struct cpPolyShape { - cpShape shape; - - cpFloat r; - - int count; - // The untransformed planes are appended at the end of the transformed planes. - struct cpSplittingPlane *planes; - - // Allocate a small number of splitting planes internally for simple poly. - struct cpSplittingPlane _planes[2*CP_POLY_SHAPE_INLINE_ALLOC]; -}; - -typedef void (*cpConstraintPreStepImpl)(cpConstraint *constraint, cpFloat dt); -typedef void (*cpConstraintApplyCachedImpulseImpl)(cpConstraint *constraint, cpFloat dt_coef); -typedef void (*cpConstraintApplyImpulseImpl)(cpConstraint *constraint, cpFloat dt); -typedef cpFloat (*cpConstraintGetImpulseImpl)(cpConstraint *constraint); - -typedef struct cpConstraintClass { - cpConstraintPreStepImpl preStep; - cpConstraintApplyCachedImpulseImpl applyCachedImpulse; - cpConstraintApplyImpulseImpl applyImpulse; - cpConstraintGetImpulseImpl getImpulse; -} cpConstraintClass; - -struct cpConstraint { - const cpConstraintClass *klass; - - cpSpace *space; - - cpBody *a, *b; - cpConstraint *next_a, *next_b; - - cpFloat maxForce; - cpFloat errorBias; - cpFloat maxBias; - - cpBool collideBodies; - - cpConstraintPreSolveFunc preSolve; - cpConstraintPostSolveFunc postSolve; - - cpDataPointer userData; -}; - -struct cpPinJoint { - cpConstraint constraint; - cpVect anchorA, anchorB; - cpFloat dist; - - cpVect r1, r2; - cpVect n; - cpFloat nMass; - - cpFloat jnAcc; - cpFloat bias; -}; - -struct cpSlideJoint { - cpConstraint constraint; - cpVect anchorA, anchorB; - cpFloat min, max; - - cpVect r1, r2; - cpVect n; - cpFloat nMass; - - cpFloat jnAcc; - cpFloat bias; -}; - -struct cpPivotJoint { - cpConstraint constraint; - cpVect anchorA, anchorB; - - cpVect r1, r2; - cpMat2x2 k; - - cpVect jAcc; - cpVect bias; -}; - -struct cpGrooveJoint { - cpConstraint constraint; - cpVect grv_n, grv_a, grv_b; - cpVect anchorB; - - cpVect grv_tn; - cpFloat clamp; - cpVect r1, r2; - cpMat2x2 k; - - cpVect jAcc; - cpVect bias; -}; - -struct cpDampedSpring { - cpConstraint constraint; - cpVect anchorA, anchorB; - cpFloat restLength; - cpFloat stiffness; - cpFloat damping; - cpDampedSpringForceFunc springForceFunc; - - cpFloat target_vrn; - cpFloat v_coef; - - cpVect r1, r2; - cpFloat nMass; - cpVect n; - - cpFloat jAcc; -}; - -struct cpDampedRotarySpring { - cpConstraint constraint; - cpFloat restAngle; - cpFloat stiffness; - cpFloat damping; - cpDampedRotarySpringTorqueFunc springTorqueFunc; - - cpFloat target_wrn; - cpFloat w_coef; - - cpFloat iSum; - cpFloat jAcc; -}; - -struct cpRotaryLimitJoint { - cpConstraint constraint; - cpFloat min, max; - - cpFloat iSum; - - cpFloat bias; - cpFloat jAcc; -}; - -struct cpRatchetJoint { - cpConstraint constraint; - cpFloat angle, phase, ratchet; - - cpFloat iSum; - - cpFloat bias; - cpFloat jAcc; -}; - -struct cpGearJoint { - cpConstraint constraint; - cpFloat phase, ratio; - cpFloat ratio_inv; - - cpFloat iSum; - - cpFloat bias; - cpFloat jAcc; -}; - -struct cpSimpleMotor { - cpConstraint constraint; - cpFloat rate; - - cpFloat iSum; - - cpFloat jAcc; -}; - -typedef struct cpContactBufferHeader cpContactBufferHeader; -typedef void (*cpSpaceArbiterApplyImpulseFunc)(cpArbiter *arb); - -struct cpSpace { - int iterations; - - cpVect gravity; - cpFloat damping; - - cpFloat idleSpeedThreshold; - cpFloat sleepTimeThreshold; - - cpFloat collisionSlop; - cpFloat collisionBias; - cpTimestamp collisionPersistence; - - cpDataPointer userData; - - cpTimestamp stamp; - cpFloat curr_dt; - - cpArray *dynamicBodies; - cpArray *staticBodies; - cpArray *rousedBodies; - cpArray *sleepingComponents; - - cpHashValue shapeIDCounter; - cpSpatialIndex *staticShapes; - cpSpatialIndex *dynamicShapes; - - cpArray *constraints; - - cpArray *arbiters; - cpContactBufferHeader *contactBuffersHead; - cpHashSet *cachedArbiters; - cpArray *pooledArbiters; - - cpArray *allocatedBuffers; - int locked; - - cpBool usesWildcards; - cpHashSet *collisionHandlers; - cpCollisionHandler defaultHandler; - - cpBool skipPostStep; - cpArray *postStepCallbacks; - - cpBody *staticBody; - cpBody _staticBody; -}; - -typedef struct cpPostStepCallback { - cpPostStepFunc func; - void *key; - void *data; -} cpPostStepCallback; - -#endif diff --git a/3rdparty/chipmunk/include/chipmunk/chipmunk_types.h b/3rdparty/chipmunk/include/chipmunk/chipmunk_types.h deleted file mode 100644 index 285e2c41b1e6..000000000000 --- a/3rdparty/chipmunk/include/chipmunk/chipmunk_types.h +++ /dev/null @@ -1,272 +0,0 @@ -/* Copyright (c) 2013 Scott Lembcke and Howling Moon Software - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -#ifndef CHIPMUNK_TYPES_H -#define CHIPMUNK_TYPES_H - -// EGNX use float precision, so disable chipmunk double use. -#define CP_USE_CGTYPES 0 -#define CP_USE_DOUBLES 0 - -#include -#include -#include - -#ifdef __APPLE__ - #include "TargetConditionals.h" -#endif - -// Use CGTypes by default on iOS and Mac. -// Also enables usage of doubles on 64 bit. -// Performance is usually very comparable when the CPU cache is well utilised. -#if (TARGET_OS_IPHONE || TARGET_OS_MAC) && (!defined CP_USE_CGTYPES) - #define CP_USE_CGTYPES 1 -#endif - -#if CP_USE_CGTYPES - #if TARGET_OS_IPHONE - #include - #include - #elif TARGET_OS_MAC - #include - #endif - - #if defined(__LP64__) && __LP64__ - #define CP_USE_DOUBLES 1 - #else - #define CP_USE_DOUBLES 0 - #endif -#endif - -#ifndef CP_USE_DOUBLES - // Use doubles by default for higher precision. - #define CP_USE_DOUBLES 1 -#endif - -/// @defgroup basicTypes Basic Types -/// Most of these types can be configured at compile time. -/// @{ - -#if CP_USE_DOUBLES -/// Chipmunk's floating point type. -/// Can be reconfigured at compile time. - typedef double cpFloat; - #define cpfsqrt sqrt - #define cpfsin sin - #define cpfcos cos - #define cpfacos acos - #define cpfatan2 atan2 - #define cpfmod fmod - #define cpfexp exp - #define cpfpow pow - #define cpffloor floor - #define cpfceil ceil - #define CPFLOAT_MIN DBL_MIN -#else - typedef float cpFloat; - #define cpfsqrt sqrtf - #define cpfsin sinf - #define cpfcos cosf - #define cpfacos acosf - #define cpfatan2 atan2f - #define cpfmod fmodf - #define cpfexp expf - #define cpfpow powf - #define cpffloor floorf - #define cpfceil ceilf - #define CPFLOAT_MIN FLT_MIN -#endif - -#ifndef INFINITY - #ifdef _MSC_VER - union MSVC_EVIL_FLOAT_HACK - { - unsigned __int8 Bytes[4]; - float Value; - }; - static union MSVC_EVIL_FLOAT_HACK INFINITY_HACK = {{0x00, 0x00, 0x80, 0x7F}}; - #define INFINITY (INFINITY_HACK.Value) - #endif - - #ifdef __GNUC__ - #define INFINITY (__builtin_inf()) - #endif - - #ifndef INFINITY - #define INFINITY (1e1000) - #endif -#endif - - -#define CP_PI ((cpFloat)3.14159265358979323846264338327950288) - - -/// Return the max of two cpFloats. -static inline cpFloat cpfmax(cpFloat a, cpFloat b) -{ - return (a > b) ? a : b; -} - -/// Return the min of two cpFloats. -static inline cpFloat cpfmin(cpFloat a, cpFloat b) -{ - return (a < b) ? a : b; -} - -/// Return the absolute value of a cpFloat. -static inline cpFloat cpfabs(cpFloat f) -{ - return (f < 0) ? -f : f; -} - -/// Clamp @c f to be between @c min and @c max. -static inline cpFloat cpfclamp(cpFloat f, cpFloat min, cpFloat max) -{ - return cpfmin(cpfmax(f, min), max); -} - -/// Clamp @c f to be between 0 and 1. -static inline cpFloat cpfclamp01(cpFloat f) -{ - return cpfmax(0.0f, cpfmin(f, 1.0f)); -} - - - -/// Linearly interpolate (or extrapolate) between @c f1 and @c f2 by @c t percent. -static inline cpFloat cpflerp(cpFloat f1, cpFloat f2, cpFloat t) -{ - return f1*(1.0f - t) + f2*t; -} - -/// Linearly interpolate from @c f1 to @c f2 by no more than @c d. -static inline cpFloat cpflerpconst(cpFloat f1, cpFloat f2, cpFloat d) -{ - return f1 + cpfclamp(f2 - f1, -d, d); -} - -/// Hash value type. -#ifdef CP_HASH_VALUE_TYPE - typedef CP_HASH_VALUE_TYPE cpHashValue; -#else - typedef uintptr_t cpHashValue; -#endif - -/// Type used internally to cache colliding object info for cpCollideShapes(). -/// Should be at least 32 bits. -typedef uint32_t cpCollisionID; - -// Oh C, how we love to define our own boolean types to get compiler compatibility -/// Chipmunk's boolean type. -#ifdef CP_BOOL_TYPE - typedef CP_BOOL_TYPE cpBool; -#else - typedef unsigned char cpBool; -#endif - -#ifndef cpTrue -/// true value. - #define cpTrue 1 -#endif - -#ifndef cpFalse -/// false value. - #define cpFalse 0 -#endif - -#ifdef CP_DATA_POINTER_TYPE - typedef CP_DATA_POINTER_TYPE cpDataPointer; -#else -/// Type used for user data pointers. - typedef void * cpDataPointer; -#endif - -#ifdef CP_COLLISION_TYPE_TYPE - typedef CP_COLLISION_TYPE_TYPE cpCollisionType; -#else -/// Type used for cpSpace.collision_type. - typedef uintptr_t cpCollisionType; -#endif - -#ifdef CP_GROUP_TYPE - typedef CP_GROUP_TYPE cpGroup; -#else -/// Type used for cpShape.group. - typedef uintptr_t cpGroup; -#endif - -#ifdef CP_BITMASK_TYPE - typedef CP_BITMASK_TYPE cpBitmask; -#else -/// Type used for cpShapeFilter category and mask. - typedef unsigned int cpBitmask; -#endif - -#ifdef CP_TIMESTAMP_TYPE - typedef CP_TIMESTAMP_TYPE cpTimestamp; -#else -/// Type used for various timestamps in Chipmunk. - typedef unsigned int cpTimestamp; -#endif - -#ifndef CP_NO_GROUP -/// Value for cpShape.group signifying that a shape is in no group. - #define CP_NO_GROUP ((cpGroup)0) -#endif - -#ifndef CP_ALL_CATEGORIES -/// Value for cpShape.layers signifying that a shape is in every layer. - #define CP_ALL_CATEGORIES (~(cpBitmask)0) -#endif - -#ifndef CP_WILDCARD_COLLISION_TYPE -/// cpCollisionType value internally reserved for hashing wildcard handlers. - #define CP_WILDCARD_COLLISION_TYPE (~(cpCollisionType)0) -#endif - -/// @} - -// CGPoints are structurally the same, and allow -// easy interoperability with other Cocoa libraries -#if CP_USE_CGTYPES - typedef CGPoint cpVect; -#else -/// Chipmunk's 2D vector type. -/// @addtogroup cpVect - typedef struct cpVect{cpFloat x,y;} cpVect; -#endif - -#if CP_USE_CGTYPES - typedef CGAffineTransform cpTransform; -#else - /// Column major affine transform. - typedef struct cpTransform { - cpFloat a, b, c, d, tx, ty; - } cpTransform; -#endif - -// NUKE -typedef struct cpMat2x2 { - // Row major [[a, b][c d]] - cpFloat a, b, c, d; -} cpMat2x2; - -#endif diff --git a/3rdparty/chipmunk/include/chipmunk/chipmunk_unsafe.h b/3rdparty/chipmunk/include/chipmunk/chipmunk_unsafe.h deleted file mode 100644 index 990bd012af45..000000000000 --- a/3rdparty/chipmunk/include/chipmunk/chipmunk_unsafe.h +++ /dev/null @@ -1,66 +0,0 @@ -/* Copyright (c) 2013 Scott Lembcke and Howling Moon Software - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/* This header defines a number of "unsafe" operations on Chipmunk objects. - * In this case "unsafe" is referring to operations which may reduce the - * physical accuracy or numerical stability of the simulation, but will not - * cause crashes. - * - * The prime example is mutating collision shapes. Chipmunk does not support - * this directly. Mutating shapes using this API will caused objects in contact - * to be pushed apart using Chipmunk's overlap solver, but not using real - * persistent velocities. Probably not what you meant, but perhaps close enough. - */ - -/// @defgroup unsafe Chipmunk Unsafe Shape Operations -/// These functions are used for mutating collision shapes. -/// Chipmunk does not have any way to get velocity information on changing shapes, -/// so the results will be unrealistic. You must explicity include the chipmunk_unsafe.h header to use them. -/// @{ - -#ifndef CHIPMUNK_UNSAFE_H -#define CHIPMUNK_UNSAFE_H - -#ifdef __cplusplus -extern "C" { -#endif - -/// Set the radius of a circle shape. -CP_EXPORT void cpCircleShapeSetRadius(cpShape *shape, cpFloat radius); -/// Set the offset of a circle shape. -CP_EXPORT void cpCircleShapeSetOffset(cpShape *shape, cpVect offset); - -/// Set the endpoints of a segment shape. -CP_EXPORT void cpSegmentShapeSetEndpoints(cpShape *shape, cpVect a, cpVect b); -/// Set the radius of a segment shape. -CP_EXPORT void cpSegmentShapeSetRadius(cpShape *shape, cpFloat radius); - -/// Set the vertexes of a poly shape. -CP_EXPORT void cpPolyShapeSetVerts(cpShape *shape, int count, cpVect *verts, cpTransform transform); -CP_EXPORT void cpPolyShapeSetVertsRaw(cpShape *shape, int count, cpVect *verts); -/// Set the radius of a poly shape. -CP_EXPORT void cpPolyShapeSetRadius(cpShape *shape, cpFloat radius); - -#ifdef __cplusplus -} -#endif -#endif -/// @} diff --git a/3rdparty/chipmunk/include/chipmunk/cpArbiter.h b/3rdparty/chipmunk/include/chipmunk/cpArbiter.h deleted file mode 100644 index 1dc130afb040..000000000000 --- a/3rdparty/chipmunk/include/chipmunk/cpArbiter.h +++ /dev/null @@ -1,145 +0,0 @@ -/* Copyright (c) 2013 Scott Lembcke and Howling Moon Software - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/// @defgroup cpArbiter cpArbiter -/// The cpArbiter struct tracks pairs of colliding shapes. -/// They are also used in conjuction with collision handler callbacks -/// allowing you to retrieve information on the collision or change it. -/// A unique arbiter value is used for each pair of colliding objects. It persists until the shapes separate. -/// @{ - -#define CP_MAX_CONTACTS_PER_ARBITER 2 - -/// Get the restitution (elasticity) that will be applied to the pair of colliding objects. -CP_EXPORT cpFloat cpArbiterGetRestitution(const cpArbiter *arb); -/// Override the restitution (elasticity) that will be applied to the pair of colliding objects. -CP_EXPORT void cpArbiterSetRestitution(cpArbiter *arb, cpFloat restitution); -/// Get the friction coefficient that will be applied to the pair of colliding objects. -CP_EXPORT cpFloat cpArbiterGetFriction(const cpArbiter *arb); -/// Override the friction coefficient that will be applied to the pair of colliding objects. -CP_EXPORT void cpArbiterSetFriction(cpArbiter *arb, cpFloat friction); - -// Get the relative surface velocity of the two shapes in contact. -CP_EXPORT cpVect cpArbiterGetSurfaceVelocity(cpArbiter *arb); - -// Override the relative surface velocity of the two shapes in contact. -// By default this is calculated to be the difference of the two surface velocities clamped to the tangent plane. -CP_EXPORT void cpArbiterSetSurfaceVelocity(cpArbiter *arb, cpVect vr); - -/// Get the user data pointer associated with this pair of colliding objects. -CP_EXPORT cpDataPointer cpArbiterGetUserData(const cpArbiter *arb); -/// Set a user data point associated with this pair of colliding objects. -/// If you need to perform any cleanup for this pointer, you must do it yourself, in the separate callback for instance. -CP_EXPORT void cpArbiterSetUserData(cpArbiter *arb, cpDataPointer userData); - -/// Calculate the total impulse including the friction that was applied by this arbiter. -/// This function should only be called from a post-solve, post-step or cpBodyEachArbiter callback. -CP_EXPORT cpVect cpArbiterTotalImpulse(const cpArbiter *arb); -/// Calculate the amount of energy lost in a collision including static, but not dynamic friction. -/// This function should only be called from a post-solve, post-step or cpBodyEachArbiter callback. -CP_EXPORT cpFloat cpArbiterTotalKE(const cpArbiter *arb); - -/// Mark a collision pair to be ignored until the two objects separate. -/// Pre-solve and post-solve callbacks will not be called, but the separate callback will be called. -CP_EXPORT cpBool cpArbiterIgnore(cpArbiter *arb); - -/// Return the colliding shapes involved for this arbiter. -/// The order of their cpSpace.collision_type values will match -/// the order set when the collision handler was registered. -CP_EXPORT void cpArbiterGetShapes(const cpArbiter *arb, cpShape **a, cpShape **b); - -/// A macro shortcut for defining and retrieving the shapes from an arbiter. -#define CP_ARBITER_GET_SHAPES(__arb__, __a__, __b__) cpShape *__a__, *__b__; cpArbiterGetShapes(__arb__, &__a__, &__b__); - -/// Return the colliding bodies involved for this arbiter. -/// The order of the cpSpace.collision_type the bodies are associated with values will match -/// the order set when the collision handler was registered. -CP_EXPORT void cpArbiterGetBodies(const cpArbiter *arb, cpBody **a, cpBody **b); - -/// A macro shortcut for defining and retrieving the bodies from an arbiter. -#define CP_ARBITER_GET_BODIES(__arb__, __a__, __b__) cpBody *__a__, *__b__; cpArbiterGetBodies(__arb__, &__a__, &__b__); - -/// A struct that wraps up the important collision data for an arbiter. -struct cpContactPointSet { - /// The number of contact points in the set. - int count; - - /// The normal of the collision. - cpVect normal; - - /// The array of contact points. - struct { - /// The position of the contact on the surface of each shape. - cpVect pointA, pointB; - /// Penetration distance of the two shapes. Overlapping means it will be negative. - /// This value is calculated as cpvdot(cpvsub(point2, point1), normal) and is ignored by cpArbiterSetContactPointSet(). - cpFloat distance; - } points[CP_MAX_CONTACTS_PER_ARBITER]; -}; - -/// Return a contact set from an arbiter. -CP_EXPORT cpContactPointSet cpArbiterGetContactPointSet(const cpArbiter *arb); - -/// Replace the contact point set for an arbiter. -/// This can be a very powerful feature, but use it with caution! -CP_EXPORT void cpArbiterSetContactPointSet(cpArbiter *arb, cpContactPointSet *set); - -/// Returns true if this is the first step a pair of objects started colliding. -CP_EXPORT cpBool cpArbiterIsFirstContact(const cpArbiter *arb); -/// Returns true if the separate callback is due to a shape being removed from the space. -CP_EXPORT cpBool cpArbiterIsRemoval(const cpArbiter *arb); - -/// Get the number of contact points for this arbiter. -CP_EXPORT int cpArbiterGetCount(const cpArbiter *arb); -/// Get the normal of the collision. -CP_EXPORT cpVect cpArbiterGetNormal(const cpArbiter *arb); -/// Get the position of the @c ith contact point on the surface of the first shape. -CP_EXPORT cpVect cpArbiterGetPointA(const cpArbiter *arb, int i); -/// Get the position of the @c ith contact point on the surface of the second shape. -CP_EXPORT cpVect cpArbiterGetPointB(const cpArbiter *arb, int i); -/// Get the depth of the @c ith contact point. -CP_EXPORT cpFloat cpArbiterGetDepth(const cpArbiter *arb, int i); - -/// If you want a custom callback to invoke the wildcard callback for the first collision type, you must call this function explicitly. -/// You must decide how to handle the wildcard's return value since it may disagree with the other wildcard handler's return value or your own. -CP_EXPORT cpBool cpArbiterCallWildcardBeginA(cpArbiter *arb, cpSpace *space); -/// If you want a custom callback to invoke the wildcard callback for the second collision type, you must call this function explicitly. -/// You must decide how to handle the wildcard's return value since it may disagree with the other wildcard handler's return value or your own. -CP_EXPORT cpBool cpArbiterCallWildcardBeginB(cpArbiter *arb, cpSpace *space); - -/// If you want a custom callback to invoke the wildcard callback for the first collision type, you must call this function explicitly. -/// You must decide how to handle the wildcard's return value since it may disagree with the other wildcard handler's return value or your own. -CP_EXPORT cpBool cpArbiterCallWildcardPreSolveA(cpArbiter *arb, cpSpace *space); -/// If you want a custom callback to invoke the wildcard callback for the second collision type, you must call this function explicitly. -/// You must decide how to handle the wildcard's return value since it may disagree with the other wildcard handler's return value or your own. -CP_EXPORT cpBool cpArbiterCallWildcardPreSolveB(cpArbiter *arb, cpSpace *space); - -/// If you want a custom callback to invoke the wildcard callback for the first collision type, you must call this function explicitly. -CP_EXPORT void cpArbiterCallWildcardPostSolveA(cpArbiter *arb, cpSpace *space); -/// If you want a custom callback to invoke the wildcard callback for the second collision type, you must call this function explicitly. -CP_EXPORT void cpArbiterCallWildcardPostSolveB(cpArbiter *arb, cpSpace *space); - -/// If you want a custom callback to invoke the wildcard callback for the first collision type, you must call this function explicitly. -CP_EXPORT void cpArbiterCallWildcardSeparateA(cpArbiter *arb, cpSpace *space); -/// If you want a custom callback to invoke the wildcard callback for the second collision type, you must call this function explicitly. -CP_EXPORT void cpArbiterCallWildcardSeparateB(cpArbiter *arb, cpSpace *space); - -/// @} diff --git a/3rdparty/chipmunk/include/chipmunk/cpBB.h b/3rdparty/chipmunk/include/chipmunk/cpBB.h deleted file mode 100644 index 8fc87049cc40..000000000000 --- a/3rdparty/chipmunk/include/chipmunk/cpBB.h +++ /dev/null @@ -1,187 +0,0 @@ -/* Copyright (c) 2013 Scott Lembcke and Howling Moon Software - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -#ifndef CHIPMUNK_BB_H -#define CHIPMUNK_BB_H - -#include "chipmunk_types.h" -#include "cpVect.h" - -/// @defgroup cpBBB cpBB -/// Chipmunk's axis-aligned 2D bounding box type along with a few handy routines. -/// @{ - -/// Chipmunk's axis-aligned 2D bounding box type. (left, bottom, right, top) -typedef struct cpBB{ - cpFloat l, b, r ,t; -} cpBB; - -/// Convenience constructor for cpBB structs. -static inline cpBB cpBBNew(const cpFloat l, const cpFloat b, const cpFloat r, const cpFloat t) -{ - cpBB bb = {l, b, r, t}; - return bb; -} - -/// Constructs a cpBB centered on a point with the given extents (half sizes). -static inline cpBB -cpBBNewForExtents(const cpVect c, const cpFloat hw, const cpFloat hh) -{ - return cpBBNew(c.x - hw, c.y - hh, c.x + hw, c.y + hh); -} - -/// Constructs a cpBB for a circle with the given position and radius. -static inline cpBB cpBBNewForCircle(const cpVect p, const cpFloat r) -{ - return cpBBNewForExtents(p, r, r); -} - -/// Returns true if @c a and @c b intersect. -static inline cpBool cpBBIntersects(const cpBB a, const cpBB b) -{ - return (a.l <= b.r && b.l <= a.r && a.b <= b.t && b.b <= a.t); -} - -/// Returns true if @c other lies completely within @c bb. -static inline cpBool cpBBContainsBB(const cpBB bb, const cpBB other) -{ - return (bb.l <= other.l && bb.r >= other.r && bb.b <= other.b && bb.t >= other.t); -} - -/// Returns true if @c bb contains @c v. -static inline cpBool cpBBContainsVect(const cpBB bb, const cpVect v) -{ - return (bb.l <= v.x && bb.r >= v.x && bb.b <= v.y && bb.t >= v.y); -} - -/// Returns a bounding box that holds both bounding boxes. -static inline cpBB cpBBMerge(const cpBB a, const cpBB b){ - return cpBBNew( - cpfmin(a.l, b.l), - cpfmin(a.b, b.b), - cpfmax(a.r, b.r), - cpfmax(a.t, b.t) - ); -} - -/// Returns a bounding box that holds both @c bb and @c v. -static inline cpBB cpBBExpand(const cpBB bb, const cpVect v){ - return cpBBNew( - cpfmin(bb.l, v.x), - cpfmin(bb.b, v.y), - cpfmax(bb.r, v.x), - cpfmax(bb.t, v.y) - ); -} - -/// Returns the center of a bounding box. -static inline cpVect -cpBBCenter(cpBB bb) -{ - return cpvlerp(cpv(bb.l, bb.b), cpv(bb.r, bb.t), 0.5f); -} - -/// Returns the area of the bounding box. -static inline cpFloat cpBBArea(cpBB bb) -{ - return (bb.r - bb.l)*(bb.t - bb.b); -} - -/// Merges @c a and @c b and returns the area of the merged bounding box. -static inline cpFloat cpBBMergedArea(cpBB a, cpBB b) -{ - return (cpfmax(a.r, b.r) - cpfmin(a.l, b.l))*(cpfmax(a.t, b.t) - cpfmin(a.b, b.b)); -} - -/// Returns the fraction along the segment query the cpBB is hit. Returns INFINITY if it doesn't hit. -static inline cpFloat cpBBSegmentQuery(cpBB bb, cpVect a, cpVect b) -{ - cpVect delta = cpvsub(b, a); - cpFloat tmin = -INFINITY, tmax = INFINITY; - - if(delta.x == 0.0f){ - if(a.x < bb.l || bb.r < a.x) return INFINITY; - } else { - cpFloat t1 = (bb.l - a.x)/delta.x; - cpFloat t2 = (bb.r - a.x)/delta.x; - tmin = cpfmax(tmin, cpfmin(t1, t2)); - tmax = cpfmin(tmax, cpfmax(t1, t2)); - } - - if(delta.y == 0.0f){ - if(a.y < bb.b || bb.t < a.y) return INFINITY; - } else { - cpFloat t1 = (bb.b - a.y)/delta.y; - cpFloat t2 = (bb.t - a.y)/delta.y; - tmin = cpfmax(tmin, cpfmin(t1, t2)); - tmax = cpfmin(tmax, cpfmax(t1, t2)); - } - - if(tmin <= tmax && 0.0f <= tmax && tmin <= 1.0f){ - return cpfmax(tmin, 0.0f); - } else { - return INFINITY; - } -} - -/// Return true if the bounding box intersects the line segment with ends @c a and @c b. -static inline cpBool cpBBIntersectsSegment(cpBB bb, cpVect a, cpVect b) -{ - return (cpBBSegmentQuery(bb, a, b) != INFINITY); -} - -/// Clamp a vector to a bounding box. -static inline cpVect -cpBBClampVect(const cpBB bb, const cpVect v) -{ - return cpv(cpfclamp(v.x, bb.l, bb.r), cpfclamp(v.y, bb.b, bb.t)); -} - -/// Wrap a vector to a bounding box. -static inline cpVect -cpBBWrapVect(const cpBB bb, const cpVect v) -{ - cpFloat dx = cpfabs(bb.r - bb.l); - cpFloat modx = cpfmod(v.x - bb.l, dx); - cpFloat x = (modx > 0.0f) ? modx : modx + dx; - - cpFloat dy = cpfabs(bb.t - bb.b); - cpFloat mody = cpfmod(v.y - bb.b, dy); - cpFloat y = (mody > 0.0f) ? mody : mody + dy; - - return cpv(x + bb.l, y + bb.b); -} - -/// Returns a bounding box offseted by @c v. -static inline cpBB -cpBBOffset(const cpBB bb, const cpVect v) -{ - return cpBBNew( - bb.l + v.x, - bb.b + v.y, - bb.r + v.x, - bb.t + v.y - ); -} - -///@} - -#endif diff --git a/3rdparty/chipmunk/include/chipmunk/cpBody.h b/3rdparty/chipmunk/include/chipmunk/cpBody.h deleted file mode 100644 index 7e6943d15794..000000000000 --- a/3rdparty/chipmunk/include/chipmunk/cpBody.h +++ /dev/null @@ -1,189 +0,0 @@ -/* Copyright (c) 2013 Scott Lembcke and Howling Moon Software - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/// @defgroup cpBody cpBody -/// Chipmunk's rigid body type. Rigid bodies hold the physical properties of an object like -/// it's mass, and position and velocity of it's center of gravity. They don't have an shape on their own. -/// They are given a shape by creating collision shapes (cpShape) that point to the body. -/// @{ - -typedef enum cpBodyType { - /// A dynamic body is one that is affected by gravity, forces, and collisions. - /// This is the default body type. - CP_BODY_TYPE_DYNAMIC, - /// A kinematic body is an infinite mass, user controlled body that is not affected by gravity, forces or collisions. - /// Instead the body only moves based on it's velocity. - /// Dynamic bodies collide normally with kinematic bodies, though the kinematic body will be unaffected. - /// Collisions between two kinematic bodies, or a kinematic body and a static body produce collision callbacks, but no collision response. - CP_BODY_TYPE_KINEMATIC, - /// A static body is a body that never (or rarely) moves. If you move a static body, you must call one of the cpSpaceReindex*() functions. - /// Chipmunk uses this information to optimize the collision detection. - /// Static bodies do not produce collision callbacks when colliding with other static bodies. - CP_BODY_TYPE_STATIC, -} cpBodyType; - -/// Rigid body velocity update function type. -typedef void (*cpBodyVelocityFunc)(cpBody *body, cpVect gravity, cpFloat damping, cpFloat dt); -/// Rigid body position update function type. -typedef void (*cpBodyPositionFunc)(cpBody *body, cpFloat dt); - -/// Allocate a cpBody. -CP_EXPORT cpBody* cpBodyAlloc(void); -/// Initialize a cpBody. -CP_EXPORT cpBody* cpBodyInit(cpBody *body, cpFloat mass, cpFloat moment); -/// Allocate and initialize a cpBody. -CP_EXPORT cpBody* cpBodyNew(cpFloat mass, cpFloat moment); - -/// Allocate and initialize a cpBody, and set it as a kinematic body. -CP_EXPORT cpBody* cpBodyNewKinematic(void); -/// Allocate and initialize a cpBody, and set it as a static body. -CP_EXPORT cpBody* cpBodyNewStatic(void); - -/// Destroy a cpBody. -CP_EXPORT void cpBodyDestroy(cpBody *body); -/// Destroy and free a cpBody. -CP_EXPORT void cpBodyFree(cpBody *body); - -// Defined in cpSpace.c -/// Wake up a sleeping or idle body. -CP_EXPORT void cpBodyActivate(cpBody *body); -/// Wake up any sleeping or idle bodies touching a static body. -CP_EXPORT void cpBodyActivateStatic(cpBody *body, cpShape *filter); - -/// Force a body to fall asleep immediately. -CP_EXPORT void cpBodySleep(cpBody *body); -/// Force a body to fall asleep immediately along with other bodies in a group. -CP_EXPORT void cpBodySleepWithGroup(cpBody *body, cpBody *group); - -/// Returns true if the body is sleeping. -CP_EXPORT cpBool cpBodyIsSleeping(const cpBody *body); - -/// Get the type of the body. -CP_EXPORT cpBodyType cpBodyGetType(cpBody *body); -/// Set the type of the body. -CP_EXPORT void cpBodySetType(cpBody *body, cpBodyType type); - -/// Get the space this body is added to. -CP_EXPORT cpSpace* cpBodyGetSpace(const cpBody *body); - -/// Get the mass of the body. -CP_EXPORT cpFloat cpBodyGetMass(const cpBody *body); -/// Set the mass of the body. -CP_EXPORT void cpBodySetMass(cpBody *body, cpFloat m); - -/// Get the moment of inertia of the body. -CP_EXPORT cpFloat cpBodyGetMoment(const cpBody *body); -/// Set the moment of inertia of the body. -CP_EXPORT void cpBodySetMoment(cpBody *body, cpFloat i); - -/// Set the position of a body. -CP_EXPORT cpVect cpBodyGetPosition(const cpBody *body); -/// Set the position of the body. -CP_EXPORT void cpBodySetPosition(cpBody *body, cpVect pos); - -/// Get the offset of the center of gravity in body local coordinates. -CP_EXPORT cpVect cpBodyGetCenterOfGravity(const cpBody *body); -/// Set the offset of the center of gravity in body local coordinates. -CP_EXPORT void cpBodySetCenterOfGravity(cpBody *body, cpVect cog); - -/// Get the velocity of the body. -CP_EXPORT cpVect cpBodyGetVelocity(const cpBody *body); -/// Set the velocity of the body. -CP_EXPORT void cpBodySetVelocity(cpBody *body, cpVect velocity); - -/// Get the force applied to the body for the next time step. -CP_EXPORT cpVect cpBodyGetForce(const cpBody *body); -/// Set the force applied to the body for the next time step. -CP_EXPORT void cpBodySetForce(cpBody *body, cpVect force); - -/// Get the angle of the body. -CP_EXPORT cpFloat cpBodyGetAngle(const cpBody *body); -/// Set the angle of a body. -CP_EXPORT void cpBodySetAngle(cpBody *body, cpFloat a); - -/// Get the angular velocity of the body. -CP_EXPORT cpFloat cpBodyGetAngularVelocity(const cpBody *body); -/// Set the angular velocity of the body. -CP_EXPORT void cpBodySetAngularVelocity(cpBody *body, cpFloat angularVelocity); - -/// Get the torque applied to the body for the next time step. -CP_EXPORT cpFloat cpBodyGetTorque(const cpBody *body); -/// Set the torque applied to the body for the next time step. -CP_EXPORT void cpBodySetTorque(cpBody *body, cpFloat torque); - -/// Get the rotation vector of the body. (The x basis vector of it's transform.) -CP_EXPORT cpVect cpBodyGetRotation(const cpBody *body); - -/// Get the user data pointer assigned to the body. -CP_EXPORT cpDataPointer cpBodyGetUserData(const cpBody *body); -/// Set the user data pointer assigned to the body. -CP_EXPORT void cpBodySetUserData(cpBody *body, cpDataPointer userData); - -/// Set the callback used to update a body's velocity. -CP_EXPORT void cpBodySetVelocityUpdateFunc(cpBody *body, cpBodyVelocityFunc velocityFunc); -/// Set the callback used to update a body's position. -/// NOTE: It's not generally recommended to override this unless you call the default position update function. -CP_EXPORT void cpBodySetPositionUpdateFunc(cpBody *body, cpBodyPositionFunc positionFunc); - -/// Default velocity integration function.. -CP_EXPORT void cpBodyUpdateVelocity(cpBody *body, cpVect gravity, cpFloat damping, cpFloat dt); -/// Default position integration function. -CP_EXPORT void cpBodyUpdatePosition(cpBody *body, cpFloat dt); - -/// Convert body relative/local coordinates to absolute/world coordinates. -CP_EXPORT cpVect cpBodyLocalToWorld(const cpBody *body, const cpVect point); -/// Convert body absolute/world coordinates to relative/local coordinates. -CP_EXPORT cpVect cpBodyWorldToLocal(const cpBody *body, const cpVect point); - -/// Apply a force to a body. Both the force and point are expressed in world coordinates. -CP_EXPORT void cpBodyApplyForceAtWorldPoint(cpBody *body, cpVect force, cpVect point); -/// Apply a force to a body. Both the force and point are expressed in body local coordinates. -CP_EXPORT void cpBodyApplyForceAtLocalPoint(cpBody *body, cpVect force, cpVect point); - -/// Apply an impulse to a body. Both the impulse and point are expressed in world coordinates. -CP_EXPORT void cpBodyApplyImpulseAtWorldPoint(cpBody *body, cpVect impulse, cpVect point); -/// Apply an impulse to a body. Both the impulse and point are expressed in body local coordinates. -CP_EXPORT void cpBodyApplyImpulseAtLocalPoint(cpBody *body, cpVect impulse, cpVect point); - -/// Get the velocity on a body (in world units) at a point on the body in world coordinates. -CP_EXPORT cpVect cpBodyGetVelocityAtWorldPoint(const cpBody *body, cpVect point); -/// Get the velocity on a body (in world units) at a point on the body in local coordinates. -CP_EXPORT cpVect cpBodyGetVelocityAtLocalPoint(const cpBody *body, cpVect point); - -/// Get the amount of kinetic energy contained by the body. -CP_EXPORT cpFloat cpBodyKineticEnergy(const cpBody *body); - -/// Body/shape iterator callback function type. -typedef void (*cpBodyShapeIteratorFunc)(cpBody *body, cpShape *shape, void *data); -/// Call @c func once for each shape attached to @c body and added to the space. -CP_EXPORT void cpBodyEachShape(cpBody *body, cpBodyShapeIteratorFunc func, void *data); - -/// Body/constraint iterator callback function type. -typedef void (*cpBodyConstraintIteratorFunc)(cpBody *body, cpConstraint *constraint, void *data); -/// Call @c func once for each constraint attached to @c body and added to the space. -CP_EXPORT void cpBodyEachConstraint(cpBody *body, cpBodyConstraintIteratorFunc func, void *data); - -/// Body/arbiter iterator callback function type. -typedef void (*cpBodyArbiterIteratorFunc)(cpBody *body, cpArbiter *arbiter, void *data); -/// Call @c func once for each arbiter that is currently active on the body. -CP_EXPORT void cpBodyEachArbiter(cpBody *body, cpBodyArbiterIteratorFunc func, void *data); - -///@} diff --git a/3rdparty/chipmunk/include/chipmunk/cpConstraint.h b/3rdparty/chipmunk/include/chipmunk/cpConstraint.h deleted file mode 100644 index b1a439f7be24..000000000000 --- a/3rdparty/chipmunk/include/chipmunk/cpConstraint.h +++ /dev/null @@ -1,95 +0,0 @@ -/* Copyright (c) 2013 Scott Lembcke and Howling Moon Software - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/// @defgroup cpConstraint cpConstraint -/// @{ - -/// Callback function type that gets called before solving a joint. -typedef void (*cpConstraintPreSolveFunc)(cpConstraint *constraint, cpSpace *space); -/// Callback function type that gets called after solving a joint. -typedef void (*cpConstraintPostSolveFunc)(cpConstraint *constraint, cpSpace *space); - -/// Destroy a constraint. -CP_EXPORT void cpConstraintDestroy(cpConstraint *constraint); -/// Destroy and free a constraint. -CP_EXPORT void cpConstraintFree(cpConstraint *constraint); - -/// Get the cpSpace this constraint is added to. -CP_EXPORT cpSpace* cpConstraintGetSpace(const cpConstraint *constraint); - -/// Get the first body the constraint is attached to. -CP_EXPORT cpBody* cpConstraintGetBodyA(const cpConstraint *constraint); - -/// Get the second body the constraint is attached to. -CP_EXPORT cpBody* cpConstraintGetBodyB(const cpConstraint *constraint); - -/// Get the maximum force that this constraint is allowed to use. -CP_EXPORT cpFloat cpConstraintGetMaxForce(const cpConstraint *constraint); -/// Set the maximum force that this constraint is allowed to use. (defaults to INFINITY) -CP_EXPORT void cpConstraintSetMaxForce(cpConstraint *constraint, cpFloat maxForce); - -/// Get rate at which joint error is corrected. -CP_EXPORT cpFloat cpConstraintGetErrorBias(const cpConstraint *constraint); -/// Set rate at which joint error is corrected. -/// Defaults to pow(1.0 - 0.1, 60.0) meaning that it will -/// correct 10% of the error every 1/60th of a second. -CP_EXPORT void cpConstraintSetErrorBias(cpConstraint *constraint, cpFloat errorBias); - -/// Get the maximum rate at which joint error is corrected. -CP_EXPORT cpFloat cpConstraintGetMaxBias(const cpConstraint *constraint); -/// Set the maximum rate at which joint error is corrected. (defaults to INFINITY) -CP_EXPORT void cpConstraintSetMaxBias(cpConstraint *constraint, cpFloat maxBias); - -/// Get if the two bodies connected by the constraint are allowed to collide or not. -CP_EXPORT cpBool cpConstraintGetCollideBodies(const cpConstraint *constraint); -/// Set if the two bodies connected by the constraint are allowed to collide or not. (defaults to cpFalse) -CP_EXPORT void cpConstraintSetCollideBodies(cpConstraint *constraint, cpBool collideBodies); - -/// Get the pre-solve function that is called before the solver runs. -CP_EXPORT cpConstraintPreSolveFunc cpConstraintGetPreSolveFunc(const cpConstraint *constraint); -/// Set the pre-solve function that is called before the solver runs. -CP_EXPORT void cpConstraintSetPreSolveFunc(cpConstraint *constraint, cpConstraintPreSolveFunc preSolveFunc); - -/// Get the post-solve function that is called before the solver runs. -CP_EXPORT cpConstraintPostSolveFunc cpConstraintGetPostSolveFunc(const cpConstraint *constraint); -/// Set the post-solve function that is called before the solver runs. -CP_EXPORT void cpConstraintSetPostSolveFunc(cpConstraint *constraint, cpConstraintPostSolveFunc postSolveFunc); - -/// Get the user definable data pointer for this constraint -CP_EXPORT cpDataPointer cpConstraintGetUserData(const cpConstraint *constraint); -/// Set the user definable data pointer for this constraint -CP_EXPORT void cpConstraintSetUserData(cpConstraint *constraint, cpDataPointer userData); - -/// Get the last impulse applied by this constraint. -CP_EXPORT cpFloat cpConstraintGetImpulse(cpConstraint *constraint); - -#include "cpPinJoint.h" -#include "cpSlideJoint.h" -#include "cpPivotJoint.h" -#include "cpGrooveJoint.h" -#include "cpDampedSpring.h" -#include "cpDampedRotarySpring.h" -#include "cpRotaryLimitJoint.h" -#include "cpRatchetJoint.h" -#include "cpGearJoint.h" -#include "cpSimpleMotor.h" - -///@} diff --git a/3rdparty/chipmunk/include/chipmunk/cpDampedRotarySpring.h b/3rdparty/chipmunk/include/chipmunk/cpDampedRotarySpring.h deleted file mode 100644 index 6f60e86e301a..000000000000 --- a/3rdparty/chipmunk/include/chipmunk/cpDampedRotarySpring.h +++ /dev/null @@ -1,58 +0,0 @@ -/* Copyright (c) 2013 Scott Lembcke and Howling Moon Software - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/// @defgroup cpDampedRotarySpring cpDampedRotarySpring -/// @{ - -/// Check if a constraint is a damped rotary springs. -CP_EXPORT cpBool cpConstraintIsDampedRotarySpring(const cpConstraint *constraint); - -/// Function type used for damped rotary spring force callbacks. -typedef cpFloat (*cpDampedRotarySpringTorqueFunc)(struct cpConstraint *spring, cpFloat relativeAngle); - -/// Allocate a damped rotary spring. -CP_EXPORT cpDampedRotarySpring* cpDampedRotarySpringAlloc(void); -/// Initialize a damped rotary spring. -CP_EXPORT cpDampedRotarySpring* cpDampedRotarySpringInit(cpDampedRotarySpring *joint, cpBody *a, cpBody *b, cpFloat restAngle, cpFloat stiffness, cpFloat damping); -/// Allocate and initialize a damped rotary spring. -CP_EXPORT cpConstraint* cpDampedRotarySpringNew(cpBody *a, cpBody *b, cpFloat restAngle, cpFloat stiffness, cpFloat damping); - -/// Get the rest length of the spring. -CP_EXPORT cpFloat cpDampedRotarySpringGetRestAngle(const cpConstraint *constraint); -/// Set the rest length of the spring. -CP_EXPORT void cpDampedRotarySpringSetRestAngle(cpConstraint *constraint, cpFloat restAngle); - -/// Get the stiffness of the spring in force/distance. -CP_EXPORT cpFloat cpDampedRotarySpringGetStiffness(const cpConstraint *constraint); -/// Set the stiffness of the spring in force/distance. -CP_EXPORT void cpDampedRotarySpringSetStiffness(cpConstraint *constraint, cpFloat stiffness); - -/// Get the damping of the spring. -CP_EXPORT cpFloat cpDampedRotarySpringGetDamping(const cpConstraint *constraint); -/// Set the damping of the spring. -CP_EXPORT void cpDampedRotarySpringSetDamping(cpConstraint *constraint, cpFloat damping); - -/// Get the damping of the spring. -CP_EXPORT cpDampedRotarySpringTorqueFunc cpDampedRotarySpringGetSpringTorqueFunc(const cpConstraint *constraint); -/// Set the damping of the spring. -CP_EXPORT void cpDampedRotarySpringSetSpringTorqueFunc(cpConstraint *constraint, cpDampedRotarySpringTorqueFunc springTorqueFunc); - -/// @} diff --git a/3rdparty/chipmunk/include/chipmunk/cpDampedSpring.h b/3rdparty/chipmunk/include/chipmunk/cpDampedSpring.h deleted file mode 100644 index b332fc7f02dc..000000000000 --- a/3rdparty/chipmunk/include/chipmunk/cpDampedSpring.h +++ /dev/null @@ -1,68 +0,0 @@ -/* Copyright (c) 2013 Scott Lembcke and Howling Moon Software - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/// @defgroup cpDampedSpring cpDampedSpring -/// @{ - -/// Check if a constraint is a slide joint. -CP_EXPORT cpBool cpConstraintIsDampedSpring(const cpConstraint *constraint); - -/// Function type used for damped spring force callbacks. -typedef cpFloat (*cpDampedSpringForceFunc)(cpConstraint *spring, cpFloat dist); - -/// Allocate a damped spring. -CP_EXPORT cpDampedSpring* cpDampedSpringAlloc(void); -/// Initialize a damped spring. -CP_EXPORT cpDampedSpring* cpDampedSpringInit(cpDampedSpring *joint, cpBody *a, cpBody *b, cpVect anchorA, cpVect anchorB, cpFloat restLength, cpFloat stiffness, cpFloat damping); -/// Allocate and initialize a damped spring. -CP_EXPORT cpConstraint* cpDampedSpringNew(cpBody *a, cpBody *b, cpVect anchorA, cpVect anchorB, cpFloat restLength, cpFloat stiffness, cpFloat damping); - -/// Get the location of the first anchor relative to the first body. -CP_EXPORT cpVect cpDampedSpringGetAnchorA(const cpConstraint *constraint); -/// Set the location of the first anchor relative to the first body. -CP_EXPORT void cpDampedSpringSetAnchorA(cpConstraint *constraint, cpVect anchorA); - -/// Get the location of the second anchor relative to the second body. -CP_EXPORT cpVect cpDampedSpringGetAnchorB(const cpConstraint *constraint); -/// Set the location of the second anchor relative to the second body. -CP_EXPORT void cpDampedSpringSetAnchorB(cpConstraint *constraint, cpVect anchorB); - -/// Get the rest length of the spring. -CP_EXPORT cpFloat cpDampedSpringGetRestLength(const cpConstraint *constraint); -/// Set the rest length of the spring. -CP_EXPORT void cpDampedSpringSetRestLength(cpConstraint *constraint, cpFloat restLength); - -/// Get the stiffness of the spring in force/distance. -CP_EXPORT cpFloat cpDampedSpringGetStiffness(const cpConstraint *constraint); -/// Set the stiffness of the spring in force/distance. -CP_EXPORT void cpDampedSpringSetStiffness(cpConstraint *constraint, cpFloat stiffness); - -/// Get the damping of the spring. -CP_EXPORT cpFloat cpDampedSpringGetDamping(const cpConstraint *constraint); -/// Set the damping of the spring. -CP_EXPORT void cpDampedSpringSetDamping(cpConstraint *constraint, cpFloat damping); - -/// Get the damping of the spring. -CP_EXPORT cpDampedSpringForceFunc cpDampedSpringGetSpringForceFunc(const cpConstraint *constraint); -/// Set the damping of the spring. -CP_EXPORT void cpDampedSpringSetSpringForceFunc(cpConstraint *constraint, cpDampedSpringForceFunc springForceFunc); - -/// @} diff --git a/3rdparty/chipmunk/include/chipmunk/cpGearJoint.h b/3rdparty/chipmunk/include/chipmunk/cpGearJoint.h deleted file mode 100644 index 8cd80e0b906c..000000000000 --- a/3rdparty/chipmunk/include/chipmunk/cpGearJoint.h +++ /dev/null @@ -1,45 +0,0 @@ -/* Copyright (c) 2013 Scott Lembcke and Howling Moon Software - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/// @defgroup cpGearJoint cpGearJoint -/// @{ - -/// Check if a constraint is a damped rotary springs. -CP_EXPORT cpBool cpConstraintIsGearJoint(const cpConstraint *constraint); - -/// Allocate a gear joint. -CP_EXPORT cpGearJoint* cpGearJointAlloc(void); -/// Initialize a gear joint. -CP_EXPORT cpGearJoint* cpGearJointInit(cpGearJoint *joint, cpBody *a, cpBody *b, cpFloat phase, cpFloat ratio); -/// Allocate and initialize a gear joint. -CP_EXPORT cpConstraint* cpGearJointNew(cpBody *a, cpBody *b, cpFloat phase, cpFloat ratio); - -/// Get the phase offset of the gears. -CP_EXPORT cpFloat cpGearJointGetPhase(const cpConstraint *constraint); -/// Set the phase offset of the gears. -CP_EXPORT void cpGearJointSetPhase(cpConstraint *constraint, cpFloat phase); - -/// Get the angular distance of each ratchet. -CP_EXPORT cpFloat cpGearJointGetRatio(const cpConstraint *constraint); -/// Set the ratio of a gear joint. -CP_EXPORT void cpGearJointSetRatio(cpConstraint *constraint, cpFloat ratio); - -/// @} diff --git a/3rdparty/chipmunk/include/chipmunk/cpGrooveJoint.h b/3rdparty/chipmunk/include/chipmunk/cpGrooveJoint.h deleted file mode 100644 index 8bdafc14aa2f..000000000000 --- a/3rdparty/chipmunk/include/chipmunk/cpGrooveJoint.h +++ /dev/null @@ -1,50 +0,0 @@ -/* Copyright (c) 2013 Scott Lembcke and Howling Moon Software - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/// @defgroup cpGrooveJoint cpGrooveJoint -/// @{ - -/// Check if a constraint is a slide joint. -CP_EXPORT cpBool cpConstraintIsGrooveJoint(const cpConstraint *constraint); - -/// Allocate a groove joint. -CP_EXPORT cpGrooveJoint* cpGrooveJointAlloc(void); -/// Initialize a groove joint. -CP_EXPORT cpGrooveJoint* cpGrooveJointInit(cpGrooveJoint *joint, cpBody *a, cpBody *b, cpVect groove_a, cpVect groove_b, cpVect anchorB); -/// Allocate and initialize a groove joint. -CP_EXPORT cpConstraint* cpGrooveJointNew(cpBody *a, cpBody *b, cpVect groove_a, cpVect groove_b, cpVect anchorB); - -/// Get the first endpoint of the groove relative to the first body. -CP_EXPORT cpVect cpGrooveJointGetGrooveA(const cpConstraint *constraint); -/// Set the first endpoint of the groove relative to the first body. -CP_EXPORT void cpGrooveJointSetGrooveA(cpConstraint *constraint, cpVect grooveA); - -/// Get the first endpoint of the groove relative to the first body. -CP_EXPORT cpVect cpGrooveJointGetGrooveB(const cpConstraint *constraint); -/// Set the first endpoint of the groove relative to the first body. -CP_EXPORT void cpGrooveJointSetGrooveB(cpConstraint *constraint, cpVect grooveB); - -/// Get the location of the second anchor relative to the second body. -CP_EXPORT cpVect cpGrooveJointGetAnchorB(const cpConstraint *constraint); -/// Set the location of the second anchor relative to the second body. -CP_EXPORT void cpGrooveJointSetAnchorB(cpConstraint *constraint, cpVect anchorB); - -/// @} diff --git a/3rdparty/chipmunk/include/chipmunk/cpHastySpace.h b/3rdparty/chipmunk/include/chipmunk/cpHastySpace.h deleted file mode 100644 index 6de2283b9fb7..000000000000 --- a/3rdparty/chipmunk/include/chipmunk/cpHastySpace.h +++ /dev/null @@ -1,27 +0,0 @@ -// Copyright 2013 Howling Moon Software. All rights reserved. -// See http://chipmunk2d.net/legal.php for more information. - -/// cpHastySpace is exclusive to Chipmunk Pro -/// Currently it enables ARM NEON optimizations in the solver, but in the future will include other optimizations such as -/// a multi-threaded solver and multi-threaded collision broadphases. - -struct cpHastySpace; -typedef struct cpHastySpace cpHastySpace; - -/// Create a new hasty space. -/// On ARM platforms that support NEON, this will enable the vectorized solver. -/// cpHastySpace also supports multiple threads, but runs single threaded by default for determinism. -CP_EXPORT cpSpace *cpHastySpaceNew(void); -CP_EXPORT void cpHastySpaceFree(cpSpace *space); - -/// Set the number of threads to use for the solver. -/// Currently Chipmunk is limited to 2 threads as using more generally provides very minimal performance gains. -/// Passing 0 as the thread count on iOS or OS X will cause Chipmunk to automatically detect the number of threads it should use. -/// On other platforms passing 0 for the thread count will set 1 thread. -CP_EXPORT void cpHastySpaceSetThreads(cpSpace *space, unsigned long threads); - -/// Returns the number of threads the solver is using to run. -CP_EXPORT unsigned long cpHastySpaceGetThreads(cpSpace *space); - -/// When stepping a hasty space, you must use this function. -CP_EXPORT void cpHastySpaceStep(cpSpace *space, cpFloat dt); diff --git a/3rdparty/chipmunk/include/chipmunk/cpMarch.h b/3rdparty/chipmunk/include/chipmunk/cpMarch.h deleted file mode 100644 index cc1f5c061d1f..000000000000 --- a/3rdparty/chipmunk/include/chipmunk/cpMarch.h +++ /dev/null @@ -1,28 +0,0 @@ -// Copyright 2013 Howling Moon Software. All rights reserved. -// See http://chipmunk2d.net/legal.php for more information. - -/// Function type used as a callback from the marching squares algorithm to sample an image function. -/// It passes you the point to sample and your context pointer, and you return the density. -typedef cpFloat (*cpMarchSampleFunc)(cpVect point, void *data); - -/// Function type used as a callback from the marching squares algorithm to output a line segment. -/// It passes you the two endpoints and your context pointer. -typedef void (*cpMarchSegmentFunc)(cpVect v0, cpVect v1, void *data); - -/// Trace an anti-aliased contour of an image along a particular threshold. -/// The given number of samples will be taken and spread across the bounding box area using the sampling function and context. -/// The segment function will be called for each segment detected that lies along the density contour for @c threshold. -CP_EXPORT void cpMarchSoft( - cpBB bb, unsigned long x_samples, unsigned long y_samples, cpFloat threshold, - cpMarchSegmentFunc segment, void *segment_data, - cpMarchSampleFunc sample, void *sample_data -); - -/// Trace an aliased curve of an image along a particular threshold. -/// The given number of samples will be taken and spread across the bounding box area using the sampling function and context. -/// The segment function will be called for each segment detected that lies along the density contour for @c threshold. -CP_EXPORT void cpMarchHard( - cpBB bb, unsigned long x_samples, unsigned long y_samples, cpFloat threshold, - cpMarchSegmentFunc segment, void *segment_data, - cpMarchSampleFunc sample, void *sample_data -); diff --git a/3rdparty/chipmunk/include/chipmunk/cpPinJoint.h b/3rdparty/chipmunk/include/chipmunk/cpPinJoint.h deleted file mode 100644 index 45aaa3e333da..000000000000 --- a/3rdparty/chipmunk/include/chipmunk/cpPinJoint.h +++ /dev/null @@ -1,50 +0,0 @@ -/* Copyright (c) 2013 Scott Lembcke and Howling Moon Software - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/// @defgroup cpPinJoint cpPinJoint -/// @{ - -/// Check if a constraint is a pin joint. -CP_EXPORT cpBool cpConstraintIsPinJoint(const cpConstraint *constraint); - -/// Allocate a pin joint. -CP_EXPORT cpPinJoint* cpPinJointAlloc(void); -/// Initialize a pin joint. -CP_EXPORT cpPinJoint* cpPinJointInit(cpPinJoint *joint, cpBody *a, cpBody *b, cpVect anchorA, cpVect anchorB); -/// Allocate and initialize a pin joint. -CP_EXPORT cpConstraint* cpPinJointNew(cpBody *a, cpBody *b, cpVect anchorA, cpVect anchorB); - -/// Get the location of the first anchor relative to the first body. -CP_EXPORT cpVect cpPinJointGetAnchorA(const cpConstraint *constraint); -/// Set the location of the first anchor relative to the first body. -CP_EXPORT void cpPinJointSetAnchorA(cpConstraint *constraint, cpVect anchorA); - -/// Get the location of the second anchor relative to the second body. -CP_EXPORT cpVect cpPinJointGetAnchorB(const cpConstraint *constraint); -/// Set the location of the second anchor relative to the second body. -CP_EXPORT void cpPinJointSetAnchorB(cpConstraint *constraint, cpVect anchorB); - -/// Get the distance the joint will maintain between the two anchors. -CP_EXPORT cpFloat cpPinJointGetDist(const cpConstraint *constraint); -/// Set the distance the joint will maintain between the two anchors. -CP_EXPORT void cpPinJointSetDist(cpConstraint *constraint, cpFloat dist); - -///@} diff --git a/3rdparty/chipmunk/include/chipmunk/cpPivotJoint.h b/3rdparty/chipmunk/include/chipmunk/cpPivotJoint.h deleted file mode 100644 index 4a620ef2553d..000000000000 --- a/3rdparty/chipmunk/include/chipmunk/cpPivotJoint.h +++ /dev/null @@ -1,47 +0,0 @@ -/* Copyright (c) 2013 Scott Lembcke and Howling Moon Software - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/// @defgroup cpPivotJoint cpPivotJoint -/// @{ - -/// Check if a constraint is a slide joint. -CP_EXPORT cpBool cpConstraintIsPivotJoint(const cpConstraint *constraint); - -/// Allocate a pivot joint -CP_EXPORT cpPivotJoint* cpPivotJointAlloc(void); -/// Initialize a pivot joint. -CP_EXPORT cpPivotJoint* cpPivotJointInit(cpPivotJoint *joint, cpBody *a, cpBody *b, cpVect anchorA, cpVect anchorB); -/// Allocate and initialize a pivot joint. -CP_EXPORT cpConstraint* cpPivotJointNew(cpBody *a, cpBody *b, cpVect pivot); -/// Allocate and initialize a pivot joint with specific anchors. -CP_EXPORT cpConstraint* cpPivotJointNew2(cpBody *a, cpBody *b, cpVect anchorA, cpVect anchorB); - -/// Get the location of the first anchor relative to the first body. -CP_EXPORT cpVect cpPivotJointGetAnchorA(const cpConstraint *constraint); -/// Set the location of the first anchor relative to the first body. -CP_EXPORT void cpPivotJointSetAnchorA(cpConstraint *constraint, cpVect anchorA); - -/// Get the location of the second anchor relative to the second body. -CP_EXPORT cpVect cpPivotJointGetAnchorB(const cpConstraint *constraint); -/// Set the location of the second anchor relative to the second body. -CP_EXPORT void cpPivotJointSetAnchorB(cpConstraint *constraint, cpVect anchorB); - -/// @} diff --git a/3rdparty/chipmunk/include/chipmunk/cpPolyShape.h b/3rdparty/chipmunk/include/chipmunk/cpPolyShape.h deleted file mode 100644 index 25f688b896ae..000000000000 --- a/3rdparty/chipmunk/include/chipmunk/cpPolyShape.h +++ /dev/null @@ -1,56 +0,0 @@ -/* Copyright (c) 2013 Scott Lembcke and Howling Moon Software - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/// @defgroup cpPolyShape cpPolyShape -/// @{ - -/// Allocate a polygon shape. -CP_EXPORT cpPolyShape* cpPolyShapeAlloc(void); -/// Initialize a polygon shape with rounded corners. -/// A convex hull will be created from the vertexes. -CP_EXPORT cpPolyShape* cpPolyShapeInit(cpPolyShape *poly, cpBody *body, int count, const cpVect *verts, cpTransform transform, cpFloat radius); -/// Initialize a polygon shape with rounded corners. -/// The vertexes must be convex with a counter-clockwise winding. -CP_EXPORT cpPolyShape* cpPolyShapeInitRaw(cpPolyShape *poly, cpBody *body, int count, const cpVect *verts, cpFloat radius); -/// Allocate and initialize a polygon shape with rounded corners. -/// A convex hull will be created from the vertexes. -CP_EXPORT cpShape* cpPolyShapeNew(cpBody *body, int count, const cpVect *verts, cpTransform transform, cpFloat radius); -/// Allocate and initialize a polygon shape with rounded corners. -/// The vertexes must be convex with a counter-clockwise winding. -CP_EXPORT cpShape* cpPolyShapeNewRaw(cpBody *body, int count, const cpVect *verts, cpFloat radius); - -/// Initialize a box shaped polygon shape with rounded corners. -CP_EXPORT cpPolyShape* cpBoxShapeInit(cpPolyShape *poly, cpBody *body, cpFloat width, cpFloat height, cpFloat radius); -/// Initialize an offset box shaped polygon shape with rounded corners. -CP_EXPORT cpPolyShape* cpBoxShapeInit2(cpPolyShape *poly, cpBody *body, cpBB box, cpFloat radius); -/// Allocate and initialize a box shaped polygon shape. -CP_EXPORT cpShape* cpBoxShapeNew(cpBody *body, cpFloat width, cpFloat height, cpFloat radius); -/// Allocate and initialize an offset box shaped polygon shape. -CP_EXPORT cpShape* cpBoxShapeNew2(cpBody *body, cpBB box, cpFloat radius); - -/// Get the number of verts in a polygon shape. -CP_EXPORT int cpPolyShapeGetCount(const cpShape *shape); -/// Get the @c ith vertex of a polygon shape. -CP_EXPORT cpVect cpPolyShapeGetVert(const cpShape *shape, int index); -/// Get the radius of a polygon shape. -CP_EXPORT cpFloat cpPolyShapeGetRadius(const cpShape *shape); - -/// @} diff --git a/3rdparty/chipmunk/include/chipmunk/cpPolyline.h b/3rdparty/chipmunk/include/chipmunk/cpPolyline.h deleted file mode 100644 index 9a6ebed3caa1..000000000000 --- a/3rdparty/chipmunk/include/chipmunk/cpPolyline.h +++ /dev/null @@ -1,70 +0,0 @@ -// Copyright 2013 Howling Moon Software. All rights reserved. -// See http://chipmunk2d.net/legal.php for more information. - -// Polylines are just arrays of vertexes. -// They are looped if the first vertex is equal to the last. -// cpPolyline structs are intended to be passed by value and destroyed when you are done with them. -typedef struct cpPolyline { - int count, capacity; - cpVect verts[]; -} cpPolyline; - -/// Destroy and free a polyline instance. -CP_EXPORT void cpPolylineFree(cpPolyline *line); - -/// Returns true if the first vertex is equal to the last. -CP_EXPORT cpBool cpPolylineIsClosed(cpPolyline *line); - -/** - Returns a copy of a polyline simplified by using the Douglas-Peucker algorithm. - This works very well on smooth or gently curved shapes, but not well on straight edged or angular shapes. -*/ -CP_EXPORT cpPolyline *cpPolylineSimplifyCurves(cpPolyline *line, cpFloat tol); - -/** - Returns a copy of a polyline simplified by discarding "flat" vertexes. - This works well on straight edged or angular shapes, not as well on smooth shapes. -*/ -CP_EXPORT cpPolyline *cpPolylineSimplifyVertexes(cpPolyline *line, cpFloat tol); - -/// Get the convex hull of a polyline as a looped polyline. -CP_EXPORT cpPolyline *cpPolylineToConvexHull(cpPolyline *line, cpFloat tol); - - -/// Polyline sets are collections of polylines, generally built by cpMarchSoft() or cpMarchHard(). -typedef struct cpPolylineSet { - int count, capacity; - cpPolyline **lines; -} cpPolylineSet; - -/// Allocate a new polyline set. -CP_EXPORT cpPolylineSet *cpPolylineSetAlloc(void); - -/// Initialize a new polyline set. -CP_EXPORT cpPolylineSet *cpPolylineSetInit(cpPolylineSet *set); - -/// Allocate and initialize a polyline set. -CP_EXPORT cpPolylineSet *cpPolylineSetNew(void); - -/// Destroy a polyline set. -CP_EXPORT void cpPolylineSetDestroy(cpPolylineSet *set, cpBool freePolylines); - -/// Destroy and free a polyline set. -CP_EXPORT void cpPolylineSetFree(cpPolylineSet *set, cpBool freePolylines); - -/** - Add a line segment to a polyline set. - A segment will either start a new polyline, join two others, or add to or loop an existing polyline. - This is mostly intended to be used as a callback directly from cpMarchSoft() or cpMarchHard(). -*/ -CP_EXPORT void cpPolylineSetCollectSegment(cpVect v0, cpVect v1, cpPolylineSet *lines); - -/** - Get an approximate convex decomposition from a polyline. - Returns a cpPolylineSet of convex hulls that match the original shape to within 'tol'. - NOTE: If the input is a self intersecting polygon, the output might end up overly simplified. -*/ - -CP_EXPORT cpPolylineSet *cpPolylineConvexDecomposition(cpPolyline *line, cpFloat tol); - -#define cpPolylineConvexDecomposition_BETA cpPolylineConvexDecomposition diff --git a/3rdparty/chipmunk/include/chipmunk/cpRatchetJoint.h b/3rdparty/chipmunk/include/chipmunk/cpRatchetJoint.h deleted file mode 100644 index 3ed4c915ee75..000000000000 --- a/3rdparty/chipmunk/include/chipmunk/cpRatchetJoint.h +++ /dev/null @@ -1,50 +0,0 @@ -/* Copyright (c) 2013 Scott Lembcke and Howling Moon Software - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/// @defgroup cpRatchetJoint cpRatchetJoint -/// @{ - -/// Check if a constraint is a damped rotary springs. -CP_EXPORT cpBool cpConstraintIsRatchetJoint(const cpConstraint *constraint); - -/// Allocate a ratchet joint. -CP_EXPORT cpRatchetJoint* cpRatchetJointAlloc(void); -/// Initialize a ratched joint. -CP_EXPORT cpRatchetJoint* cpRatchetJointInit(cpRatchetJoint *joint, cpBody *a, cpBody *b, cpFloat phase, cpFloat ratchet); -/// Allocate and initialize a ratchet joint. -CP_EXPORT cpConstraint* cpRatchetJointNew(cpBody *a, cpBody *b, cpFloat phase, cpFloat ratchet); - -/// Get the angle of the current ratchet tooth. -CP_EXPORT cpFloat cpRatchetJointGetAngle(const cpConstraint *constraint); -/// Set the angle of the current ratchet tooth. -CP_EXPORT void cpRatchetJointSetAngle(cpConstraint *constraint, cpFloat angle); - -/// Get the phase offset of the ratchet. -CP_EXPORT cpFloat cpRatchetJointGetPhase(const cpConstraint *constraint); -/// Get the phase offset of the ratchet. -CP_EXPORT void cpRatchetJointSetPhase(cpConstraint *constraint, cpFloat phase); - -/// Get the angular distance of each ratchet. -CP_EXPORT cpFloat cpRatchetJointGetRatchet(const cpConstraint *constraint); -/// Set the angular distance of each ratchet. -CP_EXPORT void cpRatchetJointSetRatchet(cpConstraint *constraint, cpFloat ratchet); - -/// @} diff --git a/3rdparty/chipmunk/include/chipmunk/cpRobust.h b/3rdparty/chipmunk/include/chipmunk/cpRobust.h deleted file mode 100644 index e4b2c42082e3..000000000000 --- a/3rdparty/chipmunk/include/chipmunk/cpRobust.h +++ /dev/null @@ -1,11 +0,0 @@ -#include "chipmunk/cpVect.h" - -// This is a private header for functions (currently just one) that need strict floating point results. -// It was easier to put this in it's own file than to fiddle with 4 different compiler specific pragmas or attributes. -// "Fast math" should be disabled here. - -// Check if c is to the left of segment (a, b). -cpBool cpCheckPointGreater(const cpVect a, const cpVect b, const cpVect c); - -// Check if p is behind one of v0 or v1 on axis n. -cpBool cpCheckAxis(cpVect v0, cpVect v1, cpVect p, cpVect n); diff --git a/3rdparty/chipmunk/include/chipmunk/cpRotaryLimitJoint.h b/3rdparty/chipmunk/include/chipmunk/cpRotaryLimitJoint.h deleted file mode 100644 index fac7ad859166..000000000000 --- a/3rdparty/chipmunk/include/chipmunk/cpRotaryLimitJoint.h +++ /dev/null @@ -1,45 +0,0 @@ -/* Copyright (c) 2013 Scott Lembcke and Howling Moon Software - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/// @defgroup cpRotaryLimitJoint cpRotaryLimitJoint -/// @{ - -/// Check if a constraint is a damped rotary springs. -CP_EXPORT cpBool cpConstraintIsRotaryLimitJoint(const cpConstraint *constraint); - -/// Allocate a damped rotary limit joint. -CP_EXPORT cpRotaryLimitJoint* cpRotaryLimitJointAlloc(void); -/// Initialize a damped rotary limit joint. -CP_EXPORT cpRotaryLimitJoint* cpRotaryLimitJointInit(cpRotaryLimitJoint *joint, cpBody *a, cpBody *b, cpFloat min, cpFloat max); -/// Allocate and initialize a damped rotary limit joint. -CP_EXPORT cpConstraint* cpRotaryLimitJointNew(cpBody *a, cpBody *b, cpFloat min, cpFloat max); - -/// Get the minimum distance the joint will maintain between the two anchors. -CP_EXPORT cpFloat cpRotaryLimitJointGetMin(const cpConstraint *constraint); -/// Set the minimum distance the joint will maintain between the two anchors. -CP_EXPORT void cpRotaryLimitJointSetMin(cpConstraint *constraint, cpFloat min); - -/// Get the maximum distance the joint will maintain between the two anchors. -CP_EXPORT cpFloat cpRotaryLimitJointGetMax(const cpConstraint *constraint); -/// Set the maximum distance the joint will maintain between the two anchors. -CP_EXPORT void cpRotaryLimitJointSetMax(cpConstraint *constraint, cpFloat max); - -/// @} diff --git a/3rdparty/chipmunk/include/chipmunk/cpShape.h b/3rdparty/chipmunk/include/chipmunk/cpShape.h deleted file mode 100644 index c78ed05d092b..000000000000 --- a/3rdparty/chipmunk/include/chipmunk/cpShape.h +++ /dev/null @@ -1,199 +0,0 @@ -/* Copyright (c) 2013 Scott Lembcke and Howling Moon Software - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/// @defgroup cpShape cpShape -/// The cpShape struct defines the shape of a rigid body. -/// @{ - -/// Point query info struct. -typedef struct cpPointQueryInfo { - /// The nearest shape, NULL if no shape was within range. - const cpShape *shape; - /// The closest point on the shape's surface. (in world space coordinates) - cpVect point; - /// The distance to the point. The distance is negative if the point is inside the shape. - cpFloat distance; - /// The gradient of the signed distance function. - /// The value should be similar to info.p/info.d, but accurate even for very small values of info.d. - cpVect gradient; -} cpPointQueryInfo; - -/// Segment query info struct. -typedef struct cpSegmentQueryInfo { - /// The shape that was hit, or NULL if no collision occured. - const cpShape *shape; - /// The point of impact. - cpVect point; - /// The normal of the surface hit. - cpVect normal; - /// The normalized distance along the query segment in the range [0, 1]. - cpFloat alpha; -} cpSegmentQueryInfo; - -/// Fast collision filtering type that is used to determine if two objects collide before calling collision or query callbacks. -typedef struct cpShapeFilter { - /// Two objects with the same non-zero group value do not collide. - /// This is generally used to group objects in a composite object together to disable self collisions. - cpGroup group; - /// A bitmask of user definable categories that this object belongs to. - /// The category/mask combinations of both objects in a collision must agree for a collision to occur. - cpBitmask categories; - /// A bitmask of user definable category types that this object object collides with. - /// The category/mask combinations of both objects in a collision must agree for a collision to occur. - cpBitmask mask; -} cpShapeFilter; - -/// Collision filter value for a shape that will collide with anything except CP_SHAPE_FILTER_NONE. -static const cpShapeFilter CP_SHAPE_FILTER_ALL = {CP_NO_GROUP, CP_ALL_CATEGORIES, CP_ALL_CATEGORIES}; -/// Collision filter value for a shape that does not collide with anything. -static const cpShapeFilter CP_SHAPE_FILTER_NONE = {CP_NO_GROUP, ~CP_ALL_CATEGORIES, ~CP_ALL_CATEGORIES}; - -/// Create a new collision filter. -static inline cpShapeFilter -cpShapeFilterNew(cpGroup group, cpBitmask categories, cpBitmask mask) -{ - cpShapeFilter filter = {group, categories, mask}; - return filter; -} - -/// Destroy a shape. -CP_EXPORT void cpShapeDestroy(cpShape *shape); -/// Destroy and Free a shape. -CP_EXPORT void cpShapeFree(cpShape *shape); - -/// Update, cache and return the bounding box of a shape based on the body it's attached to. -CP_EXPORT cpBB cpShapeCacheBB(cpShape *shape); -/// Update, cache and return the bounding box of a shape with an explicit transformation. -CP_EXPORT cpBB cpShapeUpdate(cpShape *shape, cpTransform transform); - -/// Perform a nearest point query. It finds the closest point on the surface of shape to a specific point. -/// The value returned is the distance between the points. A negative distance means the point is inside the shape. -CP_EXPORT cpFloat cpShapePointQuery(const cpShape *shape, cpVect p, cpPointQueryInfo *out); - -/// Perform a segment query against a shape. @c info must be a pointer to a valid cpSegmentQueryInfo structure. -CP_EXPORT cpBool cpShapeSegmentQuery(const cpShape *shape, cpVect a, cpVect b, cpFloat radius, cpSegmentQueryInfo *info); - -/// Return contact information about two shapes. -CP_EXPORT cpContactPointSet cpShapesCollide(const cpShape *a, const cpShape *b); - -/// The cpSpace this body is added to. -CP_EXPORT cpSpace* cpShapeGetSpace(const cpShape *shape); - -/// The cpBody this shape is connected to. -CP_EXPORT cpBody* cpShapeGetBody(const cpShape *shape); -/// Set the cpBody this shape is connected to. -/// Can only be used if the shape is not currently added to a space. -CP_EXPORT void cpShapeSetBody(cpShape *shape, cpBody *body); - -/// Get the mass of the shape if you are having Chipmunk calculate mass properties for you. -CP_EXPORT cpFloat cpShapeGetMass(cpShape *shape); -/// Set the mass of this shape to have Chipmunk calculate mass properties for you. -CP_EXPORT void cpShapeSetMass(cpShape *shape, cpFloat mass); - -/// Get the density of the shape if you are having Chipmunk calculate mass properties for you. -CP_EXPORT cpFloat cpShapeGetDensity(cpShape *shape); -/// Set the density of this shape to have Chipmunk calculate mass properties for you. -CP_EXPORT void cpShapeSetDensity(cpShape *shape, cpFloat density); - -/// Get the calculated moment of inertia for this shape. -CP_EXPORT cpFloat cpShapeGetMoment(cpShape *shape); -/// Get the calculated area of this shape. -CP_EXPORT cpFloat cpShapeGetArea(cpShape *shape); -/// Get the centroid of this shape. -CP_EXPORT cpVect cpShapeGetCenterOfGravity(cpShape *shape); - -/// Get the bounding box that contains the shape given it's current position and angle. -CP_EXPORT cpBB cpShapeGetBB(const cpShape *shape); - -/// Get if the shape is set to be a sensor or not. -CP_EXPORT cpBool cpShapeGetSensor(const cpShape *shape); -/// Set if the shape is a sensor or not. -CP_EXPORT void cpShapeSetSensor(cpShape *shape, cpBool sensor); - -/// Get the elasticity of this shape. -CP_EXPORT cpFloat cpShapeGetElasticity(const cpShape *shape); -/// Set the elasticity of this shape. -CP_EXPORT void cpShapeSetElasticity(cpShape *shape, cpFloat elasticity); - -/// Get the friction of this shape. -CP_EXPORT cpFloat cpShapeGetFriction(const cpShape *shape); -/// Set the friction of this shape. -CP_EXPORT void cpShapeSetFriction(cpShape *shape, cpFloat friction); - -/// Get the surface velocity of this shape. -CP_EXPORT cpVect cpShapeGetSurfaceVelocity(const cpShape *shape); -/// Set the surface velocity of this shape. -CP_EXPORT void cpShapeSetSurfaceVelocity(cpShape *shape, cpVect surfaceVelocity); - -/// Get the user definable data pointer of this shape. -CP_EXPORT cpDataPointer cpShapeGetUserData(const cpShape *shape); -/// Set the user definable data pointer of this shape. -CP_EXPORT void cpShapeSetUserData(cpShape *shape, cpDataPointer userData); - -/// Set the collision type of this shape. -CP_EXPORT cpCollisionType cpShapeGetCollisionType(const cpShape *shape); -/// Get the collision type of this shape. -CP_EXPORT void cpShapeSetCollisionType(cpShape *shape, cpCollisionType collisionType); - -/// Get the collision filtering parameters of this shape. -CP_EXPORT cpShapeFilter cpShapeGetFilter(const cpShape *shape); -/// Set the collision filtering parameters of this shape. -CP_EXPORT void cpShapeSetFilter(cpShape *shape, cpShapeFilter filter); - - -/// @} -/// @defgroup cpCircleShape cpCircleShape - -/// Allocate a circle shape. -CP_EXPORT cpCircleShape* cpCircleShapeAlloc(void); -/// Initialize a circle shape. -CP_EXPORT cpCircleShape* cpCircleShapeInit(cpCircleShape *circle, cpBody *body, cpFloat radius, cpVect offset); -/// Allocate and initialize a circle shape. -CP_EXPORT cpShape* cpCircleShapeNew(cpBody *body, cpFloat radius, cpVect offset); - -/// Get the offset of a circle shape. -CP_EXPORT cpVect cpCircleShapeGetOffset(const cpShape *shape); -/// Get the radius of a circle shape. -CP_EXPORT cpFloat cpCircleShapeGetRadius(const cpShape *shape); - -/// @} -/// @defgroup cpSegmentShape cpSegmentShape - -/// Allocate a segment shape. -CP_EXPORT cpSegmentShape* cpSegmentShapeAlloc(void); -/// Initialize a segment shape. -CP_EXPORT cpSegmentShape* cpSegmentShapeInit(cpSegmentShape *seg, cpBody *body, cpVect a, cpVect b, cpFloat radius); -/// Allocate and initialize a segment shape. -CP_EXPORT cpShape* cpSegmentShapeNew(cpBody *body, cpVect a, cpVect b, cpFloat radius); - -/// Let Chipmunk know about the geometry of adjacent segments to avoid colliding with endcaps. -CP_EXPORT void cpSegmentShapeSetNeighbors(cpShape *shape, cpVect prev, cpVect next); - -/// Get the first endpoint of a segment shape. -CP_EXPORT cpVect cpSegmentShapeGetA(const cpShape *shape); -/// Get the second endpoint of a segment shape. -CP_EXPORT cpVect cpSegmentShapeGetB(const cpShape *shape); -/// Get the normal of a segment shape. -CP_EXPORT cpVect cpSegmentShapeGetNormal(const cpShape *shape); -/// Get the first endpoint of a segment shape. -CP_EXPORT cpFloat cpSegmentShapeGetRadius(const cpShape *shape); - -/// @} diff --git a/3rdparty/chipmunk/include/chipmunk/cpSimpleMotor.h b/3rdparty/chipmunk/include/chipmunk/cpSimpleMotor.h deleted file mode 100644 index 811b01143e25..000000000000 --- a/3rdparty/chipmunk/include/chipmunk/cpSimpleMotor.h +++ /dev/null @@ -1,43 +0,0 @@ -/* Copyright (c) 2013 Scott Lembcke and Howling Moon Software - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/// @defgroup cpSimpleMotor cpSimpleMotor -/// @{ - -/// Opaque struct type for damped rotary springs. -typedef struct cpSimpleMotor cpSimpleMotor; - -/// Check if a constraint is a damped rotary springs. -CP_EXPORT cpBool cpConstraintIsSimpleMotor(const cpConstraint *constraint); - -/// Allocate a simple motor. -CP_EXPORT cpSimpleMotor* cpSimpleMotorAlloc(void); -/// initialize a simple motor. -CP_EXPORT cpSimpleMotor* cpSimpleMotorInit(cpSimpleMotor *joint, cpBody *a, cpBody *b, cpFloat rate); -/// Allocate and initialize a simple motor. -CP_EXPORT cpConstraint* cpSimpleMotorNew(cpBody *a, cpBody *b, cpFloat rate); - -/// Get the rate of the motor. -CP_EXPORT cpFloat cpSimpleMotorGetRate(const cpConstraint *constraint); -/// Set the rate of the motor. -CP_EXPORT void cpSimpleMotorSetRate(cpConstraint *constraint, cpFloat rate); - -/// @} diff --git a/3rdparty/chipmunk/include/chipmunk/cpSlideJoint.h b/3rdparty/chipmunk/include/chipmunk/cpSlideJoint.h deleted file mode 100644 index c41f9a42eead..000000000000 --- a/3rdparty/chipmunk/include/chipmunk/cpSlideJoint.h +++ /dev/null @@ -1,55 +0,0 @@ -/* Copyright (c) 2013 Scott Lembcke and Howling Moon Software - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/// @defgroup cpSlideJoint cpSlideJoint -/// @{ - -/// Check if a constraint is a slide joint. -CP_EXPORT cpBool cpConstraintIsSlideJoint(const cpConstraint *constraint); - -/// Allocate a slide joint. -CP_EXPORT cpSlideJoint* cpSlideJointAlloc(void); -/// Initialize a slide joint. -CP_EXPORT cpSlideJoint* cpSlideJointInit(cpSlideJoint *joint, cpBody *a, cpBody *b, cpVect anchorA, cpVect anchorB, cpFloat min, cpFloat max); -/// Allocate and initialize a slide joint. -CP_EXPORT cpConstraint* cpSlideJointNew(cpBody *a, cpBody *b, cpVect anchorA, cpVect anchorB, cpFloat min, cpFloat max); - -/// Get the location of the first anchor relative to the first body. -CP_EXPORT cpVect cpSlideJointGetAnchorA(const cpConstraint *constraint); -/// Set the location of the first anchor relative to the first body. -CP_EXPORT void cpSlideJointSetAnchorA(cpConstraint *constraint, cpVect anchorA); - -/// Get the location of the second anchor relative to the second body. -CP_EXPORT cpVect cpSlideJointGetAnchorB(const cpConstraint *constraint); -/// Set the location of the second anchor relative to the second body. -CP_EXPORT void cpSlideJointSetAnchorB(cpConstraint *constraint, cpVect anchorB); - -/// Get the minimum distance the joint will maintain between the two anchors. -CP_EXPORT cpFloat cpSlideJointGetMin(const cpConstraint *constraint); -/// Set the minimum distance the joint will maintain between the two anchors. -CP_EXPORT void cpSlideJointSetMin(cpConstraint *constraint, cpFloat min); - -/// Get the maximum distance the joint will maintain between the two anchors. -CP_EXPORT cpFloat cpSlideJointGetMax(const cpConstraint *constraint); -/// Set the maximum distance the joint will maintain between the two anchors. -CP_EXPORT void cpSlideJointSetMax(cpConstraint *constraint, cpFloat max); - -/// @} diff --git a/3rdparty/chipmunk/include/chipmunk/cpSpace.h b/3rdparty/chipmunk/include/chipmunk/cpSpace.h deleted file mode 100644 index 7bbabb857d52..000000000000 --- a/3rdparty/chipmunk/include/chipmunk/cpSpace.h +++ /dev/null @@ -1,319 +0,0 @@ -/* Copyright (c) 2013 Scott Lembcke and Howling Moon Software - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/// @defgroup cpSpace cpSpace -/// @{ - -//MARK: Definitions - -/// Collision begin event function callback type. -/// Returning false from a begin callback causes the collision to be ignored until -/// the the separate callback is called when the objects stop colliding. -typedef cpBool (*cpCollisionBeginFunc)(cpArbiter *arb, cpSpace *space, cpDataPointer userData); -/// Collision pre-solve event function callback type. -/// Returning false from a pre-step callback causes the collision to be ignored until the next step. -typedef cpBool (*cpCollisionPreSolveFunc)(cpArbiter *arb, cpSpace *space, cpDataPointer userData); -/// Collision post-solve event function callback type. -typedef void (*cpCollisionPostSolveFunc)(cpArbiter *arb, cpSpace *space, cpDataPointer userData); -/// Collision separate event function callback type. -typedef void (*cpCollisionSeparateFunc)(cpArbiter *arb, cpSpace *space, cpDataPointer userData); - -/// Struct that holds function callback pointers to configure custom collision handling. -/// Collision handlers have a pair of types; when a collision occurs between two shapes that have these types, the collision handler functions are triggered. -struct cpCollisionHandler { - /// Collision type identifier of the first shape that this handler recognizes. - /// In the collision handler callback, the shape with this type will be the first argument. Read only. - const cpCollisionType typeA; - /// Collision type identifier of the second shape that this handler recognizes. - /// In the collision handler callback, the shape with this type will be the second argument. Read only. - const cpCollisionType typeB; - /// This function is called when two shapes with types that match this collision handler begin colliding. - cpCollisionBeginFunc beginFunc; - /// This function is called each step when two shapes with types that match this collision handler are colliding. - /// It's called before the collision solver runs so that you can affect a collision's outcome. - cpCollisionPreSolveFunc preSolveFunc; - /// This function is called each step when two shapes with types that match this collision handler are colliding. - /// It's called after the collision solver runs so that you can read back information about the collision to trigger events in your game. - cpCollisionPostSolveFunc postSolveFunc; - /// This function is called when two shapes with types that match this collision handler stop colliding. - cpCollisionSeparateFunc separateFunc; - /// This is a user definable context pointer that is passed to all of the collision handler functions. - cpDataPointer userData; -}; - -// TODO: Make timestep a parameter? - - -//MARK: Memory and Initialization - -/// Allocate a cpSpace. -CP_EXPORT cpSpace* cpSpaceAlloc(void); -/// Initialize a cpSpace. -CP_EXPORT cpSpace* cpSpaceInit(cpSpace *space); -/// Allocate and initialize a cpSpace. -CP_EXPORT cpSpace* cpSpaceNew(void); - -/// Destroy a cpSpace. -CP_EXPORT void cpSpaceDestroy(cpSpace *space); -/// Destroy and free a cpSpace. -CP_EXPORT void cpSpaceFree(cpSpace *space); - - -//MARK: Properties - -/// Number of iterations to use in the impulse solver to solve contacts and other constraints. -CP_EXPORT int cpSpaceGetIterations(const cpSpace *space); -CP_EXPORT void cpSpaceSetIterations(cpSpace *space, int iterations); - -/// Gravity to pass to rigid bodies when integrating velocity. -CP_EXPORT cpVect cpSpaceGetGravity(const cpSpace *space); -CP_EXPORT void cpSpaceSetGravity(cpSpace *space, cpVect gravity); - -/// Damping rate expressed as the fraction of velocity bodies retain each second. -/// A value of 0.9 would mean that each body's velocity will drop 10% per second. -/// The default value is 1.0, meaning no damping is applied. -/// @note This damping value is different than those of cpDampedSpring and cpDampedRotarySpring. -CP_EXPORT cpFloat cpSpaceGetDamping(const cpSpace *space); -CP_EXPORT void cpSpaceSetDamping(cpSpace *space, cpFloat damping); - -/// Speed threshold for a body to be considered idle. -/// The default value of 0 means to let the space guess a good threshold based on gravity. -CP_EXPORT cpFloat cpSpaceGetIdleSpeedThreshold(const cpSpace *space); -CP_EXPORT void cpSpaceSetIdleSpeedThreshold(cpSpace *space, cpFloat idleSpeedThreshold); - -/// Time a group of bodies must remain idle in order to fall asleep. -/// Enabling sleeping also implicitly enables the the contact graph. -/// The default value of INFINITY disables the sleeping algorithm. -CP_EXPORT cpFloat cpSpaceGetSleepTimeThreshold(const cpSpace *space); -CP_EXPORT void cpSpaceSetSleepTimeThreshold(cpSpace *space, cpFloat sleepTimeThreshold); - -/// Amount of encouraged penetration between colliding shapes. -/// Used to reduce oscillating contacts and keep the collision cache warm. -/// Defaults to 0.1. If you have poor simulation quality, -/// increase this number as much as possible without allowing visible amounts of overlap. -CP_EXPORT cpFloat cpSpaceGetCollisionSlop(const cpSpace *space); -CP_EXPORT void cpSpaceSetCollisionSlop(cpSpace *space, cpFloat collisionSlop); - -/// Determines how fast overlapping shapes are pushed apart. -/// Expressed as a fraction of the error remaining after each second. -/// Defaults to pow(1.0 - 0.1, 60.0) meaning that Chipmunk fixes 10% of overlap each frame at 60Hz. -CP_EXPORT cpFloat cpSpaceGetCollisionBias(const cpSpace *space); -CP_EXPORT void cpSpaceSetCollisionBias(cpSpace *space, cpFloat collisionBias); - -/// Number of frames that contact information should persist. -/// Defaults to 3. There is probably never a reason to change this value. -CP_EXPORT cpTimestamp cpSpaceGetCollisionPersistence(const cpSpace *space); -CP_EXPORT void cpSpaceSetCollisionPersistence(cpSpace *space, cpTimestamp collisionPersistence); - -/// User definable data pointer. -/// Generally this points to your game's controller or game state -/// class so you can access it when given a cpSpace reference in a callback. -CP_EXPORT cpDataPointer cpSpaceGetUserData(const cpSpace *space); -CP_EXPORT void cpSpaceSetUserData(cpSpace *space, cpDataPointer userData); - -/// The Space provided static body for a given cpSpace. -/// This is merely provided for convenience and you are not required to use it. -CP_EXPORT cpBody* cpSpaceGetStaticBody(const cpSpace *space); - -/// Returns the current (or most recent) time step used with the given space. -/// Useful from callbacks if your time step is not a compile-time global. -CP_EXPORT cpFloat cpSpaceGetCurrentTimeStep(const cpSpace *space); - -/// returns true from inside a callback when objects cannot be added/removed. -CP_EXPORT cpBool cpSpaceIsLocked(cpSpace *space); - - -//MARK: Collision Handlers - -/// Create or return the existing collision handler that is called for all collisions that are not handled by a more specific collision handler. -CP_EXPORT cpCollisionHandler *cpSpaceAddDefaultCollisionHandler(cpSpace *space); -/// Create or return the existing collision handler for the specified pair of collision types. -/// If wildcard handlers are used with either of the collision types, it's the responibility of the custom handler to invoke the wildcard handlers. -CP_EXPORT cpCollisionHandler *cpSpaceAddCollisionHandler(cpSpace *space, cpCollisionType a, cpCollisionType b); -/// Create or return the existing wildcard collision handler for the specified type. -CP_EXPORT cpCollisionHandler *cpSpaceAddWildcardHandler(cpSpace *space, cpCollisionType type); - - -//MARK: Add/Remove objects - -/// Add a collision shape to the simulation. -/// If the shape is attached to a static body, it will be added as a static shape. -CP_EXPORT cpShape* cpSpaceAddShape(cpSpace *space, cpShape *shape); -/// Add a rigid body to the simulation. -CP_EXPORT cpBody* cpSpaceAddBody(cpSpace *space, cpBody *body); -/// Add a constraint to the simulation. -CP_EXPORT cpConstraint* cpSpaceAddConstraint(cpSpace *space, cpConstraint *constraint); - -/// Remove a collision shape from the simulation. -CP_EXPORT void cpSpaceRemoveShape(cpSpace *space, cpShape *shape); -/// Remove a rigid body from the simulation. -CP_EXPORT void cpSpaceRemoveBody(cpSpace *space, cpBody *body); -/// Remove a constraint from the simulation. -CP_EXPORT void cpSpaceRemoveConstraint(cpSpace *space, cpConstraint *constraint); - -/// Test if a collision shape has been added to the space. -CP_EXPORT cpBool cpSpaceContainsShape(cpSpace *space, cpShape *shape); -/// Test if a rigid body has been added to the space. -CP_EXPORT cpBool cpSpaceContainsBody(cpSpace *space, cpBody *body); -/// Test if a constraint has been added to the space. -CP_EXPORT cpBool cpSpaceContainsConstraint(cpSpace *space, cpConstraint *constraint); - -//MARK: Post-Step Callbacks - -/// Post Step callback function type. -typedef void (*cpPostStepFunc)(cpSpace *space, void *key, void *data); -/// Schedule a post-step callback to be called when cpSpaceStep() finishes. -/// You can only register one callback per unique value for @c key. -/// Returns true only if @c key has never been scheduled before. -/// It's possible to pass @c NULL for @c func if you only want to mark @c key as being used. -CP_EXPORT cpBool cpSpaceAddPostStepCallback(cpSpace *space, cpPostStepFunc func, void *key, void *data); - - -//MARK: Queries - -// TODO: Queries and iterators should take a cpSpace parametery. -// TODO: They should also be abortable. - -/// Nearest point query callback function type. -typedef void (*cpSpacePointQueryFunc)(cpShape *shape, cpVect point, cpFloat distance, cpVect gradient, void *data); -/// Query the space at a point and call @c func for each shape found. -CP_EXPORT void cpSpacePointQuery(cpSpace *space, cpVect point, cpFloat maxDistance, cpShapeFilter filter, cpSpacePointQueryFunc func, void *data); -/// Query the space at a point and return the nearest shape found. Returns NULL if no shapes were found. -CP_EXPORT cpShape *cpSpacePointQueryNearest(cpSpace *space, cpVect point, cpFloat maxDistance, cpShapeFilter filter, cpPointQueryInfo *out); - -/// Segment query callback function type. -typedef void (*cpSpaceSegmentQueryFunc)(cpShape *shape, cpVect point, cpVect normal, cpFloat alpha, void *data); -/// Perform a directed line segment query (like a raycast) against the space calling @c func for each shape intersected. -CP_EXPORT void cpSpaceSegmentQuery(cpSpace *space, cpVect start, cpVect end, cpFloat radius, cpShapeFilter filter, cpSpaceSegmentQueryFunc func, void *data); -/// Perform a directed line segment query (like a raycast) against the space and return the first shape hit. Returns NULL if no shapes were hit. -CP_EXPORT cpShape *cpSpaceSegmentQueryFirst(cpSpace *space, cpVect start, cpVect end, cpFloat radius, cpShapeFilter filter, cpSegmentQueryInfo *out); - -/// Rectangle Query callback function type. -typedef void (*cpSpaceBBQueryFunc)(cpShape *shape, void *data); -/// Perform a fast rectangle query on the space calling @c func for each shape found. -/// Only the shape's bounding boxes are checked for overlap, not their full shape. -CP_EXPORT void cpSpaceBBQuery(cpSpace *space, cpBB bb, cpShapeFilter filter, cpSpaceBBQueryFunc func, void *data); - -/// Shape query callback function type. -typedef void (*cpSpaceShapeQueryFunc)(cpShape *shape, cpContactPointSet *points, void *data); -/// Query a space for any shapes overlapping the given shape and call @c func for each shape found. -CP_EXPORT cpBool cpSpaceShapeQuery(cpSpace *space, cpShape *shape, cpSpaceShapeQueryFunc func, void *data); - - -//MARK: Iteration - -/// Space/body iterator callback function type. -typedef void (*cpSpaceBodyIteratorFunc)(cpBody *body, void *data); -/// Call @c func for each body in the space. -CP_EXPORT void cpSpaceEachBody(cpSpace *space, cpSpaceBodyIteratorFunc func, void *data); - -/// Space/body iterator callback function type. -typedef void (*cpSpaceShapeIteratorFunc)(cpShape *shape, void *data); -/// Call @c func for each shape in the space. -CP_EXPORT void cpSpaceEachShape(cpSpace *space, cpSpaceShapeIteratorFunc func, void *data); - -/// Space/constraint iterator callback function type. -typedef void (*cpSpaceConstraintIteratorFunc)(cpConstraint *constraint, void *data); -/// Call @c func for each shape in the space. -CP_EXPORT void cpSpaceEachConstraint(cpSpace *space, cpSpaceConstraintIteratorFunc func, void *data); - - -//MARK: Indexing - -/// Update the collision detection info for the static shapes in the space. -CP_EXPORT void cpSpaceReindexStatic(cpSpace *space); -/// Update the collision detection data for a specific shape in the space. -CP_EXPORT void cpSpaceReindexShape(cpSpace *space, cpShape *shape); -/// Update the collision detection data for all shapes attached to a body. -CP_EXPORT void cpSpaceReindexShapesForBody(cpSpace *space, cpBody *body); - -/// Switch the space to use a spatial has as it's spatial index. -CP_EXPORT void cpSpaceUseSpatialHash(cpSpace *space, cpFloat dim, int count); - - -//MARK: Time Stepping - -/// Step the space forward in time by @c dt. -CP_EXPORT void cpSpaceStep(cpSpace *space, cpFloat dt); - - -//MARK: Debug API - -#ifndef CP_SPACE_DISABLE_DEBUG_API - -/// Color type to use with the space debug drawing API. -typedef struct cpSpaceDebugColor { - float r, g, b, a; -} cpSpaceDebugColor; - -/// Callback type for a function that draws a filled, stroked circle. -typedef void (*cpSpaceDebugDrawCircleImpl)(cpVect pos, cpFloat angle, cpFloat radius, cpSpaceDebugColor outlineColor, cpSpaceDebugColor fillColor, cpDataPointer data); -/// Callback type for a function that draws a line segment. -typedef void (*cpSpaceDebugDrawSegmentImpl)(cpVect a, cpVect b, cpSpaceDebugColor color, cpDataPointer data); -/// Callback type for a function that draws a thick line segment. -typedef void (*cpSpaceDebugDrawFatSegmentImpl)(cpVect a, cpVect b, cpFloat radius, cpSpaceDebugColor outlineColor, cpSpaceDebugColor fillColor, cpDataPointer data); -/// Callback type for a function that draws a convex polygon. -typedef void (*cpSpaceDebugDrawPolygonImpl)(int count, const cpVect *verts, cpFloat radius, cpSpaceDebugColor outlineColor, cpSpaceDebugColor fillColor, cpDataPointer data); -/// Callback type for a function that draws a dot. -typedef void (*cpSpaceDebugDrawDotImpl)(cpFloat size, cpVect pos, cpSpaceDebugColor color, cpDataPointer data); -/// Callback type for a function that returns a color for a given shape. This gives you an opportunity to color shapes based on how they are used in your engine. -typedef cpSpaceDebugColor (*cpSpaceDebugDrawColorForShapeImpl)(cpShape *shape, cpDataPointer data); - -typedef enum cpSpaceDebugDrawFlags { - CP_SPACE_DEBUG_DRAW_SHAPES = 1<<0, - CP_SPACE_DEBUG_DRAW_CONSTRAINTS = 1<<1, - CP_SPACE_DEBUG_DRAW_COLLISION_POINTS = 1<<2, -} cpSpaceDebugDrawFlags; - -/// Struct used with cpSpaceDebugDraw() containing drawing callbacks and other drawing settings. -typedef struct cpSpaceDebugDrawOptions { - /// Function that will be invoked to draw circles. - cpSpaceDebugDrawCircleImpl drawCircle; - /// Function that will be invoked to draw line segments. - cpSpaceDebugDrawSegmentImpl drawSegment; - /// Function that will be invoked to draw thick line segments. - cpSpaceDebugDrawFatSegmentImpl drawFatSegment; - /// Function that will be invoked to draw convex polygons. - cpSpaceDebugDrawPolygonImpl drawPolygon; - /// Function that will be invoked to draw dots. - cpSpaceDebugDrawDotImpl drawDot; - - /// Flags that request which things to draw (collision shapes, constraints, contact points). - cpSpaceDebugDrawFlags flags; - /// Outline color passed to the drawing function. - cpSpaceDebugColor shapeOutlineColor; - /// Function that decides what fill color to draw shapes using. - cpSpaceDebugDrawColorForShapeImpl colorForShape; - /// Color passed to drawing functions for constraints. - cpSpaceDebugColor constraintColor; - /// Color passed to drawing functions for collision points. - cpSpaceDebugColor collisionPointColor; - - /// User defined context pointer passed to all of the callback functions as the 'data' argument. - cpDataPointer data; -} cpSpaceDebugDrawOptions; - -/// Debug draw the current state of the space using the supplied drawing options. -CP_EXPORT void cpSpaceDebugDraw(cpSpace *space, cpSpaceDebugDrawOptions *options); - -#endif - -/// @} diff --git a/3rdparty/chipmunk/include/chipmunk/cpSpatialIndex.h b/3rdparty/chipmunk/include/chipmunk/cpSpatialIndex.h deleted file mode 100644 index 1f7c68ca959d..000000000000 --- a/3rdparty/chipmunk/include/chipmunk/cpSpatialIndex.h +++ /dev/null @@ -1,227 +0,0 @@ -/* Copyright (c) 2013 Scott Lembcke and Howling Moon Software - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/** - @defgroup cpSpatialIndex cpSpatialIndex - - Spatial indexes are data structures that are used to accelerate collision detection - and spatial queries. Chipmunk provides a number of spatial index algorithms to pick from - and they are programmed in a generic way so that you can use them for holding more than - just cpShape structs. - - It works by using @c void pointers to the objects you add and using a callback to ask your code - for bounding boxes when it needs them. Several types of queries can be performed an index as well - as reindexing and full collision information. All communication to the spatial indexes is performed - through callback functions. - - Spatial indexes should be treated as opaque structs. - This meanns you shouldn't be reading any of the struct fields. - @{ -*/ - -//MARK: Spatial Index - -/// Spatial index bounding box callback function type. -/// The spatial index calls this function and passes you a pointer to an object you added -/// when it needs to get the bounding box associated with that object. -typedef cpBB (*cpSpatialIndexBBFunc)(void *obj); -/// Spatial index/object iterator callback function type. -typedef void (*cpSpatialIndexIteratorFunc)(void *obj, void *data); -/// Spatial query callback function type. -typedef cpCollisionID (*cpSpatialIndexQueryFunc)(void *obj1, void *obj2, cpCollisionID id, void *data); -/// Spatial segment query callback function type. -typedef cpFloat (*cpSpatialIndexSegmentQueryFunc)(void *obj1, void *obj2, void *data); - - -typedef struct cpSpatialIndexClass cpSpatialIndexClass; -typedef struct cpSpatialIndex cpSpatialIndex; - -/// @private -struct cpSpatialIndex { - cpSpatialIndexClass *klass; - - cpSpatialIndexBBFunc bbfunc; - - cpSpatialIndex *staticIndex, *dynamicIndex; -}; - - -//MARK: Spatial Hash - -typedef struct cpSpaceHash cpSpaceHash; - -/// Allocate a spatial hash. -CP_EXPORT cpSpaceHash* cpSpaceHashAlloc(void); -/// Initialize a spatial hash. -CP_EXPORT cpSpatialIndex* cpSpaceHashInit(cpSpaceHash *hash, cpFloat celldim, int numcells, cpSpatialIndexBBFunc bbfunc, cpSpatialIndex *staticIndex); -/// Allocate and initialize a spatial hash. -CP_EXPORT cpSpatialIndex* cpSpaceHashNew(cpFloat celldim, int cells, cpSpatialIndexBBFunc bbfunc, cpSpatialIndex *staticIndex); - -/// Change the cell dimensions and table size of the spatial hash to tune it. -/// The cell dimensions should roughly match the average size of your objects -/// and the table size should be ~10 larger than the number of objects inserted. -/// Some trial and error is required to find the optimum numbers for efficiency. -CP_EXPORT void cpSpaceHashResize(cpSpaceHash *hash, cpFloat celldim, int numcells); - -//MARK: AABB Tree - -typedef struct cpBBTree cpBBTree; - -/// Allocate a bounding box tree. -CP_EXPORT cpBBTree* cpBBTreeAlloc(void); -/// Initialize a bounding box tree. -CP_EXPORT cpSpatialIndex* cpBBTreeInit(cpBBTree *tree, cpSpatialIndexBBFunc bbfunc, cpSpatialIndex *staticIndex); -/// Allocate and initialize a bounding box tree. -CP_EXPORT cpSpatialIndex* cpBBTreeNew(cpSpatialIndexBBFunc bbfunc, cpSpatialIndex *staticIndex); - -/// Perform a static top down optimization of the tree. -CP_EXPORT void cpBBTreeOptimize(cpSpatialIndex *index); - -/// Bounding box tree velocity callback function. -/// This function should return an estimate for the object's velocity. -typedef cpVect (*cpBBTreeVelocityFunc)(void *obj); -/// Set the velocity function for the bounding box tree to enable temporal coherence. -CP_EXPORT void cpBBTreeSetVelocityFunc(cpSpatialIndex *index, cpBBTreeVelocityFunc func); - -//MARK: Single Axis Sweep - -typedef struct cpSweep1D cpSweep1D; - -/// Allocate a 1D sort and sweep broadphase. -CP_EXPORT cpSweep1D* cpSweep1DAlloc(void); -/// Initialize a 1D sort and sweep broadphase. -CP_EXPORT cpSpatialIndex* cpSweep1DInit(cpSweep1D *sweep, cpSpatialIndexBBFunc bbfunc, cpSpatialIndex *staticIndex); -/// Allocate and initialize a 1D sort and sweep broadphase. -CP_EXPORT cpSpatialIndex* cpSweep1DNew(cpSpatialIndexBBFunc bbfunc, cpSpatialIndex *staticIndex); - -//MARK: Spatial Index Implementation - -typedef void (*cpSpatialIndexDestroyImpl)(cpSpatialIndex *index); - -typedef int (*cpSpatialIndexCountImpl)(cpSpatialIndex *index); -typedef void (*cpSpatialIndexEachImpl)(cpSpatialIndex *index, cpSpatialIndexIteratorFunc func, void *data); - -typedef cpBool (*cpSpatialIndexContainsImpl)(cpSpatialIndex *index, void *obj, cpHashValue hashid); -typedef void (*cpSpatialIndexInsertImpl)(cpSpatialIndex *index, void *obj, cpHashValue hashid); -typedef void (*cpSpatialIndexRemoveImpl)(cpSpatialIndex *index, void *obj, cpHashValue hashid); - -typedef void (*cpSpatialIndexReindexImpl)(cpSpatialIndex *index); -typedef void (*cpSpatialIndexReindexObjectImpl)(cpSpatialIndex *index, void *obj, cpHashValue hashid); -typedef void (*cpSpatialIndexReindexQueryImpl)(cpSpatialIndex *index, cpSpatialIndexQueryFunc func, void *data); - -typedef void (*cpSpatialIndexQueryImpl)(cpSpatialIndex *index, void *obj, cpBB bb, cpSpatialIndexQueryFunc func, void *data); -typedef void (*cpSpatialIndexSegmentQueryImpl)(cpSpatialIndex *index, void *obj, cpVect a, cpVect b, cpFloat t_exit, cpSpatialIndexSegmentQueryFunc func, void *data); - -struct cpSpatialIndexClass { - cpSpatialIndexDestroyImpl destroy; - - cpSpatialIndexCountImpl count; - cpSpatialIndexEachImpl each; - - cpSpatialIndexContainsImpl contains; - cpSpatialIndexInsertImpl insert; - cpSpatialIndexRemoveImpl remove; - - cpSpatialIndexReindexImpl reindex; - cpSpatialIndexReindexObjectImpl reindexObject; - cpSpatialIndexReindexQueryImpl reindexQuery; - - cpSpatialIndexQueryImpl query; - cpSpatialIndexSegmentQueryImpl segmentQuery; -}; - -/// Destroy and free a spatial index. -CP_EXPORT void cpSpatialIndexFree(cpSpatialIndex *index); -/// Collide the objects in @c dynamicIndex against the objects in @c staticIndex using the query callback function. -CP_EXPORT void cpSpatialIndexCollideStatic(cpSpatialIndex *dynamicIndex, cpSpatialIndex *staticIndex, cpSpatialIndexQueryFunc func, void *data); - -/// Destroy a spatial index. -static inline void cpSpatialIndexDestroy(cpSpatialIndex *index) -{ - if(index->klass) index->klass->destroy(index); -} - -/// Get the number of objects in the spatial index. -static inline int cpSpatialIndexCount(cpSpatialIndex *index) -{ - return index->klass->count(index); -} - -/// Iterate the objects in the spatial index. @c func will be called once for each object. -static inline void cpSpatialIndexEach(cpSpatialIndex *index, cpSpatialIndexIteratorFunc func, void *data) -{ - index->klass->each(index, func, data); -} - -/// Returns true if the spatial index contains the given object. -/// Most spatial indexes use hashed storage, so you must provide a hash value too. -static inline cpBool cpSpatialIndexContains(cpSpatialIndex *index, void *obj, cpHashValue hashid) -{ - return index->klass->contains(index, obj, hashid); -} - -/// Add an object to a spatial index. -/// Most spatial indexes use hashed storage, so you must provide a hash value too. -static inline void cpSpatialIndexInsert(cpSpatialIndex *index, void *obj, cpHashValue hashid) -{ - index->klass->insert(index, obj, hashid); -} - -/// Remove an object from a spatial index. -/// Most spatial indexes use hashed storage, so you must provide a hash value too. -static inline void cpSpatialIndexRemove(cpSpatialIndex *index, void *obj, cpHashValue hashid) -{ - index->klass->remove(index, obj, hashid); -} - -/// Perform a full reindex of a spatial index. -static inline void cpSpatialIndexReindex(cpSpatialIndex *index) -{ - index->klass->reindex(index); -} - -/// Reindex a single object in the spatial index. -static inline void cpSpatialIndexReindexObject(cpSpatialIndex *index, void *obj, cpHashValue hashid) -{ - index->klass->reindexObject(index, obj, hashid); -} - -/// Perform a rectangle query against the spatial index, calling @c func for each potential match. -static inline void cpSpatialIndexQuery(cpSpatialIndex *index, void *obj, cpBB bb, cpSpatialIndexQueryFunc func, void *data) -{ - index->klass->query(index, obj, bb, func, data); -} - -/// Perform a segment query against the spatial index, calling @c func for each potential match. -static inline void cpSpatialIndexSegmentQuery(cpSpatialIndex *index, void *obj, cpVect a, cpVect b, cpFloat t_exit, cpSpatialIndexSegmentQueryFunc func, void *data) -{ - index->klass->segmentQuery(index, obj, a, b, t_exit, func, data); -} - -/// Simultaneously reindex and find all colliding objects. -/// @c func will be called once for each potentially overlapping pair of objects found. -/// If the spatial index was initialized with a static index, it will collide it's objects against that as well. -static inline void cpSpatialIndexReindexQuery(cpSpatialIndex *index, cpSpatialIndexQueryFunc func, void *data) -{ - index->klass->reindexQuery(index, func, data); -} - -///@} diff --git a/3rdparty/chipmunk/include/chipmunk/cpTransform.h b/3rdparty/chipmunk/include/chipmunk/cpTransform.h deleted file mode 100644 index 4a6256b91564..000000000000 --- a/3rdparty/chipmunk/include/chipmunk/cpTransform.h +++ /dev/null @@ -1,198 +0,0 @@ -/* Copyright (c) 2013 Scott Lembcke and Howling Moon Software - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -#ifndef CHIPMUNK_TRANSFORM_H -#define CHIPMUNK_TRANSFORM_H - -#include "chipmunk_types.h" -#include "cpVect.h" -#include "cpBB.h" - -/// Identity transform matrix. -static const cpTransform cpTransformIdentity = {1.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f}; - -/// Construct a new transform matrix. -/// (a, b) is the x basis vector. -/// (c, d) is the y basis vector. -/// (tx, ty) is the translation. -static inline cpTransform -cpTransformNew(cpFloat a, cpFloat b, cpFloat c, cpFloat d, cpFloat tx, cpFloat ty) -{ - cpTransform t = {a, b, c, d, tx, ty}; - return t; -} - -/// Construct a new transform matrix in transposed order. -static inline cpTransform -cpTransformNewTranspose(cpFloat a, cpFloat c, cpFloat tx, cpFloat b, cpFloat d, cpFloat ty) -{ - cpTransform t = {a, b, c, d, tx, ty}; - return t; -} - -/// Get the inverse of a transform matrix. -static inline cpTransform -cpTransformInverse(cpTransform t) -{ - cpFloat inv_det = 1.0/(t.a*t.d - t.c*t.b); - return cpTransformNewTranspose( - t.d*inv_det, -t.c*inv_det, (t.c*t.ty - t.tx*t.d)*inv_det, - -t.b*inv_det, t.a*inv_det, (t.tx*t.b - t.a*t.ty)*inv_det - ); -} - -/// Multiply two transformation matrices. -static inline cpTransform -cpTransformMult(cpTransform t1, cpTransform t2) -{ - return cpTransformNewTranspose( - t1.a*t2.a + t1.c*t2.b, t1.a*t2.c + t1.c*t2.d, t1.a*t2.tx + t1.c*t2.ty + t1.tx, - t1.b*t2.a + t1.d*t2.b, t1.b*t2.c + t1.d*t2.d, t1.b*t2.tx + t1.d*t2.ty + t1.ty - ); -} - -/// Transform an absolute point. (i.e. a vertex) -static inline cpVect -cpTransformPoint(cpTransform t, cpVect p) -{ - return cpv(t.a*p.x + t.c*p.y + t.tx, t.b*p.x + t.d*p.y + t.ty); -} - -/// Transform a vector (i.e. a normal) -static inline cpVect -cpTransformVect(cpTransform t, cpVect v) -{ - return cpv(t.a*v.x + t.c*v.y, t.b*v.x + t.d*v.y); -} - -/// Transform a cpBB. -static inline cpBB -cpTransformbBB(cpTransform t, cpBB bb) -{ - cpVect center = cpBBCenter(bb); - cpFloat hw = (bb.r - bb.l)*0.5; - cpFloat hh = (bb.t - bb.b)*0.5; - - cpFloat a = t.a*hw, b = t.c*hh, d = t.b*hw, e = t.d*hh; - cpFloat hw_max = cpfmax(cpfabs(a + b), cpfabs(a - b)); - cpFloat hh_max = cpfmax(cpfabs(d + e), cpfabs(d - e)); - return cpBBNewForExtents(cpTransformPoint(t, center), hw_max, hh_max); -} - -/// Create a transation matrix. -static inline cpTransform -cpTransformTranslate(cpVect translate) -{ - return cpTransformNewTranspose( - 1.0, 0.0, translate.x, - 0.0, 1.0, translate.y - ); -} - -/// Create a scale matrix. -static inline cpTransform -cpTransformScale(cpFloat scaleX, cpFloat scaleY) -{ - return cpTransformNewTranspose( - scaleX, 0.0, 0.0, - 0.0, scaleY, 0.0 - ); -} - -/// Create a rotation matrix. -static inline cpTransform -cpTransformRotate(cpFloat radians) -{ - cpVect rot = cpvforangle(radians); - return cpTransformNewTranspose( - rot.x, -rot.y, 0.0, - rot.y, rot.x, 0.0 - ); -} - -/// Create a rigid transformation matrix. (transation + rotation) -static inline cpTransform -cpTransformRigid(cpVect translate, cpFloat radians) -{ - cpVect rot = cpvforangle(radians); - return cpTransformNewTranspose( - rot.x, -rot.y, translate.x, - rot.y, rot.x, translate.y - ); -} - -/// Fast inverse of a rigid transformation matrix. -static inline cpTransform -cpTransformRigidInverse(cpTransform t) -{ - return cpTransformNewTranspose( - t.d, -t.c, (t.c*t.ty - t.tx*t.d), - -t.b, t.a, (t.tx*t.b - t.a*t.ty) - ); -} - -//MARK: Miscellaneous (but useful) transformation matrices. -// See source for documentation... - -static inline cpTransform -cpTransformWrap(cpTransform outer, cpTransform inner) -{ - return cpTransformMult(cpTransformInverse(outer), cpTransformMult(inner, outer)); -} - -static inline cpTransform -cpTransformWrapInverse(cpTransform outer, cpTransform inner) -{ - return cpTransformMult(outer, cpTransformMult(inner, cpTransformInverse(outer))); -} - -static inline cpTransform -cpTransformOrtho(cpBB bb) -{ - return cpTransformNewTranspose( - 2.0/(bb.r - bb.l), 0.0, -(bb.r + bb.l)/(bb.r - bb.l), - 0.0, 2.0/(bb.t - bb.b), -(bb.t + bb.b)/(bb.t - bb.b) - ); -} - -static inline cpTransform -cpTransformBoneScale(cpVect v0, cpVect v1) -{ - cpVect d = cpvsub(v1, v0); - return cpTransformNewTranspose( - d.x, -d.y, v0.x, - d.y, d.x, v0.y - ); -} - -static inline cpTransform -cpTransformAxialScale(cpVect axis, cpVect pivot, cpFloat scale) -{ - cpFloat A = axis.x*axis.y*(scale - 1.0); - cpFloat B = cpvdot(axis, pivot)*(1.0 - scale); - - return cpTransformNewTranspose( - scale*axis.x*axis.x + axis.y*axis.y, A, axis.x*B, - A, axis.x*axis.x + scale*axis.y*axis.y, axis.y*B - ); -} - -#endif diff --git a/3rdparty/chipmunk/include/chipmunk/cpVect.h b/3rdparty/chipmunk/include/chipmunk/cpVect.h deleted file mode 100644 index 8ec02bdce35c..000000000000 --- a/3rdparty/chipmunk/include/chipmunk/cpVect.h +++ /dev/null @@ -1,230 +0,0 @@ -/* Copyright (c) 2013 Scott Lembcke and Howling Moon Software - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -#ifndef CHIPMUNK_VECT_H -#define CHIPMUNK_VECT_H - -#include "chipmunk_types.h" - -/// @defgroup cpVect cpVect -/// Chipmunk's 2D vector type along with a handy 2D vector math lib. -/// @{ - -/// Constant for the zero vector. -static const cpVect cpvzero = {0.0f,0.0f}; - -/// Convenience constructor for cpVect structs. -static inline cpVect cpv(const cpFloat x, const cpFloat y) -{ - cpVect v = {x, y}; - return v; -} - -/// Check if two vectors are equal. (Be careful when comparing floating point numbers!) -static inline cpBool cpveql(const cpVect v1, const cpVect v2) -{ - return (v1.x == v2.x && v1.y == v2.y); -} - -/// Add two vectors -static inline cpVect cpvadd(const cpVect v1, const cpVect v2) -{ - return cpv(v1.x + v2.x, v1.y + v2.y); -} - -/// Subtract two vectors. -static inline cpVect cpvsub(const cpVect v1, const cpVect v2) -{ - return cpv(v1.x - v2.x, v1.y - v2.y); -} - -/// Negate a vector. -static inline cpVect cpvneg(const cpVect v) -{ - return cpv(-v.x, -v.y); -} - -/// Scalar multiplication. -static inline cpVect cpvmult(const cpVect v, const cpFloat s) -{ - return cpv(v.x*s, v.y*s); -} - -/// Vector dot product. -static inline cpFloat cpvdot(const cpVect v1, const cpVect v2) -{ - return v1.x*v2.x + v1.y*v2.y; -} - -/// 2D vector cross product analog. -/// The cross product of 2D vectors results in a 3D vector with only a z component. -/// This function returns the magnitude of the z value. -static inline cpFloat cpvcross(const cpVect v1, const cpVect v2) -{ - return v1.x*v2.y - v1.y*v2.x; -} - -/// Returns a perpendicular vector. (90 degree rotation) -static inline cpVect cpvperp(const cpVect v) -{ - return cpv(-v.y, v.x); -} - -/// Returns a perpendicular vector. (-90 degree rotation) -static inline cpVect cpvrperp(const cpVect v) -{ - return cpv(v.y, -v.x); -} - -/// Returns the vector projection of v1 onto v2. -static inline cpVect cpvproject(const cpVect v1, const cpVect v2) -{ - return cpvmult(v2, cpvdot(v1, v2)/cpvdot(v2, v2)); -} - -/// Returns the unit length vector for the given angle (in radians). -static inline cpVect cpvforangle(const cpFloat a) -{ - return cpv(cpfcos(a), cpfsin(a)); -} - -/// Returns the angular direction v is pointing in (in radians). -static inline cpFloat cpvtoangle(const cpVect v) -{ - return cpfatan2(v.y, v.x); -} - -/// Uses complex number multiplication to rotate v1 by v2. Scaling will occur if v1 is not a unit vector. -static inline cpVect cpvrotate(const cpVect v1, const cpVect v2) -{ - return cpv(v1.x*v2.x - v1.y*v2.y, v1.x*v2.y + v1.y*v2.x); -} - -/// Inverse of cpvrotate(). -static inline cpVect cpvunrotate(const cpVect v1, const cpVect v2) -{ - return cpv(v1.x*v2.x + v1.y*v2.y, v1.y*v2.x - v1.x*v2.y); -} - -/// Returns the squared length of v. Faster than cpvlength() when you only need to compare lengths. -static inline cpFloat cpvlengthsq(const cpVect v) -{ - return cpvdot(v, v); -} - -/// Returns the length of v. -static inline cpFloat cpvlength(const cpVect v) -{ - return cpfsqrt(cpvdot(v, v)); -} - -/// Linearly interpolate between v1 and v2. -static inline cpVect cpvlerp(const cpVect v1, const cpVect v2, const cpFloat t) -{ - return cpvadd(cpvmult(v1, 1.0f - t), cpvmult(v2, t)); -} - -/// Returns a normalized copy of v. -static inline cpVect cpvnormalize(const cpVect v) -{ - // Neat trick I saw somewhere to avoid div/0. - return cpvmult(v, 1.0f/(cpvlength(v) + CPFLOAT_MIN)); -} - -/// Spherical linearly interpolate between v1 and v2. -static inline cpVect -cpvslerp(const cpVect v1, const cpVect v2, const cpFloat t) -{ - cpFloat dot = cpvdot(cpvnormalize(v1), cpvnormalize(v2)); - cpFloat omega = cpfacos(cpfclamp(dot, -1.0f, 1.0f)); - - if(omega < 1e-3){ - // If the angle between two vectors is very small, lerp instead to avoid precision issues. - return cpvlerp(v1, v2, t); - } else { - cpFloat denom = 1.0f/cpfsin(omega); - return cpvadd(cpvmult(v1, cpfsin((1.0f - t)*omega)*denom), cpvmult(v2, cpfsin(t*omega)*denom)); - } -} - -/// Spherical linearly interpolate between v1 towards v2 by no more than angle a radians -static inline cpVect -cpvslerpconst(const cpVect v1, const cpVect v2, const cpFloat a) -{ - cpFloat dot = cpvdot(cpvnormalize(v1), cpvnormalize(v2)); - cpFloat omega = cpfacos(cpfclamp(dot, -1.0f, 1.0f)); - - return cpvslerp(v1, v2, cpfmin(a, omega)/omega); -} - -/// Clamp v to length len. -static inline cpVect cpvclamp(const cpVect v, const cpFloat len) -{ - return (cpvdot(v,v) > len*len) ? cpvmult(cpvnormalize(v), len) : v; -} - -/// Linearly interpolate between v1 towards v2 by distance d. -static inline cpVect cpvlerpconst(cpVect v1, cpVect v2, cpFloat d) -{ - return cpvadd(v1, cpvclamp(cpvsub(v2, v1), d)); -} - -/// Returns the distance between v1 and v2. -static inline cpFloat cpvdist(const cpVect v1, const cpVect v2) -{ - return cpvlength(cpvsub(v1, v2)); -} - -/// Returns the squared distance between v1 and v2. Faster than cpvdist() when you only need to compare distances. -static inline cpFloat cpvdistsq(const cpVect v1, const cpVect v2) -{ - return cpvlengthsq(cpvsub(v1, v2)); -} - -/// Returns true if the distance between v1 and v2 is less than dist. -static inline cpBool cpvnear(const cpVect v1, const cpVect v2, const cpFloat dist) -{ - return cpvdistsq(v1, v2) < dist*dist; -} - -/// @} - -/// @defgroup cpMat2x2 cpMat2x2 -/// 2x2 matrix type used for tensors and such. -/// @{ - -// NUKE -static inline cpMat2x2 -cpMat2x2New(cpFloat a, cpFloat b, cpFloat c, cpFloat d) -{ - cpMat2x2 m = {a, b, c, d}; - return m; -} - -static inline cpVect -cpMat2x2Transform(cpMat2x2 m, cpVect v) -{ - return cpv(v.x*m.a + v.y*m.b, v.x*m.c + v.y*m.d); -} - -///@} - -#endif diff --git a/3rdparty/chipmunk/src/CMakeLists.txt b/3rdparty/chipmunk/src/CMakeLists.txt deleted file mode 100644 index 3fb3f317626c..000000000000 --- a/3rdparty/chipmunk/src/CMakeLists.txt +++ /dev/null @@ -1,59 +0,0 @@ -file(GLOB chipmunk_source_files "*.c") -file(GLOB chipmunk_public_header "${chipmunk_SOURCE_DIR}/include/chipmunk/*.h") - -include_directories(${chipmunk_SOURCE_DIR}/include) - -# Chipmunk2D 7.0.3 -set(CHIPMUNK_VERSION_MAJOR 7) -set(CHIPMUNK_VERSION_MINOR 0) -set(CHIPMUNK_VERSION_PATCH 3) -set(CHIPMUNK_VERSION "${CHIPMUNK_VERSION_MAJOR}.${CHIPMUNK_VERSION_MINOR}.${CHIPMUNK_VERSION_PATCH}") -message("Configuring Chipmunk2D version ${CHIPMUNK_VERSION}") - - -if(CP_BUILD_SHARED) - add_library(chipmunk SHARED - ${chipmunk_source_files} - ) - # Tell MSVC to compile the code as C++. - if(MSVC) - set_source_files_properties(${chipmunk_source_files} PROPERTIES LANGUAGE CXX) - set_target_properties(chipmunk PROPERTIES LINKER_LANGUAGE CXX) - endif(MSVC) - # set the lib's version number - # But avoid on Android because symlinks to version numbered .so's don't work with Android's Java-side loadLibrary. - if(NOT ANDROID) - set_target_properties(chipmunk PROPERTIES - SOVERSION ${CHIPMUNK_VERSION_MAJOR} - VERSION ${CHIPMUNK_VERSION}) - endif(NOT ANDROID) - if(ANDROID OR UNIX) - # need to explicitly link to the math library because the CMake/Android toolchains may not do it automatically - target_link_libraries(chipmunk m) - endif(ANDROID OR UNIX) - install(TARGETS chipmunk RUNTIME DESTINATION ${BIN_INSTALL_DIR} - LIBRARY DESTINATION ${LIB_INSTALL_DIR} - ARCHIVE DESTINATION ${LIB_INSTALL_DIR}) -endif(CP_BUILD_SHARED) - -if(CP_BUILD_STATIC) - add_library(chipmunk STATIC - ${chipmunk_source_files} - ) - # Tell MSVC to compile the code as C++. - if(MSVC) - set_source_files_properties(${chipmunk_source_files} PROPERTIES LANGUAGE CXX) - set_target_properties(chipmunk PROPERTIES LINKER_LANGUAGE CXX) - endif(MSVC) - # Sets chipmunk to output "libchipmunk.a" not "libchipmunk.a" - set_target_properties(chipmunk PROPERTIES OUTPUT_NAME chipmunk) - if(CP_INSTALL_STATIC) - install(TARGETS chipmunk ARCHIVE DESTINATION ${LIB_INSTALL_DIR}) - endif(CP_INSTALL_STATIC) -endif(CP_BUILD_STATIC) - -if(CP_BUILD_SHARED OR CP_INSTALL_STATIC) - # FIXME: change to PUBLIC_HEADER to allow building frameworks - install(FILES ${chipmunk_public_header} DESTINATION include/chipmunk) - install(FILES ${chipmunk_constraint_header} DESTINATION include/chipmunk/constraints) -endif(CP_BUILD_SHARED OR CP_INSTALL_STATIC) diff --git a/3rdparty/chipmunk/src/chipmunk.c b/3rdparty/chipmunk/src/chipmunk.c deleted file mode 100644 index a6cc9d6d4d16..000000000000 --- a/3rdparty/chipmunk/src/chipmunk.c +++ /dev/null @@ -1,331 +0,0 @@ -/* Copyright (c) 2013 Scott Lembcke and Howling Moon Software - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -#include -#include -#include -#if defined(ANDROID) -# include -#endif - -#include "chipmunk/chipmunk_private.h" - -void -cpMessage(const char *condition, const char *file, int line, int isError, int isHardError, const char *message, ...) -{ - fprintf(stderr, (isError ? "Aborting due to Chipmunk error: " : "Chipmunk warning: ")); - - va_list vargs; - va_start(vargs, message); { -#if defined(ANDROID) - __android_log_print( ANDROID_LOG_INFO, "Chipmunk", "%s(%d)", file, line ); - __android_log_print( ANDROID_LOG_INFO, "Chipmunk", message, vargs ); -#else - vfprintf(stderr, message, vargs); - fprintf(stderr, "\n"); -#endif - } va_end(vargs); - -#if defined(ANDROID) - __android_log_print(ANDROID_LOG_INFO, "Chipmunk", "\tFailed condition: %s\n", condition); - __android_log_print(ANDROID_LOG_INFO, "Chipmunk", "\tSource:%s:%d\n", file, line); -#else - fprintf(stderr, "\tFailed condition: %s\n", condition); - fprintf(stderr, "\tSource:%s:%d\n", file, line); -#endif -} - -#define STR(s) #s -#define XSTR(s) STR(s) - -const char *cpVersionString = XSTR(CP_VERSION_MAJOR) "." XSTR(CP_VERSION_MINOR) "." XSTR(CP_VERSION_RELEASE); - -//MARK: Misc Functions - -cpFloat -cpMomentForCircle(cpFloat m, cpFloat r1, cpFloat r2, cpVect offset) -{ - return m*(0.5f*(r1*r1 + r2*r2) + cpvlengthsq(offset)); -} - -cpFloat -cpAreaForCircle(cpFloat r1, cpFloat r2) -{ - return (cpFloat)CP_PI*cpfabs(r1*r1 - r2*r2); -} - -cpFloat -cpMomentForSegment(cpFloat m, cpVect a, cpVect b, cpFloat r) -{ - cpVect offset = cpvlerp(a, b, 0.5f); - - // This approximates the shape as a box for rounded segments, but it's quite close. - cpFloat length = cpvdist(b, a) + 2.0f*r; - return m*((length*length + 4.0f*r*r)/12.0f + cpvlengthsq(offset)); -} - -cpFloat -cpAreaForSegment(cpVect a, cpVect b, cpFloat r) -{ - return r*((cpFloat)CP_PI*r + 2.0f*cpvdist(a, b)); -} - -cpFloat -cpMomentForPoly(cpFloat m, int count, const cpVect *verts, cpVect offset, cpFloat r) -{ - // TODO account for radius. - if(count == 2) return cpMomentForSegment(m, verts[0], verts[1], 0.0f); - - cpFloat sum1 = 0.0f; - cpFloat sum2 = 0.0f; - for(int i=0; i max.x || (v.x == max.x && v.y > max.y)){ - max = v; - (*end) = i; - } - } -} - -#define SWAP(__A__, __B__) {cpVect __TMP__ = __A__; __A__ = __B__; __B__ = __TMP__;} - -static int -QHullPartition(cpVect *verts, int count, cpVect a, cpVect b, cpFloat tol) -{ - if(count == 0) return 0; - - cpFloat max = 0; - int pivot = 0; - - cpVect delta = cpvsub(b, a); - cpFloat valueTol = tol*cpvlength(delta); - - int head = 0; - for(int tail = count-1; head <= tail;){ - cpFloat value = cpvcross(cpvsub(verts[head], a), delta); - if(value > valueTol){ - if(value > max){ - max = value; - pivot = head; - } - - head++; - } else { - SWAP(verts[head], verts[tail]); - tail--; - } - } - - // move the new pivot to the front if it's not already there. - if(pivot != 0) SWAP(verts[0], verts[pivot]); - return head; -} - -static int -QHullReduce(cpFloat tol, cpVect *verts, int count, cpVect a, cpVect pivot, cpVect b, cpVect *result) -{ - if(count < 0){ - return 0; - } else if(count == 0) { - result[0] = pivot; - return 1; - } else { - int left_count = QHullPartition(verts, count, a, pivot, tol); - int index = QHullReduce(tol, verts + 1, left_count - 1, a, verts[0], pivot, result); - - result[index++] = pivot; - - int right_count = QHullPartition(verts + left_count, count - left_count, pivot, b, tol); - return index + QHullReduce(tol, verts + left_count + 1, right_count - 1, pivot, verts[left_count], b, result + index); - } -} - -// QuickHull seemed like a neat algorithm, and efficient-ish for large input sets. -// My implementation performs an in place reduction using the result array as scratch space. -int -cpConvexHull(int count, const cpVect *verts, cpVect *result, int *first, cpFloat tol) -{ - if(verts != result){ - // Copy the line vertexes into the empty part of the result polyline to use as a scratch buffer. - memcpy(result, verts, count*sizeof(cpVect)); - } - - // Degenerate case, all points are the same. - int start, end; - cpLoopIndexes(verts, count, &start, &end); - if(start == end){ - if(first) (*first) = 0; - return 1; - } - - SWAP(result[0], result[start]); - SWAP(result[1], result[end == 0 ? start : end]); - - cpVect a = result[0]; - cpVect b = result[1]; - - if(first) (*first) = start; - return QHullReduce(tol, result + 2, count - 2, a, b, a, result + 1) + 1; -} - -//MARK: Alternate Block Iterators - -#if defined(__has_extension) -#if __has_extension(blocks) - -static void IteratorFunc(void *ptr, void (^block)(void *ptr)){block(ptr);} - -void cpSpaceEachBody_b(cpSpace *space, void (^block)(cpBody *body)){ - cpSpaceEachBody(space, (cpSpaceBodyIteratorFunc)IteratorFunc, block); -} - -void cpSpaceEachShape_b(cpSpace *space, void (^block)(cpShape *shape)){ - cpSpaceEachShape(space, (cpSpaceShapeIteratorFunc)IteratorFunc, block); -} - -void cpSpaceEachConstraint_b(cpSpace *space, void (^block)(cpConstraint *constraint)){ - cpSpaceEachConstraint(space, (cpSpaceConstraintIteratorFunc)IteratorFunc, block); -} - -static void BodyIteratorFunc(cpBody *body, void *ptr, void (^block)(void *ptr)){block(ptr);} - -void cpBodyEachShape_b(cpBody *body, void (^block)(cpShape *shape)){ - cpBodyEachShape(body, (cpBodyShapeIteratorFunc)BodyIteratorFunc, block); -} - -void cpBodyEachConstraint_b(cpBody *body, void (^block)(cpConstraint *constraint)){ - cpBodyEachConstraint(body, (cpBodyConstraintIteratorFunc)BodyIteratorFunc, block); -} - -void cpBodyEachArbiter_b(cpBody *body, void (^block)(cpArbiter *arbiter)){ - cpBodyEachArbiter(body, (cpBodyArbiterIteratorFunc)BodyIteratorFunc, block); -} - -static void PointQueryIteratorFunc(cpShape *shape, cpVect p, cpFloat d, cpVect g, cpSpacePointQueryBlock block){block(shape, p, d, g);} -void cpSpacePointQuery_b(cpSpace *space, cpVect point, cpFloat maxDistance, cpShapeFilter filter, cpSpacePointQueryBlock block){ - cpSpacePointQuery(space, point, maxDistance, filter, (cpSpacePointQueryFunc)PointQueryIteratorFunc, block); -} - -static void SegmentQueryIteratorFunc(cpShape *shape, cpVect p, cpVect n, cpFloat t, cpSpaceSegmentQueryBlock block){block(shape, p, n, t);} -void cpSpaceSegmentQuery_b(cpSpace *space, cpVect start, cpVect end, cpFloat radius, cpShapeFilter filter, cpSpaceSegmentQueryBlock block){ - cpSpaceSegmentQuery(space, start, end, radius, filter, (cpSpaceSegmentQueryFunc)SegmentQueryIteratorFunc, block); -} - -void cpSpaceBBQuery_b(cpSpace *space, cpBB bb, cpShapeFilter filter, cpSpaceBBQueryBlock block){ - cpSpaceBBQuery(space, bb, filter, (cpSpaceBBQueryFunc)IteratorFunc, block); -} - -static void ShapeQueryIteratorFunc(cpShape *shape, cpContactPointSet *points, cpSpaceShapeQueryBlock block){block(shape, points);} -cpBool cpSpaceShapeQuery_b(cpSpace *space, cpShape *shape, cpSpaceShapeQueryBlock block){ - return cpSpaceShapeQuery(space, shape, (cpSpaceShapeQueryFunc)ShapeQueryIteratorFunc, block); -} - -#endif -#endif - -#include "chipmunk/chipmunk_ffi.h" diff --git a/3rdparty/chipmunk/src/cpArbiter.c b/3rdparty/chipmunk/src/cpArbiter.c deleted file mode 100644 index 5248e6aaec08..000000000000 --- a/3rdparty/chipmunk/src/cpArbiter.c +++ /dev/null @@ -1,498 +0,0 @@ -/* Copyright (c) 2013 Scott Lembcke and Howling Moon Software - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -#include "chipmunk/chipmunk_private.h" - -// TODO: make this generic so I can reuse it for constraints also. -static inline void -unthreadHelper(cpArbiter *arb, cpBody *body) -{ - struct cpArbiterThread *thread = cpArbiterThreadForBody(arb, body); - cpArbiter *prev = thread->prev; - cpArbiter *next = thread->next; - - if(prev){ - cpArbiterThreadForBody(prev, body)->next = next; - } else if(body->arbiterList == arb) { - // IFF prev is NULL and body->arbiterList == arb, is arb at the head of the list. - // This function may be called for an arbiter that was never in a list. - // In that case, we need to protect it from wiping out the body->arbiterList pointer. - body->arbiterList = next; - } - - if(next) cpArbiterThreadForBody(next, body)->prev = prev; - - thread->prev = NULL; - thread->next = NULL; -} - -void -cpArbiterUnthread(cpArbiter *arb) -{ - unthreadHelper(arb, arb->body_a); - unthreadHelper(arb, arb->body_b); -} - -cpBool cpArbiterIsFirstContact(const cpArbiter *arb) -{ - return arb->state == CP_ARBITER_STATE_FIRST_COLLISION; -} - -cpBool cpArbiterIsRemoval(const cpArbiter *arb) -{ - return arb->state == CP_ARBITER_STATE_INVALIDATED; -} - -int cpArbiterGetCount(const cpArbiter *arb) -{ - // Return 0 contacts if we are in a separate callback. - return (arb->state < CP_ARBITER_STATE_CACHED ? arb->count : 0); -} - -cpVect -cpArbiterGetNormal(const cpArbiter *arb) -{ - return cpvmult(arb->n, arb->swapped ? -1.0f : 1.0); -} - -cpVect -cpArbiterGetPointA(const cpArbiter *arb, int i) -{ - cpAssertHard(0 <= i && i < cpArbiterGetCount(arb), "Index error: The specified contact index is invalid for this arbiter"); - return cpvadd(arb->body_a->p, arb->contacts[i].r1); -} - -cpVect -cpArbiterGetPointB(const cpArbiter *arb, int i) -{ - cpAssertHard(0 <= i && i < cpArbiterGetCount(arb), "Index error: The specified contact index is invalid for this arbiter"); - return cpvadd(arb->body_b->p, arb->contacts[i].r2); -} - -cpFloat -cpArbiterGetDepth(const cpArbiter *arb, int i) -{ - cpAssertHard(0 <= i && i < cpArbiterGetCount(arb), "Index error: The specified contact index is invalid for this arbiter"); - - struct cpContact *con = &arb->contacts[i]; - return cpvdot(cpvadd(cpvsub(con->r2, con->r1), cpvsub(arb->body_b->p, arb->body_a->p)), arb->n); -} - -cpContactPointSet -cpArbiterGetContactPointSet(const cpArbiter *arb) -{ - cpContactPointSet set; - set.count = cpArbiterGetCount(arb); - - cpBool swapped = arb->swapped; - cpVect n = arb->n; - set.normal = (swapped ? cpvneg(n) : n); - - for(int i=0; ibody_a->p, arb->contacts[i].r1); - cpVect p2 = cpvadd(arb->body_b->p, arb->contacts[i].r2); - - set.points[i].pointA = (swapped ? p2 : p1); - set.points[i].pointB = (swapped ? p1 : p2); - set.points[i].distance = cpvdot(cpvsub(p2, p1), n); - } - - return set; -} - -void -cpArbiterSetContactPointSet(cpArbiter *arb, cpContactPointSet *set) -{ - int count = set->count; - cpAssertHard(count == arb->count, "The number of contact points cannot be changed."); - - cpBool swapped = arb->swapped; - arb->n = (swapped ? cpvneg(set->normal) : set->normal); - - for(int i=0; ipoints[i].pointA; - cpVect p2 = set->points[i].pointB; - - arb->contacts[i].r1 = cpvsub(swapped ? p2 : p1, arb->body_a->p); - arb->contacts[i].r2 = cpvsub(swapped ? p1 : p2, arb->body_b->p); - } -} - -cpVect -cpArbiterTotalImpulse(const cpArbiter *arb) -{ - struct cpContact *contacts = arb->contacts; - cpVect n = arb->n; - cpVect sum = cpvzero; - - for(int i=0, count=cpArbiterGetCount(arb); ijnAcc, con->jtAcc))); - } - - return (arb->swapped ? sum : cpvneg(sum)); - return cpvzero; -} - -cpFloat -cpArbiterTotalKE(const cpArbiter *arb) -{ - cpFloat eCoef = (1 - arb->e)/(1 + arb->e); - cpFloat sum = 0.0; - - struct cpContact *contacts = arb->contacts; - for(int i=0, count=cpArbiterGetCount(arb); ijnAcc; - cpFloat jtAcc = con->jtAcc; - - sum += eCoef*jnAcc*jnAcc/con->nMass + jtAcc*jtAcc/con->tMass; - } - - return sum; -} - -cpBool -cpArbiterIgnore(cpArbiter *arb) -{ - arb->state = CP_ARBITER_STATE_IGNORE; - return cpFalse; -} - -cpFloat -cpArbiterGetRestitution(const cpArbiter *arb) -{ - return arb->e; -} - -void -cpArbiterSetRestitution(cpArbiter *arb, cpFloat restitution) -{ - arb->e = restitution; -} - -cpFloat -cpArbiterGetFriction(const cpArbiter *arb) -{ - return arb->u; -} - -void -cpArbiterSetFriction(cpArbiter *arb, cpFloat friction) -{ - arb->u = friction; -} - -cpVect -cpArbiterGetSurfaceVelocity(cpArbiter *arb) -{ - return cpvmult(arb->surface_vr, arb->swapped ? -1.0f : 1.0); -} - -void -cpArbiterSetSurfaceVelocity(cpArbiter *arb, cpVect vr) -{ - arb->surface_vr = cpvmult(vr, arb->swapped ? -1.0f : 1.0); -} - -cpDataPointer -cpArbiterGetUserData(const cpArbiter *arb) -{ - return arb->data; -} - -void -cpArbiterSetUserData(cpArbiter *arb, cpDataPointer userData) -{ - arb->data = userData; -} - -void -cpArbiterGetShapes(const cpArbiter *arb, cpShape **a, cpShape **b) -{ - if(arb->swapped){ - (*a) = (cpShape *)arb->b; - (*b) = (cpShape *)arb->a; - } else { - (*a) = (cpShape *)arb->a; - (*b) = (cpShape *)arb->b; - } -} - -void cpArbiterGetBodies(const cpArbiter *arb, cpBody **a, cpBody **b) -{ - CP_ARBITER_GET_SHAPES(arb, shape_a, shape_b); - (*a) = shape_a->body; - (*b) = shape_b->body; -} - -cpBool -cpArbiterCallWildcardBeginA(cpArbiter *arb, cpSpace *space) -{ - cpCollisionHandler *handler = arb->handlerA; - return handler->beginFunc(arb, space, handler->userData); -} - -cpBool -cpArbiterCallWildcardBeginB(cpArbiter *arb, cpSpace *space) -{ - cpCollisionHandler *handler = arb->handlerB; - arb->swapped = !arb->swapped; - cpBool retval = handler->beginFunc(arb, space, handler->userData); - arb->swapped = !arb->swapped; - return retval; -} - -cpBool -cpArbiterCallWildcardPreSolveA(cpArbiter *arb, cpSpace *space) -{ - cpCollisionHandler *handler = arb->handlerA; - return handler->preSolveFunc(arb, space, handler->userData); -} - -cpBool -cpArbiterCallWildcardPreSolveB(cpArbiter *arb, cpSpace *space) -{ - cpCollisionHandler *handler = arb->handlerB; - arb->swapped = !arb->swapped; - cpBool retval = handler->preSolveFunc(arb, space, handler->userData); - arb->swapped = !arb->swapped; - return retval; -} - -void -cpArbiterCallWildcardPostSolveA(cpArbiter *arb, cpSpace *space) -{ - cpCollisionHandler *handler = arb->handlerA; - handler->postSolveFunc(arb, space, handler->userData); -} - -void -cpArbiterCallWildcardPostSolveB(cpArbiter *arb, cpSpace *space) -{ - cpCollisionHandler *handler = arb->handlerB; - arb->swapped = !arb->swapped; - handler->postSolveFunc(arb, space, handler->userData); - arb->swapped = !arb->swapped; -} - -void -cpArbiterCallWildcardSeparateA(cpArbiter *arb, cpSpace *space) -{ - cpCollisionHandler *handler = arb->handlerA; - handler->separateFunc(arb, space, handler->userData); -} - -void -cpArbiterCallWildcardSeparateB(cpArbiter *arb, cpSpace *space) -{ - cpCollisionHandler *handler = arb->handlerB; - arb->swapped = !arb->swapped; - handler->separateFunc(arb, space, handler->userData); - arb->swapped = !arb->swapped; -} - -cpArbiter* -cpArbiterInit(cpArbiter *arb, cpShape *a, cpShape *b) -{ - arb->handler = NULL; - arb->swapped = cpFalse; - - arb->handler = NULL; - arb->handlerA = NULL; - arb->handlerB = NULL; - - arb->e = 0.0f; - arb->u = 0.0f; - arb->surface_vr = cpvzero; - - arb->count = 0; - arb->contacts = NULL; - - arb->a = a; arb->body_a = a->body; - arb->b = b; arb->body_b = b->body; - - arb->thread_a.next = NULL; - arb->thread_b.next = NULL; - arb->thread_a.prev = NULL; - arb->thread_b.prev = NULL; - - arb->stamp = 0; - arb->state = CP_ARBITER_STATE_FIRST_COLLISION; - - arb->data = NULL; - - return arb; -} - -static inline cpCollisionHandler * -cpSpaceLookupHandler(cpSpace *space, cpCollisionType a, cpCollisionType b, cpCollisionHandler *defaultValue) -{ - cpCollisionType types[] = {a, b}; - cpCollisionHandler *handler = (cpCollisionHandler *)cpHashSetFind(space->collisionHandlers, CP_HASH_PAIR(a, b), types); - return (handler ? handler : defaultValue); -} - -void -cpArbiterUpdate(cpArbiter *arb, struct cpCollisionInfo *info, cpSpace *space) -{ - const cpShape *a = info->a, *b = info->b; - - // For collisions between two similar primitive types, the order could have been swapped since the last frame. - arb->a = a; arb->body_a = a->body; - arb->b = b; arb->body_b = b->body; - - // Iterate over the possible pairs to look for hash value matches. - for(int i=0; icount; i++){ - struct cpContact *con = &info->arr[i]; - - // r1 and r2 store absolute offsets at init time. - // Need to convert them to relative offsets. - con->r1 = cpvsub(con->r1, a->body->p); - con->r2 = cpvsub(con->r2, b->body->p); - - // Cached impulses are not zeroed at init time. - con->jnAcc = con->jtAcc = 0.0f; - - for(int j=0; jcount; j++){ - struct cpContact *old = &arb->contacts[j]; - - // This could trigger false positives, but is fairly unlikely nor serious if it does. - if(con->hash == old->hash){ - // Copy the persistant contact information. - con->jnAcc = old->jnAcc; - con->jtAcc = old->jtAcc; - } - } - } - - arb->contacts = info->arr; - arb->count = info->count; - arb->n = info->n; - - arb->e = a->e * b->e; - arb->u = a->u * b->u; - - cpVect surface_vr = cpvsub(b->surfaceV, a->surfaceV); - arb->surface_vr = cpvsub(surface_vr, cpvmult(info->n, cpvdot(surface_vr, info->n))); - - cpCollisionType typeA = info->a->type, typeB = info->b->type; - cpCollisionHandler *defaultHandler = &space->defaultHandler; - cpCollisionHandler *handler = arb->handler = cpSpaceLookupHandler(space, typeA, typeB, defaultHandler); - - // Check if the types match, but don't swap for a default handler which use the wildcard for type A. - cpBool swapped = arb->swapped = (typeA != handler->typeA && handler->typeA != CP_WILDCARD_COLLISION_TYPE); - - if(handler != defaultHandler || space->usesWildcards){ - // The order of the main handler swaps the wildcard handlers too. Uffda. - arb->handlerA = cpSpaceLookupHandler(space, (swapped ? typeB : typeA), CP_WILDCARD_COLLISION_TYPE, &cpCollisionHandlerDoNothing); - arb->handlerB = cpSpaceLookupHandler(space, (swapped ? typeA : typeB), CP_WILDCARD_COLLISION_TYPE, &cpCollisionHandlerDoNothing); - } - - // mark it as new if it's been cached - if(arb->state == CP_ARBITER_STATE_CACHED) arb->state = CP_ARBITER_STATE_FIRST_COLLISION; -} - -void -cpArbiterPreStep(cpArbiter *arb, cpFloat dt, cpFloat slop, cpFloat bias) -{ - cpBody *a = arb->body_a; - cpBody *b = arb->body_b; - cpVect n = arb->n; - cpVect body_delta = cpvsub(b->p, a->p); - - for(int i=0; icount; i++){ - struct cpContact *con = &arb->contacts[i]; - - // Calculate the mass normal and mass tangent. - con->nMass = 1.0f/k_scalar(a, b, con->r1, con->r2, n); - con->tMass = 1.0f/k_scalar(a, b, con->r1, con->r2, cpvperp(n)); - - // Calculate the target bias velocity. - cpFloat dist = cpvdot(cpvadd(cpvsub(con->r2, con->r1), body_delta), n); - con->bias = -bias*cpfmin(0.0f, dist + slop)/dt; - con->jBias = 0.0f; - - // Calculate the target bounce velocity. - con->bounce = normal_relative_velocity(a, b, con->r1, con->r2, n)*arb->e; - } -} - -void -cpArbiterApplyCachedImpulse(cpArbiter *arb, cpFloat dt_coef) -{ - if(cpArbiterIsFirstContact(arb)) return; - - cpBody *a = arb->body_a; - cpBody *b = arb->body_b; - cpVect n = arb->n; - - for(int i=0; icount; i++){ - struct cpContact *con = &arb->contacts[i]; - cpVect j = cpvrotate(n, cpv(con->jnAcc, con->jtAcc)); - apply_impulses(a, b, con->r1, con->r2, cpvmult(j, dt_coef)); - } -} - -// TODO: is it worth splitting velocity/position correction? - -void -cpArbiterApplyImpulse(cpArbiter *arb) -{ - cpBody *a = arb->body_a; - cpBody *b = arb->body_b; - cpVect n = arb->n; - cpVect surface_vr = arb->surface_vr; - cpFloat friction = arb->u; - - for(int i=0; icount; i++){ - struct cpContact *con = &arb->contacts[i]; - cpFloat nMass = con->nMass; - cpVect r1 = con->r1; - cpVect r2 = con->r2; - - cpVect vb1 = cpvadd(a->v_bias, cpvmult(cpvperp(r1), a->w_bias)); - cpVect vb2 = cpvadd(b->v_bias, cpvmult(cpvperp(r2), b->w_bias)); - cpVect vr = cpvadd(relative_velocity(a, b, r1, r2), surface_vr); - - cpFloat vbn = cpvdot(cpvsub(vb2, vb1), n); - cpFloat vrn = cpvdot(vr, n); - cpFloat vrt = cpvdot(vr, cpvperp(n)); - - cpFloat jbn = (con->bias - vbn)*nMass; - cpFloat jbnOld = con->jBias; - con->jBias = cpfmax(jbnOld + jbn, 0.0f); - - cpFloat jn = -(con->bounce + vrn)*nMass; - cpFloat jnOld = con->jnAcc; - con->jnAcc = cpfmax(jnOld + jn, 0.0f); - - cpFloat jtMax = friction*con->jnAcc; - cpFloat jt = -vrt*con->tMass; - cpFloat jtOld = con->jtAcc; - con->jtAcc = cpfclamp(jtOld + jt, -jtMax, jtMax); - - apply_bias_impulses(a, b, r1, r2, cpvmult(n, con->jBias - jbnOld)); - apply_impulses(a, b, r1, r2, cpvrotate(n, cpv(con->jnAcc - jnOld, con->jtAcc - jtOld))); - } -} diff --git a/3rdparty/chipmunk/src/cpArray.c b/3rdparty/chipmunk/src/cpArray.c deleted file mode 100644 index a1f8df526fc4..000000000000 --- a/3rdparty/chipmunk/src/cpArray.c +++ /dev/null @@ -1,101 +0,0 @@ -/* Copyright (c) 2013 Scott Lembcke and Howling Moon Software - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -#include - -#include "chipmunk/chipmunk_private.h" - - -cpArray * -cpArrayNew(int size) -{ - cpArray *arr = (cpArray *)cpcalloc(1, sizeof(cpArray)); - - arr->num = 0; - arr->max = (size ? size : 4); - arr->arr = (void **)cpcalloc(arr->max, sizeof(void*)); - - return arr; -} - -void -cpArrayFree(cpArray *arr) -{ - if(arr){ - cpfree(arr->arr); - arr->arr = NULL; - - cpfree(arr); - } -} - -void -cpArrayPush(cpArray *arr, void *object) -{ - if(arr->num == arr->max){ - arr->max = 3*(arr->max + 1)/2; - arr->arr = (void **)cprealloc(arr->arr, arr->max*sizeof(void*)); - } - - arr->arr[arr->num] = object; - arr->num++; -} - -void * -cpArrayPop(cpArray *arr) -{ - arr->num--; - - void *value = arr->arr[arr->num]; - arr->arr[arr->num] = NULL; - - return value; -} - -void -cpArrayDeleteObj(cpArray *arr, void *obj) -{ - for(int i=0; inum; i++){ - if(arr->arr[i] == obj){ - arr->num--; - - arr->arr[i] = arr->arr[arr->num]; - arr->arr[arr->num] = NULL; - - return; - } - } -} - -void -cpArrayFreeEach(cpArray *arr, void (freeFunc)(void*)) -{ - for(int i=0; inum; i++) freeFunc(arr->arr[i]); -} - -cpBool -cpArrayContains(cpArray *arr, void *ptr) -{ - for(int i=0; inum; i++) - if(arr->arr[i] == ptr) return cpTrue; - - return cpFalse; -} diff --git a/3rdparty/chipmunk/src/cpBBTree.c b/3rdparty/chipmunk/src/cpBBTree.c deleted file mode 100644 index 2cef7bc755c8..000000000000 --- a/3rdparty/chipmunk/src/cpBBTree.c +++ /dev/null @@ -1,896 +0,0 @@ -/* Copyright (c) 2013 Scott Lembcke and Howling Moon Software - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -#include "stdlib.h" -#include "stdio.h" - -#include "chipmunk/chipmunk_private.h" - -static inline cpSpatialIndexClass *Klass(void); - -typedef struct Node Node; -typedef struct Pair Pair; - -struct cpBBTree { - cpSpatialIndex spatialIndex; - cpBBTreeVelocityFunc velocityFunc; - - cpHashSet *leaves; - Node *root; - - Node *pooledNodes; - Pair *pooledPairs; - cpArray *allocatedBuffers; - - cpTimestamp stamp; -}; - -struct Node { - void *obj; - cpBB bb; - Node *parent; - - union { - // Internal nodes - struct { Node *a, *b; } children; - - // Leaves - struct { - cpTimestamp stamp; - Pair *pairs; - } leaf; - } node; -}; - -// Can't use anonymous unions and still get good x-compiler compatability -#define A node.children.a -#define B node.children.b -#define STAMP node.leaf.stamp -#define PAIRS node.leaf.pairs - -typedef struct Thread { - Pair *prev; - Node *leaf; - Pair *next; -} Thread; - -struct Pair { - Thread a, b; - cpCollisionID id; -}; - -//MARK: Misc Functions - -static inline cpBB -GetBB(cpBBTree *tree, void *obj) -{ - cpBB bb = tree->spatialIndex.bbfunc(obj); - - cpBBTreeVelocityFunc velocityFunc = tree->velocityFunc; - if(velocityFunc){ - cpFloat coef = 0.1f; - cpFloat x = (bb.r - bb.l)*coef; - cpFloat y = (bb.t - bb.b)*coef; - - cpVect v = cpvmult(velocityFunc(obj), 0.1f); - return cpBBNew(bb.l + cpfmin(-x, v.x), bb.b + cpfmin(-y, v.y), bb.r + cpfmax(x, v.x), bb.t + cpfmax(y, v.y)); - } else { - return bb; - } -} - -static inline cpBBTree * -GetTree(cpSpatialIndex *index) -{ - return (index && index->klass == Klass() ? (cpBBTree *)index : NULL); -} - -static inline Node * -GetRootIfTree(cpSpatialIndex *index){ - return (index && index->klass == Klass() ? ((cpBBTree *)index)->root : NULL); -} - -static inline cpBBTree * -GetMasterTree(cpBBTree *tree) -{ - cpBBTree *dynamicTree = GetTree(tree->spatialIndex.dynamicIndex); - return (dynamicTree ? dynamicTree : tree); -} - -static inline void -IncrementStamp(cpBBTree *tree) -{ - cpBBTree *dynamicTree = GetTree(tree->spatialIndex.dynamicIndex); - if(dynamicTree){ - dynamicTree->stamp++; - } else { - tree->stamp++; - } -} - -//MARK: Pair/Thread Functions - -static void -PairRecycle(cpBBTree *tree, Pair *pair) -{ - // Share the pool of the master tree. - // TODO: would be lovely to move the pairs stuff into an external data structure. - tree = GetMasterTree(tree); - - pair->a.next = tree->pooledPairs; - tree->pooledPairs = pair; -} - -static Pair * -PairFromPool(cpBBTree *tree) -{ - // Share the pool of the master tree. - // TODO: would be lovely to move the pairs stuff into an external data structure. - tree = GetMasterTree(tree); - - Pair *pair = tree->pooledPairs; - - if(pair){ - tree->pooledPairs = pair->a.next; - return pair; - } else { - // Pool is exhausted, make more - int count = CP_BUFFER_BYTES/sizeof(Pair); - cpAssertHard(count, "Internal Error: Buffer size is too small."); - - Pair *buffer = (Pair *)cpcalloc(1, CP_BUFFER_BYTES); - cpArrayPush(tree->allocatedBuffers, buffer); - - // push all but the first one, return the first instead - for(int i=1; ia.leaf == thread.leaf) next->a.prev = prev; else next->b.prev = prev; - } - - if(prev){ - if(prev->a.leaf == thread.leaf) prev->a.next = next; else prev->b.next = next; - } else { - thread.leaf->PAIRS = next; - } -} - -static void -PairsClear(Node *leaf, cpBBTree *tree) -{ - Pair *pair = leaf->PAIRS; - leaf->PAIRS = NULL; - - while(pair){ - if(pair->a.leaf == leaf){ - Pair *next = pair->a.next; - ThreadUnlink(pair->b); - PairRecycle(tree, pair); - pair = next; - } else { - Pair *next = pair->b.next; - ThreadUnlink(pair->a); - PairRecycle(tree, pair); - pair = next; - } - } -} - -static void -PairInsert(Node *a, Node *b, cpBBTree *tree) -{ - Pair *nextA = a->PAIRS, *nextB = b->PAIRS; - Pair *pair = PairFromPool(tree); - Pair temp = {{NULL, a, nextA},{NULL, b, nextB}, 0}; - - a->PAIRS = b->PAIRS = pair; - *pair = temp; - - if(nextA){ - if(nextA->a.leaf == a) nextA->a.prev = pair; else nextA->b.prev = pair; - } - - if(nextB){ - if(nextB->a.leaf == b) nextB->a.prev = pair; else nextB->b.prev = pair; - } -} - - -//MARK: Node Functions - -static void -NodeRecycle(cpBBTree *tree, Node *node) -{ - node->parent = tree->pooledNodes; - tree->pooledNodes = node; -} - -static Node * -NodeFromPool(cpBBTree *tree) -{ - Node *node = tree->pooledNodes; - - if(node){ - tree->pooledNodes = node->parent; - return node; - } else { - // Pool is exhausted, make more - int count = CP_BUFFER_BYTES/sizeof(Node); - cpAssertHard(count, "Internal Error: Buffer size is too small."); - - Node *buffer = (Node *)cpcalloc(1, CP_BUFFER_BYTES); - cpArrayPush(tree->allocatedBuffers, buffer); - - // push all but the first one, return the first instead - for(int i=1; iA = value; - value->parent = node; -} - -static inline void -NodeSetB(Node *node, Node *value) -{ - node->B = value; - value->parent = node; -} - -static Node * -NodeNew(cpBBTree *tree, Node *a, Node *b) -{ - Node *node = NodeFromPool(tree); - - node->obj = NULL; - node->bb = cpBBMerge(a->bb, b->bb); - node->parent = NULL; - - NodeSetA(node, a); - NodeSetB(node, b); - - return node; -} - -static inline cpBool -NodeIsLeaf(Node *node) -{ - return (node->obj != NULL); -} - -static inline Node * -NodeOther(Node *node, Node *child) -{ - return (node->A == child ? node->B : node->A); -} - -static inline void -NodeReplaceChild(Node *parent, Node *child, Node *value, cpBBTree *tree) -{ - cpAssertSoft(!NodeIsLeaf(parent), "Internal Error: Cannot replace child of a leaf."); - cpAssertSoft(child == parent->A || child == parent->B, "Internal Error: Node is not a child of parent."); - - if(parent->A == child){ - NodeRecycle(tree, parent->A); - NodeSetA(parent, value); - } else { - NodeRecycle(tree, parent->B); - NodeSetB(parent, value); - } - - for(Node *node=parent; node; node = node->parent){ - node->bb = cpBBMerge(node->A->bb, node->B->bb); - } -} - -//MARK: Subtree Functions - -static inline cpFloat -cpBBProximity(cpBB a, cpBB b) -{ - return cpfabs(a.l + a.r - b.l - b.r) + cpfabs(a.b + a.t - b.b - b.t); -} - -static Node * -SubtreeInsert(Node *subtree, Node *leaf, cpBBTree *tree) -{ - if(subtree == NULL){ - return leaf; - } else if(NodeIsLeaf(subtree)){ - return NodeNew(tree, leaf, subtree); - } else { - cpFloat cost_a = cpBBArea(subtree->B->bb) + cpBBMergedArea(subtree->A->bb, leaf->bb); - cpFloat cost_b = cpBBArea(subtree->A->bb) + cpBBMergedArea(subtree->B->bb, leaf->bb); - - if(cost_a == cost_b){ - cost_a = cpBBProximity(subtree->A->bb, leaf->bb); - cost_b = cpBBProximity(subtree->B->bb, leaf->bb); - } - - if(cost_b < cost_a){ - NodeSetB(subtree, SubtreeInsert(subtree->B, leaf, tree)); - } else { - NodeSetA(subtree, SubtreeInsert(subtree->A, leaf, tree)); - } - - subtree->bb = cpBBMerge(subtree->bb, leaf->bb); - return subtree; - } -} - -static void -SubtreeQuery(Node *subtree, void *obj, cpBB bb, cpSpatialIndexQueryFunc func, void *data) -{ - if(cpBBIntersects(subtree->bb, bb)){ - if(NodeIsLeaf(subtree)){ - func(obj, subtree->obj, 0, data); - } else { - SubtreeQuery(subtree->A, obj, bb, func, data); - SubtreeQuery(subtree->B, obj, bb, func, data); - } - } -} - - -static cpFloat -SubtreeSegmentQuery(Node *subtree, void *obj, cpVect a, cpVect b, cpFloat t_exit, cpSpatialIndexSegmentQueryFunc func, void *data) -{ - if(NodeIsLeaf(subtree)){ - return func(obj, subtree->obj, data); - } else { - cpFloat t_a = cpBBSegmentQuery(subtree->A->bb, a, b); - cpFloat t_b = cpBBSegmentQuery(subtree->B->bb, a, b); - - if(t_a < t_b){ - if(t_a < t_exit) t_exit = cpfmin(t_exit, SubtreeSegmentQuery(subtree->A, obj, a, b, t_exit, func, data)); - if(t_b < t_exit) t_exit = cpfmin(t_exit, SubtreeSegmentQuery(subtree->B, obj, a, b, t_exit, func, data)); - } else { - if(t_b < t_exit) t_exit = cpfmin(t_exit, SubtreeSegmentQuery(subtree->B, obj, a, b, t_exit, func, data)); - if(t_a < t_exit) t_exit = cpfmin(t_exit, SubtreeSegmentQuery(subtree->A, obj, a, b, t_exit, func, data)); - } - - return t_exit; - } -} - -static void -SubtreeRecycle(cpBBTree *tree, Node *node) -{ - if(!NodeIsLeaf(node)){ - SubtreeRecycle(tree, node->A); - SubtreeRecycle(tree, node->B); - NodeRecycle(tree, node); - } -} - -static inline Node * -SubtreeRemove(Node *subtree, Node *leaf, cpBBTree *tree) -{ - if(leaf == subtree){ - return NULL; - } else { - Node *parent = leaf->parent; - if(parent == subtree){ - Node *other = NodeOther(subtree, leaf); - other->parent = subtree->parent; - NodeRecycle(tree, subtree); - return other; - } else { - NodeReplaceChild(parent->parent, parent, NodeOther(parent, leaf), tree); - return subtree; - } - } -} - -//MARK: Marking Functions - -typedef struct MarkContext { - cpBBTree *tree; - Node *staticRoot; - cpSpatialIndexQueryFunc func; - void *data; -} MarkContext; - -static void -MarkLeafQuery(Node *subtree, Node *leaf, cpBool left, MarkContext *context) -{ - if(cpBBIntersects(leaf->bb, subtree->bb)){ - if(NodeIsLeaf(subtree)){ - if(left){ - PairInsert(leaf, subtree, context->tree); - } else { - if(subtree->STAMP < leaf->STAMP) PairInsert(subtree, leaf, context->tree); - context->func(leaf->obj, subtree->obj, 0, context->data); - } - } else { - MarkLeafQuery(subtree->A, leaf, left, context); - MarkLeafQuery(subtree->B, leaf, left, context); - } - } -} - -static void -MarkLeaf(Node *leaf, MarkContext *context) -{ - cpBBTree *tree = context->tree; - if(leaf->STAMP == GetMasterTree(tree)->stamp){ - Node *staticRoot = context->staticRoot; - if(staticRoot) MarkLeafQuery(staticRoot, leaf, cpFalse, context); - - for(Node *node = leaf; node->parent; node = node->parent){ - if(node == node->parent->A){ - MarkLeafQuery(node->parent->B, leaf, cpTrue, context); - } else { - MarkLeafQuery(node->parent->A, leaf, cpFalse, context); - } - } - } else { - Pair *pair = leaf->PAIRS; - while(pair){ - if(leaf == pair->b.leaf){ - pair->id = context->func(pair->a.leaf->obj, leaf->obj, pair->id, context->data); - pair = pair->b.next; - } else { - pair = pair->a.next; - } - } - } -} - -static void -MarkSubtree(Node *subtree, MarkContext *context) -{ - if(NodeIsLeaf(subtree)){ - MarkLeaf(subtree, context); - } else { - MarkSubtree(subtree->A, context); - MarkSubtree(subtree->B, context); // TODO: Force TCO here? - } -} - -//MARK: Leaf Functions - -static Node * -LeafNew(cpBBTree *tree, void *obj, cpBB bb) -{ - Node *node = NodeFromPool(tree); - node->obj = obj; - node->bb = GetBB(tree, obj); - - node->parent = NULL; - node->STAMP = 0; - node->PAIRS = NULL; - - return node; -} - -static cpBool -LeafUpdate(Node *leaf, cpBBTree *tree) -{ - Node *root = tree->root; - cpBB bb = tree->spatialIndex.bbfunc(leaf->obj); - - if(!cpBBContainsBB(leaf->bb, bb)){ - leaf->bb = GetBB(tree, leaf->obj); - - root = SubtreeRemove(root, leaf, tree); - tree->root = SubtreeInsert(root, leaf, tree); - - PairsClear(leaf, tree); - leaf->STAMP = GetMasterTree(tree)->stamp; - - return cpTrue; - } else { - return cpFalse; - } -} - -static cpCollisionID VoidQueryFunc(void *obj1, void *obj2, cpCollisionID id, void *data){return id;} - -static void -LeafAddPairs(Node *leaf, cpBBTree *tree) -{ - cpSpatialIndex *dynamicIndex = tree->spatialIndex.dynamicIndex; - if(dynamicIndex){ - Node *dynamicRoot = GetRootIfTree(dynamicIndex); - if(dynamicRoot){ - cpBBTree *dynamicTree = GetTree(dynamicIndex); - MarkContext context = {dynamicTree, NULL, NULL, NULL}; - MarkLeafQuery(dynamicRoot, leaf, cpTrue, &context); - } - } else { - Node *staticRoot = GetRootIfTree(tree->spatialIndex.staticIndex); - MarkContext context = {tree, staticRoot, VoidQueryFunc, NULL}; - MarkLeaf(leaf, &context); - } -} - -//MARK: Memory Management Functions - -cpBBTree * -cpBBTreeAlloc(void) -{ - return (cpBBTree *)cpcalloc(1, sizeof(cpBBTree)); -} - -static int -leafSetEql(void *obj, Node *node) -{ - return (obj == node->obj); -} - -static void * -leafSetTrans(void *obj, cpBBTree *tree) -{ - return LeafNew(tree, obj, tree->spatialIndex.bbfunc(obj)); -} - -cpSpatialIndex * -cpBBTreeInit(cpBBTree *tree, cpSpatialIndexBBFunc bbfunc, cpSpatialIndex *staticIndex) -{ - cpSpatialIndexInit((cpSpatialIndex *)tree, Klass(), bbfunc, staticIndex); - - tree->velocityFunc = NULL; - - tree->leaves = cpHashSetNew(0, (cpHashSetEqlFunc)leafSetEql); - tree->root = NULL; - - tree->pooledNodes = NULL; - tree->allocatedBuffers = cpArrayNew(0); - - tree->stamp = 0; - - return (cpSpatialIndex *)tree; -} - -void -cpBBTreeSetVelocityFunc(cpSpatialIndex *index, cpBBTreeVelocityFunc func) -{ - if(index->klass != Klass()){ - cpAssertWarn(cpFalse, "Ignoring cpBBTreeSetVelocityFunc() call to non-tree spatial index."); - return; - } - - ((cpBBTree *)index)->velocityFunc = func; -} - -cpSpatialIndex * -cpBBTreeNew(cpSpatialIndexBBFunc bbfunc, cpSpatialIndex *staticIndex) -{ - return cpBBTreeInit(cpBBTreeAlloc(), bbfunc, staticIndex); -} - -static void -cpBBTreeDestroy(cpBBTree *tree) -{ - cpHashSetFree(tree->leaves); - - if(tree->allocatedBuffers) cpArrayFreeEach(tree->allocatedBuffers, cpfree); - cpArrayFree(tree->allocatedBuffers); -} - -//MARK: Insert/Remove - -static void -cpBBTreeInsert(cpBBTree *tree, void *obj, cpHashValue hashid) -{ - Node *leaf = (Node *)cpHashSetInsert(tree->leaves, hashid, obj, (cpHashSetTransFunc)leafSetTrans, tree); - - Node *root = tree->root; - tree->root = SubtreeInsert(root, leaf, tree); - - leaf->STAMP = GetMasterTree(tree)->stamp; - LeafAddPairs(leaf, tree); - IncrementStamp(tree); -} - -static void -cpBBTreeRemove(cpBBTree *tree, void *obj, cpHashValue hashid) -{ - Node *leaf = (Node *)cpHashSetRemove(tree->leaves, hashid, obj); - - tree->root = SubtreeRemove(tree->root, leaf, tree); - PairsClear(leaf, tree); - NodeRecycle(tree, leaf); -} - -static cpBool -cpBBTreeContains(cpBBTree *tree, void *obj, cpHashValue hashid) -{ - return (cpHashSetFind(tree->leaves, hashid, obj) != NULL); -} - -//MARK: Reindex - -static void LeafUpdateWrap(Node *leaf, cpBBTree *tree) {LeafUpdate(leaf, tree);} - -static void -cpBBTreeReindexQuery(cpBBTree *tree, cpSpatialIndexQueryFunc func, void *data) -{ - if(!tree->root) return; - - // LeafUpdate() may modify tree->root. Don't cache it. - cpHashSetEach(tree->leaves, (cpHashSetIteratorFunc)LeafUpdateWrap, tree); - - cpSpatialIndex *staticIndex = tree->spatialIndex.staticIndex; - Node *staticRoot = (staticIndex && staticIndex->klass == Klass() ? ((cpBBTree *)staticIndex)->root : NULL); - - MarkContext context = {tree, staticRoot, func, data}; - MarkSubtree(tree->root, &context); - if(staticIndex && !staticRoot) cpSpatialIndexCollideStatic((cpSpatialIndex *)tree, staticIndex, func, data); - - IncrementStamp(tree); -} - -static void -cpBBTreeReindex(cpBBTree *tree) -{ - cpBBTreeReindexQuery(tree, VoidQueryFunc, NULL); -} - -static void -cpBBTreeReindexObject(cpBBTree *tree, void *obj, cpHashValue hashid) -{ - Node *leaf = (Node *)cpHashSetFind(tree->leaves, hashid, obj); - if(leaf){ - if(LeafUpdate(leaf, tree)) LeafAddPairs(leaf, tree); - IncrementStamp(tree); - } -} - -//MARK: Query - -static void -cpBBTreeSegmentQuery(cpBBTree *tree, void *obj, cpVect a, cpVect b, cpFloat t_exit, cpSpatialIndexSegmentQueryFunc func, void *data) -{ - Node *root = tree->root; - if(root) SubtreeSegmentQuery(root, obj, a, b, t_exit, func, data); -} - -static void -cpBBTreeQuery(cpBBTree *tree, void *obj, cpBB bb, cpSpatialIndexQueryFunc func, void *data) -{ - if(tree->root) SubtreeQuery(tree->root, obj, bb, func, data); -} - -//MARK: Misc - -static int -cpBBTreeCount(cpBBTree *tree) -{ - return cpHashSetCount(tree->leaves); -} - -typedef struct eachContext { - cpSpatialIndexIteratorFunc func; - void *data; -} eachContext; - -static void each_helper(Node *node, eachContext *context){context->func(node->obj, context->data);} - -static void -cpBBTreeEach(cpBBTree *tree, cpSpatialIndexIteratorFunc func, void *data) -{ - eachContext context = {func, data}; - cpHashSetEach(tree->leaves, (cpHashSetIteratorFunc)each_helper, &context); -} - -static cpSpatialIndexClass klass = { - (cpSpatialIndexDestroyImpl)cpBBTreeDestroy, - - (cpSpatialIndexCountImpl)cpBBTreeCount, - (cpSpatialIndexEachImpl)cpBBTreeEach, - - (cpSpatialIndexContainsImpl)cpBBTreeContains, - (cpSpatialIndexInsertImpl)cpBBTreeInsert, - (cpSpatialIndexRemoveImpl)cpBBTreeRemove, - - (cpSpatialIndexReindexImpl)cpBBTreeReindex, - (cpSpatialIndexReindexObjectImpl)cpBBTreeReindexObject, - (cpSpatialIndexReindexQueryImpl)cpBBTreeReindexQuery, - - (cpSpatialIndexQueryImpl)cpBBTreeQuery, - (cpSpatialIndexSegmentQueryImpl)cpBBTreeSegmentQuery, -}; - -static inline cpSpatialIndexClass *Klass(){return &klass;} - - -//MARK: Tree Optimization - -static int -cpfcompare(const cpFloat *a, const cpFloat *b){ - return (*a < *b ? -1 : (*b < *a ? 1 : 0)); -} - -static void -fillNodeArray(Node *node, Node ***cursor){ - (**cursor) = node; - (*cursor)++; -} - -static Node * -partitionNodes(cpBBTree *tree, Node **nodes, int count) -{ - if(count == 1){ - return nodes[0]; - } else if(count == 2) { - return NodeNew(tree, nodes[0], nodes[1]); - } - - // Find the AABB for these nodes - cpBB bb = nodes[0]->bb; - for(int i=1; ibb); - - // Split it on it's longest axis - cpBool splitWidth = (bb.r - bb.l > bb.t - bb.b); - - // Sort the bounds and use the median as the splitting point - cpFloat *bounds = (cpFloat *)cpcalloc(count*2, sizeof(cpFloat)); - if(splitWidth){ - for(int i=0; ibb.l; - bounds[2*i + 1] = nodes[i]->bb.r; - } - } else { - for(int i=0; ibb.b; - bounds[2*i + 1] = nodes[i]->bb.t; - } - } - - qsort(bounds, count*2, sizeof(cpFloat), (int (*)(const void *, const void *))cpfcompare); - cpFloat split = (bounds[count - 1] + bounds[count])*0.5f; // use the medain as the split - cpfree(bounds); - - // Generate the child BBs - cpBB a = bb, b = bb; - if(splitWidth) a.r = b.l = split; else a.t = b.b = split; - - // Partition the nodes - int right = count; - for(int left=0; left < right;){ - Node *node = nodes[left]; - if(cpBBMergedArea(node->bb, b) < cpBBMergedArea(node->bb, a)){ -// if(cpBBProximity(node->bb, b) < cpBBProximity(node->bb, a)){ - right--; - nodes[left] = nodes[right]; - nodes[right] = node; - } else { - left++; - } - } - - if(right == count){ - Node *node = NULL; - for(int i=0; iroot; -// Node *node = root; -// int bit = 0; -// unsigned int path = tree->opath; -// -// while(!NodeIsLeaf(node)){ -// node = (path&(1<a : node->b); -// bit = (bit + 1)&(sizeof(unsigned int)*8 - 1); -// } -// -// root = subtreeRemove(root, node, tree); -// tree->root = subtreeInsert(root, node, tree); -// } -//} - -void -cpBBTreeOptimize(cpSpatialIndex *index) -{ - if(index->klass != &klass){ - cpAssertWarn(cpFalse, "Ignoring cpBBTreeOptimize() call to non-tree spatial index."); - return; - } - - cpBBTree *tree = (cpBBTree *)index; - Node *root = tree->root; - if(!root) return; - - int count = cpBBTreeCount(tree); - Node **nodes = (Node **)cpcalloc(count, sizeof(Node *)); - Node **cursor = nodes; - - cpHashSetEach(tree->leaves, (cpHashSetIteratorFunc)fillNodeArray, &cursor); - - SubtreeRecycle(tree, root); - tree->root = partitionNodes(tree, nodes, count); - cpfree(nodes); -} - -//MARK: Debug Draw - -//#define CP_BBTREE_DEBUG_DRAW -#ifdef CP_BBTREE_DEBUG_DRAW -#include "OpenGL/gl.h" -#include "OpenGL/glu.h" -#include - -static void -NodeRender(Node *node, int depth) -{ - if(!NodeIsLeaf(node) && depth <= 10){ - NodeRender(node->a, depth + 1); - NodeRender(node->b, depth + 1); - } - - cpBB bb = node->bb; - -// GLfloat v = depth/2.0f; -// glColor3f(1.0f - v, v, 0.0f); - glLineWidth(cpfmax(5.0f - depth, 1.0f)); - glBegin(GL_LINES); { - glVertex2f(bb.l, bb.b); - glVertex2f(bb.l, bb.t); - - glVertex2f(bb.l, bb.t); - glVertex2f(bb.r, bb.t); - - glVertex2f(bb.r, bb.t); - glVertex2f(bb.r, bb.b); - - glVertex2f(bb.r, bb.b); - glVertex2f(bb.l, bb.b); - }; glEnd(); -} - -void -cpBBTreeRenderDebug(cpSpatialIndex *index){ - if(index->klass != &klass){ - cpAssertWarn(cpFalse, "Ignoring cpBBTreeRenderDebug() call to non-tree spatial index."); - return; - } - - cpBBTree *tree = (cpBBTree *)index; - if(tree->root) NodeRender(tree->root, 0); -} -#endif diff --git a/3rdparty/chipmunk/src/cpBody.c b/3rdparty/chipmunk/src/cpBody.c deleted file mode 100644 index 4ba4b494ec52..000000000000 --- a/3rdparty/chipmunk/src/cpBody.c +++ /dev/null @@ -1,626 +0,0 @@ -/* Copyright (c) 2013 Scott Lembcke and Howling Moon Software - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -#include -#include - -#include "chipmunk/chipmunk_private.h" - -cpBody* -cpBodyAlloc(void) -{ - return (cpBody *)cpcalloc(1, sizeof(cpBody)); -} - -cpBody * -cpBodyInit(cpBody *body, cpFloat mass, cpFloat moment) -{ - body->space = NULL; - body->shapeList = NULL; - body->arbiterList = NULL; - body->constraintList = NULL; - - body->velocity_func = cpBodyUpdateVelocity; - body->position_func = cpBodyUpdatePosition; - - body->sleeping.root = NULL; - body->sleeping.next = NULL; - body->sleeping.idleTime = 0.0f; - - body->p = cpvzero; - body->v = cpvzero; - body->f = cpvzero; - - body->w = 0.0f; - body->t = 0.0f; - - body->v_bias = cpvzero; - body->w_bias = 0.0f; - - body->userData = NULL; - - // Setters must be called after full initialization so the sanity checks don't assert on garbage data. - cpBodySetMass(body, mass); - cpBodySetMoment(body, moment); - cpBodySetAngle(body, 0.0f); - - return body; -} - -cpBody* -cpBodyNew(cpFloat mass, cpFloat moment) -{ - return cpBodyInit(cpBodyAlloc(), mass, moment); -} - -cpBody* -cpBodyNewKinematic() -{ - cpBody *body = cpBodyNew(0.0f, 0.0f); - cpBodySetType(body, CP_BODY_TYPE_KINEMATIC); - - return body; -} - -cpBody* -cpBodyNewStatic() -{ - cpBody *body = cpBodyNew(0.0f, 0.0f); - cpBodySetType(body, CP_BODY_TYPE_STATIC); - - return body; -} - -void cpBodyDestroy(cpBody *body){} - -void -cpBodyFree(cpBody *body) -{ - if(body){ - cpBodyDestroy(body); - cpfree(body); - } -} - -#ifdef NDEBUG - #define cpAssertSaneBody(body) -#else - static void cpv_assert_nan(cpVect v, const char *message){cpAssertHard(v.x == v.x && v.y == v.y, message);} - static void cpv_assert_infinite(cpVect v, const char *message){cpAssertHard(cpfabs(v.x) != INFINITY && cpfabs(v.y) != INFINITY, message);} - static void cpv_assert_sane(cpVect v, const char *message){cpv_assert_nan(v, message); cpv_assert_infinite(v, message);} - - static void - cpBodySanityCheck(const cpBody *body) - { - cpAssertHard(body->m == body->m && body->m_inv == body->m_inv, "Body's mass is NaN."); - cpAssertHard(body->i == body->i && body->i_inv == body->i_inv, "Body's moment is NaN."); - cpAssertHard(body->m >= 0.0f, "Body's mass is negative."); - cpAssertHard(body->i >= 0.0f, "Body's moment is negative."); - - cpv_assert_sane(body->p, "Body's position is invalid."); - cpv_assert_sane(body->v, "Body's velocity is invalid."); - cpv_assert_sane(body->f, "Body's force is invalid."); - - cpAssertHard(body->a == body->a && cpfabs(body->a) != INFINITY, "Body's angle is invalid."); - cpAssertHard(body->w == body->w && cpfabs(body->w) != INFINITY, "Body's angular velocity is invalid."); - cpAssertHard(body->t == body->t && cpfabs(body->t) != INFINITY, "Body's torque is invalid."); - } - - #define cpAssertSaneBody(body) cpBodySanityCheck(body) -#endif - -cpBool -cpBodyIsSleeping(const cpBody *body) -{ - return (body->sleeping.root != ((cpBody*)0)); -} - -cpBodyType -cpBodyGetType(cpBody *body) -{ - if(body->sleeping.idleTime == INFINITY){ - return CP_BODY_TYPE_STATIC; - } else if(body->m == INFINITY){ - return CP_BODY_TYPE_KINEMATIC; - } else { - return CP_BODY_TYPE_DYNAMIC; - } -} - -void -cpBodySetType(cpBody *body, cpBodyType type) -{ - cpBodyType oldType = cpBodyGetType(body); - if(oldType == type) return; - - // Static bodies have their idle timers set to infinity. - // Non-static bodies should have their idle timer reset. - body->sleeping.idleTime = (type == CP_BODY_TYPE_STATIC ? INFINITY : 0.0f); - - if(type == CP_BODY_TYPE_DYNAMIC){ - body->m = body->i = 0.0f; - body->m_inv = body->i_inv = INFINITY; - - cpBodyAccumulateMassFromShapes(body); - } else { - body->m = body->i = INFINITY; - body->m_inv = body->i_inv = 0.0f; - - body->v = cpvzero; - body->w = 0.0f; - } - - // If the body is added to a space already, we'll need to update some space data structures. - cpSpace *space = cpBodyGetSpace(body); - if(space != NULL){ - cpAssertSpaceUnlocked(space); - - if(oldType == CP_BODY_TYPE_STATIC){ - // TODO This is probably not necessary -// cpBodyActivateStatic(body, NULL); - } else { - cpBodyActivate(body); - } - - // Move the bodies to the correct array. - cpArray *fromArray = cpSpaceArrayForBodyType(space, oldType); - cpArray *toArray = cpSpaceArrayForBodyType(space, type); - if(fromArray != toArray){ - cpArrayDeleteObj(fromArray, body); - cpArrayPush(toArray, body); - } - - // Move the body's shapes to the correct spatial index. - cpSpatialIndex *fromIndex = (oldType == CP_BODY_TYPE_STATIC ? space->staticShapes : space->dynamicShapes); - cpSpatialIndex *toIndex = (type == CP_BODY_TYPE_STATIC ? space->staticShapes : space->dynamicShapes); - if(fromIndex != toIndex){ - CP_BODY_FOREACH_SHAPE(body, shape){ - cpSpatialIndexRemove(fromIndex, shape, shape->hashid); - cpSpatialIndexInsert(toIndex, shape, shape->hashid); - } - } - } -} - - - -// Should *only* be called when shapes with mass info are modified, added or removed. -void -cpBodyAccumulateMassFromShapes(cpBody *body) -{ - if(body == NULL || cpBodyGetType(body) != CP_BODY_TYPE_DYNAMIC) return; - - // Reset the body's mass data. - body->m = body->i = 0.0f; - body->cog = cpvzero; - - // Cache the position to realign it at the end. - cpVect pos = cpBodyGetPosition(body); - - // Accumulate mass from shapes. - CP_BODY_FOREACH_SHAPE(body, shape){ - struct cpShapeMassInfo *info = &shape->massInfo; - cpFloat m = info->m; - - if(m > 0.0f){ - cpFloat msum = body->m + m; - - body->i += m*info->i + cpvdistsq(body->cog, info->cog)*(m*body->m)/msum; - body->cog = cpvlerp(body->cog, info->cog, m/msum); - body->m = msum; - } - } - - // Recalculate the inverses. - body->m_inv = 1.0f/body->m; - body->i_inv = 1.0f/body->i; - - // Realign the body since the CoG has probably moved. - cpBodySetPosition(body, pos); - cpAssertSaneBody(body); -} - -cpSpace * -cpBodyGetSpace(const cpBody *body) -{ - return body->space; -} - -cpFloat -cpBodyGetMass(const cpBody *body) -{ - return body->m; -} - -void -cpBodySetMass(cpBody *body, cpFloat mass) -{ - cpAssertHard(cpBodyGetType(body) == CP_BODY_TYPE_DYNAMIC, "You cannot set the mass of kinematic or static bodies."); - cpAssertHard(0.0f <= mass && mass < INFINITY, "Mass must be positive and finite."); - - cpBodyActivate(body); - body->m = mass; - body->m_inv = mass == 0.0f ? INFINITY : 1.0f/mass; - cpAssertSaneBody(body); -} - -cpFloat -cpBodyGetMoment(const cpBody *body) -{ - return body->i; -} - -void -cpBodySetMoment(cpBody *body, cpFloat moment) -{ - cpAssertHard(moment >= 0.0f, "Moment of Inertia must be positive."); - - cpBodyActivate(body); - body->i = moment; - body->i_inv = moment == 0.0f ? INFINITY : 1.0f/moment; - cpAssertSaneBody(body); -} - -cpVect -cpBodyGetRotation(const cpBody *body) -{ - return cpv(body->transform.a, body->transform.b); -} - -void -cpBodyAddShape(cpBody *body, cpShape *shape) -{ - cpShape *next = body->shapeList; - if(next) next->prev = shape; - - shape->next = next; - body->shapeList = shape; - - if(shape->massInfo.m > 0.0f){ - cpBodyAccumulateMassFromShapes(body); - } -} - -void -cpBodyRemoveShape(cpBody *body, cpShape *shape) -{ - cpShape *prev = shape->prev; - cpShape *next = shape->next; - - if(prev){ - prev->next = next; - } else { - body->shapeList = next; - } - - if(next){ - next->prev = prev; - } - - shape->prev = NULL; - shape->next = NULL; - - if(cpBodyGetType(body) == CP_BODY_TYPE_DYNAMIC && shape->massInfo.m > 0.0f){ - cpBodyAccumulateMassFromShapes(body); - } -} - -static cpConstraint * -filterConstraints(cpConstraint *node, cpBody *body, cpConstraint *filter) -{ - if(node == filter){ - return cpConstraintNext(node, body); - } else if(node->a == body){ - node->next_a = filterConstraints(node->next_a, body, filter); - } else { - node->next_b = filterConstraints(node->next_b, body, filter); - } - - return node; -} - -void -cpBodyRemoveConstraint(cpBody *body, cpConstraint *constraint) -{ - body->constraintList = filterConstraints(body->constraintList, body, constraint); -} - -// 'p' is the position of the CoG -static void -SetTransform(cpBody *body, cpVect p, cpFloat a) -{ - cpVect rot = cpvforangle(a); - cpVect c = body->cog; - - body->transform = cpTransformNewTranspose( - rot.x, -rot.y, p.x - (c.x*rot.x - c.y*rot.y), - rot.y, rot.x, p.y - (c.x*rot.y + c.y*rot.x) - ); -} - -static inline cpFloat -SetAngle(cpBody *body, cpFloat a) -{ - body->a = a; - cpAssertSaneBody(body); - - return a; -} - -cpVect -cpBodyGetPosition(const cpBody *body) -{ - return cpTransformPoint(body->transform, cpvzero); -} - -void -cpBodySetPosition(cpBody *body, cpVect position) -{ - cpBodyActivate(body); - cpVect p = body->p = cpvadd(cpTransformVect(body->transform, body->cog), position); - cpAssertSaneBody(body); - - SetTransform(body, p, body->a); -} - -cpVect -cpBodyGetCenterOfGravity(const cpBody *body) -{ - return body->cog; -} - -void -cpBodySetCenterOfGravity(cpBody *body, cpVect cog) -{ - cpBodyActivate(body); - body->cog = cog; - cpAssertSaneBody(body); -} - -cpVect -cpBodyGetVelocity(const cpBody *body) -{ - return body->v; -} - -void -cpBodySetVelocity(cpBody *body, cpVect velocity) -{ - cpBodyActivate(body); - body->v = velocity; - cpAssertSaneBody(body); -} - -cpVect -cpBodyGetForce(const cpBody *body) -{ - return body->f; -} - -void -cpBodySetForce(cpBody *body, cpVect force) -{ - cpBodyActivate(body); - body->f = force; - cpAssertSaneBody(body); -} - -cpFloat -cpBodyGetAngle(const cpBody *body) -{ - return body->a; -} - -void -cpBodySetAngle(cpBody *body, cpFloat angle) -{ - cpBodyActivate(body); - SetAngle(body, angle); - - SetTransform(body, body->p, angle); -} - -cpFloat -cpBodyGetAngularVelocity(const cpBody *body) -{ - return body->w; -} - -void -cpBodySetAngularVelocity(cpBody *body, cpFloat angularVelocity) -{ - cpBodyActivate(body); - body->w = angularVelocity; - cpAssertSaneBody(body); -} - -cpFloat -cpBodyGetTorque(const cpBody *body) -{ - return body->t; -} - -void -cpBodySetTorque(cpBody *body, cpFloat torque) -{ - cpBodyActivate(body); - body->t = torque; - cpAssertSaneBody(body); -} - -cpDataPointer -cpBodyGetUserData(const cpBody *body) -{ - return body->userData; -} - -void -cpBodySetUserData(cpBody *body, cpDataPointer userData) -{ - body->userData = userData; -} - -void -cpBodySetVelocityUpdateFunc(cpBody *body, cpBodyVelocityFunc velocityFunc) -{ - body->velocity_func = velocityFunc; -} - -void -cpBodySetPositionUpdateFunc(cpBody *body, cpBodyPositionFunc positionFunc) -{ - body->position_func = positionFunc; -} - -void -cpBodyUpdateVelocity(cpBody *body, cpVect gravity, cpFloat damping, cpFloat dt) -{ - // Skip kinematic bodies. - if(cpBodyGetType(body) == CP_BODY_TYPE_KINEMATIC) return; - - cpAssertSoft(body->m > 0.0f && body->i > 0.0f, "Body's mass and moment must be positive to simulate. (Mass: %f Moment: %f)", body->m, body->i); - - body->v = cpvadd(cpvmult(body->v, damping), cpvmult(cpvadd(gravity, cpvmult(body->f, body->m_inv)), dt)); - body->w = body->w*damping + body->t*body->i_inv*dt; - - // Reset forces. - body->f = cpvzero; - body->t = 0.0f; - - cpAssertSaneBody(body); -} - -void -cpBodyUpdatePosition(cpBody *body, cpFloat dt) -{ - cpVect p = body->p = cpvadd(body->p, cpvmult(cpvadd(body->v, body->v_bias), dt)); - cpFloat a = SetAngle(body, body->a + (body->w + body->w_bias)*dt); - SetTransform(body, p, a); - - body->v_bias = cpvzero; - body->w_bias = 0.0f; - - cpAssertSaneBody(body); -} - -cpVect -cpBodyLocalToWorld(const cpBody *body, const cpVect point) -{ - return cpTransformPoint(body->transform, point); -} - -cpVect -cpBodyWorldToLocal(const cpBody *body, const cpVect point) -{ - return cpTransformPoint(cpTransformRigidInverse(body->transform), point); -} - -void -cpBodyApplyForceAtWorldPoint(cpBody *body, cpVect force, cpVect point) -{ - cpBodyActivate(body); - body->f = cpvadd(body->f, force); - - cpVect r = cpvsub(point, cpTransformPoint(body->transform, body->cog)); - body->t += cpvcross(r, force); -} - -void -cpBodyApplyForceAtLocalPoint(cpBody *body, cpVect force, cpVect point) -{ - cpBodyApplyForceAtWorldPoint(body, cpTransformVect(body->transform, force), cpTransformPoint(body->transform, point)); -} - -void -cpBodyApplyImpulseAtWorldPoint(cpBody *body, cpVect impulse, cpVect point) -{ - cpBodyActivate(body); - - cpVect r = cpvsub(point, cpTransformPoint(body->transform, body->cog)); - apply_impulse(body, impulse, r); -} - -void -cpBodyApplyImpulseAtLocalPoint(cpBody *body, cpVect impulse, cpVect point) -{ - cpBodyApplyImpulseAtWorldPoint(body, cpTransformVect(body->transform, impulse), cpTransformPoint(body->transform, point)); -} - -cpVect -cpBodyGetVelocityAtLocalPoint(const cpBody *body, cpVect point) -{ - cpVect r = cpTransformVect(body->transform, cpvsub(point, body->cog)); - return cpvadd(body->v, cpvmult(cpvperp(r), body->w)); -} - -cpVect -cpBodyGetVelocityAtWorldPoint(const cpBody *body, cpVect point) -{ - cpVect r = cpvsub(point, cpTransformPoint(body->transform, body->cog)); - return cpvadd(body->v, cpvmult(cpvperp(r), body->w)); -} - -cpFloat -cpBodyKineticEnergy(const cpBody *body) -{ - // Need to do some fudging to avoid NaNs - cpFloat vsq = cpvdot(body->v, body->v); - cpFloat wsq = body->w*body->w; - return (vsq ? vsq*body->m : 0.0f) + (wsq ? wsq*body->i : 0.0f); -} - -void -cpBodyEachShape(cpBody *body, cpBodyShapeIteratorFunc func, void *data) -{ - cpShape *shape = body->shapeList; - while(shape){ - cpShape *next = shape->next; - func(body, shape, data); - shape = next; - } -} - -void -cpBodyEachConstraint(cpBody *body, cpBodyConstraintIteratorFunc func, void *data) -{ - cpConstraint *constraint = body->constraintList; - while(constraint){ - cpConstraint *next = cpConstraintNext(constraint, body); - func(body, constraint, data); - constraint = next; - } -} - -void -cpBodyEachArbiter(cpBody *body, cpBodyArbiterIteratorFunc func, void *data) -{ - cpArbiter *arb = body->arbiterList; - while(arb){ - cpArbiter *next = cpArbiterNext(arb, body); - - cpBool swapped = arb->swapped; { - arb->swapped = (body == arb->body_b); - func(body, arb, data); - } arb->swapped = swapped; - - arb = next; - } -} diff --git a/3rdparty/chipmunk/src/cpCollision.c b/3rdparty/chipmunk/src/cpCollision.c deleted file mode 100644 index 33b3f59917db..000000000000 --- a/3rdparty/chipmunk/src/cpCollision.c +++ /dev/null @@ -1,726 +0,0 @@ -/* Copyright (c) 2013 Scott Lembcke and Howling Moon Software - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -#include -#include - -#include "chipmunk/chipmunk_private.h" -#include "chipmunk/cpRobust.h" - -#if DEBUG && 0 -#include "ChipmunkDemo.h" -#define DRAW_ALL 0 -#define DRAW_GJK (0 || DRAW_ALL) -#define DRAW_EPA (0 || DRAW_ALL) -#define DRAW_CLOSEST (0 || DRAW_ALL) -#define DRAW_CLIP (0 || DRAW_ALL) - -#define PRINT_LOG 0 -#endif - -#define MAX_GJK_ITERATIONS 30 -#define MAX_EPA_ITERATIONS 30 -#define WARN_GJK_ITERATIONS 20 -#define WARN_EPA_ITERATIONS 20 - -static inline void -cpCollisionInfoPushContact(struct cpCollisionInfo *info, cpVect p1, cpVect p2, cpHashValue hash) -{ - cpAssertSoft(info->count <= CP_MAX_CONTACTS_PER_ARBITER, "Internal error: Tried to push too many contacts."); - - struct cpContact *con = &info->arr[info->count]; - con->r1 = p1; - con->r2 = p2; - con->hash = hash; - - info->count++; -} - -//MARK: Support Points and Edges: - -// Support points are the maximal points on a shape's perimeter along a certain axis. -// The GJK and EPA algorithms use support points to iteratively sample the surface of the two shapes' minkowski difference. - -static inline int -PolySupportPointIndex(const int count, const struct cpSplittingPlane *planes, const cpVect n) -{ - cpFloat max = -INFINITY; - int index = 0; - - for(int i=0; i max){ - max = d; - index = i; - } - } - - return index; -} - -struct SupportPoint { - cpVect p; - // Save an index of the point so it can be cheaply looked up as a starting point for the next frame. - cpCollisionID index; -}; - -static inline struct SupportPoint -SupportPointNew(cpVect p, cpCollisionID index) -{ - struct SupportPoint point = {p, index}; - return point; -} - -typedef struct SupportPoint (*SupportPointFunc)(const cpShape *shape, const cpVect n); - -static inline struct SupportPoint -CircleSupportPoint(const cpCircleShape *circle, const cpVect n) -{ - return SupportPointNew(circle->tc, 0); -} - -static inline struct SupportPoint -SegmentSupportPoint(const cpSegmentShape *seg, const cpVect n) -{ - if(cpvdot(seg->ta, n) > cpvdot(seg->tb, n)){ - return SupportPointNew(seg->ta, 0); - } else { - return SupportPointNew(seg->tb, 1); - } -} - -static inline struct SupportPoint -PolySupportPoint(const cpPolyShape *poly, const cpVect n) -{ - const struct cpSplittingPlane *planes = poly->planes; - int i = PolySupportPointIndex(poly->count, planes, n); - return SupportPointNew(planes[i].v0, i); -} - -// A point on the surface of two shape's minkowski difference. -struct MinkowskiPoint { - // Cache the two original support points. - cpVect a, b; - // b - a - cpVect ab; - // Concatenate the two support point indexes. - cpCollisionID id; -}; - -static inline struct MinkowskiPoint -MinkowskiPointNew(const struct SupportPoint a, const struct SupportPoint b) -{ - struct MinkowskiPoint point = {a.p, b.p, cpvsub(b.p, a.p), (a.index & 0xFF)<<8 | (b.index & 0xFF)}; - return point; -} - -struct SupportContext { - const cpShape *shape1, *shape2; - SupportPointFunc func1, func2; -}; - -// Calculate the maximal point on the minkowski difference of two shapes along a particular axis. -static inline struct MinkowskiPoint -Support(const struct SupportContext *ctx, const cpVect n) -{ - struct SupportPoint a = ctx->func1(ctx->shape1, cpvneg(n)); - struct SupportPoint b = ctx->func2(ctx->shape2, n); - return MinkowskiPointNew(a, b); -} - -struct EdgePoint { - cpVect p; - // Keep a hash value for Chipmunk's collision hashing mechanism. - cpHashValue hash; -}; - -// Support edges are the edges of a polygon or segment shape that are in contact. -struct Edge { - struct EdgePoint a, b; - cpFloat r; - cpVect n; -}; - -static struct Edge -SupportEdgeForPoly(const cpPolyShape *poly, const cpVect n) -{ - int count = poly->count; - int i1 = PolySupportPointIndex(poly->count, poly->planes, n); - - // TODO: get rid of mod eventually, very expensive on ARM - int i0 = (i1 - 1 + count)%count; - int i2 = (i1 + 1)%count; - - const struct cpSplittingPlane *planes = poly->planes; - cpHashValue hashid = poly->shape.hashid; - if(cpvdot(n, planes[i1].n) > cpvdot(n, planes[i2].n)){ - struct Edge edge = {{planes[i0].v0, CP_HASH_PAIR(hashid, i0)}, {planes[i1].v0, CP_HASH_PAIR(hashid, i1)}, poly->r, planes[i1].n}; - return edge; - } else { - struct Edge edge = {{planes[i1].v0, CP_HASH_PAIR(hashid, i1)}, {planes[i2].v0, CP_HASH_PAIR(hashid, i2)}, poly->r, planes[i2].n}; - return edge; - } -} - -static struct Edge -SupportEdgeForSegment(const cpSegmentShape *seg, const cpVect n) -{ - cpHashValue hashid = seg->shape.hashid; - if(cpvdot(seg->tn, n) > 0.0){ - struct Edge edge = {{seg->ta, CP_HASH_PAIR(hashid, 0)}, {seg->tb, CP_HASH_PAIR(hashid, 1)}, seg->r, seg->tn}; - return edge; - } else { - struct Edge edge = {{seg->tb, CP_HASH_PAIR(hashid, 1)}, {seg->ta, CP_HASH_PAIR(hashid, 0)}, seg->r, cpvneg(seg->tn)}; - return edge; - } -} - -// Find the closest p(t) to (0, 0) where p(t) = a*(1-t)/2 + b*(1+t)/2 -// The range for t is [-1, 1] to avoid floating point issues if the parameters are swapped. -static inline cpFloat -ClosestT(const cpVect a, const cpVect b) -{ - cpVect delta = cpvsub(b, a); - return -cpfclamp(cpvdot(delta, cpvadd(a, b))/(cpvlengthsq(delta) + CPFLOAT_MIN), -1.0f, 1.0f); -} - -// Basically the same as cpvlerp(), except t = [-1, 1] -static inline cpVect -LerpT(const cpVect a, const cpVect b, const cpFloat t) -{ - cpFloat ht = 0.5f*t; - return cpvadd(cpvmult(a, 0.5f - ht), cpvmult(b, 0.5f + ht)); -} - -// Closest points on the surface of two shapes. -struct ClosestPoints { - // Surface points in absolute coordinates. - cpVect a, b; - // Minimum separating axis of the two shapes. - cpVect n; - // Signed distance between the points. - cpFloat d; - // Concatenation of the id's of the minkoski points. - cpCollisionID id; -}; - -// Calculate the closest points on two shapes given the closest edge on their minkowski difference to (0, 0) -static inline struct ClosestPoints -ClosestPointsNew(const struct MinkowskiPoint v0, const struct MinkowskiPoint v1) -{ - // Find the closest p(t) on the minkowski difference to (0, 0) - cpFloat t = ClosestT(v0.ab, v1.ab); - cpVect p = LerpT(v0.ab, v1.ab, t); - - // Interpolate the original support points using the same 't' value as above. - // This gives you the closest surface points in absolute coordinates. NEAT! - cpVect pa = LerpT(v0.a, v1.a, t); - cpVect pb = LerpT(v0.b, v1.b, t); - cpCollisionID id = (v0.id & 0xFFFF)<<16 | (v1.id & 0xFFFF); - - // First try calculating the MSA from the minkowski difference edge. - // This gives us a nice, accurate MSA when the surfaces are close together. - cpVect delta = cpvsub(v1.ab, v0.ab); - cpVect n = cpvnormalize(cpvrperp(delta)); - cpFloat d = cpvdot(n, p); - - if(d <= 0.0f || (-1.0f < t && t < 1.0f)){ - // If the shapes are overlapping, or we have a regular vertex/edge collision, we are done. - struct ClosestPoints points = {pa, pb, n, d, id}; - return points; - } else { - // Vertex/vertex collisions need special treatment since the MSA won't be shared with an axis of the minkowski difference. - cpFloat d2 = cpvlength(p); - cpVect n2 = cpvmult(p, 1.0f/(d2 + CPFLOAT_MIN)); - - struct ClosestPoints points = {pa, pb, n2, d2, id}; - return points; - } -} - -//MARK: EPA Functions - -static inline cpFloat -ClosestDist(const cpVect v0,const cpVect v1) -{ - return cpvlengthsq(LerpT(v0, v1, ClosestT(v0, v1))); -} - -// Recursive implementation of the EPA loop. -// Each recursion adds a point to the convex hull until it's known that we have the closest point on the surface. -static struct ClosestPoints -EPARecurse(const struct SupportContext *ctx, const int count, const struct MinkowskiPoint *hull, const int iteration) -{ - int mini = 0; - cpFloat minDist = INFINITY; - - // TODO: precalculate this when building the hull and save a step. - // Find the closest segment hull[i] and hull[i + 1] to (0, 0) - for(int j=0, i=count-1; j MAX_GJK_ITERATIONS){ - cpAssertWarn(iteration < WARN_GJK_ITERATIONS, "High GJK iterations: %d", iteration); - return ClosestPointsNew(v0, v1); - } - - if(cpCheckPointGreater(v1.ab, v0.ab, cpvzero)){ - // Origin is behind axis. Flip and try again. - return GJKRecurse(ctx, v1, v0, iteration); - } else { - cpFloat t = ClosestT(v0.ab, v1.ab); - cpVect n = (-1.0f < t && t < 1.0f ? cpvperp(cpvsub(v1.ab, v0.ab)) : cpvneg(LerpT(v0.ab, v1.ab, t))); - struct MinkowskiPoint p = Support(ctx, n); - -#if DRAW_GJK - ChipmunkDebugDrawSegment(v0.ab, v1.ab, RGBAColor(1, 1, 1, 1)); - cpVect c = cpvlerp(v0.ab, v1.ab, 0.5); - ChipmunkDebugDrawSegment(c, cpvadd(c, cpvmult(cpvnormalize(n), 5.0)), RGBAColor(1, 0, 0, 1)); - - ChipmunkDebugDrawDot(5.0, p.ab, LAColor(1, 1)); -#endif - - if(cpCheckPointGreater(p.ab, v0.ab, cpvzero) && cpCheckPointGreater(v1.ab, p.ab, cpvzero)){ - // The triangle v0, p, v1 contains the origin. Use EPA to find the MSA. - cpAssertWarn(iteration < WARN_GJK_ITERATIONS, "High GJK->EPA iterations: %d", iteration); - return EPA(ctx, v0, p, v1); - } else { - if(cpCheckAxis(v0.ab, v1.ab, p.ab, n)){ - // The edge v0, v1 that we already have is the closest to (0, 0) since p was not closer. - cpAssertWarn(iteration < WARN_GJK_ITERATIONS, "High GJK iterations: %d", iteration); - return ClosestPointsNew(v0, v1); - } else { - // p was closer to the origin than our existing edge. - // Need to figure out which existing point to drop. - if(ClosestDist(v0.ab, p.ab) < ClosestDist(p.ab, v1.ab)){ - return GJKRecurse(ctx, v0, p, iteration + 1); - } else { - return GJKRecurse(ctx, p, v1, iteration + 1); - } - } - } - } -} - -// Get a SupportPoint from a cached shape and index. -static struct SupportPoint -ShapePoint(const cpShape *shape, const int i) -{ - switch(shape->klass->type){ - case CP_CIRCLE_SHAPE: { - return SupportPointNew(((cpCircleShape *)shape)->tc, 0); - } case CP_SEGMENT_SHAPE: { - cpSegmentShape *seg = (cpSegmentShape *)shape; - return SupportPointNew(i == 0 ? seg->ta : seg->tb, i); - } case CP_POLY_SHAPE: { - cpPolyShape *poly = (cpPolyShape *)shape; - // Poly shapes may change vertex count. - int index = (i < poly->count ? i : 0); - return SupportPointNew(poly->planes[index].v0, index); - } default: { - return SupportPointNew(cpvzero, 0); - } - } -} - -// Find the closest points between two shapes using the GJK algorithm. -static struct ClosestPoints -GJK(const struct SupportContext *ctx, cpCollisionID *id) -{ -#if DRAW_GJK || DRAW_EPA - int count1 = 1; - int count2 = 1; - - switch(ctx->shape1->klass->type){ - case CP_SEGMENT_SHAPE: count1 = 2; break; - case CP_POLY_SHAPE: count1 = ((cpPolyShape *)ctx->shape1)->count; break; - default: break; - } - - switch(ctx->shape2->klass->type){ - case CP_SEGMENT_SHAPE: count1 = 2; break; - case CP_POLY_SHAPE: count2 = ((cpPolyShape *)ctx->shape2)->count; break; - default: break; - } - - - // draw the minkowski difference origin - cpVect origin = cpvzero; - ChipmunkDebugDrawDot(5.0, origin, RGBAColor(1,0,0,1)); - - int mdiffCount = count1*count2; - cpVect *mdiffVerts = alloca(mdiffCount*sizeof(cpVect)); - - for(int i=0; ishape2, j).p, ShapePoint(ctx->shape1, i).p); - mdiffVerts[i*count2 + j] = v; - ChipmunkDebugDrawDot(2.0, v, RGBAColor(1, 0, 0, 1)); - } - } - - cpVect *hullVerts = alloca(mdiffCount*sizeof(cpVect)); - int hullCount = cpConvexHull(mdiffCount, mdiffVerts, hullVerts, NULL, 0.0); - - ChipmunkDebugDrawPolygon(hullCount, hullVerts, 0.0, RGBAColor(1, 0, 0, 1), RGBAColor(1, 0, 0, 0.25)); -#endif - - struct MinkowskiPoint v0, v1; - if(*id){ - // Use the minkowski points from the last frame as a starting point using the cached indexes. - v0 = MinkowskiPointNew(ShapePoint(ctx->shape1, (*id>>24)&0xFF), ShapePoint(ctx->shape2, (*id>>16)&0xFF)); - v1 = MinkowskiPointNew(ShapePoint(ctx->shape1, (*id>> 8)&0xFF), ShapePoint(ctx->shape2, (*id )&0xFF)); - } else { - // No cached indexes, use the shapes' bounding box centers as a guess for a starting axis. - cpVect axis = cpvperp(cpvsub(cpBBCenter(ctx->shape1->bb), cpBBCenter(ctx->shape2->bb))); - v0 = Support(ctx, axis); - v1 = Support(ctx, cpvneg(axis)); - } - - struct ClosestPoints points = GJKRecurse(ctx, v0, v1, 1); - *id = points.id; - return points; -} - -//MARK: Contact Clipping - -// Given two support edges, find contact point pairs on their surfaces. -static inline void -ContactPoints(const struct Edge e1, const struct Edge e2, const struct ClosestPoints points, struct cpCollisionInfo *info) -{ - cpFloat mindist = e1.r + e2.r; - if(points.d <= mindist){ -#ifdef DRAW_CLIP - ChipmunkDebugDrawFatSegment(e1.a.p, e1.b.p, e1.r, RGBAColor(0, 1, 0, 1), LAColor(0, 0)); - ChipmunkDebugDrawFatSegment(e2.a.p, e2.b.p, e2.r, RGBAColor(1, 0, 0, 1), LAColor(0, 0)); -#endif - cpVect n = info->n = points.n; - - // Distances along the axis parallel to n - cpFloat d_e1_a = cpvcross(e1.a.p, n); - cpFloat d_e1_b = cpvcross(e1.b.p, n); - cpFloat d_e2_a = cpvcross(e2.a.p, n); - cpFloat d_e2_b = cpvcross(e2.b.p, n); - - // TODO + min isn't a complete fix. - cpFloat e1_denom = 1.0f/(d_e1_b - d_e1_a + CPFLOAT_MIN); - cpFloat e2_denom = 1.0f/(d_e2_b - d_e2_a + CPFLOAT_MIN); - - // Project the endpoints of the two edges onto the opposing edge, clamping them as necessary. - // Compare the projected points to the collision normal to see if the shapes overlap there. - { - cpVect p1 = cpvadd(cpvmult(n, e1.r), cpvlerp(e1.a.p, e1.b.p, cpfclamp01((d_e2_b - d_e1_a)*e1_denom))); - cpVect p2 = cpvadd(cpvmult(n, -e2.r), cpvlerp(e2.a.p, e2.b.p, cpfclamp01((d_e1_a - d_e2_a)*e2_denom))); - cpFloat dist = cpvdot(cpvsub(p2, p1), n); - if(dist <= 0.0f){ - cpHashValue hash_1a2b = CP_HASH_PAIR(e1.a.hash, e2.b.hash); - cpCollisionInfoPushContact(info, p1, p2, hash_1a2b); - } - }{ - cpVect p1 = cpvadd(cpvmult(n, e1.r), cpvlerp(e1.a.p, e1.b.p, cpfclamp01((d_e2_a - d_e1_a)*e1_denom))); - cpVect p2 = cpvadd(cpvmult(n, -e2.r), cpvlerp(e2.a.p, e2.b.p, cpfclamp01((d_e1_b - d_e2_a)*e2_denom))); - cpFloat dist = cpvdot(cpvsub(p2, p1), n); - if(dist <= 0.0f){ - cpHashValue hash_1b2a = CP_HASH_PAIR(e1.b.hash, e2.a.hash); - cpCollisionInfoPushContact(info, p1, p2, hash_1b2a); - } - } - } -} - -//MARK: Collision Functions - -typedef void (*CollisionFunc)(const cpShape *a, const cpShape *b, struct cpCollisionInfo *info); - -// Collide circle shapes. -static void -CircleToCircle(const cpCircleShape *c1, const cpCircleShape *c2, struct cpCollisionInfo *info) -{ - cpFloat mindist = c1->r + c2->r; - cpVect delta = cpvsub(c2->tc, c1->tc); - cpFloat distsq = cpvlengthsq(delta); - - if(distsq < mindist*mindist){ - cpFloat dist = cpfsqrt(distsq); - cpVect n = info->n = (dist ? cpvmult(delta, 1.0f/dist) : cpv(1.0f, 0.0f)); - cpCollisionInfoPushContact(info, cpvadd(c1->tc, cpvmult(n, c1->r)), cpvadd(c2->tc, cpvmult(n, -c2->r)), 0); - } -} - -static void -CircleToSegment(const cpCircleShape *circle, const cpSegmentShape *segment, struct cpCollisionInfo *info) -{ - cpVect seg_a = segment->ta; - cpVect seg_b = segment->tb; - cpVect center = circle->tc; - - // Find the closest point on the segment to the circle. - cpVect seg_delta = cpvsub(seg_b, seg_a); - cpFloat closest_t = cpfclamp01(cpvdot(seg_delta, cpvsub(center, seg_a))/cpvlengthsq(seg_delta)); - cpVect closest = cpvadd(seg_a, cpvmult(seg_delta, closest_t)); - - // Compare the radii of the two shapes to see if they are colliding. - cpFloat mindist = circle->r + segment->r; - cpVect delta = cpvsub(closest, center); - cpFloat distsq = cpvlengthsq(delta); - if(distsq < mindist*mindist){ - cpFloat dist = cpfsqrt(distsq); - // Handle coincident shapes as gracefully as possible. - cpVect n = info->n = (dist ? cpvmult(delta, 1.0f/dist) : segment->tn); - - // Reject endcap collisions if tangents are provided. - cpVect rot = cpBodyGetRotation(segment->shape.body); - if( - (closest_t != 0.0f || cpvdot(n, cpvrotate(segment->a_tangent, rot)) >= 0.0) && - (closest_t != 1.0f || cpvdot(n, cpvrotate(segment->b_tangent, rot)) >= 0.0) - ){ - cpCollisionInfoPushContact(info, cpvadd(center, cpvmult(n, circle->r)), cpvadd(closest, cpvmult(n, -segment->r)), 0); - } - } -} - -static void -SegmentToSegment(const cpSegmentShape *seg1, const cpSegmentShape *seg2, struct cpCollisionInfo *info) -{ - struct SupportContext context = {(cpShape *)seg1, (cpShape *)seg2, (SupportPointFunc)SegmentSupportPoint, (SupportPointFunc)SegmentSupportPoint}; - struct ClosestPoints points = GJK(&context, &info->id); - -#if DRAW_CLOSEST -#if PRINT_LOG -// ChipmunkDemoPrintString("Distance: %.2f\n", points.d); -#endif - - ChipmunkDebugDrawDot(6.0, points.a, RGBAColor(1, 1, 1, 1)); - ChipmunkDebugDrawDot(6.0, points.b, RGBAColor(1, 1, 1, 1)); - ChipmunkDebugDrawSegment(points.a, points.b, RGBAColor(1, 1, 1, 1)); - ChipmunkDebugDrawSegment(points.a, cpvadd(points.a, cpvmult(points.n, 10.0)), RGBAColor(1, 0, 0, 1)); -#endif - - cpVect n = points.n; - cpVect rot1 = cpBodyGetRotation(seg1->shape.body); - cpVect rot2 = cpBodyGetRotation(seg2->shape.body); - - // If the closest points are nearer than the sum of the radii... - if( - points.d <= (seg1->r + seg2->r) && ( - // Reject endcap collisions if tangents are provided. - (!cpveql(points.a, seg1->ta) || cpvdot(n, cpvrotate(seg1->a_tangent, rot1)) <= 0.0) && - (!cpveql(points.a, seg1->tb) || cpvdot(n, cpvrotate(seg1->b_tangent, rot1)) <= 0.0) && - (!cpveql(points.b, seg2->ta) || cpvdot(n, cpvrotate(seg2->a_tangent, rot2)) >= 0.0) && - (!cpveql(points.b, seg2->tb) || cpvdot(n, cpvrotate(seg2->b_tangent, rot2)) >= 0.0) - ) - ){ - ContactPoints(SupportEdgeForSegment(seg1, n), SupportEdgeForSegment(seg2, cpvneg(n)), points, info); - } -} - -static void -PolyToPoly(const cpPolyShape *poly1, const cpPolyShape *poly2, struct cpCollisionInfo *info) -{ - struct SupportContext context = {(cpShape *)poly1, (cpShape *)poly2, (SupportPointFunc)PolySupportPoint, (SupportPointFunc)PolySupportPoint}; - struct ClosestPoints points = GJK(&context, &info->id); - -#if DRAW_CLOSEST -#if PRINT_LOG -// ChipmunkDemoPrintString("Distance: %.2f\n", points.d); -#endif - - ChipmunkDebugDrawDot(3.0, points.a, RGBAColor(1, 1, 1, 1)); - ChipmunkDebugDrawDot(3.0, points.b, RGBAColor(1, 1, 1, 1)); - ChipmunkDebugDrawSegment(points.a, points.b, RGBAColor(1, 1, 1, 1)); - ChipmunkDebugDrawSegment(points.a, cpvadd(points.a, cpvmult(points.n, 10.0)), RGBAColor(1, 0, 0, 1)); -#endif - - // If the closest points are nearer than the sum of the radii... - if(points.d - poly1->r - poly2->r <= 0.0){ - ContactPoints(SupportEdgeForPoly(poly1, points.n), SupportEdgeForPoly(poly2, cpvneg(points.n)), points, info); - } -} - -static void -SegmentToPoly(const cpSegmentShape *seg, const cpPolyShape *poly, struct cpCollisionInfo *info) -{ - struct SupportContext context = {(cpShape *)seg, (cpShape *)poly, (SupportPointFunc)SegmentSupportPoint, (SupportPointFunc)PolySupportPoint}; - struct ClosestPoints points = GJK(&context, &info->id); - -#if DRAW_CLOSEST -#if PRINT_LOG -// ChipmunkDemoPrintString("Distance: %.2f\n", points.d); -#endif - - ChipmunkDebugDrawDot(3.0, points.a, RGBAColor(1, 1, 1, 1)); - ChipmunkDebugDrawDot(3.0, points.b, RGBAColor(1, 1, 1, 1)); - ChipmunkDebugDrawSegment(points.a, points.b, RGBAColor(1, 1, 1, 1)); - ChipmunkDebugDrawSegment(points.a, cpvadd(points.a, cpvmult(points.n, 10.0)), RGBAColor(1, 0, 0, 1)); -#endif - - cpVect n = points.n; - cpVect rot = cpBodyGetRotation(seg->shape.body); - - if( - // If the closest points are nearer than the sum of the radii... - points.d - seg->r - poly->r <= 0.0 && ( - // Reject endcap collisions if tangents are provided. - (!cpveql(points.a, seg->ta) || cpvdot(n, cpvrotate(seg->a_tangent, rot)) <= 0.0) && - (!cpveql(points.a, seg->tb) || cpvdot(n, cpvrotate(seg->b_tangent, rot)) <= 0.0) - ) - ){ - ContactPoints(SupportEdgeForSegment(seg, n), SupportEdgeForPoly(poly, cpvneg(n)), points, info); - } -} - -static void -CircleToPoly(const cpCircleShape *circle, const cpPolyShape *poly, struct cpCollisionInfo *info) -{ - struct SupportContext context = {(cpShape *)circle, (cpShape *)poly, (SupportPointFunc)CircleSupportPoint, (SupportPointFunc)PolySupportPoint}; - struct ClosestPoints points = GJK(&context, &info->id); - -#if DRAW_CLOSEST - ChipmunkDebugDrawDot(3.0, points.a, RGBAColor(1, 1, 1, 1)); - ChipmunkDebugDrawDot(3.0, points.b, RGBAColor(1, 1, 1, 1)); - ChipmunkDebugDrawSegment(points.a, points.b, RGBAColor(1, 1, 1, 1)); - ChipmunkDebugDrawSegment(points.a, cpvadd(points.a, cpvmult(points.n, 10.0)), RGBAColor(1, 0, 0, 1)); -#endif - - // If the closest points are nearer than the sum of the radii... - if(points.d <= circle->r + poly->r){ - cpVect n = info->n = points.n; - cpCollisionInfoPushContact(info, cpvadd(points.a, cpvmult(n, circle->r)), cpvadd(points.b, cpvmult(n, -poly->r)), 0); - } -} - -static void -CollisionError(const cpShape *circle, const cpShape *poly, struct cpCollisionInfo *info) -{ - cpAssertHard(cpFalse, "Internal Error: Shape types are not sorted."); -} - - -static const CollisionFunc BuiltinCollisionFuncs[9] = { - (CollisionFunc)CircleToCircle, - CollisionError, - CollisionError, - (CollisionFunc)CircleToSegment, - (CollisionFunc)SegmentToSegment, - CollisionError, - (CollisionFunc)CircleToPoly, - (CollisionFunc)SegmentToPoly, - (CollisionFunc)PolyToPoly, -}; -static const CollisionFunc *CollisionFuncs = BuiltinCollisionFuncs; - -struct cpCollisionInfo -cpCollide(const cpShape *a, const cpShape *b, cpCollisionID id, struct cpContact *contacts) -{ - struct cpCollisionInfo info = {a, b, id, cpvzero, 0, contacts}; - - // Make sure the shape types are in order. - if(a->klass->type > b->klass->type){ - info.a = b; - info.b = a; - } - - CollisionFuncs[info.a->klass->type + info.b->klass->type*CP_NUM_SHAPES](info.a, info.b, &info); - -// if(0){ -// for(int i=0; iklass = klass; - - constraint->a = a; - constraint->b = b; - constraint->space = NULL; - - constraint->next_a = NULL; - constraint->next_b = NULL; - - constraint->maxForce = (cpFloat)INFINITY; - constraint->errorBias = cpfpow(1.0f - 0.1f, 60.0f); - constraint->maxBias = (cpFloat)INFINITY; - - constraint->collideBodies = cpTrue; - - constraint->preSolve = NULL; - constraint->postSolve = NULL; -} - -cpSpace * -cpConstraintGetSpace(const cpConstraint *constraint) -{ - return constraint->space; -} - -cpBody * -cpConstraintGetBodyA(const cpConstraint *constraint) -{ - return constraint->a; -} - -cpBody * -cpConstraintGetBodyB(const cpConstraint *constraint) -{ - return constraint->b; -} - -cpFloat -cpConstraintGetMaxForce(const cpConstraint *constraint) -{ - return constraint->maxForce; -} - -void -cpConstraintSetMaxForce(cpConstraint *constraint, cpFloat maxForce) -{ - cpAssertHard(maxForce >= 0.0f, "maxForce must be positive."); - cpConstraintActivateBodies(constraint); - constraint->maxForce = maxForce; -} - -cpFloat -cpConstraintGetErrorBias(const cpConstraint *constraint) -{ - return constraint->errorBias; -} - -void -cpConstraintSetErrorBias(cpConstraint *constraint, cpFloat errorBias) -{ - cpAssertHard(errorBias >= 0.0f, "errorBias must be positive."); - cpConstraintActivateBodies(constraint); - constraint->errorBias = errorBias; -} - -cpFloat -cpConstraintGetMaxBias(const cpConstraint *constraint) -{ - return constraint->maxBias; -} - -void -cpConstraintSetMaxBias(cpConstraint *constraint, cpFloat maxBias) -{ - cpAssertHard(maxBias >= 0.0f, "maxBias must be positive."); - cpConstraintActivateBodies(constraint); - constraint->maxBias = maxBias; -} - -cpBool -cpConstraintGetCollideBodies(const cpConstraint *constraint) -{ - return constraint->collideBodies; -} - -void -cpConstraintSetCollideBodies(cpConstraint *constraint, cpBool collideBodies) -{ - cpConstraintActivateBodies(constraint); - constraint->collideBodies = collideBodies; -} - -cpConstraintPreSolveFunc -cpConstraintGetPreSolveFunc(const cpConstraint *constraint) -{ - return constraint->preSolve; -} - -void -cpConstraintSetPreSolveFunc(cpConstraint *constraint, cpConstraintPreSolveFunc preSolveFunc) -{ - constraint->preSolve = preSolveFunc; -} - -cpConstraintPostSolveFunc -cpConstraintGetPostSolveFunc(const cpConstraint *constraint) -{ - return constraint->postSolve; -} - -void -cpConstraintSetPostSolveFunc(cpConstraint *constraint, cpConstraintPostSolveFunc postSolveFunc) -{ - constraint->postSolve = postSolveFunc; -} - -cpDataPointer -cpConstraintGetUserData(const cpConstraint *constraint) -{ - return constraint->userData; -} - -void -cpConstraintSetUserData(cpConstraint *constraint, cpDataPointer userData) -{ - constraint->userData = userData; -} - - -cpFloat -cpConstraintGetImpulse(cpConstraint *constraint) -{ - return constraint->klass->getImpulse(constraint); -} diff --git a/3rdparty/chipmunk/src/cpDampedRotarySpring.c b/3rdparty/chipmunk/src/cpDampedRotarySpring.c deleted file mode 100644 index 8d38a545e0f9..000000000000 --- a/3rdparty/chipmunk/src/cpDampedRotarySpring.c +++ /dev/null @@ -1,178 +0,0 @@ -/* Copyright (c) 2013 Scott Lembcke and Howling Moon Software - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -#include "chipmunk/chipmunk_private.h" - -static cpFloat -defaultSpringTorque(cpDampedRotarySpring *spring, cpFloat relativeAngle){ - return (relativeAngle - spring->restAngle)*spring->stiffness; -} - -static void -preStep(cpDampedRotarySpring *spring, cpFloat dt) -{ - cpBody *a = spring->constraint.a; - cpBody *b = spring->constraint.b; - - cpFloat moment = a->i_inv + b->i_inv; - cpAssertSoft(moment != 0.0, "Unsolvable spring."); - spring->iSum = 1.0f/moment; - - spring->w_coef = 1.0f - cpfexp(-spring->damping*dt*moment); - spring->target_wrn = 0.0f; - - // apply spring torque - cpFloat j_spring = spring->springTorqueFunc((cpConstraint *)spring, a->a - b->a)*dt; - spring->jAcc = j_spring; - - a->w -= j_spring*a->i_inv; - b->w += j_spring*b->i_inv; -} - -static void applyCachedImpulse(cpDampedRotarySpring *spring, cpFloat dt_coef){} - -static void -applyImpulse(cpDampedRotarySpring *spring, cpFloat dt) -{ - cpBody *a = spring->constraint.a; - cpBody *b = spring->constraint.b; - - // compute relative velocity - cpFloat wrn = a->w - b->w;//normal_relative_velocity(a, b, r1, r2, n) - spring->target_vrn; - - // compute velocity loss from drag - // not 100% certain this is derived correctly, though it makes sense - cpFloat w_damp = (spring->target_wrn - wrn)*spring->w_coef; - spring->target_wrn = wrn + w_damp; - - //apply_impulses(a, b, spring->r1, spring->r2, cpvmult(spring->n, v_damp*spring->nMass)); - cpFloat j_damp = w_damp*spring->iSum; - spring->jAcc += j_damp; - - a->w += j_damp*a->i_inv; - b->w -= j_damp*b->i_inv; -} - -static cpFloat -getImpulse(cpDampedRotarySpring *spring) -{ - return spring->jAcc; -} - -static const cpConstraintClass klass = { - (cpConstraintPreStepImpl)preStep, - (cpConstraintApplyCachedImpulseImpl)applyCachedImpulse, - (cpConstraintApplyImpulseImpl)applyImpulse, - (cpConstraintGetImpulseImpl)getImpulse, -}; - -cpDampedRotarySpring * -cpDampedRotarySpringAlloc(void) -{ - return (cpDampedRotarySpring *)cpcalloc(1, sizeof(cpDampedRotarySpring)); -} - -cpDampedRotarySpring * -cpDampedRotarySpringInit(cpDampedRotarySpring *spring, cpBody *a, cpBody *b, cpFloat restAngle, cpFloat stiffness, cpFloat damping) -{ - cpConstraintInit((cpConstraint *)spring, &klass, a, b); - - spring->restAngle = restAngle; - spring->stiffness = stiffness; - spring->damping = damping; - spring->springTorqueFunc = (cpDampedRotarySpringTorqueFunc)defaultSpringTorque; - - spring->jAcc = 0.0f; - - return spring; -} - -cpConstraint * -cpDampedRotarySpringNew(cpBody *a, cpBody *b, cpFloat restAngle, cpFloat stiffness, cpFloat damping) -{ - return (cpConstraint *)cpDampedRotarySpringInit(cpDampedRotarySpringAlloc(), a, b, restAngle, stiffness, damping); -} - -cpBool -cpConstraintIsDampedRotarySpring(const cpConstraint *constraint) -{ - return (constraint->klass == &klass); -} - -cpFloat -cpDampedRotarySpringGetRestAngle(const cpConstraint *constraint) -{ - cpAssertHard(cpConstraintIsDampedRotarySpring(constraint), "Constraint is not a damped rotary spring."); - return ((cpDampedRotarySpring *)constraint)->restAngle; -} - -void -cpDampedRotarySpringSetRestAngle(cpConstraint *constraint, cpFloat restAngle) -{ - cpAssertHard(cpConstraintIsDampedRotarySpring(constraint), "Constraint is not a damped rotary spring."); - cpConstraintActivateBodies(constraint); - ((cpDampedRotarySpring *)constraint)->restAngle = restAngle; -} - -cpFloat -cpDampedRotarySpringGetStiffness(const cpConstraint *constraint) -{ - cpAssertHard(cpConstraintIsDampedRotarySpring(constraint), "Constraint is not a damped rotary spring."); - return ((cpDampedRotarySpring *)constraint)->stiffness; -} - -void -cpDampedRotarySpringSetStiffness(cpConstraint *constraint, cpFloat stiffness) -{ - cpAssertHard(cpConstraintIsDampedRotarySpring(constraint), "Constraint is not a damped rotary spring."); - cpConstraintActivateBodies(constraint); - ((cpDampedRotarySpring *)constraint)->stiffness = stiffness; -} - -cpFloat -cpDampedRotarySpringGetDamping(const cpConstraint *constraint) -{ - cpAssertHard(cpConstraintIsDampedRotarySpring(constraint), "Constraint is not a damped rotary spring."); - return ((cpDampedRotarySpring *)constraint)->damping; -} - -void -cpDampedRotarySpringSetDamping(cpConstraint *constraint, cpFloat damping) -{ - cpAssertHard(cpConstraintIsDampedRotarySpring(constraint), "Constraint is not a damped rotary spring."); - cpConstraintActivateBodies(constraint); - ((cpDampedRotarySpring *)constraint)->damping = damping; -} - -cpDampedRotarySpringTorqueFunc -cpDampedRotarySpringGetSpringTorqueFunc(const cpConstraint *constraint) -{ - cpAssertHard(cpConstraintIsDampedRotarySpring(constraint), "Constraint is not a damped rotary spring."); - return ((cpDampedRotarySpring *)constraint)->springTorqueFunc; -} - -void -cpDampedRotarySpringSetSpringTorqueFunc(cpConstraint *constraint, cpDampedRotarySpringTorqueFunc springTorqueFunc) -{ - cpAssertHard(cpConstraintIsDampedRotarySpring(constraint), "Constraint is not a damped rotary spring."); - cpConstraintActivateBodies(constraint); - ((cpDampedRotarySpring *)constraint)->springTorqueFunc = springTorqueFunc; -} diff --git a/3rdparty/chipmunk/src/cpDampedSpring.c b/3rdparty/chipmunk/src/cpDampedSpring.c deleted file mode 100644 index e4d019e9a02f..000000000000 --- a/3rdparty/chipmunk/src/cpDampedSpring.c +++ /dev/null @@ -1,216 +0,0 @@ -/* Copyright (c) 2013 Scott Lembcke and Howling Moon Software - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -#include "chipmunk/chipmunk_private.h" - -static cpFloat -defaultSpringForce(cpDampedSpring *spring, cpFloat dist){ - return (spring->restLength - dist)*spring->stiffness; -} - -static void -preStep(cpDampedSpring *spring, cpFloat dt) -{ - cpBody *a = spring->constraint.a; - cpBody *b = spring->constraint.b; - - spring->r1 = cpTransformVect(a->transform, cpvsub(spring->anchorA, a->cog)); - spring->r2 = cpTransformVect(b->transform, cpvsub(spring->anchorB, b->cog)); - - cpVect delta = cpvsub(cpvadd(b->p, spring->r2), cpvadd(a->p, spring->r1)); - cpFloat dist = cpvlength(delta); - spring->n = cpvmult(delta, 1.0f/(dist ? dist : INFINITY)); - - cpFloat k = k_scalar(a, b, spring->r1, spring->r2, spring->n); - cpAssertSoft(k != 0.0, "Unsolvable spring."); - spring->nMass = 1.0f/k; - - spring->target_vrn = 0.0f; - spring->v_coef = 1.0f - cpfexp(-spring->damping*dt*k); - - // apply spring force - cpFloat f_spring = spring->springForceFunc((cpConstraint *)spring, dist); - cpFloat j_spring = spring->jAcc = f_spring*dt; - apply_impulses(a, b, spring->r1, spring->r2, cpvmult(spring->n, j_spring)); -} - -static void applyCachedImpulse(cpDampedSpring *spring, cpFloat dt_coef){} - -static void -applyImpulse(cpDampedSpring *spring, cpFloat dt) -{ - cpBody *a = spring->constraint.a; - cpBody *b = spring->constraint.b; - - cpVect n = spring->n; - cpVect r1 = spring->r1; - cpVect r2 = spring->r2; - - // compute relative velocity - cpFloat vrn = normal_relative_velocity(a, b, r1, r2, n); - - // compute velocity loss from drag - cpFloat v_damp = (spring->target_vrn - vrn)*spring->v_coef; - spring->target_vrn = vrn + v_damp; - - cpFloat j_damp = v_damp*spring->nMass; - spring->jAcc += j_damp; - apply_impulses(a, b, spring->r1, spring->r2, cpvmult(spring->n, j_damp)); -} - -static cpFloat -getImpulse(cpDampedSpring *spring) -{ - return spring->jAcc; -} - -static const cpConstraintClass klass = { - (cpConstraintPreStepImpl)preStep, - (cpConstraintApplyCachedImpulseImpl)applyCachedImpulse, - (cpConstraintApplyImpulseImpl)applyImpulse, - (cpConstraintGetImpulseImpl)getImpulse, -}; - -cpDampedSpring * -cpDampedSpringAlloc(void) -{ - return (cpDampedSpring *)cpcalloc(1, sizeof(cpDampedSpring)); -} - -cpDampedSpring * -cpDampedSpringInit(cpDampedSpring *spring, cpBody *a, cpBody *b, cpVect anchorA, cpVect anchorB, cpFloat restLength, cpFloat stiffness, cpFloat damping) -{ - cpConstraintInit((cpConstraint *)spring, &klass, a, b); - - spring->anchorA = anchorA; - spring->anchorB = anchorB; - - spring->restLength = restLength; - spring->stiffness = stiffness; - spring->damping = damping; - spring->springForceFunc = (cpDampedSpringForceFunc)defaultSpringForce; - - spring->jAcc = 0.0f; - - return spring; -} - -cpConstraint * -cpDampedSpringNew(cpBody *a, cpBody *b, cpVect anchorA, cpVect anchorB, cpFloat restLength, cpFloat stiffness, cpFloat damping) -{ - return (cpConstraint *)cpDampedSpringInit(cpDampedSpringAlloc(), a, b, anchorA, anchorB, restLength, stiffness, damping); -} - -cpBool -cpConstraintIsDampedSpring(const cpConstraint *constraint) -{ - return (constraint->klass == &klass); -} - -cpVect -cpDampedSpringGetAnchorA(const cpConstraint *constraint) -{ - cpAssertHard(cpConstraintIsDampedSpring(constraint), "Constraint is not a damped spring."); - return ((cpDampedSpring *)constraint)->anchorA; -} - -void -cpDampedSpringSetAnchorA(cpConstraint *constraint, cpVect anchorA) -{ - cpAssertHard(cpConstraintIsDampedSpring(constraint), "Constraint is not a damped spring."); - cpConstraintActivateBodies(constraint); - ((cpDampedSpring *)constraint)->anchorA = anchorA; -} - -cpVect -cpDampedSpringGetAnchorB(const cpConstraint *constraint) -{ - cpAssertHard(cpConstraintIsDampedSpring(constraint), "Constraint is not a damped spring."); - return ((cpDampedSpring *)constraint)->anchorB; -} - -void -cpDampedSpringSetAnchorB(cpConstraint *constraint, cpVect anchorB) -{ - cpAssertHard(cpConstraintIsDampedSpring(constraint), "Constraint is not a damped spring."); - cpConstraintActivateBodies(constraint); - ((cpDampedSpring *)constraint)->anchorB = anchorB; -} - -cpFloat -cpDampedSpringGetRestLength(const cpConstraint *constraint) -{ - cpAssertHard(cpConstraintIsDampedSpring(constraint), "Constraint is not a damped spring."); - return ((cpDampedSpring *)constraint)->restLength; -} - -void -cpDampedSpringSetRestLength(cpConstraint *constraint, cpFloat restLength) -{ - cpAssertHard(cpConstraintIsDampedSpring(constraint), "Constraint is not a damped spring."); - cpConstraintActivateBodies(constraint); - ((cpDampedSpring *)constraint)->restLength = restLength; -} - -cpFloat -cpDampedSpringGetStiffness(const cpConstraint *constraint) -{ - cpAssertHard(cpConstraintIsDampedSpring(constraint), "Constraint is not a damped spring."); - return ((cpDampedSpring *)constraint)->stiffness; -} - -void -cpDampedSpringSetStiffness(cpConstraint *constraint, cpFloat stiffness) -{ - cpAssertHard(cpConstraintIsDampedSpring(constraint), "Constraint is not a damped spring."); - cpConstraintActivateBodies(constraint); - ((cpDampedSpring *)constraint)->stiffness = stiffness; -} - -cpFloat -cpDampedSpringGetDamping(const cpConstraint *constraint) -{ - cpAssertHard(cpConstraintIsDampedSpring(constraint), "Constraint is not a damped spring."); - return ((cpDampedSpring *)constraint)->damping; -} - -void -cpDampedSpringSetDamping(cpConstraint *constraint, cpFloat damping) -{ - cpAssertHard(cpConstraintIsDampedSpring(constraint), "Constraint is not a damped spring."); - cpConstraintActivateBodies(constraint); - ((cpDampedSpring *)constraint)->damping = damping; -} - -cpDampedSpringForceFunc -cpDampedSpringGetSpringForceFunc(const cpConstraint *constraint) -{ - cpAssertHard(cpConstraintIsDampedSpring(constraint), "Constraint is not a damped spring."); - return ((cpDampedSpring *)constraint)->springForceFunc; -} - -void -cpDampedSpringSetSpringForceFunc(cpConstraint *constraint, cpDampedSpringForceFunc springForceFunc) -{ - cpAssertHard(cpConstraintIsDampedSpring(constraint), "Constraint is not a damped spring."); - cpConstraintActivateBodies(constraint); - ((cpDampedSpring *)constraint)->springForceFunc = springForceFunc; -} diff --git a/3rdparty/chipmunk/src/cpGearJoint.c b/3rdparty/chipmunk/src/cpGearJoint.c deleted file mode 100644 index 3670173b3c59..000000000000 --- a/3rdparty/chipmunk/src/cpGearJoint.c +++ /dev/null @@ -1,145 +0,0 @@ -/* Copyright (c) 2013 Scott Lembcke and Howling Moon Software - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -#include "chipmunk/chipmunk_private.h" - -static void -preStep(cpGearJoint *joint, cpFloat dt) -{ - cpBody *a = joint->constraint.a; - cpBody *b = joint->constraint.b; - - // calculate moment of inertia coefficient. - joint->iSum = 1.0f/(a->i_inv*joint->ratio_inv + joint->ratio*b->i_inv); - - // calculate bias velocity - cpFloat maxBias = joint->constraint.maxBias; - joint->bias = cpfclamp(-bias_coef(joint->constraint.errorBias, dt)*(b->a*joint->ratio - a->a - joint->phase)/dt, -maxBias, maxBias); -} - -static void -applyCachedImpulse(cpGearJoint *joint, cpFloat dt_coef) -{ - cpBody *a = joint->constraint.a; - cpBody *b = joint->constraint.b; - - cpFloat j = joint->jAcc*dt_coef; - a->w -= j*a->i_inv*joint->ratio_inv; - b->w += j*b->i_inv; -} - -static void -applyImpulse(cpGearJoint *joint, cpFloat dt) -{ - cpBody *a = joint->constraint.a; - cpBody *b = joint->constraint.b; - - // compute relative rotational velocity - cpFloat wr = b->w*joint->ratio - a->w; - - cpFloat jMax = joint->constraint.maxForce*dt; - - // compute normal impulse - cpFloat j = (joint->bias - wr)*joint->iSum; - cpFloat jOld = joint->jAcc; - joint->jAcc = cpfclamp(jOld + j, -jMax, jMax); - j = joint->jAcc - jOld; - - // apply impulse - a->w -= j*a->i_inv*joint->ratio_inv; - b->w += j*b->i_inv; -} - -static cpFloat -getImpulse(cpGearJoint *joint) -{ - return cpfabs(joint->jAcc); -} - -static const cpConstraintClass klass = { - (cpConstraintPreStepImpl)preStep, - (cpConstraintApplyCachedImpulseImpl)applyCachedImpulse, - (cpConstraintApplyImpulseImpl)applyImpulse, - (cpConstraintGetImpulseImpl)getImpulse, -}; - -cpGearJoint * -cpGearJointAlloc(void) -{ - return (cpGearJoint *)cpcalloc(1, sizeof(cpGearJoint)); -} - -cpGearJoint * -cpGearJointInit(cpGearJoint *joint, cpBody *a, cpBody *b, cpFloat phase, cpFloat ratio) -{ - cpConstraintInit((cpConstraint *)joint, &klass, a, b); - - joint->phase = phase; - joint->ratio = ratio; - joint->ratio_inv = 1.0f/ratio; - - joint->jAcc = 0.0f; - - return joint; -} - -cpConstraint * -cpGearJointNew(cpBody *a, cpBody *b, cpFloat phase, cpFloat ratio) -{ - return (cpConstraint *)cpGearJointInit(cpGearJointAlloc(), a, b, phase, ratio); -} - -cpBool -cpConstraintIsGearJoint(const cpConstraint *constraint) -{ - return (constraint->klass == &klass); -} - -cpFloat -cpGearJointGetPhase(const cpConstraint *constraint) -{ - cpAssertHard(cpConstraintIsGearJoint(constraint), "Constraint is not a ratchet joint."); - return ((cpGearJoint *)constraint)->phase; -} - -void -cpGearJointSetPhase(cpConstraint *constraint, cpFloat phase) -{ - cpAssertHard(cpConstraintIsGearJoint(constraint), "Constraint is not a ratchet joint."); - cpConstraintActivateBodies(constraint); - ((cpGearJoint *)constraint)->phase = phase; -} - -cpFloat -cpGearJointGetRatio(const cpConstraint *constraint) -{ - cpAssertHard(cpConstraintIsGearJoint(constraint), "Constraint is not a ratchet joint."); - return ((cpGearJoint *)constraint)->ratio; -} - -void -cpGearJointSetRatio(cpConstraint *constraint, cpFloat ratio) -{ - cpAssertHard(cpConstraintIsGearJoint(constraint), "Constraint is not a ratchet joint."); - cpConstraintActivateBodies(constraint); - ((cpGearJoint *)constraint)->ratio = ratio; - ((cpGearJoint *)constraint)->ratio_inv = 1.0f/ratio; -} diff --git a/3rdparty/chipmunk/src/cpGrooveJoint.c b/3rdparty/chipmunk/src/cpGrooveJoint.c deleted file mode 100644 index 50d1857d440b..000000000000 --- a/3rdparty/chipmunk/src/cpGrooveJoint.c +++ /dev/null @@ -1,197 +0,0 @@ -/* Copyright (c) 2013 Scott Lembcke and Howling Moon Software - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -#include "chipmunk/chipmunk_private.h" - -static void -preStep(cpGrooveJoint *joint, cpFloat dt) -{ - cpBody *a = joint->constraint.a; - cpBody *b = joint->constraint.b; - - // calculate endpoints in worldspace - cpVect ta = cpTransformPoint(a->transform, joint->grv_a); - cpVect tb = cpTransformPoint(a->transform, joint->grv_b); - - // calculate axis - cpVect n = cpTransformVect(a->transform, joint->grv_n); - cpFloat d = cpvdot(ta, n); - - joint->grv_tn = n; - joint->r2 = cpTransformVect(b->transform, cpvsub(joint->anchorB, b->cog)); - - // calculate tangential distance along the axis of r2 - cpFloat td = cpvcross(cpvadd(b->p, joint->r2), n); - // calculate clamping factor and r2 - if(td <= cpvcross(ta, n)){ - joint->clamp = 1.0f; - joint->r1 = cpvsub(ta, a->p); - } else if(td >= cpvcross(tb, n)){ - joint->clamp = -1.0f; - joint->r1 = cpvsub(tb, a->p); - } else { - joint->clamp = 0.0f; - joint->r1 = cpvsub(cpvadd(cpvmult(cpvperp(n), -td), cpvmult(n, d)), a->p); - } - - // Calculate mass tensor - joint->k = k_tensor(a, b, joint->r1, joint->r2); - - // calculate bias velocity - cpVect delta = cpvsub(cpvadd(b->p, joint->r2), cpvadd(a->p, joint->r1)); - joint->bias = cpvclamp(cpvmult(delta, -bias_coef(joint->constraint.errorBias, dt)/dt), joint->constraint.maxBias); -} - -static void -applyCachedImpulse(cpGrooveJoint *joint, cpFloat dt_coef) -{ - cpBody *a = joint->constraint.a; - cpBody *b = joint->constraint.b; - - apply_impulses(a, b, joint->r1, joint->r2, cpvmult(joint->jAcc, dt_coef)); -} - -static inline cpVect -grooveConstrain(cpGrooveJoint *joint, cpVect j, cpFloat dt){ - cpVect n = joint->grv_tn; - cpVect jClamp = (joint->clamp*cpvcross(j, n) > 0.0f) ? j : cpvproject(j, n); - return cpvclamp(jClamp, joint->constraint.maxForce*dt); -} - -static void -applyImpulse(cpGrooveJoint *joint, cpFloat dt) -{ - cpBody *a = joint->constraint.a; - cpBody *b = joint->constraint.b; - - cpVect r1 = joint->r1; - cpVect r2 = joint->r2; - - // compute impulse - cpVect vr = relative_velocity(a, b, r1, r2); - - cpVect j = cpMat2x2Transform(joint->k, cpvsub(joint->bias, vr)); - cpVect jOld = joint->jAcc; - joint->jAcc = grooveConstrain(joint, cpvadd(jOld, j), dt); - j = cpvsub(joint->jAcc, jOld); - - // apply impulse - apply_impulses(a, b, joint->r1, joint->r2, j); -} - -static cpFloat -getImpulse(cpGrooveJoint *joint) -{ - return cpvlength(joint->jAcc); -} - -static const cpConstraintClass klass = { - (cpConstraintPreStepImpl)preStep, - (cpConstraintApplyCachedImpulseImpl)applyCachedImpulse, - (cpConstraintApplyImpulseImpl)applyImpulse, - (cpConstraintGetImpulseImpl)getImpulse, -}; - -cpGrooveJoint * -cpGrooveJointAlloc(void) -{ - return (cpGrooveJoint *)cpcalloc(1, sizeof(cpGrooveJoint)); -} - -cpGrooveJoint * -cpGrooveJointInit(cpGrooveJoint *joint, cpBody *a, cpBody *b, cpVect groove_a, cpVect groove_b, cpVect anchorB) -{ - cpConstraintInit((cpConstraint *)joint, &klass, a, b); - - joint->grv_a = groove_a; - joint->grv_b = groove_b; - joint->grv_n = cpvperp(cpvnormalize(cpvsub(groove_b, groove_a))); - joint->anchorB = anchorB; - - joint->jAcc = cpvzero; - - return joint; -} - -cpConstraint * -cpGrooveJointNew(cpBody *a, cpBody *b, cpVect groove_a, cpVect groove_b, cpVect anchorB) -{ - return (cpConstraint *)cpGrooveJointInit(cpGrooveJointAlloc(), a, b, groove_a, groove_b, anchorB); -} - -cpBool -cpConstraintIsGrooveJoint(const cpConstraint *constraint) -{ - return (constraint->klass == &klass); -} - -cpVect -cpGrooveJointGetGrooveA(const cpConstraint *constraint) -{ - cpAssertHard(cpConstraintIsGrooveJoint(constraint), "Constraint is not a groove joint."); - return ((cpGrooveJoint *)constraint)->grv_a; -} - -void -cpGrooveJointSetGrooveA(cpConstraint *constraint, cpVect value) -{ - cpAssertHard(cpConstraintIsGrooveJoint(constraint), "Constraint is not a groove joint."); - cpGrooveJoint *g = (cpGrooveJoint *)constraint; - - g->grv_a = value; - g->grv_n = cpvperp(cpvnormalize(cpvsub(g->grv_b, value))); - - cpConstraintActivateBodies(constraint); -} - -cpVect -cpGrooveJointGetGrooveB(const cpConstraint *constraint) -{ - cpAssertHard(cpConstraintIsGrooveJoint(constraint), "Constraint is not a groove joint."); - return ((cpGrooveJoint *)constraint)->grv_b; -} - -void -cpGrooveJointSetGrooveB(cpConstraint *constraint, cpVect value) -{ - cpAssertHard(cpConstraintIsGrooveJoint(constraint), "Constraint is not a groove joint."); - cpGrooveJoint *g = (cpGrooveJoint *)constraint; - - g->grv_b = value; - g->grv_n = cpvperp(cpvnormalize(cpvsub(value, g->grv_a))); - - cpConstraintActivateBodies(constraint); -} - -cpVect -cpGrooveJointGetAnchorB(const cpConstraint *constraint) -{ - cpAssertHard(cpConstraintIsGrooveJoint(constraint), "Constraint is not a groove joint."); - return ((cpGrooveJoint *)constraint)->anchorB; -} - -void -cpGrooveJointSetAnchorB(cpConstraint *constraint, cpVect anchorB) -{ - cpAssertHard(cpConstraintIsGrooveJoint(constraint), "Constraint is not a groove joint."); - cpConstraintActivateBodies(constraint); - ((cpGrooveJoint *)constraint)->anchorB = anchorB; -} diff --git a/3rdparty/chipmunk/src/cpHashSet.c b/3rdparty/chipmunk/src/cpHashSet.c deleted file mode 100644 index b2918defb531..000000000000 --- a/3rdparty/chipmunk/src/cpHashSet.c +++ /dev/null @@ -1,253 +0,0 @@ -/* Copyright (c) 2013 Scott Lembcke and Howling Moon Software - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -#include "chipmunk/chipmunk_private.h" -#include "prime.h" - -typedef struct cpHashSetBin { - void *elt; - cpHashValue hash; - struct cpHashSetBin *next; -} cpHashSetBin; - -struct cpHashSet { - unsigned int entries, size; - - cpHashSetEqlFunc eql; - void *default_value; - - cpHashSetBin **table; - cpHashSetBin *pooledBins; - - cpArray *allocatedBuffers; -}; - -void -cpHashSetFree(cpHashSet *set) -{ - if(set){ - cpfree(set->table); - - cpArrayFreeEach(set->allocatedBuffers, cpfree); - cpArrayFree(set->allocatedBuffers); - - cpfree(set); - } -} - -cpHashSet * -cpHashSetNew(int size, cpHashSetEqlFunc eqlFunc) -{ - cpHashSet *set = (cpHashSet *)cpcalloc(1, sizeof(cpHashSet)); - - set->size = next_prime(size); - set->entries = 0; - - set->eql = eqlFunc; - set->default_value = NULL; - - set->table = (cpHashSetBin **)cpcalloc(set->size, sizeof(cpHashSetBin *)); - set->pooledBins = NULL; - - set->allocatedBuffers = cpArrayNew(0); - - return set; -} - -void -cpHashSetSetDefaultValue(cpHashSet *set, void *default_value) -{ - set->default_value = default_value; -} - -static int -setIsFull(cpHashSet *set) -{ - return (set->entries >= set->size); -} - -static void -cpHashSetResize(cpHashSet *set) -{ - // Get the next approximate doubled prime. - unsigned int newSize = next_prime(set->size + 1); - // Allocate a new table. - cpHashSetBin **newTable = (cpHashSetBin **)cpcalloc(newSize, sizeof(cpHashSetBin *)); - - // Iterate over the chains. - for(unsigned int i=0; isize; i++){ - // Rehash the bins into the new table. - cpHashSetBin *bin = set->table[i]; - while(bin){ - cpHashSetBin *next = bin->next; - - cpHashValue idx = bin->hash%newSize; - bin->next = newTable[idx]; - newTable[idx] = bin; - - bin = next; - } - } - - cpfree(set->table); - - set->table = newTable; - set->size = newSize; -} - -static inline void -recycleBin(cpHashSet *set, cpHashSetBin *bin) -{ - bin->next = set->pooledBins; - set->pooledBins = bin; - bin->elt = NULL; -} - -static cpHashSetBin * -getUnusedBin(cpHashSet *set) -{ - cpHashSetBin *bin = set->pooledBins; - - if(bin){ - set->pooledBins = bin->next; - return bin; - } else { - // Pool is exhausted, make more - int count = CP_BUFFER_BYTES/sizeof(cpHashSetBin); - cpAssertHard(count, "Internal Error: Buffer size is too small."); - - cpHashSetBin *buffer = (cpHashSetBin *)cpcalloc(1, CP_BUFFER_BYTES); - cpArrayPush(set->allocatedBuffers, buffer); - - // push all but the first one, return it instead - for(int i=1; ientries; -} - -const void * -cpHashSetInsert(cpHashSet *set, cpHashValue hash, const void *ptr, cpHashSetTransFunc trans, void *data) -{ - cpHashValue idx = hash%set->size; - - // Find the bin with the matching element. - cpHashSetBin *bin = set->table[idx]; - while(bin && !set->eql(ptr, bin->elt)) - bin = bin->next; - - // Create it if necessary. - if(!bin){ - bin = getUnusedBin(set); - bin->hash = hash; - bin->elt = (trans ? trans(ptr, data) : data); - - bin->next = set->table[idx]; - set->table[idx] = bin; - - set->entries++; - if(setIsFull(set)) cpHashSetResize(set); - } - - return bin->elt; -} - -const void * -cpHashSetRemove(cpHashSet *set, cpHashValue hash, const void *ptr) -{ - cpHashValue idx = hash%set->size; - - cpHashSetBin **prev_ptr = &set->table[idx]; - cpHashSetBin *bin = set->table[idx]; - - // Find the bin - while(bin && !set->eql(ptr, bin->elt)){ - prev_ptr = &bin->next; - bin = bin->next; - } - - // Remove it if it exists. - if(bin){ - // Update the previous linked list pointer - (*prev_ptr) = bin->next; - set->entries--; - - const void *elt = bin->elt; - recycleBin(set, bin); - - return elt; - } - - return NULL; -} - -const void * -cpHashSetFind(cpHashSet *set, cpHashValue hash, const void *ptr) -{ - cpHashValue idx = hash%set->size; - cpHashSetBin *bin = set->table[idx]; - while(bin && !set->eql(ptr, bin->elt)) - bin = bin->next; - - return (bin ? bin->elt : set->default_value); -} - -void -cpHashSetEach(cpHashSet *set, cpHashSetIteratorFunc func, void *data) -{ - for(unsigned int i=0; isize; i++){ - cpHashSetBin *bin = set->table[i]; - while(bin){ - cpHashSetBin *next = bin->next; - func(bin->elt, data); - bin = next; - } - } -} - -void -cpHashSetFilter(cpHashSet *set, cpHashSetFilterFunc func, void *data) -{ - for(unsigned int i=0; isize; i++){ - // The rest works similarly to cpHashSetRemove() above. - cpHashSetBin **prev_ptr = &set->table[i]; - cpHashSetBin *bin = set->table[i]; - while(bin){ - cpHashSetBin *next = bin->next; - - if(func(bin->elt, data)){ - prev_ptr = &bin->next; - } else { - (*prev_ptr) = next; - - set->entries--; - recycleBin(set, bin); - } - - bin = next; - } - } -} diff --git a/3rdparty/chipmunk/src/cpHastySpace.c b/3rdparty/chipmunk/src/cpHastySpace.c deleted file mode 100644 index 8422c3ebe64a..000000000000 --- a/3rdparty/chipmunk/src/cpHastySpace.c +++ /dev/null @@ -1,700 +0,0 @@ -// Copyright 2013 Howling Moon Software. All rights reserved. -// See http://chipmunk2d.net/legal.php for more information. - -#include -#include - -//TODO: Move all the thread stuff to another file - -//#include - -#ifdef __APPLE__ -#include -#endif - -#ifndef _WIN32 -#include -#elif defined(__MINGW32__) -#include -#else -#ifndef WIN32_LEAN_AND_MEAN -#define WIN32_LEAN_AND_MEAN -#endif - -#ifndef NOMINMAX -#define NOMINMAX -#endif - -#include // _beginthreadex -#include - -#ifndef ETIMEDOUT -#define ETIMEDOUT 1 -#endif - -// Simple pthread implementation for Windows -// Made from scratch to avoid the LGPL licence from pthread-win32 -enum { - SIGNAL = 0, - BROADCAST = 1, - MAX_EVENTS = 2 -}; - -typedef HANDLE pthread_t; -typedef struct -{ - // Based on http://www.cs.wustl.edu/~schmidt/win32-cv-1.html since Windows has no condition variable until NT6 - UINT waiters_count; - // Count of the number of waiters. - - CRITICAL_SECTION waiters_count_lock; - // Serialize access to . - - HANDLE events[MAX_EVENTS]; -} pthread_cond_t; -typedef CRITICAL_SECTION pthread_mutex_t; - -typedef struct {} pthread_condattr_t; // Dummy; - -int pthread_cond_destroy(pthread_cond_t* cv) -{ - CloseHandle(cv->events[BROADCAST]); - CloseHandle(cv->events[SIGNAL]); - - DeleteCriticalSection(&cv->waiters_count_lock); - - return 0; -} - -int pthread_cond_init(pthread_cond_t* cv, const pthread_condattr_t* attr) -{ - // Initialize the count to 0. - cv->waiters_count = 0; - - // Create an auto-reset event. - cv->events[SIGNAL] = CreateEvent(NULL, // no security - FALSE, // auto-reset event - FALSE, // non-signaled initially - NULL); // unnamed - - // Create a manual-reset event. - cv->events[BROADCAST] = CreateEvent(NULL, // no security - TRUE, // manual-reset - FALSE, // non-signaled initially - NULL); // unnamed - - InitializeCriticalSection(&cv->waiters_count_lock); - - return 0; -} - -int pthread_cond_broadcast(pthread_cond_t *cv) -{ - // Avoid race conditions. - EnterCriticalSection(&cv->waiters_count_lock); - int have_waiters = cv->waiters_count > 0; - LeaveCriticalSection(&cv->waiters_count_lock); - - if (have_waiters) - SetEvent(cv->events[BROADCAST]); - - return 0; -} - -int pthread_cond_signal(pthread_cond_t* cv) -{ - // Avoid race conditions. - EnterCriticalSection(&cv->waiters_count_lock); - int have_waiters = cv->waiters_count > 0; - LeaveCriticalSection(&cv->waiters_count_lock); - - if (have_waiters) - SetEvent(cv->events[SIGNAL]); - - return 0; -} - -int pthread_cond_wait(pthread_cond_t* cv, pthread_mutex_t* external_mutex) -{ - // Avoid race conditions. - EnterCriticalSection(&cv->waiters_count_lock); - cv->waiters_count++; - LeaveCriticalSection(&cv->waiters_count_lock); - - // It's ok to release the here since Win32 - // manual-reset events maintain state when used with - // . This avoids the "lost wakeup" bug... - LeaveCriticalSection(external_mutex); - - // Wait for either event to become signaled due to - // being called or being called. - int result = WaitForMultipleObjects(2, cv->events, FALSE, INFINITE); - - EnterCriticalSection(&cv->waiters_count_lock); - cv->waiters_count--; - int last_waiter = - result == WAIT_OBJECT_0 + BROADCAST - && cv->waiters_count == 0; - LeaveCriticalSection(&cv->waiters_count_lock); - - // Some thread called . - if (last_waiter) - // We're the last waiter to be notified or to stop waiting, so - // reset the manual event. - ResetEvent(cv->events[BROADCAST]); - - // Reacquire the . - EnterCriticalSection(external_mutex); - - return result == WAIT_TIMEOUT ? ETIMEDOUT : 0; -} - -typedef struct {} pthread_mutexattr_t; //< Dummy - -int pthread_mutex_init(pthread_mutex_t* mutex, const pthread_mutexattr_t* attr) -{ - InitializeCriticalSection(mutex); - return 0; -} - -int pthread_mutex_destroy(pthread_mutex_t* mutex) -{ - DeleteCriticalSection(mutex); - return 0; -} - -int pthread_mutex_lock(pthread_mutex_t* mutex) -{ - EnterCriticalSection(mutex); - return 0; -} - -int pthread_mutex_unlock(pthread_mutex_t* mutex) -{ - LeaveCriticalSection(mutex); - return 0; -} - -typedef struct {} pthread_attr_t; - -typedef struct -{ - void *(*start_routine) (void *); - void* arg; -} pthread_internal_thread; - -unsigned int __stdcall ThreadProc(void* userdata) -{ - pthread_internal_thread* ud = (pthread_internal_thread*) userdata; - ud->start_routine(ud->arg); - - free(ud); - - return 0; -} - -int pthread_create(pthread_t* thread, const pthread_attr_t* attr, void *(*start_routine) (void *), void *arg) -{ - pthread_internal_thread* ud = (pthread_internal_thread*) malloc(sizeof(pthread_internal_thread)); - ud->start_routine = start_routine; - ud->arg = arg; - - *thread = (HANDLE) (_beginthreadex(NULL, 0, &ThreadProc, ud, 0, NULL)); - if (!*thread) - return 1; - - return 0; -} - -int pthread_join(pthread_t thread, void **value_ptr) -{ - WaitForSingleObject(thread, INFINITE); - CloseHandle(thread); - - return 0; -} - -#endif - -#include "chipmunk/chipmunk_private.h" -#include "chipmunk/cpHastySpace.h" - - -//MARK: ARM NEON Solver - -#if __ARM_NEON__ -#include - -// Tested and known to work fine with Clang 3.0 and GCC 4.2 -// Doesn't work with Clang 1.6, and I have no idea why. -#if defined(__clang_major__) && __clang_major__ < 3 - #error Compiler not supported. -#endif - -#if CP_USE_DOUBLES - #if !__arm64 - #error Cannot use CP_USE_DOUBLES on 32 bit ARM. - #endif - - typedef float64_t cpFloat_t; - typedef float64x2_t cpFloatx2_t; - #define vld vld1q_f64 - #define vdup_n vdupq_n_f64 - #define vst vst1q_f64 - #define vst_lane vst1q_lane_f64 - #define vadd vaddq_f64 - #define vsub vsubq_f64 - #define vpadd vpaddq_f64 - #define vmul vmulq_f64 - #define vmul_n vmulq_n_f64 - #define vneg vnegq_f64 - #define vget_lane vgetq_lane_f64 - #define vset_lane vsetq_lane_f64 - #define vmin vminq_f64 - #define vmax vmaxq_f64 - #define vrev(__a) __builtin_shufflevector(__a, __a, 1, 0) -#else - typedef float32_t cpFloat_t; - typedef float32x2_t cpFloatx2_t; - #define vld vld1_f32 - #define vdup_n vdup_n_f32 - #define vst vst1_f32 - #define vst_lane vst1_lane_f32 - #define vadd vadd_f32 - #define vsub vsub_f32 - #define vpadd vpadd_f32 - #define vmul vmul_f32 - #define vmul_n vmul_n_f32 - #define vneg vneg_f32 - #define vget_lane vget_lane_f32 - #define vset_lane vset_lane_f32 - #define vmin vmin_f32 - #define vmax vmax_f32 - #define vrev vrev64_f32 -#endif - -// TODO could probably do better here, maybe using vcreate? -// especially for the constants -// Maybe use the {} notation for GCC/Clang? -static inline cpFloatx2_t -vmake(cpFloat_t x, cpFloat_t y) -{ -// cpFloatx2_t v = {}; -// v = vset_lane(x, v, 0); -// v = vset_lane(y, v, 1); -// -// return v; - - // This might not be super compatible, but all the NEON headers use it... - return (cpFloatx2_t){x, y}; -} - -static void -cpArbiterApplyImpulse_NEON(cpArbiter *arb) -{ - cpBody *a = arb->body_a; - cpBody *b = arb->body_b; - cpFloatx2_t surface_vr = vld((cpFloat_t *)&arb->surface_vr); - cpFloatx2_t n = vld((cpFloat_t *)&arb->n); - cpFloat_t friction = arb->u; - - int numContacts = arb->count; - struct cpContact *contacts = arb->contacts; - for(int i=0; ir1); - cpFloatx2_t r2 = vld((cpFloat_t *)&con->r2); - - cpFloatx2_t perp = vmake(-1.0, 1.0); - cpFloatx2_t r1p = vmul(vrev(r1), perp); - cpFloatx2_t r2p = vmul(vrev(r2), perp); - - cpFloatx2_t vBias_a = vld((cpFloat_t *)&a->v_bias); - cpFloatx2_t vBias_b = vld((cpFloat_t *)&b->v_bias); - cpFloatx2_t wBias = vmake(a->w_bias, b->w_bias); - - cpFloatx2_t vb1 = vadd(vBias_a, vmul_n(r1p, vget_lane(wBias, 0))); - cpFloatx2_t vb2 = vadd(vBias_b, vmul_n(r2p, vget_lane(wBias, 1))); - cpFloatx2_t vbr = vsub(vb2, vb1); - - cpFloatx2_t v_a = vld((cpFloat_t *)&a->v); - cpFloatx2_t v_b = vld((cpFloat_t *)&b->v); - cpFloatx2_t w = vmake(a->w, b->w); - cpFloatx2_t v1 = vadd(v_a, vmul_n(r1p, vget_lane(w, 0))); - cpFloatx2_t v2 = vadd(v_b, vmul_n(r2p, vget_lane(w, 1))); - cpFloatx2_t vr = vsub(v2, v1); - - cpFloatx2_t vbn_vrn = vpadd(vmul(vbr, n), vmul(vr, n)); - - cpFloatx2_t v_offset = vmake(con->bias, -con->bounce); - cpFloatx2_t jOld = vmake(con->jBias, con->jnAcc); - cpFloatx2_t jbn_jn = vmul_n(vsub(v_offset, vbn_vrn), con->nMass); - jbn_jn = vmax(vadd(jOld, jbn_jn), vdup_n(0.0)); - cpFloatx2_t jApply = vsub(jbn_jn, jOld); - - cpFloatx2_t t = vmul(vrev(n), perp); - cpFloatx2_t vrt_tmp = vmul(vadd(vr, surface_vr), t); - cpFloatx2_t vrt = vpadd(vrt_tmp, vrt_tmp); - - cpFloatx2_t jtOld = {}; jtOld = vset_lane(con->jtAcc, jtOld, 0); - cpFloatx2_t jtMax = vrev(vmul_n(jbn_jn, friction)); - cpFloatx2_t jt = vmul_n(vrt, -con->tMass); - jt = vmax(vneg(jtMax), vmin(vadd(jtOld, jt), jtMax)); - cpFloatx2_t jtApply = vsub(jt, jtOld); - - cpFloatx2_t i_inv = vmake(-a->i_inv, b->i_inv); - cpFloatx2_t nperp = vmake(1.0, -1.0); - - cpFloatx2_t jBias = vmul_n(n, vget_lane(jApply, 0)); - cpFloatx2_t jBiasCross = vmul(vrev(jBias), nperp); - cpFloatx2_t biasCrosses = vpadd(vmul(r1, jBiasCross), vmul(r2, jBiasCross)); - wBias = vadd(wBias, vmul(i_inv, biasCrosses)); - - vBias_a = vsub(vBias_a, vmul_n(jBias, a->m_inv)); - vBias_b = vadd(vBias_b, vmul_n(jBias, b->m_inv)); - - cpFloatx2_t j = vadd(vmul_n(n, vget_lane(jApply, 1)), vmul_n(t, vget_lane(jtApply, 0))); - cpFloatx2_t jCross = vmul(vrev(j), nperp); - cpFloatx2_t crosses = vpadd(vmul(r1, jCross), vmul(r2, jCross)); - w = vadd(w, vmul(i_inv, crosses)); - - v_a = vsub(v_a, vmul_n(j, a->m_inv)); - v_b = vadd(v_b, vmul_n(j, b->m_inv)); - - // TODO would moving these earlier help pipeline them better? - vst((cpFloat_t *)&a->v_bias, vBias_a); - vst((cpFloat_t *)&b->v_bias, vBias_b); - vst_lane((cpFloat_t *)&a->w_bias, wBias, 0); - vst_lane((cpFloat_t *)&b->w_bias, wBias, 1); - - vst((cpFloat_t *)&a->v, v_a); - vst((cpFloat_t *)&b->v, v_b); - vst_lane((cpFloat_t *)&a->w, w, 0); - vst_lane((cpFloat_t *)&b->w, w, 1); - - vst_lane((cpFloat_t *)&con->jBias, jbn_jn, 0); - vst_lane((cpFloat_t *)&con->jnAcc, jbn_jn, 1); - vst_lane((cpFloat_t *)&con->jtAcc, jt, 0); - } -} - -#endif - -//MARK: PThreads - -// Right now using more than 2 threads probably wont help your performance any. -// If you are using a ridiculous number of iterations it could help though. -#define MAX_THREADS 2 - -struct ThreadContext { - pthread_t thread; - cpHastySpace *space; - unsigned long thread_num; -}; - -typedef void (*cpHastySpaceWorkFunction)(cpSpace *space, unsigned long worker, unsigned long worker_count); - -struct cpHastySpace { - cpSpace space; - - // Number of worker threads (including the main thread) - unsigned long num_threads; - - // Number of worker threads currently executing. (also including the main thread) - unsigned long num_working; - - // Number of constraints (plus contacts) that must exist per step to start the worker threads. - unsigned long constraint_count_threshold; - - pthread_mutex_t mutex; - pthread_cond_t cond_work, cond_resume; - - // Work function to invoke. - cpHastySpaceWorkFunction work; - - struct ThreadContext workers[MAX_THREADS - 1]; -}; - -static void * -WorkerThreadLoop(struct ThreadContext *context) -{ - cpHastySpace *hasty = context->space; - - unsigned long thread = context->thread_num; - unsigned long num_threads = hasty->num_threads; - - for(;;){ - pthread_mutex_lock(&hasty->mutex); { - if(--hasty->num_working == 0){ - pthread_cond_signal(&hasty->cond_resume); - } - - pthread_cond_wait(&hasty->cond_work, &hasty->mutex); - } pthread_mutex_unlock(&hasty->mutex); - - cpHastySpaceWorkFunction func = hasty->work; - if(func){ - hasty->work(&hasty->space, thread, num_threads); - } else { - break; - } - } - - return NULL; -} - -static void -RunWorkers(cpHastySpace *hasty, cpHastySpaceWorkFunction func) -{ - hasty->num_working = hasty->num_threads - 1; - hasty->work = func; - - if(hasty->num_working > 0){ - pthread_mutex_lock(&hasty->mutex); { - pthread_cond_broadcast(&hasty->cond_work); - } pthread_mutex_unlock(&hasty->mutex); - - func((cpSpace *)hasty, 0, hasty->num_threads); - - pthread_mutex_lock(&hasty->mutex); { - if(hasty->num_working > 0){ - pthread_cond_wait(&hasty->cond_resume, &hasty->mutex); - } - } pthread_mutex_unlock(&hasty->mutex); - } else { - func((cpSpace *)hasty, 0, hasty->num_threads); - } - - hasty->work = NULL; -} - -static void -Solver(cpSpace *space, unsigned long worker, unsigned long worker_count) -{ - cpArray *constraints = space->constraints; - cpArray *arbiters = space->arbiters; - - cpFloat dt = space->curr_dt; - unsigned long iterations = (space->iterations + worker_count - 1)/worker_count; - - for(unsigned long i=0; inum; j++){ - cpArbiter *arb = (cpArbiter *)arbiters->arr[j]; - #ifdef __ARM_NEON__ - cpArbiterApplyImpulse_NEON(arb); - #else - cpArbiterApplyImpulse(arb); - #endif - } - - for(int j=0; jnum; j++){ - cpConstraint *constraint = (cpConstraint *)constraints->arr[j]; - constraint->klass->applyImpulse(constraint, dt); - } - } -} - -//MARK: Thread Management Functions - -static void -HaltThreads(cpHastySpace *hasty) -{ - pthread_mutex_t *mutex = &hasty->mutex; - pthread_mutex_lock(mutex); { - hasty->work = NULL; // NULL work function means break and exit - pthread_cond_broadcast(&hasty->cond_work); - } pthread_mutex_unlock(mutex); - - for(unsigned long i=0; i<(hasty->num_threads-1); i++){ - pthread_join(hasty->workers[i].thread, NULL); - } -} - -void -cpHastySpaceSetThreads(cpSpace *space, unsigned long threads) -{ -#if TARGET_IPHONE_SIMULATOR == 1 - // Individual values appear to be written non-atomically when compiled as debug for the simulator. - // No idea why, so threads are disabled. - threads = 1; -#endif - - cpHastySpace *hasty = (cpHastySpace *)space; - HaltThreads(hasty); - -#ifdef __APPLE__ - if(threads == 0){ - size_t size = sizeof(threads); - sysctlbyname("hw.ncpu", &threads, &size, NULL, 0); - } -#else - if(threads == 0) threads = 1; -#endif - - hasty->num_threads = (threads < MAX_THREADS ? threads : MAX_THREADS); - hasty->num_working = hasty->num_threads - 1; - - // Create the worker threads and wait for them to signal ready. - if(hasty->num_working > 0){ - pthread_mutex_lock(&hasty->mutex); - for(unsigned long i=0; i<(hasty->num_threads-1); i++){ - hasty->workers[i].space = hasty; - hasty->workers[i].thread_num = i + 1; - - pthread_create(&hasty->workers[i].thread, NULL, (void*(*)(void*))WorkerThreadLoop, &hasty->workers[i]); - } - - pthread_cond_wait(&hasty->cond_resume, &hasty->mutex); - pthread_mutex_unlock(&hasty->mutex); - } -} - -unsigned long -cpHastySpaceGetThreads(cpSpace *space) -{ - return ((cpHastySpace *)space)->num_threads; -} - -//MARK: Overriden cpSpace Functions. - -cpSpace * -cpHastySpaceNew(void) -{ - cpHastySpace *hasty = (cpHastySpace *)cpcalloc(1, sizeof(cpHastySpace)); - cpSpaceInit((cpSpace *)hasty); - - pthread_mutex_init(&hasty->mutex, NULL); - pthread_cond_init(&hasty->cond_work, NULL); - pthread_cond_init(&hasty->cond_resume, NULL); - - // TODO magic number, should test this more thoroughly. - hasty->constraint_count_threshold = 50; - - // Default to 1 thread for determinism. - hasty->num_threads = 1; - cpHastySpaceSetThreads((cpSpace *)hasty, 1); - - return (cpSpace *)hasty; -} - -void -cpHastySpaceFree(cpSpace *space) -{ - cpHastySpace *hasty = (cpHastySpace *)space; - - HaltThreads(hasty); - - pthread_mutex_destroy(&hasty->mutex); - pthread_cond_destroy(&hasty->cond_work); - pthread_cond_destroy(&hasty->cond_resume); - - cpSpaceFree(space); -} - -void -cpHastySpaceStep(cpSpace *space, cpFloat dt) -{ - // don't step if the timestep is 0! - if(dt == 0.0f) return; - - space->stamp++; - - cpFloat prev_dt = space->curr_dt; - space->curr_dt = dt; - - cpArray *bodies = space->dynamicBodies; - cpArray *constraints = space->constraints; - cpArray *arbiters = space->arbiters; - - // Reset and empty the arbiter list. - for(int i=0; inum; i++){ - cpArbiter *arb = (cpArbiter *)arbiters->arr[i]; - arb->state = CP_ARBITER_STATE_NORMAL; - - // If both bodies are awake, unthread the arbiter from the contact graph. - if(!cpBodyIsSleeping(arb->body_a) && !cpBodyIsSleeping(arb->body_b)){ - cpArbiterUnthread(arb); - } - } - arbiters->num = 0; - - cpSpaceLock(space); { - // Integrate positions - for(int i=0; inum; i++){ - cpBody *body = (cpBody *)bodies->arr[i]; - body->position_func(body, dt); - } - - // Find colliding pairs. - cpSpacePushFreshContactBuffer(space); - cpSpatialIndexEach(space->dynamicShapes, (cpSpatialIndexIteratorFunc)cpShapeUpdateFunc, NULL); - cpSpatialIndexReindexQuery(space->dynamicShapes, (cpSpatialIndexQueryFunc)cpSpaceCollideShapes, space); - } cpSpaceUnlock(space, cpFalse); - - // Rebuild the contact graph (and detect sleeping components if sleeping is enabled) - cpSpaceProcessComponents(space, dt); - - cpSpaceLock(space); { - // Clear out old cached arbiters and call separate callbacks - cpHashSetFilter(space->cachedArbiters, (cpHashSetFilterFunc)cpSpaceArbiterSetFilter, space); - - // Prestep the arbiters and constraints. - cpFloat slop = space->collisionSlop; - cpFloat biasCoef = 1.0f - cpfpow(space->collisionBias, dt); - for(int i=0; inum; i++){ - cpArbiterPreStep((cpArbiter *)arbiters->arr[i], dt, slop, biasCoef); - } - - for(int i=0; inum; i++){ - cpConstraint *constraint = (cpConstraint *)constraints->arr[i]; - - cpConstraintPreSolveFunc preSolve = constraint->preSolve; - if(preSolve) preSolve(constraint, space); - - constraint->klass->preStep(constraint, dt); - } - - // Integrate velocities. - cpFloat damping = cpfpow(space->damping, dt); - cpVect gravity = space->gravity; - for(int i=0; inum; i++){ - cpBody *body = (cpBody *)bodies->arr[i]; - body->velocity_func(body, gravity, damping, dt); - } - - // Apply cached impulses - cpFloat dt_coef = (prev_dt == 0.0f ? 0.0f : dt/prev_dt); - for(int i=0; inum; i++){ - cpArbiterApplyCachedImpulse((cpArbiter *)arbiters->arr[i], dt_coef); - } - - for(int i=0; inum; i++){ - cpConstraint *constraint = (cpConstraint *)constraints->arr[i]; - constraint->klass->applyCachedImpulse(constraint, dt_coef); - } - - // Run the impulse solver. - cpHastySpace *hasty = (cpHastySpace *)space; - if((unsigned long)(arbiters->num + constraints->num) > hasty->constraint_count_threshold){ - RunWorkers(hasty, Solver); - } else { - Solver(space, 0, 1); - } - - // Run the constraint post-solve callbacks - for(int i=0; inum; i++){ - cpConstraint *constraint = (cpConstraint *)constraints->arr[i]; - - cpConstraintPostSolveFunc postSolve = constraint->postSolve; - if(postSolve) postSolve(constraint, space); - } - - // run the post-solve callbacks - for(int i=0; inum; i++){ - cpArbiter *arb = (cpArbiter *) arbiters->arr[i]; - - cpCollisionHandler *handler = arb->handler; - handler->postSolveFunc(arb, space, handler->userData); - } - } cpSpaceUnlock(space, cpTrue); -} diff --git a/3rdparty/chipmunk/src/cpMarch.c b/3rdparty/chipmunk/src/cpMarch.c deleted file mode 100644 index 1ba0dabb4601..000000000000 --- a/3rdparty/chipmunk/src/cpMarch.c +++ /dev/null @@ -1,157 +0,0 @@ -// Copyright 2013 Howling Moon Software. All rights reserved. -// See http://chipmunk2d.net/legal.php for more information. - -#include -#include -#include - -#include "chipmunk/chipmunk.h" -#include "chipmunk/cpMarch.h" - - -typedef void (*cpMarchCellFunc)( - cpFloat t, cpFloat a, cpFloat b, cpFloat c, cpFloat d, - cpFloat x0, cpFloat x1, cpFloat y0, cpFloat y1, - cpMarchSegmentFunc segment, void *segment_data -); - -// The looping and sample caching code is shared between cpMarchHard() and cpMarchSoft(). -static void -cpMarchCells( - cpBB bb, unsigned long x_samples, unsigned long y_samples, cpFloat t, - cpMarchSegmentFunc segment, void *segment_data, - cpMarchSampleFunc sample, void *sample_data, - cpMarchCellFunc cell -){ - cpFloat x_denom = 1.0/(cpFloat)(x_samples - 1); - cpFloat y_denom = 1.0/(cpFloat)(y_samples - 1); - - // TODO range assertions and short circuit for 0 sized windows. - - // Keep a copy of the previous row to avoid double lookups. - cpFloat *buffer = (cpFloat *)cpcalloc(x_samples, sizeof(cpFloat)); - for(unsigned long i=0; it)<<0 | (b>t)<<1 | (c>t)<<2 | (d>t)<<3){ - case 0x1: seg(cpv(x0, midlerp(y0,y1,a,c,t)), cpv(midlerp(x0,x1,a,b,t), y0), segment, segment_data); break; - case 0x2: seg(cpv(midlerp(x0,x1,a,b,t), y0), cpv(x1, midlerp(y0,y1,b,d,t)), segment, segment_data); break; - case 0x3: seg(cpv(x0, midlerp(y0,y1,a,c,t)), cpv(x1, midlerp(y0,y1,b,d,t)), segment, segment_data); break; - case 0x4: seg(cpv(midlerp(x0,x1,c,d,t), y1), cpv(x0, midlerp(y0,y1,a,c,t)), segment, segment_data); break; - case 0x5: seg(cpv(midlerp(x0,x1,c,d,t), y1), cpv(midlerp(x0,x1,a,b,t), y0), segment, segment_data); break; - case 0x6: seg(cpv(midlerp(x0,x1,a,b,t), y0), cpv(x1, midlerp(y0,y1,b,d,t)), segment, segment_data); - seg(cpv(midlerp(x0,x1,c,d,t), y1), cpv(x0, midlerp(y0,y1,a,c,t)), segment, segment_data); break; - case 0x7: seg(cpv(midlerp(x0,x1,c,d,t), y1), cpv(x1, midlerp(y0,y1,b,d,t)), segment, segment_data); break; - case 0x8: seg(cpv(x1, midlerp(y0,y1,b,d,t)), cpv(midlerp(x0,x1,c,d,t), y1), segment, segment_data); break; - case 0x9: seg(cpv(x0, midlerp(y0,y1,a,c,t)), cpv(midlerp(x0,x1,a,b,t), y0), segment, segment_data); - seg(cpv(x1, midlerp(y0,y1,b,d,t)), cpv(midlerp(x0,x1,c,d,t), y1), segment, segment_data); break; - case 0xA: seg(cpv(midlerp(x0,x1,a,b,t), y0), cpv(midlerp(x0,x1,c,d,t), y1), segment, segment_data); break; - case 0xB: seg(cpv(x0, midlerp(y0,y1,a,c,t)), cpv(midlerp(x0,x1,c,d,t), y1), segment, segment_data); break; - case 0xC: seg(cpv(x1, midlerp(y0,y1,b,d,t)), cpv(x0, midlerp(y0,y1,a,c,t)), segment, segment_data); break; - case 0xD: seg(cpv(x1, midlerp(y0,y1,b,d,t)), cpv(midlerp(x0,x1,a,b,t), y0), segment, segment_data); break; - case 0xE: seg(cpv(midlerp(x0,x1,a,b,t), y0), cpv(x0, midlerp(y0,y1,a,c,t)), segment, segment_data); break; - default: break; // 0x0 and 0xF - } -} - -void -cpMarchSoft( - cpBB bb, unsigned long x_samples, unsigned long y_samples, cpFloat t, - cpMarchSegmentFunc segment, void *segment_data, - cpMarchSampleFunc sample, void *sample_data -){ - cpMarchCells(bb, x_samples, y_samples, t, segment, segment_data, sample, sample_data, cpMarchCellSoft); -} - - -// TODO should flip this around eventually. -static inline void -segs(cpVect a, cpVect b, cpVect c, cpMarchSegmentFunc f, void *data) -{ - seg(b, c, f, data); - seg(a, b, f, data); -} - -static void -cpMarchCellHard( - cpFloat t, cpFloat a, cpFloat b, cpFloat c, cpFloat d, - cpFloat x0, cpFloat x1, cpFloat y0, cpFloat y1, - cpMarchSegmentFunc segment, void *segment_data -){ - // midpoints - cpFloat xm = cpflerp(x0, x1, 0.5f); - cpFloat ym = cpflerp(y0, y1, 0.5f); - - switch((a>t)<<0 | (b>t)<<1 | (c>t)<<2 | (d>t)<<3){ - case 0x1: segs(cpv(x0, ym), cpv(xm, ym), cpv(xm, y0), segment, segment_data); break; - case 0x2: segs(cpv(xm, y0), cpv(xm, ym), cpv(x1, ym), segment, segment_data); break; - case 0x3: seg(cpv(x0, ym), cpv(x1, ym), segment, segment_data); break; - case 0x4: segs(cpv(xm, y1), cpv(xm, ym), cpv(x0, ym), segment, segment_data); break; - case 0x5: seg(cpv(xm, y1), cpv(xm, y0), segment, segment_data); break; - case 0x6: segs(cpv(xm, y0), cpv(xm, ym), cpv(x0, ym), segment, segment_data); - segs(cpv(xm, y1), cpv(xm, ym), cpv(x1, ym), segment, segment_data); break; - case 0x7: segs(cpv(xm, y1), cpv(xm, ym), cpv(x1, ym), segment, segment_data); break; - case 0x8: segs(cpv(x1, ym), cpv(xm, ym), cpv(xm, y1), segment, segment_data); break; - case 0x9: segs(cpv(x1, ym), cpv(xm, ym), cpv(xm, y0), segment, segment_data); - segs(cpv(x0, ym), cpv(xm, ym), cpv(xm, y1), segment, segment_data); break; - case 0xA: seg(cpv(xm, y0), cpv(xm, y1), segment, segment_data); break; - case 0xB: segs(cpv(x0, ym), cpv(xm, ym), cpv(xm, y1), segment, segment_data); break; - case 0xC: seg(cpv(x1, ym), cpv(x0, ym), segment, segment_data); break; - case 0xD: segs(cpv(x1, ym), cpv(xm, ym), cpv(xm, y0), segment, segment_data); break; - case 0xE: segs(cpv(xm, y0), cpv(xm, ym), cpv(x0, ym), segment, segment_data); break; - default: break; // 0x0 and 0xF - } -} - -void -cpMarchHard( - cpBB bb, unsigned long x_samples, unsigned long y_samples, cpFloat t, - cpMarchSegmentFunc segment, void *segment_data, - cpMarchSampleFunc sample, void *sample_data -){ - cpMarchCells(bb, x_samples, y_samples, t, segment, segment_data, sample, sample_data, cpMarchCellHard); -} diff --git a/3rdparty/chipmunk/src/cpPinJoint.c b/3rdparty/chipmunk/src/cpPinJoint.c deleted file mode 100644 index 545e78bf8b50..000000000000 --- a/3rdparty/chipmunk/src/cpPinJoint.c +++ /dev/null @@ -1,172 +0,0 @@ -/* Copyright (c) 2013 Scott Lembcke and Howling Moon Software - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -#include "chipmunk/chipmunk_private.h" - -static void -preStep(cpPinJoint *joint, cpFloat dt) -{ - cpBody *a = joint->constraint.a; - cpBody *b = joint->constraint.b; - - joint->r1 = cpTransformVect(a->transform, cpvsub(joint->anchorA, a->cog)); - joint->r2 = cpTransformVect(b->transform, cpvsub(joint->anchorB, b->cog)); - - cpVect delta = cpvsub(cpvadd(b->p, joint->r2), cpvadd(a->p, joint->r1)); - cpFloat dist = cpvlength(delta); - joint->n = cpvmult(delta, 1.0f/(dist ? dist : (cpFloat)INFINITY)); - - // calculate mass normal - joint->nMass = 1.0f/k_scalar(a, b, joint->r1, joint->r2, joint->n); - - // calculate bias velocity - cpFloat maxBias = joint->constraint.maxBias; - joint->bias = cpfclamp(-bias_coef(joint->constraint.errorBias, dt)*(dist - joint->dist)/dt, -maxBias, maxBias); -} - -static void -applyCachedImpulse(cpPinJoint *joint, cpFloat dt_coef) -{ - cpBody *a = joint->constraint.a; - cpBody *b = joint->constraint.b; - - cpVect j = cpvmult(joint->n, joint->jnAcc*dt_coef); - apply_impulses(a, b, joint->r1, joint->r2, j); -} - -static void -applyImpulse(cpPinJoint *joint, cpFloat dt) -{ - cpBody *a = joint->constraint.a; - cpBody *b = joint->constraint.b; - cpVect n = joint->n; - - // compute relative velocity - cpFloat vrn = normal_relative_velocity(a, b, joint->r1, joint->r2, n); - - cpFloat jnMax = joint->constraint.maxForce*dt; - - // compute normal impulse - cpFloat jn = (joint->bias - vrn)*joint->nMass; - cpFloat jnOld = joint->jnAcc; - joint->jnAcc = cpfclamp(jnOld + jn, -jnMax, jnMax); - jn = joint->jnAcc - jnOld; - - // apply impulse - apply_impulses(a, b, joint->r1, joint->r2, cpvmult(n, jn)); -} - -static cpFloat -getImpulse(cpPinJoint *joint) -{ - return cpfabs(joint->jnAcc); -} - -static const cpConstraintClass klass = { - (cpConstraintPreStepImpl)preStep, - (cpConstraintApplyCachedImpulseImpl)applyCachedImpulse, - (cpConstraintApplyImpulseImpl)applyImpulse, - (cpConstraintGetImpulseImpl)getImpulse, -}; - - -cpPinJoint * -cpPinJointAlloc(void) -{ - return (cpPinJoint *)cpcalloc(1, sizeof(cpPinJoint)); -} - -cpPinJoint * -cpPinJointInit(cpPinJoint *joint, cpBody *a, cpBody *b, cpVect anchorA, cpVect anchorB) -{ - cpConstraintInit((cpConstraint *)joint, &klass, a, b); - - joint->anchorA = anchorA; - joint->anchorB = anchorB; - - // STATIC_BODY_CHECK - cpVect p1 = (a ? cpTransformPoint(a->transform, anchorA) : anchorA); - cpVect p2 = (b ? cpTransformPoint(b->transform, anchorB) : anchorB); - joint->dist = cpvlength(cpvsub(p2, p1)); - - cpAssertWarn(joint->dist > 0.0, "You created a 0 length pin joint. A pivot joint will be much more stable."); - - joint->jnAcc = 0.0f; - - return joint; -} - -cpConstraint * -cpPinJointNew(cpBody *a, cpBody *b, cpVect anchorA, cpVect anchorB) -{ - return (cpConstraint *)cpPinJointInit(cpPinJointAlloc(), a, b, anchorA, anchorB); -} - -cpBool -cpConstraintIsPinJoint(const cpConstraint *constraint) -{ - return (constraint->klass == &klass); -} - -cpVect -cpPinJointGetAnchorA(const cpConstraint *constraint) -{ - cpAssertHard(cpConstraintIsPinJoint(constraint), "Constraint is not a pin joint."); - return ((cpPinJoint *)constraint)->anchorA; -} - -void -cpPinJointSetAnchorA(cpConstraint *constraint, cpVect anchorA) -{ - cpAssertHard(cpConstraintIsPinJoint(constraint), "Constraint is not a pin joint."); - cpConstraintActivateBodies(constraint); - ((cpPinJoint *)constraint)->anchorA = anchorA; -} - -cpVect -cpPinJointGetAnchorB(const cpConstraint *constraint) -{ - cpAssertHard(cpConstraintIsPinJoint(constraint), "Constraint is not a pin joint."); - return ((cpPinJoint *)constraint)->anchorB; -} - -void -cpPinJointSetAnchorB(cpConstraint *constraint, cpVect anchorB) -{ - cpAssertHard(cpConstraintIsPinJoint(constraint), "Constraint is not a pin joint."); - cpConstraintActivateBodies(constraint); - ((cpPinJoint *)constraint)->anchorB = anchorB; -} - -cpFloat -cpPinJointGetDist(const cpConstraint *constraint) -{ - cpAssertHard(cpConstraintIsPinJoint(constraint), "Constraint is not a pin joint."); - return ((cpPinJoint *)constraint)->dist; -} - -void -cpPinJointSetDist(cpConstraint *constraint, cpFloat dist) -{ - cpAssertHard(cpConstraintIsPinJoint(constraint), "Constraint is not a pin joint."); - cpConstraintActivateBodies(constraint); - ((cpPinJoint *)constraint)->dist = dist; -} diff --git a/3rdparty/chipmunk/src/cpPivotJoint.c b/3rdparty/chipmunk/src/cpPivotJoint.c deleted file mode 100644 index e45ba072bce5..000000000000 --- a/3rdparty/chipmunk/src/cpPivotJoint.c +++ /dev/null @@ -1,152 +0,0 @@ -/* Copyright (c) 2013 Scott Lembcke and Howling Moon Software - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -#include "chipmunk/chipmunk_private.h" - -static void -preStep(cpPivotJoint *joint, cpFloat dt) -{ - cpBody *a = joint->constraint.a; - cpBody *b = joint->constraint.b; - - joint->r1 = cpTransformVect(a->transform, cpvsub(joint->anchorA, a->cog)); - joint->r2 = cpTransformVect(b->transform, cpvsub(joint->anchorB, b->cog)); - - // Calculate mass tensor - joint-> k = k_tensor(a, b, joint->r1, joint->r2); - - // calculate bias velocity - cpVect delta = cpvsub(cpvadd(b->p, joint->r2), cpvadd(a->p, joint->r1)); - joint->bias = cpvclamp(cpvmult(delta, -bias_coef(joint->constraint.errorBias, dt)/dt), joint->constraint.maxBias); -} - -static void -applyCachedImpulse(cpPivotJoint *joint, cpFloat dt_coef) -{ - cpBody *a = joint->constraint.a; - cpBody *b = joint->constraint.b; - - apply_impulses(a, b, joint->r1, joint->r2, cpvmult(joint->jAcc, dt_coef)); -} - -static void -applyImpulse(cpPivotJoint *joint, cpFloat dt) -{ - cpBody *a = joint->constraint.a; - cpBody *b = joint->constraint.b; - - cpVect r1 = joint->r1; - cpVect r2 = joint->r2; - - // compute relative velocity - cpVect vr = relative_velocity(a, b, r1, r2); - - // compute normal impulse - cpVect j = cpMat2x2Transform(joint->k, cpvsub(joint->bias, vr)); - cpVect jOld = joint->jAcc; - joint->jAcc = cpvclamp(cpvadd(joint->jAcc, j), joint->constraint.maxForce*dt); - j = cpvsub(joint->jAcc, jOld); - - // apply impulse - apply_impulses(a, b, joint->r1, joint->r2, j); -} - -static cpFloat -getImpulse(cpConstraint *joint) -{ - return cpvlength(((cpPivotJoint *)joint)->jAcc); -} - -static const cpConstraintClass klass = { - (cpConstraintPreStepImpl)preStep, - (cpConstraintApplyCachedImpulseImpl)applyCachedImpulse, - (cpConstraintApplyImpulseImpl)applyImpulse, - (cpConstraintGetImpulseImpl)getImpulse, -}; - -cpPivotJoint * -cpPivotJointAlloc(void) -{ - return (cpPivotJoint *)cpcalloc(1, sizeof(cpPivotJoint)); -} - -cpPivotJoint * -cpPivotJointInit(cpPivotJoint *joint, cpBody *a, cpBody *b, cpVect anchorA, cpVect anchorB) -{ - cpConstraintInit((cpConstraint *)joint, &klass, a, b); - - joint->anchorA = anchorA; - joint->anchorB = anchorB; - - joint->jAcc = cpvzero; - - return joint; -} - -cpConstraint * -cpPivotJointNew2(cpBody *a, cpBody *b, cpVect anchorA, cpVect anchorB) -{ - return (cpConstraint *)cpPivotJointInit(cpPivotJointAlloc(), a, b, anchorA, anchorB); -} - -cpConstraint * -cpPivotJointNew(cpBody *a, cpBody *b, cpVect pivot) -{ - cpVect anchorA = (a ? cpBodyWorldToLocal(a, pivot) : pivot); - cpVect anchorB = (b ? cpBodyWorldToLocal(b, pivot) : pivot); - return cpPivotJointNew2(a, b, anchorA, anchorB); -} - -cpBool -cpConstraintIsPivotJoint(const cpConstraint *constraint) -{ - return (constraint->klass == &klass); -} - -cpVect -cpPivotJointGetAnchorA(const cpConstraint *constraint) -{ - cpAssertHard(cpConstraintIsPivotJoint(constraint), "Constraint is not a pivot joint."); - return ((cpPivotJoint *)constraint)->anchorA; -} - -void -cpPivotJointSetAnchorA(cpConstraint *constraint, cpVect anchorA) -{ - cpAssertHard(cpConstraintIsPivotJoint(constraint), "Constraint is not a pivot joint."); - cpConstraintActivateBodies(constraint); - ((cpPivotJoint *)constraint)->anchorA = anchorA; -} - -cpVect -cpPivotJointGetAnchorB(const cpConstraint *constraint) -{ - cpAssertHard(cpConstraintIsPivotJoint(constraint), "Constraint is not a pivot joint."); - return ((cpPivotJoint *)constraint)->anchorB; -} - -void -cpPivotJointSetAnchorB(cpConstraint *constraint, cpVect anchorB) -{ - cpAssertHard(cpConstraintIsPivotJoint(constraint), "Constraint is not a pivot joint."); - cpConstraintActivateBodies(constraint); - ((cpPivotJoint *)constraint)->anchorB = anchorB; -} diff --git a/3rdparty/chipmunk/src/cpPolyShape.c b/3rdparty/chipmunk/src/cpPolyShape.c deleted file mode 100644 index 554a7f8f313c..000000000000 --- a/3rdparty/chipmunk/src/cpPolyShape.c +++ /dev/null @@ -1,324 +0,0 @@ -/* Copyright (c) 2013 Scott Lembcke and Howling Moon Software - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -#include "chipmunk/chipmunk_private.h" -#include "chipmunk/chipmunk_unsafe.h" - -cpPolyShape * -cpPolyShapeAlloc(void) -{ - return (cpPolyShape *)cpcalloc(1, sizeof(cpPolyShape)); -} - -static void -cpPolyShapeDestroy(cpPolyShape *poly) -{ - if(poly->count > CP_POLY_SHAPE_INLINE_ALLOC){ - cpfree(poly->planes); - } -} - -static cpBB -cpPolyShapeCacheData(cpPolyShape *poly, cpTransform transform) -{ - int count = poly->count; - struct cpSplittingPlane *dst = poly->planes; - struct cpSplittingPlane *src = dst + count; - - cpFloat l = (cpFloat)INFINITY, r = -(cpFloat)INFINITY; - cpFloat b = (cpFloat)INFINITY, t = -(cpFloat)INFINITY; - - for(int i=0; ir; - return (poly->shape.bb = cpBBNew(l - radius, b - radius, r + radius, t + radius)); -} - -static void -cpPolyShapePointQuery(cpPolyShape *poly, cpVect p, cpPointQueryInfo *info){ - int count = poly->count; - struct cpSplittingPlane *planes = poly->planes; - cpFloat r = poly->r; - - cpVect v0 = planes[count - 1].v0; - cpFloat minDist = INFINITY; - cpVect closestPoint = cpvzero; - cpVect closestNormal = cpvzero; - cpBool outside = cpFalse; - - for(int i=0; i 0.0f); - - cpVect closest = cpClosetPointOnSegment(p, v0, v1); - - cpFloat dist = cpvdist(p, closest); - if(dist < minDist){ - minDist = dist; - closestPoint = closest; - closestNormal = planes[i].n; - } - - v0 = v1; - } - - cpFloat dist = (outside ? minDist : -minDist); - cpVect g = cpvmult(cpvsub(p, closestPoint), 1.0f/dist); - - info->shape = (cpShape *)poly; - info->point = cpvadd(closestPoint, cpvmult(g, r)); - info->distance = dist - r; - - // Use the normal of the closest segment if the distance is small. - info->gradient = (minDist > MAGIC_EPSILON ? g : closestNormal); -} - -static void -cpPolyShapeSegmentQuery(cpPolyShape *poly, cpVect a, cpVect b, cpFloat r2, cpSegmentQueryInfo *info) -{ - struct cpSplittingPlane *planes = poly->planes; - int count = poly->count; - cpFloat r = poly->r; - cpFloat rsum = r + r2; - - for(int i=0; ishape = (cpShape *)poly; - info->point = cpvsub(cpvlerp(a, b, t), cpvmult(n, r2)); - info->normal = n; - info->alpha = t; - } - } - - // Also check against the beveled vertexes. - if(rsum > 0.0f){ - for(int i=0; ishape, planes[i].v0, r, a, b, r2, &circle_info); - if(circle_info.alpha < info->alpha) (*info) = circle_info; - } - } -} - -static void -SetVerts(cpPolyShape *poly, int count, const cpVect *verts) -{ - poly->count = count; - if(count <= CP_POLY_SHAPE_INLINE_ALLOC){ - poly->planes = poly->_planes; - } else { - poly->planes = (struct cpSplittingPlane *)cpcalloc(2*count, sizeof(struct cpSplittingPlane)); - } - - for(int i=0; iplanes[i + count].v0 = b; - poly->planes[i + count].n = n; - } -} - -static struct cpShapeMassInfo -cpPolyShapeMassInfo(cpFloat mass, int count, const cpVect *verts, cpFloat radius) -{ - // TODO moment is approximate due to radius. - - cpVect centroid = cpCentroidForPoly(count, verts); - struct cpShapeMassInfo info = { - mass, cpMomentForPoly(1.0f, count, verts, cpvneg(centroid), radius), - centroid, - cpAreaForPoly(count, verts, radius), - }; - - return info; -} - -static const cpShapeClass polyClass = { - CP_POLY_SHAPE, - (cpShapeCacheDataImpl)cpPolyShapeCacheData, - (cpShapeDestroyImpl)cpPolyShapeDestroy, - (cpShapePointQueryImpl)cpPolyShapePointQuery, - (cpShapeSegmentQueryImpl)cpPolyShapeSegmentQuery, -}; - -cpPolyShape * -cpPolyShapeInit(cpPolyShape *poly, cpBody *body, int count, const cpVect *verts, cpTransform transform, cpFloat radius) -{ - cpVect *hullVerts = (cpVect *)alloca(count*sizeof(cpVect)); - - // Transform the verts before building the hull in case of a negative scale. - for(int i=0; ir = radius; - - return poly; -} - -cpShape * -cpPolyShapeNew(cpBody *body, int count, const cpVect *verts, cpTransform transform, cpFloat radius) -{ - return (cpShape *)cpPolyShapeInit(cpPolyShapeAlloc(), body, count, verts, transform, radius); -} - -cpShape * -cpPolyShapeNewRaw(cpBody *body, int count, const cpVect *verts, cpFloat radius) -{ - return (cpShape *)cpPolyShapeInitRaw(cpPolyShapeAlloc(), body, count, verts, radius); -} - -cpPolyShape * -cpBoxShapeInit(cpPolyShape *poly, cpBody *body, cpFloat width, cpFloat height, cpFloat radius) -{ - cpFloat hw = width/2.0f; - cpFloat hh = height/2.0f; - - return cpBoxShapeInit2(poly, body, cpBBNew(-hw, -hh, hw, hh), radius); -} - -cpPolyShape * -cpBoxShapeInit2(cpPolyShape *poly, cpBody *body, cpBB box, cpFloat radius) -{ - cpVect verts[4] = { - cpv(box.r, box.b), - cpv(box.r, box.t), - cpv(box.l, box.t), - cpv(box.l, box.b), - }; - - return cpPolyShapeInitRaw(poly, body, 4, verts, radius); -} - -cpShape * -cpBoxShapeNew(cpBody *body, cpFloat width, cpFloat height, cpFloat radius) -{ - return (cpShape *)cpBoxShapeInit(cpPolyShapeAlloc(), body, width, height, radius); -} - -cpShape * -cpBoxShapeNew2(cpBody *body, cpBB box, cpFloat radius) -{ - return (cpShape *)cpBoxShapeInit2(cpPolyShapeAlloc(), body, box, radius); -} - -int -cpPolyShapeGetCount(const cpShape *shape) -{ - cpAssertHard(shape->klass == &polyClass, "Shape is not a poly shape."); - return ((cpPolyShape *)shape)->count; -} - -cpVect -cpPolyShapeGetVert(const cpShape *shape, int i) -{ - cpAssertHard(shape->klass == &polyClass, "Shape is not a poly shape."); - - int count = cpPolyShapeGetCount(shape); - cpAssertHard(0 <= i && i < count, "Index out of range."); - - return ((cpPolyShape *)shape)->planes[i + count].v0; -} - -cpFloat -cpPolyShapeGetRadius(const cpShape *shape) -{ - cpAssertHard(shape->klass == &polyClass, "Shape is not a poly shape."); - return ((cpPolyShape *)shape)->r; -} - -// Unsafe API (chipmunk_unsafe.h) - -void -cpPolyShapeSetVerts(cpShape *shape, int count, cpVect *verts, cpTransform transform) -{ - cpVect *hullVerts = (cpVect *)alloca(count*sizeof(cpVect)); - - // Transform the verts before building the hull in case of a negative scale. - for(int i=0; iklass == &polyClass, "Shape is not a poly shape."); - cpPolyShape *poly = (cpPolyShape *)shape; - cpPolyShapeDestroy(poly); - - SetVerts(poly, count, verts); - - cpFloat mass = shape->massInfo.m; - shape->massInfo = cpPolyShapeMassInfo(shape->massInfo.m, count, verts, poly->r); - if(mass > 0.0f) cpBodyAccumulateMassFromShapes(shape->body); -} - -void -cpPolyShapeSetRadius(cpShape *shape, cpFloat radius) -{ - cpAssertHard(shape->klass == &polyClass, "Shape is not a poly shape."); - cpPolyShape *poly = (cpPolyShape *)shape; - poly->r = radius; - - - // TODO radius is not handled by moment/area -// cpFloat mass = shape->massInfo.m; -// shape->massInfo = cpPolyShapeMassInfo(shape->massInfo.m, poly->count, poly->verts, poly->r); -// if(mass > 0.0f) cpBodyAccumulateMassFromShapes(shape->body); -} diff --git a/3rdparty/chipmunk/src/cpPolyline.c b/3rdparty/chipmunk/src/cpPolyline.c deleted file mode 100644 index 5b375348d4ea..000000000000 --- a/3rdparty/chipmunk/src/cpPolyline.c +++ /dev/null @@ -1,652 +0,0 @@ -// Copyright 2013 Howling Moon Software. All rights reserved. -// See http://chipmunk2d.net/legal.php for more information. - -#include -#include -#include -#include - -#include "chipmunk/chipmunk_private.h" -#include "chipmunk/cpPolyline.h" - - -static inline int Next(int i, int count){return (i+1)%count;} - -//MARK: Polylines - -#define DEFAULT_POLYLINE_CAPACITY 16 - -static int -cpPolylineSizeForCapacity(int capacity) -{ - return sizeof(cpPolyline) + capacity*sizeof(cpVect); -} - -static cpPolyline * -cpPolylineMake(int capacity) -{ - capacity = (capacity > DEFAULT_POLYLINE_CAPACITY ? capacity : DEFAULT_POLYLINE_CAPACITY); - - cpPolyline *line = (cpPolyline *)cpcalloc(1, cpPolylineSizeForCapacity(capacity)); - line->count = 0; - line->capacity = capacity; - - return line; -} - -static cpPolyline * -cpPolylineMake2(int capacity, cpVect a, cpVect b) -{ - cpPolyline *line = cpPolylineMake(capacity); - line->count = 2; - line->verts[0] = a; - line->verts[1] = b; - - return line; -} - -static cpPolyline * -cpPolylineShrink(cpPolyline *line) -{ - line->capacity = line->count; - return (cpPolyline*) cprealloc(line, cpPolylineSizeForCapacity(line->count)); -} - -void -cpPolylineFree(cpPolyline *line) -{ - cpfree(line); -} - -// Grow the allocated memory for a polyline. -static cpPolyline * -cpPolylineGrow(cpPolyline *line, int count) -{ - line->count += count; - - int capacity = line->capacity; - while(line->count > capacity) capacity *= 2; - - if(line->capacity < capacity){ - line->capacity = capacity; - line = (cpPolyline*) cprealloc(line, cpPolylineSizeForCapacity(capacity)); - } - - return line; -} - -// Push v onto the end of line. -static cpPolyline * -cpPolylinePush(cpPolyline *line, cpVect v) -{ - int count = line->count; - line = cpPolylineGrow(line, 1); - line->verts[count] = v; - - return line; -} - -// Push v onto the beginning of line. -static cpPolyline * -cpPolylineEnqueue(cpPolyline *line, cpVect v) -{ - // TODO could optimize this to grow in both directions. - // Probably doesn't matter though. - int count = line->count; - line = cpPolylineGrow(line, 1); - memmove(line->verts + 1, line->verts, count*sizeof(cpVect)); - line->verts[0] = v; - - return line; -} - -// Returns true if the polyline starts and ends with the same vertex. -cpBool -cpPolylineIsClosed(cpPolyline *line) -{ - return (line->count > 1 && cpveql(line->verts[0], line->verts[line->count-1])); -} - -// Check if a cpPolyline is longer than a certain length -// Takes a range which can wrap around if the polyline is looped. -static cpBool -cpPolylineIsShort(cpVect *points, int count, int start, int end, cpFloat min) -{ - cpFloat length = 0.0f; - for(int i=start; i!=end; i=Next(i, count)){ - length += cpvdist(points[i], points[Next(i, count)]); - if(length > min) return cpFalse; - } - - return cpTrue; -} - -//MARK: Polyline Simplification - -static inline cpFloat -Sharpness(cpVect a, cpVect b, cpVect c) -{ - // TODO could speed this up by caching the normals instead of calculating each twice. - return cpvdot(cpvnormalize(cpvsub(a, b)), cpvnormalize(cpvsub(c, b))); -} - -// Join similar adjacent line segments together. Works well for hard edged shapes. -// 'tol' is the minimum anglular difference in radians of a vertex. -cpPolyline * -cpPolylineSimplifyVertexes(cpPolyline *line, cpFloat tol) -{ - cpPolyline *reduced = cpPolylineMake2(0, line->verts[0], line->verts[1]); - - cpFloat minSharp = -cpfcos(tol); - - for(int i=2; icount; i++){ - cpVect vert = line->verts[i]; - cpFloat sharp = Sharpness(reduced->verts[reduced->count - 2], reduced->verts[reduced->count - 1], vert); - - if(sharp <= minSharp){ - reduced->verts[reduced->count - 1] = vert; - } else { - reduced = cpPolylinePush(reduced, vert); - } - } - - if( - cpPolylineIsClosed(line) && - Sharpness(reduced->verts[reduced->count - 2], reduced->verts[0], reduced->verts[1]) < minSharp - ){ - reduced->verts[0] = reduced->verts[reduced->count - 2]; - reduced->count--; - } - - // TODO shrink - return reduced; -} - -// Recursive function used by cpPolylineSimplifyCurves(). -static cpPolyline * -DouglasPeucker( - cpVect *verts, cpPolyline *reduced, - int length, int start, int end, - cpFloat min, cpFloat tol -){ - // Early exit if the points are adjacent - if((end - start + length)%length < 2) return reduced; - - cpVect a = verts[start]; - cpVect b = verts[end]; - - // Check if the length is below the threshold - if(cpvnear(a, b, min) && cpPolylineIsShort(verts, length, start, end, min)) return reduced; - - // Find the maximal vertex to split and recurse on - cpFloat max = 0.0; - int maxi = start; - - cpVect n = cpvnormalize(cpvperp(cpvsub(b, a))); - cpFloat d = cpvdot(n, a); - - for(int i=Next(start, length); i!=end; i=Next(i, length)){ - cpFloat dist = fabs(cpvdot(n, verts[i]) - d); - - if(dist > max){ - max = dist; - maxi = i; - } - } - - if(max > tol){ - reduced = DouglasPeucker(verts, reduced, length, start, maxi, min, tol); - reduced = cpPolylinePush(reduced, verts[maxi]); - reduced = DouglasPeucker(verts, reduced, length, maxi, end, min, tol); - } - - return reduced; -} - -// Recursively reduce the vertex count on a polyline. Works best for smooth shapes. -// 'tol' is the maximum error for the reduction. -// The reduced polyline will never be farther than this distance from the original polyline. -cpPolyline * -cpPolylineSimplifyCurves(cpPolyline *line, cpFloat tol) -{ - cpPolyline *reduced = cpPolylineMake(line->count); - - cpFloat min = tol/2.0f; - - if(cpPolylineIsClosed(line)){ - int start, end; - cpLoopIndexes(line->verts, line->count - 1, &start, &end); - - reduced = cpPolylinePush(reduced, line->verts[start]); - reduced = DouglasPeucker(line->verts, reduced, line->count - 1, start, end, min, tol); - reduced = cpPolylinePush(reduced, line->verts[end]); - reduced = DouglasPeucker(line->verts, reduced, line->count - 1, end, start, min, tol); - reduced = cpPolylinePush(reduced, line->verts[start]); - } else { - reduced = cpPolylinePush(reduced, line->verts[0]); - reduced = DouglasPeucker(line->verts, reduced, line->count, 0, line->count - 1, min, tol); - reduced = cpPolylinePush(reduced, line->verts[line->count - 1]); - } - - return cpPolylineShrink(reduced); -} - -//MARK: Polyline Sets - -cpPolylineSet * -cpPolylineSetAlloc(void) -{ - return (cpPolylineSet *)cpcalloc(1, sizeof(cpPolylineSet)); -} - -cpPolylineSet * -cpPolylineSetInit(cpPolylineSet *set) -{ - set->count = 0; - set->capacity = 8; - set->lines = (cpPolyline**) cpcalloc(set->capacity, sizeof(cpPolyline)); - - return set; -} - - -cpPolylineSet * -cpPolylineSetNew(void) -{ - return cpPolylineSetInit(cpPolylineSetAlloc()); -} - -void -cpPolylineSetDestroy(cpPolylineSet *set, cpBool freePolylines) -{ - if(freePolylines){ - for(int i=0; icount; i++){ - cpPolylineFree(set->lines[i]); - } - } - - cpfree(set->lines); -} - - -void -cpPolylineSetFree(cpPolylineSet *set, cpBool freePolylines) -{ - if(set){ - cpPolylineSetDestroy(set, freePolylines); - cpfree(set); - } -} - -// Find the polyline that ends with v. -static int -cpPolylineSetFindEnds(cpPolylineSet *set, cpVect v){ - int count = set->count; - cpPolyline **lines = set->lines; - - for(int i=0; iverts[line->count - 1], v)) return i; - } - - return -1; -} - -// Find the polyline that starts with v. -static int -cpPolylineSetFindStarts(cpPolylineSet *set, cpVect v){ - int count = set->count; - cpPolyline **lines = set->lines; - - for(int i=0; iverts[0], v)) return i; - } - - return -1; -} - -// Add a new polyline to a polyline set. -static void -cpPolylineSetPush(cpPolylineSet *set, cpPolyline *line) -{ - // grow set - set->count++; - if(set->count > set->capacity){ - set->capacity *= 2; - set->lines = (cpPolyline**) cprealloc(set->lines, set->capacity*sizeof(cpPolyline)); - } - - set->lines[set->count - 1] = line; -} - -// Add a new polyline to a polyline set. -static void -cpPolylineSetAdd(cpPolylineSet *set, cpVect v0, cpVect v1) -{ - cpPolylineSetPush(set, cpPolylineMake2(DEFAULT_POLYLINE_CAPACITY, v0, v1)); -} - -// Join two cpPolylines in a polyline set together. -static void -cpPolylineSetJoin(cpPolylineSet *set, int before, int after) -{ - cpPolyline *lbefore = set->lines[before]; - cpPolyline *lafter = set->lines[after]; - - // append - int count = lbefore->count; - lbefore = cpPolylineGrow(lbefore, lafter->count); - memmove(lbefore->verts + count, lafter->verts, lafter->count*sizeof(cpVect)); - set->lines[before] = lbefore; - - // delete lafter - set->count--; - cpPolylineFree(set->lines[after]); - set->lines[after] = set->lines[set->count]; -} - -// Add a segment to a polyline set. -// A segment will either start a new polyline, join two others, or add to or loop an existing polyline. -void -cpPolylineSetCollectSegment(cpVect v0, cpVect v1, cpPolylineSet *lines) -{ - int before = cpPolylineSetFindEnds(lines, v0); - int after = cpPolylineSetFindStarts(lines, v1); - - if(before >= 0 && after >= 0){ - if(before == after){ - // loop by pushing v1 onto before - lines->lines[before] = cpPolylinePush(lines->lines[before], v1); - } else { - // join before and after - cpPolylineSetJoin(lines, before, after); - } - } else if(before >= 0){ - // push v1 onto before - lines->lines[before] = cpPolylinePush(lines->lines[before], v1); - } else if(after >= 0){ - // enqueue v0 onto after - lines->lines[after] = cpPolylineEnqueue(lines->lines[after], v0); - } else { - // create new line from v0 and v1 - cpPolylineSetAdd(lines, v0, v1); - } -} - -//MARK: Convex Hull Functions - -cpPolyline * -cpPolylineToConvexHull(cpPolyline *line, cpFloat tol) -{ - cpPolyline *hull = cpPolylineMake(line->count + 1); - hull->count = cpConvexHull(line->count, line->verts, hull->verts, NULL, tol); - hull = cpPolylinePush(hull, hull->verts[0]); - - return cpPolylineShrink(hull); -} - -//MARK: Approximate Concave Decompostition - -struct Notch { - int i; - cpFloat d; - cpVect v; - cpVect n; -}; - -static cpFloat -FindSteiner(int count, cpVect *verts, struct Notch notch) -{ - cpFloat min = INFINITY; - cpFloat feature = -1.0; - - for(int i=1; i= 0.0 && dist <= min){ - min = dist; - feature = index + t; - } - } - } - - return feature; -} - -//static cpFloat -//FindSteiner2(cpVect *verts, int count, struct Notch notch) -//{ -// cpVect a = verts[(notch.i + count - 1)%count]; -// cpVect b = verts[(notch.i + 1)%count]; -// cpVect n = cpvnormalize(cpvadd(cpvnormalize(cpvsub(notch.v, a)), cpvnormalize(cpvsub(notch.v, b)))); -// -// cpFloat min = INFINITY; -// cpFloat feature = -1.0; -// -// for(int i=1; i= 0.0 && dist <= min){ -// min = dist; -// feature = index + t; -// } -// } -// } -// -// cpAssertSoft(feature >= 0.0, "No closest features detected. This is likely due to a self intersecting polygon."); -// return feature; -//} - -//struct Range {cpFloat min, max;}; -//static inline struct Range -//clip_range(cpVect delta_a, cpVect delta_b, cpVect clip) -//{ -// cpFloat da = cpvcross(delta_a, clip); -// cpFloat db = cpvcross(delta_b, clip); -// cpFloat clamp = da/(da - db); -// if(da > db){ -// return (struct Range){-INFINITY, clamp}; -// } else if(da < db){ -// return (struct Range){clamp, INFINITY}; -// } else { -// return (struct Range){-INFINITY, INFINITY}; -// } -//} -// -//static cpFloat -//FindSteiner3(cpVect *verts, int count, struct Notch notch) -//{ -// cpFloat min = INFINITY; -// cpFloat feature = -1.0; -// -// cpVect support_a = verts[(notch.i - 1 + count)%count]; -// cpVect support_b = verts[(notch.i + 1)%count]; -// -// cpVect clip_a = cpvlerp(support_a, support_b, 0.1); -// cpVect clip_b = cpvlerp(support_b, support_b, 0.9); -// -// for(int i=1; i 0.0){ -// struct Range range1 = clip_range(delta_a, delta_b, cpvsub(notch.v, clip_a)); -// struct Range range2 = clip_range(delta_a, delta_b, cpvsub(clip_b, notch.v)); -// -// cpFloat min_t = cpfmax(0.0, cpfmax(range1.min, range2.min)); -// cpFloat max_t = cpfmin(1.0, cpfmin(range1.max, range2.max)); -// -// // Ignore if the segment has been completely clipped away. -// if(min_t < max_t){ -// cpVect seg_delta = cpvsub(seg_b, seg_a); -// cpFloat closest_t = cpfclamp(cpvdot(seg_delta, cpvsub(notch.v, seg_a))/cpvlengthsq(seg_delta), min_t, max_t); -// cpVect closest = cpvlerp(seg_a, seg_b, closest_t); -// -// cpFloat dist = cpvdistsq(notch.v, closest); -// if(dist < min){ -// min = dist; -// feature = index + closest_t; -// } -// } -// } -// } -// -// cpAssertWarn(feature >= 0.0, "Internal Error: No closest features detected."); -// return feature; -//} - -//static cpBool -//VertexUnobscured(int count, cpVect *verts, int index, int notch_i) -//{ -// cpVect v = verts[notch_i]; -// cpVect n = cpvnormalize(cpvsub(verts[index], v)); -// -// for(int i=0; i= 0.0, "No closest features detected. This is likely due to a self intersecting polygon."); -// return feature; -//} - -static struct Notch -DeepestNotch(int count, cpVect *verts, int hullCount, cpVect *hullVerts, int first, cpFloat tol) -{ - struct Notch notch = {}; - int j = Next(first, count); - - for(int i=0; i notch.d){ - notch.d = depth; - notch.i = j; - notch.v = v; - notch.n = n; - } - - j = Next(j, count); - v = verts[j]; - } - - j = Next(j, count); - } - - return notch; -} - -static inline int IMAX(int a, int b){return (a > b ? a : b);} - -static void -ApproximateConcaveDecomposition(cpVect *verts, int count, cpFloat tol, cpPolylineSet *set) -{ - int first; - cpVect *hullVerts = (cpVect*) alloca(count*sizeof(cpVect)); - int hullCount = cpConvexHull(count, verts, hullVerts, &first, 0.0); - - if(hullCount != count){ - struct Notch notch = DeepestNotch(count, verts, hullCount, hullVerts, first, tol); - - if(notch.d > tol){ - cpFloat steiner_it = FindSteiner(count, verts, notch); - - if(steiner_it >= 0.0){ - int steiner_i = (int)steiner_it; - cpVect steiner = cpvlerp(verts[steiner_i], verts[Next(steiner_i, count)], steiner_it - steiner_i); - - // Vertex counts NOT including the steiner point. - int sub1_count = (steiner_i - notch.i + count)%count + 1; - int sub2_count = count - (steiner_i - notch.i + count)%count; - cpVect *scratch = (cpVect*) alloca((IMAX(sub1_count, sub2_count) + 1)*sizeof(cpVect)); - - for(int i=0; iverts, hullVerts, hullCount*sizeof(cpVect)); - hull->verts[hullCount] = hullVerts[0]; - hull->count = hullCount + 1; - - cpPolylineSetPush(set, hull); -} - -cpPolylineSet * -cpPolylineConvexDecomposition_BETA(cpPolyline *line, cpFloat tol) -{ - cpAssertSoft(cpPolylineIsClosed(line), "Cannot decompose an open polygon."); - cpAssertSoft(cpAreaForPoly(line->count, line->verts, 0.0) >= 0.0, "Winding is backwards. (Are you passing a hole?)"); - - cpPolylineSet *set = cpPolylineSetNew(); - ApproximateConcaveDecomposition(line->verts, line->count - 1, tol, set); - - return set; -} diff --git a/3rdparty/chipmunk/src/cpRatchetJoint.c b/3rdparty/chipmunk/src/cpRatchetJoint.c deleted file mode 100644 index b3c9687e3562..000000000000 --- a/3rdparty/chipmunk/src/cpRatchetJoint.c +++ /dev/null @@ -1,179 +0,0 @@ -/* Copyright (c) 2013 Scott Lembcke and Howling Moon Software - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -#include "chipmunk/chipmunk_private.h" - -static void -preStep(cpRatchetJoint *joint, cpFloat dt) -{ - cpBody *a = joint->constraint.a; - cpBody *b = joint->constraint.b; - - cpFloat angle = joint->angle; - cpFloat phase = joint->phase; - cpFloat ratchet = joint->ratchet; - - cpFloat delta = b->a - a->a; - cpFloat diff = angle - delta; - cpFloat pdist = 0.0f; - - if(diff*ratchet > 0.0f){ - pdist = diff; - } else { - joint->angle = cpffloor((delta - phase)/ratchet)*ratchet + phase; - } - - // calculate moment of inertia coefficient. - joint->iSum = 1.0f/(a->i_inv + b->i_inv); - - // calculate bias velocity - cpFloat maxBias = joint->constraint.maxBias; - joint->bias = cpfclamp(-bias_coef(joint->constraint.errorBias, dt)*pdist/dt, -maxBias, maxBias); - - // If the bias is 0, the joint is not at a limit. Reset the impulse. - if(!joint->bias) joint->jAcc = 0.0f; -} - -static void -applyCachedImpulse(cpRatchetJoint *joint, cpFloat dt_coef) -{ - cpBody *a = joint->constraint.a; - cpBody *b = joint->constraint.b; - - cpFloat j = joint->jAcc*dt_coef; - a->w -= j*a->i_inv; - b->w += j*b->i_inv; -} - -static void -applyImpulse(cpRatchetJoint *joint, cpFloat dt) -{ - if(!joint->bias) return; // early exit - - cpBody *a = joint->constraint.a; - cpBody *b = joint->constraint.b; - - // compute relative rotational velocity - cpFloat wr = b->w - a->w; - cpFloat ratchet = joint->ratchet; - - cpFloat jMax = joint->constraint.maxForce*dt; - - // compute normal impulse - cpFloat j = -(joint->bias + wr)*joint->iSum; - cpFloat jOld = joint->jAcc; - joint->jAcc = cpfclamp((jOld + j)*ratchet, 0.0f, jMax*cpfabs(ratchet))/ratchet; - j = joint->jAcc - jOld; - - // apply impulse - a->w -= j*a->i_inv; - b->w += j*b->i_inv; -} - -static cpFloat -getImpulse(cpRatchetJoint *joint) -{ - return cpfabs(joint->jAcc); -} - -static const cpConstraintClass klass = { - (cpConstraintPreStepImpl)preStep, - (cpConstraintApplyCachedImpulseImpl)applyCachedImpulse, - (cpConstraintApplyImpulseImpl)applyImpulse, - (cpConstraintGetImpulseImpl)getImpulse, -}; - -cpRatchetJoint * -cpRatchetJointAlloc(void) -{ - return (cpRatchetJoint *)cpcalloc(1, sizeof(cpRatchetJoint)); -} - -cpRatchetJoint * -cpRatchetJointInit(cpRatchetJoint *joint, cpBody *a, cpBody *b, cpFloat phase, cpFloat ratchet) -{ - cpConstraintInit((cpConstraint *)joint, &klass, a, b); - - joint->angle = 0.0f; - joint->phase = phase; - joint->ratchet = ratchet; - - // STATIC_BODY_CHECK - joint->angle = (b ? b->a : 0.0f) - (a ? a->a : 0.0f); - - return joint; -} - -cpConstraint * -cpRatchetJointNew(cpBody *a, cpBody *b, cpFloat phase, cpFloat ratchet) -{ - return (cpConstraint *)cpRatchetJointInit(cpRatchetJointAlloc(), a, b, phase, ratchet); -} - -cpBool -cpConstraintIsRatchetJoint(const cpConstraint *constraint) -{ - return (constraint->klass == &klass); -} - -cpFloat -cpRatchetJointGetAngle(const cpConstraint *constraint) -{ - cpAssertHard(cpConstraintIsRatchetJoint(constraint), "Constraint is not a ratchet joint."); - return ((cpRatchetJoint *)constraint)->angle; -} - -void -cpRatchetJointSetAngle(cpConstraint *constraint, cpFloat angle) -{ - cpAssertHard(cpConstraintIsRatchetJoint(constraint), "Constraint is not a ratchet joint."); - cpConstraintActivateBodies(constraint); - ((cpRatchetJoint *)constraint)->angle = angle; -} - -cpFloat -cpRatchetJointGetPhase(const cpConstraint *constraint) -{ - cpAssertHard(cpConstraintIsRatchetJoint(constraint), "Constraint is not a ratchet joint."); - return ((cpRatchetJoint *)constraint)->phase; -} - -void -cpRatchetJointSetPhase(cpConstraint *constraint, cpFloat phase) -{ - cpAssertHard(cpConstraintIsRatchetJoint(constraint), "Constraint is not a ratchet joint."); - cpConstraintActivateBodies(constraint); - ((cpRatchetJoint *)constraint)->phase = phase; -} -cpFloat -cpRatchetJointGetRatchet(const cpConstraint *constraint) -{ - cpAssertHard(cpConstraintIsRatchetJoint(constraint), "Constraint is not a ratchet joint."); - return ((cpRatchetJoint *)constraint)->ratchet; -} - -void -cpRatchetJointSetRatchet(cpConstraint *constraint, cpFloat ratchet) -{ - cpAssertHard(cpConstraintIsRatchetJoint(constraint), "Constraint is not a ratchet joint."); - cpConstraintActivateBodies(constraint); - ((cpRatchetJoint *)constraint)->ratchet = ratchet; -} diff --git a/3rdparty/chipmunk/src/cpRobust.c b/3rdparty/chipmunk/src/cpRobust.c deleted file mode 100644 index 57507d14e6b4..000000000000 --- a/3rdparty/chipmunk/src/cpRobust.c +++ /dev/null @@ -1,13 +0,0 @@ -#include "chipmunk/cpRobust.h" - - -cpBool -cpCheckPointGreater(const cpVect a, const cpVect b, const cpVect c) -{ - return (b.y - a.y)*(a.x + b.x - 2*c.x) > (b.x - a.x)*(a.y + b.y - 2*c.y); -} - -cpBool -cpCheckAxis(cpVect v0, cpVect v1, cpVect p, cpVect n){ - return cpvdot(p, n) <= cpfmax(cpvdot(v0, n), cpvdot(v1, n)); -} diff --git a/3rdparty/chipmunk/src/cpRotaryLimitJoint.c b/3rdparty/chipmunk/src/cpRotaryLimitJoint.c deleted file mode 100644 index 548adbebf053..000000000000 --- a/3rdparty/chipmunk/src/cpRotaryLimitJoint.c +++ /dev/null @@ -1,160 +0,0 @@ -/* Copyright (c) 2013 Scott Lembcke and Howling Moon Software - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -#include "chipmunk/chipmunk_private.h" - -static void -preStep(cpRotaryLimitJoint *joint, cpFloat dt) -{ - cpBody *a = joint->constraint.a; - cpBody *b = joint->constraint.b; - - cpFloat dist = b->a - a->a; - cpFloat pdist = 0.0f; - if(dist > joint->max) { - pdist = joint->max - dist; - } else if(dist < joint->min) { - pdist = joint->min - dist; - } - - // calculate moment of inertia coefficient. - joint->iSum = 1.0f/(a->i_inv + b->i_inv); - - // calculate bias velocity - cpFloat maxBias = joint->constraint.maxBias; - joint->bias = cpfclamp(-bias_coef(joint->constraint.errorBias, dt)*pdist/dt, -maxBias, maxBias); - - // If the bias is 0, the joint is not at a limit. Reset the impulse. - if(!joint->bias) joint->jAcc = 0.0f; -} - -static void -applyCachedImpulse(cpRotaryLimitJoint *joint, cpFloat dt_coef) -{ - cpBody *a = joint->constraint.a; - cpBody *b = joint->constraint.b; - - cpFloat j = joint->jAcc*dt_coef; - a->w -= j*a->i_inv; - b->w += j*b->i_inv; -} - -static void -applyImpulse(cpRotaryLimitJoint *joint, cpFloat dt) -{ - if(!joint->bias) return; // early exit - - cpBody *a = joint->constraint.a; - cpBody *b = joint->constraint.b; - - // compute relative rotational velocity - cpFloat wr = b->w - a->w; - - cpFloat jMax = joint->constraint.maxForce*dt; - - // compute normal impulse - cpFloat j = -(joint->bias + wr)*joint->iSum; - cpFloat jOld = joint->jAcc; - if(joint->bias < 0.0f){ - joint->jAcc = cpfclamp(jOld + j, 0.0f, jMax); - } else { - joint->jAcc = cpfclamp(jOld + j, -jMax, 0.0f); - } - j = joint->jAcc - jOld; - - // apply impulse - a->w -= j*a->i_inv; - b->w += j*b->i_inv; -} - -static cpFloat -getImpulse(cpRotaryLimitJoint *joint) -{ - return cpfabs(joint->jAcc); -} - -static const cpConstraintClass klass = { - (cpConstraintPreStepImpl)preStep, - (cpConstraintApplyCachedImpulseImpl)applyCachedImpulse, - (cpConstraintApplyImpulseImpl)applyImpulse, - (cpConstraintGetImpulseImpl)getImpulse, -}; - -cpRotaryLimitJoint * -cpRotaryLimitJointAlloc(void) -{ - return (cpRotaryLimitJoint *)cpcalloc(1, sizeof(cpRotaryLimitJoint)); -} - -cpRotaryLimitJoint * -cpRotaryLimitJointInit(cpRotaryLimitJoint *joint, cpBody *a, cpBody *b, cpFloat min, cpFloat max) -{ - cpConstraintInit((cpConstraint *)joint, &klass, a, b); - - joint->min = min; - joint->max = max; - - joint->jAcc = 0.0f; - - return joint; -} - -cpConstraint * -cpRotaryLimitJointNew(cpBody *a, cpBody *b, cpFloat min, cpFloat max) -{ - return (cpConstraint *)cpRotaryLimitJointInit(cpRotaryLimitJointAlloc(), a, b, min, max); -} - -cpBool -cpConstraintIsRotaryLimitJoint(const cpConstraint *constraint) -{ - return (constraint->klass == &klass); -} - -cpFloat -cpRotaryLimitJointGetMin(const cpConstraint *constraint) -{ - cpAssertHard(cpConstraintIsRotaryLimitJoint(constraint), "Constraint is not a rotary limit joint."); - return ((cpRotaryLimitJoint *)constraint)->min; -} - -void -cpRotaryLimitJointSetMin(cpConstraint *constraint, cpFloat min) -{ - cpAssertHard(cpConstraintIsRotaryLimitJoint(constraint), "Constraint is not a rotary limit joint."); - cpConstraintActivateBodies(constraint); - ((cpRotaryLimitJoint *)constraint)->min = min; -} - -cpFloat -cpRotaryLimitJointGetMax(const cpConstraint *constraint) -{ - cpAssertHard(cpConstraintIsRotaryLimitJoint(constraint), "Constraint is not a rotary limit joint."); - return ((cpRotaryLimitJoint *)constraint)->max; -} - -void -cpRotaryLimitJointSetMax(cpConstraint *constraint, cpFloat max) -{ - cpAssertHard(cpConstraintIsRotaryLimitJoint(constraint), "Constraint is not a rotary limit joint."); - cpConstraintActivateBodies(constraint); - ((cpRotaryLimitJoint *)constraint)->max = max; -} diff --git a/3rdparty/chipmunk/src/cpShape.c b/3rdparty/chipmunk/src/cpShape.c deleted file mode 100644 index 513b5353ed48..000000000000 --- a/3rdparty/chipmunk/src/cpShape.c +++ /dev/null @@ -1,604 +0,0 @@ -/* Copyright (c) 2013 Scott Lembcke and Howling Moon Software - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -#include "chipmunk/chipmunk_private.h" -#include "chipmunk/chipmunk_unsafe.h" - -#define CP_DefineShapeGetter(struct, type, member, name) \ -CP_DeclareShapeGetter(struct, type, name){ \ - cpAssertHard(shape->klass == &struct##Class, "shape is not a "#struct); \ - return ((struct *)shape)->member; \ -} - -cpShape * -cpShapeInit(cpShape *shape, const cpShapeClass *klass, cpBody *body, struct cpShapeMassInfo massInfo) -{ - shape->klass = klass; - - shape->body = body; - shape->massInfo = massInfo; - - shape->sensor = 0; - - shape->e = 0.0f; - shape->u = 0.0f; - shape->surfaceV = cpvzero; - - shape->type = 0; - shape->filter.group = CP_NO_GROUP; - shape->filter.categories = CP_ALL_CATEGORIES; - shape->filter.mask = CP_ALL_CATEGORIES; - - shape->userData = NULL; - - shape->space = NULL; - - shape->next = NULL; - shape->prev = NULL; - - return shape; -} - -void -cpShapeDestroy(cpShape *shape) -{ - if(shape->klass && shape->klass->destroy) shape->klass->destroy(shape); -} - -void -cpShapeFree(cpShape *shape) -{ - if(shape){ - cpShapeDestroy(shape); - cpfree(shape); - } -} - -cpSpace * -cpShapeGetSpace(const cpShape *shape) -{ - return shape->space; -} - -cpBody * -cpShapeGetBody(const cpShape *shape) -{ - return shape->body; -} - -void -cpShapeSetBody(cpShape *shape, cpBody *body) -{ - cpAssertHard(!cpShapeActive(shape), "You cannot change the body on an active shape. You must remove the shape from the space before changing the body."); - shape->body = body; -} - -cpFloat cpShapeGetMass(cpShape *shape){ return shape->massInfo.m; } - -void -cpShapeSetMass(cpShape *shape, cpFloat mass){ - cpBody *body = shape->body; - cpBodyActivate(body); - - shape->massInfo.m = mass; - cpBodyAccumulateMassFromShapes(body); -} - -cpFloat cpShapeGetDensity(cpShape *shape){ return shape->massInfo.m/shape->massInfo.area; } -void cpShapeSetDensity(cpShape *shape, cpFloat density){ cpShapeSetMass(shape, density*shape->massInfo.area); } - -cpFloat cpShapeGetMoment(cpShape *shape){ return shape->massInfo.m*shape->massInfo.i; } -cpFloat cpShapeGetArea(cpShape *shape){ return shape->massInfo.area; } -cpVect cpShapeGetCenterOfGravity(cpShape *shape) { return shape->massInfo.cog; } - -cpBB -cpShapeGetBB(const cpShape *shape) -{ - return shape->bb; -} - -cpBool -cpShapeGetSensor(const cpShape *shape) -{ - return shape->sensor; -} - -void -cpShapeSetSensor(cpShape *shape, cpBool sensor) -{ - cpBodyActivate(shape->body); - shape->sensor = sensor; -} - -cpFloat -cpShapeGetElasticity(const cpShape *shape) -{ - return shape->e; -} - -void -cpShapeSetElasticity(cpShape *shape, cpFloat elasticity) -{ - cpAssertHard(elasticity >= 0.0f, "Elasticity must be positive."); - cpBodyActivate(shape->body); - shape->e = elasticity; -} - -cpFloat -cpShapeGetFriction(const cpShape *shape) -{ - return shape->u; -} - -void -cpShapeSetFriction(cpShape *shape, cpFloat friction) -{ - cpAssertHard(friction >= 0.0f, "Friction must be postive."); - cpBodyActivate(shape->body); - shape->u = friction; -} - -cpVect -cpShapeGetSurfaceVelocity(const cpShape *shape) -{ - return shape->surfaceV; -} - -void -cpShapeSetSurfaceVelocity(cpShape *shape, cpVect surfaceVelocity) -{ - cpBodyActivate(shape->body); - shape->surfaceV = surfaceVelocity; -} - -cpDataPointer -cpShapeGetUserData(const cpShape *shape) -{ - return shape->userData; -} - -void -cpShapeSetUserData(cpShape *shape, cpDataPointer userData) -{ - shape->userData = userData; -} - -cpCollisionType -cpShapeGetCollisionType(const cpShape *shape) -{ - return shape->type; -} - -void -cpShapeSetCollisionType(cpShape *shape, cpCollisionType collisionType) -{ - cpBodyActivate(shape->body); - shape->type = collisionType; -} - -cpShapeFilter -cpShapeGetFilter(const cpShape *shape) -{ - return shape->filter; -} - -void -cpShapeSetFilter(cpShape *shape, cpShapeFilter filter) -{ - cpBodyActivate(shape->body); - shape->filter = filter; -} - -cpBB -cpShapeCacheBB(cpShape *shape) -{ - return cpShapeUpdate(shape, shape->body->transform); -} - -cpBB -cpShapeUpdate(cpShape *shape, cpTransform transform) -{ - return (shape->bb = shape->klass->cacheData(shape, transform)); -} - -cpFloat -cpShapePointQuery(const cpShape *shape, cpVect p, cpPointQueryInfo *info) -{ - cpPointQueryInfo blank = {NULL, cpvzero, INFINITY, cpvzero}; - if(info){ - (*info) = blank; - } else { - info = ␣ - } - - shape->klass->pointQuery(shape, p, info); - return info->distance; -} - - -cpBool -cpShapeSegmentQuery(const cpShape *shape, cpVect a, cpVect b, cpFloat radius, cpSegmentQueryInfo *info){ - cpSegmentQueryInfo blank = {NULL, b, cpvzero, 1.0f}; - if(info){ - (*info) = blank; - } else { - info = ␣ - } - - cpPointQueryInfo nearest; - shape->klass->pointQuery(shape, a, &nearest); - if(nearest.distance <= radius){ - info->shape = shape; - info->alpha = 0.0; - info->normal = cpvnormalize(cpvsub(a, nearest.point)); - } else { - shape->klass->segmentQuery(shape, a, b, radius, info); - } - - return (info->shape != NULL); -} - -cpContactPointSet -cpShapesCollide(const cpShape *a, const cpShape *b) -{ - struct cpContact contacts[CP_MAX_CONTACTS_PER_ARBITER]; - struct cpCollisionInfo info = cpCollide(a, b, 0, contacts); - - cpContactPointSet set; - set.count = info.count; - - // cpCollideShapes() may have swapped the contact order. Flip the normal. - cpBool swapped = (a != info.a); - set.normal = (swapped ? cpvneg(info.n) : info.n); - - for(int i=0; itc = cpTransformPoint(transform, circle->c); - return cpBBNewForCircle(c, circle->r); -} - -static void -cpCircleShapePointQuery(cpCircleShape *circle, cpVect p, cpPointQueryInfo *info) -{ - cpVect delta = cpvsub(p, circle->tc); - cpFloat d = cpvlength(delta); - cpFloat r = circle->r; - - info->shape = (cpShape *)circle; - cpFloat r_over_d = d > 0.0f ? r/d : r; - info->point = cpvadd(circle->tc, cpvmult(delta, r_over_d)); // TODO: div/0 - info->distance = d - r; - - // Use up for the gradient if the distance is very small. - info->gradient = (d > MAGIC_EPSILON ? cpvmult(delta, 1.0f/d) : cpv(0.0f, 1.0f)); -} - -static void -cpCircleShapeSegmentQuery(cpCircleShape *circle, cpVect a, cpVect b, cpFloat radius, cpSegmentQueryInfo *info) -{ - CircleSegmentQuery((cpShape *)circle, circle->tc, circle->r, a, b, radius, info); -} - -static struct cpShapeMassInfo -cpCircleShapeMassInfo(cpFloat mass, cpFloat radius, cpVect center) -{ - struct cpShapeMassInfo info = { - mass, cpMomentForCircle(1.0f, 0.0f, radius, cpvzero), - center, - cpAreaForCircle(0.0f, radius), - }; - - return info; -} - -static const cpShapeClass cpCircleShapeClass = { - CP_CIRCLE_SHAPE, - (cpShapeCacheDataImpl)cpCircleShapeCacheData, - NULL, - (cpShapePointQueryImpl)cpCircleShapePointQuery, - (cpShapeSegmentQueryImpl)cpCircleShapeSegmentQuery, -}; - -cpCircleShape * -cpCircleShapeInit(cpCircleShape *circle, cpBody *body, cpFloat radius, cpVect offset) -{ - circle->c = offset; - circle->r = radius; - - cpShapeInit((cpShape *)circle, &cpCircleShapeClass, body, cpCircleShapeMassInfo(0.0f, radius, offset)); - - return circle; -} - -cpShape * -cpCircleShapeNew(cpBody *body, cpFloat radius, cpVect offset) -{ - return (cpShape *)cpCircleShapeInit(cpCircleShapeAlloc(), body, radius, offset); -} - -cpVect -cpCircleShapeGetOffset(const cpShape *shape) -{ - cpAssertHard(shape->klass == &cpCircleShapeClass, "Shape is not a circle shape."); - return ((cpCircleShape *)shape)->c; -} - -cpFloat -cpCircleShapeGetRadius(const cpShape *shape) -{ - cpAssertHard(shape->klass == &cpCircleShapeClass, "Shape is not a circle shape."); - return ((cpCircleShape *)shape)->r; -} - - -cpSegmentShape * -cpSegmentShapeAlloc(void) -{ - return (cpSegmentShape *)cpcalloc(1, sizeof(cpSegmentShape)); -} - -static cpBB -cpSegmentShapeCacheData(cpSegmentShape *seg, cpTransform transform) -{ - seg->ta = cpTransformPoint(transform, seg->a); - seg->tb = cpTransformPoint(transform, seg->b); - seg->tn = cpTransformVect(transform, seg->n); - - cpFloat l,r,b,t; - - if(seg->ta.x < seg->tb.x){ - l = seg->ta.x; - r = seg->tb.x; - } else { - l = seg->tb.x; - r = seg->ta.x; - } - - if(seg->ta.y < seg->tb.y){ - b = seg->ta.y; - t = seg->tb.y; - } else { - b = seg->tb.y; - t = seg->ta.y; - } - - cpFloat rad = seg->r; - return cpBBNew(l - rad, b - rad, r + rad, t + rad); -} - -static void -cpSegmentShapePointQuery(cpSegmentShape *seg, cpVect p, cpPointQueryInfo *info) -{ - cpVect closest = cpClosetPointOnSegment(p, seg->ta, seg->tb); - - cpVect delta = cpvsub(p, closest); - cpFloat d = cpvlength(delta); - cpFloat r = seg->r; - cpVect g = cpvmult(delta, 1.0f/d); - - info->shape = (cpShape *)seg; - info->point = (d ? cpvadd(closest, cpvmult(g, r)) : closest); - info->distance = d - r; - - // Use the segment's normal if the distance is very small. - info->gradient = (d > MAGIC_EPSILON ? g : seg->n); -} - -static void -cpSegmentShapeSegmentQuery(cpSegmentShape *seg, cpVect a, cpVect b, cpFloat r2, cpSegmentQueryInfo *info) -{ - cpVect n = seg->tn; - cpFloat d = cpvdot(cpvsub(seg->ta, a), n); - cpFloat r = seg->r + r2; - - cpVect flipped_n = (d > 0.0f ? cpvneg(n) : n); - cpVect seg_offset = cpvsub(cpvmult(flipped_n, r), a); - - // Make the endpoints relative to 'a' and move them by the thickness of the segment. - cpVect seg_a = cpvadd(seg->ta, seg_offset); - cpVect seg_b = cpvadd(seg->tb, seg_offset); - cpVect delta = cpvsub(b, a); - - if(cpvcross(delta, seg_a)*cpvcross(delta, seg_b) <= 0.0f){ - cpFloat d_offset = d + (d > 0.0f ? -r : r); - cpFloat ad = -d_offset; - cpFloat bd = cpvdot(delta, n) - d_offset; - - if(ad*bd < 0.0f){ - cpFloat t = ad/(ad - bd); - - info->shape = (cpShape *)seg; - info->point = cpvsub(cpvlerp(a, b, t), cpvmult(flipped_n, r2)); - info->normal = flipped_n; - info->alpha = t; - } - } else if(r != 0.0f){ - cpSegmentQueryInfo info1 = {NULL, b, cpvzero, 1.0f}; - cpSegmentQueryInfo info2 = {NULL, b, cpvzero, 1.0f}; - CircleSegmentQuery((cpShape *)seg, seg->ta, seg->r, a, b, r2, &info1); - CircleSegmentQuery((cpShape *)seg, seg->tb, seg->r, a, b, r2, &info2); - - if(info1.alpha < info2.alpha){ - (*info) = info1; - } else { - (*info) = info2; - } - } -} - -static struct cpShapeMassInfo -cpSegmentShapeMassInfo(cpFloat mass, cpVect a, cpVect b, cpFloat r) -{ - struct cpShapeMassInfo info = { - mass, cpMomentForBox(1.0f, cpvdist(a, b) + 2.0f*r, 2.0f*r), // TODO is an approximation. - cpvlerp(a, b, 0.5f), - cpAreaForSegment(a, b, r), - }; - - return info; -} - -static const cpShapeClass cpSegmentShapeClass = { - CP_SEGMENT_SHAPE, - (cpShapeCacheDataImpl)cpSegmentShapeCacheData, - NULL, - (cpShapePointQueryImpl)cpSegmentShapePointQuery, - (cpShapeSegmentQueryImpl)cpSegmentShapeSegmentQuery, -}; - -cpSegmentShape * -cpSegmentShapeInit(cpSegmentShape *seg, cpBody *body, cpVect a, cpVect b, cpFloat r) -{ - seg->a = a; - seg->b = b; - seg->n = cpvrperp(cpvnormalize(cpvsub(b, a))); - - seg->r = r; - - seg->a_tangent = cpvzero; - seg->b_tangent = cpvzero; - - cpShapeInit((cpShape *)seg, &cpSegmentShapeClass, body, cpSegmentShapeMassInfo(0.0f, a, b, r)); - - return seg; -} - -cpShape* -cpSegmentShapeNew(cpBody *body, cpVect a, cpVect b, cpFloat r) -{ - return (cpShape *)cpSegmentShapeInit(cpSegmentShapeAlloc(), body, a, b, r); -} - -cpVect -cpSegmentShapeGetA(const cpShape *shape) -{ - cpAssertHard(shape->klass == &cpSegmentShapeClass, "Shape is not a segment shape."); - return ((cpSegmentShape *)shape)->a; -} - -cpVect -cpSegmentShapeGetB(const cpShape *shape) -{ - cpAssertHard(shape->klass == &cpSegmentShapeClass, "Shape is not a segment shape."); - return ((cpSegmentShape *)shape)->b; -} - -cpVect -cpSegmentShapeGetNormal(const cpShape *shape) -{ - cpAssertHard(shape->klass == &cpSegmentShapeClass, "Shape is not a segment shape."); - return ((cpSegmentShape *)shape)->n; -} - -cpFloat -cpSegmentShapeGetRadius(const cpShape *shape) -{ - cpAssertHard(shape->klass == &cpSegmentShapeClass, "Shape is not a segment shape."); - return ((cpSegmentShape *)shape)->r; -} - -void -cpSegmentShapeSetNeighbors(cpShape *shape, cpVect prev, cpVect next) -{ - cpAssertHard(shape->klass == &cpSegmentShapeClass, "Shape is not a segment shape."); - cpSegmentShape *seg = (cpSegmentShape *)shape; - - seg->a_tangent = cpvsub(prev, seg->a); - seg->b_tangent = cpvsub(next, seg->b); -} - -// Unsafe API (chipmunk_unsafe.h) - -// TODO setters should wake the shape up? - -void -cpCircleShapeSetRadius(cpShape *shape, cpFloat radius) -{ - cpAssertHard(shape->klass == &cpCircleShapeClass, "Shape is not a circle shape."); - cpCircleShape *circle = (cpCircleShape *)shape; - - circle->r = radius; - - cpFloat mass = shape->massInfo.m; - shape->massInfo = cpCircleShapeMassInfo(mass, circle->r, circle->c); - if(mass > 0.0f) cpBodyAccumulateMassFromShapes(shape->body); -} - -void -cpCircleShapeSetOffset(cpShape *shape, cpVect offset) -{ - cpAssertHard(shape->klass == &cpCircleShapeClass, "Shape is not a circle shape."); - cpCircleShape *circle = (cpCircleShape *)shape; - - circle->c = offset; - - cpFloat mass = shape->massInfo.m; - shape->massInfo = cpCircleShapeMassInfo(shape->massInfo.m, circle->r, circle->c); - if(mass > 0.0f) cpBodyAccumulateMassFromShapes(shape->body); -} - -void -cpSegmentShapeSetEndpoints(cpShape *shape, cpVect a, cpVect b) -{ - cpAssertHard(shape->klass == &cpSegmentShapeClass, "Shape is not a segment shape."); - cpSegmentShape *seg = (cpSegmentShape *)shape; - - seg->a = a; - seg->b = b; - seg->n = cpvperp(cpvnormalize(cpvsub(b, a))); - - cpFloat mass = shape->massInfo.m; - shape->massInfo = cpSegmentShapeMassInfo(shape->massInfo.m, seg->a, seg->b, seg->r); - if(mass > 0.0f) cpBodyAccumulateMassFromShapes(shape->body); -} - -void -cpSegmentShapeSetRadius(cpShape *shape, cpFloat radius) -{ - cpAssertHard(shape->klass == &cpSegmentShapeClass, "Shape is not a segment shape."); - cpSegmentShape *seg = (cpSegmentShape *)shape; - - seg->r = radius; - - cpFloat mass = shape->massInfo.m; - shape->massInfo = cpSegmentShapeMassInfo(shape->massInfo.m, seg->a, seg->b, seg->r); - if(mass > 0.0f) cpBodyAccumulateMassFromShapes(shape->body); -} diff --git a/3rdparty/chipmunk/src/cpSimpleMotor.c b/3rdparty/chipmunk/src/cpSimpleMotor.c deleted file mode 100644 index 2bea74a525da..000000000000 --- a/3rdparty/chipmunk/src/cpSimpleMotor.c +++ /dev/null @@ -1,123 +0,0 @@ -/* Copyright (c) 2013 Scott Lembcke and Howling Moon Software - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -#include "chipmunk/chipmunk_private.h" - -static void -preStep(cpSimpleMotor *joint, cpFloat dt) -{ - cpBody *a = joint->constraint.a; - cpBody *b = joint->constraint.b; - - // calculate moment of inertia coefficient. - joint->iSum = 1.0f/(a->i_inv + b->i_inv); -} - -static void -applyCachedImpulse(cpSimpleMotor *joint, cpFloat dt_coef) -{ - cpBody *a = joint->constraint.a; - cpBody *b = joint->constraint.b; - - cpFloat j = joint->jAcc*dt_coef; - a->w -= j*a->i_inv; - b->w += j*b->i_inv; -} - -static void -applyImpulse(cpSimpleMotor *joint, cpFloat dt) -{ - cpBody *a = joint->constraint.a; - cpBody *b = joint->constraint.b; - - // compute relative rotational velocity - cpFloat wr = b->w - a->w + joint->rate; - - cpFloat jMax = joint->constraint.maxForce*dt; - - // compute normal impulse - cpFloat j = -wr*joint->iSum; - cpFloat jOld = joint->jAcc; - joint->jAcc = cpfclamp(jOld + j, -jMax, jMax); - j = joint->jAcc - jOld; - - // apply impulse - a->w -= j*a->i_inv; - b->w += j*b->i_inv; -} - -static cpFloat -getImpulse(cpSimpleMotor *joint) -{ - return cpfabs(joint->jAcc); -} - -static const cpConstraintClass klass = { - (cpConstraintPreStepImpl)preStep, - (cpConstraintApplyCachedImpulseImpl)applyCachedImpulse, - (cpConstraintApplyImpulseImpl)applyImpulse, - (cpConstraintGetImpulseImpl)getImpulse, -}; - -cpSimpleMotor * -cpSimpleMotorAlloc(void) -{ - return (cpSimpleMotor *)cpcalloc(1, sizeof(cpSimpleMotor)); -} - -cpSimpleMotor * -cpSimpleMotorInit(cpSimpleMotor *joint, cpBody *a, cpBody *b, cpFloat rate) -{ - cpConstraintInit((cpConstraint *)joint, &klass, a, b); - - joint->rate = rate; - - joint->jAcc = 0.0f; - - return joint; -} - -cpConstraint * -cpSimpleMotorNew(cpBody *a, cpBody *b, cpFloat rate) -{ - return (cpConstraint *)cpSimpleMotorInit(cpSimpleMotorAlloc(), a, b, rate); -} - -cpBool -cpConstraintIsSimpleMotor(const cpConstraint *constraint) -{ - return (constraint->klass == &klass); -} - -cpFloat -cpSimpleMotorGetRate(const cpConstraint *constraint) -{ - cpAssertHard(cpConstraintIsSimpleMotor(constraint), "Constraint is not a SimpleMotor."); - return ((cpSimpleMotor *)constraint)->rate; -} - -void -cpSimpleMotorSetRate(cpConstraint *constraint, cpFloat rate) -{ - cpAssertHard(cpConstraintIsSimpleMotor(constraint), "Constraint is not a SimpleMotor."); - cpConstraintActivateBodies(constraint); - ((cpSimpleMotor *)constraint)->rate = rate; -} diff --git a/3rdparty/chipmunk/src/cpSlideJoint.c b/3rdparty/chipmunk/src/cpSlideJoint.c deleted file mode 100644 index 61afe33e4b1a..000000000000 --- a/3rdparty/chipmunk/src/cpSlideJoint.c +++ /dev/null @@ -1,195 +0,0 @@ -/* Copyright (c) 2013 Scott Lembcke and Howling Moon Software - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -#include "chipmunk/chipmunk_private.h" - -static void -preStep(cpSlideJoint *joint, cpFloat dt) -{ - cpBody *a = joint->constraint.a; - cpBody *b = joint->constraint.b; - - joint->r1 = cpTransformVect(a->transform, cpvsub(joint->anchorA, a->cog)); - joint->r2 = cpTransformVect(b->transform, cpvsub(joint->anchorB, b->cog)); - - cpVect delta = cpvsub(cpvadd(b->p, joint->r2), cpvadd(a->p, joint->r1)); - cpFloat dist = cpvlength(delta); - cpFloat pdist = 0.0f; - if(dist > joint->max) { - pdist = dist - joint->max; - joint->n = cpvnormalize(delta); - } else if(dist < joint->min) { - pdist = joint->min - dist; - joint->n = cpvneg(cpvnormalize(delta)); - } else { - joint->n = cpvzero; - joint->jnAcc = 0.0f; - } - - // calculate mass normal - joint->nMass = 1.0f/k_scalar(a, b, joint->r1, joint->r2, joint->n); - - // calculate bias velocity - cpFloat maxBias = joint->constraint.maxBias; - joint->bias = cpfclamp(-bias_coef(joint->constraint.errorBias, dt)*pdist/dt, -maxBias, maxBias); -} - -static void -applyCachedImpulse(cpSlideJoint *joint, cpFloat dt_coef) -{ - cpBody *a = joint->constraint.a; - cpBody *b = joint->constraint.b; - - cpVect j = cpvmult(joint->n, joint->jnAcc*dt_coef); - apply_impulses(a, b, joint->r1, joint->r2, j); -} - -static void -applyImpulse(cpSlideJoint *joint, cpFloat dt) -{ - if(cpveql(joint->n, cpvzero)) return; // early exit - - cpBody *a = joint->constraint.a; - cpBody *b = joint->constraint.b; - - cpVect n = joint->n; - cpVect r1 = joint->r1; - cpVect r2 = joint->r2; - - // compute relative velocity - cpVect vr = relative_velocity(a, b, r1, r2); - cpFloat vrn = cpvdot(vr, n); - - // compute normal impulse - cpFloat jn = (joint->bias - vrn)*joint->nMass; - cpFloat jnOld = joint->jnAcc; - joint->jnAcc = cpfclamp(jnOld + jn, -joint->constraint.maxForce*dt, 0.0f); - jn = joint->jnAcc - jnOld; - - // apply impulse - apply_impulses(a, b, joint->r1, joint->r2, cpvmult(n, jn)); -} - -static cpFloat -getImpulse(cpConstraint *joint) -{ - return cpfabs(((cpSlideJoint *)joint)->jnAcc); -} - -static const cpConstraintClass klass = { - (cpConstraintPreStepImpl)preStep, - (cpConstraintApplyCachedImpulseImpl)applyCachedImpulse, - (cpConstraintApplyImpulseImpl)applyImpulse, - (cpConstraintGetImpulseImpl)getImpulse, -}; - -cpSlideJoint * -cpSlideJointAlloc(void) -{ - return (cpSlideJoint *)cpcalloc(1, sizeof(cpSlideJoint)); -} - -cpSlideJoint * -cpSlideJointInit(cpSlideJoint *joint, cpBody *a, cpBody *b, cpVect anchorA, cpVect anchorB, cpFloat min, cpFloat max) -{ - cpConstraintInit((cpConstraint *)joint, &klass, a, b); - - joint->anchorA = anchorA; - joint->anchorB = anchorB; - joint->min = min; - joint->max = max; - - joint->jnAcc = 0.0f; - - return joint; -} - -cpConstraint * -cpSlideJointNew(cpBody *a, cpBody *b, cpVect anchorA, cpVect anchorB, cpFloat min, cpFloat max) -{ - return (cpConstraint *)cpSlideJointInit(cpSlideJointAlloc(), a, b, anchorA, anchorB, min, max); -} - -cpBool -cpConstraintIsSlideJoint(const cpConstraint *constraint) -{ - return (constraint->klass == &klass); -} - -cpVect -cpSlideJointGetAnchorA(const cpConstraint *constraint) -{ - cpAssertHard(cpConstraintIsSlideJoint(constraint), "Constraint is not a slide joint."); - return ((cpSlideJoint *)constraint)->anchorA; -} - -void -cpSlideJointSetAnchorA(cpConstraint *constraint, cpVect anchorA) -{ - cpAssertHard(cpConstraintIsSlideJoint(constraint), "Constraint is not a slide joint."); - cpConstraintActivateBodies(constraint); - ((cpSlideJoint *)constraint)->anchorA = anchorA; -} - -cpVect -cpSlideJointGetAnchorB(const cpConstraint *constraint) -{ - cpAssertHard(cpConstraintIsSlideJoint(constraint), "Constraint is not a slide joint."); - return ((cpSlideJoint *)constraint)->anchorB; -} - -void -cpSlideJointSetAnchorB(cpConstraint *constraint, cpVect anchorB) -{ - cpAssertHard(cpConstraintIsSlideJoint(constraint), "Constraint is not a slide joint."); - cpConstraintActivateBodies(constraint); - ((cpSlideJoint *)constraint)->anchorB = anchorB; -} - -cpFloat -cpSlideJointGetMin(const cpConstraint *constraint) -{ - cpAssertHard(cpConstraintIsSlideJoint(constraint), "Constraint is not a slide joint."); - return ((cpSlideJoint *)constraint)->min; -} - -void -cpSlideJointSetMin(cpConstraint *constraint, cpFloat min) -{ - cpAssertHard(cpConstraintIsSlideJoint(constraint), "Constraint is not a slide joint."); - cpConstraintActivateBodies(constraint); - ((cpSlideJoint *)constraint)->min = min; -} - -cpFloat -cpSlideJointGetMax(const cpConstraint *constraint) -{ - cpAssertHard(cpConstraintIsSlideJoint(constraint), "Constraint is not a slide joint."); - return ((cpSlideJoint *)constraint)->max; -} - -void -cpSlideJointSetMax(cpConstraint *constraint, cpFloat max) -{ - cpAssertHard(cpConstraintIsSlideJoint(constraint), "Constraint is not a slide joint."); - cpConstraintActivateBodies(constraint); - ((cpSlideJoint *)constraint)->max = max; -} diff --git a/3rdparty/chipmunk/src/cpSpace.c b/3rdparty/chipmunk/src/cpSpace.c deleted file mode 100644 index b319d3a4e831..000000000000 --- a/3rdparty/chipmunk/src/cpSpace.c +++ /dev/null @@ -1,701 +0,0 @@ -/* Copyright (c) 2013 Scott Lembcke and Howling Moon Software - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -#include -#include - -#include "chipmunk/chipmunk_private.h" - -//MARK: Contact Set Helpers - -// Equal function for arbiterSet. -static cpBool -arbiterSetEql(cpShape **shapes, cpArbiter *arb) -{ - cpShape *a = shapes[0]; - cpShape *b = shapes[1]; - - return ((a == arb->a && b == arb->b) || (b == arb->a && a == arb->b)); -} - -//MARK: Collision Handler Set HelperFunctions - -// Equals function for collisionHandlers. -static cpBool -handlerSetEql(cpCollisionHandler *check, cpCollisionHandler *pair) -{ - return ( - (check->typeA == pair->typeA && check->typeB == pair->typeB) || - (check->typeB == pair->typeA && check->typeA == pair->typeB) - ); -} - -// Transformation function for collisionHandlers. -static void * -handlerSetTrans(cpCollisionHandler *handler, void *unused) -{ - cpCollisionHandler *copy = (cpCollisionHandler *)cpcalloc(1, sizeof(cpCollisionHandler)); - memcpy(copy, handler, sizeof(cpCollisionHandler)); - - return copy; -} - -//MARK: Misc Helper Funcs - -// Default collision functions. - -static cpBool -DefaultBegin(cpArbiter *arb, cpSpace *space, cpDataPointer data){ - cpBool retA = cpArbiterCallWildcardBeginA(arb, space); - cpBool retB = cpArbiterCallWildcardBeginB(arb, space); - return retA && retB; -} - -static cpBool -DefaultPreSolve(cpArbiter *arb, cpSpace *space, cpDataPointer data){ - cpBool retA = cpArbiterCallWildcardPreSolveA(arb, space); - cpBool retB = cpArbiterCallWildcardPreSolveB(arb, space); - return retA && retB; -} - -static void -DefaultPostSolve(cpArbiter *arb, cpSpace *space, cpDataPointer data){ - cpArbiterCallWildcardPostSolveA(arb, space); - cpArbiterCallWildcardPostSolveB(arb, space); -} - -static void -DefaultSeparate(cpArbiter *arb, cpSpace *space, cpDataPointer data){ - cpArbiterCallWildcardSeparateA(arb, space); - cpArbiterCallWildcardSeparateB(arb, space); -} - -// Use the wildcard identifier since the default handler should never match any type pair. -static cpCollisionHandler cpCollisionHandlerDefault = { - CP_WILDCARD_COLLISION_TYPE, CP_WILDCARD_COLLISION_TYPE, - DefaultBegin, DefaultPreSolve, DefaultPostSolve, DefaultSeparate, NULL -}; - -static cpBool AlwaysCollide(cpArbiter *arb, cpSpace *space, cpDataPointer data){return cpTrue;} -static void DoNothing(cpArbiter *arb, cpSpace *space, cpDataPointer data){} - -cpCollisionHandler cpCollisionHandlerDoNothing = { - CP_WILDCARD_COLLISION_TYPE, CP_WILDCARD_COLLISION_TYPE, - AlwaysCollide, AlwaysCollide, DoNothing, DoNothing, NULL -}; - -// function to get the estimated velocity of a shape for the cpBBTree. -static cpVect ShapeVelocityFunc(cpShape *shape){return shape->body->v;} - -// Used for disposing of collision handlers. -static void FreeWrap(void *ptr, void *unused){cpfree(ptr);} - -//MARK: Memory Management Functions - -cpSpace * -cpSpaceAlloc(void) -{ - return (cpSpace *)cpcalloc(1, sizeof(cpSpace)); -} - -cpSpace* -cpSpaceInit(cpSpace *space) -{ -#ifndef NDEBUG - static cpBool done = cpFalse; - if(!done){ - printf("Initializing cpSpace - Chipmunk v%s (Debug Enabled)\n", cpVersionString); - printf("Compile with -DNDEBUG defined to disable debug mode and runtime assertion checks\n"); - done = cpTrue; - } -#endif - - space->iterations = 10; - - space->gravity = cpvzero; - space->damping = 1.0f; - - space->collisionSlop = 0.1f; - space->collisionBias = cpfpow(1.0f - 0.1f, 60.0f); - space->collisionPersistence = 3; - - space->locked = 0; - space->stamp = 0; - - space->shapeIDCounter = 0; - space->staticShapes = cpBBTreeNew((cpSpatialIndexBBFunc)cpShapeGetBB, NULL); - space->dynamicShapes = cpBBTreeNew((cpSpatialIndexBBFunc)cpShapeGetBB, space->staticShapes); - cpBBTreeSetVelocityFunc(space->dynamicShapes, (cpBBTreeVelocityFunc)ShapeVelocityFunc); - - space->allocatedBuffers = cpArrayNew(0); - - space->dynamicBodies = cpArrayNew(0); - space->staticBodies = cpArrayNew(0); - space->sleepingComponents = cpArrayNew(0); - space->rousedBodies = cpArrayNew(0); - - space->sleepTimeThreshold = INFINITY; - space->idleSpeedThreshold = 0.0f; - - space->arbiters = cpArrayNew(0); - space->pooledArbiters = cpArrayNew(0); - - space->contactBuffersHead = NULL; - space->cachedArbiters = cpHashSetNew(0, (cpHashSetEqlFunc)arbiterSetEql); - - space->constraints = cpArrayNew(0); - - space->usesWildcards = cpFalse; - memcpy(&space->defaultHandler, &cpCollisionHandlerDoNothing, sizeof(cpCollisionHandler)); - space->collisionHandlers = cpHashSetNew(0, (cpHashSetEqlFunc)handlerSetEql); - - space->postStepCallbacks = cpArrayNew(0); - space->skipPostStep = cpFalse; - - cpBody *staticBody = cpBodyInit(&space->_staticBody, 0.0f, 0.0f); - cpBodySetType(staticBody, CP_BODY_TYPE_STATIC); - cpSpaceSetStaticBody(space, staticBody); - - return space; -} - -cpSpace* -cpSpaceNew(void) -{ - return cpSpaceInit(cpSpaceAlloc()); -} - -static void cpBodyActivateWrap(cpBody *body, void *unused){cpBodyActivate(body);} - -void -cpSpaceDestroy(cpSpace *space) -{ - cpSpaceEachBody(space, (cpSpaceBodyIteratorFunc)cpBodyActivateWrap, NULL); - - cpSpatialIndexFree(space->staticShapes); - cpSpatialIndexFree(space->dynamicShapes); - - cpArrayFree(space->dynamicBodies); - cpArrayFree(space->staticBodies); - cpArrayFree(space->sleepingComponents); - cpArrayFree(space->rousedBodies); - - cpArrayFree(space->constraints); - - cpHashSetFree(space->cachedArbiters); - - cpArrayFree(space->arbiters); - cpArrayFree(space->pooledArbiters); - - if(space->allocatedBuffers){ - cpArrayFreeEach(space->allocatedBuffers, cpfree); - cpArrayFree(space->allocatedBuffers); - } - - if(space->postStepCallbacks){ - cpArrayFreeEach(space->postStepCallbacks, cpfree); - cpArrayFree(space->postStepCallbacks); - } - - if(space->collisionHandlers) cpHashSetEach(space->collisionHandlers, FreeWrap, NULL); - cpHashSetFree(space->collisionHandlers); -} - -void -cpSpaceFree(cpSpace *space) -{ - if(space){ - cpSpaceDestroy(space); - cpfree(space); - } -} - - -//MARK: Basic properties: - -int -cpSpaceGetIterations(const cpSpace *space) -{ - return space->iterations; -} - -void -cpSpaceSetIterations(cpSpace *space, int iterations) -{ - cpAssertHard(iterations > 0, "Iterations must be positive and non-zero."); - space->iterations = iterations; -} - -cpVect -cpSpaceGetGravity(const cpSpace *space) -{ - return space->gravity; -} - -void -cpSpaceSetGravity(cpSpace *space, cpVect gravity) -{ - space->gravity = gravity; - - // Wake up all of the bodies since the gravity changed. - cpArray *components = space->sleepingComponents; - for(int i=0; inum; i++){ - cpBodyActivate((cpBody *)components->arr[i]); - } -} - -cpFloat -cpSpaceGetDamping(const cpSpace *space) -{ - return space->damping; -} - -void -cpSpaceSetDamping(cpSpace *space, cpFloat damping) -{ - cpAssertHard(damping >= 0.0, "Damping must be positive."); - space->damping = damping; -} - -cpFloat -cpSpaceGetIdleSpeedThreshold(const cpSpace *space) -{ - return space->idleSpeedThreshold; -} - -void -cpSpaceSetIdleSpeedThreshold(cpSpace *space, cpFloat idleSpeedThreshold) -{ - space->idleSpeedThreshold = idleSpeedThreshold; -} - -cpFloat -cpSpaceGetSleepTimeThreshold(const cpSpace *space) -{ - return space->sleepTimeThreshold; -} - -void -cpSpaceSetSleepTimeThreshold(cpSpace *space, cpFloat sleepTimeThreshold) -{ - space->sleepTimeThreshold = sleepTimeThreshold; -} - -cpFloat -cpSpaceGetCollisionSlop(const cpSpace *space) -{ - return space->collisionSlop; -} - -void -cpSpaceSetCollisionSlop(cpSpace *space, cpFloat collisionSlop) -{ - space->collisionSlop = collisionSlop; -} - -cpFloat -cpSpaceGetCollisionBias(const cpSpace *space) -{ - return space->collisionBias; -} - -void -cpSpaceSetCollisionBias(cpSpace *space, cpFloat collisionBias) -{ - space->collisionBias = collisionBias; -} - -cpTimestamp -cpSpaceGetCollisionPersistence(const cpSpace *space) -{ - return space->collisionPersistence; -} - -void -cpSpaceSetCollisionPersistence(cpSpace *space, cpTimestamp collisionPersistence) -{ - space->collisionPersistence = collisionPersistence; -} - -cpDataPointer -cpSpaceGetUserData(const cpSpace *space) -{ - return space->userData; -} - -void -cpSpaceSetUserData(cpSpace *space, cpDataPointer userData) -{ - space->userData = userData; -} - -cpBody * -cpSpaceGetStaticBody(const cpSpace *space) -{ - return space->staticBody; -} - -cpFloat -cpSpaceGetCurrentTimeStep(const cpSpace *space) -{ - return space->curr_dt; -} - -void -cpSpaceSetStaticBody(cpSpace *space, cpBody *body) -{ - if(space->staticBody != NULL){ - cpAssertHard(space->staticBody->shapeList == NULL, "Internal Error: Changing the designated static body while the old one still had shapes attached."); - space->staticBody->space = NULL; - } - - space->staticBody = body; - body->space = space; -} - -cpBool -cpSpaceIsLocked(cpSpace *space) -{ - return (space->locked > 0); -} - -//MARK: Collision Handler Function Management - -static void -cpSpaceUseWildcardDefaultHandler(cpSpace *space) -{ - // Spaces default to using the slightly faster "do nothing" default handler until wildcards are potentially needed. - if(!space->usesWildcards){ - space->usesWildcards = cpTrue; - memcpy(&space->defaultHandler, &cpCollisionHandlerDefault, sizeof(cpCollisionHandler)); - } -} - -cpCollisionHandler *cpSpaceAddDefaultCollisionHandler(cpSpace *space) -{ - cpSpaceUseWildcardDefaultHandler(space); - return &space->defaultHandler; -} - -cpCollisionHandler *cpSpaceAddCollisionHandler(cpSpace *space, cpCollisionType a, cpCollisionType b) -{ - cpHashValue hash = CP_HASH_PAIR(a, b); - cpCollisionHandler handler = {a, b, DefaultBegin, DefaultPreSolve, DefaultPostSolve, DefaultSeparate, NULL}; - return (cpCollisionHandler*)cpHashSetInsert(space->collisionHandlers, hash, &handler, (cpHashSetTransFunc)handlerSetTrans, NULL); -} - -cpCollisionHandler * -cpSpaceAddWildcardHandler(cpSpace *space, cpCollisionType type) -{ - cpSpaceUseWildcardDefaultHandler(space); - - cpHashValue hash = CP_HASH_PAIR(type, CP_WILDCARD_COLLISION_TYPE); - cpCollisionHandler handler = {type, CP_WILDCARD_COLLISION_TYPE, AlwaysCollide, AlwaysCollide, DoNothing, DoNothing, NULL}; - return (cpCollisionHandler*)cpHashSetInsert(space->collisionHandlers, hash, &handler, (cpHashSetTransFunc)handlerSetTrans, NULL); -} - - -//MARK: Body, Shape, and Joint Management -cpShape * -cpSpaceAddShape(cpSpace *space, cpShape *shape) -{ - cpAssertHard(shape->space != space, "You have already added this shape to this space. You must not add it a second time."); - cpAssertHard(!shape->space, "You have already added this shape to another space. You cannot add it to a second."); - cpAssertHard(shape->body, "The shape's body is not defined."); - cpAssertHard(shape->body->space == space, "The shape's body must be added to the space before the shape."); - cpAssertSpaceUnlocked(space); - - cpBody *body = shape->body; - - cpBool isStatic = (cpBodyGetType(body) == CP_BODY_TYPE_STATIC); - if(!isStatic) cpBodyActivate(body); - cpBodyAddShape(body, shape); - - shape->hashid = space->shapeIDCounter++; - cpShapeUpdate(shape, body->transform); - cpSpatialIndexInsert(isStatic ? space->staticShapes : space->dynamicShapes, shape, shape->hashid); - shape->space = space; - - return shape; -} - -cpBody * -cpSpaceAddBody(cpSpace *space, cpBody *body) -{ - cpAssertHard(body->space != space, "You have already added this body to this space. You must not add it a second time."); - cpAssertHard(!body->space, "You have already added this body to another space. You cannot add it to a second."); - cpAssertSpaceUnlocked(space); - - cpArrayPush(cpSpaceArrayForBodyType(space, cpBodyGetType(body)), body); - body->space = space; - - return body; -} - -cpConstraint * -cpSpaceAddConstraint(cpSpace *space, cpConstraint *constraint) -{ - cpAssertHard(constraint->space != space, "You have already added this constraint to this space. You must not add it a second time."); - cpAssertHard(!constraint->space, "You have already added this constraint to another space. You cannot add it to a second."); - cpAssertSpaceUnlocked(space); - - cpBody *a = constraint->a, *b = constraint->b; - cpAssertHard(a != NULL && b != NULL, "Constraint is attached to a NULL body."); -// cpAssertHard(a->space == space && b->space == space, "The constraint's bodies must be added to the space before the constraint."); - - cpBodyActivate(a); - cpBodyActivate(b); - cpArrayPush(space->constraints, constraint); - - // Push onto the heads of the bodies' constraint lists - constraint->next_a = a->constraintList; a->constraintList = constraint; - constraint->next_b = b->constraintList; b->constraintList = constraint; - constraint->space = space; - - return constraint; -} - -struct arbiterFilterContext { - cpSpace *space; - cpBody *body; - cpShape *shape; -}; - -static cpBool -cachedArbitersFilter(cpArbiter *arb, struct arbiterFilterContext *context) -{ - cpShape *shape = context->shape; - cpBody *body = context->body; - - - // Match on the filter shape, or if it's NULL the filter body - if( - (body == arb->body_a && (shape == arb->a || shape == NULL)) || - (body == arb->body_b && (shape == arb->b || shape == NULL)) - ){ - // Call separate when removing shapes. - if(shape && arb->state != CP_ARBITER_STATE_CACHED){ - // Invalidate the arbiter since one of the shapes was removed. - arb->state = CP_ARBITER_STATE_INVALIDATED; - - cpCollisionHandler *handler = arb->handler; - handler->separateFunc(arb, context->space, handler->userData); - } - - cpArbiterUnthread(arb); - cpArrayDeleteObj(context->space->arbiters, arb); - cpArrayPush(context->space->pooledArbiters, arb); - - return cpFalse; - } - - return cpTrue; -} - -void -cpSpaceFilterArbiters(cpSpace *space, cpBody *body, cpShape *filter) -{ - cpSpaceLock(space); { - struct arbiterFilterContext context = {space, body, filter}; - cpHashSetFilter(space->cachedArbiters, (cpHashSetFilterFunc)cachedArbitersFilter, &context); - } cpSpaceUnlock(space, cpTrue); -} - -void -cpSpaceRemoveShape(cpSpace *space, cpShape *shape) -{ - cpBody *body = shape->body; - cpAssertHard(cpSpaceContainsShape(space, shape), "Cannot remove a shape that was not added to the space. (Removed twice maybe?)"); - cpAssertSpaceUnlocked(space); - - cpBool isStatic = (cpBodyGetType(body) == CP_BODY_TYPE_STATIC); - if(isStatic){ - cpBodyActivateStatic(body, shape); - } else { - cpBodyActivate(body); - } - - cpBodyRemoveShape(body, shape); - cpSpaceFilterArbiters(space, body, shape); - cpSpatialIndexRemove(isStatic ? space->staticShapes : space->dynamicShapes, shape, shape->hashid); - shape->space = NULL; - shape->hashid = 0; -} - -void -cpSpaceRemoveBody(cpSpace *space, cpBody *body) -{ - cpAssertHard(body != cpSpaceGetStaticBody(space), "Cannot remove the designated static body for the space."); - cpAssertHard(cpSpaceContainsBody(space, body), "Cannot remove a body that was not added to the space. (Removed twice maybe?)"); -// cpAssertHard(body->shapeList == NULL, "Cannot remove a body from the space before removing the bodies attached to it."); -// cpAssertHard(body->constraintList == NULL, "Cannot remove a body from the space before removing the constraints attached to it."); - cpAssertSpaceUnlocked(space); - - cpBodyActivate(body); -// cpSpaceFilterArbiters(space, body, NULL); - cpArrayDeleteObj(cpSpaceArrayForBodyType(space, cpBodyGetType(body)), body); - body->space = NULL; -} - -void -cpSpaceRemoveConstraint(cpSpace *space, cpConstraint *constraint) -{ - cpAssertHard(cpSpaceContainsConstraint(space, constraint), "Cannot remove a constraint that was not added to the space. (Removed twice maybe?)"); - cpAssertSpaceUnlocked(space); - - cpBodyActivate(constraint->a); - cpBodyActivate(constraint->b); - cpArrayDeleteObj(space->constraints, constraint); - - cpBodyRemoveConstraint(constraint->a, constraint); - cpBodyRemoveConstraint(constraint->b, constraint); - constraint->space = NULL; -} - -cpBool cpSpaceContainsShape(cpSpace *space, cpShape *shape) -{ - return (shape->space == space); -} - -cpBool cpSpaceContainsBody(cpSpace *space, cpBody *body) -{ - return (body->space == space); -} - -cpBool cpSpaceContainsConstraint(cpSpace *space, cpConstraint *constraint) -{ - return (constraint->space == space); -} - -//MARK: Iteration - -void -cpSpaceEachBody(cpSpace *space, cpSpaceBodyIteratorFunc func, void *data) -{ - cpSpaceLock(space); { - cpArray *bodies = space->dynamicBodies; - for(int i=0; inum; i++){ - func((cpBody *)bodies->arr[i], data); - } - - cpArray *otherBodies = space->staticBodies; - for(int i=0; inum; i++){ - func((cpBody *)otherBodies->arr[i], data); - } - - cpArray *components = space->sleepingComponents; - for(int i=0; inum; i++){ - cpBody *root = (cpBody *)components->arr[i]; - - cpBody *body = root; - while(body){ - cpBody *next = body->sleeping.next; - func(body, data); - body = next; - } - } - } cpSpaceUnlock(space, cpTrue); -} - -typedef struct spaceShapeContext { - cpSpaceShapeIteratorFunc func; - void *data; -} spaceShapeContext; - -static void -spaceEachShapeIterator(cpShape *shape, spaceShapeContext *context) -{ - context->func(shape, context->data); -} - -void -cpSpaceEachShape(cpSpace *space, cpSpaceShapeIteratorFunc func, void *data) -{ - cpSpaceLock(space); { - spaceShapeContext context = {func, data}; - cpSpatialIndexEach(space->dynamicShapes, (cpSpatialIndexIteratorFunc)spaceEachShapeIterator, &context); - cpSpatialIndexEach(space->staticShapes, (cpSpatialIndexIteratorFunc)spaceEachShapeIterator, &context); - } cpSpaceUnlock(space, cpTrue); -} - -void -cpSpaceEachConstraint(cpSpace *space, cpSpaceConstraintIteratorFunc func, void *data) -{ - cpSpaceLock(space); { - cpArray *constraints = space->constraints; - - for(int i=0; inum; i++){ - func((cpConstraint *)constraints->arr[i], data); - } - } cpSpaceUnlock(space, cpTrue); -} - -//MARK: Spatial Index Management - -void -cpSpaceReindexStatic(cpSpace *space) -{ - cpAssertHard(!space->locked, "You cannot manually reindex objects while the space is locked. Wait until the current query or step is complete."); - - cpSpatialIndexEach(space->staticShapes, (cpSpatialIndexIteratorFunc)&cpShapeUpdateFunc, NULL); - cpSpatialIndexReindex(space->staticShapes); -} - -void -cpSpaceReindexShape(cpSpace *space, cpShape *shape) -{ - cpAssertHard(!space->locked, "You cannot manually reindex objects while the space is locked. Wait until the current query or step is complete."); - - cpShapeCacheBB(shape); - - // attempt to rehash the shape in both hashes - cpSpatialIndexReindexObject(space->dynamicShapes, shape, shape->hashid); - cpSpatialIndexReindexObject(space->staticShapes, shape, shape->hashid); -} - -void -cpSpaceReindexShapesForBody(cpSpace *space, cpBody *body) -{ - CP_BODY_FOREACH_SHAPE(body, shape) cpSpaceReindexShape(space, shape); -} - - -static void -copyShapes(cpShape *shape, cpSpatialIndex *index) -{ - cpSpatialIndexInsert(index, shape, shape->hashid); -} - -void -cpSpaceUseSpatialHash(cpSpace *space, cpFloat dim, int count) -{ - cpSpatialIndex *staticShapes = cpSpaceHashNew(dim, count, (cpSpatialIndexBBFunc)cpShapeGetBB, NULL); - cpSpatialIndex *dynamicShapes = cpSpaceHashNew(dim, count, (cpSpatialIndexBBFunc)cpShapeGetBB, staticShapes); - - cpSpatialIndexEach(space->staticShapes, (cpSpatialIndexIteratorFunc)copyShapes, staticShapes); - cpSpatialIndexEach(space->dynamicShapes, (cpSpatialIndexIteratorFunc)copyShapes, dynamicShapes); - - cpSpatialIndexFree(space->staticShapes); - cpSpatialIndexFree(space->dynamicShapes); - - space->staticShapes = staticShapes; - space->dynamicShapes = dynamicShapes; -} diff --git a/3rdparty/chipmunk/src/cpSpaceComponent.c b/3rdparty/chipmunk/src/cpSpaceComponent.c deleted file mode 100644 index 7b2d60699111..000000000000 --- a/3rdparty/chipmunk/src/cpSpaceComponent.c +++ /dev/null @@ -1,349 +0,0 @@ -/* Copyright (c) 2007 Scott Lembcke - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -#include - -#include "chipmunk/chipmunk_private.h" - -//MARK: Sleeping Functions - -void -cpSpaceActivateBody(cpSpace *space, cpBody *body) -{ - cpAssertHard(cpBodyGetType(body) == CP_BODY_TYPE_DYNAMIC, "Internal error: Attempting to activate a non-dynamic body."); - - if(space->locked){ - // cpSpaceActivateBody() is called again once the space is unlocked - if(!cpArrayContains(space->rousedBodies, body)) cpArrayPush(space->rousedBodies, body); - } else { - cpAssertSoft(body->sleeping.root == NULL && body->sleeping.next == NULL, "Internal error: Activating body non-NULL node pointers."); - cpArrayPush(space->dynamicBodies, body); - - CP_BODY_FOREACH_SHAPE(body, shape){ - cpSpatialIndexRemove(space->staticShapes, shape, shape->hashid); - cpSpatialIndexInsert(space->dynamicShapes, shape, shape->hashid); - } - - CP_BODY_FOREACH_ARBITER(body, arb){ - cpBody *bodyA = arb->body_a; - - // Arbiters are shared between two bodies that are always woken up together. - // You only want to restore the arbiter once, so bodyA is arbitrarily chosen to own the arbiter. - // The edge case is when static bodies are involved as the static bodies never actually sleep. - // If the static body is bodyB then all is good. If the static body is bodyA, that can easily be checked. - if(body == bodyA || cpBodyGetType(bodyA) == CP_BODY_TYPE_STATIC){ - int numContacts = arb->count; - struct cpContact *contacts = arb->contacts; - - // Restore contact values back to the space's contact buffer memory - arb->contacts = cpContactBufferGetArray(space); - memcpy(arb->contacts, contacts, numContacts*sizeof(struct cpContact)); - cpSpacePushContacts(space, numContacts); - - // Reinsert the arbiter into the arbiter cache - const cpShape *a = arb->a, *b = arb->b; - const cpShape *shape_pair[] = {a, b}; - cpHashValue arbHashID = CP_HASH_PAIR((cpHashValue)a, (cpHashValue)b); - cpHashSetInsert(space->cachedArbiters, arbHashID, shape_pair, NULL, arb); - - // Update the arbiter's state - arb->stamp = space->stamp; - cpArrayPush(space->arbiters, arb); - - cpfree(contacts); - } - } - - CP_BODY_FOREACH_CONSTRAINT(body, constraint){ - cpBody *bodyA = constraint->a; - if(body == bodyA || cpBodyGetType(bodyA) == CP_BODY_TYPE_STATIC) cpArrayPush(space->constraints, constraint); - } - } -} - -static void -cpSpaceDeactivateBody(cpSpace *space, cpBody *body) -{ - cpAssertHard(cpBodyGetType(body) == CP_BODY_TYPE_DYNAMIC, "Internal error: Attempting to deactivate a non-dynamic body."); - - cpArrayDeleteObj(space->dynamicBodies, body); - - CP_BODY_FOREACH_SHAPE(body, shape){ - cpSpatialIndexRemove(space->dynamicShapes, shape, shape->hashid); - cpSpatialIndexInsert(space->staticShapes, shape, shape->hashid); - } - - CP_BODY_FOREACH_ARBITER(body, arb){ - cpBody *bodyA = arb->body_a; - if(body == bodyA || cpBodyGetType(bodyA) == CP_BODY_TYPE_STATIC){ - cpSpaceUncacheArbiter(space, arb); - - // Save contact values to a new block of memory so they won't time out - size_t bytes = arb->count*sizeof(struct cpContact); - struct cpContact *contacts = (struct cpContact *)cpcalloc(1, bytes); - memcpy(contacts, arb->contacts, bytes); - arb->contacts = contacts; - } - } - - CP_BODY_FOREACH_CONSTRAINT(body, constraint){ - cpBody *bodyA = constraint->a; - if(body == bodyA || cpBodyGetType(bodyA) == CP_BODY_TYPE_STATIC) cpArrayDeleteObj(space->constraints, constraint); - } -} - -static inline cpBody * -ComponentRoot(cpBody *body) -{ - return (body ? body->sleeping.root : NULL); -} - -void -cpBodyActivate(cpBody *body) -{ - if(body != NULL && cpBodyGetType(body) == CP_BODY_TYPE_DYNAMIC){ - body->sleeping.idleTime = 0.0f; - - cpBody *root = ComponentRoot(body); - if(root && cpBodyIsSleeping(root)){ - // TODO should cpBodyIsSleeping(root) be an assertion? - cpAssertSoft(cpBodyGetType(root) == CP_BODY_TYPE_DYNAMIC, "Internal Error: Non-dynamic body component root detected."); - - cpSpace *space = root->space; - cpBody *body = root; - while(body){ - cpBody *next = body->sleeping.next; - - body->sleeping.idleTime = 0.0f; - body->sleeping.root = NULL; - body->sleeping.next = NULL; - cpSpaceActivateBody(space, body); - - body = next; - } - - cpArrayDeleteObj(space->sleepingComponents, root); - } - - CP_BODY_FOREACH_ARBITER(body, arb){ - // Reset the idle timer of things the body is touching as well. - // That way things don't get left hanging in the air. - cpBody *other = (arb->body_a == body ? arb->body_b : arb->body_a); - if(cpBodyGetType(other) != CP_BODY_TYPE_STATIC) other->sleeping.idleTime = 0.0f; - } - } -} - -void -cpBodyActivateStatic(cpBody *body, cpShape *filter) -{ - cpAssertHard(cpBodyGetType(body) == CP_BODY_TYPE_STATIC, "cpBodyActivateStatic() called on a non-static body."); - - CP_BODY_FOREACH_ARBITER(body, arb){ - if(!filter || filter == arb->a || filter == arb->b){ - cpBodyActivate(arb->body_a == body ? arb->body_b : arb->body_a); - } - } - - // TODO: should also activate joints? -} - -static inline void -cpBodyPushArbiter(cpBody *body, cpArbiter *arb) -{ - cpAssertSoft(cpArbiterThreadForBody(arb, body)->next == NULL, "Internal Error: Dangling contact graph pointers detected. (A)"); - cpAssertSoft(cpArbiterThreadForBody(arb, body)->prev == NULL, "Internal Error: Dangling contact graph pointers detected. (B)"); - - cpArbiter *next = body->arbiterList; - cpAssertSoft(next == NULL || cpArbiterThreadForBody(next, body)->prev == NULL, "Internal Error: Dangling contact graph pointers detected. (C)"); - cpArbiterThreadForBody(arb, body)->next = next; - - if(next) cpArbiterThreadForBody(next, body)->prev = arb; - body->arbiterList = arb; -} - -static inline void -ComponentAdd(cpBody *root, cpBody *body){ - body->sleeping.root = root; - - if(body != root){ - body->sleeping.next = root->sleeping.next; - root->sleeping.next = body; - } -} - -static inline void -FloodFillComponent(cpBody *root, cpBody *body) -{ - // Kinematic bodies cannot be put to sleep and prevent bodies they are touching from sleeping. - // Static bodies are effectively sleeping all the time. - if(cpBodyGetType(body) == CP_BODY_TYPE_DYNAMIC){ - cpBody *other_root = ComponentRoot(body); - if(other_root == NULL){ - ComponentAdd(root, body); - CP_BODY_FOREACH_ARBITER(body, arb) FloodFillComponent(root, (body == arb->body_a ? arb->body_b : arb->body_a)); - CP_BODY_FOREACH_CONSTRAINT(body, constraint) FloodFillComponent(root, (body == constraint->a ? constraint->b : constraint->a)); - } else { - cpAssertSoft(other_root == root, "Internal Error: Inconsistency dectected in the contact graph."); - } - } -} - -static inline cpBool -ComponentActive(cpBody *root, cpFloat threshold) -{ - CP_BODY_FOREACH_COMPONENT(root, body){ - if(body->sleeping.idleTime < threshold) return cpTrue; - } - - return cpFalse; -} - -void -cpSpaceProcessComponents(cpSpace *space, cpFloat dt) -{ - cpBool sleep = (space->sleepTimeThreshold != INFINITY); - cpArray *bodies = space->dynamicBodies; - -#ifndef NDEBUG - for(int i=0; inum; i++){ - cpBody *body = (cpBody*)bodies->arr[i]; - - cpAssertSoft(body->sleeping.next == NULL, "Internal Error: Dangling next pointer detected in contact graph."); - cpAssertSoft(body->sleeping.root == NULL, "Internal Error: Dangling root pointer detected in contact graph."); - } -#endif - - // Calculate the kinetic energy of all the bodies. - if(sleep){ - cpFloat dv = space->idleSpeedThreshold; - cpFloat dvsq = (dv ? dv*dv : cpvlengthsq(space->gravity)*dt*dt); - - // update idling and reset component nodes - for(int i=0; inum; i++){ - cpBody *body = (cpBody*)bodies->arr[i]; - - // TODO should make a separate array for kinematic bodies. - if(cpBodyGetType(body) != CP_BODY_TYPE_DYNAMIC) continue; - - // Need to deal with infinite mass objects - cpFloat keThreshold = (dvsq ? body->m*dvsq : 0.0f); - body->sleeping.idleTime = (cpBodyKineticEnergy(body) > keThreshold ? 0.0f : body->sleeping.idleTime + dt); - } - } - - // Awaken any sleeping bodies found and then push arbiters to the bodies' lists. - cpArray *arbiters = space->arbiters; - for(int i=0, count=arbiters->num; iarr[i]; - cpBody *a = arb->body_a, *b = arb->body_b; - - if(sleep){ - // TODO checking cpBodyIsSleepin() redundant? - if(cpBodyGetType(b) == CP_BODY_TYPE_KINEMATIC || cpBodyIsSleeping(a)) cpBodyActivate(a); - if(cpBodyGetType(a) == CP_BODY_TYPE_KINEMATIC || cpBodyIsSleeping(b)) cpBodyActivate(b); - } - - cpBodyPushArbiter(a, arb); - cpBodyPushArbiter(b, arb); - } - - if(sleep){ - // Bodies should be held active if connected by a joint to a kinematic. - cpArray *constraints = space->constraints; - for(int i=0; inum; i++){ - cpConstraint *constraint = (cpConstraint *)constraints->arr[i]; - cpBody *a = constraint->a, *b = constraint->b; - - if(cpBodyGetType(b) == CP_BODY_TYPE_KINEMATIC) cpBodyActivate(a); - if(cpBodyGetType(a) == CP_BODY_TYPE_KINEMATIC) cpBodyActivate(b); - } - - // Generate components and deactivate sleeping ones - for(int i=0; inum;){ - cpBody *body = (cpBody*)bodies->arr[i]; - - if(ComponentRoot(body) == NULL){ - // Body not in a component yet. Perform a DFS to flood fill mark - // the component in the contact graph using this body as the root. - FloodFillComponent(body, body); - - // Check if the component should be put to sleep. - if(!ComponentActive(body, space->sleepTimeThreshold)){ - cpArrayPush(space->sleepingComponents, body); - CP_BODY_FOREACH_COMPONENT(body, other) cpSpaceDeactivateBody(space, other); - - // cpSpaceDeactivateBody() removed the current body from the list. - // Skip incrementing the index counter. - continue; - } - } - - i++; - - // Only sleeping bodies retain their component node pointers. - body->sleeping.root = NULL; - body->sleeping.next = NULL; - } - } -} - -void -cpBodySleep(cpBody *body) -{ - cpBodySleepWithGroup(body, NULL); -} - -void -cpBodySleepWithGroup(cpBody *body, cpBody *group){ - cpAssertHard(cpBodyGetType(body) == CP_BODY_TYPE_DYNAMIC, "Non-dynamic bodies cannot be put to sleep."); - - cpSpace *space = body->space; - cpAssertHard(!cpSpaceIsLocked(space), "Bodies cannot be put to sleep during a query or a call to cpSpaceStep(). Put these calls into a post-step callback."); - cpAssertHard(cpSpaceGetSleepTimeThreshold(space) < INFINITY, "Sleeping is not enabled on the space. You cannot sleep a body without setting a sleep time threshold on the space."); - cpAssertHard(group == NULL || cpBodyIsSleeping(group), "Cannot use a non-sleeping body as a group identifier."); - - if(cpBodyIsSleeping(body)){ - cpAssertHard(ComponentRoot(body) == ComponentRoot(group), "The body is already sleeping and it's group cannot be reassigned."); - return; - } - - CP_BODY_FOREACH_SHAPE(body, shape) cpShapeCacheBB(shape); - cpSpaceDeactivateBody(space, body); - - if(group){ - cpBody *root = ComponentRoot(group); - - body->sleeping.root = root; - body->sleeping.next = root->sleeping.next; - body->sleeping.idleTime = 0.0f; - - root->sleeping.next = body; - } else { - body->sleeping.root = body; - body->sleeping.next = NULL; - body->sleeping.idleTime = 0.0f; - - cpArrayPush(space->sleepingComponents, body); - } - - cpArrayDeleteObj(space->dynamicBodies, body); -} diff --git a/3rdparty/chipmunk/src/cpSpaceDebug.c b/3rdparty/chipmunk/src/cpSpaceDebug.c deleted file mode 100644 index 6b80894b0f18..000000000000 --- a/3rdparty/chipmunk/src/cpSpaceDebug.c +++ /dev/null @@ -1,187 +0,0 @@ -/* Copyright (c) 2013 Scott Lembcke and Howling Moon Software - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -#include "chipmunk/chipmunk_private.h" - -#ifndef CP_SPACE_DISABLE_DEBUG_API - -static void -cpSpaceDebugDrawShape(cpShape *shape, cpSpaceDebugDrawOptions *options) -{ - cpBody *body = shape->body; - cpDataPointer data = options->data; - - cpSpaceDebugColor outline_color = options->shapeOutlineColor; - cpSpaceDebugColor fill_color = options->colorForShape(shape, data); - - switch(shape->klass->type){ - case CP_CIRCLE_SHAPE: { - cpCircleShape *circle = (cpCircleShape *)shape; - options->drawCircle(circle->tc, body->a, circle->r, outline_color, fill_color, data); - break; - } - case CP_SEGMENT_SHAPE: { - cpSegmentShape *seg = (cpSegmentShape *)shape; - options->drawFatSegment(seg->ta, seg->tb, seg->r, outline_color, fill_color, data); - break; - } - case CP_POLY_SHAPE: { - cpPolyShape *poly = (cpPolyShape *)shape; - - int count = poly->count; - struct cpSplittingPlane *planes = poly->planes; - cpVect *verts = (cpVect *)alloca(count*sizeof(cpVect)); - - for(int i=0; idrawPolygon(count, verts, poly->r, outline_color, fill_color, data); - break; - } - default: break; - } -} - -static const cpVect spring_verts[] = { - {0.00f, 0.0f}, - {0.20f, 0.0f}, - {0.25f, 3.0f}, - {0.30f,-6.0f}, - {0.35f, 6.0f}, - {0.40f,-6.0f}, - {0.45f, 6.0f}, - {0.50f,-6.0f}, - {0.55f, 6.0f}, - {0.60f,-6.0f}, - {0.65f, 6.0f}, - {0.70f,-3.0f}, - {0.75f, 6.0f}, - {0.80f, 0.0f}, - {1.00f, 0.0f}, -}; -static const int spring_count = sizeof(spring_verts)/sizeof(cpVect); - -static void -cpSpaceDebugDrawConstraint(cpConstraint *constraint, cpSpaceDebugDrawOptions *options) -{ - cpDataPointer data = options->data; - cpSpaceDebugColor color = options->constraintColor; - - cpBody *body_a = constraint->a; - cpBody *body_b = constraint->b; - - if(cpConstraintIsPinJoint(constraint)){ - cpPinJoint *joint = (cpPinJoint *)constraint; - - cpVect a = cpTransformPoint(body_a->transform, joint->anchorA); - cpVect b = cpTransformPoint(body_b->transform, joint->anchorB); - - options->drawDot(5, a, color, data); - options->drawDot(5, b, color, data); - options->drawSegment(a, b, color, data); - } else if(cpConstraintIsSlideJoint(constraint)){ - cpSlideJoint *joint = (cpSlideJoint *)constraint; - - cpVect a = cpTransformPoint(body_a->transform, joint->anchorA); - cpVect b = cpTransformPoint(body_b->transform, joint->anchorB); - - options->drawDot(5, a, color, data); - options->drawDot(5, b, color, data); - options->drawSegment(a, b, color, data); - } else if(cpConstraintIsPivotJoint(constraint)){ - cpPivotJoint *joint = (cpPivotJoint *)constraint; - - cpVect a = cpTransformPoint(body_a->transform, joint->anchorA); - cpVect b = cpTransformPoint(body_b->transform, joint->anchorB); - - options->drawDot(5, a, color, data); - options->drawDot(5, b, color, data); - } else if(cpConstraintIsGrooveJoint(constraint)){ - cpGrooveJoint *joint = (cpGrooveJoint *)constraint; - - cpVect a = cpTransformPoint(body_a->transform, joint->grv_a); - cpVect b = cpTransformPoint(body_a->transform, joint->grv_b); - cpVect c = cpTransformPoint(body_b->transform, joint->anchorB); - - options->drawDot(5, c, color, data); - options->drawSegment(a, b, color, data); - } else if(cpConstraintIsDampedSpring(constraint)){ - cpDampedSpring *spring = (cpDampedSpring *)constraint; - - cpVect a = cpTransformPoint(body_a->transform, spring->anchorA); - cpVect b = cpTransformPoint(body_b->transform, spring->anchorB); - - options->drawDot(5, a, color, data); - options->drawDot(5, b, color, data); - - cpVect delta = cpvsub(b, a); - cpFloat cos = delta.x; - cpFloat sin = delta.y; - cpFloat s = 1.0f/cpvlength(delta); - - cpVect r1 = cpv(cos, -sin*s); - cpVect r2 = cpv(sin, cos*s); - - cpVect *verts = (cpVect *)alloca(spring_count*sizeof(cpVect)); - for(int i=0; idrawSegment(verts[i], verts[i + 1], color, data); - } - } -} - -void -cpSpaceDebugDraw(cpSpace *space, cpSpaceDebugDrawOptions *options) -{ - if(options->flags & CP_SPACE_DEBUG_DRAW_SHAPES){ - cpSpaceEachShape(space, (cpSpaceShapeIteratorFunc)cpSpaceDebugDrawShape, options); - } - - if(options->flags & CP_SPACE_DEBUG_DRAW_CONSTRAINTS){ - cpSpaceEachConstraint(space, (cpSpaceConstraintIteratorFunc)cpSpaceDebugDrawConstraint, options); - } - - if(options->flags & CP_SPACE_DEBUG_DRAW_COLLISION_POINTS){ - cpArray *arbiters = space->arbiters; - cpSpaceDebugColor color = options->collisionPointColor; - cpSpaceDebugDrawSegmentImpl draw_seg = options->drawSegment; - cpDataPointer data = options->data; - - for(int i=0; inum; i++){ - cpArbiter *arb = (cpArbiter*)arbiters->arr[i]; - cpVect n = arb->n; - - for(int j=0; jcount; j++){ - cpVect p1 = cpvadd(arb->body_a->p, arb->contacts[j].r1); - cpVect p2 = cpvadd(arb->body_b->p, arb->contacts[j].r2); - - cpFloat d = 2.0f; - cpVect a = cpvadd(p1, cpvmult(n, -d)); - cpVect b = cpvadd(p2, cpvmult(n, d)); - draw_seg(a, b, color, data); - } - } - } -} - -#endif diff --git a/3rdparty/chipmunk/src/cpSpaceHash.c b/3rdparty/chipmunk/src/cpSpaceHash.c deleted file mode 100644 index 556c8d38dc2c..000000000000 --- a/3rdparty/chipmunk/src/cpSpaceHash.c +++ /dev/null @@ -1,634 +0,0 @@ -/* Copyright (c) 2013 Scott Lembcke and Howling Moon Software - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -#include "chipmunk/chipmunk_private.h" -#include "prime.h" - -typedef struct cpSpaceHashBin cpSpaceHashBin; -typedef struct cpHandle cpHandle; - -struct cpSpaceHash { - cpSpatialIndex spatialIndex; - - int numcells; - cpFloat celldim; - - cpSpaceHashBin **table; - cpHashSet *handleSet; - - cpSpaceHashBin *pooledBins; - cpArray *pooledHandles; - cpArray *allocatedBuffers; - - cpTimestamp stamp; -}; - - -//MARK: Handle Functions - -struct cpHandle { - void *obj; - int retain; - cpTimestamp stamp; -}; - -static cpHandle* -cpHandleInit(cpHandle *hand, void *obj) -{ - hand->obj = obj; - hand->retain = 0; - hand->stamp = 0; - - return hand; -} - -static inline void cpHandleRetain(cpHandle *hand){hand->retain++;} - -static inline void -cpHandleRelease(cpHandle *hand, cpArray *pooledHandles) -{ - hand->retain--; - if(hand->retain == 0) cpArrayPush(pooledHandles, hand); -} - -static int handleSetEql(void *obj, cpHandle *hand){return (obj == hand->obj);} - -static void * -handleSetTrans(void *obj, cpSpaceHash *hash) -{ - if(hash->pooledHandles->num == 0){ - // handle pool is exhausted, make more - int count = CP_BUFFER_BYTES/sizeof(cpHandle); - cpAssertHard(count, "Internal Error: Buffer size is too small."); - - cpHandle *buffer = (cpHandle *)cpcalloc(1, CP_BUFFER_BYTES); - cpArrayPush(hash->allocatedBuffers, buffer); - - for(int i=0; ipooledHandles, buffer + i); - } - - cpHandle *hand = cpHandleInit((cpHandle *)cpArrayPop(hash->pooledHandles), obj); - cpHandleRetain(hand); - - return hand; -} - -//MARK: Bin Functions - -struct cpSpaceHashBin { - cpHandle *handle; - cpSpaceHashBin *next; -}; - -static inline void -recycleBin(cpSpaceHash *hash, cpSpaceHashBin *bin) -{ - bin->next = hash->pooledBins; - hash->pooledBins = bin; -} - -static inline void -clearTableCell(cpSpaceHash *hash, int idx) -{ - cpSpaceHashBin *bin = hash->table[idx]; - while(bin){ - cpSpaceHashBin *next = bin->next; - - cpHandleRelease(bin->handle, hash->pooledHandles); - recycleBin(hash, bin); - - bin = next; - } - - hash->table[idx] = NULL; -} - -static void -clearTable(cpSpaceHash *hash) -{ - for(int i=0; inumcells; i++) clearTableCell(hash, i); -} - -// Get a recycled or new bin. -static inline cpSpaceHashBin * -getEmptyBin(cpSpaceHash *hash) -{ - cpSpaceHashBin *bin = hash->pooledBins; - - if(bin){ - hash->pooledBins = bin->next; - return bin; - } else { - // Pool is exhausted, make more - int count = CP_BUFFER_BYTES/sizeof(cpSpaceHashBin); - cpAssertHard(count, "Internal Error: Buffer size is too small."); - - cpSpaceHashBin *buffer = (cpSpaceHashBin *)cpcalloc(1, CP_BUFFER_BYTES); - cpArrayPush(hash->allocatedBuffers, buffer); - - // push all but the first one, return the first instead - for(int i=1; itable); - - hash->numcells = numcells; - hash->table = (cpSpaceHashBin **)cpcalloc(numcells, sizeof(cpSpaceHashBin *)); -} - -static inline cpSpatialIndexClass *Klass(void); - -cpSpatialIndex * -cpSpaceHashInit(cpSpaceHash *hash, cpFloat celldim, int numcells, cpSpatialIndexBBFunc bbfunc, cpSpatialIndex *staticIndex) -{ - cpSpatialIndexInit((cpSpatialIndex *)hash, Klass(), bbfunc, staticIndex); - - cpSpaceHashAllocTable(hash, next_prime(numcells)); - hash->celldim = celldim; - - hash->handleSet = cpHashSetNew(0, (cpHashSetEqlFunc)handleSetEql); - - hash->pooledHandles = cpArrayNew(0); - - hash->pooledBins = NULL; - hash->allocatedBuffers = cpArrayNew(0); - - hash->stamp = 1; - - return (cpSpatialIndex *)hash; -} - -cpSpatialIndex * -cpSpaceHashNew(cpFloat celldim, int cells, cpSpatialIndexBBFunc bbfunc, cpSpatialIndex *staticIndex) -{ - return cpSpaceHashInit(cpSpaceHashAlloc(), celldim, cells, bbfunc, staticIndex); -} - -static void -cpSpaceHashDestroy(cpSpaceHash *hash) -{ - if(hash->table) clearTable(hash); - cpfree(hash->table); - - cpHashSetFree(hash->handleSet); - - cpArrayFreeEach(hash->allocatedBuffers, cpfree); - cpArrayFree(hash->allocatedBuffers); - cpArrayFree(hash->pooledHandles); -} - -//MARK: Helper Functions - -static inline cpBool -containsHandle(cpSpaceHashBin *bin, cpHandle *hand) -{ - while(bin){ - if(bin->handle == hand) return cpTrue; - bin = bin->next; - } - - return cpFalse; -} - -// The hash function itself. -static inline cpHashValue -hash_func(cpHashValue x, cpHashValue y, cpHashValue n) -{ - return (x*1640531513ul ^ y*2654435789ul) % n; -} - -// Much faster than (int)floor(f) -// Profiling showed floor() to be a sizable performance hog -static inline int -floor_int(cpFloat f) -{ - int i = (int)f; - return (f < 0.0f && f != i ? i - 1 : i); -} - -static inline void -hashHandle(cpSpaceHash *hash, cpHandle *hand, cpBB bb) -{ - // Find the dimensions in cell coordinates. - cpFloat dim = hash->celldim; - int l = floor_int(bb.l/dim); // Fix by ShiftZ - int r = floor_int(bb.r/dim); - int b = floor_int(bb.b/dim); - int t = floor_int(bb.t/dim); - - int n = hash->numcells; - for(int i=l; i<=r; i++){ - for(int j=b; j<=t; j++){ - cpHashValue idx = hash_func(i,j,n); - cpSpaceHashBin *bin = hash->table[idx]; - - // Don't add an object twice to the same cell. - if(containsHandle(bin, hand)) continue; - - cpHandleRetain(hand); - // Insert a new bin for the handle in this cell. - cpSpaceHashBin *newBin = getEmptyBin(hash); - newBin->handle = hand; - newBin->next = bin; - hash->table[idx] = newBin; - } - } -} - -//MARK: Basic Operations - -static void -cpSpaceHashInsert(cpSpaceHash *hash, void *obj, cpHashValue hashid) -{ - cpHandle *hand = (cpHandle *)cpHashSetInsert(hash->handleSet, hashid, obj, (cpHashSetTransFunc)handleSetTrans, hash); - hashHandle(hash, hand, hash->spatialIndex.bbfunc(obj)); -} - -static void -cpSpaceHashRehashObject(cpSpaceHash *hash, void *obj, cpHashValue hashid) -{ - cpHandle *hand = (cpHandle *)cpHashSetRemove(hash->handleSet, hashid, obj); - - if(hand){ - hand->obj = NULL; - cpHandleRelease(hand, hash->pooledHandles); - - cpSpaceHashInsert(hash, obj, hashid); - } -} - -static void -rehash_helper(cpHandle *hand, cpSpaceHash *hash) -{ - hashHandle(hash, hand, hash->spatialIndex.bbfunc(hand->obj)); -} - -static void -cpSpaceHashRehash(cpSpaceHash *hash) -{ - clearTable(hash); - cpHashSetEach(hash->handleSet, (cpHashSetIteratorFunc)rehash_helper, hash); -} - -static void -cpSpaceHashRemove(cpSpaceHash *hash, void *obj, cpHashValue hashid) -{ - cpHandle *hand = (cpHandle *)cpHashSetRemove(hash->handleSet, hashid, obj); - - if(hand){ - hand->obj = NULL; - cpHandleRelease(hand, hash->pooledHandles); - } -} - -typedef struct eachContext { - cpSpatialIndexIteratorFunc func; - void *data; -} eachContext; - -static void eachHelper(cpHandle *hand, eachContext *context){context->func(hand->obj, context->data);} - -static void -cpSpaceHashEach(cpSpaceHash *hash, cpSpatialIndexIteratorFunc func, void *data) -{ - eachContext context = {func, data}; - cpHashSetEach(hash->handleSet, (cpHashSetIteratorFunc)eachHelper, &context); -} - -static void -remove_orphaned_handles(cpSpaceHash *hash, cpSpaceHashBin **bin_ptr) -{ - cpSpaceHashBin *bin = *bin_ptr; - while(bin){ - cpHandle *hand = bin->handle; - cpSpaceHashBin *next = bin->next; - - if(!hand->obj){ - // orphaned handle, unlink and recycle the bin - (*bin_ptr) = bin->next; - recycleBin(hash, bin); - - cpHandleRelease(hand, hash->pooledHandles); - } else { - bin_ptr = &bin->next; - } - - bin = next; - } -} - -//MARK: Query Functions - -static inline void -query_helper(cpSpaceHash *hash, cpSpaceHashBin **bin_ptr, void *obj, cpSpatialIndexQueryFunc func, void *data) -{ - restart: - for(cpSpaceHashBin *bin = *bin_ptr; bin; bin = bin->next){ - cpHandle *hand = bin->handle; - void *other = hand->obj; - - if(hand->stamp == hash->stamp || obj == other){ - continue; - } else if(other){ - func(obj, other, 0, data); - hand->stamp = hash->stamp; - } else { - // The object for this handle has been removed - // cleanup this cell and restart the query - remove_orphaned_handles(hash, bin_ptr); - goto restart; // GCC not smart enough/able to tail call an inlined function. - } - } -} - -static void -cpSpaceHashQuery(cpSpaceHash *hash, void *obj, cpBB bb, cpSpatialIndexQueryFunc func, void *data) -{ - // Get the dimensions in cell coordinates. - cpFloat dim = hash->celldim; - int l = floor_int(bb.l/dim); // Fix by ShiftZ - int r = floor_int(bb.r/dim); - int b = floor_int(bb.b/dim); - int t = floor_int(bb.t/dim); - - int n = hash->numcells; - cpSpaceHashBin **table = hash->table; - - // Iterate over the cells and query them. - for(int i=l; i<=r; i++){ - for(int j=b; j<=t; j++){ - query_helper(hash, &table[hash_func(i,j,n)], obj, func, data); - } - } - - hash->stamp++; -} - -// Similar to struct eachPair above. -typedef struct queryRehashContext { - cpSpaceHash *hash; - cpSpatialIndexQueryFunc func; - void *data; -} queryRehashContext; - -// Hashset iterator func used with cpSpaceHashQueryRehash(). -static void -queryRehash_helper(cpHandle *hand, queryRehashContext *context) -{ - cpSpaceHash *hash = context->hash; - cpSpatialIndexQueryFunc func = context->func; - void *data = context->data; - - cpFloat dim = hash->celldim; - int n = hash->numcells; - - void *obj = hand->obj; - cpBB bb = hash->spatialIndex.bbfunc(obj); - - int l = floor_int(bb.l/dim); - int r = floor_int(bb.r/dim); - int b = floor_int(bb.b/dim); - int t = floor_int(bb.t/dim); - - cpSpaceHashBin **table = hash->table; - - for(int i=l; i<=r; i++){ - for(int j=b; j<=t; j++){ - cpHashValue idx = hash_func(i,j,n); - cpSpaceHashBin *bin = table[idx]; - - if(containsHandle(bin, hand)) continue; - - cpHandleRetain(hand); // this MUST be done first in case the object is removed in func() - query_helper(hash, &bin, obj, func, data); - - cpSpaceHashBin *newBin = getEmptyBin(hash); - newBin->handle = hand; - newBin->next = bin; - table[idx] = newBin; - } - } - - // Increment the stamp for each object hashed. - hash->stamp++; -} - -static void -cpSpaceHashReindexQuery(cpSpaceHash *hash, cpSpatialIndexQueryFunc func, void *data) -{ - clearTable(hash); - - queryRehashContext context = {hash, func, data}; - cpHashSetEach(hash->handleSet, (cpHashSetIteratorFunc)queryRehash_helper, &context); - - cpSpatialIndexCollideStatic((cpSpatialIndex *)hash, hash->spatialIndex.staticIndex, func, data); -} - -static inline cpFloat -segmentQuery_helper(cpSpaceHash *hash, cpSpaceHashBin **bin_ptr, void *obj, cpSpatialIndexSegmentQueryFunc func, void *data) -{ - cpFloat t = 1.0f; - - restart: - for(cpSpaceHashBin *bin = *bin_ptr; bin; bin = bin->next){ - cpHandle *hand = bin->handle; - void *other = hand->obj; - - // Skip over certain conditions - if(hand->stamp == hash->stamp){ - continue; - } else if(other){ - t = cpfmin(t, func(obj, other, data)); - hand->stamp = hash->stamp; - } else { - // The object for this handle has been removed - // cleanup this cell and restart the query - remove_orphaned_handles(hash, bin_ptr); - goto restart; // GCC not smart enough/able to tail call an inlined function. - } - } - - return t; -} - -// modified from http://playtechs.blogspot.com/2007/03/raytracing-on-grid.html -static void -cpSpaceHashSegmentQuery(cpSpaceHash *hash, void *obj, cpVect a, cpVect b, cpFloat t_exit, cpSpatialIndexSegmentQueryFunc func, void *data) -{ - a = cpvmult(a, 1.0f/hash->celldim); - b = cpvmult(b, 1.0f/hash->celldim); - - int cell_x = floor_int(a.x), cell_y = floor_int(a.y); - - cpFloat t = 0; - - int x_inc, y_inc; - cpFloat temp_v, temp_h; - - if (b.x > a.x){ - x_inc = 1; - temp_h = (cpffloor(a.x + 1.0f) - a.x); - } else { - x_inc = -1; - temp_h = (a.x - cpffloor(a.x)); - } - - if (b.y > a.y){ - y_inc = 1; - temp_v = (cpffloor(a.y + 1.0f) - a.y); - } else { - y_inc = -1; - temp_v = (a.y - cpffloor(a.y)); - } - - // Division by zero is *very* slow on ARM - cpFloat dx = cpfabs(b.x - a.x), dy = cpfabs(b.y - a.y); - cpFloat dt_dx = (dx ? 1.0f/dx : INFINITY), dt_dy = (dy ? 1.0f/dy : INFINITY); - - // fix NANs in horizontal directions - cpFloat next_h = (temp_h ? temp_h*dt_dx : dt_dx); - cpFloat next_v = (temp_v ? temp_v*dt_dy : dt_dy); - - int n = hash->numcells; - cpSpaceHashBin **table = hash->table; - - while(t < t_exit){ - cpHashValue idx = hash_func(cell_x, cell_y, n); - t_exit = cpfmin(t_exit, segmentQuery_helper(hash, &table[idx], obj, func, data)); - - if (next_v < next_h){ - cell_y += y_inc; - t = next_v; - next_v += dt_dy; - } else { - cell_x += x_inc; - t = next_h; - next_h += dt_dx; - } - } - - hash->stamp++; -} - -//MARK: Misc - -void -cpSpaceHashResize(cpSpaceHash *hash, cpFloat celldim, int numcells) -{ - if(hash->spatialIndex.klass != Klass()){ - cpAssertWarn(cpFalse, "Ignoring cpSpaceHashResize() call to non-cpSpaceHash spatial index."); - return; - } - - clearTable(hash); - - hash->celldim = celldim; - cpSpaceHashAllocTable(hash, next_prime(numcells)); -} - -static int -cpSpaceHashCount(cpSpaceHash *hash) -{ - return cpHashSetCount(hash->handleSet); -} - -static int -cpSpaceHashContains(cpSpaceHash *hash, void *obj, cpHashValue hashid) -{ - return cpHashSetFind(hash->handleSet, hashid, obj) != NULL; -} - -static cpSpatialIndexClass klass = { - (cpSpatialIndexDestroyImpl)cpSpaceHashDestroy, - - (cpSpatialIndexCountImpl)cpSpaceHashCount, - (cpSpatialIndexEachImpl)cpSpaceHashEach, - (cpSpatialIndexContainsImpl)cpSpaceHashContains, - - (cpSpatialIndexInsertImpl)cpSpaceHashInsert, - (cpSpatialIndexRemoveImpl)cpSpaceHashRemove, - - (cpSpatialIndexReindexImpl)cpSpaceHashRehash, - (cpSpatialIndexReindexObjectImpl)cpSpaceHashRehashObject, - (cpSpatialIndexReindexQueryImpl)cpSpaceHashReindexQuery, - - (cpSpatialIndexQueryImpl)cpSpaceHashQuery, - (cpSpatialIndexSegmentQueryImpl)cpSpaceHashSegmentQuery, -}; - -static inline cpSpatialIndexClass *Klass(){return &klass;} - -//MARK: Debug Drawing - -//#define CP_BBTREE_DEBUG_DRAW -#ifdef CP_BBTREE_DEBUG_DRAW -#include "OpenGL/gl.h" -#include "OpenGL/glu.h" -#include - -void -cpSpaceHashRenderDebug(cpSpatialIndex *index) -{ - if(index->klass != &klass){ - cpAssertWarn(cpFalse, "Ignoring cpSpaceHashRenderDebug() call to non-spatial hash spatial index."); - return; - } - - cpSpaceHash *hash = (cpSpaceHash *)index; - cpBB bb = cpBBNew(-320, -240, 320, 240); - - cpFloat dim = hash->celldim; - int n = hash->numcells; - - int l = (int)floor(bb.l/dim); - int r = (int)floor(bb.r/dim); - int b = (int)floor(bb.b/dim); - int t = (int)floor(bb.t/dim); - - for(int i=l; i<=r; i++){ - for(int j=b; j<=t; j++){ - int cell_count = 0; - - int index = hash_func(i,j,n); - for(cpSpaceHashBin *bin = hash->table[index]; bin; bin = bin->next) - cell_count++; - - GLfloat v = 1.0f - (GLfloat)cell_count/10.0f; - glColor3f(v,v,v); - glRectf(i*dim, j*dim, (i + 1)*dim, (j + 1)*dim); - } - } -} -#endif diff --git a/3rdparty/chipmunk/src/cpSpaceQuery.c b/3rdparty/chipmunk/src/cpSpaceQuery.c deleted file mode 100644 index 1ce4a10c15a3..000000000000 --- a/3rdparty/chipmunk/src/cpSpaceQuery.c +++ /dev/null @@ -1,246 +0,0 @@ -/* Copyright (c) 2013 Scott Lembcke and Howling Moon Software - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -#include "chipmunk/chipmunk_private.h" - -//MARK: Nearest Point Query Functions - -struct PointQueryContext { - cpVect point; - cpFloat maxDistance; - cpShapeFilter filter; - cpSpacePointQueryFunc func; -}; - -static cpCollisionID -NearestPointQuery(struct PointQueryContext *context, cpShape *shape, cpCollisionID id, void *data) -{ - if( - !cpShapeFilterReject(shape->filter, context->filter) - ){ - cpPointQueryInfo info; - cpShapePointQuery(shape, context->point, &info); - - if(info.shape && info.distance < context->maxDistance) context->func(shape, info.point, info.distance, info.gradient, data); - } - - return id; -} - -void -cpSpacePointQuery(cpSpace *space, cpVect point, cpFloat maxDistance, cpShapeFilter filter, cpSpacePointQueryFunc func, void *data) -{ - struct PointQueryContext context = {point, maxDistance, filter, func}; - cpBB bb = cpBBNewForCircle(point, cpfmax(maxDistance, 0.0f)); - - cpSpaceLock(space); { - cpSpatialIndexQuery(space->dynamicShapes, &context, bb, (cpSpatialIndexQueryFunc)NearestPointQuery, data); - cpSpatialIndexQuery(space->staticShapes, &context, bb, (cpSpatialIndexQueryFunc)NearestPointQuery, data); - } cpSpaceUnlock(space, cpTrue); -} - -static cpCollisionID -NearestPointQueryNearest(struct PointQueryContext *context, cpShape *shape, cpCollisionID id, cpPointQueryInfo *out) -{ - if( - !cpShapeFilterReject(shape->filter, context->filter) && !shape->sensor - ){ - cpPointQueryInfo info; - cpShapePointQuery(shape, context->point, &info); - - if(info.distance < out->distance) (*out) = info; - } - - return id; -} - -cpShape * -cpSpacePointQueryNearest(cpSpace *space, cpVect point, cpFloat maxDistance, cpShapeFilter filter, cpPointQueryInfo *out) -{ - cpPointQueryInfo info = {NULL, cpvzero, maxDistance, cpvzero}; - if(out){ - (*out) = info; - } else { - out = &info; - } - - struct PointQueryContext context = { - point, maxDistance, - filter, - NULL - }; - - cpBB bb = cpBBNewForCircle(point, cpfmax(maxDistance, 0.0f)); - cpSpatialIndexQuery(space->dynamicShapes, &context, bb, (cpSpatialIndexQueryFunc)NearestPointQueryNearest, out); - cpSpatialIndexQuery(space->staticShapes, &context, bb, (cpSpatialIndexQueryFunc)NearestPointQueryNearest, out); - - return (cpShape *)out->shape; -} - - -//MARK: Segment Query Functions - -struct SegmentQueryContext { - cpVect start, end; - cpFloat radius; - cpShapeFilter filter; - cpSpaceSegmentQueryFunc func; -}; - -static cpFloat -SegmentQuery(struct SegmentQueryContext *context, cpShape *shape, void *data) -{ - cpSegmentQueryInfo info; - - if( - !cpShapeFilterReject(shape->filter, context->filter) && - cpShapeSegmentQuery(shape, context->start, context->end, context->radius, &info) - ){ - context->func(shape, info.point, info.normal, info.alpha, data); - } - - return 1.0f; -} - -void -cpSpaceSegmentQuery(cpSpace *space, cpVect start, cpVect end, cpFloat radius, cpShapeFilter filter, cpSpaceSegmentQueryFunc func, void *data) -{ - struct SegmentQueryContext context = { - start, end, - radius, - filter, - func, - }; - - cpSpaceLock(space); { - cpSpatialIndexSegmentQuery(space->staticShapes, &context, start, end, 1.0f, (cpSpatialIndexSegmentQueryFunc)SegmentQuery, data); - cpSpatialIndexSegmentQuery(space->dynamicShapes, &context, start, end, 1.0f, (cpSpatialIndexSegmentQueryFunc)SegmentQuery, data); - } cpSpaceUnlock(space, cpTrue); -} - -static cpFloat -SegmentQueryFirst(struct SegmentQueryContext *context, cpShape *shape, cpSegmentQueryInfo *out) -{ - cpSegmentQueryInfo info; - - if( - !cpShapeFilterReject(shape->filter, context->filter) && !shape->sensor && - cpShapeSegmentQuery(shape, context->start, context->end, context->radius, &info) && - info.alpha < out->alpha - ){ - (*out) = info; - } - - return out->alpha; -} - -cpShape * -cpSpaceSegmentQueryFirst(cpSpace *space, cpVect start, cpVect end, cpFloat radius, cpShapeFilter filter, cpSegmentQueryInfo *out) -{ - cpSegmentQueryInfo info = {NULL, end, cpvzero, 1.0f}; - if(out){ - (*out) = info; - } else { - out = &info; - } - - struct SegmentQueryContext context = { - start, end, - radius, - filter, - NULL - }; - - cpSpatialIndexSegmentQuery(space->staticShapes, &context, start, end, 1.0f, (cpSpatialIndexSegmentQueryFunc)SegmentQueryFirst, out); - cpSpatialIndexSegmentQuery(space->dynamicShapes, &context, start, end, out->alpha, (cpSpatialIndexSegmentQueryFunc)SegmentQueryFirst, out); - - return (cpShape *)out->shape; -} - -//MARK: BB Query Functions - -struct BBQueryContext { - cpBB bb; - cpShapeFilter filter; - cpSpaceBBQueryFunc func; -}; - -static cpCollisionID -BBQuery(struct BBQueryContext *context, cpShape *shape, cpCollisionID id, void *data) -{ - if( - !cpShapeFilterReject(shape->filter, context->filter) && - cpBBIntersects(context->bb, shape->bb) - ){ - context->func(shape, data); - } - - return id; -} - -void -cpSpaceBBQuery(cpSpace *space, cpBB bb, cpShapeFilter filter, cpSpaceBBQueryFunc func, void *data) -{ - struct BBQueryContext context = {bb, filter, func}; - - cpSpaceLock(space); { - cpSpatialIndexQuery(space->dynamicShapes, &context, bb, (cpSpatialIndexQueryFunc)BBQuery, data); - cpSpatialIndexQuery(space->staticShapes, &context, bb, (cpSpatialIndexQueryFunc)BBQuery, data); - } cpSpaceUnlock(space, cpTrue); -} - -//MARK: Shape Query Functions - -struct ShapeQueryContext { - cpSpaceShapeQueryFunc func; - void *data; - cpBool anyCollision; -}; - -// Callback from the spatial hash. -static cpCollisionID -ShapeQuery(cpShape *a, cpShape *b, cpCollisionID id, struct ShapeQueryContext *context) -{ - if(cpShapeFilterReject(a->filter, b->filter) || a == b) return id; - - cpContactPointSet set = cpShapesCollide(a, b); - if(set.count){ - if(context->func) context->func(b, &set, context->data); - context->anyCollision = !(a->sensor || b->sensor); - } - - return id; -} - -cpBool -cpSpaceShapeQuery(cpSpace *space, cpShape *shape, cpSpaceShapeQueryFunc func, void *data) -{ - cpBody *body = shape->body; - cpBB bb = (body ? cpShapeUpdate(shape, body->transform) : shape->bb); - struct ShapeQueryContext context = {func, data, cpFalse}; - - cpSpaceLock(space); { - cpSpatialIndexQuery(space->dynamicShapes, shape, bb, (cpSpatialIndexQueryFunc)ShapeQuery, &context); - cpSpatialIndexQuery(space->staticShapes, shape, bb, (cpSpatialIndexQueryFunc)ShapeQuery, &context); - } cpSpaceUnlock(space, cpTrue); - - return context.anyCollision; -} diff --git a/3rdparty/chipmunk/src/cpSpaceStep.c b/3rdparty/chipmunk/src/cpSpaceStep.c deleted file mode 100644 index 85cbb3d0c865..000000000000 --- a/3rdparty/chipmunk/src/cpSpaceStep.c +++ /dev/null @@ -1,445 +0,0 @@ -/* Copyright (c) 2013 Scott Lembcke and Howling Moon Software - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -#include "chipmunk/chipmunk_private.h" - -//MARK: Post Step Callback Functions - -cpPostStepCallback * -cpSpaceGetPostStepCallback(cpSpace *space, void *key) -{ - cpArray *arr = space->postStepCallbacks; - for(int i=0; inum; i++){ - cpPostStepCallback *callback = (cpPostStepCallback *)arr->arr[i]; - if(callback && callback->key == key) return callback; - } - - return NULL; -} - -static void PostStepDoNothing(cpSpace *space, void *obj, void *data){} - -cpBool -cpSpaceAddPostStepCallback(cpSpace *space, cpPostStepFunc func, void *key, void *data) -{ - cpAssertWarn(space->locked, - "Adding a post-step callback when the space is not locked is unnecessary. " - "Post-step callbacks will not called until the end of the next call to cpSpaceStep() or the next query."); - - if(!cpSpaceGetPostStepCallback(space, key)){ - cpPostStepCallback *callback = (cpPostStepCallback *)cpcalloc(1, sizeof(cpPostStepCallback)); - callback->func = (func ? func : PostStepDoNothing); - callback->key = key; - callback->data = data; - - cpArrayPush(space->postStepCallbacks, callback); - return cpTrue; - } else { - return cpFalse; - } -} - -//MARK: Locking Functions - -void -cpSpaceLock(cpSpace *space) -{ - space->locked++; -} - -void -cpSpaceUnlock(cpSpace *space, cpBool runPostStep) -{ - space->locked--; - cpAssertHard(space->locked >= 0, "Internal Error: Space lock underflow."); - - if(space->locked == 0){ - cpArray *waking = space->rousedBodies; - - for(int i=0, count=waking->num; iarr[i]); - waking->arr[i] = NULL; - } - - waking->num = 0; - - if(space->locked == 0 && runPostStep && !space->skipPostStep){ - space->skipPostStep = cpTrue; - - cpArray *arr = space->postStepCallbacks; - for(int i=0; inum; i++){ - cpPostStepCallback *callback = (cpPostStepCallback *)arr->arr[i]; - cpPostStepFunc func = callback->func; - - // Mark the func as NULL in case calling it calls cpSpaceRunPostStepCallbacks() again. - // TODO: need more tests around this case I think. - callback->func = NULL; - if(func) func(space, callback->key, callback->data); - - arr->arr[i] = NULL; - cpfree(callback); - } - - arr->num = 0; - space->skipPostStep = cpFalse; - } - } -} - -//MARK: Contact Buffer Functions - -struct cpContactBufferHeader { - cpTimestamp stamp; - cpContactBufferHeader *next; - unsigned int numContacts; -}; - -#define CP_CONTACTS_BUFFER_SIZE ((CP_BUFFER_BYTES - sizeof(cpContactBufferHeader))/sizeof(struct cpContact)) -typedef struct cpContactBuffer { - cpContactBufferHeader header; - struct cpContact contacts[CP_CONTACTS_BUFFER_SIZE]; -} cpContactBuffer; - -static cpContactBufferHeader * -cpSpaceAllocContactBuffer(cpSpace *space) -{ - cpContactBuffer *buffer = (cpContactBuffer *)cpcalloc(1, sizeof(cpContactBuffer)); - cpArrayPush(space->allocatedBuffers, buffer); - return (cpContactBufferHeader *)buffer; -} - -static cpContactBufferHeader * -cpContactBufferHeaderInit(cpContactBufferHeader *header, cpTimestamp stamp, cpContactBufferHeader *splice) -{ - header->stamp = stamp; - header->next = (splice ? splice->next : header); - header->numContacts = 0; - - return header; -} - -void -cpSpacePushFreshContactBuffer(cpSpace *space) -{ - cpTimestamp stamp = space->stamp; - - cpContactBufferHeader *head = space->contactBuffersHead; - - if(!head){ - // No buffers have been allocated, make one - space->contactBuffersHead = cpContactBufferHeaderInit(cpSpaceAllocContactBuffer(space), stamp, NULL); - } else if(stamp - head->next->stamp > space->collisionPersistence){ - // The tail buffer is available, rotate the ring - cpContactBufferHeader *tail = head->next; - space->contactBuffersHead = cpContactBufferHeaderInit(tail, stamp, tail); - } else { - // Allocate a new buffer and push it into the ring - cpContactBufferHeader *buffer = cpContactBufferHeaderInit(cpSpaceAllocContactBuffer(space), stamp, head); - space->contactBuffersHead = head->next = buffer; - } -} - - -struct cpContact * -cpContactBufferGetArray(cpSpace *space) -{ - if(space->contactBuffersHead->numContacts + CP_MAX_CONTACTS_PER_ARBITER > CP_CONTACTS_BUFFER_SIZE){ - // contact buffer could overflow on the next collision, push a fresh one. - cpSpacePushFreshContactBuffer(space); - } - - cpContactBufferHeader *head = space->contactBuffersHead; - return ((cpContactBuffer *)head)->contacts + head->numContacts; -} - -void -cpSpacePushContacts(cpSpace *space, int count) -{ - cpAssertHard(count <= CP_MAX_CONTACTS_PER_ARBITER, "Internal Error: Contact buffer overflow!"); - space->contactBuffersHead->numContacts += count; -} - -static void -cpSpacePopContacts(cpSpace *space, int count){ - space->contactBuffersHead->numContacts -= count; -} - -//MARK: Collision Detection Functions - -static void * -cpSpaceArbiterSetTrans(cpShape **shapes, cpSpace *space) -{ - if(space->pooledArbiters->num == 0){ - // arbiter pool is exhausted, make more - int count = CP_BUFFER_BYTES/sizeof(cpArbiter); - cpAssertHard(count, "Internal Error: Buffer size too small."); - - cpArbiter *buffer = (cpArbiter *)cpcalloc(1, CP_BUFFER_BYTES); - cpArrayPush(space->allocatedBuffers, buffer); - - for(int i=0; ipooledArbiters, buffer + i); - } - - return cpArbiterInit((cpArbiter *)cpArrayPop(space->pooledArbiters), shapes[0], shapes[1]); -} - -static inline cpBool -QueryRejectConstraint(cpBody *a, cpBody *b) -{ - CP_BODY_FOREACH_CONSTRAINT(a, constraint){ - if( - !constraint->collideBodies && ( - (constraint->a == a && constraint->b == b) || - (constraint->a == b && constraint->b == a) - ) - ) return cpTrue; - } - - return cpFalse; -} - -static inline cpBool -QueryReject(cpShape *a, cpShape *b) -{ - return ( - // BBoxes must overlap - !cpBBIntersects(a->bb, b->bb) - // Don't collide shapes attached to the same body. - || a->body == b->body - // Don't collide shapes that are filtered. - || cpShapeFilterReject(a->filter, b->filter) - // Don't collide bodies if they have a constraint with collideBodies == cpFalse. - || QueryRejectConstraint(a->body, b->body) - ); -} - -// Callback from the spatial hash. -cpCollisionID -cpSpaceCollideShapes(cpShape *a, cpShape *b, cpCollisionID id, cpSpace *space) -{ - // Reject any of the simple cases - if(QueryReject(a,b)) return id; - - // Narrow-phase collision detection. - struct cpCollisionInfo info = cpCollide(a, b, id, cpContactBufferGetArray(space)); - - if(info.count == 0) return info.id; // Shapes are not colliding. - cpSpacePushContacts(space, info.count); - - // Get an arbiter from space->arbiterSet for the two shapes. - // This is where the persistant contact magic comes from. - const cpShape *shape_pair[] = {info.a, info.b}; - cpHashValue arbHashID = CP_HASH_PAIR((cpHashValue)info.a, (cpHashValue)info.b); - cpArbiter *arb = (cpArbiter *)cpHashSetInsert(space->cachedArbiters, arbHashID, shape_pair, (cpHashSetTransFunc)cpSpaceArbiterSetTrans, space); - cpArbiterUpdate(arb, &info, space); - - cpCollisionHandler *handler = arb->handler; - - // Call the begin function first if it's the first step - if(arb->state == CP_ARBITER_STATE_FIRST_COLLISION && !handler->beginFunc(arb, space, handler->userData)){ - cpArbiterIgnore(arb); // permanently ignore the collision until separation - } - - if( - // Ignore the arbiter if it has been flagged - (arb->state != CP_ARBITER_STATE_IGNORE) && - // Call preSolve - handler->preSolveFunc(arb, space, handler->userData) && - // Check (again) in case the pre-solve() callback called cpArbiterIgnored(). - arb->state != CP_ARBITER_STATE_IGNORE && - // Process, but don't add collisions for sensors. - !(a->sensor || b->sensor) && - // Don't process collisions between two infinite mass bodies. - // This includes collisions between two kinematic bodies, or a kinematic body and a static body. - !(a->body->m == INFINITY && b->body->m == INFINITY) - ){ - cpArrayPush(space->arbiters, arb); - } else { - cpSpacePopContacts(space, info.count); - - arb->contacts = NULL; - arb->count = 0; - - // Normally arbiters are set as used after calling the post-solve callback. - // However, post-solve() callbacks are not called for sensors or arbiters rejected from pre-solve. - if(arb->state != CP_ARBITER_STATE_IGNORE) arb->state = CP_ARBITER_STATE_NORMAL; - } - - // Time stamp the arbiter so we know it was used recently. - arb->stamp = space->stamp; - return info.id; -} - -// Hashset filter func to throw away old arbiters. -cpBool -cpSpaceArbiterSetFilter(cpArbiter *arb, cpSpace *space) -{ - cpTimestamp ticks = space->stamp - arb->stamp; - - cpBody *a = arb->body_a, *b = arb->body_b; - - // TODO: should make an arbiter state for this so it doesn't require filtering arbiters for dangling body pointers on body removal. - // Preserve arbiters on sensors and rejected arbiters for sleeping objects. - // This prevents errant separate callbacks from happenening. - if( - (cpBodyGetType(a) == CP_BODY_TYPE_STATIC || cpBodyIsSleeping(a)) && - (cpBodyGetType(b) == CP_BODY_TYPE_STATIC || cpBodyIsSleeping(b)) - ){ - return cpTrue; - } - - // Arbiter was used last frame, but not this one - if(ticks >= 1 && arb->state != CP_ARBITER_STATE_CACHED){ - arb->state = CP_ARBITER_STATE_CACHED; - cpCollisionHandler *handler = arb->handler; - handler->separateFunc(arb, space, handler->userData); - } - - if(ticks >= space->collisionPersistence){ - arb->contacts = NULL; - arb->count = 0; - - cpArrayPush(space->pooledArbiters, arb); - return cpFalse; - } - - return cpTrue; -} - -//MARK: All Important cpSpaceStep() Function - - void -cpShapeUpdateFunc(cpShape *shape, void *unused) -{ - cpShapeCacheBB(shape); -} - -void -cpSpaceStep(cpSpace *space, cpFloat dt) -{ - // don't step if the timestep is 0! - if(dt == 0.0f) return; - - space->stamp++; - - cpFloat prev_dt = space->curr_dt; - space->curr_dt = dt; - - cpArray *bodies = space->dynamicBodies; - cpArray *constraints = space->constraints; - cpArray *arbiters = space->arbiters; - - // Reset and empty the arbiter lists. - for(int i=0; inum; i++){ - cpArbiter *arb = (cpArbiter *)arbiters->arr[i]; - arb->state = CP_ARBITER_STATE_NORMAL; - - // If both bodies are awake, unthread the arbiter from the contact graph. - if(!cpBodyIsSleeping(arb->body_a) && !cpBodyIsSleeping(arb->body_b)){ - cpArbiterUnthread(arb); - } - } - arbiters->num = 0; - - cpSpaceLock(space); { - // Integrate positions - for(int i=0; inum; i++){ - cpBody *body = (cpBody *)bodies->arr[i]; - body->position_func(body, dt); - } - - // Find colliding pairs. - cpSpacePushFreshContactBuffer(space); - cpSpatialIndexEach(space->dynamicShapes, (cpSpatialIndexIteratorFunc)cpShapeUpdateFunc, NULL); - cpSpatialIndexReindexQuery(space->dynamicShapes, (cpSpatialIndexQueryFunc)cpSpaceCollideShapes, space); - } cpSpaceUnlock(space, cpFalse); - - // Rebuild the contact graph (and detect sleeping components if sleeping is enabled) - cpSpaceProcessComponents(space, dt); - - cpSpaceLock(space); { - // Clear out old cached arbiters and call separate callbacks - cpHashSetFilter(space->cachedArbiters, (cpHashSetFilterFunc)cpSpaceArbiterSetFilter, space); - - // Prestep the arbiters and constraints. - cpFloat slop = space->collisionSlop; - cpFloat biasCoef = 1.0f - cpfpow(space->collisionBias, dt); - for(int i=0; inum; i++){ - cpArbiterPreStep((cpArbiter *)arbiters->arr[i], dt, slop, biasCoef); - } - - for(int i=0; inum; i++){ - cpConstraint *constraint = (cpConstraint *)constraints->arr[i]; - - cpConstraintPreSolveFunc preSolve = constraint->preSolve; - if(preSolve) preSolve(constraint, space); - - constraint->klass->preStep(constraint, dt); - } - - // Integrate velocities. - cpFloat damping = cpfpow(space->damping, dt); - cpVect gravity = space->gravity; - for(int i=0; inum; i++){ - cpBody *body = (cpBody *)bodies->arr[i]; - body->velocity_func(body, gravity, damping, dt); - } - - // Apply cached impulses - cpFloat dt_coef = (prev_dt == 0.0f ? 0.0f : dt/prev_dt); - for(int i=0; inum; i++){ - cpArbiterApplyCachedImpulse((cpArbiter *)arbiters->arr[i], dt_coef); - } - - for(int i=0; inum; i++){ - cpConstraint *constraint = (cpConstraint *)constraints->arr[i]; - constraint->klass->applyCachedImpulse(constraint, dt_coef); - } - - // Run the impulse solver. - for(int i=0; iiterations; i++){ - for(int j=0; jnum; j++){ - cpArbiterApplyImpulse((cpArbiter *)arbiters->arr[j]); - } - - for(int j=0; jnum; j++){ - cpConstraint *constraint = (cpConstraint *)constraints->arr[j]; - constraint->klass->applyImpulse(constraint, dt); - } - } - - // Run the constraint post-solve callbacks - for(int i=0; inum; i++){ - cpConstraint *constraint = (cpConstraint *)constraints->arr[i]; - - cpConstraintPostSolveFunc postSolve = constraint->postSolve; - if(postSolve) postSolve(constraint, space); - } - - // run the post-solve callbacks - for(int i=0; inum; i++){ - cpArbiter *arb = (cpArbiter *) arbiters->arr[i]; - - cpCollisionHandler *handler = arb->handler; - handler->postSolveFunc(arb, space, handler->userData); - } - } cpSpaceUnlock(space, cpTrue); -} diff --git a/3rdparty/chipmunk/src/cpSpatialIndex.c b/3rdparty/chipmunk/src/cpSpatialIndex.c deleted file mode 100644 index 3fb7cb5d9113..000000000000 --- a/3rdparty/chipmunk/src/cpSpatialIndex.c +++ /dev/null @@ -1,69 +0,0 @@ -/* Copyright (c) 2013 Scott Lembcke and Howling Moon Software - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -#include "chipmunk/chipmunk_private.h" - -void -cpSpatialIndexFree(cpSpatialIndex *index) -{ - if(index){ - cpSpatialIndexDestroy(index); - cpfree(index); - } -} - -cpSpatialIndex * -cpSpatialIndexInit(cpSpatialIndex *index, cpSpatialIndexClass *klass, cpSpatialIndexBBFunc bbfunc, cpSpatialIndex *staticIndex) -{ - index->klass = klass; - index->bbfunc = bbfunc; - index->staticIndex = staticIndex; - - if(staticIndex){ - cpAssertHard(!staticIndex->dynamicIndex, "This static index is already associated with a dynamic index."); - staticIndex->dynamicIndex = index; - } - - return index; -} - -typedef struct dynamicToStaticContext { - cpSpatialIndexBBFunc bbfunc; - cpSpatialIndex *staticIndex; - cpSpatialIndexQueryFunc queryFunc; - void *data; -} dynamicToStaticContext; - -static void -dynamicToStaticIter(void *obj, dynamicToStaticContext *context) -{ - cpSpatialIndexQuery(context->staticIndex, obj, context->bbfunc(obj), context->queryFunc, context->data); -} - -void -cpSpatialIndexCollideStatic(cpSpatialIndex *dynamicIndex, cpSpatialIndex *staticIndex, cpSpatialIndexQueryFunc func, void *data) -{ - if(staticIndex && cpSpatialIndexCount(staticIndex) > 0){ - dynamicToStaticContext context = {dynamicIndex->bbfunc, staticIndex, func, data}; - cpSpatialIndexEach(dynamicIndex, (cpSpatialIndexIteratorFunc)dynamicToStaticIter, &context); - } -} - diff --git a/3rdparty/chipmunk/src/cpSweep1D.c b/3rdparty/chipmunk/src/cpSweep1D.c deleted file mode 100644 index 94c4e2255c6f..000000000000 --- a/3rdparty/chipmunk/src/cpSweep1D.c +++ /dev/null @@ -1,254 +0,0 @@ -/* Copyright (c) 2013 Scott Lembcke and Howling Moon Software - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -#include "chipmunk/chipmunk_private.h" - -static inline cpSpatialIndexClass *Klass(void); - -//MARK: Basic Structures - -typedef struct Bounds { - cpFloat min, max; -} Bounds; - -typedef struct TableCell { - void *obj; - Bounds bounds; -} TableCell; - -struct cpSweep1D -{ - cpSpatialIndex spatialIndex; - - int num; - int max; - TableCell *table; -}; - -static inline cpBool -BoundsOverlap(Bounds a, Bounds b) -{ - return (a.min <= b.max && b.min <= a.max); -} - -static inline Bounds -BBToBounds(cpSweep1D *sweep, cpBB bb) -{ - Bounds bounds = {bb.l, bb.r}; - return bounds; -} - -static inline TableCell -MakeTableCell(cpSweep1D *sweep, void *obj) -{ - TableCell cell = {obj, BBToBounds(sweep, sweep->spatialIndex.bbfunc(obj))}; - return cell; -} - -//MARK: Memory Management Functions - -cpSweep1D * -cpSweep1DAlloc(void) -{ - return (cpSweep1D *)cpcalloc(1, sizeof(cpSweep1D)); -} - -static void -ResizeTable(cpSweep1D *sweep, int size) -{ - sweep->max = size; - sweep->table = (TableCell *)cprealloc(sweep->table, size*sizeof(TableCell)); -} - -cpSpatialIndex * -cpSweep1DInit(cpSweep1D *sweep, cpSpatialIndexBBFunc bbfunc, cpSpatialIndex *staticIndex) -{ - cpSpatialIndexInit((cpSpatialIndex *)sweep, Klass(), bbfunc, staticIndex); - - sweep->num = 0; - ResizeTable(sweep, 32); - - return (cpSpatialIndex *)sweep; -} - -cpSpatialIndex * -cpSweep1DNew(cpSpatialIndexBBFunc bbfunc, cpSpatialIndex *staticIndex) -{ - return cpSweep1DInit(cpSweep1DAlloc(), bbfunc, staticIndex); -} - -static void -cpSweep1DDestroy(cpSweep1D *sweep) -{ - cpfree(sweep->table); - sweep->table = NULL; -} - -//MARK: Misc - -static int -cpSweep1DCount(cpSweep1D *sweep) -{ - return sweep->num; -} - -static void -cpSweep1DEach(cpSweep1D *sweep, cpSpatialIndexIteratorFunc func, void *data) -{ - TableCell *table = sweep->table; - for(int i=0, count=sweep->num; itable; - for(int i=0, count=sweep->num; inum == sweep->max) ResizeTable(sweep, sweep->max*2); - - sweep->table[sweep->num] = MakeTableCell(sweep, obj); - sweep->num++; -} - -static void -cpSweep1DRemove(cpSweep1D *sweep, void *obj, cpHashValue hashid) -{ - TableCell *table = sweep->table; - for(int i=0, count=sweep->num; inum; - - table[i] = table[num]; - table[num].obj = NULL; - - return; - } - } -} - -//MARK: Reindexing Functions - -static void -cpSweep1DReindexObject(cpSweep1D *sweep, void *obj, cpHashValue hashid) -{ - // Nothing to do here -} - -static void -cpSweep1DReindex(cpSweep1D *sweep) -{ - // Nothing to do here - // Could perform a sort, but queries are not accelerated anyway. -} - -//MARK: Query Functions - -static void -cpSweep1DQuery(cpSweep1D *sweep, void *obj, cpBB bb, cpSpatialIndexQueryFunc func, void *data) -{ - // Implementing binary search here would allow you to find an upper limit - // but not a lower limit. Probably not worth the hassle. - - Bounds bounds = BBToBounds(sweep, bb); - - TableCell *table = sweep->table; - for(int i=0, count=sweep->num; itable; - for(int i=0, count=sweep->num; ibounds.min < b->bounds.min ? -1 : (a->bounds.min > b->bounds.min ? 1 : 0)); -} - -static void -cpSweep1DReindexQuery(cpSweep1D *sweep, cpSpatialIndexQueryFunc func, void *data) -{ - TableCell *table = sweep->table; - int count = sweep->num; - - // Update bounds and sort - for(int i=0; ispatialIndex.staticIndex, func, data); -} - -static cpSpatialIndexClass klass = { - (cpSpatialIndexDestroyImpl)cpSweep1DDestroy, - - (cpSpatialIndexCountImpl)cpSweep1DCount, - (cpSpatialIndexEachImpl)cpSweep1DEach, - (cpSpatialIndexContainsImpl)cpSweep1DContains, - - (cpSpatialIndexInsertImpl)cpSweep1DInsert, - (cpSpatialIndexRemoveImpl)cpSweep1DRemove, - - (cpSpatialIndexReindexImpl)cpSweep1DReindex, - (cpSpatialIndexReindexObjectImpl)cpSweep1DReindexObject, - (cpSpatialIndexReindexQueryImpl)cpSweep1DReindexQuery, - - (cpSpatialIndexQueryImpl)cpSweep1DQuery, - (cpSpatialIndexSegmentQueryImpl)cpSweep1DSegmentQuery, -}; - -static inline cpSpatialIndexClass *Klass(){return &klass;} - diff --git a/3rdparty/chipmunk/src/prime.h b/3rdparty/chipmunk/src/prime.h deleted file mode 100644 index d470c2cdd76d..000000000000 --- a/3rdparty/chipmunk/src/prime.h +++ /dev/null @@ -1,68 +0,0 @@ -/* Copyright (c) 2013 Scott Lembcke and Howling Moon Software - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -// Used for resizing hash tables. -// Values approximately double. -// http://planetmath.org/encyclopedia/GoodHashTablePrimes.html -static int primes[] = { - 5, - 13, - 23, - 47, - 97, - 193, - 389, - 769, - 1543, - 3079, - 6151, - 12289, - 24593, - 49157, - 98317, - 196613, - 393241, - 786433, - 1572869, - 3145739, - 6291469, - 12582917, - 25165843, - 50331653, - 100663319, - 201326611, - 402653189, - 805306457, - 1610612741, - 0, -}; - -static inline int -next_prime(int n) -{ - int i = 0; - while(n > primes[i]){ - i++; - cpAssertHard(primes[i], "Tried to resize a hash table to a size greater than 1610612741 O_o"); // realistically this should never happen - } - - return primes[i]; -} diff --git a/core/2d/Node.cpp b/core/2d/Node.cpp index 674bcfa09e72..3ec2a65eecaf 100644 --- a/core/2d/Node.cpp +++ b/core/2d/Node.cpp @@ -125,7 +125,7 @@ Node::Node() , _onExitCallback(nullptr) , _onEnterTransitionDidFinishCallback(nullptr) , _onExitTransitionDidStartCallback(nullptr) -#if defined(AX_ENABLE_PHYSICS) +#if defined(AX_ENABLE_PHYSICS) && 0 , _physicsBody(nullptr) #endif { diff --git a/core/2d/Node.h b/core/2d/Node.h index 008251a0cd9b..a95b50e464d2 100644 --- a/core/2d/Node.h +++ b/core/2d/Node.h @@ -41,7 +41,7 @@ #include "2d/ComponentContainer.h" #include "2d/Component.h" -#if defined(AX_ENABLE_PHYSICS) +#if defined(AX_ENABLE_PHYSICS) && 0 # include "physics/PhysicsBody.h" #endif @@ -2026,7 +2026,7 @@ class AX_DLL Node : public Object backend::ProgramState* _programState = nullptr; // Physics:remaining backwardly compatible -#if defined(AX_ENABLE_PHYSICS) +#if defined(AX_ENABLE_PHYSICS) && 0 PhysicsBody* _physicsBody; public: diff --git a/core/2d/Scene.cpp b/core/2d/Scene.cpp index 1f80eff0782a..631f171dcb71 100644 --- a/core/2d/Scene.cpp +++ b/core/2d/Scene.cpp @@ -35,11 +35,11 @@ THE SOFTWARE. #include "base/UTF8.h" #include "renderer/Renderer.h" -#if defined(AX_ENABLE_PHYSICS) +#if defined(AX_ENABLE_PHYSICS) && 0 # include "physics/PhysicsWorld.h" #endif -#if defined(AX_ENABLE_3D_PHYSICS) && AX_ENABLE_BULLET_INTEGRATION +#if defined(AX_ENABLE_3D_PHYSICS) # include "physics3d/Physics3DWorld.h" # include "physics3d/Physics3DComponent.h" #endif @@ -66,7 +66,7 @@ Scene::Scene() Scene::~Scene() { -#if defined(AX_ENABLE_3D_PHYSICS) && AX_ENABLE_BULLET_INTEGRATION +#if defined(AX_ENABLE_3D_PHYSICS) AX_SAFE_RELEASE(_physics3DWorld); AX_SAFE_RELEASE(_physics3dDebugCamera); #endif @@ -233,7 +233,7 @@ void Scene::render(Renderer* renderer, const Mat4& eyeTransform, const Mat4* eye // camera->setNodeToParentTransform(eyeCopy); } -#if defined(AX_ENABLE_3D_PHYSICS) && AX_ENABLE_BULLET_INTEGRATION +#if defined(AX_ENABLE_3D_PHYSICS) if (_physics3DWorld && _physics3DWorld->isDebugDrawEnabled()) { Camera* physics3dDebugCamera = _physics3dDebugCamera != nullptr ? _physics3dDebugCamera : defaultCamera; @@ -307,7 +307,7 @@ void Scene::removeAllChildren() } } -#if defined(AX_ENABLE_3D_PHYSICS) && AX_ENABLE_BULLET_INTEGRATION +#if defined(AX_ENABLE_3D_PHYSICS) void Scene::setPhysics3DDebugCamera(Camera* camera) { AX_SAFE_RETAIN(camera); @@ -326,7 +326,7 @@ void Scene::setNavMeshDebugCamera(Camera* camera) #endif -#if (defined(AX_ENABLE_PHYSICS) || (defined(AX_ENABLE_3D_PHYSICS) && AX_ENABLE_BULLET_INTEGRATION)) +#if defined(AX_ENABLE_PHYSICS) || defined(AX_ENABLE_3D_PHYSICS) Scene* Scene::createWithPhysics() { @@ -351,7 +351,7 @@ bool Scene::initWithPhysics() bool Scene::initPhysicsWorld() { -# if defined(AX_ENABLE_PHYSICS) +# if defined(AX_ENABLE_PHYSICS) && 0 _physicsWorld = PhysicsWorld::construct(this); # endif @@ -360,7 +360,7 @@ bool Scene::initPhysicsWorld() { this->setContentSize(_director->getWinSize()); -# if defined(AX_ENABLE_3D_PHYSICS) && AX_ENABLE_BULLET_INTEGRATION +# if defined(AX_ENABLE_3D_PHYSICS) Physics3DWorldDes info; AX_BREAK_IF(!(_physics3DWorld = Physics3DWorld::create(&info))); _physics3DWorld->retain(); @@ -374,15 +374,15 @@ bool Scene::initPhysicsWorld() #endif -#if (defined(AX_ENABLE_PHYSICS) || (defined(AX_ENABLE_3D_PHYSICS) && AX_ENABLE_BULLET_INTEGRATION) || defined(AX_ENABLE_NAVMESH)) +#if defined(AX_ENABLE_PHYSICS) || defined(AX_ENABLE_3D_PHYSICS) || defined(AX_ENABLE_NAVMESH) void Scene::stepPhysicsAndNavigation(float deltaTime) { -# if defined(AX_ENABLE_PHYSICS) +# if defined(AX_ENABLE_PHYSICS) && 0 if (_physicsWorld && _physicsWorld->isAutoStep()) _physicsWorld->update(deltaTime); # endif -# if defined(AX_ENABLE_3D_PHYSICS) && AX_ENABLE_BULLET_INTEGRATION +# if defined(AX_ENABLE_3D_PHYSICS) if (_physics3DWorld) { _physics3DWorld->stepSimulate(deltaTime); diff --git a/core/2d/Scene.h b/core/2d/Scene.h index 573bae39723e..b69e7c84fe56 100644 --- a/core/2d/Scene.h +++ b/core/2d/Scene.h @@ -44,7 +44,7 @@ class EventCustom; #if defined(AX_ENABLE_PHYSICS) class PhysicsWorld; #endif -#if defined(AX_ENABLE_3D_PHYSICS) && AX_ENABLE_BULLET_INTEGRATION +#if defined(AX_ENABLE_3D_PHYSICS) class Physics3DWorld; #endif #if defined(AX_ENABLE_NAVMESH) @@ -157,7 +157,7 @@ class AX_DLL Scene : public Node private: AX_DISALLOW_COPY_AND_ASSIGN(Scene); -#if (AX_ENABLE_PHYSICS || (defined(AX_ENABLE_3D_PHYSICS) && AX_ENABLE_BULLET_INTEGRATION)) +#if AX_ENABLE_PHYSICS || defined(AX_ENABLE_3D_PHYSICS) public: # if defined(AX_ENABLE_PHYSICS) /** Get the physics world of the scene. @@ -167,7 +167,7 @@ class AX_DLL Scene : public Node PhysicsWorld* getPhysicsWorld() const { return _physicsWorld; } # endif -# if defined(AX_ENABLE_3D_PHYSICS) && AX_ENABLE_BULLET_INTEGRATION +# if defined(AX_ENABLE_3D_PHYSICS) /** Get the 3d physics world of the scene. * @return The 3d physics world of the scene. * @js NA @@ -197,7 +197,7 @@ class AX_DLL Scene : public Node PhysicsWorld* _physicsWorld = nullptr; # endif -# if defined(AX_ENABLE_3D_PHYSICS) && AX_ENABLE_BULLET_INTEGRATION +# if defined(AX_ENABLE_3D_PHYSICS) Physics3DWorld* _physics3DWorld = nullptr; Camera* _physics3dDebugCamera = nullptr; # endif @@ -219,7 +219,7 @@ class AX_DLL Scene : public Node Camera* _navMeshDebugCamera = nullptr; #endif -#if (defined(AX_ENABLE_PHYSICS) || (defined(AX_ENABLE_3D_PHYSICS) && AX_ENABLE_BULLET_INTEGRATION) || defined(AX_ENABLE_NAVMESH)) +#if defined(AX_ENABLE_PHYSICS) || defined(AX_ENABLE_3D_PHYSICS) || defined(AX_ENABLE_NAVMESH) public: void stepPhysicsAndNavigation(float deltaTime); #endif diff --git a/core/axmol.h b/core/axmol.h index 3c82b780b802..db2277fc96eb 100644 --- a/core/axmol.h +++ b/core/axmol.h @@ -165,11 +165,11 @@ THE SOFTWARE. #include "renderer/Shaders.h" // physics -#include "physics/PhysicsBody.h" -#include "physics/PhysicsContact.h" -#include "physics/PhysicsJoint.h" -#include "physics/PhysicsShape.h" -#include "physics/PhysicsWorld.h" +// #include "physics/PhysicsBody.h" +// #include "physics/PhysicsContact.h" +// #include "physics/PhysicsJoint.h" +// #include "physics/PhysicsCollider.h" +// #include "physics/PhysicsWorld.h" // platform #include "platform/Common.h" diff --git a/core/base/Config.h b/core/base/Config.h index f2ce9105e13f..d51fb74be64b 100644 --- a/core/base/Config.h +++ b/core/base/Config.h @@ -252,27 +252,6 @@ THE SOFTWARE. # define AX_LUA_ENGINE_DEBUG 0 #endif -/** Use physics integration API. */ -// It works with: -// Chipmunk2D or Box2D -#if defined(AX_ENABLE_PHYSICS) -/** Use Chipmunk2D physics 2d engine on physics integration API. */ -# ifndef AX_ENABLE_CHIPMUNK_INTEGRATION -# define AX_ENABLE_CHIPMUNK_INTEGRATION 0 -# endif -/** or use Box2D physics 2d engine on physics integration API. */ -# ifndef AX_ENABLE_BOX2D_INTEGRATION -# define AX_ENABLE_BOX2D_INTEGRATION 1 -# endif -#endif // defined(AX_ENABLE_PHYSICS) - -#if defined(AX_ENABLE_3D_PHYSICS) -/** Use bullet physics engine. */ -# ifndef AX_ENABLE_BULLET_INTEGRATION -# define AX_ENABLE_BULLET_INTEGRATION 1 -# endif -#endif - /** Use culling or not. */ #ifndef AX_USE_CULLING # define AX_USE_CULLING 1 diff --git a/core/base/Director.cpp b/core/base/Director.cpp index 1ccfcee8f74b..c57d172fbca3 100644 --- a/core/base/Director.cpp +++ b/core/base/Director.cpp @@ -305,8 +305,8 @@ void Director::drawScene() if (_runningScene) { -#if (defined(AX_ENABLE_PHYSICS) || (defined(AX_ENABLE_3D_PHYSICS) && AX_ENABLE_BULLET_INTEGRATION) || \ - defined(AX_ENABLE_NAVMESH)) +#if defined(AX_ENABLE_PHYSICS) || defined(AX_ENABLE_3D_PHYSICS) || \ + defined(AX_ENABLE_NAVMESH) _runningScene->stepPhysicsAndNavigation(_deltaTime); #endif // clear draw stats diff --git a/core/physics/CMakeLists.txt b/core/physics/CMakeLists.txt index 12df34ee9d20..41e69b84df46 100644 --- a/core/physics/CMakeLists.txt +++ b/core/physics/CMakeLists.txt @@ -1,17 +1 @@ -set(_AX_PHYSICS_HEADER - physics/PhysicsContact.h - physics/PhysicsWorld.h - physics/PhysicsBody.h - physics/cpCompat62.h - physics/PhysicsShape.h - physics/PhysicsHelper.h - physics/PhysicsJoint.h - ) - -set(_AX_PHYSICS_SRC - physics/PhysicsBody.cpp - physics/PhysicsContact.cpp - physics/PhysicsJoint.cpp - physics/PhysicsShape.cpp - physics/PhysicsWorld.cpp - ) +# place holder for builtin physics based on box2d v3 \ No newline at end of file diff --git a/core/physics/PhysicsBody.cpp b/core/physics/PhysicsBody.cpp deleted file mode 100644 index bf0f21d7aca8..000000000000 --- a/core/physics/PhysicsBody.cpp +++ /dev/null @@ -1,1027 +0,0 @@ -/**************************************************************************** - Copyright (c) 2013-2016 Chukong Technologies Inc. - Copyright (c) 2017-2018 Xiamen Yaji Software Co., Ltd. - Copyright (c) 2019-present Axmol Engine contributors (see AUTHORS.md). - - https://axmol.dev/ - - 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 "physics/PhysicsBody.h" -#if defined(AX_ENABLE_PHYSICS) - -# include -# include -# include - -# include "chipmunk/chipmunk_private.h" - -# include "2d/Scene.h" -# include "physics/PhysicsShape.h" -# include "physics/PhysicsJoint.h" -# include "physics/PhysicsWorld.h" -# include "physics/PhysicsHelper.h" - -static void internalBodySetMass(cpBody* body, cpFloat mass) -{ - cpBodyActivate(body); - body->m = mass; - body->m_inv = 1.0f / mass; - // cpAssertSaneBody(body); -} - -static void internalBodyUpdateVelocity(cpBody* body, cpVect gravity, cpFloat damping, cpFloat dt) -{ - cpBodyUpdateVelocity(body, cpvzero, damping, dt); - // Skip kinematic bodies. - if (cpBodyGetType(body) == CP_BODY_TYPE_KINEMATIC) - return; - - cpAssertSoft(body->m > 0.0f && body->i > 0.0f, - "Body's mass and moment must be positive to simulate. (Mass: %f Moment: f)", body->m, body->i); - - ax::PhysicsBody* physicsBody = static_cast(body->userData); - - if (physicsBody->isGravityEnabled()) - body->v = - cpvclamp(cpvadd(cpvmult(body->v, damping), cpvmult(cpvadd(gravity, cpvmult(body->f, body->m_inv)), dt)), - physicsBody->getVelocityLimit()); - else - body->v = cpvclamp(cpvadd(cpvmult(body->v, damping), cpvmult(cpvmult(body->f, body->m_inv), dt)), - physicsBody->getVelocityLimit()); - cpFloat w_limit = physicsBody->getAngularVelocityLimit(); - body->w = cpfclamp(body->w * damping + body->t * body->i_inv * dt, -w_limit, w_limit); - - // Reset forces. - body->f = cpvzero; - // to check body sanity - cpBodySetTorque(body, 0.0f); -} - -namespace ax -{ -extern const float PHYSICS_INFINITY; - -const std::string PhysicsBody::COMPONENT_NAME = "PhysicsBody"; - -namespace -{ -static const float MASS_DEFAULT = 1.0; -static const float MOMENT_DEFAULT = 200; -} // namespace - -PhysicsBody::PhysicsBody() - : _world(nullptr) - , _cpBody(nullptr) - , _dynamic(true) - , _rotationEnabled(true) - , _gravityEnabled(true) - , _massDefault(true) - , _momentDefault(true) - , _mass(MASS_DEFAULT) - , _area(0.0f) - , _density(0.0f) - , _moment(MOMENT_DEFAULT) - , _velocityLimit(PHYSICS_INFINITY) - , _angularVelocityLimit(PHYSICS_INFINITY) - , _isDamping(false) - , _linearDamping(0.0f) - , _angularDamping(0.0f) - , _tag(0) - , _massSetByUser(false) - , _momentSetByUser(false) - , _rotationOffset(0) - , _recordedRotation(0.0f) - , _recordedAngle(0.0) - , _recordScaleX(1.f) - , _recordScaleY(1.f) - , _fixedUpdate(false) -{ - _name = COMPONENT_NAME; -} - -PhysicsBody::~PhysicsBody() -{ - for (auto&& joint : _joints) - { - PhysicsBody* other = joint->getBodyA() == this ? joint->getBodyB() : joint->getBodyA(); - other->removeJoint(joint); - delete joint; - } - - if (_cpBody) - { - cpBodyFree(_cpBody); - } -} - -PhysicsBody* PhysicsBody::create() -{ - PhysicsBody* body = new PhysicsBody(); - if (body->init()) - { - body->autorelease(); - return body; - } - - AX_SAFE_DELETE(body); - return nullptr; -} - -PhysicsBody* PhysicsBody::create(float mass) -{ - PhysicsBody* body = new PhysicsBody(); - if (body) - { - body->_mass = mass; - body->_massDefault = false; - if (body->init()) - { - body->autorelease(); - return body; - } - } - - AX_SAFE_DELETE(body); - return nullptr; -} - -PhysicsBody* PhysicsBody::create(float mass, float moment) -{ - PhysicsBody* body = new PhysicsBody(); - if (body) - { - body->_mass = mass; - body->_massDefault = false; - body->_moment = moment; - body->_momentDefault = false; - if (body->init()) - { - body->autorelease(); - return body; - } - } - - AX_SAFE_DELETE(body); - return nullptr; -} - -PhysicsBody* PhysicsBody::createCircle(float radius, const PhysicsMaterial& material, const Vec2& offset) -{ - PhysicsBody* body = new PhysicsBody(); - if (body->init()) - { - body->addShape(PhysicsShapeCircle::create(radius, material, offset)); - body->autorelease(); - return body; - } - - AX_SAFE_DELETE(body); - return nullptr; -} - -PhysicsBody* PhysicsBody::createBox(const Vec2& size, const PhysicsMaterial& material, const Vec2& offset) -{ - PhysicsBody* body = new PhysicsBody(); - if (body->init()) - { - body->addShape(PhysicsShapeBox::create(size, material, offset)); - body->autorelease(); - return body; - } - - AX_SAFE_DELETE(body); - return nullptr; -} - -PhysicsBody* PhysicsBody::createPolygon(const Vec2* points, - int count, - const PhysicsMaterial& material, - const Vec2& offset) -{ - PhysicsBody* body = new PhysicsBody(); - if (body->init()) - { - body->addShape(PhysicsShapePolygon::create(points, count, material, offset)); - body->autorelease(); - return body; - } - - AX_SAFE_DELETE(body); - return nullptr; -} - -PhysicsBody* PhysicsBody::createEdgeSegment(const Vec2& a, - const Vec2& b, - const PhysicsMaterial& material, - float border /* = 1*/) -{ - PhysicsBody* body = new PhysicsBody(); - if (body->init()) - { - body->addShape(PhysicsShapeEdgeSegment::create(a, b, material, border)); - body->setDynamic(false); - body->autorelease(); - return body; - } - - AX_SAFE_DELETE(body); - return nullptr; -} - -PhysicsBody* PhysicsBody::createEdgeBox(const Vec2& size, - const PhysicsMaterial& material, - float border /* = 1*/, - const Vec2& offset) -{ - PhysicsBody* body = new PhysicsBody(); - if (body->init()) - { - body->addShape(PhysicsShapeEdgeBox::create(size, material, border, offset)); - body->setDynamic(false); - body->autorelease(); - return body; - } - - AX_SAFE_DELETE(body); - - return nullptr; -} - -PhysicsBody* PhysicsBody::createEdgePolygon(const Vec2* points, - int count, - const PhysicsMaterial& material, - float border /* = 1*/) -{ - PhysicsBody* body = new PhysicsBody(); - if (body->init()) - { - body->addShape(PhysicsShapeEdgePolygon::create(points, count, material, border)); - body->setDynamic(false); - body->autorelease(); - return body; - } - - AX_SAFE_DELETE(body); - - return nullptr; -} - -PhysicsBody* PhysicsBody::createEdgeChain(const Vec2* points, - int count, - const PhysicsMaterial& material, - float border /* = 1*/) -{ - PhysicsBody* body = new PhysicsBody(); - if (body->init()) - { - body->addShape(PhysicsShapeEdgeChain::create(points, count, material, border)); - body->setDynamic(false); - body->autorelease(); - return body; - } - - AX_SAFE_DELETE(body); - - return nullptr; -} - -bool PhysicsBody::init() -{ - do - { - _cpBody = cpBodyNew(_mass, _moment); - internalBodySetMass(_cpBody, _mass); - cpBodySetUserData(_cpBody, this); - cpBodySetVelocityUpdateFunc(_cpBody, internalBodyUpdateVelocity); - - AX_BREAK_IF(_cpBody == nullptr); - - return true; - } while (false); - - return false; -} - -void PhysicsBody::removeJoint(PhysicsJoint* joint) -{ - auto it = std::find(_joints.begin(), _joints.end(), joint); - - if (it != _joints.end()) - { - _joints.erase(it); - } -} - -void PhysicsBody::setDynamic(bool dynamic) -{ - if (dynamic != _dynamic) - { - _dynamic = dynamic; - if (dynamic) - { - cpBodySetType(_cpBody, CP_BODY_TYPE_DYNAMIC); - internalBodySetMass(_cpBody, _mass); - cpBodySetMoment(_cpBody, _moment); - } - else - { - cpBodySetType(_cpBody, CP_BODY_TYPE_KINEMATIC); - } - } -} - -void PhysicsBody::setRotationEnable(bool enable) -{ - if (_rotationEnabled != enable) - { - cpBodySetMoment(_cpBody, enable ? _moment : PHYSICS_INFINITY); - _rotationEnabled = enable; - } -} - -void PhysicsBody::setGravityEnable(bool enable) -{ - _gravityEnabled = enable; -} - -void PhysicsBody::setRotation(float rotation) -{ - _recordedRotation = rotation; - _recordedAngle = -(rotation + _rotationOffset) * (M_PI / 180.0); - cpBodySetAngle(_cpBody, _recordedAngle); -} - -void PhysicsBody::setScale(float scaleX, float scaleY) -{ - for (auto&& shape : _shapes) - { - _area -= shape->getArea(); - if (!_massSetByUser) - addMass(-shape->getMass()); - if (!_momentSetByUser) - addMoment(-shape->getMoment()); - - shape->setScale(scaleX, scaleY); - - _area += shape->getArea(); - if (!_massSetByUser) - addMass(shape->getMass()); - if (!_momentSetByUser) - addMoment(shape->getMoment()); - } -} - -void PhysicsBody::setPosition(float positionX, float positionY) -{ - cpVect tt; - - tt.x = positionX + _positionOffset.x; - tt.y = positionY + _positionOffset.y; - - cpBodySetPosition(_cpBody, tt); -} - -Vec2 PhysicsBody::getPosition() const -{ - cpVect tt = cpBodyGetPosition(_cpBody); - return Vec2(tt.x - _positionOffset.x, tt.y - _positionOffset.y); -} - -void PhysicsBody::setPositionOffset(const Vec2& position) -{ - if (!_positionOffset.equals(position)) - { - Vec2 pos = getPosition(); - _positionOffset = position; - setPosition(pos.x, pos.y); - } -} - -float PhysicsBody::getRotation() -{ - if (_recordedAngle != cpBodyGetAngle(_cpBody)) - { - _recordedAngle = cpBodyGetAngle(_cpBody); - _recordedRotation = -_recordedAngle * 180.0 / M_PI - _rotationOffset; - } - return _recordedRotation; -} - -PhysicsShape* PhysicsBody::addShape(PhysicsShape* shape, bool addMassAndMoment /* = true*/) -{ - if (shape == nullptr) - return nullptr; - - // add shape to body - if (_shapes.getIndex(shape) == -1) - { - shape->setBody(this); - - // calculate the area, mass, and density - // area must update before mass, because the density changes depend on it. - if (addMassAndMoment) - { - _area += shape->getArea(); - addMass(shape->getMass()); - addMoment(shape->getMoment()); - } - - if (_world && cpBodyGetSpace(_cpBody)) - { - _world->addShape(shape); - } - - _shapes.pushBack(shape); - } - - return shape; -} - -void PhysicsBody::applyForce(const Vec2& force, const Vec2& offset) -{ - if (_dynamic && _mass != PHYSICS_INFINITY) - { - cpBodyApplyForceAtLocalPoint(_cpBody, PhysicsHelper::vec22cpv(force), PhysicsHelper::vec22cpv(offset)); - } -} - -void PhysicsBody::resetForces() -{ - cpBodySetForce(_cpBody, PhysicsHelper::vec22cpv(Vec2(0, 0))); -} - -void PhysicsBody::applyImpulse(const Vec2& impulse, const Vec2& offset) -{ - cpBodyApplyImpulseAtLocalPoint(_cpBody, PhysicsHelper::vec22cpv(impulse), PhysicsHelper::vec22cpv(offset)); -} - -void PhysicsBody::applyTorque(float torque) -{ - cpBodySetTorque(_cpBody, torque); -} - -void PhysicsBody::setMass(float mass) -{ - if (mass <= 0) - { - return; - } - _mass = mass; - _massDefault = false; - _massSetByUser = true; - - // update density - if (_mass == PHYSICS_INFINITY) - { - _density = PHYSICS_INFINITY; - } - else - { - if (_area > 0) - { - _density = _mass / _area; - } - else - { - _density = 0; - } - } - - // the static body's mass and moment is always infinity - if (_dynamic) - { - internalBodySetMass(_cpBody, _mass); - } -} - -void PhysicsBody::addMass(float mass) -{ - if (mass == PHYSICS_INFINITY) - { - _mass = PHYSICS_INFINITY; - _massDefault = false; - _density = PHYSICS_INFINITY; - } - else if (mass == -PHYSICS_INFINITY) - { - return; - } - else - { - if (_massDefault) - { - _mass = 0; - _massDefault = false; - } - - if (_mass + mass > 0) - { - _mass += mass; - } - else - { - _mass = MASS_DEFAULT; - _massDefault = true; - } - - if (_area > 0) - { - _density = _mass / _area; - } - else - { - _density = 0; - } - } - - // the static body's mass and moment is always infinity - if (_dynamic) - { - internalBodySetMass(_cpBody, _mass); - } -} - -void PhysicsBody::addMoment(float moment) -{ - if (moment == PHYSICS_INFINITY) - { - // if moment is PHYSICS_INFINITY, the moment of the body will become PHYSICS_INFINITY - _moment = PHYSICS_INFINITY; - _momentDefault = false; - } - else if (moment == -PHYSICS_INFINITY) - { - return; - } - else - { - // if moment of the body is PHYSICS_INFINITY is has no effect - if (_moment != PHYSICS_INFINITY) - { - if (_momentDefault) - { - _moment = 0; - _momentDefault = false; - } - - if (_moment + moment > 0) - { - _moment += moment; - } - else - { - _moment = MOMENT_DEFAULT; - _momentDefault = true; - } - } - } - - // the static body's mass and moment is always infinity - if (_rotationEnabled && _dynamic) - { - cpBodySetMoment(_cpBody, _moment); - } -} - -void PhysicsBody::setVelocity(const Vec2& velocity) -{ - if (cpBodyGetType(_cpBody) == CP_BODY_TYPE_STATIC) - { - AXLOGD("physics warning: you can't set velocity for a static body."); - return; - } - - cpBodySetVelocity(_cpBody, PhysicsHelper::vec22cpv(velocity)); -} - -Vec2 PhysicsBody::getVelocity() -{ - return PhysicsHelper::cpv2vec2(cpBodyGetVelocity(_cpBody)); -} - -Vec2 PhysicsBody::getVelocityAtLocalPoint(const Vec2& point) -{ - return PhysicsHelper::cpv2vec2(cpBodyGetVelocityAtLocalPoint(_cpBody, PhysicsHelper::vec22cpv(point))); -} - -Vec2 PhysicsBody::getVelocityAtWorldPoint(const Vec2& point) -{ - return PhysicsHelper::cpv2vec2(cpBodyGetVelocityAtWorldPoint(_cpBody, PhysicsHelper::vec22cpv(point))); -} - -void PhysicsBody::setAngularVelocity(float velocity) -{ - if (cpBodyGetType(_cpBody) == CP_BODY_TYPE_STATIC) - { - AXLOGD("physics warning: you can't set angular velocity for a static body."); - return; - } - - cpBodySetAngularVelocity(_cpBody, velocity); -} - -float PhysicsBody::getAngularVelocity() -{ - return PhysicsHelper::cpfloat2float(cpBodyGetAngularVelocity(_cpBody)); -} - -void PhysicsBody::setVelocityLimit(float limit) -{ - _velocityLimit = limit; -} - -float PhysicsBody::getVelocityLimit() -{ - return _velocityLimit; -} - -void PhysicsBody::setAngularVelocityLimit(float limit) -{ - _angularVelocityLimit = limit; -} - -float PhysicsBody::getAngularVelocityLimit() -{ - return _angularVelocityLimit; -} - -void PhysicsBody::setMoment(float moment) -{ - _moment = moment; - _momentDefault = false; - _momentSetByUser = true; - - // the static body's mass and moment is always infinity - if (_rotationEnabled && _dynamic) - { - cpBodySetMoment(_cpBody, _moment); - } -} - -PhysicsShape* PhysicsBody::getShape(int tag) const -{ - for (auto&& shape : _shapes) - { - if (shape->getTag() == tag) - { - return shape; - } - } - - return nullptr; -} - -void PhysicsBody::removeShape(int tag, bool reduceMassAndMoment /* = true*/) -{ - for (auto&& shape : _shapes) - { - if (shape->getTag() == tag) - { - removeShape(shape, reduceMassAndMoment); - return; - } - } -} - -void PhysicsBody::removeShape(PhysicsShape* shape, bool reduceMassAndMoment /* = true*/) -{ - if (_shapes.getIndex(shape) != -1) - { - // deduce the area, mass and moment - // area must update before mass, because the density changes depend on it. - if (reduceMassAndMoment) - { - _area -= shape->getArea(); - addMass(-shape->getMass()); - addMoment(-shape->getMoment()); - } - - // remove - if (_world) - { - _world->removeShape(shape); - } - - // set shape->_body = nullptr make the shape->setBody will not trigger the _body->removeShape function call. - shape->_body = nullptr; - shape->setBody(nullptr); - _shapes.eraseObject(shape); - } -} - -void PhysicsBody::removeAllShapes(bool reduceMassAndMoment /* = true*/) -{ - for (auto&& child : _shapes) - { - PhysicsShape* shape = dynamic_cast(child); - - // deduce the area, mass and moment - // area must update before mass, because the density changes depend on it. - if (reduceMassAndMoment) - { - _area -= shape->getArea(); - addMass(-shape->getMass()); - addMoment(-shape->getMoment()); - } - - if (_world) - { - _world->removeShape(shape); - } - - // set shape->_body = nullptr make the shape->setBody will not trigger the _body->removeShape function call. - shape->_body = nullptr; - shape->setBody(nullptr); - } - - _shapes.clear(); -} - -void PhysicsBody::removeFromWorld() -{ - removeFromPhysicsWorld(); -} - -void PhysicsBody::setEnabled(bool enable) -{ - if (_enabled != enable) - { - _enabled = enable; - - if (_world) - { - if (enable) - { - _world->addBodyOrDelay(this); - } - else - { - _world->removeBodyOrDelay(this); - } - } - } -} - -bool PhysicsBody::isResting() const -{ - return cpBodyIsSleeping(_cpBody) != cpFalse; -} - -void PhysicsBody::setResting(bool rest) const -{ - if (rest && !isResting()) - { - cpBodySleep(_cpBody); - } - else if (!rest && isResting()) - { - cpBodyActivate(_cpBody); - } -} - -void PhysicsBody::update(float delta) -{ - // damping compute - if (!_fixedUpdate && _isDamping && _dynamic && !isResting()) - { - _cpBody->v.x *= cpfclamp(1.0f - delta * _linearDamping, 0.0f, 1.0f); - _cpBody->v.y *= cpfclamp(1.0f - delta * _linearDamping, 0.0f, 1.0f); - _cpBody->w *= cpfclamp(1.0f - delta * _angularDamping, 0.0f, 1.0f); - } -} - -void PhysicsBody::fixedUpdate(float delta) -{ - if (_fixedUpdate && _isDamping && _dynamic && !isResting()) - { - _cpBody->v.x *= cpfclamp(1.0f - delta * _linearDamping, 0.0f, 1.0f); - _cpBody->v.y *= cpfclamp(1.0f - delta * _linearDamping, 0.0f, 1.0f); - _cpBody->w *= cpfclamp(1.0f - delta * _angularDamping, 0.0f, 1.0f); - } -} - -void PhysicsBody::setCategoryBitmask(int bitmask) -{ - for (auto&& shape : _shapes) - { - shape->setCategoryBitmask(bitmask); - } -} - -int PhysicsBody::getCategoryBitmask() const -{ - if (!_shapes.empty()) - { - return _shapes.front()->getCategoryBitmask(); - } - else - { - return UINT_MAX; - } -} - -void PhysicsBody::setContactTestBitmask(int bitmask) -{ - for (auto&& shape : _shapes) - { - shape->setContactTestBitmask(bitmask); - } -} - -int PhysicsBody::getContactTestBitmask() const -{ - if (!_shapes.empty()) - { - return _shapes.front()->getContactTestBitmask(); - } - else - { - return 0x00000000; - } -} - -void PhysicsBody::setCollisionBitmask(int bitmask) -{ - for (auto&& shape : _shapes) - { - shape->setCollisionBitmask(bitmask); - } -} - -int PhysicsBody::getCollisionBitmask() const -{ - if (!_shapes.empty()) - { - return _shapes.front()->getCollisionBitmask(); - } - else - { - return UINT_MAX; - } -} - -void PhysicsBody::setGroup(int group) -{ - for (auto&& shape : _shapes) - { - shape->setGroup(group); - } -} - -int PhysicsBody::getGroup() const -{ - if (!_shapes.empty()) - { - return _shapes.front()->getGroup(); - } - else - { - return 0; - } -} - -void PhysicsBody::setRotationOffset(float rotation) -{ - if (std::abs(_rotationOffset - rotation) > 0.5f) - { - float rot = getRotation(); - _rotationOffset = rotation; - setRotation(rot); - } -} - -Vec2 PhysicsBody::world2Local(const Vec2& point) -{ - return PhysicsHelper::cpv2vec2(cpBodyWorldToLocal(_cpBody, PhysicsHelper::vec22cpv(point))); -} - -Vec2 PhysicsBody::local2World(const Vec2& point) -{ - return PhysicsHelper::cpv2vec2(cpBodyLocalToWorld(_cpBody, PhysicsHelper::vec22cpv(point))); -} - -void PhysicsBody::beforeSimulation(const Mat4& parentToWorldTransform, - const Mat4& nodeToWorldTransform, - float scaleX, - float scaleY, - float rotation) -{ - if (_recordScaleX != scaleX || _recordScaleY != scaleY) - { - _recordScaleX = scaleX; - _recordScaleY = scaleY; - setScale(scaleX, scaleY); - } - - // set rotation - if (_recordedRotation != rotation) - { - setRotation(rotation); - } - - // set position - auto worldPosition = _ownerCenterOffset; - nodeToWorldTransform.transformVector(worldPosition.x, worldPosition.y, worldPosition.z, 1.f, &worldPosition); - setPosition(worldPosition.x, worldPosition.y); - - _recordPosX = worldPosition.x; - _recordPosY = worldPosition.y; - - if (_owner->getAnchorPoint() != Vec2::ANCHOR_MIDDLE) - { - parentToWorldTransform.getInversed().transformVector(worldPosition.x, worldPosition.y, worldPosition.z, 1.f, - &worldPosition); - _offset.x = worldPosition.x - _owner->getPositionX(); - _offset.y = worldPosition.y - _owner->getPositionY(); - } -} - -void PhysicsBody::afterSimulation(const Mat4& parentToWorldTransform, float parentRotation) -{ - // set Node position - auto tmp = getPosition(); - Vec3 positionInParent(tmp.x, tmp.y, 0.f); - if (_recordPosX != positionInParent.x || _recordPosY != positionInParent.y) - { - parentToWorldTransform.getInversed().transformVector(positionInParent.x, positionInParent.y, positionInParent.z, - 1.f, &positionInParent); - _owner->setPosition(positionInParent.x - _offset.x, positionInParent.y - _offset.y); - } - - // set Node rotation - _owner->setRotation(getRotation() - parentRotation); -} - -void PhysicsBody::onEnter() -{ - addToPhysicsWorld(); -} - -void PhysicsBody::onExit() -{ - removeFromPhysicsWorld(); -} - -void PhysicsBody::onAdd() -{ - _owner->_physicsBody = this; - auto contentSize = _owner->getContentSize(); - _ownerCenterOffset.x = 0.5f * contentSize.width; - _ownerCenterOffset.y = 0.5f * contentSize.height; - - setRotationOffset(_owner->getRotation()); - - // component may be added after onEnter() has been invoked, so we should add - // this line to make sure physics body is added to physics world - addToPhysicsWorld(); -} - -void PhysicsBody::onRemove() -{ - AXASSERT(_owner != nullptr, "_owner can't be nullptr"); - - removeFromPhysicsWorld(); - - _owner->_physicsBody = nullptr; -} - -void PhysicsBody::addToPhysicsWorld() -{ - if (_owner) - { - auto scene = _owner->getScene(); - if (scene) - scene->getPhysicsWorld()->addBody(this); - } -} - -void PhysicsBody::removeFromPhysicsWorld() -{ - if (_owner) - { - auto scene = _owner->getScene(); - if (scene) - scene->getPhysicsWorld()->removeBody(this); - } -} - -} - -#endif // defined(AX_ENABLE_PHYSICS) diff --git a/core/physics/PhysicsBody.h b/core/physics/PhysicsBody.h deleted file mode 100644 index 528f169eb8ef..000000000000 --- a/core/physics/PhysicsBody.h +++ /dev/null @@ -1,630 +0,0 @@ -/**************************************************************************** - Copyright (c) 2013-2016 Chukong Technologies Inc. - Copyright (c) 2017-2018 Xiamen Yaji Software Co., Ltd. - Copyright (c) 2019-present Axmol Engine contributors (see AUTHORS.md). - - https://axmol.dev/ - - 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 __CCPHYSICS_BODY_H__ -#define __CCPHYSICS_BODY_H__ - -#include "base/Config.h" -#if defined(AX_ENABLE_PHYSICS) - -# include "2d/Component.h" -# include "math/Math.h" -# include "physics/PhysicsShape.h" -# include "base/Vector.h" - -struct cpBody; - -namespace ax -{ - -class Node; -class PhysicsWorld; -class PhysicsJoint; - -const PhysicsMaterial PHYSICSBODY_MATERIAL_DEFAULT(0.1f, 0.5f, 0.5f); - -/** - * @addtogroup physics - * @{ - * @addtogroup physics_2d - * @{ - */ - -/** - * A body affect by physics. - * - * It can attach one or more shapes. - * If you create body with createXXX, it will automatically compute mass and moment with density your specified(which is - * PHYSICSBODY_MATERIAL_DEFAULT by default, and the density value is 0.1f), and it based on the formula: mass = density - * * area. If you create body with createEdgeXXX, the mass and moment will be PHYSICS_INFINITY by default. And it's a - * static body. You can change mass and moment with setMass() and setMoment(). And you can change the body to be dynamic - * or static by use function setDynamic(). - */ -class AX_DLL PhysicsBody : public Component -{ -public: - const static std::string COMPONENT_NAME; - - /** - * Create a body with default mass and moment. - * - * This default mass value is 1.0. - * This default moment value is 200. - * @return An autoreleased PhysicsBody object pointer. - */ - static PhysicsBody* create(); - - /** - * Create a body with mass and default moment. - * - * @param mass This body's mass. - * @return An autoreleased PhysicsBody object pointer. - */ - static PhysicsBody* create(float mass); - - /** - * Create a body with mass and moment. - * - * @param mass This body's mass. - * @param moment This body's moment. - * @return An autoreleased PhysicsBody object pointer. - */ - static PhysicsBody* create(float mass, float moment); - - /** - * Create a body contains a circle. - * - * @param radius A float number, it is the circle's radius. - * @param material A PhysicsMaterial object, the default value is PHYSICSSHAPE_MATERIAL_DEFAULT. - * @param offset A Vec2 object, it is the offset from the body's center of gravity in body local coordinates. - * @return An autoreleased PhysicsBody object pointer. - */ - static PhysicsBody* createCircle(float radius, - const PhysicsMaterial& material = PHYSICSBODY_MATERIAL_DEFAULT, - const Vec2& offset = Vec2::ZERO); - /** - * Create a body contains a box shape. - * - * @param size Size contains this box's width and height. - * @param material A PhysicsMaterial object, the default value is PHYSICSSHAPE_MATERIAL_DEFAULT. - * @param offset A Vec2 object, it is the offset from the body's center of gravity in body local coordinates. - * @return An autoreleased PhysicsBody object pointer. - */ - static PhysicsBody* createBox(const Vec2& size, - const PhysicsMaterial& material = PHYSICSBODY_MATERIAL_DEFAULT, - const Vec2& offset = Vec2::ZERO); - - /** - * @brief Create a body contains a polygon shape. - * - * @param points Points is an array of Vec2 structs defining a convex hull with a clockwise winding. - * @param count An integer number, contains the count of the points array. - * @param material A PhysicsMaterial object, the default value is PHYSICSSHAPE_MATERIAL_DEFAULT. - * @param offset A Vec2 object, it is the offset from the body's center of gravity in body local coordinates. - * @return An autoreleased PhysicsBody object pointer. - */ - static PhysicsBody* createPolygon(const Vec2* points, - int count, - const PhysicsMaterial& material = PHYSICSBODY_MATERIAL_DEFAULT, - const Vec2& offset = Vec2::ZERO); - - /** - * Create a body contains a EdgeSegment shape. - * - * @param a It's the edge's begin position. - * @param b It's the edge's end position. - * @param material A PhysicsMaterial object, the default value is PHYSICSSHAPE_MATERIAL_DEFAULT. - * @param border It's a edge's border width. - * @return An autoreleased PhysicsBody object pointer. - */ - static PhysicsBody* createEdgeSegment(const Vec2& a, - const Vec2& b, - const PhysicsMaterial& material = PHYSICSBODY_MATERIAL_DEFAULT, - float border = 1); - - /** - * Create a body contains a EdgeBox shape. - * @param size The size contains this box's width and height. - * @param material A PhysicsMaterial object, the default value is PHYSICSSHAPE_MATERIAL_DEFAULT. - * @param border It's a edge's border width. - * @param offset A Vec2 object, it is the offset from the body's center of gravity in body local coordinates. - * @return An autoreleased PhysicsBody object pointer. - */ - static PhysicsBody* createEdgeBox(const Vec2& size, - const PhysicsMaterial& material = PHYSICSBODY_MATERIAL_DEFAULT, - float border = 1, - const Vec2& offset = Vec2::ZERO); - - /** - * Create a body contains a EdgePolygon shape. - * - * @param points Points is an array of Vec2 structs defining a convex hull with a clockwise winding. - * @param count An integer number, contains the count of the points array. - * @param material A PhysicsMaterial object, the default value is PHYSICSSHAPE_MATERIAL_DEFAULT. - * @param border It's a edge's border width. - * @return An autoreleased PhysicsBody object pointer. - */ - static PhysicsBody* createEdgePolygon(const Vec2* points, - int count, - const PhysicsMaterial& material = PHYSICSBODY_MATERIAL_DEFAULT, - float border = 1); - - /** - * Create a body contains a EdgeChain shape. - * - * @param points A Vec2 object pointer, it contains an array of points. - * @param count An integer number, contains the count of the points array. - * @param material A PhysicsMaterial object, the default value is PHYSICSSHAPE_MATERIAL_DEFAULT. - * @param border It's a edge's border width. - * @return An autoreleased PhysicsBody object pointer. - */ - static PhysicsBody* createEdgeChain(const Vec2* points, - int count, - const PhysicsMaterial& material = PHYSICSBODY_MATERIAL_DEFAULT, - float border = 1); - - /** - * @brief Add a shape to body. - * @param shape The shape to be added. - * @param addMassAndMoment If this is true, the shape's mass and moment will be added to body. The default is true. - * @return This shape's pointer if added success or nullptr if failed. - */ - virtual PhysicsShape* addShape(PhysicsShape* shape, bool addMassAndMoment = true); - - /** - * @brief Remove a shape from body. - * @param shape Shape the shape to be removed. - * @param reduceMassAndMoment If this is true, the body mass and moment will be reduced by shape. The default is - * true. - */ - void removeShape(PhysicsShape* shape, bool reduceMassAndMoment = true); - - /** - * @brief Remove a shape from body. - * @param tag The tag of the shape to be removed. - * @param reduceMassAndMoment If this is true, the body mass and moment will be reduced by shape. The default is - * true. - */ - void removeShape(int tag, bool reduceMassAndMoment = true); - - /** - * Remove all shapes. - * - * @param reduceMassAndMoment If this is true, the body mass and moment will be reduced by shape. The default is - * true. - */ - void removeAllShapes(bool reduceMassAndMoment = true); - - /** - * Get the body shapes. - * - * @return A Vector object contains PhysicsShape pointer. - */ - const Vector& getShapes() const { return _shapes; } - - /** - * Get the first shape of the body shapes. - * - * @return The first shape in this body. - */ - PhysicsShape* getFirstShape() const { return _shapes.size() >= 1 ? _shapes.at(0) : nullptr; } - - /** - * get the shape of the body. - * - * @param tag An integer number that identifies a PhysicsShape object. - * @return A PhysicsShape object pointer or nullptr if no shapes were found. - */ - PhysicsShape* getShape(int tag) const; - - /** - * Applies a continuous force to body. - * - * @param force The force is applies to this body. - * @param offset A Vec2 object, it is the offset from the body's center of gravity in world coordinates. - */ - virtual void applyForce(const Vec2& force, const Vec2& offset = Vec2::ZERO); - - /** - * reset all the force applied to body. - */ - virtual void resetForces(); - - /** - * Applies a immediate force to body. - * - * @param impulse The impulse is applies to this body. - * @param offset A Vec2 object, it is the offset from the body's center of gravity in world coordinates. - */ - virtual void applyImpulse(const Vec2& impulse, const Vec2& offset = Vec2::ZERO); - - /** - * Applies a torque force to body. - * - * @param torque The torque is applies to this body. - */ - virtual void applyTorque(float torque); - - /** - * Set the velocity of a body. - * - * @param velocity The velocity is set to this body. - */ - virtual void setVelocity(const Vec2& velocity); - - /** Get the velocity of a body. */ - virtual Vec2 getVelocity(); - - /** - * Set the angular velocity of a body. - * - * @param velocity The angular velocity is set to this body. - */ - virtual void setAngularVelocity(float velocity); - - /** Get the angular velocity of a body at a local point.*/ - virtual Vec2 getVelocityAtLocalPoint(const Vec2& point); - - /** get the angular velocity of a body at a world point */ - virtual Vec2 getVelocityAtWorldPoint(const Vec2& point); - - /** get the angular velocity of a body */ - virtual float getAngularVelocity(); - - /** set the max of velocity */ - virtual void setVelocityLimit(float limit); - - /** get the max of velocity */ - virtual float getVelocityLimit(); - - /** set the max of angular velocity */ - virtual void setAngularVelocityLimit(float limit); - - /** get the max of angular velocity */ - virtual float getAngularVelocityLimit(); - - /** remove the body from the world it added to */ - void removeFromWorld(); - - /** get the world body added to. */ - PhysicsWorld* getWorld() const { return _world; } - - /** get all joints the body have */ - const std::vector& getJoints() const { return _joints; } - - /** get the node the body set to. */ - Node* getNode() const { return _owner; } - - /** - * A mask that defines which categories this physics body belongs to. - * - * Every physics body in a scene can be assigned to up to 32 different categories, each corresponding to a bit in - * the bit mask. You define the mask values used in your game. In conjunction with the collisionBitMask and - * contactTestBitMask properties, you define which physics bodies interact with each other and when your game is - * notified of these interactions. - * @param bitmask An integer number, the default value is 0xFFFFFFFF (all bits set). - */ - void setCategoryBitmask(int bitmask); - - /** - * A mask that defines which categories of bodies cause intersection notifications with this physics body. - * - * When two bodies share the same space, each body's category mask is tested against the other body's contact mask - * by performing a logical AND operation. If either comparison results in a non-zero value, an PhysicsContact object - * is created and passed to the physics world’s delegate. For best performance, only set bits in the contacts mask - * for interactions you are interested in. - * @param bitmask An integer number, the default value is 0x00000000 (all bits cleared). - */ - void setContactTestBitmask(int bitmask); - - /** - * A mask that defines which categories of physics bodies can collide with this physics body. - * - * When two physics bodies contact each other, a collision may occur. This body's collision mask is compared to the - * other body's category mask by performing a logical AND operation. If the result is a non-zero value, then this - * body is affected by the collision. Each body independently chooses whether it wants to be affected by the other - * body. For example, you might use this to avoid collision calculations that would make negligible changes to a - * body's velocity. - * @param bitmask An integer number, the default value is 0xFFFFFFFF (all bits set). - */ - void setCollisionBitmask(int bitmask); - - /** - * Return bitmask of first shape. - * - * @return If there is no shape in body, return default value.(0xFFFFFFFF) - */ - int getCategoryBitmask() const; - - /** - * Return bitmask of first shape. - * - * @return If there is no shape in body, return default value.(0x00000000) - */ - int getContactTestBitmask() const; - - /** - * Return bitmask of first shape. - * - * @return If there is no shape in body, return default value.(0xFFFFFFFF) - */ - int getCollisionBitmask() const; - - /** - * Set the group of body. - * - * Collision groups let you specify an integral group index. You can have all fixtures with the same group index - * always collide (positive index) or never collide (negative index). It have high priority than bit masks. - */ - void setGroup(int group); - - /** - * Return group of first shape. - * - * @return If there is no shape in body, return default value.(0) - */ - int getGroup() const; - - /** get the body position. */ - Vec2 getPosition() const; - - /** get the body rotation. */ - float getRotation(); - - /** set body position offset, it's the position witch relative to node */ - void setPositionOffset(const Vec2& position); - - /** get body position offset. */ - const Vec2& getPositionOffset() const { return _positionOffset; } - - /** set body rotation offset, it's the rotation witch relative to node */ - void setRotationOffset(float rotation); - - /** set the body rotation offset */ - float getRotationOffset() const { return _rotationOffset; } - - /** - * @brief Test the body is dynamic or not. - * - * A dynamic body will effect with gravity. - */ - bool isDynamic() const { return _dynamic; } - /** - * @brief Set dynamic to body. - * - * A dynamic body will effect with gravity. - */ - void setDynamic(bool dynamic); - - /** - * @brief Set the body mass. - * - * @attention If you need add/subtract mass to body, don't use setMass(getMass() +/- mass), because the mass of body - * may be equal to PHYSICS_INFINITY, it will cause some unexpected result, please use addMass() instead. - */ - void setMass(float mass); - - /** Get the body mass. */ - float getMass() const { return _mass; } - - /** - * @brief Add mass to body. - * - * @param mass If _mass(mass of the body) == PHYSICS_INFINITY, it remains. - * if mass == PHYSICS_INFINITY, _mass will be PHYSICS_INFINITY. - * if mass == -PHYSICS_INFINITY, _mass will not change. - * if mass + _mass <= 0, _mass will equal to MASS_DEFAULT(1.0) - * other wise, mass = mass + _mass; - */ - void addMass(float mass); - - /** - * @brief Set the body moment of inertia. - * - * @note If you need add/subtract moment to body, don't use setMoment(getMoment() +/- moment), because the moment of - * body may be equal to PHYSICS_INFINITY, it will cause some unexpected result, please use addMoment() instead. - */ - void setMoment(float moment); - - /** Get the body moment of inertia. */ - float getMoment() const { return _moment; } - - /** - * @brief Add moment of inertia to body. - * - * @param moment If _moment(moment of the body) == PHYSICS_INFINITY, it remains. - * if moment == PHYSICS_INFINITY, _moment will be PHYSICS_INFINITY. - * if moment == -PHYSICS_INFINITY, _moment will not change. - * if moment + _moment <= 0, _moment will equal to MASS_DEFAULT(1.0) - * other wise, moment = moment + _moment; - */ - void addMoment(float moment); - - /** get linear damping. */ - float getLinearDamping() const { return _linearDamping; } - - /** - * Set linear damping. - * - * it is used to simulate fluid or air friction forces on the body. - * @param damping The value is 0.0f to 1.0f. - */ - void setLinearDamping(float damping) - { - _linearDamping = damping; - updateDamping(); - } - - /** Get angular damping. */ - float getAngularDamping() const { return _angularDamping; } - - /** - * Set angular damping. - * - * It is used to simulate fluid or air friction forces on the body. - * @param damping The value is 0.0f to 1.0f. - */ - void setAngularDamping(float damping) - { - _angularDamping = damping; - updateDamping(); - } - - /** Whether the body is at rest. */ - bool isResting() const; - - /** set body to rest */ - void setResting(bool rest) const; - - /** - * Set the enable value. - * - * If the body it isn't enabled, it will not has simulation by world. - */ - virtual void setEnabled(bool enable) override; - - /** Whether the body can rotation. */ - bool isRotationEnabled() const { return _rotationEnabled; } - - /** Set the body is allow rotation or not */ - void setRotationEnable(bool enable); - - /** Whether this physics body is affected by the physics world's gravitational force. */ - bool isGravityEnabled() const { return _gravityEnabled; } - - /** Set the body is affected by the physics world's gravitational force or not. */ - void setGravityEnable(bool enable); - - /** Get the body's tag. */ - int getTag() const { return _tag; } - - /** set the body's tag. */ - void setTag(int tag) { _tag = tag; } - - /** Convert the world point to local. */ - Vec2 world2Local(const Vec2& point); - - /** Convert the local point to world. */ - Vec2 local2World(const Vec2& point); - - /** Get the rigid body of chipmunk. */ - cpBody* getCPBody() const { return _cpBody; } - - /** Set fixed update state */ - void setFixedUpdate(bool fixedUpdate) { _fixedUpdate = fixedUpdate; } - - virtual void onEnter() override; - virtual void onExit() override; - virtual void onAdd() override; - virtual void onRemove() override; - -protected: - PhysicsBody(); - virtual ~PhysicsBody(); - - virtual bool init() override; - - virtual void setPosition(float positionX, float positionY); - - virtual void setRotation(float rotation); - - virtual void setScale(float scaleX, float scaleY); - - void update(float delta) override; - void fixedUpdate(float delta); - - void removeJoint(PhysicsJoint* joint); - - void updateDamping() { _isDamping = _linearDamping != 0.0f || _angularDamping != 0.0f; } - - void addToPhysicsWorld(); - void removeFromPhysicsWorld(); - - void beforeSimulation(const Mat4& parentToWorldTransform, - const Mat4& nodeToWorldTransform, - float scaleX, - float scaleY, - float rotation); - void afterSimulation(const Mat4& parentToWorldTransform, float parentRotation); - -protected: - std::vector _joints; - Vector _shapes; - PhysicsWorld* _world; - - cpBody* _cpBody; - bool _dynamic; - bool _rotationEnabled; - bool _gravityEnabled; - bool _massDefault; - bool _momentDefault; - float _mass; - float _area; - float _density; - float _moment; - float _velocityLimit; - float _angularVelocityLimit; - bool _isDamping; - float _linearDamping; - float _angularDamping; - - int _tag; - - // when setMass() is invoked, it means body's mass is not calculated by shapes - bool _massSetByUser; - // when setMoment() is invoked, it means body's moment is not calculated by shapes - bool _momentSetByUser; - - Vec2 _positionOffset; - float _rotationOffset; - float _recordedRotation; - double _recordedAngle; - - // offset between owner's center point and down left point - Vec3 _ownerCenterOffset; - // offset of owner's center point and anchor point in parent coordinate - Vec2 _offset; - float _recordScaleX; - float _recordScaleY; - - float _recordPosX; - float _recordPosY; - - // fixed update state - bool _fixedUpdate; - - friend class PhysicsWorld; - friend class PhysicsShape; - friend class PhysicsJoint; -}; - -/** @} */ -/** @} */ - -} - -#endif // defined(AX_ENABLE_PHYSICS) -#endif // __CCPHYSICS_BODY_H__ diff --git a/core/physics/PhysicsContact.cpp b/core/physics/PhysicsContact.cpp deleted file mode 100644 index 267ff043be21..000000000000 --- a/core/physics/PhysicsContact.cpp +++ /dev/null @@ -1,435 +0,0 @@ -/**************************************************************************** - Copyright (c) 2013-2016 Chukong Technologies Inc. - Copyright (c) 2017-2018 Xiamen Yaji Software Co., Ltd. - Copyright (c) 2019-present Axmol Engine contributors (see AUTHORS.md). - - https://axmol.dev/ - - 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 "physics/PhysicsContact.h" -#if defined(AX_ENABLE_PHYSICS) -# include "chipmunk/chipmunk.h" - -# include "physics/PhysicsBody.h" -# include "physics/PhysicsHelper.h" -# include "base/EventCustom.h" - -namespace ax -{ - -const char* PHYSICSCONTACT_EVENT_NAME = "PhysicsContactEvent"; - -PhysicsContact::PhysicsContact() - : EventCustom(PHYSICSCONTACT_EVENT_NAME) - , _world(nullptr) - , _shapeA(nullptr) - , _shapeB(nullptr) - , _eventCode(EventCode::NONE) - , _notificationEnable(true) - , _result(true) - , _data(nullptr) - , _contactInfo(nullptr) - , _contactData(nullptr) - , _preContactData(nullptr) -{} - -PhysicsContact::~PhysicsContact() -{ - AX_SAFE_DELETE(_contactData); - AX_SAFE_DELETE(_preContactData); -} - -PhysicsContact* PhysicsContact::construct(PhysicsShape* a, PhysicsShape* b) -{ - PhysicsContact* contact = new PhysicsContact(); - if (contact->init(a, b)) - { - return contact; - } - - AX_SAFE_DELETE(contact); - return nullptr; -} - -bool PhysicsContact::init(PhysicsShape* a, PhysicsShape* b) -{ - do - { - AX_BREAK_IF(a == nullptr || b == nullptr); - - _shapeA = a; - _shapeB = b; - - return true; - } while (false); - - return false; -} - -void PhysicsContact::generateContactData() -{ - if (_contactInfo == nullptr) - { - return; - } - - cpArbiter* arb = static_cast(_contactInfo); - AX_SAFE_DELETE(_preContactData); - _preContactData = _contactData; - _contactData = new PhysicsContactData(); - _contactData->count = cpArbiterGetCount(arb); - for (int i = 0; i < _contactData->count && i < PhysicsContactData::POINT_MAX; ++i) - { - _contactData->points[i] = PhysicsHelper::cpv2vec2(cpArbiterGetPointA(arb, i)); - } - - _contactData->normal = _contactData->count > 0 ? PhysicsHelper::cpv2vec2(cpArbiterGetNormal(arb)) : Vec2::ZERO; -} - -// PhysicsContactPreSolve implementation -PhysicsContactPreSolve::PhysicsContactPreSolve(void* contactInfo) : _contactInfo(contactInfo) {} - -PhysicsContactPreSolve::~PhysicsContactPreSolve() {} - -float PhysicsContactPreSolve::getRestitution() const -{ - return cpArbiterGetRestitution(static_cast(_contactInfo)); -} - -float PhysicsContactPreSolve::getFriction() const -{ - return cpArbiterGetFriction(static_cast(_contactInfo)); -} - -Vec2 PhysicsContactPreSolve::getSurfaceVelocity() const -{ - return PhysicsHelper::cpv2vec2(cpArbiterGetSurfaceVelocity(static_cast(_contactInfo))); -} - -void PhysicsContactPreSolve::setRestitution(float restitution) -{ - cpArbiterSetRestitution(static_cast(_contactInfo), restitution); -} - -void PhysicsContactPreSolve::setFriction(float friction) -{ - cpArbiterSetFriction(static_cast(_contactInfo), friction); -} - -void PhysicsContactPreSolve::setSurfaceVelocity(const Vec2& velocity) -{ - cpArbiterSetSurfaceVelocity(static_cast(_contactInfo), PhysicsHelper::vec22cpv(velocity)); -} - -void PhysicsContactPreSolve::ignore() -{ - cpArbiterIgnore(static_cast(_contactInfo)); -} - -// PhysicsContactPostSolve implementation -PhysicsContactPostSolve::PhysicsContactPostSolve(void* contactInfo) : _contactInfo(contactInfo) {} - -PhysicsContactPostSolve::~PhysicsContactPostSolve() {} - -float PhysicsContactPostSolve::getRestitution() const -{ - return cpArbiterGetRestitution(static_cast(_contactInfo)); -} - -float PhysicsContactPostSolve::getFriction() const -{ - return cpArbiterGetFriction(static_cast(_contactInfo)); -} - -Vec2 PhysicsContactPostSolve::getSurfaceVelocity() const -{ - return PhysicsHelper::cpv2vec2(cpArbiterGetSurfaceVelocity(static_cast(_contactInfo))); -} - -EventListenerPhysicsContact::EventListenerPhysicsContact() - : onContactBegin(nullptr), onContactPreSolve(nullptr), onContactPostSolve(nullptr), onContactSeparate(nullptr) -{} - -bool EventListenerPhysicsContact::init() -{ - auto func = [this](EventCustom* event) -> void { onEvent(event); }; - - return EventListenerCustom::init(PHYSICSCONTACT_EVENT_NAME, func); -} - -void EventListenerPhysicsContact::onEvent(EventCustom* event) -{ - PhysicsContact* contact = dynamic_cast(event); - - if (contact == nullptr) - { - return; - } - - switch (contact->getEventCode()) - { - case PhysicsContact::EventCode::BEGIN: - { - bool ret = true; - - if (onContactBegin != nullptr && hitTest(contact->getShapeA(), contact->getShapeB())) - { - contact->generateContactData(); - ret = onContactBegin(*contact); - } - - contact->setResult(ret); - break; - } - case PhysicsContact::EventCode::PRESOLVE: - { - bool ret = true; - - if (onContactPreSolve != nullptr && hitTest(contact->getShapeA(), contact->getShapeB())) - { - PhysicsContactPreSolve solve(contact->_contactInfo); - contact->generateContactData(); - - ret = onContactPreSolve(*contact, solve); - } - - contact->setResult(ret); - break; - } - case PhysicsContact::EventCode::POSTSOLVE: - { - if (onContactPostSolve != nullptr && hitTest(contact->getShapeA(), contact->getShapeB())) - { - PhysicsContactPostSolve solve(contact->_contactInfo); - onContactPostSolve(*contact, solve); - } - break; - } - case PhysicsContact::EventCode::SEPARATE: - { - if (onContactSeparate != nullptr && hitTest(contact->getShapeA(), contact->getShapeB())) - { - onContactSeparate(*contact); - } - break; - } - default: - break; - } -} - -EventListenerPhysicsContact::~EventListenerPhysicsContact() {} - -EventListenerPhysicsContact* EventListenerPhysicsContact::create() -{ - EventListenerPhysicsContact* obj = new EventListenerPhysicsContact(); - - if (obj->init()) - { - obj->autorelease(); - return obj; - } - - AX_SAFE_DELETE(obj); - return nullptr; -} - -bool EventListenerPhysicsContact::hitTest(PhysicsShape* /*shapeA*/, PhysicsShape* /*shapeB*/) -{ - return true; -} - -bool EventListenerPhysicsContact::checkAvailable() -{ - if (onContactBegin == nullptr && onContactPreSolve == nullptr && onContactPostSolve == nullptr && - onContactSeparate == nullptr) - { - AXASSERT(false, "Invalid PhysicsContactListener."); - return false; - } - - return true; -} - -EventListenerPhysicsContact* EventListenerPhysicsContact::clone() -{ - EventListenerPhysicsContact* obj = EventListenerPhysicsContact::create(); - - if (obj != nullptr) - { - obj->onContactBegin = onContactBegin; - obj->onContactPreSolve = onContactPreSolve; - obj->onContactPostSolve = onContactPostSolve; - obj->onContactSeparate = onContactSeparate; - - return obj; - } - - AX_SAFE_DELETE(obj); - return nullptr; -} - -EventListenerPhysicsContactWithBodies* EventListenerPhysicsContactWithBodies::create(PhysicsBody* bodyA, - PhysicsBody* bodyB) -{ - EventListenerPhysicsContactWithBodies* obj = new EventListenerPhysicsContactWithBodies(); - - if (obj->init()) - { - obj->_a = bodyA; - obj->_b = bodyB; - obj->autorelease(); - return obj; - } - - AX_SAFE_DELETE(obj); - return nullptr; -} - -EventListenerPhysicsContactWithBodies::EventListenerPhysicsContactWithBodies() : _a(nullptr), _b(nullptr) {} - -EventListenerPhysicsContactWithBodies::~EventListenerPhysicsContactWithBodies() {} - -bool EventListenerPhysicsContactWithBodies::hitTest(PhysicsShape* shapeA, PhysicsShape* shapeB) -{ - if ((shapeA->getBody() == _a && shapeB->getBody() == _b) || (shapeA->getBody() == _b && shapeB->getBody() == _a)) - { - return true; - } - - return false; -} - -EventListenerPhysicsContactWithBodies* EventListenerPhysicsContactWithBodies::clone() -{ - EventListenerPhysicsContactWithBodies* obj = EventListenerPhysicsContactWithBodies::create(_a, _b); - - if (obj != nullptr) - { - obj->onContactBegin = onContactBegin; - obj->onContactPreSolve = onContactPreSolve; - obj->onContactPostSolve = onContactPostSolve; - obj->onContactSeparate = onContactSeparate; - - return obj; - } - - AX_SAFE_DELETE(obj); - return nullptr; -} - -EventListenerPhysicsContactWithShapes::EventListenerPhysicsContactWithShapes() : _a(nullptr), _b(nullptr) {} - -EventListenerPhysicsContactWithShapes::~EventListenerPhysicsContactWithShapes() {} - -EventListenerPhysicsContactWithShapes* EventListenerPhysicsContactWithShapes::create(PhysicsShape* shapeA, - PhysicsShape* shapeB) -{ - EventListenerPhysicsContactWithShapes* obj = new EventListenerPhysicsContactWithShapes(); - - if (obj->init()) - { - obj->_a = shapeA; - obj->_b = shapeB; - obj->autorelease(); - return obj; - } - - AX_SAFE_DELETE(obj); - return nullptr; -} - -bool EventListenerPhysicsContactWithShapes::hitTest(PhysicsShape* shapeA, PhysicsShape* shapeB) -{ - if ((shapeA == _a && shapeB == _b) || (shapeA == _b && shapeB == _a)) - { - return true; - } - - return false; -} - -EventListenerPhysicsContactWithShapes* EventListenerPhysicsContactWithShapes::clone() -{ - EventListenerPhysicsContactWithShapes* obj = EventListenerPhysicsContactWithShapes::create(_a, _b); - - if (obj != nullptr) - { - obj->onContactBegin = onContactBegin; - obj->onContactPreSolve = onContactPreSolve; - obj->onContactPostSolve = onContactPostSolve; - obj->onContactSeparate = onContactSeparate; - - return obj; - } - - AX_SAFE_DELETE(obj); - return nullptr; -} - -EventListenerPhysicsContactWithGroup::EventListenerPhysicsContactWithGroup() : _group(CP_NO_GROUP) {} - -EventListenerPhysicsContactWithGroup::~EventListenerPhysicsContactWithGroup() {} - -EventListenerPhysicsContactWithGroup* EventListenerPhysicsContactWithGroup::create(int group) -{ - EventListenerPhysicsContactWithGroup* obj = new EventListenerPhysicsContactWithGroup(); - - if (obj->init()) - { - obj->_group = group; - obj->autorelease(); - return obj; - } - - AX_SAFE_DELETE(obj); - return nullptr; -} - -bool EventListenerPhysicsContactWithGroup::hitTest(PhysicsShape* shapeA, PhysicsShape* shapeB) -{ - if (shapeA->getGroup() == _group || shapeB->getGroup() == _group) - { - return true; - } - - return false; -} - -EventListenerPhysicsContactWithGroup* EventListenerPhysicsContactWithGroup::clone() -{ - EventListenerPhysicsContactWithGroup* obj = EventListenerPhysicsContactWithGroup::create(_group); - - if (obj != nullptr) - { - obj->onContactBegin = onContactBegin; - obj->onContactPreSolve = onContactPreSolve; - obj->onContactPostSolve = onContactPostSolve; - obj->onContactSeparate = onContactSeparate; - - return obj; - } - - AX_SAFE_DELETE(obj); - return nullptr; -} - -} -#endif // defined(AX_ENABLE_PHYSICS) diff --git a/core/physics/PhysicsContact.h b/core/physics/PhysicsContact.h deleted file mode 100644 index fe125bafe4ff..000000000000 --- a/core/physics/PhysicsContact.h +++ /dev/null @@ -1,327 +0,0 @@ -/**************************************************************************** - Copyright (c) 2013-2016 Chukong Technologies Inc. - Copyright (c) 2017-2018 Xiamen Yaji Software Co., Ltd. - Copyright (c) 2019-present Axmol Engine contributors (see AUTHORS.md). - - https://axmol.dev/ - - 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 __CCPHYSICS_CONTACT_H__ -#define __CCPHYSICS_CONTACT_H__ - -#include "base/Config.h" -#if defined(AX_ENABLE_PHYSICS) - -# include "base/Object.h" -# include "math/Math.h" -# include "base/EventListenerCustom.h" -# include "base/Event.h" -# include "base/EventCustom.h" - -namespace ax -{ - -class PhysicsShape; -class PhysicsBody; -class PhysicsWorld; - -typedef struct AX_DLL PhysicsContactData -{ - static const int POINT_MAX = 4; - Vec2 points[POINT_MAX]; - int count; - Vec2 normal; - - PhysicsContactData() : count(0) {} -} PhysicsContactData; - -/** - * @addtogroup physics - * @{ - * @addtogroup physics_2d - * @{ - */ - -/** - * @brief Contact information. - - * It will created automatically when two shape contact with each other. And it will destroyed automatically when two - shape separated. - */ -class AX_DLL PhysicsContact : public EventCustom -{ -public: - enum class EventCode - { - NONE, - BEGIN, - PRESOLVE, - POSTSOLVE, - SEPARATE - }; - - /** Get contact shape A. */ - PhysicsShape* getShapeA() const { return _shapeA; } - - /** Get contact shape B. */ - PhysicsShape* getShapeB() const { return _shapeB; } - - /** Get contact data. */ - const PhysicsContactData* getContactData() const { return _contactData; } - - /** Get previous contact data */ - const PhysicsContactData* getPreContactData() const { return _preContactData; } - - /** - * Get data. - * @lua NA - */ - void* getData() const { return _data; } - - /** - * @brief Set data to contact. - - * You must manage the memory yourself, Generally you can set data at contact begin, and destroy it at contact - separate. - * - * @lua NA - */ - void setData(void* data) { _data = data; } - - /** Get the event code */ - EventCode getEventCode() const { return _eventCode; }; - -private: - static PhysicsContact* construct(PhysicsShape* a, PhysicsShape* b); - bool init(PhysicsShape* a, PhysicsShape* b); - - void setEventCode(EventCode eventCode) { _eventCode = eventCode; }; - bool isNotificationEnabled() const { return _notificationEnable; } - void setNotificationEnable(bool enable) { _notificationEnable = enable; } - PhysicsWorld* getWorld() const { return _world; } - void setWorld(PhysicsWorld* world) { _world = world; } - void setResult(bool result) { _result = result; } - bool resetResult() - { - bool ret = _result; - _result = true; - return ret; - } - - void generateContactData(); - -private: - PhysicsContact(); - ~PhysicsContact(); - -private: - PhysicsWorld* _world; - PhysicsShape* _shapeA; - PhysicsShape* _shapeB; - EventCode _eventCode; - bool _notificationEnable; - bool _result; - - void* _data; - void* _contactInfo; - PhysicsContactData* _contactData; - PhysicsContactData* _preContactData; - - friend class EventListenerPhysicsContact; - friend class PhysicsWorldCallback; - friend class PhysicsWorld; -}; - -/** - * @brief Presolve value generated when onContactPreSolve called. - */ -class AX_DLL PhysicsContactPreSolve -{ -public: - /** Get restitution between two bodies.*/ - float getRestitution() const; - /** Get friction between two bodies.*/ - float getFriction() const; - /** Get surface velocity between two bodies.*/ - Vec2 getSurfaceVelocity() const; - /** Set the restitution.*/ - void setRestitution(float restitution); - /** Set the friction.*/ - void setFriction(float friction); - /** Set the surface velocity.*/ - void setSurfaceVelocity(const Vec2& velocity); - /** Ignore the rest of the contact presolve and postsolve callbacks. */ - void ignore(); - -private: - PhysicsContactPreSolve(void* contactInfo); - ~PhysicsContactPreSolve(); - -private: - void* _contactInfo; - - friend class EventListenerPhysicsContact; -}; - -/** - * @brief Postsolve value generated when onContactPostSolve called. - */ -class AX_DLL PhysicsContactPostSolve -{ -public: - /** Get restitution between two bodies.*/ - float getRestitution() const; - /** Get friction between two bodies.*/ - float getFriction() const; - /** Get surface velocity between two bodies.*/ - Vec2 getSurfaceVelocity() const; - -private: - PhysicsContactPostSolve(void* contactInfo); - ~PhysicsContactPostSolve(); - -private: - void* _contactInfo; - - friend class EventListenerPhysicsContact; -}; - -/** Contact listener. It will receive all the contact callbacks. */ -class AX_DLL EventListenerPhysicsContact : public EventListenerCustom -{ -public: - /** Create the listener. */ - static EventListenerPhysicsContact* create(); - - /** Check the listener is available. - - * @return True if there's one available callback function at least, false if there's no one. - */ - virtual bool checkAvailable() override; - - /** Clone an object from this listener.*/ - virtual EventListenerPhysicsContact* clone() override; - -protected: - /** - * It will be call when two body have contact. - * if return false, it will not invoke callbacks. - */ - virtual bool hitTest(PhysicsShape* shapeA, PhysicsShape* shapeB); - -public: - /** - * @brief It will called at two shapes start to contact, and only call it once. - */ - std::function onContactBegin; - /** - * @brief Two shapes are touching during this step. Return false from the callback to make world ignore the - * collision this step or true to process it normally. Additionally, you may override collision values, restitution, - * or surface velocity values. - */ - std::function onContactPreSolve; - /** - * @brief Two shapes are touching and their collision response has been processed. You can retrieve the collision - * impulse or kinetic energy at this time if you want to use it to calculate sound volumes or damage amounts. See - * cpArbiter for more info - */ - std::function onContactPostSolve; - /** - * @brief It will called at two shapes separated, and only call it once. - * onContactBegin and onContactSeparate will called in pairs. - */ - std::function onContactSeparate; - -protected: - bool init(); - void onEvent(EventCustom* event); - -protected: - EventListenerPhysicsContact(); - virtual ~EventListenerPhysicsContact(); - - friend class PhysicsWorld; -}; - -/** This event listener only be called when bodyA and bodyB have contacts. */ -class AX_DLL EventListenerPhysicsContactWithBodies : public EventListenerPhysicsContact -{ -public: - /** Create the listener. */ - static EventListenerPhysicsContactWithBodies* create(PhysicsBody* bodyA, PhysicsBody* bodyB); - - virtual bool hitTest(PhysicsShape* shapeA, PhysicsShape* shapeB) override; - - virtual EventListenerPhysicsContactWithBodies* clone() override; - -protected: - PhysicsBody* _a; - PhysicsBody* _b; - -protected: - EventListenerPhysicsContactWithBodies(); - virtual ~EventListenerPhysicsContactWithBodies(); -}; - -/** This event listener only be called when shapeA and shapeB have contacts. */ -class AX_DLL EventListenerPhysicsContactWithShapes : public EventListenerPhysicsContact -{ -public: - /** Create the listener. */ - static EventListenerPhysicsContactWithShapes* create(PhysicsShape* shapeA, PhysicsShape* shapeB); - - virtual bool hitTest(PhysicsShape* shapeA, PhysicsShape* shapeB) override; - virtual EventListenerPhysicsContactWithShapes* clone() override; - -protected: - PhysicsShape* _a; - PhysicsShape* _b; - -protected: - EventListenerPhysicsContactWithShapes(); - virtual ~EventListenerPhysicsContactWithShapes(); -}; - -/** This event listener only be called when shapeA or shapeB is in the group your specified */ -class AX_DLL EventListenerPhysicsContactWithGroup : public EventListenerPhysicsContact -{ -public: - /** Create the listener. */ - static EventListenerPhysicsContactWithGroup* create(int group); - - virtual bool hitTest(PhysicsShape* shapeA, PhysicsShape* shapeB) override; - virtual EventListenerPhysicsContactWithGroup* clone() override; - -protected: - int _group; - -protected: - EventListenerPhysicsContactWithGroup(); - virtual ~EventListenerPhysicsContactWithGroup(); -}; - -/** @} */ -/** @} */ - -} - -#endif // defined(AX_ENABLE_PHYSICS) -#endif //__CCPHYSICS_CONTACT_H__ diff --git a/core/physics/PhysicsHelper.h b/core/physics/PhysicsHelper.h deleted file mode 100644 index 80ccedc734a9..000000000000 --- a/core/physics/PhysicsHelper.h +++ /dev/null @@ -1,117 +0,0 @@ -/**************************************************************************** - Copyright (c) 2013 cocos2d-x.org - Copyright (c) 2017-2018 Xiamen Yaji Software Co., Ltd. - Copyright (c) 2019-present Axmol Engine contributors (see AUTHORS.md). - - https://axmol.dev/ - - 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 __CCPHYSICS_HELPER_H__ -#define __CCPHYSICS_HELPER_H__ - -#include "base/Config.h" -#if defined(AX_ENABLE_PHYSICS) - -# include "chipmunk/chipmunk.h" -# include "platform/PlatformMacros.h" -# include "math/Math.h" - -namespace ax -{ - -/** - * @addtogroup physics - * @{ - * @addtogroup physics_2d - * @{ - */ - -/** - * A physics helper class. - * - * Support for conversion between the chipmunk types and cocos types, eg: cpVect to Vec2, Vec2 to cpVect, cpFloat to - * float. - */ -class PhysicsHelper -{ -public: - /** Make cpVect type convert to Vec2 type. */ - static Vec2 cpv2vec2(const cpVect& vec) { return Vec2(vec.x, vec.y); } - - /** Make Vec2 type convert to cpVect type. */ - static cpVect vec22cpv(const Vec2& point) { return cpv(point.x, point.y); } - - /** Make cpFloat type convert to float type. */ - static float cpfloat2float(cpFloat f) { return f; } - - /** Make Rect type convert to cpBB type. */ - static cpBB rect2cpbb(const Rect& rect) - { - return cpBBNew(rect.origin.x, rect.origin.y, rect.origin.x + rect.size.width, rect.origin.y + rect.size.height); - } - - /** Make cpBB type convert to Rect type. */ - static Rect cpbb2rect(const cpBB& bb) { return Rect(bb.l, bb.b, bb.r - bb.l, bb.t - bb.b); } - - /** - Make cpVect array convert to Vec2 array. - - @param cpvs The be converted object, it's a cpVect array. - @param out The converted object, it's a Vec2 array. - @param count It's cpvs array length. - @return The out object's pointer. - */ - static Vec2* cpvs2points(const cpVect* cpvs, Vec2* out, int count) - { - for (int i = 0; i < count; ++i) - { - out[i] = cpv2vec2(cpvs[i]); - } - - return out; - } - - /** - Make Vec2 array convert to cpVect array. - - @param points The be converted object, it's a Vec2 array. - @param out The converted object, it's a cpVect array. - @param count It's points array length. - @return The out object's pointer. - */ - static cpVect* points2cpvs(const Vec2* points, cpVect* out, int count) - { - for (int i = 0; i < count; ++i) - { - out[i] = vec22cpv(points[i]); - } - - return out; - } -}; - -/** @} */ -/** @} */ - -} - -#endif // AX_ENABLE_PHYSICS -#endif // __CCPHYSICS_HELPER_H__ diff --git a/core/physics/PhysicsJoint.cpp b/core/physics/PhysicsJoint.cpp deleted file mode 100644 index 33063b046c8e..000000000000 --- a/core/physics/PhysicsJoint.cpp +++ /dev/null @@ -1,936 +0,0 @@ -/**************************************************************************** - Copyright (c) 2013-2016 Chukong Technologies Inc. - Copyright (c) 2017-2018 Xiamen Yaji Software Co., Ltd. - Copyright (c) 2019-present Axmol Engine contributors (see AUTHORS.md). - - https://axmol.dev/ - - 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 "physics/PhysicsJoint.h" -#if defined(AX_ENABLE_PHYSICS) -# include "chipmunk/chipmunk.h" - -# include "physics/PhysicsBody.h" -# include "physics/PhysicsWorld.h" -# include "physics/PhysicsHelper.h" -# include "2d/Node.h" - -namespace ax -{ - -template -class Optional -{ - -public: - Optional() {} - Optional(T d) : _isSet(true), _data(d) {} - Optional(const Optional& t) : _isSet(t._isSet), _data(t._data) {} - - // bool isNull() const { return !_isSet; } - // bool isDefineded() const { return _isSet; } - // bool isEmpty() const { return !_isSet; } - - T get() const - { - AXASSERT(_isSet, "data should be set!"); - return _data; - } - void set(T d) - { - _isSet = true; - _data = d; - } - -private: - bool _isSet = false; - T _data; -}; - -class WriteCache -{ -public: - Optional _grooveA; - Optional _grooveB; - Optional _anchr1; - Optional _anchr2; - Optional _min; - Optional _max; - Optional _distance; - Optional _restLength; - Optional _restAngle; - Optional _stiffness; - Optional _damping; - Optional _angle; - Optional _phase; - Optional _ratchet; - Optional _ratio; - Optional _rate; -}; - -# if (defined(__GNUC__) && __GNUC__ >= 4) || defined(__clang__) -# define LIKELY(x) (__builtin_expect((x), 1)) -# define UNLIKELY(x) (__builtin_expect((x), 0)) -# else -# define LIKELY(x) (x) -# define UNLIKELY(x) (x) -# endif - -# define AX_PJOINT_CACHE_READ(field) \ - do \ - { \ - if (UNLIKELY(_initDirty)) \ - { \ - return _writeCache->field.get(); \ - } \ - } while (0) - -# define AX_PJOINT_CACHE_WRITE2(field, method, arg, convertedArg) \ - do \ - { \ - if (UNLIKELY(_initDirty)) \ - { \ - _writeCache->field.set(arg); \ - delay([this, arg]() { method(_cpConstraints.front(), convertedArg); }); \ - } \ - else \ - { \ - method(_cpConstraints.front(), convertedArg); \ - } \ - } while (0) - -# define AX_PJOINT_CACHE_WRITE(field, method, arg) AX_PJOINT_CACHE_WRITE2(field, method, arg, arg) - -PhysicsJoint::PhysicsJoint() - : _bodyA(nullptr) - , _bodyB(nullptr) - , _world(nullptr) - , _enable(false) - , _collisionEnable(true) - , _destroyMark(false) - , _tag(0) - , _maxForce(PHYSICS_INFINITY) - , _initDirty(true) -{ - _writeCache = new WriteCache(); -} - -PhysicsJoint::~PhysicsJoint() -{ - // reset the shapes collision group - setCollisionEnable(true); - - for (cpConstraint* joint : _cpConstraints) - { - cpConstraintFree(joint); - } - _cpConstraints.clear(); - - delete _writeCache; -} - -bool PhysicsJoint::init(ax::PhysicsBody* a, ax::PhysicsBody* b) -{ - do - { - AXASSERT(a != nullptr && b != nullptr, "the body passed in is nil"); - AXASSERT(a != b, "the two bodies are equal"); - - _bodyA = a; - _bodyB = b; - _bodyA->_joints.emplace_back(this); - _bodyB->_joints.emplace_back(this); - - return true; - } while (false); - - return false; -} - -bool PhysicsJoint::initJoint() -{ - bool ret = !_initDirty; - while (_initDirty) - { - ret = createConstraints(); - AX_BREAK_IF(!ret); - - for (auto&& subjoint : _cpConstraints) - { - cpConstraintSetMaxForce(subjoint, _maxForce); - cpConstraintSetErrorBias(subjoint, cpfpow(1.0f - 0.15f, 60.0f)); - cpSpaceAddConstraint(_world->_cpSpace, subjoint); - } - _initDirty = false; - ret = true; - } - - return ret; -} - -void PhysicsJoint::flushDelayTasks() -{ - for (const auto& tsk : _delayTasks) - { - tsk(); - } - _delayTasks.clear(); -} - -void PhysicsJoint::setEnable(bool enable) -{ - if (_enable != enable) - { - _enable = enable; - - if (_world) - { - if (enable) - { - _world->addJoint(this); - } - else - { - _world->removeJoint(this, false); - } - } - } -} - -void PhysicsJoint::setCollisionEnable(bool enable) -{ - if (_collisionEnable != enable) - { - _collisionEnable = enable; - } -} - -void PhysicsJoint::removeFormWorld() -{ - if (_world) - { - _world->removeJoint(this, false); - } -} - -void PhysicsJoint::setMaxForce(float force) -{ - if (_initDirty) - { - delay([this, force]() { - _maxForce = force; - for (auto&& joint : _cpConstraints) - { - cpConstraintSetMaxForce(joint, force); - } - }); - } - else - { - _maxForce = force; - for (auto&& joint : _cpConstraints) - { - cpConstraintSetMaxForce(joint, force); - } - } -} - -PhysicsJointFixed* PhysicsJointFixed::construct(PhysicsBody* a, PhysicsBody* b, const Vec2& anchr) -{ - auto joint = new PhysicsJointFixed(); - - if (joint->init(a, b)) - { - joint->_anchr = anchr; - return joint; - } - - AX_SAFE_DELETE(joint); - return nullptr; -} - -bool PhysicsJointFixed::createConstraints() -{ - do - { - _bodyA->getNode()->setPosition(_anchr); - _bodyB->getNode()->setPosition(_anchr); - - // add a pivot joint to fixed two body together - auto joint = cpPivotJointNew(_bodyA->getCPBody(), _bodyB->getCPBody(), PhysicsHelper::vec22cpv(_anchr)); - AX_BREAK_IF(joint == nullptr); - _cpConstraints.emplace_back(joint); - - // add a gear joint to make two body have the same rotation. - joint = cpGearJointNew(_bodyA->getCPBody(), _bodyB->getCPBody(), 0, 1); - AX_BREAK_IF(joint == nullptr); - _cpConstraints.emplace_back(joint); - - _collisionEnable = false; - - return true; - } while (false); - - return false; -} - -PhysicsJointPin* PhysicsJointPin::construct(PhysicsBody* a, PhysicsBody* b, const Vec2& pivot) -{ - auto joint = new PhysicsJointPin(); - - if (joint->init(a, b)) - { - joint->_anchr1 = pivot; - joint->_useSpecificAnchr = false; - return joint; - } - - AX_SAFE_DELETE(joint); - return nullptr; -} - -PhysicsJointPin* PhysicsJointPin::construct(PhysicsBody* a, PhysicsBody* b, const Vec2& anchr1, const Vec2& anchr2) -{ - auto joint = new PhysicsJointPin(); - - if (joint->init(a, b)) - { - joint->_anchr1 = anchr1; - joint->_anchr2 = anchr2; - joint->_useSpecificAnchr = true; - - return joint; - } - - AX_SAFE_DELETE(joint); - return nullptr; -} - -bool PhysicsJointPin::createConstraints() -{ - do - { - cpConstraint* joint = nullptr; - if (_useSpecificAnchr) - { - joint = cpPivotJointNew2(_bodyA->getCPBody(), _bodyB->getCPBody(), PhysicsHelper::vec22cpv(_anchr1), - PhysicsHelper::vec22cpv(_anchr2)); - } - else - { - joint = cpPivotJointNew(_bodyA->getCPBody(), _bodyB->getCPBody(), PhysicsHelper::vec22cpv(_anchr1)); - } - - AX_BREAK_IF(joint == nullptr); - _cpConstraints.emplace_back(joint); - - return true; - } while (false); - - return false; -} - -PhysicsJointLimit* PhysicsJointLimit::construct(PhysicsBody* a, - PhysicsBody* b, - const Vec2& anchr1, - const Vec2& anchr2, - float min, - float max) -{ - auto joint = new PhysicsJointLimit(); - - if (joint->init(a, b)) - { - joint->_anchr1 = anchr1; - joint->_anchr2 = anchr2; - joint->_min = min; - joint->_max = max; - - return joint; - } - - AX_SAFE_DELETE(joint); - return nullptr; -} - -PhysicsJointLimit* PhysicsJointLimit::construct(PhysicsBody* a, PhysicsBody* b, const Vec2& anchr1, const Vec2& anchr2) -{ - return construct(a, b, anchr1, anchr2, 0, b->local2World(anchr1).getDistance(a->local2World(anchr2))); -} - -bool PhysicsJointLimit::createConstraints() -{ - do - { - auto joint = cpSlideJointNew(_bodyA->getCPBody(), _bodyB->getCPBody(), PhysicsHelper::vec22cpv(_anchr1), - PhysicsHelper::vec22cpv(_anchr2), _min, _max); - - AX_BREAK_IF(joint == nullptr); - _cpConstraints.emplace_back(joint); - - return true; - } while (false); - - return false; -} - -float PhysicsJointLimit::getMin() const -{ - AX_PJOINT_CACHE_READ(_min); - return PhysicsHelper::cpfloat2float(cpSlideJointGetMin(_cpConstraints.front())); -} - -void PhysicsJointLimit::setMin(float min) -{ - AX_PJOINT_CACHE_WRITE(_min, cpSlideJointSetMin, min); -} - -float PhysicsJointLimit::getMax() const -{ - AX_PJOINT_CACHE_READ(_max); - return PhysicsHelper::cpfloat2float(cpSlideJointGetMax(_cpConstraints.front())); -} - -void PhysicsJointLimit::setMax(float max) -{ - AX_PJOINT_CACHE_WRITE(_max, cpSlideJointSetMax, max); -} - -Vec2 PhysicsJointLimit::getAnchr1() const -{ - AX_PJOINT_CACHE_READ(_anchr1); - return PhysicsHelper::cpv2vec2(cpSlideJointGetAnchorA(_cpConstraints.front())); -} - -void PhysicsJointLimit::setAnchr1(const Vec2& anchr) -{ - AX_PJOINT_CACHE_WRITE2(_anchr1, cpSlideJointSetAnchorA, anchr, PhysicsHelper::vec22cpv(anchr)); -} - -Vec2 PhysicsJointLimit::getAnchr2() const -{ - AX_PJOINT_CACHE_READ(_anchr2); - return PhysicsHelper::cpv2vec2(cpSlideJointGetAnchorB(_cpConstraints.front())); -} - -void PhysicsJointLimit::setAnchr2(const Vec2& anchr) -{ - AX_PJOINT_CACHE_WRITE2(_anchr2, cpSlideJointSetAnchorB, anchr, PhysicsHelper::vec22cpv(anchr)); -} - -PhysicsJointDistance* PhysicsJointDistance::construct(PhysicsBody* a, - PhysicsBody* b, - const Vec2& anchr1, - const Vec2& anchr2) -{ - auto joint = new PhysicsJointDistance(); - - if (joint->init(a, b)) - { - joint->_anchr1 = anchr1; - joint->_anchr2 = anchr2; - - return joint; - } - - AX_SAFE_DELETE(joint); - return nullptr; -} - -bool PhysicsJointDistance::createConstraints() -{ - do - { - auto joint = cpPinJointNew(_bodyA->getCPBody(), _bodyB->getCPBody(), PhysicsHelper::vec22cpv(_anchr1), - PhysicsHelper::vec22cpv(_anchr2)); - AX_BREAK_IF(joint == nullptr); - _cpConstraints.emplace_back(joint); - - return true; - } while (false); - - return false; -} - -float PhysicsJointDistance::getDistance() const -{ - AX_PJOINT_CACHE_READ(_distance); - return PhysicsHelper::cpfloat2float(cpPinJointGetDist(_cpConstraints.front())); -} - -void PhysicsJointDistance::setDistance(float distance) -{ - AX_PJOINT_CACHE_WRITE(_distance, cpPinJointSetDist, distance); -} - -PhysicsJointSpring* PhysicsJointSpring::construct(PhysicsBody* a, - PhysicsBody* b, - const Vec2& anchr1, - const Vec2& anchr2, - float stiffness, - float damping) -{ - auto joint = new PhysicsJointSpring(); - - if (joint->init(a, b)) - { - joint->_anchr1 = anchr1; - joint->_anchr2 = anchr2; - joint->_stiffness = stiffness; - joint->_damping = damping; - - return joint; - } - - AX_SAFE_DELETE(joint); - return nullptr; -} - -bool PhysicsJointSpring::createConstraints() -{ - do - { - auto joint = cpDampedSpringNew(_bodyA->getCPBody(), _bodyB->getCPBody(), PhysicsHelper::vec22cpv(_anchr1), - PhysicsHelper::vec22cpv(_anchr2), - _bodyB->local2World(_anchr1).getDistance(_bodyA->local2World(_anchr2)), - _stiffness, _damping); - - AX_BREAK_IF(joint == nullptr); - _cpConstraints.emplace_back(joint); - - return true; - } while (false); - - return false; -} - -Vec2 PhysicsJointSpring::getAnchr1() const -{ - AX_PJOINT_CACHE_READ(_anchr1); - return PhysicsHelper::cpv2vec2(cpDampedSpringGetAnchorA(_cpConstraints.front())); -} - -void PhysicsJointSpring::setAnchr1(const Vec2& anchr) -{ - AX_PJOINT_CACHE_WRITE2(_anchr1, cpDampedSpringSetAnchorA, anchr, PhysicsHelper::vec22cpv(anchr)); -} - -Vec2 PhysicsJointSpring::getAnchr2() const -{ - AX_PJOINT_CACHE_READ(_anchr2); - return PhysicsHelper::cpv2vec2(cpDampedSpringGetAnchorB(_cpConstraints.front())); -} - -void PhysicsJointSpring::setAnchr2(const Vec2& anchr) -{ - AX_PJOINT_CACHE_WRITE2(_anchr2, cpDampedSpringSetAnchorB, anchr, PhysicsHelper::vec22cpv(anchr)); -} - -float PhysicsJointSpring::getRestLength() const -{ - AX_PJOINT_CACHE_READ(_restLength); - return PhysicsHelper::cpfloat2float(cpDampedSpringGetRestLength(_cpConstraints.front())); -} - -void PhysicsJointSpring::setRestLength(float restLength) -{ - AX_PJOINT_CACHE_WRITE(_restLength, cpDampedSpringSetRestLength, restLength); -} - -float PhysicsJointSpring::getStiffness() const -{ - AX_PJOINT_CACHE_READ(_stiffness); - return PhysicsHelper::cpfloat2float(cpDampedSpringGetStiffness(_cpConstraints.front())); -} - -void PhysicsJointSpring::setStiffness(float stiffness) -{ - AX_PJOINT_CACHE_WRITE(_stiffness, cpDampedSpringSetStiffness, stiffness); -} - -float PhysicsJointSpring::getDamping() const -{ - AX_PJOINT_CACHE_READ(_damping); - return PhysicsHelper::cpfloat2float(cpDampedSpringGetDamping(_cpConstraints.front())); -} - -void PhysicsJointSpring::setDamping(float damping) -{ - AX_PJOINT_CACHE_WRITE(_damping, cpDampedSpringSetDamping, damping); -} - -PhysicsJointGroove* PhysicsJointGroove::construct(PhysicsBody* a, - PhysicsBody* b, - const Vec2& grooveA, - const Vec2& grooveB, - const Vec2& anchr2) -{ - auto joint = new PhysicsJointGroove(); - - if (joint->init(a, b)) - { - joint->_grooveA = grooveA; - joint->_grooveB = grooveB; - joint->_anchr2 = anchr2; - - return joint; - } - - AX_SAFE_DELETE(joint); - return nullptr; -} - -bool PhysicsJointGroove::createConstraints() -{ - do - { - auto joint = cpGrooveJointNew(_bodyA->getCPBody(), _bodyB->getCPBody(), PhysicsHelper::vec22cpv(_grooveA), - PhysicsHelper::vec22cpv(_grooveB), PhysicsHelper::vec22cpv(_anchr2)); - - AX_BREAK_IF(joint == nullptr); - _cpConstraints.emplace_back(joint); - - return true; - } while (false); - - return false; -} - -Vec2 PhysicsJointGroove::getGrooveA() const -{ - AX_PJOINT_CACHE_READ(_grooveA); - return PhysicsHelper::cpv2vec2(cpGrooveJointGetGrooveA(_cpConstraints.front())); -} - -void PhysicsJointGroove::setGrooveA(const Vec2& grooveA) -{ - AX_PJOINT_CACHE_WRITE2(_grooveA, cpGrooveJointSetGrooveA, grooveA, PhysicsHelper::vec22cpv(grooveA)); -} - -Vec2 PhysicsJointGroove::getGrooveB() const -{ - AX_PJOINT_CACHE_READ(_grooveB); - return PhysicsHelper::cpv2vec2(cpGrooveJointGetGrooveB(_cpConstraints.front())); -} - -void PhysicsJointGroove::setGrooveB(const Vec2& grooveB) -{ - AX_PJOINT_CACHE_WRITE2(_grooveB, cpGrooveJointSetGrooveB, grooveB, PhysicsHelper::vec22cpv(grooveB)); -} - -Vec2 PhysicsJointGroove::getAnchr2() const -{ - AX_PJOINT_CACHE_READ(_anchr2); - return PhysicsHelper::cpv2vec2(cpGrooveJointGetAnchorB(_cpConstraints.front())); -} - -void PhysicsJointGroove::setAnchr2(const Vec2& anchr2) -{ - AX_PJOINT_CACHE_WRITE2(_anchr2, cpGrooveJointSetAnchorB, anchr2, PhysicsHelper::vec22cpv(anchr2)); -} - -PhysicsJointRotarySpring* PhysicsJointRotarySpring::construct(PhysicsBody* a, - PhysicsBody* b, - float stiffness, - float damping) -{ - auto joint = new PhysicsJointRotarySpring(); - - if (joint->init(a, b)) - { - joint->_stiffness = stiffness; - joint->_damping = damping; - - return joint; - } - - AX_SAFE_DELETE(joint); - return nullptr; -} - -bool PhysicsJointRotarySpring::createConstraints() -{ - do - { - auto joint = cpDampedRotarySpringNew(_bodyA->getCPBody(), _bodyB->getCPBody(), - _bodyB->getRotation() - _bodyA->getRotation(), _stiffness, _damping); - - AX_BREAK_IF(joint == nullptr); - _cpConstraints.emplace_back(joint); - - return true; - } while (false); - - return false; -} - -float PhysicsJointRotarySpring::getRestAngle() const -{ - AX_PJOINT_CACHE_READ(_restAngle); - return PhysicsHelper::cpfloat2float(cpDampedRotarySpringGetRestAngle(_cpConstraints.front())); -} - -void PhysicsJointRotarySpring::setRestAngle(float restAngle) -{ - AX_PJOINT_CACHE_WRITE(_restAngle, cpDampedRotarySpringSetRestAngle, restAngle); -} - -float PhysicsJointRotarySpring::getStiffness() const -{ - AX_PJOINT_CACHE_READ(_stiffness); - return PhysicsHelper::cpfloat2float(cpDampedRotarySpringGetStiffness(_cpConstraints.front())); -} - -void PhysicsJointRotarySpring::setStiffness(float stiffness) -{ - AX_PJOINT_CACHE_WRITE(_stiffness, cpDampedRotarySpringSetStiffness, stiffness); -} - -float PhysicsJointRotarySpring::getDamping() const -{ - AX_PJOINT_CACHE_READ(_damping); - return PhysicsHelper::cpfloat2float(cpDampedRotarySpringGetDamping(_cpConstraints.front())); -} - -void PhysicsJointRotarySpring::setDamping(float damping) -{ - AX_PJOINT_CACHE_WRITE(_damping, cpDampedRotarySpringSetDamping, damping); -} - -PhysicsJointRotaryLimit* PhysicsJointRotaryLimit::construct(PhysicsBody* a, PhysicsBody* b, float min, float max) -{ - auto joint = new PhysicsJointRotaryLimit(); - - if (joint->init(a, b)) - { - joint->_min = min; - joint->_max = max; - - return joint; - } - - AX_SAFE_DELETE(joint); - return nullptr; -} - -PhysicsJointRotaryLimit* PhysicsJointRotaryLimit::construct(PhysicsBody* a, PhysicsBody* b) -{ - return construct(a, b, 0.0f, 0.0f); -} - -bool PhysicsJointRotaryLimit::createConstraints() -{ - do - { - auto joint = cpRotaryLimitJointNew(_bodyA->getCPBody(), _bodyB->getCPBody(), _min, _max); - - AX_BREAK_IF(joint == nullptr); - _cpConstraints.emplace_back(joint); - - return true; - } while (false); - - return false; -} - -float PhysicsJointRotaryLimit::getMin() const -{ - AX_PJOINT_CACHE_READ(_min); - return PhysicsHelper::cpfloat2float(cpRotaryLimitJointGetMin(_cpConstraints.front())); -} - -void PhysicsJointRotaryLimit::setMin(float min) -{ - AX_PJOINT_CACHE_WRITE(_min, cpRotaryLimitJointSetMin, min); -} - -float PhysicsJointRotaryLimit::getMax() const -{ - AX_PJOINT_CACHE_READ(_max); - return PhysicsHelper::cpfloat2float(cpRotaryLimitJointGetMax(_cpConstraints.front())); -} - -void PhysicsJointRotaryLimit::setMax(float max) -{ - AX_PJOINT_CACHE_WRITE(_max, cpRotaryLimitJointSetMax, max); -} - -PhysicsJointRatchet* PhysicsJointRatchet::construct(PhysicsBody* a, PhysicsBody* b, float phase, float ratchet) -{ - auto joint = new PhysicsJointRatchet(); - - if (joint->init(a, b)) - { - joint->_phase = phase; - joint->_ratchet = ratchet; - - return joint; - } - - AX_SAFE_DELETE(joint); - return nullptr; -} - -bool PhysicsJointRatchet::createConstraints() -{ - do - { - auto joint = - cpRatchetJointNew(_bodyA->getCPBody(), _bodyB->getCPBody(), _phase, PhysicsHelper::cpfloat2float(_ratchet)); - - AX_BREAK_IF(joint == nullptr); - _cpConstraints.emplace_back(joint); - - return true; - } while (false); - - return false; -} - -float PhysicsJointRatchet::getAngle() const -{ - AX_PJOINT_CACHE_READ(_angle); - return PhysicsHelper::cpfloat2float(cpRatchetJointGetAngle(_cpConstraints.front())); -} - -void PhysicsJointRatchet::setAngle(float angle) -{ - AX_PJOINT_CACHE_WRITE(_angle, cpRatchetJointSetAngle, angle); -} - -float PhysicsJointRatchet::getPhase() const -{ - AX_PJOINT_CACHE_READ(_phase); - return PhysicsHelper::cpfloat2float(cpRatchetJointGetPhase(_cpConstraints.front())); -} - -void PhysicsJointRatchet::setPhase(float phase) -{ - AX_PJOINT_CACHE_WRITE(_phase, cpRatchetJointSetPhase, phase); -} - -float PhysicsJointRatchet::getRatchet() const -{ - AX_PJOINT_CACHE_READ(_ratchet); - return PhysicsHelper::cpfloat2float(cpRatchetJointGetRatchet(_cpConstraints.front())); -} - -void PhysicsJointRatchet::setRatchet(float ratchet) -{ - AX_PJOINT_CACHE_WRITE(_ratchet, cpRatchetJointSetRatchet, ratchet); -} - -PhysicsJointGear* PhysicsJointGear::construct(PhysicsBody* a, PhysicsBody* b, float phase, float ratio) -{ - auto joint = new PhysicsJointGear(); - - if (joint->init(a, b)) - { - joint->_phase = phase; - joint->_ratio = ratio; - - return joint; - } - - AX_SAFE_DELETE(joint); - return nullptr; -} - -bool PhysicsJointGear::createConstraints() -{ - do - { - auto joint = cpGearJointNew(_bodyA->getCPBody(), _bodyB->getCPBody(), _phase, _ratio); - - AX_BREAK_IF(joint == nullptr); - _cpConstraints.emplace_back(joint); - - return true; - } while (false); - - return false; -} - -float PhysicsJointGear::getPhase() const -{ - AX_PJOINT_CACHE_READ(_phase); - return PhysicsHelper::cpfloat2float(cpGearJointGetPhase(_cpConstraints.front())); -} - -void PhysicsJointGear::setPhase(float phase) -{ - AX_PJOINT_CACHE_WRITE(_phase, cpGearJointSetPhase, phase); -} - -float PhysicsJointGear::getRatio() const -{ - AX_PJOINT_CACHE_READ(_ratio); - return PhysicsHelper::cpfloat2float(cpGearJointGetRatio(_cpConstraints.front())); -} - -void PhysicsJointGear::setRatio(float ratio) -{ - AX_PJOINT_CACHE_WRITE(_ratio, cpGearJointSetRatio, ratio); -} - -PhysicsJointMotor* PhysicsJointMotor::construct(PhysicsBody* a, PhysicsBody* b, float rate) -{ - auto joint = new PhysicsJointMotor(); - - if (joint->init(a, b)) - { - joint->_rate = rate; - - return joint; - } - - AX_SAFE_DELETE(joint); - return nullptr; -} - -bool PhysicsJointMotor::createConstraints() -{ - do - { - auto joint = cpSimpleMotorNew(_bodyA->getCPBody(), _bodyB->getCPBody(), _rate); - - AX_BREAK_IF(joint == nullptr); - _cpConstraints.emplace_back(joint); - - return true; - } while (false); - - return false; -} - -float PhysicsJointMotor::getRate() const -{ - AX_PJOINT_CACHE_READ(_rate); - return PhysicsHelper::cpfloat2float(cpSimpleMotorGetRate(_cpConstraints.front())); -} - -void PhysicsJointMotor::setRate(float rate) -{ - AX_PJOINT_CACHE_WRITE(_rate, cpSimpleMotorSetRate, rate); -} - -} -#endif // AX_ENABLE_PHYSICS diff --git a/core/physics/PhysicsJoint.h b/core/physics/PhysicsJoint.h deleted file mode 100644 index 148108a1e417..000000000000 --- a/core/physics/PhysicsJoint.h +++ /dev/null @@ -1,609 +0,0 @@ -/**************************************************************************** - Copyright (c) 2013-2016 Chukong Technologies Inc. - Copyright (c) 2017-2018 Xiamen Yaji Software Co., Ltd. - Copyright (c) 2019-present Axmol Engine contributors (see AUTHORS.md). - - https://axmol.dev/ - - 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 __CCPHYSICS_JOINT_H__ -#define __CCPHYSICS_JOINT_H__ - -#include - -#include "base/Config.h" -#if defined(AX_ENABLE_PHYSICS) - -# include "base/Object.h" -# include "math/Math.h" - -struct cpConstraint; - -namespace ax -{ - -class Node; -class PhysicsBody; -class PhysicsWorld; - -class WriteCache; - -/** - * @addtogroup physics - * @{ - * @addtogroup physics_2d - * @{ - */ - -/** - * @brief An PhysicsJoint object connects two physics bodies together. - */ -class AX_DLL PhysicsJoint -{ -protected: - typedef std::function DelayTask; - -protected: - PhysicsJoint(); - virtual ~PhysicsJoint() = 0; - -public: - /**Get physics body a connected to this joint.*/ - PhysicsBody* getBodyA() const { return _bodyA; } - - /**Get physics body b connected to this joint.*/ - PhysicsBody* getBodyB() const { return _bodyB; } - - /**Get the physics world.*/ - PhysicsWorld* getWorld() const { return _world; } - - /** - * Get this joint's tag. - * - * @return An integer number. - */ - int getTag() const { return _tag; } - - /** - * Set this joint's tag. - * - * @param tag An integer number that identifies a PhysicsJoint. - */ - void setTag(int tag) { _tag = tag; } - - /** Determines if the joint is enable. */ - bool isEnabled() const { return _enable; } - - /** Enable/Disable the joint. */ - void setEnable(bool enable); - - /** Determines if the collision is enable. */ - bool isCollisionEnabled() const { return _collisionEnable; } - - /** Enable/disable the collision between two bodies. */ - void setCollisionEnable(bool enable); - - /** Remove the joint from the world. */ - void removeFormWorld(); - - /** Set the max force between two bodies. */ - void setMaxForce(float force); - - /** Get the max force setting. */ - float getMaxForce() const { return _maxForce; } - -protected: - bool init(PhysicsBody* a, PhysicsBody* b); - - bool initJoint(); - - void delay(const DelayTask& task) { _delayTasks.emplace_back(task); } - - void flushDelayTasks(); - - /** Create constraints for this type joint */ - virtual bool createConstraints() { return false; } - - std::vector _cpConstraints; - std::vector _delayTasks; - PhysicsBody* _bodyA; - PhysicsBody* _bodyB; - PhysicsWorld* _world; - - WriteCache* _writeCache = nullptr; - - bool _enable; - bool _collisionEnable; - bool _destroyMark; - int _tag; - float _maxForce; - - bool _initDirty; - - friend class PhysicsBody; - friend class PhysicsWorld; - friend class PhysicsDebugDraw; -}; - -/** - * @brief A fixed joint fuses the two bodies together at a reference point. Fixed joints are useful for creating complex - * shapes that can be broken apart later. - */ -class AX_DLL PhysicsJointFixed : public PhysicsJoint -{ -public: - /** Create a fixed joint. - - @param a A is the body to connect. - @param b B is the body to connect. - @param anchr It's the pivot position. - @return A object pointer. - */ - static PhysicsJointFixed* construct(PhysicsBody* a, PhysicsBody* b, const Vec2& anchr); - - virtual bool createConstraints() override; - -protected: - PhysicsJointFixed() {} - virtual ~PhysicsJointFixed() {} - - Vec2 _anchr; -}; - -/** - * @brief A limit joint imposes a maximum distance between the two bodies, as if they were connected by a rope. - */ -class AX_DLL PhysicsJointLimit : public PhysicsJoint -{ -public: - /** Create a limit joint. - - @param a A is the body to connect. - @param b B is the body to connect. - @param anchr1 Anchr1 is the anchor point on body a. - @param anchr2 Anchr2 is the anchor point on body b. - @return A object pointer. - */ - static PhysicsJointLimit* construct(PhysicsBody* a, PhysicsBody* b, const Vec2& anchr1, const Vec2& anchr2); - - /** Create a limit joint. - - @param a A is the body to connect. - @param b B is the body to connect. - @param anchr1 Anchr1 is the anchor point on body a. - @param anchr2 Anchr2 is the anchor point on body b. - @param min Define the allowed min distance of the anchor points. - @param max Define the allowed max distance of the anchor points. - @return A object pointer. - */ - static PhysicsJointLimit* construct(PhysicsBody* a, - PhysicsBody* b, - const Vec2& anchr1, - const Vec2& anchr2, - float min, - float max); - - /** Get the anchor point on body a.*/ - Vec2 getAnchr1() const; - - /** Set the anchor point on body a.*/ - void setAnchr1(const Vec2& anchr1); - - /** Get the anchor point on body b.*/ - Vec2 getAnchr2() const; - - /** Set the anchor point on body b.*/ - void setAnchr2(const Vec2& anchr2); - - /** Get the allowed min distance of the anchor points.*/ - float getMin() const; - /** Set the min distance of the anchor points.*/ - void setMin(float min); - - /** Get the allowed max distance of the anchor points.*/ - float getMax() const; - /** Set the max distance of the anchor points.*/ - void setMax(float max); - - virtual bool createConstraints() override; - -protected: - PhysicsJointLimit() {} - virtual ~PhysicsJointLimit() {} - - Vec2 _anchr1; - Vec2 _anchr2; - float _min; - float _max; -}; - -/** - * @brief A pin joint allows the two bodies to independently rotate around the anchor point as if pinned together. - */ -class AX_DLL PhysicsJointPin : public PhysicsJoint -{ -public: - /** Create a pin joint. - - @param a A is the body to connect. - @param b B is the body to connect. - @param pivot It's the pivot position. - @return A object pointer. - */ - static PhysicsJointPin* construct(PhysicsBody* a, PhysicsBody* b, const Vec2& pivot); - - /** Create a pin joint. - - @param a A is the body to connect. - @param b B is the body to connect. - @param anchr1 Anchr1 is the anchor point on body a. - @param anchr2 Anchr2 is the anchor point on body b. - @return A object pointer. - */ - static PhysicsJointPin* construct(PhysicsBody* a, PhysicsBody* b, const Vec2& anchr1, const Vec2& anchr2); - - virtual bool createConstraints() override; - -protected: - PhysicsJointPin() {} - virtual ~PhysicsJointPin() {} - - bool _useSpecificAnchr; - Vec2 _anchr1; - Vec2 _anchr2; -}; - -/** Set the fixed distance with two bodies */ -class AX_DLL PhysicsJointDistance : public PhysicsJoint -{ -public: - /** Create a fixed distance joint. - - @param a A is the body to connect. - @param b B is the body to connect. - @param anchr1 Anchr1 is the anchor point on body a. - @param anchr2 Anchr2 is the anchor point on body b. - @return A object pointer. - */ - static PhysicsJointDistance* construct(PhysicsBody* a, PhysicsBody* b, const Vec2& anchr1, const Vec2& anchr2); - - /** Get the distance of the anchor points.*/ - float getDistance() const; - /** Set the distance of the anchor points.*/ - void setDistance(float distance); - virtual bool createConstraints() override; - -protected: - PhysicsJointDistance() {} - virtual ~PhysicsJointDistance() {} - - Vec2 _anchr1; - Vec2 _anchr2; -}; - -/** Connecting two physics bodies together with a spring. */ -class AX_DLL PhysicsJointSpring : public PhysicsJoint -{ -public: - /** Create a fixed distance joint. - - @param a A is the body to connect. - @param b B is the body to connect. - @param anchr1 Anchr1 is the anchor point on body a. - @param anchr2 Anchr2 is the anchor point on body b. - @param stiffness It's the spring constant. - @param damping It's how soft to make the damping of the spring. - @return A object pointer. - */ - static PhysicsJointSpring* construct(PhysicsBody* a, - PhysicsBody* b, - const Vec2& anchr1, - const Vec2& anchr2, - float stiffness, - float damping); - - /** Get the anchor point on body a.*/ - Vec2 getAnchr1() const; - - /** Set the anchor point on body a.*/ - void setAnchr1(const Vec2& anchr1); - - /** Get the anchor point on body b.*/ - Vec2 getAnchr2() const; - - /** Set the anchor point on body b.*/ - void setAnchr2(const Vec2& anchr2); - - /** Get the distance of the anchor points.*/ - float getRestLength() const; - - /** Set the distance of the anchor points.*/ - void setRestLength(float restLength); - - /** Get the spring constant.*/ - float getStiffness() const; - - /** Set the spring constant.*/ - void setStiffness(float stiffness); - - /** Get the spring soft constant.*/ - float getDamping() const; - - /** Set the spring soft constant.*/ - void setDamping(float damping); - - virtual bool createConstraints() override; - -protected: - PhysicsJointSpring() {} - virtual ~PhysicsJointSpring() {} - - Vec2 _anchr1; - Vec2 _anchr2; - float _stiffness; - float _damping; -}; - -/** Attach body a to a line, and attach body b to a dot. */ -class AX_DLL PhysicsJointGroove : public PhysicsJoint -{ -public: - /** Create a groove joint. - - @param a A is the body to connect. - @param b B is the body to connect. - @param grooveA The line begin position. - @param grooveB The line end position. - @param anchr2 Anchr2 is the anchor point on body b. - @return A object pointer. - */ - static PhysicsJointGroove* construct(PhysicsBody* a, - PhysicsBody* b, - const Vec2& grooveA, - const Vec2& grooveB, - const Vec2& anchr2); - - /** Get the line begin position*/ - Vec2 getGrooveA() const; - - /** Set the line begin position*/ - void setGrooveA(const Vec2& grooveA); - - /** Get the line end position*/ - Vec2 getGrooveB() const; - - /** Set the line end position*/ - void setGrooveB(const Vec2& grooveB); - - /** Get the anchor point on body b.*/ - Vec2 getAnchr2() const; - - /** Set the anchor point on body b.*/ - void setAnchr2(const Vec2& anchr2); - - virtual bool createConstraints() override; - -protected: - PhysicsJointGroove() {} - virtual ~PhysicsJointGroove() {} - - Vec2 _grooveA; - Vec2 _grooveB; - Vec2 _anchr2; -}; - -/** Likes a spring joint, but works with rotary. */ -class AX_DLL PhysicsJointRotarySpring : public PhysicsJoint -{ -public: - /** Create a damped rotary spring joint. - - @param a A is the body to connect. - @param b B is the body to connect. - @param stiffness It's the spring constant. - @param damping It's how soft to make the damping of the spring. - @return A object pointer. - */ - static PhysicsJointRotarySpring* construct(PhysicsBody* a, PhysicsBody* b, float stiffness, float damping); - - /** Get the relative angle in radians from the body a to b.*/ - float getRestAngle() const; - - /** Set the relative angle in radians from the body a to b.*/ - void setRestAngle(float restAngle); - - /** Get the spring constant.*/ - float getStiffness() const; - - /** Set the spring constant.*/ - void setStiffness(float stiffness); - - /** Get the spring soft constant.*/ - float getDamping() const; - - /** Set the spring soft constant.*/ - void setDamping(float damping); - - virtual bool createConstraints() override; - -protected: - PhysicsJointRotarySpring() {} - virtual ~PhysicsJointRotarySpring() {} - - float _stiffness; - float _damping; -}; - -/** Likes a limit joint, but works with rotary. */ -class AX_DLL PhysicsJointRotaryLimit : public PhysicsJoint -{ -public: - /** Create a limit rotary joint. - - @param a A is the body to connect. - @param b B is the body to connect. - @param min It's min rotation limit in radians. - @param max It's max rotation limit in radians. - @return A object pointer. - */ - static PhysicsJointRotaryLimit* construct(PhysicsBody* a, PhysicsBody* b, float min, float max); - - /** Create a limit rotary joint. - - @param a A is the body to connect. - @param b B is the body to connect. - @return A object pointer. - */ - static PhysicsJointRotaryLimit* construct(PhysicsBody* a, PhysicsBody* b); - - /** Get the min rotation limit.*/ - float getMin() const; - - /** Set the min rotation limit.*/ - void setMin(float min); - - /** Get the max rotation limit.*/ - float getMax() const; - - /** Set the max rotation limit.*/ - void setMax(float max); - - virtual bool createConstraints() override; - -protected: - PhysicsJointRotaryLimit() {} - virtual ~PhysicsJointRotaryLimit() {} - - float _min; - float _max; -}; - -/** Works like a socket wrench. */ -class AX_DLL PhysicsJointRatchet : public PhysicsJoint -{ -public: - /** Create a ratchet joint. - - @param a A is the body to connect. - @param b B is the body to connect. - @param phase Phase is the initial offset to use when deciding where the ratchet angles are. - @param ratchet Ratchet is the distance between "clicks". - @return A object pointer. - */ - static PhysicsJointRatchet* construct(PhysicsBody* a, PhysicsBody* b, float phase, float ratchet); - - /** Get the ratchet angle.*/ - float getAngle() const; - - /** Set the ratchet angle.*/ - void setAngle(float angle); - - /** Get the initial offset.*/ - float getPhase() const; - - /** Set the initial offset.*/ - void setPhase(float phase); - - /** Get the distance between "clicks".*/ - float getRatchet() const; - - /** Set the distance between "clicks".*/ - void setRatchet(float ratchet); - virtual bool createConstraints() override; - -protected: - PhysicsJointRatchet() {} - virtual ~PhysicsJointRatchet() {} - - float _phase; - float _ratchet; -}; - -/** Keeps the angular velocity ratio of a pair of bodies constant. */ -class AX_DLL PhysicsJointGear : public PhysicsJoint -{ -public: - /** Create a gear joint. - - @param a A is the body to connect. - @param b B is the body to connect. - @param phase Phase is the initial angular offset of the two bodies. - @param ratio Ratio is always measured in absolute terms. - @return A object pointer. - */ - static PhysicsJointGear* construct(PhysicsBody* a, PhysicsBody* b, float phase, float ratio); - - /** Get the angular offset of the two bodies.*/ - float getPhase() const; - - /** Set the angular offset of the two bodies.*/ - void setPhase(float phase); - - /** Get the ratio.*/ - float getRatio() const; - - /** Set the ratio.*/ - void setRatio(float ratchet); - - virtual bool createConstraints() override; - -protected: - PhysicsJointGear() {} - virtual ~PhysicsJointGear() {} - - float _phase; - float _ratio; -}; - -/** Keeps the relative angular velocity of a pair of bodies constant. */ -class AX_DLL PhysicsJointMotor : public PhysicsJoint -{ -public: - /** Create a motor joint. - - @param a A is the body to connect. - @param b B is the body to connect. - @param rate Rate is the desired relative angular velocity. - @return A object pointer. - */ - static PhysicsJointMotor* construct(PhysicsBody* a, PhysicsBody* b, float rate); - - /** Get the relative angular velocity.*/ - float getRate() const; - - /** Set the relative angular velocity.*/ - void setRate(float rate); - virtual bool createConstraints() override; - -protected: - PhysicsJointMotor() {} - virtual ~PhysicsJointMotor() {} - - float _rate; -}; - -/** @} */ -/** @} */ - -} - -#endif // defined(AX_ENABLE_PHYSICS) -#endif // __CCPHYSICS_JOINT_H__ diff --git a/core/physics/PhysicsShape.cpp b/core/physics/PhysicsShape.cpp deleted file mode 100644 index 6abaf180e61b..000000000000 --- a/core/physics/PhysicsShape.cpp +++ /dev/null @@ -1,980 +0,0 @@ -/**************************************************************************** - Copyright (c) 2013-2016 Chukong Technologies Inc. - Copyright (c) 2017-2018 Xiamen Yaji Software Co., Ltd. - Copyright (c) 2019-present Axmol Engine contributors (see AUTHORS.md). - - https://axmol.dev/ - - 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 "physics/PhysicsShape.h" -#if defined(AX_ENABLE_PHYSICS) - -# include -# include -# include - -# include "chipmunk/chipmunk.h" -# include "chipmunk/chipmunk_unsafe.h" - -# include "physics/PhysicsBody.h" -# include "physics/PhysicsWorld.h" -# include "physics/PhysicsHelper.h" - -namespace ax -{ -extern const float PHYSICS_INFINITY; -static cpBody* s_sharedBody = nullptr; - -PhysicsShape::PhysicsShape() - : _body(nullptr) - , _type(Type::UNKNOWN) - , _area(0.0f) - , _mass(0.0f) - , _moment(0.0f) - , _sensor(false) - , _scaleX(1.0f) - , _scaleY(1.0f) - , _newScaleX(1.0f) - , _newScaleY(1.0f) - , _tag(0) - , _categoryBitmask(UINT_MAX) - , _collisionBitmask(UINT_MAX) - , _contactTestBitmask(0) - , _group(0) -{ - if (s_sharedBody == nullptr) - { - s_sharedBody = cpBodyNewStatic(); - } -} - -PhysicsShape::~PhysicsShape() -{ - for (auto&& shape : _cpShapes) - { - cpShapeFree(shape); - } -} - -void PhysicsShape::setMass(float mass) -{ - if (mass < 0) - { - return; - } - - if (_body) - { - _body->addMass(-_mass); - _body->addMass(mass); - }; - - _mass = mass; -} - -void PhysicsShape::setMoment(float moment) -{ - if (moment < 0) - { - return; - } - - if (_body) - { - _body->addMoment(-_moment); - _body->addMoment(moment); - }; - - _moment = moment; -} - -void PhysicsShape::setMaterial(const PhysicsMaterial& material) -{ - setDensity(material.density); - setRestitution(material.restitution); - setFriction(material.friction); -} - -void PhysicsShape::setScale(float scaleX, float scaleY) -{ - if (std::abs(_scaleX - scaleX) > FLT_EPSILON || std::abs(_scaleY - scaleY) > FLT_EPSILON) - { - if (_type == Type::CIRCLE && scaleX != scaleY) - { - AXLOGD("PhysicsShapeCircle WARNING: CANNOT support setScale with different x and y"); - return; - } - _newScaleX = scaleX; - _newScaleY = scaleY; - - updateScale(); - - // re-calculate area and mass - _area = calculateArea(); - _mass = _material.density * _area; - _moment = calculateDefaultMoment(); - } -} - -void PhysicsShape::updateScale() -{ - _scaleX = _newScaleX; - _scaleY = _newScaleY; -} - -void PhysicsShape::addShape(cpShape* shape) -{ - if (shape) - { - cpShapeSetUserData(shape, this); - cpShapeSetFilter(shape, cpShapeFilterNew(_group, CP_ALL_CATEGORIES, CP_ALL_CATEGORIES)); - _cpShapes.emplace_back(shape); - } -} - -PhysicsShapeCircle::PhysicsShapeCircle() {} - -PhysicsShapeCircle::~PhysicsShapeCircle() {} - -PhysicsShapeBox::PhysicsShapeBox() {} - -PhysicsShapeBox::~PhysicsShapeBox() {} - -PhysicsShapePolygon::PhysicsShapePolygon() {} - -PhysicsShapePolygon::~PhysicsShapePolygon() {} - -PhysicsShapeEdgeBox::PhysicsShapeEdgeBox() {} - -PhysicsShapeEdgeBox::~PhysicsShapeEdgeBox() {} - -PhysicsShapeEdgeChain::PhysicsShapeEdgeChain() {} - -PhysicsShapeEdgeChain::~PhysicsShapeEdgeChain() {} - -PhysicsShapeEdgePolygon::PhysicsShapeEdgePolygon() {} - -PhysicsShapeEdgePolygon::~PhysicsShapeEdgePolygon() {} - -PhysicsShapeEdgeSegment::PhysicsShapeEdgeSegment() {} - -PhysicsShapeEdgeSegment::~PhysicsShapeEdgeSegment() {} - -void PhysicsShape::setDensity(float density) -{ - if (density < 0) - { - return; - } - - _material.density = density; - - if (_material.density == PHYSICS_INFINITY) - { - setMass(PHYSICS_INFINITY); - } - else if (_area > 0) - { - setMass(_material.density * _area); - } -} - -void PhysicsShape::setRestitution(float restitution) -{ - _material.restitution = restitution; - - for (cpShape* shape : _cpShapes) - { - cpShapeSetElasticity(shape, restitution); - } -} - -void PhysicsShape::setFriction(float friction) -{ - _material.friction = friction; - - for (cpShape* shape : _cpShapes) - { - cpShapeSetFriction(shape, friction); - } -} - -void PhysicsShape::setSensor(bool sensor) -{ - if (sensor != _sensor) - { - for (cpShape* shape : _cpShapes) - { - cpShapeSetSensor(shape, sensor); - } - _sensor = sensor; - } -} - -void PhysicsShape::recenterPoints(Vec2* points, int count, const Vec2& center) -{ - cpVect* cpvs = new cpVect[count]; - cpVect centroid = cpCentroidForPoly(count, cpvs); - for (int i = 0; i < count; i++) - { - cpvs[i] = cpvsub(cpvs[i], centroid); - } - PhysicsHelper::cpvs2points(cpvs, points, count); - delete[] cpvs; - - if (center != Vec2::ZERO) - { - for (int i = 0; i < count; ++i) - { - points[i] += center; - } - } -} - -Vec2 PhysicsShape::getPolygonCenter(const Vec2* points, int count) -{ - cpVect* cpvs = new cpVect[count]; - cpVect center = cpCentroidForPoly(count, PhysicsHelper::points2cpvs(points, cpvs, count)); - delete[] cpvs; - - return PhysicsHelper::cpv2vec2(center); -} - -void PhysicsShape::setBody(PhysicsBody* body) -{ - // already added - if (body && _body == body) - { - return; - } - - if (_body) - { - _body->removeShape(this); - } - - for (auto&& shape : _cpShapes) - { - cpShapeSetBody(shape, body == nullptr ? s_sharedBody : body->_cpBody); - } - _body = body; -} - -// PhysicsShapeCircle -PhysicsShapeCircle* PhysicsShapeCircle::create(float radius, - const PhysicsMaterial& material /* = MaterialDefault*/, - const Vec2& offset /* = Vec2(0, 0)*/) -{ - PhysicsShapeCircle* shape = new PhysicsShapeCircle(); - if (shape->init(radius, material, offset)) - { - shape->autorelease(); - return shape; - } - - AX_SAFE_DELETE(shape); - return nullptr; -} - -bool PhysicsShapeCircle::init(float radius, - const PhysicsMaterial& material /* = MaterialDefault*/, - const Vec2& offset /*= Vec2(0, 0)*/) -{ - do - { - _type = Type::CIRCLE; - - auto shape = cpCircleShapeNew(s_sharedBody, radius, PhysicsHelper::vec22cpv(offset)); - AX_BREAK_IF(shape == nullptr); - cpShapeSetUserData(shape, this); - - addShape(shape); - - _area = calculateArea(); - _mass = material.density == PHYSICS_INFINITY ? PHYSICS_INFINITY : material.density * _area; - _moment = calculateDefaultMoment(); - - setMaterial(material); - return true; - } while (false); - - return false; -} - -float PhysicsShapeCircle::calculateArea(float radius) -{ - return PhysicsHelper::cpfloat2float(cpAreaForCircle(0, radius)); -} - -float PhysicsShapeCircle::calculateMoment(float mass, float radius, const Vec2& offset) -{ - return mass == PHYSICS_INFINITY - ? PHYSICS_INFINITY - : PhysicsHelper::cpfloat2float(cpMomentForCircle(mass, 0, radius, PhysicsHelper::vec22cpv(offset))); -} - -float PhysicsShapeCircle::calculateArea() -{ - return PhysicsHelper::cpfloat2float(cpAreaForCircle(0, cpCircleShapeGetRadius(_cpShapes.front()))); -} - -float PhysicsShapeCircle::calculateDefaultMoment() -{ - auto shape = _cpShapes.front(); - - return _mass == PHYSICS_INFINITY ? PHYSICS_INFINITY - : PhysicsHelper::cpfloat2float(cpMomentForCircle( - _mass, 0, cpCircleShapeGetRadius(shape), cpCircleShapeGetOffset(shape))); -} - -float PhysicsShapeCircle::getRadius() const -{ - return PhysicsHelper::cpfloat2float(cpCircleShapeGetRadius(_cpShapes.front())); -} - -Vec2 PhysicsShapeCircle::getOffset() -{ - return PhysicsHelper::cpv2vec2(cpCircleShapeGetOffset(_cpShapes.front())); -} - -void PhysicsShapeCircle::updateScale() -{ - cpFloat factor = std::abs(_newScaleX / _scaleX); - - cpShape* shape = _cpShapes.front(); - cpVect v = cpCircleShapeGetOffset(shape); - v = cpvmult(v, factor); - cpCircleShapeSetOffset(shape, v); - - cpCircleShapeSetRadius(shape, cpCircleShapeGetRadius(shape) * factor); - - PhysicsShape::updateScale(); -} - -// PhysicsShapeEdgeSegment -PhysicsShapeEdgeSegment* PhysicsShapeEdgeSegment::create(const Vec2& a, - const Vec2& b, - const PhysicsMaterial& material /* = MaterialDefault*/, - float border /* = 1*/) -{ - PhysicsShapeEdgeSegment* shape = new PhysicsShapeEdgeSegment(); - if (shape->init(a, b, material, border)) - { - shape->autorelease(); - return shape; - } - - AX_SAFE_DELETE(shape); - return nullptr; -} - -bool PhysicsShapeEdgeSegment::init(const Vec2& a, - const Vec2& b, - const PhysicsMaterial& material /* = MaterialDefault*/, - float border /* = 1*/) -{ - do - { - _type = Type::EDGESEGMENT; - - auto shape = cpSegmentShapeNew(s_sharedBody, PhysicsHelper::vec22cpv(a), PhysicsHelper::vec22cpv(b), border); - AX_BREAK_IF(shape == nullptr); - cpShapeSetUserData(shape, this); - - addShape(shape); - - _mass = PHYSICS_INFINITY; - _moment = PHYSICS_INFINITY; - - setMaterial(material); - - return true; - } while (false); - - return false; -} - -Vec2 PhysicsShapeEdgeSegment::getPointA() const -{ - return PhysicsHelper::cpv2vec2(cpSegmentShapeGetA(_cpShapes.front())); -} - -Vec2 PhysicsShapeEdgeSegment::getPointB() const -{ - return PhysicsHelper::cpv2vec2(cpSegmentShapeGetB(_cpShapes.front())); -} - -Vec2 PhysicsShapeEdgeSegment::getCenter() -{ - auto a = PhysicsHelper::cpv2vec2(cpSegmentShapeGetA(_cpShapes.front())); - auto b = PhysicsHelper::cpv2vec2(cpSegmentShapeGetB(_cpShapes.front())); - return (a + b) / 2; -} - -void PhysicsShapeEdgeSegment::updateScale() -{ - cpFloat factorX = _newScaleX / _scaleX; - cpFloat factorY = _newScaleY / _scaleY; - - cpShape* shape = _cpShapes.front(); - cpVect a = cpSegmentShapeGetA(shape); - a.x *= factorX; - a.y *= factorY; - cpVect b = cpSegmentShapeGetB(shape); - b.x *= factorX; - b.y *= factorY; - cpSegmentShapeSetEndpoints(shape, a, b); - - PhysicsShape::updateScale(); -} - -// PhysicsShapeBox -PhysicsShapeBox* PhysicsShapeBox::create(const Vec2& size, - const PhysicsMaterial& material /* = MaterialDefault*/, - const Vec2& offset /* = Vec2(0, 0)*/, - float radius /* = 0.0f*/) -{ - PhysicsShapeBox* shape = new PhysicsShapeBox(); - if (shape->init(size, material, offset, radius)) - { - shape->autorelease(); - return shape; - } - - AX_SAFE_DELETE(shape); - return nullptr; -} - -bool PhysicsShapeBox::init(const Vec2& size, - const PhysicsMaterial& material /* = MaterialDefault*/, - const Vec2& offset /*= Vec2(0, 0)*/, - float radius /* = 0.0f*/) -{ - do - { - _type = Type::BOX; - - auto wh = PhysicsHelper::vec22cpv(size); - cpVect vec[4] = {{-wh.x / 2.0f, -wh.y / 2.0f}, - {-wh.x / 2.0f, wh.y / 2.0f}, - {wh.x / 2.0f, wh.y / 2.0f}, - {wh.x / 2.0f, -wh.y / 2.0f}}; - - cpTransform transform = cpTransformTranslate(PhysicsHelper::vec22cpv(offset)); - - auto shape = cpPolyShapeNew(s_sharedBody, 4, vec, transform, radius); - AX_BREAK_IF(shape == nullptr); - cpShapeSetUserData(shape, this); - - addShape(shape); - - _area = calculateArea(); - _mass = material.density == PHYSICS_INFINITY ? PHYSICS_INFINITY : material.density * _area; - _moment = calculateDefaultMoment(); - - setMaterial(material); - - return true; - } while (false); - - return false; -} - -Vec2 PhysicsShapeBox::getSize() const -{ - cpShape* shape = _cpShapes.front(); - return PhysicsHelper::cpv2vec2(cpv(cpvdist(cpPolyShapeGetVert(shape, 1), cpPolyShapeGetVert(shape, 2)), - cpvdist(cpPolyShapeGetVert(shape, 0), cpPolyShapeGetVert(shape, 1)))); -} - -// PhysicsShapePolygon -PhysicsShapePolygon* PhysicsShapePolygon::create(const Vec2* points, - int count, - const PhysicsMaterial& material /* = MaterialDefault*/, - const Vec2& offset /* = Vec2(0, 0)*/, - float radius /* = 0.0f*/) -{ - PhysicsShapePolygon* shape = new PhysicsShapePolygon(); - if (shape->init(points, count, material, offset, radius)) - { - shape->autorelease(); - return shape; - } - - AX_SAFE_DELETE(shape); - return nullptr; -} - -bool PhysicsShapePolygon::init(const Vec2* points, - int count, - const PhysicsMaterial& material /* = MaterialDefault*/, - const Vec2& offset /* = Vec2(0, 0)*/, - float radius /* = 0.0f*/) -{ - do - { - _type = Type::POLYGON; - - auto vecs = new cpVect[count]; - PhysicsHelper::points2cpvs(points, vecs, count); // count = cpConvexHull((int)count, vecs, nullptr, nullptr, - // 0); - cpTransform transform = cpTransformTranslate(PhysicsHelper::vec22cpv(offset)); - auto shape = cpPolyShapeNew(s_sharedBody, count, vecs, transform, radius); - AX_SAFE_DELETE_ARRAY(vecs); - - AX_BREAK_IF(shape == nullptr); - cpShapeSetUserData(shape, this); - - addShape(shape); - - _area = calculateArea(); - _mass = material.density == PHYSICS_INFINITY ? PHYSICS_INFINITY : material.density * _area; - _moment = calculateDefaultMoment(); - - setMaterial(material); - - return true; - } while (false); - - return false; -} - -float PhysicsShapePolygon::calculateArea(const Vec2* points, int count) -{ - cpVect* vecs = new cpVect[count]; - PhysicsHelper::points2cpvs(points, vecs, count); - float area = PhysicsHelper::cpfloat2float(cpAreaForPoly(count, vecs, 0.0f)); - AX_SAFE_DELETE_ARRAY(vecs); - - return area; -} - -float PhysicsShapePolygon::calculateMoment(float mass, const Vec2* points, int count, const Vec2& offset, float radius) -{ - cpVect* vecs = new cpVect[count]; - PhysicsHelper::points2cpvs(points, vecs, count); - float moment = - mass == PHYSICS_INFINITY - ? PHYSICS_INFINITY - : PhysicsHelper::cpfloat2float(cpMomentForPoly(mass, count, vecs, PhysicsHelper::vec22cpv(offset), radius)); - AX_SAFE_DELETE_ARRAY(vecs); - - return moment; -} - -float PhysicsShapePolygon::calculateArea() -{ - auto shape = _cpShapes.front(); - int count = cpPolyShapeGetCount(shape); - cpVect* vecs = new cpVect[count]; - for (int i = 0; i < count; ++i) - vecs[i] = cpPolyShapeGetVert(shape, i); - float area = PhysicsHelper::cpfloat2float(cpAreaForPoly(count, vecs, cpPolyShapeGetRadius(shape))); - AX_SAFE_DELETE_ARRAY(vecs); - return area; -} - -float PhysicsShapePolygon::calculateDefaultMoment() -{ - if (_mass == PHYSICS_INFINITY) - { - return PHYSICS_INFINITY; - } - else - { - auto shape = _cpShapes.front(); - int count = cpPolyShapeGetCount(shape); - cpVect* vecs = new cpVect[count]; - for (int i = 0; i < count; ++i) - vecs[i] = cpPolyShapeGetVert(shape, i); - float moment = - PhysicsHelper::cpfloat2float(cpMomentForPoly(_mass, count, vecs, cpvzero, cpPolyShapeGetRadius(shape))); - AX_SAFE_DELETE_ARRAY(vecs); - return moment; - } -} - -Vec2 PhysicsShapePolygon::getPoint(int i) const -{ - return PhysicsHelper::cpv2vec2(cpPolyShapeGetVert(_cpShapes.front(), i)); -} - -void PhysicsShapePolygon::getPoints(Vec2* outPoints) const -{ - auto shape = _cpShapes.front(); - int count = cpPolyShapeGetCount(shape); - cpVect* vecs = new cpVect[count]; - for (int i = 0; i < count; ++i) - vecs[i] = cpPolyShapeGetVert(shape, i); - PhysicsHelper::cpvs2points(vecs, outPoints, count); - AX_SAFE_DELETE_ARRAY(vecs); -} - -int PhysicsShapePolygon::getPointsCount() const -{ - return cpPolyShapeGetCount(_cpShapes.front()); -} - -Vec2 PhysicsShapePolygon::getCenter() -{ - auto shape = _cpShapes.front(); - int count = cpPolyShapeGetCount(shape); - cpVect* vecs = new cpVect[count]; - for (int i = 0; i < count; ++i) - vecs[i] = cpPolyShapeGetVert(shape, i); - - Vec2 center = PhysicsHelper::cpv2vec2(cpCentroidForPoly(count, vecs)); - AX_SAFE_DELETE_ARRAY(vecs); - - return center; -} - -void PhysicsShapePolygon::updateScale() -{ - cpFloat factorX = _newScaleX / _scaleX; - cpFloat factorY = _newScaleY / _scaleY; - - auto shape = _cpShapes.front(); - int count = cpPolyShapeGetCount(shape); - cpVect* vects = new cpVect[count]; - for (int i = 0; i < count; ++i) - vects[i] = cpPolyShapeGetVert(shape, i); - - for (int i = 0; i < count; ++i) - { - vects[i].x *= factorX; - vects[i].y *= factorY; - } - - // convert hole to clockwise - if (factorX * factorY < 0) - { - for (int i = 0; i < count / 2; ++i) - { - cpVect v = vects[i]; - vects[i] = vects[count - i - 1]; - vects[count - i - 1] = v; - } - } - - cpPolyShapeSetVertsRaw(shape, count, vects); - AX_SAFE_DELETE_ARRAY(vects); - - PhysicsShape::updateScale(); -} - -// PhysicsShapeEdgeBox -PhysicsShapeEdgeBox* PhysicsShapeEdgeBox::create(const Vec2& size, - const PhysicsMaterial& material /* = MaterialDefault*/, - float border /* = 1*/, - const Vec2& offset /* = Vec2(0, 0)*/) -{ - PhysicsShapeEdgeBox* shape = new PhysicsShapeEdgeBox(); - if (shape->init(size, material, border, offset)) - { - shape->autorelease(); - return shape; - } - - AX_SAFE_DELETE(shape); - return nullptr; -} - -bool PhysicsShapeEdgeBox::init(const Vec2& size, - const PhysicsMaterial& material /* = MaterialDefault*/, - float border /* = 1*/, - const Vec2& offset /*= Vec2(0, 0)*/) -{ - do - { - _type = Type::EDGEBOX; - - cpVect vec[4] = {}; - vec[0] = PhysicsHelper::vec22cpv(Vec2(-size.width / 2 + offset.x, -size.height / 2 + offset.y)); - vec[1] = PhysicsHelper::vec22cpv(Vec2(+size.width / 2 + offset.x, -size.height / 2 + offset.y)); - vec[2] = PhysicsHelper::vec22cpv(Vec2(+size.width / 2 + offset.x, +size.height / 2 + offset.y)); - vec[3] = PhysicsHelper::vec22cpv(Vec2(-size.width / 2 + offset.x, +size.height / 2 + offset.y)); - - int i = 0; - for (; i < 4; ++i) - { - auto shape = cpSegmentShapeNew(s_sharedBody, vec[i], vec[(i + 1) % 4], border); - AX_BREAK_IF(shape == nullptr); - cpShapeSetUserData(shape, this); - addShape(shape); - } - AX_BREAK_IF(i < 4); - - _mass = PHYSICS_INFINITY; - _moment = PHYSICS_INFINITY; - - setMaterial(material); - - return true; - } while (false); - - return false; -} - -// PhysicsShapeEdgeBox -PhysicsShapeEdgePolygon* PhysicsShapeEdgePolygon::create(const Vec2* points, - int count, - const PhysicsMaterial& material /* = MaterialDefault*/, - float border /* = 1*/) -{ - PhysicsShapeEdgePolygon* shape = new PhysicsShapeEdgePolygon(); - if (shape->init(points, count, material, border)) - { - shape->autorelease(); - return shape; - } - - AX_SAFE_DELETE(shape); - return nullptr; -} - -bool PhysicsShapeEdgePolygon::init(const Vec2* points, - int count, - const PhysicsMaterial& material /* = MaterialDefault*/, - float border /* = 1*/) -{ - cpVect* vec = nullptr; - do - { - _type = Type::EDGEPOLYGON; - - vec = new cpVect[count]; - PhysicsHelper::points2cpvs(points, vec, count); - - int i = 0; - for (; i < count; ++i) - { - auto shape = cpSegmentShapeNew(s_sharedBody, vec[i], vec[(i + 1) % count], border); - AX_BREAK_IF(shape == nullptr); - cpShapeSetUserData(shape, this); - cpShapeSetElasticity(shape, 1.0f); - cpShapeSetFriction(shape, 1.0f); - addShape(shape); - } - AX_SAFE_DELETE_ARRAY(vec); - - AX_BREAK_IF(i < count); - - _mass = PHYSICS_INFINITY; - _moment = PHYSICS_INFINITY; - - setMaterial(material); - - return true; - } while (false); - - AX_SAFE_DELETE_ARRAY(vec); - - return false; -} - -Vec2 PhysicsShapeEdgePolygon::getCenter() -{ - int count = (int)_cpShapes.size(); - cpVect* points = new cpVect[count]; - int i = 0; - for (auto&& shape : _cpShapes) - { - points[i++] = cpSegmentShapeGetA(shape); - } - - Vec2 center = PhysicsHelper::cpv2vec2(cpCentroidForPoly(count, points)); - delete[] points; - - return center; -} - -void PhysicsShapeEdgePolygon::getPoints(ax::Vec2* outPoints) const -{ - int i = 0; - for (auto&& shape : _cpShapes) - { - outPoints[i++] = PhysicsHelper::cpv2vec2(cpSegmentShapeGetA(shape)); - } -} - -int PhysicsShapeEdgePolygon::getPointsCount() const -{ - return static_cast(_cpShapes.size()); -} - -// PhysicsShapeEdgeChain -PhysicsShapeEdgeChain* PhysicsShapeEdgeChain::create(const Vec2* points, - int count, - const PhysicsMaterial& material /* = MaterialDefault*/, - float border /* = 1*/) -{ - PhysicsShapeEdgeChain* shape = new PhysicsShapeEdgeChain(); - if (shape->init(points, count, material, border)) - { - shape->autorelease(); - return shape; - } - - AX_SAFE_DELETE(shape); - return nullptr; -} - -void PhysicsShapeEdgePolygon::updateScale() -{ - cpFloat factorX = _newScaleX / _scaleX; - cpFloat factorY = _newScaleY / _scaleY; - - for (auto&& shape : _cpShapes) - { - cpVect a = cpSegmentShapeGetA(shape); - a.x *= factorX; - a.y *= factorY; - cpVect b = cpSegmentShapeGetB(shape); - b.x *= factorX; - b.y *= factorY; - cpSegmentShapeSetEndpoints(shape, a, b); - } - - PhysicsShape::updateScale(); -} - -bool PhysicsShapeEdgeChain::init(const Vec2* points, - int count, - const PhysicsMaterial& material /* = MaterialDefault*/, - float border /* = 1*/) -{ - cpVect* vec = nullptr; - do - { - _type = Type::EDGECHAIN; - - vec = new cpVect[count]; - PhysicsHelper::points2cpvs(points, vec, count); - - int i = 0; - for (; i < count - 1; ++i) - { - auto shape = cpSegmentShapeNew(s_sharedBody, vec[i], vec[i + 1], border); - AX_BREAK_IF(shape == nullptr); - cpShapeSetUserData(shape, this); - cpShapeSetElasticity(shape, 1.0f); - cpShapeSetFriction(shape, 1.0f); - addShape(shape); - } - AX_SAFE_DELETE_ARRAY(vec); - AX_BREAK_IF(i < count - 1); - - _mass = PHYSICS_INFINITY; - _moment = PHYSICS_INFINITY; - - setMaterial(material); - - return true; - } while (false); - - AX_SAFE_DELETE_ARRAY(vec); - - return false; -} - -Vec2 PhysicsShapeEdgeChain::getCenter() -{ - int count = (int)_cpShapes.size() + 1; - cpVect* points = new cpVect[count]; - int i = 0; - for (auto&& shape : _cpShapes) - { - points[i++] = cpSegmentShapeGetA(shape); - } - - points[i++] = cpSegmentShapeGetB(_cpShapes.back()); - - Vec2 center = PhysicsHelper::cpv2vec2(cpCentroidForPoly(count, points)); - delete[] points; - - return center; -} - -void PhysicsShapeEdgeChain::getPoints(Vec2* outPoints) const -{ - int i = 0; - for (auto&& shape : _cpShapes) - { - outPoints[i++] = PhysicsHelper::cpv2vec2(cpSegmentShapeGetA(shape)); - } - - outPoints[i++] = PhysicsHelper::cpv2vec2(cpSegmentShapeGetB(_cpShapes.back())); -} - -int PhysicsShapeEdgeChain::getPointsCount() const -{ - return static_cast(_cpShapes.size() + 1); -} - -void PhysicsShapeEdgeChain::updateScale() -{ - cpFloat factorX = _newScaleX / _scaleX; - cpFloat factorY = _newScaleY / _scaleY; - - for (auto&& shape : _cpShapes) - { - cpVect a = cpSegmentShapeGetA(shape); - a.x *= factorX; - a.y *= factorY; - cpVect b = cpSegmentShapeGetB(shape); - b.x *= factorX; - b.y *= factorY; - cpSegmentShapeSetEndpoints(shape, a, b); - } - - PhysicsShape::updateScale(); -} - -void PhysicsShape::setGroup(int group) -{ - if (group < 0) - { - for (auto&& shape : _cpShapes) - { - cpShapeSetFilter(shape, cpShapeFilterNew(group, CP_ALL_CATEGORIES, CP_ALL_CATEGORIES)); - } - } - - _group = group; -} - -bool PhysicsShape::containsPoint(const Vec2& point) const -{ - for (auto&& shape : _cpShapes) - { - if (cpShapePointQuery(shape, PhysicsHelper::vec22cpv(point), nullptr) < 0) - { - return true; - } - } - - return false; -} - -} - -#endif // defined(AX_ENABLE_PHYSICS) diff --git a/core/physics/PhysicsShape.h b/core/physics/PhysicsShape.h deleted file mode 100644 index e37d2cb116fc..000000000000 --- a/core/physics/PhysicsShape.h +++ /dev/null @@ -1,791 +0,0 @@ -/**************************************************************************** - Copyright (c) 2013-2016 Chukong Technologies Inc. - Copyright (c) 2017-2018 Xiamen Yaji Software Co., Ltd. - Copyright (c) 2019-present Axmol Engine contributors (see AUTHORS.md). - - https://axmol.dev/ - - 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 __CCPHYSICS_SHAPE_H__ -#define __CCPHYSICS_SHAPE_H__ - -#include "base/Config.h" -#if defined(AX_ENABLE_PHYSICS) - -# include "base/Object.h" -# include "math/Math.h" - -struct cpShape; - -namespace ax -{ - -class PhysicsBody; - -typedef struct AX_DLL PhysicsMaterial -{ - float density; ///< The density of the object. - float restitution; ///< The bounciness of the physics body. - float friction; ///< The roughness of the surface of a shape. - - PhysicsMaterial() : density(0.0f), restitution(0.0f), friction(0.0f) {} - - PhysicsMaterial(float aDensity, float aRestitution, float aFriction) - : density(aDensity), restitution(aRestitution), friction(aFriction) - {} -} PhysicsMaterial; - -const PhysicsMaterial PHYSICSSHAPE_MATERIAL_DEFAULT; - -/** - * @addtogroup physics - * @{ - * @addtogroup physics_2d - * @{ - - */ - -/** - * @brief A shape for body. You do not create PhysicsWorld objects directly, instead, you can view PhysicsBody to see - * how to create it. - */ -class AX_DLL PhysicsShape : public Object -{ -public: - enum class Type - { - UNKNOWN, - CIRCLE, - BOX, - POLYGON, - EDGESEGMENT, - EDGEBOX, - EDGEPOLYGON, - EDGECHAIN, - - /** @deprecated Use Type::POLYGON instead. */ - POLYGEN = POLYGON, - - /** @deprecated Use Type::EDGEPOLYGON instead. */ - EDGEPOLYGEN = EDGEPOLYGON, - }; - -public: - /** - * Get the body that this shape attaches. - * - * @return A PhysicsBody object pointer. - */ - PhysicsBody* getBody() const { return _body; } - - /** - * Return this shape's type. - * - * @return A Type object. - */ - Type getType() const { return _type; } - - /** - * Return this shape's area. - * - * @return A float number. - */ - float getArea() const { return _area; } - - /** - * Get this shape's moment. - * - * @return A float number. - */ - float getMoment() const { return _moment; } - - /** - * Set this shape's moment. - * - * It will change the body's moment this shape attaches. - * - * @param moment A float number. - */ - void setMoment(float moment); - - /** - * Set this shape's tag. - * - * @param tag An integer number that identifies a shape object. - */ - void setTag(int tag) { _tag = tag; } - - /** - * Get this shape's tag. - * - * @return An integer number. - */ - int getTag() const { return _tag; } - - /** - * Get the mass of this shape. - * - * @return A float number. - */ - float getMass() const { return _mass; } - - /** - * Set this shape's mass. - * - * It will change the body's mass this shape attaches. - * - * @param mass A float number. - */ - void setMass(float mass); - - /** - * Get this shape's density. - * - * @return A float number. - */ - float getDensity() const { return _material.density; } - - /** - * Set this shape's density. - * - * It will change the body's mass this shape attaches. - * - * @param density A float number. - */ - void setDensity(float density); - - /** - * Get this shape's restitution. - * - * @return A float number. - */ - float getRestitution() const { return _material.restitution; } - - /** - * Set this shape's restitution. - * - * It will change the shape's elasticity. - * - * @param restitution A float number. - */ - void setRestitution(float restitution); - - /** - * Get this shape's friction. - * - * @return A float number. - */ - float getFriction() const { return _material.friction; } - - /** - * Set this shape's friction. - * - * It will change the shape's friction. - * - * @param friction A float number. - */ - void setFriction(float friction); - - /** - * Get this shape's PhysicsMaterial object. - * - * @return A PhysicsMaterial object reference. - */ - const PhysicsMaterial& getMaterial() const { return _material; } - - /** - * Set this shape's material. - * - * It will change the shape's mass, elasticity and friction. - * - * @param material A PhysicsMaterial object. - */ - void setMaterial(const PhysicsMaterial& material); - bool isSensor() const { return _sensor; } - void setSensor(bool sensor); - - /** - * Calculate the default moment value. - * - * This function should be overridden in inherit classes. - * @return A float number, equals 0.0. - */ - virtual float calculateDefaultMoment() { return 0.0f; } - - /** - * Get this shape's position offset. - * - * This function should be overridden in inherit classes. - * @return A Vec2 object. - */ - virtual Vec2 getOffset() { return Vec2::ZERO; } - - /** - * Get this shape's center position. - * - * This function should be overridden in inherit classes. - * @return A Vec2 object. - */ - virtual Vec2 getCenter() { return getOffset(); } - - /** - * Test point is inside this shape or not. - * - * @param point A Vec2 object. - * @return A bool object. - */ - bool containsPoint(const Vec2& point) const; - - /** - * Move the points to the center. - * - * @param points A Vec2 object pointer. - * @param count An integer number. - * @param center A Vec2 object, default value is Vec2(0,0). - */ - static void recenterPoints(Vec2* points, int count, const Vec2& center = Vec2::ZERO); - - /** - * Get center of the polygon points. - * - * @param points A Vec2 object pointer. - * @param count An integer number. - * @return A Vec2 object. - */ - static Vec2 getPolygonCenter(const Vec2* points, int count); - - /** - * Set a mask that defines which categories this physics body belongs to. - * - * Every physics body in a scene can be assigned to up to 32 different categories, each corresponding to a bit in - * the bit mask. You define the mask values used in your game. In conjunction with the collisionBitMask and - * contactTestBitMask properties, you define which physics bodies interact with each other and when your game is - * notified of these interactions. - * @param bitmask An integer number, the default value is 0xFFFFFFFF (all bits set). - */ - void setCategoryBitmask(int bitmask) { _categoryBitmask = bitmask; } - - /** - * Get a mask that defines which categories this physics body belongs to. - * - * @return An integer number. - */ - int getCategoryBitmask() const { return _categoryBitmask; } - - /** - * A mask that defines which categories of bodies cause intersection notifications with this physics body. - * - * When two bodies share the same space, each body's category mask is tested against the other body's contact mask - * by performing a logical AND operation. If either comparison results in a non-zero value, an PhysicsContact object - * is created and passed to the physics world’s delegate. For best performance, only set bits in the contacts mask - * for interactions you are interested in. - * @param bitmask An integer number, the default value is 0x00000000 (all bits cleared). - */ - void setContactTestBitmask(int bitmask) { _contactTestBitmask = bitmask; } - - /** - * Get a mask that defines which categories of bodies cause intersection notifications with this physics body. - * - * @return An integer number. - */ - int getContactTestBitmask() const { return _contactTestBitmask; } - - /** - * A mask that defines which categories of physics bodies can collide with this physics body. - * - * When two physics bodies contact each other, a collision may occur. This body's collision mask is compared to the - * other body's category mask by performing a logical AND operation. If the result is a non-zero value, then this - * body is affected by the collision. Each body independently chooses whether it wants to be affected by the other - * body. For example, you might use this to avoid collision calculations that would make negligible changes to a - * body's velocity. - * @param bitmask An integer number, the default value is 0xFFFFFFFF (all bits set). - */ - void setCollisionBitmask(int bitmask) { _collisionBitmask = bitmask; } - - /** - * Get a mask that defines which categories of physics bodies can collide with this physics body. - * - * @return An integer number. - */ - int getCollisionBitmask() const { return _collisionBitmask; } - - /** - * Set the group of body. - * - * Collision groups let you specify an integral group index. You can have all fixtures with the same group index - * always collide (positive index) or never collide (negative index). - * @param group An integer number, it have high priority than bit masks. - */ - void setGroup(int group); - - /** - * Get the group of body. - * - * @return An integer number. - */ - int getGroup() { return _group; } - -protected: - void setBody(PhysicsBody* body); - - /** calculate the area of this shape */ - virtual float calculateArea() { return 0.0f; } - - virtual void setScale(float scaleX, float scaleY); - virtual void updateScale(); - void addShape(cpShape* shape); - -protected: - PhysicsShape(); - virtual ~PhysicsShape() = 0; - -protected: - PhysicsBody* _body; - std::vector _cpShapes; - - Type _type; - float _area; - float _mass; - float _moment; - bool _sensor; - float _scaleX; - float _scaleY; - float _newScaleX; - float _newScaleY; - PhysicsMaterial _material; - int _tag; - int _categoryBitmask; - int _collisionBitmask; - int _contactTestBitmask; - int _group; - - friend class PhysicsWorld; - friend class PhysicsBody; - friend class PhysicsJoint; - friend class PhysicsDebugDraw; -}; - -/** A circle shape. */ -class AX_DLL PhysicsShapeCircle : public PhysicsShape -{ -public: - /** - * Creates a PhysicsShapeCircle with specified value. - * - * @param radius A float number, it is the circle's radius. - * @param material A PhysicsMaterial object, the default value is PHYSICSSHAPE_MATERIAL_DEFAULT. - * @param offset A Vec2 object, it is the offset from the body's center of gravity in body local coordinates. - * @return An autoreleased PhysicsShapeCircle object pointer. - */ - static PhysicsShapeCircle* create(float radius, - const PhysicsMaterial& material = PHYSICSSHAPE_MATERIAL_DEFAULT, - const Vec2& offset = Vec2(0.0f, 0.0f)); - - /** - * Calculate the area of a circle with specified radius. - * - * @param radius A float number - * @return A float number - */ - static float calculateArea(float radius); - - /** - * Calculate the moment of a circle with specified value. - * - * @param mass A float number - * @param radius A float number - * @param offset A Vec2 object, it is the offset from the body's center of gravity in body local coordinates. - * @return A float number - */ - static float calculateMoment(float mass, float radius, const Vec2& offset = Vec2::ZERO); - - /** - * Calculate the moment for a circle. - * - * @return A float number. - */ - virtual float calculateDefaultMoment() override; - - /** - * Get the circle's radius. - * - * @return A float number. - */ - float getRadius() const; - - /** - * Get this circle's position offset. - * - * @return A Vec2 object. - */ - virtual Vec2 getOffset() override; - -protected: - bool init(float radius, - const PhysicsMaterial& material = PHYSICSSHAPE_MATERIAL_DEFAULT, - const Vec2& offset = Vec2::ZERO); - virtual float calculateArea() override; - virtual void updateScale() override; - -protected: - PhysicsShapeCircle(); - virtual ~PhysicsShapeCircle(); -}; - -/** A polygon shape. */ -class AX_DLL PhysicsShapePolygon : public PhysicsShape -{ -public: - /** - * Creates a PhysicsShapePolygon with specified value. - * - * @param points A Vec2 object pointer, it is an array of Vec2. - * @param count An integer number, contains the count of the points array. - * @param material A PhysicsMaterial object, the default value is PHYSICSSHAPE_MATERIAL_DEFAULT. - * @param offset A Vec2 object, it is the offset from the body's center of gravity in body local coordinates. - * @return An autoreleased PhysicsShapePolygon object pointer. - */ - static PhysicsShapePolygon* create(const Vec2* points, - int count, - const PhysicsMaterial& material = PHYSICSSHAPE_MATERIAL_DEFAULT, - const Vec2& offset = Vec2::ZERO, - float radius = 0.0f); - - /** - * Calculate the area of a polygon with specified value. - * - * @param points A Vec2 object pointer, it is an array of Vec2. - * @param count An integer number, contains the count of the points array. - * @return A float number. - */ - static float calculateArea(const Vec2* points, int count); - - /** - * Calculate the moment of a polygon with specified value. - * - * @param mass A float number - * @param points A Vec2 object pointer, it is an array of Vec2. - * @param count An integer number, contains the count of the points array. - * @param offset A Vec2 object, it is the offset from the body's center of gravity in body local coordinates. - * @return A float number - */ - static float calculateMoment(float mass, - const Vec2* points, - int count, - const Vec2& offset = Vec2::ZERO, - float radius = 0.0f); - - /** - * Calculate the moment for a polygon. - * - * @return A float number. - */ - float calculateDefaultMoment() override; - - /** - * Get a point of this polygon's points array. - * - * @param i A index of this polygon's points array. - * @return A point value. - */ - Vec2 getPoint(int i) const; - - /** - * Get this polygon's points array. - * - * @param outPoints A Vec2 array pointer. - */ - void getPoints(Vec2* outPoints) const; - - /** - * Get this polygon's points array count. - * - * @return An integer number. - */ - int getPointsCount() const; - - /** - * Get this polygon's center position. - * - * @return A Vec2 object. - */ - virtual Vec2 getCenter() override; - -protected: - bool init(const Vec2* points, - int count, - const PhysicsMaterial& material = PHYSICSSHAPE_MATERIAL_DEFAULT, - const Vec2& offset = Vec2::ZERO, - float radius = 0.0f); - float calculateArea() override; - virtual void updateScale() override; - -protected: - PhysicsShapePolygon(); - virtual ~PhysicsShapePolygon(); -}; - -/** A box shape. */ -class AX_DLL PhysicsShapeBox : public PhysicsShapePolygon -{ -public: - /** - * Creates a PhysicsShapeBox with specified value. - * - * @param size The size contains this box's width and height. - * @param material A PhysicsMaterial object, the default value is PHYSICSSHAPE_MATERIAL_DEFAULT. - * @param offset A Vec2 object, it is the offset from the body's center of gravity in body local coordinates. - * @return An autoreleased PhysicsShapeBox object pointer. - */ - static PhysicsShapeBox* create(const Vec2& size, - const PhysicsMaterial& material = PHYSICSSHAPE_MATERIAL_DEFAULT, - const Vec2& offset = Vec2::ZERO, - float radius = 0.0f); - - /** - * Get this box's width and height. - * - * @return An Vec2 object. - */ - Vec2 getSize() const; - - /** - * Get this box's position offset. - * - * @return A Vec2 object. - */ - virtual Vec2 getOffset() override { return getCenter(); } - -protected: - bool init(const Vec2& size, - const PhysicsMaterial& material = PHYSICSSHAPE_MATERIAL_DEFAULT, - const Vec2& offset = Vec2::ZERO, - float radius = 0.0f); - -protected: - PhysicsShapeBox(); - virtual ~PhysicsShapeBox(); -}; - -/** A segment shape. */ -class AX_DLL PhysicsShapeEdgeSegment : public PhysicsShape -{ -public: - /** - * Creates a PhysicsShapeEdgeSegment with specified value. - * - * @param a It's the edge's begin position. - * @param b It's the edge's end position. - * @param material A PhysicsMaterial object, the default value is PHYSICSSHAPE_MATERIAL_DEFAULT. - * @param border It's a edge's border width. - * @return An autoreleased PhysicsShapeEdgeSegment object pointer. - */ - static PhysicsShapeEdgeSegment* create(const Vec2& a, - const Vec2& b, - const PhysicsMaterial& material = PHYSICSSHAPE_MATERIAL_DEFAULT, - float border = 1); - - /** - * Get this edge's begin position. - * - * @return A Vec2 object. - */ - Vec2 getPointA() const; - - /** - * Get this edge's end position. - * - * @return A Vec2 object. - */ - Vec2 getPointB() const; - - /** - * Get this edge's center position. - * - * @return A Vec2 object. - */ - virtual Vec2 getCenter() override; - -protected: - bool init(const Vec2& a, - const Vec2& b, - const PhysicsMaterial& material = PHYSICSSHAPE_MATERIAL_DEFAULT, - float border = 1); - virtual void updateScale() override; - -protected: - PhysicsShapeEdgeSegment(); - virtual ~PhysicsShapeEdgeSegment(); - - friend class PhysicsBody; -}; - -/** An edge polygon shape. */ -class AX_DLL PhysicsShapeEdgePolygon : public PhysicsShape -{ -public: - /** - * Creates a PhysicsShapeEdgePolygon with specified value. - * - * @param points A Vec2 object pointer, it contains an array of points. - * @param count An integer number, contains the count of the points array. - * @param material A PhysicsMaterial object, the default value is PHYSICSSHAPE_MATERIAL_DEFAULT. - * @param border It's a edge's border width. - * @return An autoreleased PhysicsShapeEdgePolygon object pointer. - */ - static PhysicsShapeEdgePolygon* create(const Vec2* points, - int count, - const PhysicsMaterial& material = PHYSICSSHAPE_MATERIAL_DEFAULT, - float border = 1); - - /** - * Get this polygon's center position. - * - * @return A Vec2 object. - */ - virtual Vec2 getCenter() override; - - /** - * Get this polygon's points array. - * - * @param outPoints A Vec2 array pointer. - */ - void getPoints(Vec2* outPoints) const; - - /** - * Get this polygon's points array count. - * - * @return An integer number. - */ - int getPointsCount() const; - -protected: - bool init(const Vec2* points, - int count, - const PhysicsMaterial& material = PHYSICSSHAPE_MATERIAL_DEFAULT, - float border = 1); - virtual void updateScale() override; - -protected: - PhysicsShapeEdgePolygon(); - virtual ~PhysicsShapeEdgePolygon(); - - friend class PhysicsBody; -}; - -/** An edge box shape. */ -class AX_DLL PhysicsShapeEdgeBox : public PhysicsShapeEdgePolygon -{ -public: - /** - * Creates a PhysicsShapeEdgeBox with specified value. - * - * @param size The size contains this box's width and height. - * @param material A PhysicsMaterial object, the default value is PHYSICSSHAPE_MATERIAL_DEFAULT. - * @param border It's a edge's border width. - * @param offset A Vec2 object, it is the offset from the body's center of gravity in body local coordinates. - * @return An autoreleased PhysicsShapeEdgeBox object pointer. - */ - static PhysicsShapeEdgeBox* create(const Vec2& size, - const PhysicsMaterial& material = PHYSICSSHAPE_MATERIAL_DEFAULT, - float border = 0, - const Vec2& offset = Vec2::ZERO); - - /** - * Get this box's position offset. - * - * @return A Vec2 object. - */ - virtual Vec2 getOffset() override { return getCenter(); } - -protected: - bool init(const Vec2& size, - const PhysicsMaterial& material = PHYSICSSHAPE_MATERIAL_DEFAULT, - float border = 1, - const Vec2& offset = Vec2::ZERO); - -protected: - PhysicsShapeEdgeBox(); - virtual ~PhysicsShapeEdgeBox(); - - friend class PhysicsBody; -}; - -/** A chain shape. */ -class AX_DLL PhysicsShapeEdgeChain : public PhysicsShape -{ -public: - /** - * Creates a PhysicsShapeEdgeChain with specified value. - * - * @param points A Vec2 object pointer, it contains an array of points. - * @param count An integer number, contains the count of the points array. - * @param material A PhysicsMaterial object, the default value is PHYSICSSHAPE_MATERIAL_DEFAULT. - * @param border It's a edge's border width. - * @return An autoreleased PhysicsShapeEdgeChain object pointer. - */ - static PhysicsShapeEdgeChain* create(const Vec2* points, - int count, - const PhysicsMaterial& material = PHYSICSSHAPE_MATERIAL_DEFAULT, - float border = 1); - - /** - * Get this chain's center position. - * - * @return A Vec2 object. - */ - virtual Vec2 getCenter() override; - - /** - * Get this chain's points array. - * - * @param outPoints A Vec2 array pointer. - */ - void getPoints(Vec2* outPoints) const; - - /** - * Get this chain's points array count. - * - * @return An integer number. - */ - int getPointsCount() const; - -protected: - bool init(const Vec2* points, - int count, - const PhysicsMaterial& material = PHYSICSSHAPE_MATERIAL_DEFAULT, - float border = 1); - virtual void updateScale() override; - -protected: - PhysicsShapeEdgeChain(); - virtual ~PhysicsShapeEdgeChain(); - - friend class PhysicsBody; -}; - -/** @} */ -/** @} */ - -} - -#endif // defined(AX_ENABLE_PHYSICS) -#endif // __CCPHYSICS_FIXTURE_H__ diff --git a/core/physics/PhysicsWorld.cpp b/core/physics/PhysicsWorld.cpp deleted file mode 100644 index a2cfcbe8dae6..000000000000 --- a/core/physics/PhysicsWorld.cpp +++ /dev/null @@ -1,1107 +0,0 @@ -/**************************************************************************** - Copyright (c) 2013-2016 Chukong Technologies Inc. - Copyright (c) 2017-2018 Xiamen Yaji Software Co., Ltd. - Copyright (c) 2019-present Axmol Engine contributors (see AUTHORS.md). - - https://axmol.dev/ - - 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 "physics/PhysicsWorld.h" -#if defined(AX_ENABLE_PHYSICS) -# include -# include - -# include "chipmunk/chipmunk_private.h" -# include "physics/PhysicsBody.h" -# include "physics/PhysicsShape.h" -# include "physics/PhysicsContact.h" -# include "physics/PhysicsJoint.h" -# include "physics/PhysicsHelper.h" - -# include "2d/DrawNode.h" -# include "2d/Scene.h" -# include "base/Director.h" -# include "base/EventDispatcher.h" -# include "base/EventCustom.h" - -namespace ax -{ -const float PHYSICS_INFINITY = FLT_MAX; -extern const char* PHYSICSCONTACT_EVENT_NAME; - -const int PhysicsWorld::DEBUGDRAW_NONE = 0x00; -const int PhysicsWorld::DEBUGDRAW_SHAPE = 0x01; -const int PhysicsWorld::DEBUGDRAW_JOINT = 0x02; -const int PhysicsWorld::DEBUGDRAW_CONTACT = 0x04; -const int PhysicsWorld::DEBUGDRAW_ALL = DEBUGDRAW_SHAPE | DEBUGDRAW_JOINT | DEBUGDRAW_CONTACT; - -const float _debugDrawThickness = 0.5f; // thickness of the DebugDraw lines, circles, dots, polygons - -namespace -{ -typedef struct RayCastCallbackInfo -{ - PhysicsWorld* world; - PhysicsRayCastCallbackFunc func; - Vec2 p1; - Vec2 p2; - void* data; -} RayCastCallbackInfo; - -typedef struct RectQueryCallbackInfo -{ - PhysicsWorld* world; - PhysicsQueryRectCallbackFunc func; - void* data; -} RectQueryCallbackInfo; - -typedef struct PointQueryCallbackInfo -{ - PhysicsWorld* world; - PhysicsQueryPointCallbackFunc func; - void* data; -} PointQueryCallbackInfo; -} // namespace - -class PhysicsWorldCallback -{ -public: - static cpBool collisionBeginCallbackFunc(cpArbiter* arb, struct cpSpace* space, PhysicsWorld* world); - static cpBool collisionPreSolveCallbackFunc(cpArbiter* arb, cpSpace* space, PhysicsWorld* world); - static void collisionPostSolveCallbackFunc(cpArbiter* arb, cpSpace* space, PhysicsWorld* world); - static void collisionSeparateCallbackFunc(cpArbiter* arb, cpSpace* space, PhysicsWorld* world); - static void rayCastCallbackFunc(cpShape* shape, - cpVect point, - cpVect normal, - cpFloat alpha, - RayCastCallbackInfo* info); - static void queryRectCallbackFunc(cpShape* shape, RectQueryCallbackInfo* info); - static void queryPointFunc(cpShape* shape, - cpVect point, - cpFloat distance, - cpVect gradient, - PointQueryCallbackInfo* info); - static void getShapesAtPointFunc(cpShape* shape, - cpVect point, - cpFloat distance, - cpVect gradient, - Vector* arr); - -public: - static bool continues; -}; - -bool PhysicsWorldCallback::continues = true; - -cpBool PhysicsWorldCallback::collisionBeginCallbackFunc(cpArbiter* arb, struct cpSpace* /*space*/, PhysicsWorld* world) -{ - CP_ARBITER_GET_SHAPES(arb, a, b); - - PhysicsShape* shapeA = static_cast(cpShapeGetUserData(a)); - PhysicsShape* shapeB = static_cast(cpShapeGetUserData(b)); - AX_ASSERT(shapeA != nullptr && shapeB != nullptr); - - auto contact = PhysicsContact::construct(shapeA, shapeB); - cpArbiterSetUserData(arb, contact); - contact->_contactInfo = arb; - - return world->collisionBeginCallback(*contact); -} - -cpBool PhysicsWorldCallback::collisionPreSolveCallbackFunc(cpArbiter* arb, cpSpace* /*space*/, PhysicsWorld* world) -{ - return world->collisionPreSolveCallback(*static_cast(cpArbiterGetUserData(arb))); -} - -void PhysicsWorldCallback::collisionPostSolveCallbackFunc(cpArbiter* arb, cpSpace* /*space*/, PhysicsWorld* world) -{ - world->collisionPostSolveCallback(*static_cast(cpArbiterGetUserData(arb))); -} - -void PhysicsWorldCallback::collisionSeparateCallbackFunc(cpArbiter* arb, cpSpace* /*space*/, PhysicsWorld* world) -{ - PhysicsContact* contact = static_cast(cpArbiterGetUserData(arb)); - - world->collisionSeparateCallback(*contact); - - delete contact; -} - -void PhysicsWorldCallback::rayCastCallbackFunc(cpShape* shape, - cpVect point, - cpVect normal, - cpFloat alpha, - RayCastCallbackInfo* info) -{ - if (!PhysicsWorldCallback::continues) - { - return; - } - - PhysicsShape* physicsShape = static_cast(cpShapeGetUserData(shape)); - AX_ASSERT(physicsShape != nullptr); - - PhysicsRayCastInfo callbackInfo = { - physicsShape, - info->p1, - info->p2, - PhysicsHelper::cpv2vec2(point), - PhysicsHelper::cpv2vec2(normal), - static_cast(alpha), - }; - - PhysicsWorldCallback::continues = info->func(*info->world, callbackInfo, info->data); -} - -void PhysicsWorldCallback::queryRectCallbackFunc(cpShape* shape, RectQueryCallbackInfo* info) -{ - PhysicsShape* physicsShape = static_cast(cpShapeGetUserData(shape)); - AX_ASSERT(physicsShape != nullptr); - - if (!PhysicsWorldCallback::continues) - { - return; - } - - PhysicsWorldCallback::continues = info->func(*info->world, *physicsShape, info->data); -} - -void PhysicsWorldCallback::getShapesAtPointFunc(cpShape* shape, - cpVect /*point*/, - cpFloat /*distance*/, - cpVect /*gradient*/, - Vector* arr) -{ - PhysicsShape* physicsShape = static_cast(cpShapeGetUserData(shape)); - AX_ASSERT(physicsShape != nullptr); - arr->pushBack(physicsShape); -} - -void PhysicsWorldCallback::queryPointFunc(cpShape* shape, - cpVect /*point*/, - cpFloat /*distance*/, - cpVect /*gradient*/, - PointQueryCallbackInfo* info) -{ - PhysicsShape* physicsShape = static_cast(cpShapeGetUserData(shape)); - AX_ASSERT(physicsShape != nullptr); - PhysicsWorldCallback::continues = info->func(*info->world, *physicsShape, info->data); -} - -static inline cpSpaceDebugColor RGBAColor(float r, float g, float b, float a) -{ - cpSpaceDebugColor color = {r, g, b, a}; - return color; -} - -static inline cpSpaceDebugColor LAColor(float l, float a) -{ - cpSpaceDebugColor color = {l, l, l, a}; - return color; -} - -static void DrawCircle(cpVect p, - cpFloat /*a*/, - cpFloat r, - cpSpaceDebugColor outline, - cpSpaceDebugColor fill, - cpDataPointer data) -{ - const Color4B fillColor(fill.r, fill.g, fill.b, fill.a); - const Color4B outlineColor(outline.r, outline.g, outline.b, outline.a); - DrawNode* drawNode = static_cast(data); - float radius = PhysicsHelper::cpfloat2float(r); - Vec2 centre = PhysicsHelper::cpv2vec2(p); - - static const int CIRCLE_SEG_NUM = 12; - Vec2 seg[CIRCLE_SEG_NUM] = {}; - - for (int i = 0; i < CIRCLE_SEG_NUM; ++i) - { - float angle = (float)i * M_PI / (float)CIRCLE_SEG_NUM * 2.0f; - Vec2 d(radius * cosf(angle), radius * sinf(angle)); - seg[i] = centre + d; - } - drawNode->drawPolygon(seg, CIRCLE_SEG_NUM, fillColor, _debugDrawThickness, outlineColor); -} - -static void DrawFatSegment(cpVect a, - cpVect b, - cpFloat r, - cpSpaceDebugColor outline, - cpSpaceDebugColor /*fill*/, - cpDataPointer data) -{ - const Color4B outlineColor(outline.r, outline.g, outline.b, outline.a); - DrawNode* drawNode = static_cast(data); - drawNode->drawSegment(PhysicsHelper::cpv2vec2(a), PhysicsHelper::cpv2vec2(b), - PhysicsHelper::cpfloat2float(r == 0 ? _debugDrawThickness : r), outlineColor); -} - -static void DrawSegment(cpVect a, cpVect b, cpSpaceDebugColor color, cpDataPointer data) -{ - DrawFatSegment(a, b, 0.0, color, color, data); -} - -static void DrawPolygon(int count, - const cpVect* verts, - cpFloat /*r*/, - cpSpaceDebugColor outline, - cpSpaceDebugColor fill, - cpDataPointer data) -{ - const Color4B fillColor(fill.r, fill.g, fill.b, fill.a); - const Color4B outlineColor(outline.r, outline.g, outline.b, outline.a); - DrawNode* drawNode = static_cast(data); - int num = count; - Vec2* seg = new Vec2[num]; - for (int i = 0; i < num; ++i) - seg[i] = PhysicsHelper::cpv2vec2(verts[i]); - - drawNode->drawPolygon(seg, num, fillColor, _debugDrawThickness, outlineColor); - - delete[] seg; -} - -static void DrawDot(cpFloat /*size*/, cpVect pos, cpSpaceDebugColor color, cpDataPointer data) -{ - const Color4B dotColor(color.r, color.g, color.b, color.a); - DrawNode* drawNode = static_cast(data); - drawNode->drawDot(PhysicsHelper::cpv2vec2(pos), _debugDrawThickness, dotColor); -} - -static cpSpaceDebugColor ColorForShape(cpShape* shape, cpDataPointer /*data*/) -{ - if (cpShapeGetSensor(shape)) - { - return LAColor(1.0f, 0.3f); - } - else - { - cpBody* body = cpShapeGetBody(shape); - - if (cpBodyIsSleeping(body)) - { - return LAColor(0.2f, 0.3f); - } - else if (body->sleeping.idleTime > shape->space->sleepTimeThreshold) - { - return LAColor(0.66f, 0.3f); - } - else - { - - float intensity = (cpBodyGetType(body) == CP_BODY_TYPE_STATIC ? 0.15f : 0.75f); - return RGBAColor(intensity, 0.0f, 0.0f, 0.3f); - } - } -} - -void PhysicsWorld::debugDraw() -{ - if (_debugDraw == nullptr) - { - _debugDraw = DrawNode::create(); - _debugDraw->setIsolated(true); - _debugDraw->retain(); - Director::getInstance()->getRunningScene()->addChild(_debugDraw); - } - - cpSpaceDebugDrawOptions drawOptions = { - DrawCircle, - DrawSegment, - DrawFatSegment, - DrawPolygon, - DrawDot, - - (cpSpaceDebugDrawFlags)(_debugDrawMask), - - {1.0f, 0.0f, 0.0f, 1.0f}, - ColorForShape, - {0.0f, 0.75f, 0.0f, 1.0f}, - {0.0f, 0.0f, 1.0f, 1.0f}, - _debugDraw, - }; - if (_debugDraw) - { - _debugDraw->clear(); - cpSpaceDebugDraw(_cpSpace, &drawOptions); - } -} - -bool PhysicsWorld::collisionBeginCallback(PhysicsContact& contact) -{ - bool ret = true; - - PhysicsShape* shapeA = contact.getShapeA(); - PhysicsShape* shapeB = contact.getShapeB(); - PhysicsBody* bodyA = shapeA->getBody(); - PhysicsBody* bodyB = shapeB->getBody(); - auto&& jointsA = bodyA->getJoints(); - - // check the joint is collision enable or not - for (PhysicsJoint* joint : jointsA) - { - if (std::find(_joints.begin(), _joints.end(), joint) == _joints.end()) - { - continue; - } - - if (!joint->isCollisionEnabled()) - { - PhysicsBody* body = joint->getBodyA() == bodyA ? joint->getBodyB() : joint->getBodyA(); - - if (body == bodyB) - { - contact.setNotificationEnable(false); - return false; - } - } - } - - // bitmask check - if ((shapeA->getCategoryBitmask() & shapeB->getContactTestBitmask()) == 0 || - (shapeA->getContactTestBitmask() & shapeB->getCategoryBitmask()) == 0) - { - contact.setNotificationEnable(false); - } - - if (shapeA->getGroup() != 0 && shapeA->getGroup() == shapeB->getGroup()) - { - ret = shapeA->getGroup() > 0; - } - else - { - if ((shapeA->getCategoryBitmask() & shapeB->getCollisionBitmask()) == 0 || - (shapeB->getCategoryBitmask() & shapeA->getCollisionBitmask()) == 0) - { - ret = false; - } - } - - if (contact.isNotificationEnabled()) - { - contact.setEventCode(PhysicsContact::EventCode::BEGIN); - contact.setWorld(this); - _eventDispatcher->dispatchEvent(&contact); - } - - return ret ? contact.resetResult() : false; -} - -bool PhysicsWorld::collisionPreSolveCallback(PhysicsContact& contact) -{ - if (!contact.isNotificationEnabled()) - { - return true; - } - - contact.setEventCode(PhysicsContact::EventCode::PRESOLVE); - contact.setWorld(this); - _eventDispatcher->dispatchEvent(&contact); - - return contact.resetResult(); -} - -void PhysicsWorld::collisionPostSolveCallback(PhysicsContact& contact) -{ - if (!contact.isNotificationEnabled()) - { - return; - } - - contact.setEventCode(PhysicsContact::EventCode::POSTSOLVE); - contact.setWorld(this); - _eventDispatcher->dispatchEvent(&contact); -} - -void PhysicsWorld::collisionSeparateCallback(PhysicsContact& contact) -{ - if (!contact.isNotificationEnabled()) - { - return; - } - - contact.setEventCode(PhysicsContact::EventCode::SEPARATE); - contact.setWorld(this); - _eventDispatcher->dispatchEvent(&contact); -} - -void PhysicsWorld::rayCast(PhysicsRayCastCallbackFunc func, const Vec2& point1, const Vec2& point2, void* data) -{ - AXASSERT(func != nullptr, "func shouldn't be nullptr"); - - if (func != nullptr) - { - if (!_delayAddBodies.empty() || !_delayRemoveBodies.empty()) - { - updateBodies(); - } - RayCastCallbackInfo info = {this, func, point1, point2, data}; - - PhysicsWorldCallback::continues = true; - cpSpaceSegmentQuery(_cpSpace, PhysicsHelper::vec22cpv(point1), PhysicsHelper::vec22cpv(point2), 0.0f, - CP_SHAPE_FILTER_ALL, (cpSpaceSegmentQueryFunc)PhysicsWorldCallback::rayCastCallbackFunc, - &info); - } -} - -void PhysicsWorld::queryRect(PhysicsQueryRectCallbackFunc func, const Rect& rect, void* data) -{ - AXASSERT(func != nullptr, "func shouldn't be nullptr"); - - if (func != nullptr) - { - if (!_delayAddBodies.empty() || !_delayRemoveBodies.empty()) - { - updateBodies(); - } - RectQueryCallbackInfo info = {this, func, data}; - - PhysicsWorldCallback::continues = true; - cpSpaceBBQuery(_cpSpace, PhysicsHelper::rect2cpbb(rect), CP_SHAPE_FILTER_ALL, - (cpSpaceBBQueryFunc)PhysicsWorldCallback::queryRectCallbackFunc, &info); - } -} - -void PhysicsWorld::queryPoint(PhysicsQueryPointCallbackFunc func, const Vec2& point, void* data) -{ - AXASSERT(func != nullptr, "func shouldn't be nullptr"); - - if (func != nullptr) - { - if (!_delayAddBodies.empty() || !_delayRemoveBodies.empty()) - { - updateBodies(); - } - PointQueryCallbackInfo info = {this, func, data}; - - PhysicsWorldCallback::continues = true; - cpSpacePointQuery(_cpSpace, PhysicsHelper::vec22cpv(point), 0, CP_SHAPE_FILTER_ALL, - (cpSpacePointQueryFunc)PhysicsWorldCallback::queryPointFunc, &info); - } -} - -Vector PhysicsWorld::getShapes(const Vec2& point) const -{ - Vector arr; - cpSpacePointQuery(_cpSpace, PhysicsHelper::vec22cpv(point), 0, CP_SHAPE_FILTER_ALL, - (cpSpacePointQueryFunc)PhysicsWorldCallback::getShapesAtPointFunc, &arr); - - return arr; -} - -PhysicsShape* PhysicsWorld::getShape(const Vec2& point) const -{ - cpShape* shape = - cpSpacePointQueryNearest(_cpSpace, PhysicsHelper::vec22cpv(point), 0, CP_SHAPE_FILTER_ALL, nullptr); - return shape == nullptr ? nullptr : static_cast(cpShapeGetUserData(shape)); -} - -bool PhysicsWorld::init() -{ - do - { -# if AX_TARGET_PLATFORM == AX_PLATFORM_WIN32 - _cpSpace = cpSpaceNew(); -# else - _cpSpace = cpHastySpaceNew(); - cpHastySpaceSetThreads(_cpSpace, 0); -# endif - AX_BREAK_IF(_cpSpace == nullptr); - - cpSpaceSetGravity(_cpSpace, PhysicsHelper::vec22cpv(_gravity)); - - cpCollisionHandler* handler = cpSpaceAddDefaultCollisionHandler(_cpSpace); - handler->userData = this; - handler->beginFunc = (cpCollisionBeginFunc)PhysicsWorldCallback::collisionBeginCallbackFunc; - handler->preSolveFunc = (cpCollisionPreSolveFunc)PhysicsWorldCallback::collisionPreSolveCallbackFunc; - handler->postSolveFunc = (cpCollisionPostSolveFunc)PhysicsWorldCallback::collisionPostSolveCallbackFunc; - handler->separateFunc = (cpCollisionSeparateFunc)PhysicsWorldCallback::collisionSeparateCallbackFunc; - - return true; - } while (false); - - return false; -} - -void PhysicsWorld::addBody(PhysicsBody* body) -{ - AXASSERT(body != nullptr, "the body can not be nullptr"); - - if (body->getWorld() == this) - { - return; - } - - if (body->getWorld() != nullptr) - { - body->removeFromWorld(); - } - - addBodyOrDelay(body); - _bodies.pushBack(body); - body->_world = this; - body->setFixedUpdate(_fixedRate > 0); -} - -void PhysicsWorld::doAddBody(PhysicsBody* body) -{ - if (body->isEnabled()) - { - // add body to space - if (!cpSpaceContainsBody(_cpSpace, body->_cpBody)) - { - cpSpaceAddBody(_cpSpace, body->_cpBody); - } - - // add shapes to space - for (auto&& shape : body->getShapes()) - { - addShape(dynamic_cast(shape)); - } - } -} - -void PhysicsWorld::addBodyOrDelay(PhysicsBody* body) -{ - auto removeBodyIter = _delayRemoveBodies.find(body); - if (removeBodyIter != _delayRemoveBodies.end()) - { - _delayRemoveBodies.erase(removeBodyIter); - return; - } - - if (_delayAddBodies.find(body) == _delayAddBodies.end()) - { - _delayAddBodies.pushBack(body); - } -} - -void PhysicsWorld::updateBodies() -{ - if (cpSpaceIsLocked(_cpSpace)) - { - return; - } - - // issue #4944, contact callback will be invoked when add/remove body, _delayAddBodies maybe changed, so we need - // make a copy. - auto addCopy = _delayAddBodies; - _delayAddBodies.clear(); - for (auto&& body : addCopy) - { - doAddBody(body); - } - - auto removeCopy = _delayRemoveBodies; - _delayRemoveBodies.clear(); - for (auto&& body : removeCopy) - { - doRemoveBody(body); - } -} - -void PhysicsWorld::removeBody(int tag) -{ - for (auto&& body : _bodies) - { - if (body->getTag() == tag) - { - removeBody(body); - return; - } - } -} - -void PhysicsWorld::removeBody(PhysicsBody* body) -{ - if (body->getWorld() != this) - { - AXLOGD("Physics Warning: this body doesn't belong to this world"); - return; - } - - // destroy the body's joints - auto removeCopy = body->_joints; - for (auto&& joint : removeCopy) - { - removeJoint(joint, true); - } - body->_joints.clear(); - - removeBodyOrDelay(body); - _bodies.eraseObject(body); - body->_world = nullptr; -} - -void PhysicsWorld::removeBodyOrDelay(PhysicsBody* body) -{ - if (_delayAddBodies.getIndex(body) != AX_INVALID_INDEX) - { - _delayAddBodies.eraseObject(body); - return; - } - - if (cpSpaceIsLocked(_cpSpace)) - { - if (_delayRemoveBodies.getIndex(body) == AX_INVALID_INDEX) - { - _delayRemoveBodies.pushBack(body); - } - } - else - { - doRemoveBody(body); - } -} - -void PhysicsWorld::removeJoint(PhysicsJoint* joint, bool destroy) -{ - if (joint) - { - if (joint->getWorld() != this && destroy) - { - AXLOGD( - "physics warning: the joint is not in this world, it won't be destroyed until the body it connects is " - "destroyed"); - return; - } - - joint->_destroyMark = destroy; - - bool removedFromDelayAdd = false; - auto it = std::find(_delayAddJoints.begin(), _delayAddJoints.end(), joint); - if (it != _delayAddJoints.end()) - { - _delayAddJoints.erase(it); - removedFromDelayAdd = true; - } - - if (cpSpaceIsLocked(_cpSpace)) - { - if (removedFromDelayAdd) - return; - if (std::find(_delayRemoveJoints.rbegin(), _delayRemoveJoints.rend(), joint) == _delayRemoveJoints.rend()) - { - _delayRemoveJoints.emplace_back(joint); - } - } - else - { - doRemoveJoint(joint); - } - } -} - -void PhysicsWorld::updateJoints() -{ - if (cpSpaceIsLocked(_cpSpace)) - { - return; - } - - for (auto&& joint : _delayAddJoints) - { - joint->_world = this; - if (joint->initJoint()) - { - _joints.emplace_back(joint); - } - else - { - delete joint; - } - } - _delayAddJoints.clear(); - - for (auto&& joint : _delayRemoveJoints) - { - doRemoveJoint(joint); - } - _delayRemoveJoints.clear(); - - for (auto&& joint : _joints) - { - joint->flushDelayTasks(); - } -} - -void PhysicsWorld::removeShape(PhysicsShape* shape) -{ - if (shape) - { - for (auto&& cps : shape->_cpShapes) - { - if (cpSpaceContainsShape(_cpSpace, cps)) - { - cpSpaceRemoveShape(_cpSpace, cps); - } - } - } -} - -void PhysicsWorld::addJoint(PhysicsJoint* joint) -{ - if (joint) - { - AXASSERT(joint->getWorld() == nullptr, "Can not add joint already add to other world!"); - - joint->_world = this; - auto it = std::find(_delayRemoveJoints.begin(), _delayRemoveJoints.end(), joint); - if (it != _delayRemoveJoints.end()) - { - _delayRemoveJoints.erase(it); - return; - } - - if (std::find(_delayAddJoints.begin(), _delayAddJoints.end(), joint) == _delayAddJoints.end()) - { - _delayAddJoints.emplace_back(joint); - } - } -} - -void PhysicsWorld::removeAllJoints(bool destroy) -{ - auto removeCopy = _joints; - for (auto&& joint : removeCopy) - { - removeJoint(joint, destroy); - } -} - -void PhysicsWorld::addShape(PhysicsShape* physicsShape) -{ - if (physicsShape) - { - for (auto&& shape : physicsShape->_cpShapes) - { - cpSpaceAddShape(_cpSpace, shape); - } - } -} - -void PhysicsWorld::doRemoveBody(PhysicsBody* body) -{ - AXASSERT(body != nullptr, "the body can not be nullptr"); - - // remove shapes - for (auto&& shape : body->getShapes()) - { - removeShape(shape); - } - - // remove body - if (cpSpaceContainsBody(_cpSpace, body->_cpBody)) - { - cpSpaceRemoveBody(_cpSpace, body->_cpBody); - } -} - -void PhysicsWorld::doRemoveJoint(PhysicsJoint* joint) -{ - for (auto&& constraint : joint->_cpConstraints) - { - cpSpaceRemoveConstraint(_cpSpace, constraint); - } - _joints.remove(joint); - joint->_world = nullptr; - - if (joint->getBodyA()) - { - joint->getBodyA()->removeJoint(joint); - } - - if (joint->getBodyB()) - { - joint->getBodyB()->removeJoint(joint); - } - - if (joint->_destroyMark) - { - delete joint; - } -} - -void PhysicsWorld::removeAllBodies() -{ - for (auto&& child : _bodies) - { - removeBodyOrDelay(child); - child->_world = nullptr; - } - - _bodies.clear(); -} - -void PhysicsWorld::setDebugDrawMask(int mask) -{ - if (mask == DEBUGDRAW_NONE && _debugDraw) - { - _debugDraw->removeFromParent(); - AX_SAFE_RELEASE_NULL(_debugDraw); - } - - _debugDrawMask = mask; -} - -const Vector& PhysicsWorld::getAllBodies() const -{ - return _bodies; -} - -PhysicsBody* PhysicsWorld::getBody(int tag) const -{ - for (auto&& body : _bodies) - { - if (body->getTag() == tag) - { - return body; - } - } - - return nullptr; -} - -void PhysicsWorld::setGravity(const Vec2& gravity) -{ - _gravity = gravity; - cpSpaceSetGravity(_cpSpace, PhysicsHelper::vec22cpv(gravity)); -} - -void PhysicsWorld::setSlopBias(float slop, float bias) -{ - cpSpaceSetCollisionSlop(_cpSpace, slop); - cpSpaceSetCollisionBias(_cpSpace, bias); -} - -void PhysicsWorld::setSubsteps(int steps) -{ - if (steps > 0) - { - _substeps = steps; - if (steps > 1) - { - _updateRate = 1; - } - } -} - -void PhysicsWorld::step(float delta) -{ - if (_autoStep) - { - AXLOGD("Physics Warning: You need to close auto step( setAutoStep(false) ) first"); - } - else - { - update(delta, true); - } -} - -void PhysicsWorld::update(float delta, bool userCall /* = false*/) -{ - - if (_preUpdateCallback) - _preUpdateCallback(); // fix #11154 - - if (!_delayAddBodies.empty()) - { - updateBodies(); - } - else if (!_delayRemoveBodies.empty()) - { - updateBodies(); - } - - auto sceneToWorldTransform = _scene->getNodeToParentTransform(); - beforeSimulation(_scene, sceneToWorldTransform, 1.f, 1.f, 0.f); - - if (!_delayAddJoints.empty() || !_delayRemoveJoints.empty()) - { - updateJoints(); - } - - if (delta < FLT_EPSILON) - { - return; - } - - if (userCall) - { -# if AX_TARGET_PLATFORM == AX_PLATFORM_WIN32 - cpSpaceStep(_cpSpace, delta); -# else - cpHastySpaceStep(_cpSpace, delta); -# endif - } - else - { - _updateTime += delta; - if (_fixedRate) - { - const float step = 1.0f / _fixedRate; - const float dt = step * _speed; - while (_updateTime > step) - { - _updateTime -= step; - for (auto&& body : _bodies) - { - body->fixedUpdate(dt); - } - _scene->fixedUpdate(dt); - -# if AX_TARGET_PLATFORM == AX_PLATFORM_WIN32 - cpSpaceStep(_cpSpace, dt); -# else - cpHastySpaceStep(_cpSpace, dt); -# endif - } - } - else - { - if (++_updateRateCount >= _updateRate) - { - const float dt = _updateTime * _speed / _substeps; - for (int i = 0; i < _substeps; ++i) - { -# if AX_TARGET_PLATFORM == AX_PLATFORM_WIN32 - cpSpaceStep(_cpSpace, dt); -# else - cpHastySpaceStep(_cpSpace, dt); -# endif - } - _updateRateCount = 0; - _updateTime = 0.0f; - } - } - } - - if (_debugDrawMask != DEBUGDRAW_NONE) - { - debugDraw(); - } - - // Update physics position, should loop as the same sequence as node tree. - // PhysicsWorld::afterSimulation() will depend on the sequence. - afterSimulation(_scene, sceneToWorldTransform, 0.f); - - if (_postUpdateCallback) - _postUpdateCallback(); // fix #11154 -} - -PhysicsWorld* PhysicsWorld::construct(Scene* scene) -{ - PhysicsWorld* world = new PhysicsWorld(); - if (world->init()) - { - world->_scene = scene; - world->_eventDispatcher = scene->getEventDispatcher(); - return world; - } - - AX_SAFE_DELETE(world); - return nullptr; -} - -PhysicsWorld::PhysicsWorld() - : _gravity(Vec2(0.0f, -98.0f)) - , _speed(1.0f) - , _updateRate(1) - , _updateRateCount(0) - , _updateTime(0.0f) - , _substeps(1) - , _fixedRate(0) - , _cpSpace(nullptr) - , _updateBodyTransform(false) - , _scene(nullptr) - , _autoStep(true) - , _debugDraw(nullptr) - , _debugDrawMask(DEBUGDRAW_NONE) - , _eventDispatcher(nullptr) -{} - -PhysicsWorld::~PhysicsWorld() -{ - removeAllJoints(true); - removeAllBodies(); - if (_cpSpace) - { -# if AX_TARGET_PLATFORM == AX_PLATFORM_WIN32 - cpSpaceFree(_cpSpace); -# else - cpHastySpaceFree(_cpSpace); -# endif - } - AX_SAFE_RELEASE_NULL(_debugDraw); -} - -void PhysicsWorld::beforeSimulation(Node* node, - const Mat4& parentToWorldTransform, - float nodeParentScaleX, - float nodeParentScaleY, - float parentRotation) -{ - auto scaleX = nodeParentScaleX * node->getScaleX(); - auto scaleY = nodeParentScaleY * node->getScaleY(); - auto rotation = parentRotation + node->getRotation(); - - auto nodeToWorldTransform = parentToWorldTransform * node->getNodeToParentTransform(); - - auto physicsBody = node->getPhysicsBody(); - if (physicsBody) - { - physicsBody->beforeSimulation(parentToWorldTransform, nodeToWorldTransform, scaleX, scaleY, rotation); - } - - for (auto&& child : node->getChildren()) - beforeSimulation(child, nodeToWorldTransform, scaleX, scaleY, rotation); -} - -void PhysicsWorld::afterSimulation(Node* node, const Mat4& parentToWorldTransform, float parentRotation) -{ - auto nodeToWorldTransform = parentToWorldTransform * node->getNodeToParentTransform(); - auto nodeRotation = parentRotation + node->getRotation(); - - auto physicsBody = node->getPhysicsBody(); - if (physicsBody) - { - physicsBody->afterSimulation(parentToWorldTransform, parentRotation); - } - - for (auto&& child : node->getChildren()) - afterSimulation(child, nodeToWorldTransform, nodeRotation); -} - -void PhysicsWorld::setPostUpdateCallback(const std::function& callback) -{ - _postUpdateCallback = callback; -} - -void PhysicsWorld::setPreUpdateCallback(const std::function& callback) -{ - _preUpdateCallback = callback; -} - -} - -#endif // defined(AX_ENABLE_PHYSICS) diff --git a/core/physics/PhysicsWorld.h b/core/physics/PhysicsWorld.h deleted file mode 100644 index 6619298e18c1..000000000000 --- a/core/physics/PhysicsWorld.h +++ /dev/null @@ -1,481 +0,0 @@ -/**************************************************************************** - Copyright (c) 2013-2016 Chukong Technologies Inc. - Copyright (c) 2017-2018 Xiamen Yaji Software Co., Ltd. - Copyright (c) 2019-present Axmol Engine contributors (see AUTHORS.md). - - https://axmol.dev/ - - 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 __CCPHYSICS_WORLD_H__ -#define __CCPHYSICS_WORLD_H__ - -#include "base/Config.h" -#if defined(AX_ENABLE_PHYSICS) - -# include -# include "base/Vector.h" -# include "math/Math.h" -# include "physics/PhysicsBody.h" - -struct cpSpace; - -namespace ax -{ - -class PhysicsBody; -class PhysicsJoint; -class PhysicsShape; -class PhysicsContact; - -class Director; -class Node; -class Sprite; -class Scene; -class DrawNode; -class PhysicsDebugDraw; -class EventDispatcher; - -class PhysicsWorld; - -typedef struct PhysicsRayCastInfo -{ - PhysicsShape* shape; - Vec2 start; - Vec2 end; ///< in lua, it's name is "ended" - Vec2 contact; - Vec2 normal; - - // FIXME: correct thing to do is use `cpFloat` instead of float. - // but in order to do so, we should include "chipmunk_types.h" - // in Chipmunk v7.0, chipmunk_types includes all the mac types that - // conflicts with cocos2d Vec2, Point,... etc types. And all the CocosStudio - // lib will need to use the `ax::` namespace prefix. And it is easier to do this - // than change all the cocosstudio library (and also users code) - float fraction; - void* data; -} PhysicsRayCastInfo; - -/** - * @brief Called for each fixture found in the query. You control how the ray cast - * proceeds by returning a float: - * return true: continue - * return false: terminate the ray cast - * @param fixture the fixture hit by the ray - * @param point the point of initial intersection - * @param normal the normal vector at the point of intersection - * @return true to continue, false to terminate - */ -typedef std::function PhysicsRayCastCallbackFunc; -typedef std::function PhysicsQueryRectCallbackFunc; -typedef PhysicsQueryRectCallbackFunc PhysicsQueryPointCallbackFunc; - -/** - * @addtogroup physics - * @{ - * @addtogroup physics_2d - * @{ - */ - -/** - * @class PhysicsWorld CCPhysicsWorld.h - * @brief An PhysicsWorld object simulates collisions and other physical properties. You do not create PhysicsWorld - * objects directly; instead, you can get it from an Scene object. - */ -class AX_DLL PhysicsWorld -{ -public: - static const int DEBUGDRAW_NONE; ///< draw nothing - static const int DEBUGDRAW_SHAPE; ///< draw shapes - static const int DEBUGDRAW_JOINT; ///< draw joints - static const int DEBUGDRAW_CONTACT; ///< draw contact - static const int DEBUGDRAW_ALL; ///< draw all - -public: - /** - * Adds a joint to this physics world. - * - * This joint will be added to this physics world at next frame. - * @attention If this joint is already added to another physics world, it will be removed from that world first and - * then add to this world. - * @param joint A pointer to an existing PhysicsJoint object. - */ - virtual void addJoint(PhysicsJoint* joint); - - /** - * Remove a joint from this physics world. - * - * If this world is not locked, the joint is removed immediately, otherwise at next frame. - * If this joint is connected with a body, it will be removed from the body also. - * @param joint A pointer to an existing PhysicsJoint object. - * @param destroy true this joint will be destroyed after remove from this world, false otherwise. - */ - virtual void removeJoint(PhysicsJoint* joint, bool destroy = true); - - /** - * Remove all joints from this physics world. - * - * @attention This function is invoked in the destructor of this physics world, you do not use this api in common. - * @param destroy true all joints will be destroyed after remove from this world, false otherwise. - */ - virtual void removeAllJoints(bool destroy = true); - - /** - * Remove a body from this physics world. - * - * If this world is not locked, the body is removed immediately, otherwise at next frame. - * @attention If this body has joints, those joints will be removed also. - * @param body A pointer to an existing PhysicsBody object. - */ - virtual void removeBody(PhysicsBody* body); - - /** - * Remove body by tag. - * - * If this world is not locked, the object is removed immediately, otherwise at next frame. - * @attention If this body has joints, those joints will be removed also. - * @param tag An integer number that identifies a PhysicsBody object. - */ - virtual void removeBody(int tag); - - /** - * Remove all bodies from physics world. - * - * If this world is not locked, those body are removed immediately, otherwise at next frame. - */ - virtual void removeAllBodies(); - - /** - * Searches for physics shapes that intersects the ray. - * - * Query this physics world along the line segment from start to end. - * @param func Func is called for each shape found. - * @param start A Vec2 object contains the begin position of the ray. - * @param end A Vec2 object contains the end position of the ray. - * @param data User defined data, it is passed to func. - */ - void rayCast(PhysicsRayCastCallbackFunc func, const Vec2& start, const Vec2& end, void* data); - - /** - * Searches for physics shapes that contains in the rect. - * - * Query this physics world to find all shapes overlap rect. - * @param func Func is called for each shape whose bounding box overlaps rect. - * @param rect A Rect object contains a rectangle's x, y, width and height. - * @param data User defined data, it is passed to func. - */ - void queryRect(PhysicsQueryRectCallbackFunc func, const Rect& rect, void* data); - - /** - * Searches for physics shapes that contains the point. - * - * @attention The point must lie inside a shape. - * @param func Func is called for each shape contains the point. - * @param point A Vec2 object contains the position of the point. - * @param data User defined data, it is passed to func. - */ - void queryPoint(PhysicsQueryPointCallbackFunc func, const Vec2& point, void* data); - - /** - * Get physics shapes that contains the point. - * - * All shapes contains the point will be pushed in a Vector object. - * @attention The point must lie inside a shape. - * @param point A Vec2 object contains the position of the point. - * @return A Vector object contains all found PhysicsShape pointer. - */ - Vector getShapes(const Vec2& point) const; - - /** - * Get the nearest physics shape that contains the point. - * - * Query this physics world at point and return the closest shape. - * @param point A Vec2 object contains the position of the point. - * @return A PhysicsShape object pointer or nullptr if no shapes were found - */ - PhysicsShape* getShape(const Vec2& point) const; - - /** - * Get all the bodies that in this physics world. - * - * @return A Vector& object contains all bodies in this physics world. - */ - const Vector& getAllBodies() const; - - /** - * Get a body by tag. - * - * @param tag An integer number that identifies a PhysicsBody object. - * @return A PhysicsBody object pointer or nullptr if no shapes were found. - */ - PhysicsBody* getBody(int tag) const; - - /** - * Get a scene contain this physics world. - * - * @attention This value is initialized in constructor - * @return A Scene object reference. - */ - Scene& getScene() const { return *_scene; } - - /** - * Get the gravity value of this physics world. - * - * @return A Vec2 object. - */ - Vec2 getGravity() const { return _gravity; } - - /** - * set the gravity value of this physics world. - * - * @param gravity A gravity value of this physics world. - */ - void setGravity(const Vec2& gravity); - - /** - * set the slop and bias value of this physics world. - * - * @param slop Amount of encouraged penetration between colliding shapes. Used to reduce oscillating contacts and keep the collision cache warm. - * @param bias Determines how fast overlapping shapes are pushed apart. Expressed as a fraction of the error remaining after each second. Defaults to pow(1.0 - 0.1, 60.0) meaning that Chipmunk fixes 10% of overlap each frame at 60Hz. - */ - void setSlopBias(float slop, float bias); - - /** - * Set the speed of this physics world. - * - * @attention if you setAutoStep(false), this won't work. - * @param speed A float number. Speed is the rate at which the simulation executes. default value is 1.0. - */ - void setSpeed(float speed) - { - if (speed >= 0.0f) - { - _speed = speed; - } - } - - /** - * Get the speed of this physics world. - * - * @return A float number. - */ - float getSpeed() { return _speed; } - - /** - * Set the update rate of this physics world - * - * Update rate is the value of EngineUpdateTimes/PhysicsWorldUpdateTimes. - * Set it higher can improve performance, set it lower can improve accuracy of physics world simulation. - * @attention if you setAutoStep(false), this won't work. - * @param rate An integer number, default value is 1.0. - */ - void setUpdateRate(int rate) - { - if (rate > 0) - { - _updateRate = rate; - } - } - - /** - * Get the update rate of this physics world. - * - * @return An integer number. - */ - int getUpdateRate() { return _updateRate; } - - /** - * set the number of substeps in an update of the physics world. - * - * One physics update will be divided into several substeps to increase its accuracy. - * @param steps An integer number, default value is 1. - */ - void setSubsteps(int steps); - - /** - * Get the number of substeps of this physics world. - * - * @return An integer number. - */ - int getSubsteps() const { return _substeps; } - - /** - * set the number of update of the physics world in a second. - * 0 - disable fixed step system - * default value is 0 - */ - void setFixedUpdateRate(int updatesPerSecond) - { - if (updatesPerSecond > 0) - { - _fixedRate = updatesPerSecond; - for (auto body : _bodies) - { - body->setFixedUpdate(true); - } - } - else - { - for (auto body : _bodies) - { - body->setFixedUpdate(false); - } - } - } - /** get the number of substeps */ - int getFixedUpdateRate() const { return _fixedRate; } - - /** - * Set the debug draw mask of this physics world. - * - * This physics world will draw shapes and joints by DrawNode according to mask. - * @param mask Mask has four value:DEBUGDRAW_NONE, DEBUGDRAW_SHAPE, DEBUGDRAW_JOINT, DEBUGDRAW_CONTACT and - * DEBUGDRAW_ALL, default is DEBUGDRAW_NONE - */ - void setDebugDrawMask(int mask); - - /** - * set the callback which invoked before update of each object in physics world. - */ - void setPreUpdateCallback(const std::function& callback); - - /** - * set the callback which invoked after update of each object in physics world. - */ - void setPostUpdateCallback(const std::function& callback); - - /** - * Get the debug draw mask. - * - * @return An integer number. - */ - int getDebugDrawMask() { return _debugDrawMask; } - - /** - * To control the step of physics. - * - * If you want control it by yourself( fixed-timestep for example ), you can set this to false and call step by - * yourself. - * @attention If you set auto step to false, setSpeed setSubsteps and setUpdateRate won't work, you need to control - * the time step by yourself. - * @param autoStep A bool object, default value is true. - */ - void setAutoStep(bool autoStep) { _autoStep = autoStep; } - - /** - * Get the auto step of this physics world. - * - * @return A bool object. - */ - bool isAutoStep() { return _autoStep; } - - /** - * The step for physics world. - * - * The times passing for simulate the physics. - * @attention You need to setAutoStep(false) first before it can work. - * @param delta A float number. - */ - void step(float delta); - -protected: - static PhysicsWorld* construct(Scene* scene); - bool init(); - - virtual void addBody(PhysicsBody* body); - virtual void addShape(PhysicsShape* shape); - virtual void removeShape(PhysicsShape* shape); - virtual void update(float delta, bool userCall = false); - - virtual void debugDraw(); - - virtual bool collisionBeginCallback(PhysicsContact& contact); - virtual bool collisionPreSolveCallback(PhysicsContact& contact); - virtual void collisionPostSolveCallback(PhysicsContact& contact); - virtual void collisionSeparateCallback(PhysicsContact& contact); - - virtual void doAddBody(PhysicsBody* body); - virtual void doRemoveBody(PhysicsBody* body); - virtual void doRemoveJoint(PhysicsJoint* joint); - virtual void addBodyOrDelay(PhysicsBody* body); - virtual void removeBodyOrDelay(PhysicsBody* body); - virtual void updateBodies(); - virtual void updateJoints(); - -protected: - Vec2 _gravity; - float _speed; - int _updateRate; - int _updateRateCount; - float _updateTime; - int _substeps; - int _fixedRate; - cpSpace* _cpSpace; - - bool _updateBodyTransform; - Vector _bodies; - std::list _joints; - Scene* _scene; - - bool _autoStep; - DrawNode* _debugDraw; - int _debugDrawMask; - - EventDispatcher* _eventDispatcher; - - Vector _delayAddBodies; - Vector _delayRemoveBodies; - std::vector _delayAddJoints; - std::vector _delayRemoveJoints; - - std::function _preUpdateCallback; - std::function _postUpdateCallback; - -protected: - PhysicsWorld(); - virtual ~PhysicsWorld(); - - void beforeSimulation(Node* node, - const Mat4& parentToWorldTransform, - float nodeParentScaleX, - float nodeParentScaleY, - float parentRotation); - void afterSimulation(Node* node, const Mat4& parentToWorldTransform, float parentRotation); - - friend class Node; - friend class Sprite; - friend class Scene; - friend class Director; - friend class PhysicsBody; - friend class PhysicsShape; - friend class PhysicsJoint; - friend class PhysicsWorldCallback; - friend class PhysicsDebugDraw; -}; - -extern const float AX_DLL PHYSICS_INFINITY; - -/** @} */ -/** @} */ - -} - -#endif // defined(AX_ENABLE_PHYSICS) -#endif // __CCPHYSICS_WORLD_H__ diff --git a/core/physics/cpCompat62.h b/core/physics/cpCompat62.h deleted file mode 100644 index 1d0e6ecb94ed..000000000000 --- a/core/physics/cpCompat62.h +++ /dev/null @@ -1,122 +0,0 @@ -/* Copyright (c) 2013 Scott Lembcke and Howling Moon Software - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -#ifndef CHIPMUNK_COMPAT_62_H -#define CHIPMUNK_COMPAT_62_H - -#include "chipmunk/chipmunk.h" - -// -// Body -// -inline cpVect cpBodyGetVelAtWorldPoint(const cpBody* body) -{ - return cpBodyGetVelocityAtWorldPoint(body); -} -inline cpVect cpBodyGetVelAtLocalPoint(const cpBody* body) -{ - return cpBodyGetVelocityAtLocalPoint(body); -} -inline cpVect cpBodyGetVel(const cpBody* body) -{ - return cpBodyGetVelocity(body); -} -inline void cpBodySetVel(cpBody* body, cpVect velocity) -{ - cpBodySetVelocity(body, velocity); -} -inline cpVect cpBodyGetPos(const cpBody* body) -{ - return cpBodyGetPosition(body); -} -inline void cpBodySetPos(cpBody* body, cpVect pos) -{ - cpBodySetPosition(body, pos); -} -inline cpVect cpBodyGetRot(const cpBody* body) -{ - return cpBodyGetRotation(body); -} -inline cpFloat cpBodyGetAngVel(const cpBody* body) -{ - return cpBodyGetAngularVelocity(body); -} -inline void cpBodySetAngVel(cpBody* body, cpFloat angularVelocity) -{ - cpBodySetAngularVelocity(body, angularVelocity); -} -inline cpVect cpBodyLocal2World(const cpBody* body, const cpVect point) -{ - return cpBodyLocalToWorld(body, point); -} -inline cpVect cpBodyWorld2Local(const cpBody* body, const cpVect point) -{ - return cpBodyWorldToLocal(body, point); -} -inline void cpBodyApplyImpulse(cpBody* body, const cpVect j, const cpVect r) -{ - cpBodyApplyImpulseAtWorldPoint(body, j, r); -} - -// -// Shapes -// -inline void cpShapeSetLayers(cpShape* shape, unsigned int layer) -{ - cpShapeFilter filter = cpShapeGetFilter(shape); - filter.mask = layer; - filter.categories = layer; - cpShapeSetFilter(shape, filter); -} -inline unsigned int cpShapeGetLayers(cpShape* shape) -{ - cpShapeFilter filter = cpShapeGetFilter(shape); - return filter.mask; -} -inline void cpShapeSetGroup(cpShape* shape, uintptr_t group) -{ - cpShapeFilter filter = cpShapeGetFilter(shape); - filter.group = group; - cpShapeSetFilter(shape, filter); -} -inline uintptr_t cpShapeGetGroup(cpShape* shape) -{ - cpShapeFilter filter = cpShapeGetFilter(shape); - return filter.group; -} -inline int cpPolyShapeGetNumVerts(const cpShape* shape) -{ - return cpPolyShapeGetCount(shape); -} -inline cpFloat cpShapeNearestPointQuery(cpShape* shape, cpVect p, cpPointQueryInfo* out) -{ - return cpShapePointQuery(shape, p, out); -} - -// -// Space -// -inline cpShape* cpSpaceAddStaticShape(cpSpace* space, cpShape* shape) -{ - return cpSpaceAddShape(space, shape); -} - -#endif // CHIPMUNK_COMPAT_62_H diff --git a/core/physics3d/Physics3D.cpp b/core/physics3d/Physics3D.cpp index 9c20b6be10fc..5f9b1cac43a1 100644 --- a/core/physics3d/Physics3D.cpp +++ b/core/physics3d/Physics3D.cpp @@ -28,16 +28,12 @@ #if defined(AX_ENABLE_3D_PHYSICS) -# if (AX_ENABLE_BULLET_INTEGRATION) - namespace ax { AX_DLL const char* physics3dVersion() { -# if AX_ENABLE_BULLET_INTEGRATION return "bullet2.82"; -# endif } } @@ -93,6 +89,4 @@ btQuaternion convertQuatTobtQuat(const ax::Quaternion& quat) return btQuaternion(quat.x, quat.y, quat.z, quat.w); } -# endif // AX_ENABLE_BULLET_INTEGRATION - #endif // defined(AX_ENABLE_3D_PHYSICS) diff --git a/core/physics3d/Physics3D.h b/core/physics3d/Physics3D.h index 767f14e864c7..728725db3710 100644 --- a/core/physics3d/Physics3D.h +++ b/core/physics3d/Physics3D.h @@ -46,8 +46,6 @@ AX_DLL const char* physics3dVersion(); } -# if (AX_ENABLE_BULLET_INTEGRATION) - // include bullet header files # include "bullet/LinearMath/btTransform.h" # include "bullet/LinearMath/btVector3.h" @@ -65,8 +63,6 @@ btTransform convertMat4TobtTransform(const ax::Mat4& mat4); ax::Quaternion convertbtQuatToQuat(const btQuaternion& btQuat); btQuaternion convertQuatTobtQuat(const ax::Quaternion& quat); -# endif // AX_ENABLE_BULLET_INTEGRATION - #endif // defined(AX_ENABLE_3D_PHYSICS) #endif // __PHYSICS_3D_H__ diff --git a/core/physics3d/Physics3DComponent.cpp b/core/physics3d/Physics3DComponent.cpp index 27e3e12cacb5..eec30b50b786 100644 --- a/core/physics3d/Physics3DComponent.cpp +++ b/core/physics3d/Physics3DComponent.cpp @@ -30,8 +30,6 @@ THE SOFTWARE. #if defined(AX_ENABLE_3D_PHYSICS) -# if (AX_ENABLE_BULLET_INTEGRATION) - namespace ax { @@ -252,6 +250,4 @@ void Physics3DComponent::syncNodeToPhysics() } -# endif // AX_ENABLE_BULLET_INTEGRATION - #endif // defined(AX_ENABLE_3D_PHYSICS) diff --git a/core/physics3d/Physics3DComponent.h b/core/physics3d/Physics3DComponent.h index 219804227429..d2fccbb1fed1 100644 --- a/core/physics3d/Physics3DComponent.h +++ b/core/physics3d/Physics3DComponent.h @@ -34,8 +34,6 @@ #if defined(AX_ENABLE_3D_PHYSICS) -# if (AX_ENABLE_BULLET_INTEGRATION) - namespace ax { @@ -145,8 +143,6 @@ class AX_DLL Physics3DComponent : public ax::Component /// @} } -# endif // AX_ENABLE_BULLET_INTEGRATION - #endif // defined(AX_ENABLE_3D_PHYSICS) #endif // __PHYSICS_3D_COMPONENT_H__ diff --git a/core/physics3d/Physics3DConstraint.cpp b/core/physics3d/Physics3DConstraint.cpp index c20aa3c4f1ae..31886b0c9163 100644 --- a/core/physics3d/Physics3DConstraint.cpp +++ b/core/physics3d/Physics3DConstraint.cpp @@ -28,8 +28,6 @@ #if defined(AX_ENABLE_3D_PHYSICS) -# if (AX_ENABLE_BULLET_INTEGRATION) - namespace ax { @@ -882,6 +880,4 @@ void Physics3D6DofConstraint::setUseFrameOffset(bool frameOffsetOnOff) const } -# endif // AX_ENABLE_BULLET_INTEGRATION - #endif // defined(AX_ENABLE_3D_PHYSICS) diff --git a/core/physics3d/Physics3DConstraint.h b/core/physics3d/Physics3DConstraint.h index 3ee27417e463..05e0c12fd1e9 100644 --- a/core/physics3d/Physics3DConstraint.h +++ b/core/physics3d/Physics3DConstraint.h @@ -33,8 +33,6 @@ #if defined(AX_ENABLE_3D_PHYSICS) -# if (AX_ENABLE_BULLET_INTEGRATION) - class btTypedConstraint; namespace ax @@ -117,9 +115,7 @@ class AX_DLL Physics3DConstraint : public Object */ void setOverrideNumSolverIterations(int overrideNumIterations); -# if (AX_ENABLE_BULLET_INTEGRATION) btTypedConstraint* getbtContraint() { return _constraint; } -# endif protected: Physics3DConstraint(); @@ -617,8 +613,6 @@ class AX_DLL Physics3D6DofConstraint : public Physics3DConstraint } -# endif // AX_ENABLE_BULLET_INTEGRATION - #endif // defined(AX_ENABLE_3D_PHYSICS) #endif // __PHYSICS_3D_CONSTRAINT_H__ diff --git a/core/physics3d/Physics3DDebugDrawer.cpp b/core/physics3d/Physics3DDebugDrawer.cpp index 85ba1964cae2..ef3551354035 100644 --- a/core/physics3d/Physics3DDebugDrawer.cpp +++ b/core/physics3d/Physics3DDebugDrawer.cpp @@ -37,8 +37,6 @@ #if defined(AX_ENABLE_3D_PHYSICS) -# if (AX_ENABLE_BULLET_INTEGRATION) - namespace ax { @@ -163,6 +161,4 @@ void Physics3DDebugDrawer::clear() } -# endif // AX_ENABLE_BULLET_INTEGRATION - #endif // defined(AX_ENABLE_3D_PHYSICS) diff --git a/core/physics3d/Physics3DDebugDrawer.h b/core/physics3d/Physics3DDebugDrawer.h index 105fce3de232..7386899dfe90 100644 --- a/core/physics3d/Physics3DDebugDrawer.h +++ b/core/physics3d/Physics3DDebugDrawer.h @@ -37,7 +37,6 @@ #if defined(AX_ENABLE_3D_PHYSICS) -# if (AX_ENABLE_BULLET_INTEGRATION) # include "bullet/LinearMath/btIDebugDraw.h" namespace ax @@ -98,8 +97,6 @@ class Physics3DDebugDrawer : public btIDebugDraw } -# endif // AX_ENABLE_BULLET_INTEGRATION - #endif // defined(AX_ENABLE_3D_PHYSICS) #endif // __PHYSICS_3D_VIEWER_H__ diff --git a/core/physics3d/Physics3DObject.cpp b/core/physics3d/Physics3DObject.cpp index 869f23009914..815f6271ec74 100644 --- a/core/physics3d/Physics3DObject.cpp +++ b/core/physics3d/Physics3DObject.cpp @@ -29,8 +29,6 @@ #if defined(AX_ENABLE_3D_PHYSICS) -# if (AX_ENABLE_BULLET_INTEGRATION) - # include "bullet/btBulletCollisionCommon.h" # include "bullet/btBulletDynamicsCommon.h" @@ -553,6 +551,4 @@ ax::Mat4 Physics3DCollider::getWorldTransform() const } -# endif // AX_ENABLE_BULLET_INTEGRATION - #endif // defined(AX_ENABLE_3D_PHYSICS) diff --git a/core/physics3d/Physics3DObject.h b/core/physics3d/Physics3DObject.h index 0ab62b520970..2f04b1715fbc 100644 --- a/core/physics3d/Physics3DObject.h +++ b/core/physics3d/Physics3DObject.h @@ -35,8 +35,6 @@ #if defined(AX_ENABLE_3D_PHYSICS) -# if (AX_ENABLE_BULLET_INTEGRATION) - class btCollisionShape; class btRigidBody; class btPersistentManifold; @@ -497,8 +495,6 @@ class AX_DLL Physics3DCollider : public Physics3DObject } -# endif // AX_ENABLE_BULLET_INTEGRATION - #endif // defined(AX_ENABLE_3D_PHYSICS) #endif // __PHYSICS_3D_OBJECT_H__ diff --git a/core/physics3d/Physics3DShape.cpp b/core/physics3d/Physics3DShape.cpp index 561301748cad..a5bd8d31ad90 100644 --- a/core/physics3d/Physics3DShape.cpp +++ b/core/physics3d/Physics3DShape.cpp @@ -28,7 +28,6 @@ #if defined(AX_ENABLE_3D_PHYSICS) -# if (AX_ENABLE_BULLET_INTEGRATION) # include "bullet/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h" namespace ax @@ -41,14 +40,11 @@ Physics3DShape::ShapeType Physics3DShape::getShapeType() const Physics3DShape::Physics3DShape() : _shapeType(ShapeType::UNKNOWN) { -# if (AX_ENABLE_BULLET_INTEGRATION) _btShape = nullptr; _heightfieldData = nullptr; -# endif } Physics3DShape::~Physics3DShape() { -# if (AX_ENABLE_BULLET_INTEGRATION) AX_SAFE_DELETE(_btShape); AX_SAFE_DELETE_ARRAY(_heightfieldData); for (auto&& iter : _compoundChildShapes) @@ -56,7 +52,6 @@ Physics3DShape::~Physics3DShape() AX_SAFE_RELEASE(iter); } _compoundChildShapes.clear(); -# endif } Physics3DShape* Physics3DShape::createBox(const ax::Vec3& extent) @@ -220,6 +215,4 @@ bool Physics3DShape::initCompoundShape(const std::vector>& shapes); -# if AX_ENABLE_BULLET_INTEGRATION btCollisionShape* getbtShape() const { return _btShape; } -# endif Physics3DShape(); ~Physics3DShape(); @@ -160,11 +156,9 @@ class AX_DLL Physics3DShape : public Object protected: ShapeType _shapeType; // shape type -# if (AX_ENABLE_BULLET_INTEGRATION) btCollisionShape* _btShape; unsigned char* _heightfieldData; std::vector _compoundChildShapes; -# endif }; // end of 3d group @@ -172,8 +166,6 @@ class AX_DLL Physics3DShape : public Object } -# endif // AX_ENABLE_BULLET_INTEGRATION - #endif // defined(AX_ENABLE_3D_PHYSICS) #endif // __PHYSICS_3D_SHAPE_H__ diff --git a/core/physics3d/Physics3DWorld.cpp b/core/physics3d/Physics3DWorld.cpp index 2c4ead8ffe0e..6a1a3240d9e9 100644 --- a/core/physics3d/Physics3DWorld.cpp +++ b/core/physics3d/Physics3DWorld.cpp @@ -29,8 +29,6 @@ #if defined(AX_ENABLE_3D_PHYSICS) -# if (AX_ENABLE_BULLET_INTEGRATION) - namespace ax { @@ -425,6 +423,4 @@ void Physics3DWorld::setGhostPairCallback() } -# endif // AX_ENABLE_BULLET_INTEGRATION - #endif // defined(AX_ENABLE_3D_PHYSICS) diff --git a/core/physics3d/Physics3DWorld.h b/core/physics3d/Physics3DWorld.h index e40f3c6cc05f..07de7d601686 100644 --- a/core/physics3d/Physics3DWorld.h +++ b/core/physics3d/Physics3DWorld.h @@ -33,8 +33,6 @@ #if defined(AX_ENABLE_3D_PHYSICS) -# if (AX_ENABLE_BULLET_INTEGRATION) - class btDynamicsWorld; class btDefaultCollisionConfiguration; class btCollisionDispatcher; @@ -169,7 +167,6 @@ class AX_DLL Physics3DWorld : public Object bool _collisionCheckingFlag; bool _needGhostPairCallbackChecking; -# if (AX_ENABLE_BULLET_INTEGRATION) btDynamicsWorld* _btPhyiscsWorld; btDefaultCollisionConfiguration* _collisionConfiguration; btCollisionDispatcher* _dispatcher; @@ -177,15 +174,12 @@ class AX_DLL Physics3DWorld : public Object btSequentialImpulseConstraintSolver* _solver; btGhostPairCallback* _ghostCallback; Physics3DDebugDrawer* _debugDrawer; -# endif // AX_ENABLE_BULLET_INTEGRATION }; // end of 3d group /// @} } -# endif - #endif // defined(AX_ENABLE_3D_PHYSICS) #endif // __PHYSICS_3D_WORLD_H__ diff --git a/core/physics3d/PhysicsMeshRenderer.cpp b/core/physics3d/PhysicsMeshRenderer.cpp index 816366026179..664eeef11bc4 100644 --- a/core/physics3d/PhysicsMeshRenderer.cpp +++ b/core/physics3d/PhysicsMeshRenderer.cpp @@ -28,8 +28,6 @@ #if defined(AX_ENABLE_3D_PHYSICS) -# if (AX_ENABLE_BULLET_INTEGRATION) - namespace ax { @@ -101,6 +99,4 @@ PhysicsMeshRenderer::~PhysicsMeshRenderer() {} } -# endif // AX_ENABLE_BULLET_INTEGRATION - #endif // defined(AX_ENABLE_3D_PHYSICS) diff --git a/core/physics3d/PhysicsMeshRenderer.h b/core/physics3d/PhysicsMeshRenderer.h index ef763cb2e4c7..d82916766717 100644 --- a/core/physics3d/PhysicsMeshRenderer.h +++ b/core/physics3d/PhysicsMeshRenderer.h @@ -34,8 +34,6 @@ #if defined(AX_ENABLE_3D_PHYSICS) -# if (AX_ENABLE_BULLET_INTEGRATION) - namespace ax { /** @@ -84,8 +82,6 @@ class AX_DLL PhysicsMeshRenderer : public ax::MeshRenderer /// @} } -# endif // AX_ENABLE_BULLET_INTEGRATION - #endif // defined(AX_ENABLE_3D_PHYSICS) #endif // __PHYSICS_MESH_RENDERER_H__ diff --git a/extensions/README.md b/extensions/README.md index 8493ad7a8a82..22f37828da31 100644 --- a/extensions/README.md +++ b/extensions/README.md @@ -65,7 +65,7 @@ ## physics-nodes - Upstream: https://github.com/axmolengine/axmol -- Version: axmol-1.0 +- Version: axmol-3.0 - License: MIT ## scripting/lua diff --git a/extensions/axmol-ext.h b/extensions/axmol-ext.h index 7ae623f9e81a..fe7262051285 100644 --- a/extensions/axmol-ext.h +++ b/extensions/axmol-ext.h @@ -32,11 +32,7 @@ // Physics integration #include "physics-nodes/src/physics-nodes/PhysicsDebugNode.h" -#include "physics-nodes/src/physics-nodes/PhysicsDebugNodeBox2D.h" -#include "physics-nodes/src/physics-nodes/PhysicsDebugNodeChipmunk2D.h" #include "physics-nodes/src/physics-nodes/PhysicsSprite.h" -#include "physics-nodes/src/physics-nodes/PhysicsSpriteBox2D.h" -#include "physics-nodes/src/physics-nodes/PhysicsSpriteChipmunk2D.h" #include "assets-manager/src/assets-manager/AssetsManager.h" #include "assets-manager/src/assets-manager/AssetsManagerEx.h" diff --git a/extensions/physics-nodes/CMakeLists.txt b/extensions/physics-nodes/CMakeLists.txt index 80040ca28c33..08e82ba0a800 100644 --- a/extensions/physics-nodes/CMakeLists.txt +++ b/extensions/physics-nodes/CMakeLists.txt @@ -6,4 +6,5 @@ add_library(${target_name} ${PHYSICS_NODES_SOURCES}) target_include_directories( ${target_name} PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/src ) + setup_ax_extension_config(${target_name}) diff --git a/extensions/physics-nodes/src/physics-nodes/PhysicsDebugNode.cpp b/extensions/physics-nodes/src/physics-nodes/PhysicsDebugNode.cpp index 6e903917f550..fb8e4433d2e3 100644 --- a/extensions/physics-nodes/src/physics-nodes/PhysicsDebugNode.cpp +++ b/extensions/physics-nodes/src/physics-nodes/PhysicsDebugNode.cpp @@ -1,252 +1,185 @@ -/* Copyright (c) 2012 Scott Lembcke and Howling Moon Software - * Copyright (c) 2012 cocos2d-x.org - * Copyright (c) 2017-2018 Xiamen Yaji Software Co., Ltd. +/* * Copyright (c) 2021 @aismann; Peter Eismann, Germany; dreifrankensoft * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. + * This software is provided 'as-is', without any express or implied + * warranty. In no event will the authors be held liable for any damages + * arising from the use of this software. + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as 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 "PhysicsDebugNode.h" -#include "chipmunk/chipmunk_private.h" - -#include "base/Types.h" -#include "math/Math.h" +#include "PhysicsDebugNode.h" -#include -#include -#include -#include -#include +#if defined(_WIN32) +# pragma push_macro("TRANSPARENT") +# undef TRANSPARENT +#endif NS_AX_EXT_BEGIN -/* - IMPORTANT - READ ME! - - This file sets pokes around in the private API a lot to provide efficient - debug rendering given nothing more than reference to a Chipmunk space. - It is not recommended to write rendering code like this in your own games - as the private API may change with little or no warning. - */ - -static const cpVect spring_verts[] = { - {0.00f, 0.0f}, {0.20f, 0.0f}, {0.25f, 3.0f}, {0.30f, -6.0f}, {0.35f, 6.0f}, - {0.40f, -6.0f}, {0.45f, 6.0f}, {0.50f, -6.0f}, {0.55f, 6.0f}, {0.60f, -6.0f}, - {0.65f, 6.0f}, {0.70f, -3.0f}, {0.75f, 6.0f}, {0.80f, 0.0f}, {1.00f, 0.0f}, -}; -static const int spring_count = sizeof(spring_verts) / sizeof(cpVect); - -static Color4F ColorForBody(cpBody* body) +static Color4F toColor4F(b2HexColor color) { - if (CP_BODY_TYPE_STATIC == cpBodyGetType(body) || cpBodyIsSleeping(body)) - { - return Color4F(0.5f, 0.5f, 0.5f, 0.5f); - } - else if (body->sleeping.idleTime > cpBodyGetSpace(body)->sleepTimeThreshold) - { - return Color4F(0.33f, 0.33f, 0.33f, 0.5f); - } - else - { - return Color4F(1.0f, 0.0f, 0.0f, 0.5f); - } + unsigned int r = ((unsigned int)color >> 16) & 0xff; + unsigned int g = ((unsigned int)color >> 8) & 0xff; + unsigned int b = ((unsigned int)color) & 0xff; + return Color4F{r / 255.f, g / 255.f, b / 255.f, 1.0f}; } -static Vec2 cpVert2Point(const cpVect& vert) +static Vec2 toVec2(const b2Vec2& v) { - return (Vec2(vert.x, vert.y)); + return Vec2{v.x, v.y}; } -static void DrawShape(cpShape* shape, DrawNode* renderer) +static b2HexColor tob2HexColor(Color4F color) { - cpBody* body = cpShapeGetBody(shape); - Color4F color = ColorForBody(body); + Color3B ret(color); + return (b2HexColor)(static_cast(ret.r) << 16 | static_cast(ret.g) << 8 | + static_cast(ret.b)); +} - switch (shape->klass->type) - { - case CP_CIRCLE_SHAPE: - { - cpCircleShape* circle = (cpCircleShape*)shape; - cpVect center = circle->tc; - cpFloat radius = circle->r; - renderer->drawDot(cpVert2Point(center), cpfmax(radius, 1.0), color); - renderer->drawSegment(cpVert2Point(center), - cpVert2Point(cpvadd(center, cpvmult(cpBodyGetRotation(body), radius))), 1.0, color); - } - break; - case CP_SEGMENT_SHAPE: +/// Draw a closed polygon provided in CCW order. +// void (*DrawPolygon)(const b2Vec2* vertices, int vertexCount, b2HexColor color, void* context); +static void b2DrawPolygon(const b2Vec2* verts, int vertexCount, b2HexColor color, PhysicsDebugNode* dn) +{ + Vec2* vec = new Vec2[vertexCount]; + for (size_t i = 0; i < vertexCount; i++) { - cpSegmentShape* seg = (cpSegmentShape*)shape; - renderer->drawSegment(cpVert2Point(seg->ta), cpVert2Point(seg->tb), cpfmax(seg->r, 1.0), color); + vec[i] = Vec2(verts[i].x * dn->getPTMRatio(), verts[i].y * dn->getPTMRatio()) + dn->getWorldOffset(); } - break; - case CP_POLY_SHAPE: + dn->drawPolygon(vec, vertexCount, Color4F::BLACK, 0.4f, toColor4F(color)); +} + +/// Draw a solid closed polygon provided in CCW order. +// void (*DrawSolidPolygon)(b2Transform, +// const b2Vec2* vertices, +// int vertexCount, +// float radius, +// b2HexColor color, +// void* context); +static void b2DrawSolidPolygon(b2Transform t, + const b2Vec2* verts, + int vertexCount, + float radius, + b2HexColor color, + PhysicsDebugNode* dn) +{ + axstd::pod_vector vec(vertexCount); + for (size_t i = 0; i < vertexCount; i++) { - cpPolyShape* poly = (cpPolyShape*)shape; - Color4F line = color; - line.a = cpflerp(color.a, 1.0, 0.5); - int num = poly->count; - Vec2* pPoints = new Vec2[num]; - for (int i = 0; i < num; ++i) - pPoints[i] = cpVert2Point(poly->planes[i].v0); - if (cpfmax(poly->r, 1.0) > 1.0) - { - renderer->drawPolygon(pPoints, num, Color4F(0.5f, 0.5f, 0.5f, 0.0f), poly->r, color); - } - else - { - renderer->drawPolygon(pPoints, num, color, 1.0, line); - } - - AX_SAFE_DELETE_ARRAY(pPoints); - } - break; - default: - cpAssertHard(false, "Bad assertion in DrawShape()"); + auto pt = b2TransformPoint(t, verts[i]); + vec[i] = Vec2(pt.x * dn->getPTMRatio(), pt.y * dn->getPTMRatio()) + dn->getWorldOffset(); } + auto color4f = toColor4F(color); + dn->drawPolygon(vec.data(), vertexCount, Color4F(color4f.r / 2, color4f.g / 2, color4f.b / 2, color4f.a), 0.5f, + color4f); } -static Color4F CONSTRAINT_COLOR(0, 1, 0, 0.5); +/// Draw a circle. +// void (*DrawCircle)(b2Vec2 center, float radius, b2HexColor color, void* context); +static void b2DrawCircle(b2Vec2 center, float radius, b2HexColor color, PhysicsDebugNode* dn) +{ + dn->drawCircle(Vec2(center.x * dn->getPTMRatio(), center.y * dn->getPTMRatio()) + dn->getWorldOffset(), + radius * dn->getPTMRatio(), AX_DEGREES_TO_RADIANS(0), 30, true, 1.0f, 1.0f, toColor4F(color)); +} -static void DrawConstraint(cpConstraint* constraint, DrawNode* renderer) +/// Draw a solid circle. +// void (*DrawSolidCircle)(b2Transform, float radius, b2HexColor color, void* context); +static void b2DrawSolidCircle(b2Transform t, float radius, b2HexColor color, PhysicsDebugNode* dn) { - cpBody* body_a = cpConstraintGetBodyA(constraint); - cpBody* body_b = cpConstraintGetBodyB(constraint); + auto center = b2TransformPoint(t, b2Vec2_zero); + Vec2 c = {Vec2(center.x * dn->getPTMRatio(), center.y * dn->getPTMRatio()) + dn->getWorldOffset()}; + auto color4f = toColor4F(color); + + dn->drawSolidCircle(c, radius * dn->getPTMRatio(), AX_DEGREES_TO_RADIANS(0), 20, 1.0f, 1.0f, + Color4F(color4f.r / 2, color4f.g / 2, color4f.b / 2, color4f.a), 0.4f, color4f); + // Draw a line fixed in the circle to animate rotation. + b2Vec2 pp = {(center + radius * b2Rot_GetXAxis(t.q))}; + Vec2 cp = {Vec2(pp.x * dn->getPTMRatio(), pp.y * dn->getPTMRatio()) + dn->getWorldOffset()}; + dn->drawLine(c, cp, color4f); +} - if (cpConstraintIsPinJoint(constraint)) - { - cpVect a = - cpvadd(cpBodyGetPosition(body_a), cpvrotate(cpPinJointGetAnchorA(constraint), cpBodyGetRotation(body_a))); - cpVect b = - cpvadd(cpBodyGetPosition(body_b), cpvrotate(cpPinJointGetAnchorB(constraint), cpBodyGetRotation(body_b))); - - renderer->drawDot(cpVert2Point(a), 3.0, CONSTRAINT_COLOR); - renderer->drawDot(cpVert2Point(b), 3.0, CONSTRAINT_COLOR); - renderer->drawSegment(cpVert2Point(a), cpVert2Point(b), 1.0, CONSTRAINT_COLOR); - } - else if (cpConstraintIsSlideJoint(constraint)) - { - cpVect a = - cpvadd(cpBodyGetPosition(body_a), cpvrotate(cpSlideJointGetAnchorA(constraint), cpBodyGetRotation(body_a))); - cpVect b = - cpvadd(cpBodyGetPosition(body_b), cpvrotate(cpSlideJointGetAnchorB(constraint), cpBodyGetRotation(body_b))); - - renderer->drawDot(cpVert2Point(a), 3.0, CONSTRAINT_COLOR); - renderer->drawDot(cpVert2Point(b), 3.0, CONSTRAINT_COLOR); - renderer->drawSegment(cpVert2Point(a), cpVert2Point(b), 1.0, CONSTRAINT_COLOR); - } - else if (cpConstraintIsPivotJoint(constraint)) - { - cpVect a = - cpvadd(cpBodyGetPosition(body_a), cpvrotate(cpPivotJointGetAnchorA(constraint), cpBodyGetRotation(body_a))); - cpVect b = - cpvadd(cpBodyGetPosition(body_b), cpvrotate(cpPivotJointGetAnchorB(constraint), cpBodyGetRotation(body_b))); +/// Draw a solid capsule. +// void (*DrawSolidCapsule)(b2Vec2 p1, b2Vec2 p2, float radius, b2HexColor color, void* context); - renderer->drawDot(cpVert2Point(a), 3.0, CONSTRAINT_COLOR); - renderer->drawDot(cpVert2Point(b), 3.0, CONSTRAINT_COLOR); - } - else if (cpConstraintIsGrooveJoint(constraint)) - { - cpVect a = cpvadd(cpBodyGetPosition(body_a), - cpvrotate(cpGrooveJointGetGrooveA(constraint), cpBodyGetRotation(body_a))); - cpVect b = cpvadd(cpBodyGetPosition(body_a), - cpvrotate(cpGrooveJointGetGrooveB(constraint), cpBodyGetRotation(body_a))); - cpVect c = cpvadd(cpBodyGetPosition(body_b), - cpvrotate(cpGrooveJointGetAnchorB(constraint), cpBodyGetRotation(body_b))); - - renderer->drawDot(cpVert2Point(c), 3.0, CONSTRAINT_COLOR); - renderer->drawSegment(cpVert2Point(a), cpVert2Point(b), 1.0, CONSTRAINT_COLOR); - } - else if (cpConstraintIsDampedSpring(constraint)) - { - cpDampedSpring* spring = (cpDampedSpring*)constraint; +/// Draw a line segment. +// void (*DrawSegment)(b2Vec2 p1, b2Vec2 p2, b2HexColor color, void* context); +static void b2DrawSegment(b2Vec2 p1, b2Vec2 p2, b2HexColor color, PhysicsDebugNode* dn) +{ + dn->drawLine(Vec2(p1.x * dn->getPTMRatio(), p1.y * dn->getPTMRatio()) + dn->getWorldOffset(), + Vec2(p2.x * dn->getPTMRatio(), p2.y * dn->getPTMRatio()) + dn->getWorldOffset(), toColor4F(color)); +} - cpVect a = cpTransformPoint(body_a->transform, spring->anchorA); - cpVect b = cpTransformPoint(body_b->transform, spring->anchorB); +/// Draw a transform. Choose your own length scale. +// void (*DrawTransform)(b2Transform transform, void* context); +static void b2DrawTransform(b2Transform t, PhysicsDebugNode* dn) +{ + b2Vec2 p1 = t.p, p2; + const float k_axisScale = 0.4f; - renderer->drawDot(cpVert2Point(a), 3.0, CONSTRAINT_COLOR); - renderer->drawDot(cpVert2Point(b), 3.0, CONSTRAINT_COLOR); + p2 = p1 + k_axisScale * b2Rot_GetXAxis(t.q); + b2DrawSegment(p1, p2, b2HexColor::b2_colorRed, dn); - cpVect delta = cpvsub(b, a); - cpFloat cos = delta.x; - cpFloat sin = delta.y; - cpFloat s = 1.0f / cpvlength(delta); + p2 = p1 + k_axisScale * b2Rot_GetYAxis(t.q); + b2DrawSegment(p1, p2, b2HexColor::b2_colorGreen, dn); +} - cpVect r1 = cpv(cos, -sin * s); - cpVect r2 = cpv(sin, cos * s); +/// Draw a point. +// void (*DrawPoint)(b2Vec2 p, float size, b2HexColor color, void* context); +static void b2DrawPoint(b2Vec2 p, float size, b2HexColor color, PhysicsDebugNode* dn) +{ + dn->drawPoint(Vec2(p.x * dn->getPTMRatio(), p.y * dn->getPTMRatio()) + dn->getWorldOffset(), size, + toColor4F(color)); +} - cpVect* verts = (cpVect*)alloca(spring_count * sizeof(cpVect)); - for (int i = 0; i < spring_count; i++) - { - cpVect v = spring_verts[i]; - verts[i] = cpv(cpvdot(v, r1) + a.x, cpvdot(v, r2) + a.y); - } +bool PhysicsDebugNode::initWithWorld(b2WorldId worldId) +{ + bool ret = DrawNode::init(); - for (int i = 0; i < spring_count - 1; i++) - { - renderer->drawSegment(cpVert2Point(verts[i]), cpVert2Point(verts[i + 1]), 1.0, CONSTRAINT_COLOR); - } - } - else - { - AXLOGD("Cannot draw constraint"); - } + _world = worldId; + return ret; } -// implementation of PhysicsDebugNode - void PhysicsDebugNode::draw(Renderer* renderer, const Mat4& transform, uint32_t flags) { - if (!_spacePtr) + if (!b2World_IsValid(_world)) { return; } - // clear the shapes information before draw current shapes. - DrawNode::clear(); - cpSpaceEachShape(_spacePtr, (cpSpaceShapeIteratorFunc)DrawShape, this); - cpSpaceEachConstraint(_spacePtr, (cpSpaceConstraintIteratorFunc)DrawConstraint, this); + if (_autoDraw) + { + // clear the shapes information before draw current shapes. + clear(); + b2World_Draw(_world, &_debugDraw); + } DrawNode::draw(renderer, transform, flags); } -PhysicsDebugNode::PhysicsDebugNode() : _spacePtr(nullptr) {} - -PhysicsDebugNode* PhysicsDebugNode::create(cpSpace* space) +PhysicsDebugNode::PhysicsDebugNode() { - PhysicsDebugNode* node = new PhysicsDebugNode(); - node->init(); - node->_spacePtr = space; - node->autorelease(); - return node; -} - -PhysicsDebugNode::~PhysicsDebugNode() {} - -cpSpace* PhysicsDebugNode::getSpace() const -{ - return _spacePtr; -} - -void PhysicsDebugNode::setSpace(cpSpace* space) -{ - _spacePtr = space; + _debugDraw.context = this; +#define __b2_setfun(f) _debugDraw.f = reinterpret_cast(b2##f); + __b2_setfun(DrawPolygon); + __b2_setfun(DrawSolidPolygon); + __b2_setfun(DrawCircle); + __b2_setfun(DrawSolidCircle); + __b2_setfun(DrawSegment); + __b2_setfun(DrawTransform); + __b2_setfun(DrawPoint); +#undef __b2_setfun } NS_AX_EXT_END + +#if defined(_WIN32) +# pragma pop_macro("TRANSPARENT") +#endif diff --git a/extensions/physics-nodes/src/physics-nodes/PhysicsDebugNode.h b/extensions/physics-nodes/src/physics-nodes/PhysicsDebugNode.h index ae751ab5a54e..772ac831b604 100644 --- a/extensions/physics-nodes/src/physics-nodes/PhysicsDebugNode.h +++ b/extensions/physics-nodes/src/physics-nodes/PhysicsDebugNode.h @@ -1,73 +1,71 @@ -/* Copyright (c) 2012 Scott Lembcke and Howling Moon Software - * Copyright (c) 2012 cocos2d-x.org - * Copyright (c) 2017-2018 Xiamen Yaji Software Co., Ltd. +/* + * Copyright (c) 2019 Erin Catto * Copyright (c) 2021 @aismann; Peter Eismann, Germany; dreifrankensoft * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. + * This software is provided 'as-is', without any express or implied + * warranty. In no event will the authors be held liable for any damages + * arising from the use of this software. + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as 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 __PHYSICSNODES_DEBUGNODE_H__ -#define __PHYSICSNODES_DEBUGNODE_H__ +#ifndef __PHYSICSNODES_DEBUGNODE_BOX2D_H__ +#define __PHYSICSNODES_DEBUGNODE_BOX2D_H__ #include "extensions/ExtensionMacros.h" -#include "2d/DrawNode.h" #include "extensions/ExtensionExport.h" - -struct cpSpace; +#include "2d/DrawNode.h" +#include "box2d/box2d.h" +#include "axmol.h" NS_AX_EXT_BEGIN -/** - * A BaseData that draws the components of a physics engine. - - * Supported physics engines: - * - Chipmunk - * - Objective-Chipmunk - * @since v2.1 - * @lua NA - */ - +// This class implements debug drawing callbacks that are invoked inside b2World::Step. class AX_EX_DLL PhysicsDebugNode : public DrawNode { - public: - /** Create a debug node for a regular Chipmunk space. */ - static PhysicsDebugNode* create(cpSpace* space); - /** - * @js ctor - */ PhysicsDebugNode(); - /** - * @js NA - */ - virtual ~PhysicsDebugNode(); + virtual bool initWithWorld(b2WorldId worldId); + + void setAutoDraw(bool bval) { _autoDraw = bval; } + bool isAutoDraw() const { return _autoDraw; } + + // control border thinkness + void setThinkness(bool bval) { _thinkness; } + float getThinkness() const { return _thinkness; } + + void setWorldOffset(const Vec2& offset) { _worldOffset = offset; } + const Vec2& getWorldOffset() const { return _worldOffset; } - cpSpace* getSpace() const; - void setSpace(cpSpace* space); + void setPTMRatio(float ratio) { _ratio = ratio; } + float getPTMRatio() const { return _ratio; } + + void drawCircleInstanced(b2Vec2 center, float radius, b2HexColor color); + void drawSolidCircleInstanced(b2Transform transform, b2Vec2 center, float radius, b2HexColor color); // Overrides virtual void draw(Renderer* renderer, const Mat4& transform, uint32_t flags) override; + b2DebugDraw& getB2DebugDraw() { return _debugDraw; } + protected: - cpSpace* _spacePtr; + b2WorldId _world{}; + b2DebugDraw _debugDraw{b2DefaultDebugDraw()}; + bool _autoDraw{true}; + float _thinkness{0.5f}; + + ax::Vec2 _worldOffset{}; + float _ratio{1.0f}; }; NS_AX_EXT_END -#endif // __PHYSICSNODES_DEBUGNODE_H__ +#endif //__PHYSICSNODES_DEBUGNODE_BOX2D_H__ diff --git a/extensions/physics-nodes/src/physics-nodes/PhysicsDebugNodeBox2D.cpp b/extensions/physics-nodes/src/physics-nodes/PhysicsDebugNodeBox2D.cpp deleted file mode 100644 index b63fd556ba49..000000000000 --- a/extensions/physics-nodes/src/physics-nodes/PhysicsDebugNodeBox2D.cpp +++ /dev/null @@ -1,113 +0,0 @@ -/* - * Copyright (c) 2021 @aismann; Peter Eismann, Germany; dreifrankensoft - * - * This software is provided 'as-is', without any express or implied - * warranty. In no event will the authors be held liable for any damages - * arising from the use of this software. - * Permission is granted to anyone to use this software for any purpose, - * including commercial applications, and to alter it and redistribute it - * freely, subject to the following restrictions: - * 1. The origin of this software must not be misrepresented; you must not - * claim that you wrote the original software. If you use this software - * in a product, an acknowledgment in the product documentation would be - * appreciated but is not required. - * 2. Altered source versions must be plainly marked as 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 "PhysicsDebugNodeBox2D.h" - -NS_AX_EXT_BEGIN - -PhysicsDebugNodeBox2D::PhysicsDebugNodeBox2D() -{ - drawBP = DrawNode::create(); - debugNodeOffset = {40.0f, 0.0f}; - mRatio = 1.0f; -} - -PhysicsDebugNodeBox2D::~PhysicsDebugNodeBox2D() {} - -ax::DrawNode* PhysicsDebugNodeBox2D::GetDrawNode() -{ - return drawBP; -} - -void PhysicsDebugNodeBox2D::SetDrawNode(ax::DrawNode* drawNode) -{ - drawBP = drawNode; -} - -ax::Vec2& PhysicsDebugNodeBox2D::GetDebugNodeOffset() -{ - return debugNodeOffset; -} - -void PhysicsDebugNodeBox2D::DrawPolygon(const b2Vec2* verts, int vertexCount, const b2Color& color) -{ - Vec2* vec = new Vec2[vertexCount]; - for (size_t i = 0; i < vertexCount; i++) - { - vec[i] = Vec2(verts[i].x * mRatio, verts[i].y * mRatio) + debugNodeOffset; - } - drawBP->drawPolygon(vec, vertexCount, Color4F::BLACK, 0.4f, Color4F(color.r, color.g, color.b, color.a)); -} - -void PhysicsDebugNodeBox2D::DrawSolidPolygon(const b2Vec2* verts, int vertexCount, const b2Color& color) -{ - Vec2* vec = new Vec2[vertexCount]; - for (size_t i = 0; i < vertexCount; i++) - { - vec[i] = Vec2(verts[i].x * mRatio, verts[i].y * mRatio) + debugNodeOffset; - } - drawBP->drawPolygon(vec, vertexCount, Color4F(color.r / 2, color.g / 2, color.b / 2, color.a), 0.4f, - Color4F(color.r, color.g, color.b, color.a)); -} - -void PhysicsDebugNodeBox2D::DrawCircle(const b2Vec2& center, float radius, const b2Color& color) -{ - drawBP->drawCircle(Vec2(center.x * mRatio, center.y * mRatio) + debugNodeOffset, radius * mRatio, - AX_DEGREES_TO_RADIANS(0), 30, true, 1.0f, 1.0f, Color4F(color.r, color.g, color.b, color.a)); -} - -void PhysicsDebugNodeBox2D::DrawSolidCircle(const b2Vec2& center, - float radius, - const b2Vec2& axis, - const b2Color& color) -{ - Vec2 c = {Vec2(center.x * mRatio, center.y * mRatio) + debugNodeOffset}; - drawBP->drawSolidCircle(c, radius * mRatio, AX_DEGREES_TO_RADIANS(0), 20, 1.0f, 1.0f, - Color4F(color.r / 2, color.g / 2, color.b / 2, color.a), 0.4f, - Color4F(color.r, color.g, color.b, color.a)); - - // Draw a line fixed in the circle to animate rotation. - b2Vec2 pp = {(center + radius * axis)}; - Vec2 cp = {Vec2(pp.x * mRatio, pp.y * mRatio) + debugNodeOffset}; - drawBP->drawLine(c, cp, Color4F(color.r, color.g, color.b, color.a)); -} - -void PhysicsDebugNodeBox2D::DrawSegment(const b2Vec2& p1, const b2Vec2& p2, const b2Color& color) -{ - drawBP->drawLine(Vec2(p1.x * mRatio, p1.y * mRatio) + debugNodeOffset, - Vec2(p2.x * mRatio, p2.y * mRatio) + debugNodeOffset, Color4F(color.r, color.g, color.b, color.a)); -} - -void PhysicsDebugNodeBox2D::DrawTransform(const b2Transform& xf) -{ - b2Vec2 p1 = xf.p, p2; - const float k_axisScale = 0.4f; - p2 = p1 + k_axisScale * xf.q.GetXAxis(); - DrawSegment(p1, p2, b2Color(1.0f, 0.0f, 0.0f)); - - p2 = p1 + k_axisScale * xf.q.GetYAxis(); - DrawSegment(p1, p2, b2Color(0.0f, 1.0f, 0.0f)); -} - -void PhysicsDebugNodeBox2D::DrawPoint(const b2Vec2& p, float size, const b2Color& color) -{ - drawBP->drawPoint(Vec2(p.x * mRatio, p.y * mRatio) + debugNodeOffset, size, - Color4F(color.r, color.g, color.b, color.a)); -} - -NS_AX_EXT_END \ No newline at end of file diff --git a/extensions/physics-nodes/src/physics-nodes/PhysicsDebugNodeBox2D.h b/extensions/physics-nodes/src/physics-nodes/PhysicsDebugNodeBox2D.h deleted file mode 100644 index 98edb644a532..000000000000 --- a/extensions/physics-nodes/src/physics-nodes/PhysicsDebugNodeBox2D.h +++ /dev/null @@ -1,69 +0,0 @@ -/* - * Copyright (c) 2019 Erin Catto - * Copyright (c) 2021 @aismann; Peter Eismann, Germany; dreifrankensoft - * - * This software is provided 'as-is', without any express or implied - * warranty. In no event will the authors be held liable for any damages - * arising from the use of this software. - * Permission is granted to anyone to use this software for any purpose, - * including commercial applications, and to alter it and redistribute it - * freely, subject to the following restrictions: - * 1. The origin of this software must not be misrepresented; you must not - * claim that you wrote the original software. If you use this software - * in a product, an acknowledgment in the product documentation would be - * appreciated but is not required. - * 2. Altered source versions must be plainly marked as 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 __PHYSICSNODES_DEBUGNODE_BOX2D_H__ -#define __PHYSICSNODES_DEBUGNODE_BOX2D_H__ - -#include "extensions/ExtensionMacros.h" -#include "extensions/ExtensionExport.h" -#include "2d/DrawNode.h" -#include "box2d/box2d.h" -#include "axmol.h" - -NS_AX_EXT_BEGIN - -// This class implements debug drawing callbacks that are invoked inside b2World::Step. -class AX_EX_DLL PhysicsDebugNodeBox2D : public b2Draw -{ -public: - PhysicsDebugNodeBox2D(); - ~PhysicsDebugNodeBox2D(); - - void Create(); - void Destroy(); - - void DrawPolygon(const b2Vec2* vertices, int vertexCount, const b2Color& color) override; - - void DrawSolidPolygon(const b2Vec2* vertices, int vertexCount, const b2Color& color) override; - - void DrawCircle(const b2Vec2& center, float radius, const b2Color& color) override; - - void DrawSolidCircle(const b2Vec2& center, float radius, const b2Vec2& axis, const b2Color& color) override; - - void DrawSegment(const b2Vec2& p1, const b2Vec2& p2, const b2Color& color) override; - - void DrawTransform(const b2Transform& xf) override; - - void DrawPoint(const b2Vec2& p, float size, const b2Color& color) override; - - // axis stuffs - ax::DrawNode* GetDrawNode(); - void SetDrawNode(ax::DrawNode* drawNode); - ax::Vec2& GetDebugNodeOffset(); - - ax::DrawNode* drawBP = NULL; // axis "interface"! - ax::Vec2 debugNodeOffset; - float mRatio; - -private: -}; - -NS_AX_EXT_END - -#endif //__PHYSICSNODES_DEBUGNODE_BOX2D_H__ diff --git a/extensions/physics-nodes/src/physics-nodes/PhysicsDebugNodeChipmunk2D.cpp b/extensions/physics-nodes/src/physics-nodes/PhysicsDebugNodeChipmunk2D.cpp deleted file mode 100644 index bd313debc5e2..000000000000 --- a/extensions/physics-nodes/src/physics-nodes/PhysicsDebugNodeChipmunk2D.cpp +++ /dev/null @@ -1,254 +0,0 @@ -/* Copyright (c) 2012 Scott Lembcke and Howling Moon Software - * Copyright (c) 2012 cocos2d-x.org - * Copyright (c) 2017-2018 Xiamen Yaji Software Co., Ltd. - * Copyright (c) 2021 @aismann; Peter Eismann, Germany; dreifrankensoft - * - * 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 "PhysicsDebugNodeChipmunk2D.h" - -#include "chipmunk/chipmunk_private.h" - -#include "base/Types.h" -#include "math/Math.h" - -#include -#include -#include -#include -#include - -NS_AX_EXT_BEGIN - -Vec2 physicsDebugNodeOffset; - -/* - IMPORTANT - READ ME! - - This file sets pokes around in the private API a lot to provide efficient - debug rendering given nothing more than reference to a Chipmunk space. - It is not recommended to write rendering code like this in your own games - as the private API may change with little or no warning. - */ - -static const cpVect spring_verts[] = { - {0.00f, 0.0f}, {0.20f, 0.0f}, {0.25f, 3.0f}, {0.30f, -6.0f}, {0.35f, 6.0f}, - {0.40f, -6.0f}, {0.45f, 6.0f}, {0.50f, -6.0f}, {0.55f, 6.0f}, {0.60f, -6.0f}, - {0.65f, 6.0f}, {0.70f, -3.0f}, {0.75f, 6.0f}, {0.80f, 0.0f}, {1.00f, 0.0f}, -}; -static const int spring_count = sizeof(spring_verts) / sizeof(cpVect); - -static Color4B ColorForBody(cpBody* body) -{ - if (CP_BODY_TYPE_STATIC == cpBodyGetType(body) || cpBodyIsSleeping(body)) - { - return Color4B(127, 127, 127, 127); - } - else if (body->sleeping.idleTime > cpBodyGetSpace(body)->sleepTimeThreshold) - { - return Color4B(85, 85, 85, 127); - } - else - { - return Color4B(255, 0, 0, 127); - } -} - -static Vec2 cpVert2Point(const cpVect& vert) -{ - return (Vec2(vert.x, vert.y) + physicsDebugNodeOffset); -} - -static void DrawShape(cpShape* shape, DrawNode* renderer) -{ - cpBody* body = cpShapeGetBody(shape); - Color4B color = ColorForBody(body); - - switch (shape->klass->type) - { - case CP_CIRCLE_SHAPE: - { - cpCircleShape* circle = (cpCircleShape*)shape; - cpVect center = circle->tc; - cpFloat radius = circle->r; - renderer->drawDot(cpVert2Point(center), cpfmax(radius, 1.0), color); - renderer->drawSegment(cpVert2Point(center), - cpVert2Point(cpvadd(center, cpvmult(cpBodyGetRotation(body), radius))), 1.0, color); - } - break; - case CP_SEGMENT_SHAPE: - { - cpSegmentShape* seg = (cpSegmentShape*)shape; - renderer->drawSegment(cpVert2Point(seg->ta), cpVert2Point(seg->tb), cpfmax(seg->r, 1.0), color); - } - break; - case CP_POLY_SHAPE: - { - cpPolyShape* poly = (cpPolyShape*)shape; - Color4B line = color; - line.a = cpflerp(color.a, 1.0, 0.5); - int num = poly->count; - Vec2* pPoints = new Vec2[num]; - for (int i = 0; i < num; ++i) - pPoints[i] = cpVert2Point(poly->planes[i].v0); - if (cpfmax(poly->r, 1.0) > 1.0) - { - renderer->drawPolygon(pPoints, num, Color4B(127, 127, 127,0), poly->r, color); - } - else - { - renderer->drawPolygon(pPoints, num, color, 1.0, line); - } - - AX_SAFE_DELETE_ARRAY(pPoints); - } - break; - default: - cpAssertHard(false, "Bad assertion in DrawShape()"); - } -} - -static Color4B CONSTRAINT_COLOR(0, 255, 0, 127); - -static void DrawConstraint(cpConstraint* constraint, DrawNode* renderer) -{ - cpBody* body_a = cpConstraintGetBodyA(constraint); - cpBody* body_b = cpConstraintGetBodyB(constraint); - - if (cpConstraintIsPinJoint(constraint)) - { - cpVect a = - cpvadd(cpBodyGetPosition(body_a), cpvrotate(cpPinJointGetAnchorA(constraint), cpBodyGetRotation(body_a))); - cpVect b = - cpvadd(cpBodyGetPosition(body_b), cpvrotate(cpPinJointGetAnchorB(constraint), cpBodyGetRotation(body_b))); - - renderer->drawDot(cpVert2Point(a), 3.0, CONSTRAINT_COLOR); - renderer->drawDot(cpVert2Point(b), 3.0, CONSTRAINT_COLOR); - renderer->drawSegment(cpVert2Point(a), cpVert2Point(b), 1.0, CONSTRAINT_COLOR); - } - else if (cpConstraintIsSlideJoint(constraint)) - { - cpVect a = - cpvadd(cpBodyGetPosition(body_a), cpvrotate(cpSlideJointGetAnchorA(constraint), cpBodyGetRotation(body_a))); - cpVect b = - cpvadd(cpBodyGetPosition(body_b), cpvrotate(cpSlideJointGetAnchorB(constraint), cpBodyGetRotation(body_b))); - - renderer->drawDot(cpVert2Point(a), 3.0, CONSTRAINT_COLOR); - renderer->drawDot(cpVert2Point(b), 3.0, CONSTRAINT_COLOR); - renderer->drawSegment(cpVert2Point(a), cpVert2Point(b), 1.0, CONSTRAINT_COLOR); - } - else if (cpConstraintIsPivotJoint(constraint)) - { - cpVect a = - cpvadd(cpBodyGetPosition(body_a), cpvrotate(cpPivotJointGetAnchorA(constraint), cpBodyGetRotation(body_a))); - cpVect b = - cpvadd(cpBodyGetPosition(body_b), cpvrotate(cpPivotJointGetAnchorB(constraint), cpBodyGetRotation(body_b))); - - renderer->drawDot(cpVert2Point(a), 3.0, CONSTRAINT_COLOR); - renderer->drawDot(cpVert2Point(b), 3.0, CONSTRAINT_COLOR); - } - else if (cpConstraintIsGrooveJoint(constraint)) - { - cpVect a = cpvadd(cpBodyGetPosition(body_a), - cpvrotate(cpGrooveJointGetGrooveA(constraint), cpBodyGetRotation(body_a))); - cpVect b = cpvadd(cpBodyGetPosition(body_a), - cpvrotate(cpGrooveJointGetGrooveB(constraint), cpBodyGetRotation(body_a))); - cpVect c = cpvadd(cpBodyGetPosition(body_b), - cpvrotate(cpGrooveJointGetAnchorB(constraint), cpBodyGetRotation(body_b))); - - renderer->drawDot(cpVert2Point(c), 3.0, CONSTRAINT_COLOR); - renderer->drawSegment(cpVert2Point(a), cpVert2Point(b), 1.0, CONSTRAINT_COLOR); - } - else if (cpConstraintIsDampedSpring(constraint)) - { - cpDampedSpring* spring = (cpDampedSpring*)constraint; - - cpVect a = cpTransformPoint(body_a->transform, spring->anchorA); - cpVect b = cpTransformPoint(body_b->transform, spring->anchorB); - - renderer->drawDot(cpVert2Point(a), 3.0, CONSTRAINT_COLOR); - renderer->drawDot(cpVert2Point(b), 3.0, CONSTRAINT_COLOR); - - cpVect delta = cpvsub(b, a); - cpFloat cos = delta.x; - cpFloat sin = delta.y; - cpFloat s = 1.0f / cpvlength(delta); - - cpVect r1 = cpv(cos, -sin * s); - cpVect r2 = cpv(sin, cos * s); - - cpVect* verts = (cpVect*)alloca(spring_count * sizeof(cpVect)); - for (int i = 0; i < spring_count; i++) - { - cpVect v = spring_verts[i]; - verts[i] = cpv(cpvdot(v, r1) + a.x, cpvdot(v, r2) + a.y); - } - - for (int i = 0; i < spring_count - 1; i++) - { - renderer->drawSegment(cpVert2Point(verts[i]), cpVert2Point(verts[i + 1]), 1.0, CONSTRAINT_COLOR); - } - } - else - { - AXLOGD("Cannot draw constraint"); - } -} - -// implementation of PhysicsDebugNode - -void PhysicsDebugNodeChipmunk2D::draw(Renderer* renderer, const Mat4& transform, uint32_t flags) -{ - if (!_spacePtr) - { - return; - } - // clear the shapes information before draw current shapes. - DrawNode::clear(); - - cpSpaceEachShape(_spacePtr, (cpSpaceShapeIteratorFunc)DrawShape, this); - cpSpaceEachConstraint(_spacePtr, (cpSpaceConstraintIteratorFunc)DrawConstraint, this); - - DrawNode::draw(renderer, transform, flags); -} - -PhysicsDebugNodeChipmunk2D::PhysicsDebugNodeChipmunk2D() : _spacePtr(nullptr) {} - -PhysicsDebugNodeChipmunk2D* PhysicsDebugNodeChipmunk2D::create(cpSpace* space) -{ - PhysicsDebugNodeChipmunk2D* node = new PhysicsDebugNodeChipmunk2D(); - node->init(); - node->_spacePtr = space; - node->autorelease(); - return node; -} - -PhysicsDebugNodeChipmunk2D::~PhysicsDebugNodeChipmunk2D() {} - -cpSpace* PhysicsDebugNodeChipmunk2D::getSpace() const -{ - return _spacePtr; -} - -void PhysicsDebugNodeChipmunk2D::setSpace(cpSpace* space) -{ - _spacePtr = space; -} - -NS_AX_EXT_END diff --git a/extensions/physics-nodes/src/physics-nodes/PhysicsDebugNodeChipmunk2D.h b/extensions/physics-nodes/src/physics-nodes/PhysicsDebugNodeChipmunk2D.h deleted file mode 100644 index 435615817ece..000000000000 --- a/extensions/physics-nodes/src/physics-nodes/PhysicsDebugNodeChipmunk2D.h +++ /dev/null @@ -1,75 +0,0 @@ -/* Copyright (c) 2012 Scott Lembcke and Howling Moon Software - * Copyright (c) 2012 cocos2d-x.org - * Copyright (c) 2017-2018 Xiamen Yaji Software Co., Ltd. - * Copyright (c) 2021 @aismann; Peter Eismann, Germany; dreifrankensoft - * - * 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 __PHYSICSNODES_DEBUGNODE_CHIPMUNK2D_H__ -#define __PHYSICSNODES_DEBUGNODE_CHIPMUNK2D_H__ - -#include "extensions/ExtensionMacros.h" -#include "2d/DrawNode.h" -#include "extensions/ExtensionExport.h" - -struct cpSpace; - -NS_AX_EXT_BEGIN - -extern AX_EX_DLL Vec2 physicsDebugNodeOffset; - -/** - * A BaseData that draws the components of a physics engine. - - * Supported physics engines: - * - Chipmunk - * - Objective-Chipmunk - * @since v2.1 - * @lua NA - */ - -class AX_EX_DLL PhysicsDebugNodeChipmunk2D : public DrawNode -{ - -public: - /** Create a debug node for a regular Chipmunk space. */ - static PhysicsDebugNodeChipmunk2D* create(cpSpace* space); - /** - * @js ctor - */ - PhysicsDebugNodeChipmunk2D(); - /** - * @js NA - */ - virtual ~PhysicsDebugNodeChipmunk2D(); - - cpSpace* getSpace() const; - void setSpace(cpSpace* space); - - // Overrides - virtual void draw(Renderer* renderer, const Mat4& transform, uint32_t flags) override; - -protected: - cpSpace* _spacePtr; -}; - -NS_AX_EXT_END - -#endif // __PHYSICSNODES_DEBUGNODE_CHIPMUNK2D_H__ diff --git a/extensions/physics-nodes/src/physics-nodes/PhysicsSprite.cpp b/extensions/physics-nodes/src/physics-nodes/PhysicsSprite.cpp index e26033fe5053..3b024c85544a 100644 --- a/extensions/physics-nodes/src/physics-nodes/PhysicsSprite.cpp +++ b/extensions/physics-nodes/src/physics-nodes/PhysicsSprite.cpp @@ -26,18 +26,10 @@ #include "base/Director.h" #include "base/EventDispatcher.h" -#if (AX_ENABLE_CHIPMUNK_INTEGRATION || AX_ENABLE_BOX2D_INTEGRATION) - -# if AX_ENABLE_CHIPMUNK_INTEGRATION -# include "chipmunk/chipmunk.h" -# elif AX_ENABLE_BOX2D_INTEGRATION -# include "box2d/box2d.h" -# endif - NS_AX_EXT_BEGIN PhysicsSprite::PhysicsSprite() - : _ignoreBodyRotation(false), _CPBody(nullptr), _pB2Body(nullptr), _PTMRatio(0.0f), _syncTransform(nullptr) + : _ignoreBodyRotation(false), _PTMRatio(0.0f), _syncTransform(nullptr) {} PhysicsSprite* PhysicsSprite::create() @@ -196,67 +188,24 @@ Vec3 PhysicsSprite::getPosition3D() const return Vec3(pos.x, pos.y, 0); } -// -// Chipmunk only -// - -cpBody* PhysicsSprite::getCPBody() const +b2BodyId PhysicsSprite::getB2Body() const { -# if AX_ENABLE_CHIPMUNK_INTEGRATION - return _CPBody; -# else - AXASSERT(false, "Can't call chipmunk methods when Chipmunk is disabled"); - return nullptr; -# endif + return _bodyId; } -void PhysicsSprite::setCPBody(cpBody* pBody) +void PhysicsSprite::setB2Body(b2BodyId pBody) { -# if AX_ENABLE_CHIPMUNK_INTEGRATION - _CPBody = pBody; -# else - AXASSERT(false, "Can't call chipmunk methods when Chipmunk is disabled"); -# endif -} - -b2Body* PhysicsSprite::getB2Body() const -{ -# if AX_ENABLE_BOX2D_INTEGRATION - return _pB2Body; -# else - AXASSERT(false, "Can't call box2d methods when Box2d is disabled"); - return nullptr; -# endif -} - -void PhysicsSprite::setB2Body(b2Body* pBody) -{ -# if AX_ENABLE_BOX2D_INTEGRATION - _pB2Body = pBody; -# else - AX_UNUSED_PARAM(pBody); - AXASSERT(false, "Can't call box2d methods when Box2d is disabled"); -# endif + _bodyId = pBody; } float PhysicsSprite::getPTMRatio() const { -# if AX_ENABLE_BOX2D_INTEGRATION return _PTMRatio; -# else - AXASSERT(false, "Can't call box2d methods when Box2d is disabled"); - return 0; -# endif } void PhysicsSprite::setPTMRatio(float fRatio) { -# if AX_ENABLE_BOX2D_INTEGRATION _PTMRatio = fRatio; -# else - AX_UNUSED_PARAM(fRatio); - AXASSERT(false, "Can't call box2d methods when Box2d is disabled"); -# endif } // @@ -266,33 +215,18 @@ void PhysicsSprite::setPTMRatio(float fRatio) const Vec2& PhysicsSprite::getPosFromPhysics() const { static Vec2 s_physicPosion; -# if AX_ENABLE_CHIPMUNK_INTEGRATION - - cpVect cpPos = cpBodyGetPosition(_CPBody); - s_physicPosion = Vec2(cpPos.x, cpPos.y); - -# elif AX_ENABLE_BOX2D_INTEGRATION - - b2Vec2 pos = _pB2Body->GetPosition(); + b2Vec2 pos = b2Body_GetPosition(_bodyId); float x = pos.x * _PTMRatio; float y = pos.y * _PTMRatio; s_physicPosion.set(x, y); -# endif + return s_physicPosion; } void PhysicsSprite::setPosition(float x, float y) { -# if AX_ENABLE_CHIPMUNK_INTEGRATION - - cpVect cpPos = cpv(x, y); - cpBodySetPosition(_CPBody, cpPos); - -# elif AX_ENABLE_BOX2D_INTEGRATION - - float angle = _pB2Body->GetAngle(); - _pB2Body->SetTransform(b2Vec2(x / _PTMRatio, y / _PTMRatio), angle); -# endif + auto rot = b2Body_GetRotation(_bodyId); + b2Body_SetTransform(_bodyId, b2Vec2{x / _PTMRatio, y / _PTMRatio}, rot); } void PhysicsSprite::setPosition(const Vec2& pos) @@ -317,16 +251,8 @@ void PhysicsSprite::setPosition3D(const Vec3& position) float PhysicsSprite::getRotation() const { -# if AX_ENABLE_CHIPMUNK_INTEGRATION - - return (_ignoreBodyRotation ? Sprite::getRotation() : -AX_RADIANS_TO_DEGREES(cpBodyGetAngle(_CPBody))); - -# elif AX_ENABLE_BOX2D_INTEGRATION - - return (_ignoreBodyRotation ? Sprite::getRotation() : AX_RADIANS_TO_DEGREES(_pB2Body->GetAngle())); -# else - return 0.0f; -# endif + auto angle = b2Rot_GetAngle(b2Body_GetRotation(_bodyId)); + return (_ignoreBodyRotation ? Sprite::getRotation() : AX_RADIANS_TO_DEGREES(angle)); } void PhysicsSprite::setRotation(float fRotation) @@ -336,20 +262,13 @@ void PhysicsSprite::setRotation(float fRotation) Sprite::setRotation(fRotation); } -# if AX_ENABLE_CHIPMUNK_INTEGRATION - else - { - cpBodySetAngle(_CPBody, -AX_DEGREES_TO_RADIANS(fRotation)); - } - -# elif AX_ENABLE_BOX2D_INTEGRATION else { - b2Vec2 p = _pB2Body->GetPosition(); + b2Vec2 p = b2Body_GetPosition(_bodyId); float radians = AX_DEGREES_TO_RADIANS(fRotation); - _pB2Body->SetTransform(p, radians); + auto rot = b2MakeRot(radians); + b2Body_SetTransform(_bodyId, p, rot); } -# endif } void PhysicsSprite::syncPhysicsTransform() const @@ -358,42 +277,7 @@ void PhysicsSprite::syncPhysicsTransform() const // the sprite is animated (scaled up/down) using actions. // For more info see: http://www.cocos2d-iphone.org/forum/topic/68990 -# if AX_ENABLE_CHIPMUNK_INTEGRATION - - cpVect rot = (_ignoreBodyRotation ? cpvforangle(-AX_DEGREES_TO_RADIANS(_rotationX)) : cpBodyGetRotation(_CPBody)); - float x = cpBodyGetPosition(_CPBody).x + rot.x * -_anchorPointInPoints.x * _scaleX - - rot.y * -_anchorPointInPoints.y * _scaleY; - float y = cpBodyGetPosition(_CPBody).y + rot.y * -_anchorPointInPoints.x * _scaleX + - rot.x * -_anchorPointInPoints.y * _scaleY; - - if (_ignoreAnchorPointForPosition) - { - x += _anchorPointInPoints.x; - y += _anchorPointInPoints.y; - } - - float mat[] = {(float)rot.x * _scaleX, - (float)rot.y * _scaleX, - 0, - 0, - (float)-rot.y * _scaleY, - (float)rot.x * _scaleY, - 0, - 0, - 0, - 0, - 1, - 0, - x, - y, - 0, - 1}; - - _transform.set(mat); - -# elif AX_ENABLE_BOX2D_INTEGRATION - - b2Vec2 pos = _pB2Body->GetPosition(); + b2Vec2 pos = b2Body_GetPosition(_bodyId); float x = pos.x * _PTMRatio; float y = pos.y * _PTMRatio; @@ -405,7 +289,8 @@ void PhysicsSprite::syncPhysicsTransform() const } // Make matrix - float radians = _pB2Body->GetAngle(); + auto rot = b2Body_GetRotation(_bodyId); + float radians = b2Rot_GetAngle(rot); float c = cosf(radians); float s = sinf(radians); @@ -435,7 +320,6 @@ void PhysicsSprite::syncPhysicsTransform() const 1}; _transform.set(mat); -# endif } void PhysicsSprite::onEnter() @@ -466,5 +350,3 @@ void PhysicsSprite::afterUpdate(EventCustom* /*event*/) } NS_AX_EXT_END - -#endif // AX_ENABLE_CHIPMUNK_INTEGRATION || AX_ENABLE_BOX2D_INTEGRATION diff --git a/extensions/physics-nodes/src/physics-nodes/PhysicsSprite.h b/extensions/physics-nodes/src/physics-nodes/PhysicsSprite.h index 12b4781889b3..a58d035358ae 100644 --- a/extensions/physics-nodes/src/physics-nodes/PhysicsSprite.h +++ b/extensions/physics-nodes/src/physics-nodes/PhysicsSprite.h @@ -22,26 +22,21 @@ * SOFTWARE. */ -#ifndef __PHYSICSNODES_CCPHYSICSSPRITE_H__ -#define __PHYSICSNODES_CCPHYSICSSPRITE_H__ +#ifndef __AX_PHYSICSNODES_PHYSICSSPRITE_H__ +#define __AX_PHYSICSNODES_PHYSICSSPRITE_H__ #include "2d/Sprite.h" #include "extensions/ExtensionMacros.h" #include "extensions/ExtensionExport.h" #include "base/EventListenerCustom.h" -#if (AX_ENABLE_CHIPMUNK_INTEGRATION || AX_ENABLE_BOX2D_INTEGRATION) - -struct cpBody; -class b2Body; +#include "box2d/box2d.h" NS_AX_EXT_BEGIN /** A Sprite subclass that is bound to a physics body. It works with: - - Chipmunk: Preprocessor macro AX_ENABLE_CHIPMUNK_INTEGRATION should be defined - - Objective-Chipmunk: Preprocessor macro AX_ENABLE_CHIPMUNK_INTEGRATION should be defined - - Box2d: Preprocessor macro AX_ENABLE_BOX2D_INTEGRATION should be defined + - Box2D Features and Limitations: - Scale and Skew properties are ignored. @@ -94,19 +89,12 @@ class AX_EX_DLL PhysicsSprite : public Sprite bool isIgnoreBodyRotation() const; void setIgnoreBodyRotation(bool bIgnoreBodyRotation); - // - // Chipmunk specific - // - /** Body accessor when using regular Chipmunk */ - cpBody* getCPBody() const; - void setCPBody(cpBody* pBody); - // // Box2d specific // /** Body accessor when using box2d */ - b2Body* getB2Body() const; - void setB2Body(b2Body* pBody); + b2BodyId getB2Body() const; + void setB2Body(b2BodyId pBody); float getPTMRatio() const; void setPTMRatio(float fPTMRatio); @@ -136,11 +124,8 @@ class AX_EX_DLL PhysicsSprite : public Sprite protected: bool _ignoreBodyRotation; - // chipmunk specific - cpBody* _CPBody; - // box2d specific - b2Body* _pB2Body; + b2BodyId _bodyId{}; float _PTMRatio; // Event for update synchronise physic transform @@ -149,6 +134,4 @@ class AX_EX_DLL PhysicsSprite : public Sprite NS_AX_EXT_END -#endif // AX_ENABLE_CHIPMUNK_INTEGRATION || AX_ENABLE_BOX2D_INTEGRATION - #endif // __PHYSICSNODES_CCPHYSICSSPRITE_H__ diff --git a/extensions/physics-nodes/src/physics-nodes/PhysicsSpriteBox2D.cpp b/extensions/physics-nodes/src/physics-nodes/PhysicsSpriteBox2D.cpp deleted file mode 100644 index 352d5b8fb7cf..000000000000 --- a/extensions/physics-nodes/src/physics-nodes/PhysicsSpriteBox2D.cpp +++ /dev/null @@ -1,352 +0,0 @@ -/* Copyright (c) 2012 Scott Lembcke and Howling Moon Software - * Copyright (c) 2012 cocos2d-x.org - * Copyright (c) 2017-2018 Xiamen Yaji Software Co., Ltd. - * Copyright (c) 2021 @aismann; Peter Eismann, Germany; dreifrankensoft - * - * 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 "PhysicsSpriteBox2D.h" -#include "base/Director.h" -#include "base/EventDispatcher.h" - -#include "box2d/box2d.h" - -NS_AX_EXT_BEGIN - -PhysicsSpriteBox2D::PhysicsSpriteBox2D() - : _ignoreBodyRotation(false), _pB2Body(nullptr), _PTMRatio(0.0f), _syncTransform(nullptr) -{} - -PhysicsSpriteBox2D* PhysicsSpriteBox2D::create() -{ - PhysicsSpriteBox2D* pRet = new PhysicsSpriteBox2D(); - if (pRet->init()) - { - pRet->autorelease(); - } - else - { - AX_SAFE_DELETE(pRet); - } - - return pRet; -} - -PhysicsSpriteBox2D* PhysicsSpriteBox2D::createWithTexture(Texture2D* pTexture) -{ - PhysicsSpriteBox2D* pRet = new PhysicsSpriteBox2D(); - if (pRet->initWithTexture(pTexture)) - { - pRet->autorelease(); - } - else - { - AX_SAFE_DELETE(pRet); - } - - return pRet; -} - -PhysicsSpriteBox2D* PhysicsSpriteBox2D::createWithTexture(Texture2D* pTexture, const Rect& rect) -{ - PhysicsSpriteBox2D* pRet = new PhysicsSpriteBox2D(); - if (pRet->initWithTexture(pTexture, rect)) - { - pRet->autorelease(); - } - else - { - AX_SAFE_DELETE(pRet); - } - - return pRet; -} - -PhysicsSpriteBox2D* PhysicsSpriteBox2D::createWithSpriteFrame(SpriteFrame* pSpriteFrame) -{ - PhysicsSpriteBox2D* pRet = new PhysicsSpriteBox2D(); - if (pRet->initWithSpriteFrame(pSpriteFrame)) - { - pRet->autorelease(); - } - else - { - AX_SAFE_DELETE(pRet); - } - - return pRet; -} - -PhysicsSpriteBox2D* PhysicsSpriteBox2D::createWithSpriteFrameName(const char* pszSpriteFrameName) -{ - PhysicsSpriteBox2D* pRet = new PhysicsSpriteBox2D(); - if (pRet->initWithSpriteFrameName(pszSpriteFrameName)) - { - pRet->autorelease(); - } - else - { - AX_SAFE_DELETE(pRet); - } - - return pRet; -} - -PhysicsSpriteBox2D* PhysicsSpriteBox2D::create(const char* pszFileName) -{ - PhysicsSpriteBox2D* pRet = new PhysicsSpriteBox2D(); - if (pRet->initWithFile(pszFileName)) - { - pRet->autorelease(); - } - else - { - AX_SAFE_DELETE(pRet); - } - - return pRet; -} - -PhysicsSpriteBox2D* PhysicsSpriteBox2D::create(const char* pszFileName, const Rect& rect) -{ - PhysicsSpriteBox2D* pRet = new PhysicsSpriteBox2D(); - if (pRet->initWithFile(pszFileName, rect)) - { - pRet->autorelease(); - } - else - { - AX_SAFE_DELETE(pRet); - } - - return pRet; -} - -// this method will only get called if the sprite is batched. -// return YES if the physic's values (angles, position ) changed. -// If you return NO, then getNodeToParentTransform won't be called. -bool PhysicsSpriteBox2D::isDirty() const -{ - return true; -} - -bool PhysicsSpriteBox2D::isIgnoreBodyRotation() const -{ - return _ignoreBodyRotation; -} - -void PhysicsSpriteBox2D::setIgnoreBodyRotation(bool bIgnoreBodyRotation) -{ - _ignoreBodyRotation = bIgnoreBodyRotation; -} - -// Override the setters and getters to always reflect the body's properties. -const Vec2& PhysicsSpriteBox2D::getPosition() const -{ - return getPosFromPhysics(); -} - -void PhysicsSpriteBox2D::getPosition(float* x, float* y) const -{ - if (x == nullptr || y == nullptr) - { - return; - } - const Vec2& pos = getPosFromPhysics(); - *x = pos.x; - *y = pos.y; -} - -float PhysicsSpriteBox2D::getPositionX() const -{ - return getPosFromPhysics().x; -} - -float PhysicsSpriteBox2D::getPositionY() const -{ - return getPosFromPhysics().y; -} - -Vec3 PhysicsSpriteBox2D::getPosition3D() const -{ - Vec2 pos = getPosFromPhysics(); - return Vec3(pos.x, pos.y, 0); -} - -b2Body* PhysicsSpriteBox2D::getB2Body() const -{ - return _pB2Body; -} - -void PhysicsSpriteBox2D::setB2Body(b2Body* pBody) -{ - _pB2Body = pBody; -} - -float PhysicsSpriteBox2D::getPTMRatio() const -{ - return _PTMRatio; -} - -void PhysicsSpriteBox2D::setPTMRatio(float fRatio) -{ - _PTMRatio = fRatio; -} - -// -// Common to Box2d and Chipmunk -// - -const Vec2& PhysicsSpriteBox2D::getPosFromPhysics() const -{ - static Vec2 s_physicPosion; - - b2Vec2 pos = _pB2Body->GetPosition(); - float x = pos.x * _PTMRatio; - float y = pos.y * _PTMRatio; - s_physicPosion.set(x, y); - - return s_physicPosion; -} - -void PhysicsSpriteBox2D::setPosition(float x, float y) -{ - float angle = _pB2Body->GetAngle(); - _pB2Body->SetTransform(b2Vec2(x / _PTMRatio, y / _PTMRatio), angle); -} - -void PhysicsSpriteBox2D::setPosition(const Vec2& pos) -{ - setPosition(pos.x, pos.y); -} - -void PhysicsSpriteBox2D::setPositionX(float x) -{ - setPosition(x, getPositionY()); -} - -void PhysicsSpriteBox2D::setPositionY(float y) -{ - setPosition(getPositionX(), y); -} - -void PhysicsSpriteBox2D::setPosition3D(const Vec3& position) -{ - setPosition(position.x, position.y); -} - -float PhysicsSpriteBox2D::getRotation() const -{ - return (_ignoreBodyRotation ? Sprite::getRotation() : AX_RADIANS_TO_DEGREES(_pB2Body->GetAngle())); -} - -void PhysicsSpriteBox2D::setRotation(float fRotation) -{ - if (_ignoreBodyRotation) - { - Sprite::setRotation(fRotation); - } - - else - { - b2Vec2 p = _pB2Body->GetPosition(); - float radians = AX_DEGREES_TO_RADIANS(fRotation); - _pB2Body->SetTransform(p, radians); - } -} - -void PhysicsSpriteBox2D::syncPhysicsTransform() const -{ - // Although scale is not used by physics engines, it is calculated just in case - // the sprite is animated (scaled up/down) using actions. - // For more info see: http://www.cocos2d-iphone.org/forum/topic/68990 - - b2Vec2 pos = _pB2Body->GetPosition(); - - float x = pos.x * _PTMRatio; - float y = pos.y * _PTMRatio; - - if (_ignoreAnchorPointForPosition) - { - x += _anchorPointInPoints.x; - y += _anchorPointInPoints.y; - } - - // Make matrix - float radians = _pB2Body->GetAngle(); - float c = cosf(radians); - float s = sinf(radians); - - if (!_anchorPointInPoints.isZero()) - { - x += ((c * -_anchorPointInPoints.x * _scaleX) + (-s * -_anchorPointInPoints.y * _scaleY)); - y += ((s * -_anchorPointInPoints.x * _scaleX) + (c * -_anchorPointInPoints.y * _scaleY)); - } - - // Rot, Translate Matrix - - float mat[] = {(float)c * _scaleX, - (float)s * _scaleX, - 0, - 0, - (float)-s * _scaleY, - (float)c * _scaleY, - 0, - 0, - 0, - 0, - 1, - 0, - x, - y, - 0, - 1}; - - _transform.set(mat); -} - -void PhysicsSpriteBox2D::onEnter() -{ - Node::onEnter(); - _syncTransform = Director::getInstance()->getEventDispatcher()->addCustomEventListener( - Director::EVENT_AFTER_UPDATE, std::bind(&PhysicsSpriteBox2D::afterUpdate, this, std::placeholders::_1)); - _syncTransform->retain(); -} - -void PhysicsSpriteBox2D::onExit() -{ - if (_syncTransform != nullptr) - { - Director::getInstance()->getEventDispatcher()->removeEventListener(_syncTransform); - _syncTransform->release(); - } - Node::onExit(); -} - -void PhysicsSpriteBox2D::afterUpdate(EventCustom* /*event*/) -{ - syncPhysicsTransform(); - - _transformDirty = false; - _transformUpdated = true; - setDirtyRecursively(true); -} - -NS_AX_EXT_END \ No newline at end of file diff --git a/extensions/physics-nodes/src/physics-nodes/PhysicsSpriteBox2D.h b/extensions/physics-nodes/src/physics-nodes/PhysicsSpriteBox2D.h deleted file mode 100644 index 3ded174c1c3a..000000000000 --- a/extensions/physics-nodes/src/physics-nodes/PhysicsSpriteBox2D.h +++ /dev/null @@ -1,137 +0,0 @@ -/* Copyright (c) 2012 Scott Lembcke and Howling Moon Software - * Copyright (c) 2012 cocos2d-x.org - * Copyright (c) 2017-2018 Xiamen Yaji Software Co., Ltd. - * Copyright (c) 2021 @aismann; Peter Eismann, Germany; dreifrankensoft - * - * 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 __PHYSICSNODES_CCPHYSICSSPRITEBOX2D_H__ -#define __PHYSICSNODES_CCPHYSICSSPRITEBOX2D_H__ - -#include "2d/Sprite.h" -#include "extensions/ExtensionMacros.h" -#include "extensions/ExtensionExport.h" -#include "base/EventListenerCustom.h" - -class b2Body; - -NS_AX_EXT_BEGIN - -/** A Sprite subclass that is bound to a physics body. - It works with: - - Box2D - - Features and Limitations: - - Scale and Skew properties are ignored. - - Position and rotation are going to updated from the physics body - - If you update the rotation or position manually, the physics body will be updated - - You can't enble both Chipmunk support and Box2d support at the same time. Only one can be enabled at compile time - * @lua NA - */ -class AX_EX_DLL PhysicsSpriteBox2D : public Sprite -{ -public: - static PhysicsSpriteBox2D* create(); - /** Creates an sprite with a texture. - The rect used will be the size of the texture. - The offset will be (0,0). - */ - static PhysicsSpriteBox2D* createWithTexture(Texture2D* pTexture); - - /** Creates an sprite with a texture and a rect. - The offset will be (0,0). - */ - static PhysicsSpriteBox2D* createWithTexture(Texture2D* pTexture, const Rect& rect); - - /** Creates an sprite with an sprite frame. */ - static PhysicsSpriteBox2D* createWithSpriteFrame(SpriteFrame* pSpriteFrame); - - /** Creates an sprite with an sprite frame name. - An SpriteFrame will be fetched from the SpriteFrameCache by name. - If the SpriteFrame doesn't exist it will raise an exception. - @since v0.9 - */ - static PhysicsSpriteBox2D* createWithSpriteFrameName(const char* pszSpriteFrameName); - - /** Creates an sprite with an image filename. - The rect used will be the size of the image. - The offset will be (0,0). - */ - static PhysicsSpriteBox2D* create(const char* pszFileName); - - /** Creates an sprite with an image filename and a rect. - The offset will be (0,0). - */ - static PhysicsSpriteBox2D* create(const char* pszFileName, const Rect& rect); - - PhysicsSpriteBox2D(); - - virtual bool isDirty() const override; - - /** Keep the sprite's rotation separate from the body. */ - bool isIgnoreBodyRotation() const; - void setIgnoreBodyRotation(bool bIgnoreBodyRotation); - - // - // Box2d specific - // - /** Body accessor when using box2d */ - b2Body* getB2Body() const; - void setB2Body(b2Body* pBody); - - float getPTMRatio() const; - void setPTMRatio(float fPTMRatio); - virtual void syncPhysicsTransform() const; - - // overrides - virtual const Vec2& getPosition() const override; - virtual void getPosition(float* x, float* y) const override; - virtual float getPositionX() const override; - virtual float getPositionY() const override; - virtual Vec3 getPosition3D() const override; - virtual void setPosition(const Vec2& position) override; - virtual void setPosition(float x, float y) override; - virtual void setPositionX(float x) override; - virtual void setPositionY(float y) override; - virtual void setPosition3D(const Vec3& position) override; - virtual float getRotation() const override; - virtual void setRotation(float fRotation) override; - - virtual void onEnter() override; - virtual void onExit() override; - -protected: - const Vec2& getPosFromPhysics() const; - void afterUpdate(EventCustom* event); - -protected: - bool _ignoreBodyRotation; - - // box2d specific - b2Body* _pB2Body; - float _PTMRatio; - - // Event for update synchronise physic transform - ax::EventListenerCustom* _syncTransform; -}; - -NS_AX_EXT_END - -#endif // __PHYSICSNODES_CCPHYSICSSPRITEBOX2D_H__ diff --git a/extensions/physics-nodes/src/physics-nodes/PhysicsSpriteChipmunk2D.cpp b/extensions/physics-nodes/src/physics-nodes/PhysicsSpriteChipmunk2D.cpp deleted file mode 100644 index 56f6377868b5..000000000000 --- a/extensions/physics-nodes/src/physics-nodes/PhysicsSpriteChipmunk2D.cpp +++ /dev/null @@ -1,329 +0,0 @@ -/* Copyright (c) 2012 Scott Lembcke and Howling Moon Software - * Copyright (c) 2012 cocos2d-x.org - * Copyright (c) 2017-2018 Xiamen Yaji Software Co., Ltd. - * Copyright (c) 2021 @aismann; Peter Eismann, Germany; dreifrankensoft - * - * 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 "PhysicsSpriteChipmunk2D.h" -#include "base/Director.h" -#include "base/EventDispatcher.h" - -#include "chipmunk/chipmunk.h" - -NS_AX_EXT_BEGIN - -PhysicsSpriteChipmunk2D::PhysicsSpriteChipmunk2D() - : _ignoreBodyRotation(false), _CPBody(nullptr), _syncTransform(nullptr) -{} - -PhysicsSpriteChipmunk2D* PhysicsSpriteChipmunk2D::create() -{ - PhysicsSpriteChipmunk2D* pRet = new PhysicsSpriteChipmunk2D(); - if (pRet->init()) - { - pRet->autorelease(); - } - else - { - AX_SAFE_DELETE(pRet); - } - - return pRet; -} - -PhysicsSpriteChipmunk2D* PhysicsSpriteChipmunk2D::createWithTexture(Texture2D* pTexture) -{ - PhysicsSpriteChipmunk2D* pRet = new PhysicsSpriteChipmunk2D(); - if (pRet->initWithTexture(pTexture)) - { - pRet->autorelease(); - } - else - { - AX_SAFE_DELETE(pRet); - } - - return pRet; -} - -PhysicsSpriteChipmunk2D* PhysicsSpriteChipmunk2D::createWithTexture(Texture2D* pTexture, const Rect& rect) -{ - PhysicsSpriteChipmunk2D* pRet = new PhysicsSpriteChipmunk2D(); - if (pRet->initWithTexture(pTexture, rect)) - { - pRet->autorelease(); - } - else - { - AX_SAFE_DELETE(pRet); - } - - return pRet; -} - -PhysicsSpriteChipmunk2D* PhysicsSpriteChipmunk2D::createWithSpriteFrame(SpriteFrame* pSpriteFrame) -{ - PhysicsSpriteChipmunk2D* pRet = new PhysicsSpriteChipmunk2D(); - if (pRet->initWithSpriteFrame(pSpriteFrame)) - { - pRet->autorelease(); - } - else - { - AX_SAFE_DELETE(pRet); - } - - return pRet; -} - -PhysicsSpriteChipmunk2D* PhysicsSpriteChipmunk2D::createWithSpriteFrameName(const char* pszSpriteFrameName) -{ - PhysicsSpriteChipmunk2D* pRet = new PhysicsSpriteChipmunk2D(); - if (pRet->initWithSpriteFrameName(pszSpriteFrameName)) - { - pRet->autorelease(); - } - else - { - AX_SAFE_DELETE(pRet); - } - - return pRet; -} - -PhysicsSpriteChipmunk2D* PhysicsSpriteChipmunk2D::create(const char* pszFileName) -{ - PhysicsSpriteChipmunk2D* pRet = new PhysicsSpriteChipmunk2D(); - if (pRet->initWithFile(pszFileName)) - { - pRet->autorelease(); - } - else - { - AX_SAFE_DELETE(pRet); - } - - return pRet; -} - -PhysicsSpriteChipmunk2D* PhysicsSpriteChipmunk2D::create(const char* pszFileName, const Rect& rect) -{ - PhysicsSpriteChipmunk2D* pRet = new PhysicsSpriteChipmunk2D(); - if (pRet->initWithFile(pszFileName, rect)) - { - pRet->autorelease(); - } - else - { - AX_SAFE_DELETE(pRet); - } - - return pRet; -} - -// this method will only get called if the sprite is batched. -// return YES if the physic's values (angles, position ) changed. -// If you return NO, then getNodeToParentTransform won't be called. -bool PhysicsSpriteChipmunk2D::isDirty() const -{ - return true; -} - -bool PhysicsSpriteChipmunk2D::isIgnoreBodyRotation() const -{ - return _ignoreBodyRotation; -} - -void PhysicsSpriteChipmunk2D::setIgnoreBodyRotation(bool bIgnoreBodyRotation) -{ - _ignoreBodyRotation = bIgnoreBodyRotation; -} - -// Override the setters and getters to always reflect the body's properties. -const Vec2& PhysicsSpriteChipmunk2D::getPosition() const -{ - return getPosFromPhysics(); -} - -void PhysicsSpriteChipmunk2D::getPosition(float* x, float* y) const -{ - if (x == nullptr || y == nullptr) - { - return; - } - const Vec2& pos = getPosFromPhysics(); - *x = pos.x; - *y = pos.y; -} - -float PhysicsSpriteChipmunk2D::getPositionX() const -{ - return getPosFromPhysics().x; -} - -float PhysicsSpriteChipmunk2D::getPositionY() const -{ - return getPosFromPhysics().y; -} - -Vec3 PhysicsSpriteChipmunk2D::getPosition3D() const -{ - Vec2 pos = getPosFromPhysics(); - return Vec3(pos.x, pos.y, 0); -} - -// -// Chipmunk only -// - -cpBody* PhysicsSpriteChipmunk2D::getCPBody() const -{ - return _CPBody; -} - -void PhysicsSpriteChipmunk2D::setCPBody(cpBody* pBody) -{ - _CPBody = pBody; -} - -// -// Common to Box2d and Chipmunk -// - -const Vec2& PhysicsSpriteChipmunk2D::getPosFromPhysics() const -{ - static Vec2 s_physicPosion; - - cpVect cpPos = cpBodyGetPosition(_CPBody); - s_physicPosion = Vec2(cpPos.x, cpPos.y); - - return s_physicPosion; -} - -void PhysicsSpriteChipmunk2D::setPosition(float x, float y) -{ - cpVect cpPos = cpv(x, y); - cpBodySetPosition(_CPBody, cpPos); -} - -void PhysicsSpriteChipmunk2D::setPosition(const Vec2& pos) -{ - setPosition(pos.x, pos.y); -} - -void PhysicsSpriteChipmunk2D::setPositionX(float x) -{ - setPosition(x, getPositionY()); -} - -void PhysicsSpriteChipmunk2D::setPositionY(float y) -{ - setPosition(getPositionX(), y); -} - -void PhysicsSpriteChipmunk2D::setPosition3D(const Vec3& position) -{ - setPosition(position.x, position.y); -} - -float PhysicsSpriteChipmunk2D::getRotation() const -{ - return (_ignoreBodyRotation ? Sprite::getRotation() : -AX_RADIANS_TO_DEGREES(cpBodyGetAngle(_CPBody))); -} - -void PhysicsSpriteChipmunk2D::setRotation(float fRotation) -{ - if (_ignoreBodyRotation) - { - Sprite::setRotation(fRotation); - } - else - { - cpBodySetAngle(_CPBody, -AX_DEGREES_TO_RADIANS(fRotation)); - } -} - -void PhysicsSpriteChipmunk2D::syncPhysicsTransform() const -{ - // Although scale is not used by physics engines, it is calculated just in case - // the sprite is animated (scaled up/down) using actions. - // For more info see: http://www.cocos2d-iphone.org/forum/topic/68990 - - cpVect rot = (_ignoreBodyRotation ? cpvforangle(-AX_DEGREES_TO_RADIANS(_rotationX)) : cpBodyGetRotation(_CPBody)); - float x = cpBodyGetPosition(_CPBody).x + rot.x * -_anchorPointInPoints.x * _scaleX - - rot.y * -_anchorPointInPoints.y * _scaleY; - float y = cpBodyGetPosition(_CPBody).y + rot.y * -_anchorPointInPoints.x * _scaleX + - rot.x * -_anchorPointInPoints.y * _scaleY; - - if (_ignoreAnchorPointForPosition) - { - x += _anchorPointInPoints.x; - y += _anchorPointInPoints.y; - } - - float mat[] = {(float)rot.x * _scaleX, - (float)rot.y * _scaleX, - 0, - 0, - (float)-rot.y * _scaleY, - (float)rot.x * _scaleY, - 0, - 0, - 0, - 0, - 1, - 0, - x, - y, - 0, - 1}; - - _transform.set(mat); -} - -void PhysicsSpriteChipmunk2D::onEnter() -{ - Node::onEnter(); - _syncTransform = Director::getInstance()->getEventDispatcher()->addCustomEventListener( - Director::EVENT_AFTER_UPDATE, std::bind(&PhysicsSpriteChipmunk2D::afterUpdate, this, std::placeholders::_1)); - _syncTransform->retain(); -} - -void PhysicsSpriteChipmunk2D::onExit() -{ - if (_syncTransform != nullptr) - { - Director::getInstance()->getEventDispatcher()->removeEventListener(_syncTransform); - _syncTransform->release(); - } - Node::onExit(); -} - -void PhysicsSpriteChipmunk2D::afterUpdate(EventCustom* /*event*/) -{ - syncPhysicsTransform(); - - _transformDirty = false; - _transformUpdated = true; - setDirtyRecursively(true); -} - -NS_AX_EXT_END \ No newline at end of file diff --git a/extensions/physics-nodes/src/physics-nodes/PhysicsSpriteChipmunk2D.h b/extensions/physics-nodes/src/physics-nodes/PhysicsSpriteChipmunk2D.h deleted file mode 100644 index b2be1bd709c4..000000000000 --- a/extensions/physics-nodes/src/physics-nodes/PhysicsSpriteChipmunk2D.h +++ /dev/null @@ -1,136 +0,0 @@ -/* Copyright (c) 2012 Scott Lembcke and Howling Moon Software - * Copyright (c) 2012 cocos2d-x.org - * Copyright (c) 2017-2018 Xiamen Yaji Software Co., Ltd. - * Copyright (c) 2021 @aismann; Peter Eismann, Germany; dreifrankensoft - * - * 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 __PHYSICSNODES_CCPhysicsSpriteChipmunk2DCHIPMUNK2D_H__ -#define __PHYSICSNODES_CCPhysicsSpriteChipmunk2DCHIPMUNK2D_H__ - -#include "2d/Sprite.h" -#include "extensions/ExtensionMacros.h" -#include "extensions/ExtensionExport.h" -#include "base/EventListenerCustom.h" - -struct cpBody; - -NS_AX_EXT_BEGIN - -/** A Sprite subclass that is bound to a physics body. - It works with: - - Chipmunk2D: - - Features and Limitations: - - Scale and Skew properties are ignored. - - Position and rotation are going to updated from the physics body - - If you update the rotation or position manually, the physics body will be updated - - You can't enble both Chipmunk support and Box2d support at the same time. Only one can be enabled at compile time - * @lua NA - */ -class AX_EX_DLL PhysicsSpriteChipmunk2D : public Sprite -{ -public: - static PhysicsSpriteChipmunk2D* create(); - /** Creates an sprite with a texture. - The rect used will be the size of the texture. - The offset will be (0,0). - */ - static PhysicsSpriteChipmunk2D* createWithTexture(Texture2D* pTexture); - - /** Creates an sprite with a texture and a rect. - The offset will be (0,0). - */ - static PhysicsSpriteChipmunk2D* createWithTexture(Texture2D* pTexture, const Rect& rect); - - /** Creates an sprite with an sprite frame. */ - static PhysicsSpriteChipmunk2D* createWithSpriteFrame(SpriteFrame* pSpriteFrame); - - /** Creates an sprite with an sprite frame name. - An SpriteFrame will be fetched from the SpriteFrameCache by name. - If the SpriteFrame doesn't exist it will raise an exception. - @since v0.9 - */ - static PhysicsSpriteChipmunk2D* createWithSpriteFrameName(const char* pszSpriteFrameName); - - /** Creates an sprite with an image filename. - The rect used will be the size of the image. - The offset will be (0,0). - */ - static PhysicsSpriteChipmunk2D* create(const char* pszFileName); - - /** Creates an sprite with an image filename and a rect. - The offset will be (0,0). - */ - static PhysicsSpriteChipmunk2D* create(const char* pszFileName, const Rect& rect); - - PhysicsSpriteChipmunk2D(); - - virtual bool isDirty() const override; - - /** Keep the sprite's rotation separate from the body. */ - bool isIgnoreBodyRotation() const; - void setIgnoreBodyRotation(bool bIgnoreBodyRotation); - - // - // Chipmunk specific - // - /** Body accessor when using regular Chipmunk */ - cpBody* getCPBody() const; - void setCPBody(cpBody* pBody); - - float getPTMRatio() const; - void setPTMRatio(float fPTMRatio); - virtual void syncPhysicsTransform() const; - - // overrides - virtual const Vec2& getPosition() const override; - virtual void getPosition(float* x, float* y) const override; - virtual float getPositionX() const override; - virtual float getPositionY() const override; - virtual Vec3 getPosition3D() const override; - virtual void setPosition(const Vec2& position) override; - virtual void setPosition(float x, float y) override; - virtual void setPositionX(float x) override; - virtual void setPositionY(float y) override; - virtual void setPosition3D(const Vec3& position) override; - virtual float getRotation() const override; - virtual void setRotation(float fRotation) override; - - virtual void onEnter() override; - virtual void onExit() override; - -protected: - const Vec2& getPosFromPhysics() const; - void afterUpdate(EventCustom* event); - -protected: - bool _ignoreBodyRotation; - - // chipmunk specific - cpBody* _CPBody; - - // Event for update synchronise physic transform - ax::EventListenerCustom* _syncTransform; -}; - -NS_AX_EXT_END - -#endif // __PHYSICSNODES_CCPhysicsSpriteChipmunk2DCHIPMUNK2D_H__ diff --git a/tests/cpp-tests/CMakeLists.txt b/tests/cpp-tests/CMakeLists.txt index 18248706f187..ac87a04aa514 100644 --- a/tests/cpp-tests/CMakeLists.txt +++ b/tests/cpp-tests/CMakeLists.txt @@ -404,74 +404,8 @@ list(APPEND GAME_SOURCE ) if((WINDOWS OR MACOSX OR LINUX OR WASM) AND (NOT WINRT)) - list(APPEND GAME_HEADER - Source/Box2DTestBed/tests/test.h - Source/Box2DTestBed/tests/settings.h - Source/Box2DTestBed/Box2DTestBed.h - ) - - list(APPEND GAME_SOURCE - Source/Box2DTestBed/Box2DTestBed.cpp - Source/Box2DTestBed/test.cpp - Source/Box2DTestBed/tests/add_pair.cpp - Source/Box2DTestBed/tests/apply_force.cpp - Source/Box2DTestBed/tests/body_types.cpp - Source/Box2DTestBed/tests/box_stack.cpp - Source/Box2DTestBed/tests/breakable.cpp - Source/Box2DTestBed/tests/bridge.cpp - Source/Box2DTestBed/tests/bullet_test.cpp - Source/Box2DTestBed/tests/cantilever.cpp - Source/Box2DTestBed/tests/car.cpp - Source/Box2DTestBed/tests/chain.cpp - Source/Box2DTestBed/tests/chain_problem.cpp - Source/Box2DTestBed/tests/character_collision.cpp - Source/Box2DTestBed/tests/circle_stack.cpp - Source/Box2DTestBed/tests/collision_filtering.cpp - Source/Box2DTestBed/tests/collision_processing.cpp - Source/Box2DTestBed/tests/compound_shapes.cpp - Source/Box2DTestBed/tests/confined.cpp - Source/Box2DTestBed/tests/continuous_test.cpp - Source/Box2DTestBed/tests/convex_hull.cpp - Source/Box2DTestBed/tests/conveyor_belt.cpp - Source/Box2DTestBed/tests/distance_joint.cpp - Source/Box2DTestBed/tests/distance_test.cpp - Source/Box2DTestBed/tests/dominos.cpp - Source/Box2DTestBed/tests/dump_loader.cpp - Source/Box2DTestBed/tests/dynamic_tree.cpp - Source/Box2DTestBed/tests/edge_shapes.cpp - Source/Box2DTestBed/tests/edge_test.cpp - Source/Box2DTestBed/tests/friction.cpp - Source/Box2DTestBed/tests/gear_joint.cpp - Source/Box2DTestBed/tests/heavy1.cpp - Source/Box2DTestBed/tests/heavy2.cpp - Source/Box2DTestBed/tests/mobile_balanced.cpp - Source/Box2DTestBed/tests/mobile_unbalanced.cpp - Source/Box2DTestBed/tests/motor_joint.cpp - Source/Box2DTestBed/tests/pinball.cpp - Source/Box2DTestBed/tests/platformer.cpp - Source/Box2DTestBed/tests/polygon_collision.cpp - Source/Box2DTestBed/tests/polygon_shapes.cpp - Source/Box2DTestBed/tests/prismatic_joint.cpp - Source/Box2DTestBed/tests/pulley_joint.cpp - Source/Box2DTestBed/tests/pyramid.cpp - Source/Box2DTestBed/tests/ray_cast.cpp - Source/Box2DTestBed/tests/restitution.cpp - Source/Box2DTestBed/tests/revolute_joint.cpp - Source/Box2DTestBed/tests/rope.cpp - Source/Box2DTestBed/tests/sensor.cpp - Source/Box2DTestBed/tests/shape_cast.cpp - Source/Box2DTestBed/tests/shape_editing.cpp - Source/Box2DTestBed/tests/skier.cpp - Source/Box2DTestBed/tests/slider_crank_1.cpp - Source/Box2DTestBed/tests/slider_crank_2.cpp - Source/Box2DTestBed/tests/theo_jansen.cpp - Source/Box2DTestBed/tests/tiles.cpp - Source/Box2DTestBed/tests/time_of_impact.cpp - Source/Box2DTestBed/tests/tumbler.cpp - Source/Box2DTestBed/tests/web.cpp - Source/Box2DTestBed/tests/wheel_joint.cpp - Source/Box2DTestBed/tests/wrecking_ball.cpp - ) + file(GLOB_RECURSE BOX2D_TESTBED_SOURCES Source/Box2DTestBed/*.h;Source/Box2DTestBed/*.cpp) + list(APPEND GAME_SOURCE ${BOX2D_TESTBED_SOURCES}) endif() list(APPEND GAME_HEADER @@ -484,51 +418,12 @@ list(APPEND GAME_SOURCE ) list(APPEND GAME_HEADER Source/PhysicsTest/PhysicsTest.h - Source/ChipmunkTest/ChipmunkTest.h ) list(APPEND GAME_SOURCE Source/PhysicsTest/PhysicsTest.cpp - Source/ChipmunkTest/ChipmunkTest.cpp ) -if(WINDOWS OR MACOSX OR LINUX OR WASM) - list(APPEND GAME_HEADER - Source/ChipmunkTestBed/demo/ChipmunkDemo.h - Source/ChipmunkTestBed/ChipmunkTestBed.h - ) - set (TESTBED_C_SORUCES - Source/ChipmunkTestBed/demo/Bench.cpp - Source/ChipmunkTestBed/demo/Chains.cpp - Source/ChipmunkTestBed/demo/Convex.cpp - Source/ChipmunkTestBed/demo/Crane.cpp - Source/ChipmunkTestBed/demo/Joints.cpp - Source/ChipmunkTestBed/demo/LogoSmash.cpp - Source/ChipmunkTestBed/demo/OneWay.cpp - Source/ChipmunkTestBed/demo/Planet.cpp - Source/ChipmunkTestBed/demo/Player.cpp - Source/ChipmunkTestBed/demo/Plink.cpp - Source/ChipmunkTestBed/demo/Pump.cpp - Source/ChipmunkTestBed/demo/PyramidStack.cpp - Source/ChipmunkTestBed/demo/PyramidTopple.cpp - Source/ChipmunkTestBed/demo/Shatter.cpp - Source/ChipmunkTestBed/demo/Springies.cpp - Source/ChipmunkTestBed/demo/Sticky.cpp - Source/ChipmunkTestBed/demo/Tank.cpp - Source/ChipmunkTestBed/demo/TheoJansen.cpp - Source/ChipmunkTestBed/demo/Buoyancy.cpp - Source/ChipmunkTestBed/demo/ContactGraph.cpp - Source/ChipmunkTestBed/demo/Example.cpp - Source/ChipmunkTestBed/demo/Query.cpp - Source/ChipmunkTestBed/demo/Slice.cpp - Source/ChipmunkTestBed/demo/Unicycle.cpp - Source/ChipmunkTestBed/demo/Tumble.cpp) - list(APPEND GAME_SOURCE - ${TESTBED_C_SORUCES} - Source/ChipmunkTestBed/ChipmunkTestBed.cpp - ) -endif() - if (AX_ENABLE_EXT_EFFEKSEER) list(APPEND GAME_HEADER Source/EffekseerTest/EffekseerTest.h) list(APPEND GAME_SOURCE Source/EffekseerTest/EffekseerTest.cpp) diff --git a/tests/cpp-tests/Source/BaseTest.cpp b/tests/cpp-tests/Source/BaseTest.cpp index 84fbef1d5c1e..6d5cfd5787a6 100644 --- a/tests/cpp-tests/Source/BaseTest.cpp +++ b/tests/cpp-tests/Source/BaseTest.cpp @@ -343,6 +343,17 @@ void TestSuite::enterNextTest() Director::getInstance()->replaceScene(scene); } +void TestSuite::enterTest(int index) { + _currTestIndex = index % _childTestNames.size(); + + auto scene = _testCallbacks[_currTestIndex](); + auto testCase = getTestCase(scene); + testCase->setTestSuite(this); + testCase->setTestCaseName(_childTestNames[_currTestIndex]); + + Director::getInstance()->replaceScene(scene); +} + void TestSuite::enterPreviousTest() { if (_currTestIndex > 0) diff --git a/tests/cpp-tests/Source/BaseTest.h b/tests/cpp-tests/Source/BaseTest.h index 8951d1123f6c..210c5f9fb77d 100644 --- a/tests/cpp-tests/Source/BaseTest.h +++ b/tests/cpp-tests/Source/BaseTest.h @@ -172,6 +172,8 @@ class TestSuite : public TestBase virtual void enterNextTest(); virtual void enterPreviousTest(); + void enterTest(int index); + int getCurrTestIndex() { return _currTestIndex; } virtual void runThisTest() override; diff --git a/tests/cpp-tests/Source/Box2DTest/Box2dTest.cpp b/tests/cpp-tests/Source/Box2DTest/Box2dTest.cpp index 4e07c56d12cb..b98ca7b42f9b 100644 --- a/tests/cpp-tests/Source/Box2DTest/Box2dTest.cpp +++ b/tests/cpp-tests/Source/Box2DTest/Box2dTest.cpp @@ -94,154 +94,139 @@ bool Box2DTest::init() this->addChild(menu); menu->setPosition(VisibleRect::right().x - 100, VisibleRect::top().y - 60); - drawBox2D = g_debugDraw.GetDrawNode(); - addChild(drawBox2D, 100); - drawBox2D->setOpacity(150); - scheduleUpdate(); return true; } -Box2DTest::Box2DTest() : _spriteTexture(nullptr), world(nullptr) {} +Box2DTest::Box2DTest() : _spriteTexture(nullptr) {} Box2DTest::~Box2DTest() { - AX_SAFE_DELETE(world); + b2DestroyWorld(world); } void Box2DTest::toggleDebugCallback(Object* sender) { showDebugDraw = !showDebugDraw; - drawBox2D->clear(); + _debugDrawNode->setVisible(showDebugDraw); } -void Box2DTest::initPhysics() +b2BodyId Box2DTest::createRigibody(b2BodyDef* def) { - b2Vec2 gravity; - gravity.Set(0.0f, -10.0f); - world = new b2World(gravity); + auto bodyId = b2CreateBody(world, def); + return bodyId; +} - // Do we want to let bodies sleep? - world->SetAllowSleeping(true); +void Box2DTest::initPhysics() +{ + b2Vec2 gravity = {0.0f, -10.0f}; + b2WorldDef worldDef = b2DefaultWorldDef(); + worldDef.gravity = gravity; + world = b2CreateWorld(&worldDef); - world->SetContinuousPhysics(true); + // debug draw node for world + _debugDrawNode = utils::createInstance(&PhysicsDebugNode::initWithWorld, world); + addChild(_debugDrawNode, 100); + _debugDrawNode->setOpacity(150); - // Define the ground body. - b2BodyDef groundBodyDef; - groundBodyDef.position.Set(0, 0); // bottom-left corner + // Do we want to let bodies sleep? + b2World_EnableSleeping(world, true); + b2World_EnableContinuous(world, true); - // Call the body factory which allocates memory for the ground body - // from a pool and creates the ground box shape (also from a pool). - // The body is also added to the world. - b2Body* groundBody = world->CreateBody(&groundBodyDef); + // The segment ground + b2BodyDef bodyDef = b2DefaultBodyDef(); + b2BodyId groundId = createRigibody(&bodyDef); - // Define the ground box shape. - b2EdgeShape groundBox; + b2ShapeDef shapeDef = b2DefaultShapeDef(); - // bottom - groundBox.SetTwoSided(b2Vec2(VisibleRect::leftBottom().x / PTM_RATIO, VisibleRect::leftBottom().y / PTM_RATIO), - b2Vec2(VisibleRect::rightBottom().x / PTM_RATIO, VisibleRect::rightBottom().y / PTM_RATIO)); - groundBody->CreateFixture(&groundBox, 0); + auto conerOffset = 1.0f / PTM_RATIO; + auto viewRect = VisibleRect::getVisibleRect(); + b2Segment segment = {{conerOffset, conerOffset}, {viewRect.size.width / PTM_RATIO - conerOffset, conerOffset}}; + b2CreateSegmentShape(groundId, &shapeDef, &segment); // bottom - // top - groundBox.SetTwoSided(b2Vec2(VisibleRect::leftTop().x / PTM_RATIO, VisibleRect::leftTop().y / PTM_RATIO), - b2Vec2(VisibleRect::rightTop().x / PTM_RATIO, VisibleRect::rightTop().y / PTM_RATIO)); - groundBody->CreateFixture(&groundBox, 0); + segment = {{conerOffset, viewRect.size.height / PTM_RATIO - conerOffset}, + {viewRect.size.width / PTM_RATIO - conerOffset, viewRect.size.height / PTM_RATIO - conerOffset}}; + b2CreateSegmentShape(groundId, &shapeDef, &segment); // top - // left - groundBox.SetTwoSided(b2Vec2(VisibleRect::leftTop().x / PTM_RATIO, VisibleRect::leftTop().y / PTM_RATIO), - b2Vec2(VisibleRect::leftBottom().x / PTM_RATIO, VisibleRect::leftBottom().y / PTM_RATIO)); - groundBody->CreateFixture(&groundBox, 0); + segment = {{conerOffset, conerOffset}, {conerOffset, viewRect.size.height / PTM_RATIO - conerOffset}}; + b2CreateSegmentShape(groundId, &shapeDef, &segment); // left - // right - groundBox.SetTwoSided(b2Vec2(VisibleRect::rightBottom().x / PTM_RATIO, VisibleRect::rightBottom().y / PTM_RATIO), - b2Vec2(VisibleRect::rightTop().x / PTM_RATIO, VisibleRect::rightTop().y / PTM_RATIO)); - groundBody->CreateFixture(&groundBox, 0); + segment = {{viewRect.size.width / PTM_RATIO - conerOffset, conerOffset}, + {viewRect.size.width / PTM_RATIO - conerOffset, viewRect.size.height / PTM_RATIO - conerOffset}}; + b2CreateSegmentShape(groundId, &shapeDef, &segment); // right // Small triangle b2Vec2 vertices[3]; - vertices[0].Set(-1.0f, 0.0f); - vertices[1].Set(1.0f, 0.0f); - vertices[2].Set(0.0f, 2.0f); - - b2PolygonShape polygon; - polygon.Set(vertices, 3); - - b2FixtureDef triangleShapeDef; - triangleShapeDef.shape = &polygon; - triangleShapeDef.density = 1.0f; - - b2BodyDef triangleBodyDef; - triangleBodyDef.type = b2_dynamicBody; - triangleBodyDef.position.Set(rand() % 13 + 3, 4); - - b2Body* body1 = world->CreateBody(&triangleBodyDef); - body1->CreateFixture(&triangleShapeDef); + vertices[0] = b2Vec2{-1.0f, 0.0f}; + vertices[1] = b2Vec2{1.0f, 0.0f}; + vertices[2] = b2Vec2{0.0f, 2.0f}; + b2Hull hull = b2ComputeHull(vertices, 3); + b2Polygon polygon = b2MakePolygon(&hull, 0.0f); + + b2BodyDef triangleBodyDef = b2DefaultBodyDef(); + triangleBodyDef.type = b2_dynamicBody; + triangleBodyDef.position = b2Vec2{static_cast(rand() % 13 + 3), 4.0f}; // bottom-left corner + auto body1 = createRigibody(&triangleBodyDef); + b2ShapeDef triangleShapeDef = b2DefaultShapeDef(); + triangleShapeDef.density = 1.0f; + b2CreatePolygonShape(body1, &triangleShapeDef, &polygon); // Large triangle (recycle definitions) vertices[0] *= 2.0f; vertices[1] *= 2.0f; vertices[2] *= 2.0f; - polygon.Set(vertices, 3); - - triangleBodyDef.position.Set(rand() % 13 + 3, 4); - b2Body* body2 = world->CreateBody(&triangleBodyDef); - body2->CreateFixture(&triangleShapeDef); + triangleBodyDef.position = b2Vec2{static_cast(rand() % 13 + 3), 4.0f}; + auto body2 = createRigibody(&triangleBodyDef); + hull = b2ComputeHull(vertices, 3); + polygon = b2MakePolygon(&hull, 0.0f); + b2CreatePolygonShape(body2, &triangleShapeDef, &polygon); // Small box - polygon.SetAsBox(1.0f, 0.5f); + polygon = b2MakeBox(1.0f, 0.5f); // .SetAsBox(1.0f, 0.5f); - b2FixtureDef boxShapeDef; - boxShapeDef.shape = &polygon; + auto boxShapeDef = b2DefaultShapeDef(); boxShapeDef.density = 1.0f; - b2BodyDef boxBodyDef; - boxBodyDef.type = b2_dynamicBody; - boxBodyDef.position.Set(rand() % 13 + 3, 4); + b2BodyDef boxBodyDef = b2DefaultBodyDef(); + boxBodyDef.type = b2_dynamicBody; + boxBodyDef.position = b2Vec2{static_cast(rand() % 13 + 3), 4.0f}; - b2Body* body3 = world->CreateBody(&boxBodyDef); - body3->CreateFixture(&boxShapeDef); + auto body3 = createRigibody(&boxBodyDef); + b2CreatePolygonShape(body3, &boxShapeDef, &polygon); // Large box (recycle definitions) - polygon.SetAsBox(2.0f, 1.0f); - boxBodyDef.position.Set(rand() % 13 + 3, 4); + polygon = b2MakeBox(2.0f, 1.0f); // .SetAsBox(2.0f, 1.0f); + boxBodyDef.position = b2Vec2{static_cast(rand() % 13 + 3), 4.0f}; - b2Body* body4 = world->CreateBody(&boxBodyDef); - body4->CreateFixture(&boxShapeDef); + auto body4 = createRigibody(&boxBodyDef); + b2CreatePolygonShape(body4, &boxShapeDef, &polygon); // Small circle - b2CircleShape circle; - circle.m_radius = 1.0f; - - b2FixtureDef circleShapeDef; - circleShapeDef.shape = &circle; + auto circleShapeDef = b2DefaultShapeDef(); circleShapeDef.density = 1.0f; - b2BodyDef circleBodyDef; - circleBodyDef.type = b2_dynamicBody; - circleBodyDef.position.Set(rand() % 13 + 3, 4); + b2Circle circle{{0.0f, 0.0f}, 1.0f}; - b2Body* body5 = world->CreateBody(&circleBodyDef); - body5->CreateFixture(&circleShapeDef); + b2BodyDef circleBodyDef = b2DefaultBodyDef(); + circleBodyDef.type = b2_dynamicBody; + circleBodyDef.position = b2Vec2{static_cast(rand() % 13 + 3), 4.0f}; + + auto body5 = createRigibody(&circleBodyDef); + b2CreateCircleShape(body5, &circleShapeDef, &circle); // Large circle - circle.m_radius *= 2.0f; - circleBodyDef.position.Set(rand() % 13 + 3, 4); - - b2Body* body6 = world->CreateBody(&circleBodyDef); - body6->CreateFixture(&circleShapeDef); - - uint32 flags = 0; - flags += 1 * b2Draw::e_shapeBit; - flags += 1 * b2Draw::e_jointBit; - flags += 0 * b2Draw::e_aabbBit; - flags += 0 * b2Draw::e_centerOfMassBit; - g_debugDraw.SetFlags(flags); - g_debugDraw.mRatio = PTM_RATIO; - g_debugDraw.debugNodeOffset = {0, 0}; - world->SetDebugDraw(&g_debugDraw); + circle.radius *= 2.0f; + circleBodyDef.position = b2Vec2{static_cast(rand() % 13 + 3), 4.0f}; + + auto body6 = createRigibody(&circleBodyDef); + b2CreateCircleShape(body6, &circleShapeDef, &circle); + + _debugDrawNode->setPTMRatio(PTM_RATIO); + auto& settings = _debugDrawNode->getB2DebugDraw(); + settings.drawShapes = true; + settings.drawJoints = true; } void Box2DTest::createResetButton() @@ -261,24 +246,22 @@ void Box2DTest::addNewSpriteAtPosition(Vec2 p) // Define the dynamic body. // Set up a 1m squared box in the physics world - b2BodyDef bodyDef; - bodyDef.type = b2_dynamicBody; - bodyDef.position.Set(p.x / PTM_RATIO, p.y / PTM_RATIO); + b2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.type = b2_dynamicBody; + bodyDef.position = b2Vec2{p.x / PTM_RATIO, p.y / PTM_RATIO}; AXLOGD("Add PTM_RATIO sprite x: {:.2} y: {:.2}", p.x / PTM_RATIO, p.y / PTM_RATIO); - b2Body* body = world->CreateBody(&bodyDef); + auto body = createRigibody(&bodyDef); // Define another box shape for our dynamic body. - b2PolygonShape dynamicBox; - dynamicBox.SetAsBox(.5f, .5f); // These are mid points for our 1m box + auto dynamicBox = b2MakeBox(.5f, .5f); // These are mid points for our 1m box // Define the dynamic body fixture. - b2FixtureDef fixtureDef; - fixtureDef.shape = &dynamicBox; - fixtureDef.density = 1.0f; - fixtureDef.friction = 0.3f; - body->CreateFixture(&fixtureDef); + b2ShapeDef shapeDef = b2DefaultShapeDef(); + shapeDef.density = 1.0f; + shapeDef.friction = 0.3f; + b2CreatePolygonShape(body, &shapeDef, &dynamicBox); auto parent = this->getChildByTag(kTagParentNode); @@ -286,7 +269,7 @@ void Box2DTest::addNewSpriteAtPosition(Vec2 p) // just randomly picking one of the images int idx = (AXRANDOM_0_1() > .5 ? 0 : 1); int idy = (AXRANDOM_0_1() > .5 ? 0 : 1); - auto sprite = PhysicsSpriteBox2D::createWithTexture(_spriteTexture, Rect(32 * idx, 32 * idy, 32, 32)); + auto sprite = PhysicsSprite::createWithTexture(_spriteTexture, Rect(32 * idx, 32 * idy, 32, 32)); parent->addChild(sprite); sprite->setB2Body(body); sprite->setPTMRatio(PTM_RATIO); @@ -300,19 +283,9 @@ void Box2DTest::update(float dt) // You need to make an informed choice, the following URL is useful // http://gafferongames.com/game-physics/fix-your-timestep/ - int velocityIterations = 8; - int positionIterations = 1; - - // Instruct the world to perform a single step of simulation. It is - // generally best to keep the time step and iterations fixed. - world->Step(dt, velocityIterations, positionIterations); - - // Debug draw - if (showDebugDraw) - { - drawBox2D->clear(); - world->DebugDraw(); - } + // The number of sub-steps, increasing the sub-step count can increase accuracy. Typically 4. + constexpr int subStepCount = 4; + b2World_Step(world, _director->getAnimationInterval(), subStepCount); } void Box2DTest::onTouchesEnded(const std::vector& touches, Event* event) diff --git a/tests/cpp-tests/Source/Box2DTest/Box2dTest.h b/tests/cpp-tests/Source/Box2DTest/Box2dTest.h index 3defee8d622d..dbf75366d8a5 100644 --- a/tests/cpp-tests/Source/Box2DTest/Box2dTest.h +++ b/tests/cpp-tests/Source/Box2DTest/Box2dTest.h @@ -53,13 +53,14 @@ class Box2DTest : public TestCase void toggleDebugCallback(ax::Object* sender); + b2BodyId createRigibody(b2BodyDef*); + private: - b2World* world; + b2WorldId world{}; - ax::Texture2D* _spriteTexture; - ax::DrawNode* drawBox2D; - ax::extension::PhysicsDebugNodeBox2D g_debugDraw; - bool showDebugDraw = true; + ax::Texture2D* _spriteTexture{nullptr}; + ax::extension::PhysicsDebugNode* _debugDrawNode{nullptr}; + bool showDebugDraw{true}; }; #endif diff --git a/tests/cpp-tests/Source/Box2DTestBed/Box2DTestBed.cpp b/tests/cpp-tests/Source/Box2DTestBed/Box2DTestBed.cpp index 7166c3f6caaa..5e071913a8ef 100644 --- a/tests/cpp-tests/Source/Box2DTestBed/Box2DTestBed.cpp +++ b/tests/cpp-tests/Source/Box2DTestBed/Box2DTestBed.cpp @@ -28,8 +28,9 @@ #include "axmol.h" #include "Box2DTestBed.h" -#include "tests/test.h" -#include "tests/settings.h" +#include "samples/sample.h" +#include "samples/settings.h" +#include "GLView.h" using namespace ax; USING_NS_AX_EXT; @@ -39,35 +40,52 @@ enum kTagParentNode = 1, }; -Settings settings; -ax::Label* labelDebugDraw; +static Settings s_settings; enum { kTagBox2DNode, }; -TestEntry g_testEntries[MAX_TESTS] = {{nullptr}}; -int g_testCount = 0; +static b2Vec2 tob2Vec2(const Vec2& val) +{ + return b2Vec2{val.x, val.y}; +} -int RegisterTest(const char* category, const char* name, TestCreateFcn* fcn) +static inline int CompareSamples(const void* a, const void* b) { - int index = g_testCount; - if (index < MAX_TESTS) + SampleEntry* sa = (SampleEntry*)a; + SampleEntry* sb = (SampleEntry*)b; + + int result = strcmp(sa->category, sb->category); + if (result == 0) { - g_testEntries[index] = {category, name, fcn}; - ++g_testCount; - return index; + result = strcmp(sa->name, sb->name); } - return -1; + return result; +} + +static void SortTests() +{ + qsort(g_sampleEntries, g_sampleCount, sizeof(SampleEntry), CompareSamples); } Box2DTestBedTests::Box2DTestBedTests() { - for (int entryId = 0; entryId < g_testCount; ++entryId) + // TODO: determine properly view size + g_camera.m_width = 1920; + g_camera.m_height = 1080; + g_camera.m_zoom = 150; + g_camera.m_center = b2Vec2_zero; + + ImGuiPresenter::getInstance()->setViewResolution(g_camera.m_width, g_camera.m_height); + + SortTests(); + + for (int idx = 0; idx < g_sampleCount; ++idx) { - addTestCase(g_testEntries[entryId].name, [entryId]() { return Box2DTestBed::createWithEntryID(entryId); }); + addTestCase(g_sampleEntries[idx].name, [idx]() { return Box2DTestBed::create(idx); }); } } @@ -81,19 +99,20 @@ Box2DTestBed::Box2DTestBed() {} Box2DTestBed::~Box2DTestBed() { - Layer::_eventDispatcher->removeEventListener(_touchListener); + //_eventDispatcher->removeEventListener(_touchListener); + _eventDispatcher->removeEventListener(_keyboardListener); + _eventDispatcher->removeEventListener(_mouseListener); } -Box2DTestBed* Box2DTestBed::createWithEntryID(int entryId) +Box2DTestBed* Box2DTestBed::create(int index) { auto layer = new Box2DTestBed(); - layer->initWithEntryID(entryId); - // layer->autorelease(); - + layer->initWithEntryIndex(index); + layer->autorelease(); return layer; } -bool Box2DTestBed::initWithEntryID(int entryId) +bool Box2DTestBed::initWithEntryIndex(int index) { if (!TestCase::init()) { @@ -103,177 +122,312 @@ bool Box2DTestBed::initWithEntryID(int entryId) Vec2 visibleOrigin = director->getVisibleOrigin(); Size visibleSize = director->getVisibleSize(); - m_entryID = entryId; + m_entryIndex = index; - m_entry = g_testEntries + entryId; - m_test = m_entry->createFcn(); - - debugDrawNode = g_debugDraw.GetDrawNode(); - m_test->debugDrawNode = debugDrawNode; - m_test->g_debugDraw = g_debugDraw; - - TestCase::addChild(debugDrawNode, 100); + m_entry = g_sampleEntries + index; + m_sample = m_entry->createFcn(s_settings); // init physics this->initPhysics(); auto label = Label::createWithTTF(m_entry->name, "fonts/arial.ttf", 28); - TestCase::addChild(label, 1); + addChild(label, 1); label->setPosition(visibleOrigin.x + visibleSize.width / 2, visibleOrigin.y + visibleSize.height - 50); // Adds touch event listener - _touchListener = EventListenerTouchOneByOne::create(); - _touchListener->setSwallowTouches(true); - _touchListener->onTouchBegan = AX_CALLBACK_2(Box2DTestBed::onTouchBegan, this); - _touchListener->onTouchMoved = AX_CALLBACK_2(Box2DTestBed::onTouchMoved, this); - _touchListener->onTouchEnded = AX_CALLBACK_2(Box2DTestBed::onTouchEnded, this); - TestCase::_eventDispatcher->addEventListenerWithFixedPriority(_touchListener, 10); + // _touchListener = EventListenerTouchOneByOne::create(); + // _touchListener->setSwallowTouches(true); + // _touchListener->onTouchBegan = AX_CALLBACK_2(Box2DTestBed::onTouchBegan, this); + // _touchListener->onTouchMoved = AX_CALLBACK_2(Box2DTestBed::onTouchMoved, this); + // _touchListener->onTouchEnded = AX_CALLBACK_2(Box2DTestBed::onTouchEnded, this); + // _eventDispatcher->addEventListenerWithFixedPriority(_touchListener, 10); // Adds Keyboard event listener _keyboardListener = EventListenerKeyboard::create(); _keyboardListener->onKeyPressed = AX_CALLBACK_2(Box2DTestBed::onKeyPressed, this); _keyboardListener->onKeyReleased = AX_CALLBACK_2(Box2DTestBed::onKeyReleased, this); - TestCase::_eventDispatcher->addEventListenerWithFixedPriority(_keyboardListener, 11); + _eventDispatcher->addEventListenerWithFixedPriority(_keyboardListener, 11); - auto _mouseListener = EventListenerMouse::create(); + _mouseListener = EventListenerMouse::create(); _mouseListener->onMouseMove = AX_CALLBACK_1(Box2DTestBed::onMouseMove, this); _mouseListener->onMouseUp = AX_CALLBACK_1(Box2DTestBed::onMouseUp, this); _mouseListener->onMouseDown = AX_CALLBACK_1(Box2DTestBed::onMouseDown, this); _mouseListener->onMouseScroll = AX_CALLBACK_1(Box2DTestBed::onMouseScroll, this); - TestCase::_eventDispatcher->addEventListenerWithFixedPriority(_mouseListener, 12); - - // Demo messageString - labelDebugDraw = Label::createWithTTF("TEST", "fonts/arial.ttf", 8.0f); - labelDebugDraw->setAnchorPoint(Vec2(0, 1)); - labelDebugDraw->setPosition(VisibleRect::left().x, VisibleRect::top().y - 10); - labelDebugDraw->setColor(Color3B::WHITE); - TestCase::addChild(labelDebugDraw, 100); + _eventDispatcher->addEventListenerWithFixedPriority(_mouseListener, 12); - TestCase::scheduleUpdate(); + scheduleUpdate(); return true; } -bool Box2DTestBed::onTouchBegan(Touch* touch, Event* event) -{ - auto location = touch->getLocation() - g_debugDraw.debugNodeOffset; - b2Vec2 pos = {location.x / g_debugDraw.mRatio, location.y / g_debugDraw.mRatio}; - return m_test->MouseDown(pos); -} - -void Box2DTestBed::onTouchMoved(Touch* touch, Event* event) -{ - auto location = touch->getLocation() - g_debugDraw.debugNodeOffset; - b2Vec2 pos = {location.x / g_debugDraw.mRatio, location.y / g_debugDraw.mRatio}; - m_test->MouseMove(pos); -} - -void Box2DTestBed::onTouchEnded(Touch* touch, Event* event) -{ - auto location = touch->getLocation() - g_debugDraw.debugNodeOffset; - b2Vec2 pos = {location.x / g_debugDraw.mRatio, location.y / g_debugDraw.mRatio}; - m_test->MouseUp(pos); -} - void Box2DTestBed::onKeyPressed(EventKeyboard::KeyCode code, Event* event) { - AXLOGD("onKeyPressed, keycode: {}", static_cast(code)); - m_test->Keyboard((static_cast(code) - 59)); // its a bad hack! + // AXLOGD("onKeyPressed, keycode: {}", static_cast(code)); + // m_sample->Keyboard((static_cast(code) - 59)); // its a bad hack! } void Box2DTestBed::onKeyReleased(EventKeyboard::KeyCode code, Event* event) { AXLOGD("onKeyPressed, keycode: {}", static_cast(code)); - m_test->KeyboardUp((static_cast(code) - 59)); // its a bad hack! + // m_sample->KeyboardUp((static_cast(code) - 59)); // its a bad hack! + m_sample->Keyboard((static_cast(code) - 59)); } void Box2DTestBed::onMouseDown(Event* event) { - EventMouse* e = (EventMouse*)event; - button[(int)EventMouse::MouseButton::BUTTON_LEFT] = false; - button[(int)EventMouse::MouseButton::BUTTON_RIGHT] = false; - button[(int)EventMouse::MouseButton::BUTTON_MIDDLE] = false; - switch (e->getMouseButton()) - { - case EventMouse::MouseButton::BUTTON_LEFT: - button[(int)EventMouse::MouseButton::BUTTON_LEFT] = true; - break; - case EventMouse::MouseButton::BUTTON_RIGHT: - button[(int)EventMouse::MouseButton::BUTTON_RIGHT] = true; - break; - case EventMouse::MouseButton::BUTTON_MIDDLE: - button[(int)EventMouse::MouseButton::BUTTON_MIDDLE] = true; - break; - } + EventMouse* e = static_cast(event); + + auto location = e->getLocation() - _debugDraw->getWorldOffset(); + b2Vec2 pos = {location.x / _debugDraw->getPTMRatio(), location.y / _debugDraw->getPTMRatio()}; + + int mods = 0; +#if defined(_WIN32) + if (GetAsyncKeyState(VK_SHIFT) & 0x80) + mods |= GLFW_MOD_SHIFT; + if (GetAsyncKeyState(VK_CONTROL) & 0x80) + mods |= GLFW_MOD_CONTROL; + if (GetAsyncKeyState(VK_MENU) & 0x80) + mods |= GLFW_MOD_ALT; +#endif + _draging = true; + _mouseDownPos = pos; + _dragingStartPos = _debugDraw->getPosition(); + + m_sample->MouseDown(pos, static_cast(e->getMouseButton()), mods); } void Box2DTestBed::onMouseUp(Event* event) { - button[(int)EventMouse::MouseButton::BUTTON_LEFT] = false; - button[(int)EventMouse::MouseButton::BUTTON_RIGHT] = false; - button[(int)EventMouse::MouseButton::BUTTON_MIDDLE] = false; + const auto ratio = _debugDraw->getPTMRatio(); + _draging = false; + EventMouse* e = static_cast(event); + auto location = e->getLocation() - _debugDraw->getWorldOffset(); + b2Vec2 pos = {location.x / ratio, location.y / ratio}; + m_sample->MouseUp(pos, static_cast(e->getMouseButton())); } void Box2DTestBed::onMouseMove(Event* event) { - EventMouse* e = (EventMouse*)event; - auto pt = e->getLocation(); - pos = {pt.x / g_debugDraw.mRatio, pt.y / g_debugDraw.mRatio}; + const auto ratio = _debugDraw->getPTMRatio(); + EventMouse* e = static_cast(event); + + auto location = e->getLocation() - _debugDraw->getWorldOffset(); + b2Vec2 pos{location.x / ratio, location.y / ratio}; + m_sample->MouseMove(pos); - if (button[(int)EventMouse::MouseButton::BUTTON_RIGHT]) + if (e->getMouseButton() == EventMouse::MouseButton::BUTTON_RIGHT) { - (pos.x > oldPos.x) ? g_debugDraw.debugNodeOffset.x += 4 : g_debugDraw.debugNodeOffset.x -= 4; - (pos.y < oldPos.y) ? g_debugDraw.debugNodeOffset.y -= 2 : g_debugDraw.debugNodeOffset.y += 2; + auto diff = b2Sub(pos, _mouseDownPos); + _debugDraw->setPosition(_dragingStartPos.x + diff.x, _dragingStartPos.y + diff.y); } - oldPos = pos; } void Box2DTestBed::onMouseScroll(Event* event) { EventMouse* e = (EventMouse*)event; - g_debugDraw.mRatio += e->getScrollY(); + _debugDraw->setPTMRatio(_debugDraw->getPTMRatio() - e->getScrollY()); } void Box2DTestBed::onEnter() { Scene::onEnter(); ImGuiPresenter::getInstance()->addFont(FileUtils::getInstance()->fullPathForFilename("fonts/arial.ttf")); - ImGuiPresenter::getInstance()->addRenderLoop("#im01", AX_CALLBACK_0(Box2DTestBed::onDrawImGui, this), this); + ImGuiPresenter::getInstance()->addRenderLoop("#bv3t", AX_CALLBACK_0(Box2DTestBed::onDrawImGui, this), this); } void Box2DTestBed::onExit() { + ImGuiPresenter::getInstance()->removeRenderLoop("#bv3t"); Scene::onExit(); - ImGuiPresenter::getInstance()->removeRenderLoop("#im01"); } void Box2DTestBed::update(float dt) { // Debug draw - m_test->debugString = ""; - labelDebugDraw->setString(""); - debugDrawNode->clear(); - m_test->Step(settings); - m_test->m_world->DebugDraw(); + _debugDraw->clear(); + m_sample->Step(s_settings); } void Box2DTestBed::initPhysics() { - uint32 flags = 0; - flags += 1 * b2Draw::e_shapeBit; - flags += 1 * b2Draw::e_jointBit; - flags += 0 * b2Draw::e_aabbBit; - flags += 0 * b2Draw::e_centerOfMassBit; - g_debugDraw.SetFlags(flags); - g_debugDraw.mRatio = 8; - m_test->m_world->SetDebugDraw(&g_debugDraw); - m_test->g_debugDraw = g_debugDraw; - g_debugDraw.debugNodeOffset = {250, 70}; - m_test->g_debugDraw.debugNodeOffset = g_debugDraw.debugNodeOffset; - - settings.m_hertz = 60; + _debugDraw = + utils::createInstance(&Box2DTestDebugDrawNode::initWithWorld, m_sample->m_worldId); + _debugDraw->setAutoDraw(false); + addChild(_debugDraw); + + auto& b2dw = _debugDraw->getB2DebugDraw(); + b2dw.drawShapes = true; + b2dw.drawJoints = true; + b2dw.drawAABBs = false; + + _debugDraw->setWorldOffset({250, 70}); + _debugDraw->setPTMRatio(3.0f); + + s_settings.hertz = 60; +} + +void Box2DTestBed::RestartSample() +{ + getTestSuite()->restartCurrTest(); } void Box2DTestBed::onDrawImGui() { - m_test->UpdateUI(); + int maxWorkers = enki::GetNumHardwareThreads(); + + float menuWidth = 180.0f; + auto cursorPos = ImGui::GetCursorScreenPos(); + ImVec2 toolWindowPos = {cursorPos.x + g_camera.m_width - menuWidth - 80, cursorPos.y - 80}; + ImVec2 toolWindowSize = {menuWidth, g_camera.m_height - 200.0f}; + if (g_draw.m_showUI) + { + ImGui::SetNextWindowPos(toolWindowPos, ImGuiCond_FirstUseEver); + ImGui::SetNextWindowSize(toolWindowSize, ImGuiCond_FirstUseEver); + ImGui::Begin("Tools", &g_draw.m_showUI, + /*ImGuiWindowFlags_NoMove | */ImGuiWindowFlags_NoResize | ImGuiWindowFlags_NoCollapse); + + if (ImGui::BeginTabBar("ControlTabs", ImGuiTabBarFlags_None)) + { + if (ImGui::BeginTabItem("Controls")) + { + ImGui::PushItemWidth(100.0f); + ImGui::SliderInt("Sub-steps", &s_settings.subStepCount, 1, 50); + ImGui::SliderFloat("Hertz", &s_settings.hertz, 5.0f, 120.0f, "%.0f hz"); + + if (ImGui::SliderInt("Workers", &s_settings.workerCount, 1, maxWorkers)) + { + s_settings.workerCount = b2ClampInt(s_settings.workerCount, 1, maxWorkers); + RestartSample(); + } + ImGui::PopItemWidth(); + + ImGui::Separator(); + + ImGui::Checkbox("Sleep", &s_settings.enableSleep); + ImGui::Checkbox("Warm Starting", &s_settings.enableWarmStarting); + ImGui::Checkbox("Continuous", &s_settings.enableContinuous); + + ImGui::Separator(); + + ImGui::Checkbox("Shapes", &s_settings.drawShapes); + ImGui::Checkbox("Joints", &s_settings.drawJoints); + ImGui::Checkbox("Joint Extras", &s_settings.drawJointExtras); + ImGui::Checkbox("AABBs", &s_settings.drawAABBs); + ImGui::Checkbox("Contact Points", &s_settings.drawContactPoints); + ImGui::Checkbox("Contact Normals", &s_settings.drawContactNormals); + ImGui::Checkbox("Contact Impulses", &s_settings.drawContactImpulses); + ImGui::Checkbox("Friction Impulses", &s_settings.drawFrictionImpulses); + ImGui::Checkbox("Center of Masses", &s_settings.drawMass); + ImGui::Checkbox("Graph Colors", &s_settings.drawGraphColors); + ImGui::Checkbox("Counters", &s_settings.drawCounters); + ImGui::Checkbox("Profile", &s_settings.drawProfile); + + ImVec2 button_sz = ImVec2(-1, 0); + if (ImGui::Button("Pause (P)", button_sz)) + { + s_settings.pause = !s_settings.pause; + } + + if (ImGui::Button("Single Step (O)", button_sz)) + { + s_settings.singleStep = !s_settings.singleStep; + } + + if (ImGui::Button("Dump Mem Stats", button_sz)) + { + b2World_DumpMemoryStats(m_sample->m_worldId); + } + + if (ImGui::Button("Reset Profile", button_sz)) + { + m_sample->ResetProfile(); + } + + if (ImGui::Button("Restart (R)", button_sz)) + { + RestartSample(); + } + + if (ImGui::Button("Quit", button_sz)) + { + glfwSetWindowShouldClose(g_mainWindow, GL_TRUE); + } + + ImGui::EndTabItem(); + } + + ImGuiTreeNodeFlags leafNodeFlags = ImGuiTreeNodeFlags_OpenOnArrow | ImGuiTreeNodeFlags_OpenOnDoubleClick; + leafNodeFlags |= ImGuiTreeNodeFlags_Leaf | ImGuiTreeNodeFlags_NoTreePushOnOpen; + + ImGuiTreeNodeFlags nodeFlags = ImGuiTreeNodeFlags_OpenOnArrow | ImGuiTreeNodeFlags_OpenOnDoubleClick; + + if (ImGui::BeginTabItem("Tests")) + { + int categoryIndex = 0; + const char* category = g_sampleEntries[categoryIndex].category; + int i = 0; + while (i < g_sampleCount) + { + bool categorySelected = strcmp(category, g_sampleEntries[s_settings.sampleIndex].category) == 0; + ImGuiTreeNodeFlags nodeSelectionFlags = categorySelected ? ImGuiTreeNodeFlags_Selected : 0; + bool nodeOpen = ImGui::TreeNodeEx(category, nodeFlags | nodeSelectionFlags); + + if (nodeOpen) + { + while (i < g_sampleCount && strcmp(category, g_sampleEntries[i].category) == 0) + { + ImGuiTreeNodeFlags selectionFlags = 0; + if (s_settings.sampleIndex == i) + { + selectionFlags = ImGuiTreeNodeFlags_Selected; + } + ImGui::TreeNodeEx((void*)(intptr_t)i, leafNodeFlags | selectionFlags, "%s", + g_sampleEntries[i].name); + if (ImGui::IsItemClicked()) + { + getTestSuite()->enterTest(i); + } + ++i; + } + ImGui::TreePop(); + } + else + { + while (i < g_sampleCount && strcmp(category, g_sampleEntries[i].category) == 0) + { + ++i; + } + } + + if (i < g_sampleCount) + { + category = g_sampleEntries[i].category; + categoryIndex = i; + } + } + ImGui::EndTabItem(); + } + ImGui::EndTabBar(); + } + + ImGui::End(); + + m_sample->UpdateUI(); + } + + // if (g_draw.m_showUI) + { + char buffer[128]; + snprintf(buffer, 128, "%.1f ms - step %d - camera (%g, %g, %g)", 1000.0f * _director->getDeltaTime(), + m_sample->m_stepCount, g_camera.m_center.x, g_camera.m_center.y, g_camera.m_zoom); + // snprintf( buffer, 128, "%.1f ms", 1000.0f * frameTime ); + + ImGui::SetNextWindowPos(ImVec2{cursorPos.x + 92, cursorPos.y + g_camera.m_height - 235}, + ImGuiCond_FirstUseEver); + ImGui::Begin("Overlay", nullptr, + ImGuiWindowFlags_NoTitleBar | ImGuiWindowFlags_NoInputs | ImGuiWindowFlags_AlwaysAutoResize | + ImGuiWindowFlags_NoScrollbar); + // ImGui::SetCursorPos(ImVec2(5.0f, g_camera.m_height - 20.0f)); + ImGui::TextColored(ImColor(153, 230, 153, 255), "%s", buffer); + ImGui::End(); + } } diff --git a/tests/cpp-tests/Source/Box2DTestBed/Box2DTestBed.h b/tests/cpp-tests/Source/Box2DTestBed/Box2DTestBed.h index 027a1e887252..2e3f5171fc7a 100644 --- a/tests/cpp-tests/Source/Box2DTestBed/Box2DTestBed.h +++ b/tests/cpp-tests/Source/Box2DTestBed/Box2DTestBed.h @@ -28,28 +28,17 @@ #include "axmol.h" #include "box2d/box2d.h" #include "../BaseTest.h" +#include "Box2DTestDebugDrawNode.h" -DEFINE_TEST_SUITE(Box2DTestBedTests); - -class Test; -typedef Test* TestCreateFcn(); - -struct TestEntry -{ - const char* category; - const char* name; - TestCreateFcn* createFcn; -}; +class SampleEntry; +class Sample; -#define MAX_TESTS 256 -extern TestEntry g_testEntries[MAX_TESTS]; - -int RegisterTest(const char* category, const char* name, TestCreateFcn* fcn); +DEFINE_TEST_SUITE(Box2DTestBedTests); -class Box2DTestBed : public TestCase, ax::Layer +class Box2DTestBed : public TestCase { public: - static Box2DTestBed* createWithEntryID(int entryId); + static Box2DTestBed* create(int entryIndex); Box2DTestBed(); virtual ~Box2DTestBed(); @@ -62,13 +51,7 @@ class Box2DTestBed : public TestCase, ax::Layer void initPhysics(); void update(float dt) override; - void createResetButton(); - - bool initWithEntryID(int entryId); - - bool onTouchBegan(ax::Touch* touch, ax::Event* event); - void onTouchMoved(ax::Touch* touch, ax::Event* event); - void onTouchEnded(ax::Touch* touch, ax::Event* event); + bool initWithEntryIndex(int entryIndex); void onKeyPressed(ax::EventKeyboard::KeyCode code, ax::Event* event); void onKeyReleased(ax::EventKeyboard::KeyCode code, ax::Event* event); @@ -78,24 +61,23 @@ class Box2DTestBed : public TestCase, ax::Layer void onMouseMove(ax::Event* event); void onMouseScroll(ax::Event* event); - ax::EventListenerTouchOneByOne* _touchListener; - ax::EventListenerKeyboard* _keyboardListener; + void RestartSample(); - TestEntry* m_entry; - Test* m_test; - int m_entryID; + SampleEntry* m_entry{}; + Sample* m_sample{}; + int m_entryIndex{}; private: - b2World* world; - ax::Texture2D* _spriteTexture; + b2Vec2 _mouseDownPos{}; + ax::Vec2 _dragingStartPos; + bool _draging{false}; - b2Vec2 pos; - b2Vec2 oldPos; - bool button[2]; + /*ax::EventListenerTouchOneByOne* _touchListener{};*/ + ax::EventListenerKeyboard* _keyboardListener{}; + ax::EventListenerMouse* _mouseListener{}; // Debug stuff - ax::DrawNode* debugDrawNode; - ax::extension::PhysicsDebugNodeBox2D g_debugDraw; + Box2DTestDebugDrawNode* _debugDraw{}; }; #endif diff --git a/tests/cpp-tests/Source/Box2DTestBed/Box2DTestDebugDrawNode.cpp b/tests/cpp-tests/Source/Box2DTestBed/Box2DTestDebugDrawNode.cpp new file mode 100644 index 000000000000..ca92060babe5 --- /dev/null +++ b/tests/cpp-tests/Source/Box2DTestBed/Box2DTestDebugDrawNode.cpp @@ -0,0 +1,423 @@ +#include "Box2DTestDebugDrawNode.h" +#include "VisibleRect.h" + +using namespace ax; + +Box2DTestDebugDrawNode* g_pDebugDrawNode; +GLFWwindow* g_mainWindow; +b2SampleCamera g_camera; + +static Color4F toColor4F(b2HexColor color) +{ + unsigned int r = ((unsigned int)color >> 16) & 0xff; + unsigned int g = ((unsigned int)color >> 8) & 0xff; + unsigned int b = ((unsigned int)color) & 0xff; + return Color4F{r / 255.f, g / 255.f, b / 255.f, 1.0f}; +} + +static Vec2 toVec2(const b2Vec2& v) +{ + return Vec2{v.x, v.y}; +} + +static b2Vec2 tob2Vec2(const Vec2& v) +{ + return b2Vec2{v.x, v.y}; +} + +b2AABB b2SampleCamera::GetViewBounds() +{ + b2AABB bounds; + bounds.lowerBound = ConvertScreenToWorld({0.0f, (float)m_height}); + bounds.upperBound = ConvertScreenToWorld({(float)m_width, 0.0f}); + return bounds; +} + +b2Vec2 b2SampleCamera::ConvertScreenToWorld(b2Vec2 ps) +{ + float w = float(m_width); + float h = float(m_height); + float u = ps.x / w; + float v = (h - ps.y) / h; + + float ratio = w / h; + b2Vec2 extents = {m_zoom * ratio, m_zoom}; + + b2Vec2 lower = b2Sub(m_center, extents); + b2Vec2 upper = b2Add(m_center, extents); + + b2Vec2 pw = {(1.0f - u) * lower.x + u * upper.x, (1.0f - v) * lower.y + v * upper.y}; + return pw; +} + +static void b2DrawCircle(b2Vec2 center, float radius, b2HexColor color, Box2DTestDebugDrawNode* context) +{ + auto ratio = context->getPTMRatio(); + auto offset = context->getWorldOffset(); + context->AddCircle(CircleData{b2Vec2{center.x * ratio + offset.x, center.y * ratio + offset.y}, radius * ratio, toColor4F(color)}); +} + +static void b2DrawSolidCircle(b2Transform t, float radius, b2HexColor color, Box2DTestDebugDrawNode* context) +{ + // RGBA8 rgba = MakeRGBA8(color, 1.0f); + // m_circles.push_back({{transform.p.x, transform.p.y, transform.q.c, transform.q.s}, radius, rgba}); + auto ratio = context->getPTMRatio(); + auto offset = context->getWorldOffset(); + context->AddCircle( + {{t.p.x * ratio + offset.x, t.p.y * ratio + offset.y, t.q.c, t.q.s}, radius * ratio, toColor4F(color)}); +} + +static void b2DrawSolidCapsule(b2Vec2 pt1, b2Vec2 pt2, float radius, b2HexColor c, Box2DTestDebugDrawNode* context) +{ + auto ratio = context->getPTMRatio(); + auto offset = context->getWorldOffset(); + Vec2 p1{pt1.x * ratio, pt1.y * ratio}, p2{pt2.x * ratio, pt2.y * ratio}; + + Vec2 d = (p2 - p1); + float length = d.length(); + if (length < 0.001f) + { + printf("WARNING: sample app: capsule too short!\n"); + return; + } + + b2Vec2 axis = {d.x / length, d.y / length}; + b2Transform transform; + transform.p = tob2Vec2(0.5f * (p1 + p2)); + transform.q.c = axis.x; + transform.q.s = axis.y; + + ax::Color4F rgba = toColor4F(c); + + context->AddCapsule({{transform.p.x + offset.x, transform.p.y + offset.y, transform.q.c, transform.q.s}, + radius * ratio, + length, + rgba}); +} + +bool Box2DTestDebugDrawNode::initWithWorld(b2WorldId worldId) +{ + g_pDebugDrawNode = this; + g_mainWindow = static_cast(_director->getGLView())->getWindow(); + + bool ret = ax::extension::PhysicsDebugNode::initWithWorld(worldId); + +#define __b2_setfun(f) _debugDraw.f = reinterpret_cast(b2##f); + __b2_setfun(DrawCircle); + __b2_setfun(DrawSolidCircle); + __b2_setfun(DrawSolidCapsule); +#undef __b2_setfun + + // Demo messageString + _textRender = Label::createWithTTF("TEST", "fonts/arial.ttf", 8.0f); + _textRender->setAnchorPoint(Vec2(0, 1)); + _textRender->setPosition(VisibleRect::left().x, VisibleRect::top().y - 10); + _textRender->setColor(Color3B::WHITE); + this->addChild(_textRender, 99); + + /// circle shader + { + auto& cmd = _customCommandCircle; + auto& pipelinePS = cmd.getPipelineDescriptor().programState; + AX_SAFE_RELEASE(pipelinePS); + + // vertex attributes + auto program = + backend::ProgramManager::getInstance()->loadProgram("custom/circle_vs", "custom/circle_fs"); + pipelinePS = new backend::ProgramState(program); + auto vfmt = pipelinePS->getMutableVertexLayout(); + vfmt->setAttrib("a_localPosition", program->getAttributeLocation("a_localPosition"), + backend::VertexFormat::FLOAT2, 0, false); + vfmt->setStride(sizeof(Vec2)); + cmd.createVertexBuffer(sizeof(Vec2), 6, CustomCommand::BufferUsage::STATIC); + float a = 1.1f; + b2Vec2 vertices[] = {{-a, -a}, {a, -a}, {-a, a}, {a, -a}, {a, a}, {-a, a}}; + cmd.updateVertexBuffer(vertices, sizeof(vertices)); + cmd.setVertexDrawInfo(0, 6); + + // instanced attributes + auto vfmtInstanced = pipelinePS->getMutableVertexLayout(true); + vfmtInstanced->setAttrib("a_instanceTransform", program->getAttributeLocation("a_instancePosition"), + backend::VertexFormat::FLOAT2, offsetof(CircleData, position), false); + vfmtInstanced->setAttrib("a_instanceRadius", program->getAttributeLocation("a_instanceRadius"), + backend::VertexFormat::FLOAT, offsetof(CircleData, radius), false); + vfmtInstanced->setAttrib("a_instanceColor", program->getAttributeLocation("a_instanceColor"), + backend::VertexFormat::FLOAT4, offsetof(CircleData, rgba), false); + vfmtInstanced->setStride(sizeof(CircleData)); + + cmd.setPrimitiveType(CustomCommand::PrimitiveType::TRIANGLE); + cmd.setDrawType(CustomCommand::DrawType::ARRAY_INSTANCED); + } + + /// solid circle shader + { + auto& cmd = _customCommandSolidCircle; + auto& pipelinePS = cmd.getPipelineDescriptor().programState; + AX_SAFE_RELEASE(pipelinePS); + + // vertex attributes + auto program = + backend::ProgramManager::getInstance()->loadProgram("custom/solid_circle_vs", "custom/solid_circle_fs"); + pipelinePS = new backend::ProgramState(program); + auto vfmt = pipelinePS->getMutableVertexLayout(); + vfmt->setAttrib("a_localPosition", program->getAttributeLocation("a_localPosition"), + backend::VertexFormat::FLOAT2, 0, false); + vfmt->setStride(sizeof(Vec2)); + cmd.createVertexBuffer(sizeof(Vec2), 6, CustomCommand::BufferUsage::STATIC); + float a = 1.1f; + b2Vec2 vertices[] = {{-a, -a}, {a, -a}, {-a, a}, {a, -a}, {a, a}, {-a, a}}; + cmd.updateVertexBuffer(vertices, sizeof(vertices)); + cmd.setVertexDrawInfo(0, 6); + + // instanced attributes + auto vfmtInstanced = pipelinePS->getMutableVertexLayout(true); + vfmtInstanced->setAttrib("a_instanceTransform", program->getAttributeLocation("a_instanceTransform"), + backend::VertexFormat::FLOAT4, offsetof(SolidCircleData, transform), false); + vfmtInstanced->setAttrib("a_instanceRadius", program->getAttributeLocation("a_instanceRadius"), + backend::VertexFormat::FLOAT, offsetof(SolidCircleData, radius), false); + vfmtInstanced->setAttrib("a_instanceColor", program->getAttributeLocation("a_instanceColor"), + backend::VertexFormat::FLOAT4, offsetof(SolidCircleData, rgba), false); + vfmtInstanced->setStride(sizeof(SolidCircleData)); + + cmd.setPrimitiveType(CustomCommand::PrimitiveType::TRIANGLE); + cmd.setDrawType(CustomCommand::DrawType::ARRAY_INSTANCED); + } + + /// solid capsule shader + { + auto& cmd = _customCommandCapsule; + auto& pipelinePS = cmd.getPipelineDescriptor().programState; + AX_SAFE_RELEASE(pipelinePS); + + // vertex attributes + auto program = + backend::ProgramManager::getInstance()->loadProgram("custom/solid_capsule_vs", "custom/solid_capsule_fs"); + pipelinePS = new backend::ProgramState(program); + auto vfmt = pipelinePS->getMutableVertexLayout(); + vfmt->setAttrib("a_localPosition", program->getAttributeLocation("a_localPosition"), + backend::VertexFormat::FLOAT2, 0, false); + vfmt->setStride(sizeof(Vec2)); + cmd.createVertexBuffer(sizeof(Vec2), 6, CustomCommand::BufferUsage::STATIC); + float a = 1.1f; + b2Vec2 vertices[] = {{-a, -a}, {a, -a}, {-a, a}, {a, -a}, {a, a}, {-a, a}}; + cmd.updateVertexBuffer(vertices, sizeof(vertices)); + cmd.setVertexDrawInfo(0, 6); + + // instanced attributes + auto vfmtInstanced = pipelinePS->getMutableVertexLayout(true); + vfmtInstanced->setAttrib("a_instanceTransform", program->getAttributeLocation("a_instanceTransform"), + backend::VertexFormat::FLOAT4, offsetof(CapsuleData, transform), false); + vfmtInstanced->setAttrib("a_instanceRadius", program->getAttributeLocation("a_instanceRadius"), + backend::VertexFormat::FLOAT, offsetof(CapsuleData, radius), false); + vfmtInstanced->setAttrib("a_instanceLength", program->getAttributeLocation("a_instanceLength"), + backend::VertexFormat::FLOAT, offsetof(CapsuleData, length), false); + vfmtInstanced->setAttrib("a_instanceColor", program->getAttributeLocation("a_instanceColor"), + backend::VertexFormat::FLOAT4, offsetof(CapsuleData, rgba), false); + vfmtInstanced->setStride(sizeof(CapsuleData)); + + cmd.setPrimitiveType(CustomCommand::PrimitiveType::TRIANGLE); + cmd.setDrawType(CustomCommand::DrawType::ARRAY_INSTANCED); + } + + return ret; +} + +void Box2DTestDebugDrawNode::DrawPolygon(const b2Vec2* vertices, int32_t vertexCount, b2HexColor color) +{ + _debugDraw.DrawPolygon(vertices, vertexCount, color, this); +} + +void Box2DTestDebugDrawNode::DrawSolidPolygon(b2Transform transform, + const b2Vec2* vertices, + int32_t vertexCount, + float radius, + b2HexColor color) +{ + _debugDraw.DrawSolidPolygon(transform, vertices, vertexCount, radius, color, this); +} + +void Box2DTestDebugDrawNode::DrawCircle(b2Vec2 center, float radius, b2HexColor color) +{ + _debugDraw.DrawCircle(center, radius, color, this); +} +void Box2DTestDebugDrawNode::DrawSolidCircle(b2Transform transform, b2Vec2 center, float radius, b2HexColor color) +{ + _debugDraw.DrawSolidCircle(transform, radius, color, this); +} + +void Box2DTestDebugDrawNode::DrawSolidCapsule(b2Vec2 p1, b2Vec2 p2, float radius, b2HexColor color) +{ + _debugDraw.DrawSolidCapsule(p1, p2, radius, color, this); +} + +void Box2DTestDebugDrawNode::DrawSegment(b2Vec2 p1, b2Vec2 p2, b2HexColor color) +{ + _debugDraw.DrawSegment(p1, p2, color, this); +} + +void Box2DTestDebugDrawNode::DrawTransform(b2Transform transform) +{ + _debugDraw.DrawTransform(transform, this); +} + +void Box2DTestDebugDrawNode::DrawPoint(b2Vec2 p, float size, b2HexColor color) +{ + _debugDraw.DrawPoint(p, size, color, this); +} + +void Box2DTestDebugDrawNode::DrawString(int x, int y, const char* pszFormat, ...) +{ + va_list args; + va_start(args, pszFormat); + auto ret = StringUtils::vformat(pszFormat, args); + va_end(args); + + _debugString.append(ret); + _debugString.push_back('\n'); + _textRender->setString(_debugString); +} + +void Box2DTestDebugDrawNode::DrawString(b2Vec2 p, const char* pszFormat, ...) +{ + va_list args; + va_start(args, pszFormat); + auto ret = StringUtils::vformat(pszFormat, args); + va_end(args); + + _debugString.append(ret); + _debugString.push_back('\n'); + _textRender->setString(_debugString); +} + +void Box2DTestDebugDrawNode::AddCapsule(const CapsuleData& capsule) +{ + _capsules.push_back(capsule); + _capsulesDirty = true; +} + +void Box2DTestDebugDrawNode::AddCircle(const CircleData& circle) +{ + _circles.push_back(circle); + _circlesDirty = true; +} + +void Box2DTestDebugDrawNode::AddCircle(const SolidCircleData& circle) +{ + _solidCircles.push_back(circle); + _solidCirclesDirty = true; +} + +void Box2DTestDebugDrawNode::draw(Renderer* renderer, const Mat4& transform, uint32_t flags) +{ + PhysicsDebugNode::draw(renderer, transform, flags); + + /// circle + + // update buffer + if (_circlesDirty) + { + _circlesDirty = false; + if (_circles.empty()) + _customCommandCircle.setInstanceBuffer(nullptr, 0); + else + { + _customCommandCircle.createInstanceBuffer(sizeof(CircleData), _circles.size(), + CustomCommand::BufferUsage::DYNAMIC); + auto instanceBuffer = _customCommandCircle.getInstanceBuffer(); + instanceBuffer->updateData(_circles.data(), _circles.size() * sizeof(CircleData)); + } + } + + // submit draw command + if (_customCommandCircle.getInstanceCount() > 0) + submitDrawCommand(renderer, _customCommandCircle, transform); + + /// solid circle + + // update buffer + if (_solidCirclesDirty) + { + _solidCirclesDirty = false; + if (_solidCircles.empty()) + _customCommandSolidCircle.setInstanceBuffer(nullptr, 0); + else + { + _customCommandSolidCircle.createInstanceBuffer(sizeof(SolidCircleData), _solidCircles.size(), + CustomCommand::BufferUsage::DYNAMIC); + auto instanceBuffer = _customCommandSolidCircle.getInstanceBuffer(); + instanceBuffer->updateData(_solidCircles.data(), _solidCircles.size() * sizeof(SolidCircleData)); + } + } + + // submit draw command + if (_customCommandSolidCircle.getInstanceCount() > 0) + submitDrawCommand(renderer, _customCommandSolidCircle, transform); + + /// capsule + + // update buffer + if (_capsulesDirty) + { + _capsulesDirty = false; + if (_capsules.empty()) + _customCommandCapsule.setInstanceBuffer(nullptr, 0); + else + { + _customCommandCapsule.createInstanceBuffer(sizeof(CapsuleData), _capsules.size(), + CustomCommand::BufferUsage::DYNAMIC); + auto instanceBuffer = _customCommandCapsule.getInstanceBuffer(); + instanceBuffer->updateData(_capsules.data(), _capsules.size() * sizeof(CapsuleData)); + } + } + + // submit draw command + if (_customCommandCapsule.getInstanceCount() > 0) + submitDrawCommand(renderer, _customCommandCapsule, transform); +} + +void Box2DTestDebugDrawNode::submitDrawCommand(Renderer* renderer, CustomCommand& cmd, const Mat4& transform) +{ + backend::BlendDescriptor& blendDescriptor = cmd.getPipelineDescriptor().blendDescriptor; + blendDescriptor.blendEnabled = true; + blendDescriptor.sourceRGBBlendFactor = backend::BlendFactor::SRC_ALPHA; + blendDescriptor.destinationRGBBlendFactor = backend::BlendFactor::ONE_MINUS_SRC_ALPHA; + blendDescriptor.sourceAlphaBlendFactor = backend::BlendFactor::SRC_ALPHA; + blendDescriptor.destinationAlphaBlendFactor = backend::BlendFactor::ONE_MINUS_SRC_ALPHA; + + auto& pipelineDescriptor = cmd.getPipelineDescriptor(); + const auto& matrixP = _director->getMatrix(MATRIX_STACK_TYPE::MATRIX_STACK_PROJECTION); + Mat4 matrixMVP = matrixP * transform; + auto mvpLocation = pipelineDescriptor.programState->getUniformLocation("u_MVPMatrix"); + pipelineDescriptor.programState->setUniform(mvpLocation, matrixMVP.m, sizeof(matrixMVP.m)); + + auto viewHeight = _director->getGLView()->getDesignResolutionSize().height; + constexpr float zoom = 25.0 * 2.35f; + float pixelScale = viewHeight / zoom; + auto uniformLocation = pipelineDescriptor.programState->getUniformLocation("u_pixelScale"); + pipelineDescriptor.programState->setUniform(uniformLocation, &pixelScale, sizeof(pixelScale)); + + cmd.init(_globalZOrder); + renderer->addCommand(&cmd); +} + +void Box2DTestDebugDrawNode::clear() +{ + ax::extension::PhysicsDebugNode::clear(); + + _circles.clear(); + _circlesDirty = true; + + _solidCircles.clear(); + _solidCirclesDirty = true; + + _capsules.clear(); + _capsulesDirty = true; + + _debugString.clear(); +} + +void Box2DTestDebugDrawNode::DrawAABB(b2AABB aabb, b2HexColor color) +{ + this->drawRect(Vec2{aabb.lowerBound.x, aabb.lowerBound.y}, Vec2{aabb.upperBound.x, aabb.upperBound.y}, + toColor4F(color)); +} diff --git a/tests/cpp-tests/Source/Box2DTestBed/Box2DTestDebugDrawNode.h b/tests/cpp-tests/Source/Box2DTestBed/Box2DTestDebugDrawNode.h new file mode 100644 index 000000000000..b07b8cf9cd3b --- /dev/null +++ b/tests/cpp-tests/Source/Box2DTestBed/Box2DTestDebugDrawNode.h @@ -0,0 +1,105 @@ +#pragma once + +#include "physics-nodes/PhysicsDebugNode.h" + +using namespace ax; + +struct CircleData +{ + b2Vec2 position; + float radius; + ax::Color4F rgba; +}; + +struct SolidCircleData +{ + b2Transform transform; + float radius; + ax::Color4F rgba; +}; + +struct CapsuleData +{ + b2Transform transform; + float radius; + float length; + ax::Color4F rgba; +}; + +class Box2DTestDebugDrawNode : public ax::extension::PhysicsDebugNode +{ +public: + Box2DTestDebugDrawNode() : m_debugDraw(_debugDraw) {} + ~Box2DTestDebugDrawNode() + { + + } + bool initWithWorld(b2WorldId worldId) override; + + void DrawPolygon(const b2Vec2* vertices, int32_t vertexCount, b2HexColor color); + void DrawSolidPolygon(b2Transform transform, + const b2Vec2* vertices, + int32_t vertexCount, + float radius, + b2HexColor color); + + void DrawCircle(b2Vec2 center, float radius, b2HexColor color); + void DrawSolidCircle(b2Transform transform, b2Vec2 center, float radius, b2HexColor color); + + void DrawSolidCapsule(b2Vec2 p1, b2Vec2 p2, float radius, b2HexColor color); + + void DrawSegment(b2Vec2 p1, b2Vec2 p2, b2HexColor color); + + void DrawTransform(b2Transform transform); + + void DrawPoint(b2Vec2 p, float size, b2HexColor color); + + void DrawString(int x, int y, const char* string, ...); + + void DrawString(b2Vec2 p, const char* string, ...); + + void DrawAABB(b2AABB aabb, b2HexColor color); + + void AddCircle(const CircleData& circle); + void AddCircle(const SolidCircleData& circle); + void AddCapsule(const CapsuleData& capsule); + + void submitDrawCommand(Renderer* renderer, CustomCommand& cmd, const Mat4& transform); + + void draw(Renderer* renderer, const Mat4& transform, uint32_t flags); + void clear() override; + + b2DebugDraw& m_debugDraw; + bool m_showUI{true}; + + ax::Label* _textRender{nullptr}; + std::string _debugString; + + CustomCommand _customCommandCircle; + CustomCommand _customCommandSolidCircle; + CustomCommand _customCommandCapsule; + axstd::pod_vector _circles; + axstd::pod_vector _solidCircles; + axstd::pod_vector _capsules; + bool _circlesDirty{true}; + bool _solidCirclesDirty{true}; + bool _capsulesDirty{true}; +}; + +extern Box2DTestDebugDrawNode* g_pDebugDrawNode; +#define g_draw (*g_pDebugDrawNode) + +extern GLFWwindow* g_mainWindow; + +struct b2SampleCamera +{ + b2Vec2 ConvertScreenToWorld(b2Vec2 ps); + b2AABB GetViewBounds(); + b2Vec2 m_center{0.0f, 0.0f}; + float m_zoom{1.0f}; + float m_width{480}; + float m_height{320}; +}; + +extern b2SampleCamera g_camera; + diff --git a/tests/cpp-tests/Source/Box2DTestBed/samples/CMakeLists.txt b/tests/cpp-tests/Source/Box2DTestBed/samples/CMakeLists.txt new file mode 100644 index 000000000000..0114c790edb1 --- /dev/null +++ b/tests/cpp-tests/Source/Box2DTestBed/samples/CMakeLists.txt @@ -0,0 +1,111 @@ +# Box2D samples app + +# glad for OpenGL API +set(GLAD_DIR ${CMAKE_SOURCE_DIR}/extern/glad) + +add_library( + glad STATIC + ${GLAD_DIR}/src/glad.c + ${GLAD_DIR}/include/glad/glad.h + ${GLAD_DIR}/include/KHR/khrplatform.h +) +target_include_directories(glad PUBLIC ${GLAD_DIR}/include) + +# glfw for windowing and input +set(GLFW_BUILD_DOCS OFF CACHE BOOL "GLFW Docs") +set(GLFW_BUILD_EXAMPLES OFF CACHE BOOL "GLFW Examples") +set(GLFW_BUILD_TESTS OFF CACHE BOOL "GLFW Tests") +set(GLFW_INSTALL OFF CACHE BOOL "GLFW Install") + +FetchContent_Declare( + glfw + GIT_REPOSITORY https://github.com/glfw/glfw.git + GIT_TAG master + GIT_SHALLOW TRUE + GIT_PROGRESS TRUE +) +FetchContent_MakeAvailable(glfw) + +# imgui and glfw backend for GUI +# https://gist.github.com/jeffamstutz/992723dfabac4e3ffff265eb71a24cd9 +FetchContent_Populate(imgui + URL https://github.com/ocornut/imgui/archive/docking.zip + SOURCE_DIR ${CMAKE_SOURCE_DIR}/build/imgui +) + +set(IMGUI_DIR ${CMAKE_SOURCE_DIR}/build/imgui) + +add_library(imgui STATIC + ${IMGUI_DIR}/imgui.cpp + ${IMGUI_DIR}/imgui_draw.cpp + ${IMGUI_DIR}/imgui_demo.cpp + ${IMGUI_DIR}/imgui_tables.cpp + ${IMGUI_DIR}/imgui_widgets.cpp + + ${IMGUI_DIR}/backends/imgui_impl_glfw.cpp + ${IMGUI_DIR}/backends/imgui_impl_opengl3.cpp +) + +target_link_libraries(imgui PUBLIC glfw glad) +target_include_directories(imgui PUBLIC ${IMGUI_DIR} ${IMGUI_DIR}/backends) +target_compile_definitions(imgui PUBLIC IMGUI_DISABLE_OBSOLETE_FUNCTIONS) + +# jsmn for json +set(JSMN_DIR ${CMAKE_SOURCE_DIR}/extern/jsmn) + +add_library(jsmn INTERFACE ${JSMN_DIR}/jsmn.h) +target_include_directories(jsmn INTERFACE ${JSMN_DIR}) + +add_executable(samples + car.cpp + car.h + donut.cpp + donut.h + doohickey.cpp + doohickey.h + draw.cpp + draw.h + human.cpp + human.h + main.cpp + sample.cpp + sample.h + sample_benchmark.cpp + sample_bodies.cpp + sample_collision.cpp + sample_continuous.cpp + sample_determinism.cpp + sample_events.cpp + sample_geometry.cpp + sample_joints.cpp + sample_robustness.cpp + sample_shapes.cpp + sample_stacking.cpp + sample_world.cpp + settings.cpp + settings.h + shader.cpp + shader.h +) + +set_target_properties(samples PROPERTIES + CXX_STANDARD 17 + CXX_STANDARD_REQUIRED YES + CXX_EXTENSIONS NO +) + +target_include_directories(samples PRIVATE ${CMAKE_CURRENT_SOURCE_DIR} ${JSMN_DIR}) +target_link_libraries(samples PUBLIC box2d imgui glfw glad enkiTS) + +# target_compile_definitions(samples PRIVATE "$<$:SAMPLES_DEBUG>") +# message(STATUS "runtime = ${CMAKE_RUNTIME_OUTPUT_DIRECTORY}") +# message(STATUS "binary = ${CMAKE_CURRENT_BINARY_DIR}") + +# Copy font files, etc +add_custom_command( + TARGET samples POST_BUILD + COMMAND ${CMAKE_COMMAND} -E copy_directory + ${CMAKE_CURRENT_SOURCE_DIR}/data/ + ${CMAKE_CURRENT_BINARY_DIR}/data/) + +# source_group(TREE ${CMAKE_CURRENT_SOURCE_DIR} FILES ${BOX2D_SAMPLES}) diff --git a/tests/cpp-tests/Source/Box2DTestBed/samples/LockLessMultiReadPipe.h b/tests/cpp-tests/Source/Box2DTestBed/samples/LockLessMultiReadPipe.h new file mode 100644 index 000000000000..339c8bdd486c --- /dev/null +++ b/tests/cpp-tests/Source/Box2DTestBed/samples/LockLessMultiReadPipe.h @@ -0,0 +1,283 @@ +// Copyright (c) 2013 Doug Binks +// +// This software is provided 'as-is', without any express or implied +// warranty. In no event will the authors be held liable for any damages +// arising from the use of this software. +// +// Permission is granted to anyone to use this software for any purpose, +// including commercial applications, and to alter it and redistribute it +// freely, subject to the following restrictions: +// +// 1. The origin of this software must not be misrepresented; you must not +// claim that you wrote the original software. If you use this software +// in a product, an acknowledgement in the product documentation would be +// appreciated but is not required. +// 2. Altered source versions must be plainly marked as such, and must not be +// misrepresented as being the original software. +// 3. This notice may not be removed or altered from any source distribution. + +#pragma once + +#include +#include +#include + +#ifndef ENKI_ASSERT +#include +#define ENKI_ASSERT(x) assert(x) +#endif + +namespace enki +{ + // LockLessMultiReadPipe - Single writer, multiple reader thread safe pipe using (semi) lockless programming + // Readers can only read from the back of the pipe + // The single writer can write to the front of the pipe, and read from both ends (a writer can be a reader) + // for many of the principles used here, see http://msdn.microsoft.com/en-us/library/windows/desktop/ee418650(v=vs.85).aspx + // Note: using log2 sizes so we do not need to clamp (multi-operation) + // T is the contained type + // Note this is not true lockless as the use of flags as a form of lock state. + template class LockLessMultiReadPipe + { + public: + LockLessMultiReadPipe(); + ~LockLessMultiReadPipe() {} + + // ReaderTryReadBack returns false if we were unable to read + // This is thread safe for both multiple readers and the writer + bool ReaderTryReadBack( T* pOut ); + + // WriterTryReadFront returns false if we were unable to read + // This is thread safe for the single writer, but should not be called by readers + bool WriterTryReadFront( T* pOut ); + + // WriterTryWriteFront returns false if we were unable to write + // This is thread safe for the single writer, but should not be called by readers + bool WriterTryWriteFront( const T& in ); + + // IsPipeEmpty() is a utility function, not intended for general use + // Should only be used very prudently. + bool IsPipeEmpty() const + { + return 0 == m_WriteIndex.load( std::memory_order_relaxed ) - m_ReadCount.load( std::memory_order_relaxed ); + } + + void Clear() + { + m_WriteIndex = 0; + m_ReadIndex = 0; + m_ReadCount = 0; + memset( (void*)m_Flags, 0, sizeof( m_Flags ) ); + } + + private: + const static uint32_t ms_cSize = ( 1 << cSizeLog2 ); + const static uint32_t ms_cIndexMask = ms_cSize - 1; + const static uint32_t FLAG_INVALID = 0xFFFFFFFF; // 32bit for CAS + const static uint32_t FLAG_CAN_WRITE = 0x00000000; // 32bit for CAS + const static uint32_t FLAG_CAN_READ = 0x11111111; // 32bit for CAS + + T m_Buffer[ ms_cSize ]; + + // read and write indexes allow fast access to the pipe, but actual access + // controlled by the access flags. + std::atomic m_WriteIndex; + std::atomic m_ReadCount; + std::atomic m_Flags[ ms_cSize ]; + std::atomic m_ReadIndex; + }; + + template inline + LockLessMultiReadPipe::LockLessMultiReadPipe() + : m_WriteIndex(0) + , m_ReadCount(0) + , m_ReadIndex(0) + { + ENKI_ASSERT( cSizeLog2 < 32 ); + memset( (void*)m_Flags, 0, sizeof( m_Flags ) ); + } + + template inline + bool LockLessMultiReadPipe::ReaderTryReadBack( T* pOut ) + { + + uint32_t actualReadIndex; + uint32_t readCount = m_ReadCount.load( std::memory_order_relaxed ); + + // We get hold of read index for consistency + // and do first pass starting at read count + uint32_t readIndexToUse = readCount; + while(true) + { + + uint32_t writeIndex = m_WriteIndex.load( std::memory_order_relaxed ); + // power of two sizes ensures we can use a simple calc without modulus + uint32_t numInPipe = writeIndex - readCount; + if( 0 == numInPipe ) + { + return false; + } + if( readIndexToUse >= writeIndex ) + { + readIndexToUse = m_ReadIndex.load( std::memory_order_relaxed ); + } + + // power of two sizes ensures we can perform AND for a modulus + actualReadIndex = readIndexToUse & ms_cIndexMask; + + // Multiple potential readers mean we should check if the data is valid, + // using an atomic compare exchange + uint32_t previous = FLAG_CAN_READ; + bool bSuccess = m_Flags[ actualReadIndex ].compare_exchange_strong( previous, FLAG_INVALID, std::memory_order_acq_rel, std::memory_order_relaxed ); + if( bSuccess ) + { + break; + } + ++readIndexToUse; + + // Update read count + readCount = m_ReadCount.load( std::memory_order_relaxed ); + } + + // we update the read index using an atomic add, as we've only read one piece of data. + // this ensure consistency of the read index, and the above loop ensures readers + // only read from unread data + m_ReadCount.fetch_add(1, std::memory_order_relaxed ); + + // now read data, ensuring we do so after above reads & CAS + *pOut = m_Buffer[ actualReadIndex ]; + + m_Flags[ actualReadIndex ].store( FLAG_CAN_WRITE, std::memory_order_release ); + + return true; + } + + template inline + bool LockLessMultiReadPipe::WriterTryReadFront( T* pOut ) + { + uint32_t writeIndex = m_WriteIndex.load( std::memory_order_relaxed ); + uint32_t frontReadIndex = writeIndex; + + // Multiple potential readers mean we should check if the data is valid, + // using an atomic compare exchange - which acts as a form of lock (so not quite lockless really). + uint32_t actualReadIndex = 0; + while(true) + { + uint32_t readCount = m_ReadCount.load( std::memory_order_relaxed ); + // power of two sizes ensures we can use a simple calc without modulus + uint32_t numInPipe = writeIndex - readCount; + if( 0 == numInPipe ) + { + m_ReadIndex.store( readCount, std::memory_order_release ); + return false; + } + --frontReadIndex; + actualReadIndex = frontReadIndex & ms_cIndexMask; + uint32_t previous = FLAG_CAN_READ; + bool success = m_Flags[ actualReadIndex ].compare_exchange_strong( previous, FLAG_INVALID, std::memory_order_acq_rel, std::memory_order_relaxed ); + if( success ) + { + break; + } + else if( m_ReadIndex.load( std::memory_order_acquire ) >= frontReadIndex ) + { + return false; + } + } + + // now read data, ensuring we do so after above reads & CAS + *pOut = m_Buffer[ actualReadIndex ]; + + m_Flags[ actualReadIndex ].store( FLAG_CAN_WRITE, std::memory_order_relaxed ); + + m_WriteIndex.store(writeIndex-1, std::memory_order_relaxed); + return true; + } + + + template inline + bool LockLessMultiReadPipe::WriterTryWriteFront( const T& in ) + { + // The writer 'owns' the write index, and readers can only reduce + // the amount of data in the pipe. + // We get hold of both values for consistency and to reduce false sharing + // impacting more than one access + uint32_t writeIndex = m_WriteIndex; + + // power of two sizes ensures we can perform AND for a modulus + uint32_t actualWriteIndex = writeIndex & ms_cIndexMask; + + // a reader may still be reading this item, as there are multiple readers + if( m_Flags[ actualWriteIndex ].load(std::memory_order_acquire) != FLAG_CAN_WRITE ) + { + return false; // still being read, so have caught up with tail. + } + + // as we are the only writer we can update the data without atomics + // whilst the write index has not been updated + m_Buffer[ actualWriteIndex ] = in; + m_Flags[ actualWriteIndex ].store( FLAG_CAN_READ, std::memory_order_release ); + + m_WriteIndex.fetch_add(1, std::memory_order_relaxed); + return true; + } + + + // Lockless multiwriter intrusive list + // Type T must implement T* volatile pNext; + template class LocklessMultiWriteIntrusiveList + { + + std::atomic pHead; + T tail; + public: + LocklessMultiWriteIntrusiveList() : pHead( &tail ) + { + tail.pNext = NULL; + } + + bool IsListEmpty() const + { + return pHead == &tail; + } + + // Add - safe to perform from any thread + void WriterWriteFront( T* pNode_ ) + { + ENKI_ASSERT( pNode_ ); + pNode_->pNext = NULL; + T* pPrev = pHead.exchange( pNode_ ); + pPrev->pNext = pNode_; + } + + // Remove - only thread safe for owner + T* ReaderReadBack() + { + T* pTailPlus1 = tail.pNext; + if( pTailPlus1 ) + { + T* pTailPlus2 = pTailPlus1->pNext; + if( pTailPlus2 ) + { + //not head + tail.pNext = pTailPlus2; + } + else + { + tail.pNext = NULL; + T* pCompare = pTailPlus1; // we need preserve pTailPlus1 as compare will alter it on failure + // pTailPlus1 is the head, attempt swap with tail + if( !pHead.compare_exchange_strong( pCompare, &tail ) ) + { + // pCompare receives the revised pHead on failure. + // pTailPlus1 is no longer the head, so pTailPlus1->pNext should be non NULL + while( (T*)NULL == pTailPlus1->pNext ) {;} // wait for pNext to be updated as head may have just changed. + tail.pNext = pTailPlus1->pNext.load(); + pTailPlus1->pNext = NULL; + } + } + } + return pTailPlus1; + } + }; + +} diff --git a/tests/cpp-tests/Source/Box2DTestBed/samples/TaskScheduler.cpp b/tests/cpp-tests/Source/Box2DTestBed/samples/TaskScheduler.cpp new file mode 100644 index 000000000000..a0edaa951bc1 --- /dev/null +++ b/tests/cpp-tests/Source/Box2DTestBed/samples/TaskScheduler.cpp @@ -0,0 +1,1547 @@ +// Copyright (c) 2013 Doug Binks +// +// This software is provided 'as-is', without any express or implied +// warranty. In no event will the authors be held liable for any damages +// arising from the use of this software. +// +// Permission is granted to anyone to use this software for any purpose, +// including commercial applications, and to alter it and redistribute it +// freely, subject to the following restrictions: +// +// 1. The origin of this software must not be misrepresented; you must not +// claim that you wrote the original software. If you use this software +// in a product, an acknowledgement in the product documentation would be +// appreciated but is not required. +// 2. Altered source versions must be plainly marked as 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 "TaskScheduler.h" +#include "LockLessMultiReadPipe.h" + +#include + +#if defined __i386__ || defined __x86_64__ +#include "x86intrin.h" +#elif defined _WIN32 +#include +#endif + +using namespace enki; + +#if defined(ENKI_CUSTOM_ALLOC_FILE_AND_LINE) +#define ENKI_FILE_AND_LINE __FILE__, __LINE__ +#else +namespace +{ + const char* gc_File = ""; + const uint32_t gc_Line = 0; +} +#define ENKI_FILE_AND_LINE gc_File, gc_Line +#endif + +// UWP and MinGW don't have GetActiveProcessorCount +#if defined(_WIN64) \ + && !defined(__MINGW32__) \ + && !(defined(WINAPI_FAMILY) && (WINAPI_FAMILY == WINAPI_FAMILY_PC_APP || WINAPI_FAMILY == WINAPI_FAMILY_PHONE_APP)) +#define ENKI_USE_WINDOWS_PROCESSOR_API +#endif + +#ifdef ENKI_USE_WINDOWS_PROCESSOR_API +#ifndef WIN32_LEAN_AND_MEAN + #define WIN32_LEAN_AND_MEAN +#endif + +#ifndef NOMINMAX + #define NOMINMAX +#endif +#include +#endif + +uint32_t enki::GetNumHardwareThreads() +{ +#ifdef ENKI_USE_WINDOWS_PROCESSOR_API + return GetActiveProcessorCount(ALL_PROCESSOR_GROUPS); +#else + return std::thread::hardware_concurrency(); +#endif +} + +namespace enki +{ + static constexpr int32_t gc_TaskStartCount = 2; + static constexpr int32_t gc_TaskAlmostCompleteCount = 1; // GetIsComplete() will return false, but execution is done and about to complete + static constexpr uint32_t gc_PipeSizeLog2 = 8; + static constexpr uint32_t gc_SpinCount = 10; + static constexpr uint32_t gc_SpinBackOffMultiplier = 100; + static constexpr uint32_t gc_MaxNumInitialPartitions = 8; + static constexpr uint32_t gc_MaxStolenPartitions = 1 << gc_PipeSizeLog2; + static constexpr uint32_t gc_CacheLineSize = 64; + // awaiting std::hardware_constructive_interference_size +} + +// thread_local not well supported yet by some older C++11 compilers. +// For XCode before version 8 thread_local is not defined, so add to your compile defines: ENKI_THREAD_LOCAL __thread +#ifndef ENKI_THREAD_LOCAL +#if defined(_MSC_VER) && _MSC_VER <= 1800 + #define ENKI_THREAD_LOCAL __declspec(thread) +// Removed below as XCode supports thread_local since version 8 +// #elif __APPLE__ +// // Apple thread_local currently not implemented in XCode before version 8 despite it being in Clang. +// #define ENKI_THREAD_LOCAL __thread +#else + #define ENKI_THREAD_LOCAL thread_local +#endif +#endif + + +// each software thread gets its own copy of gtl_threadNum, so this is safe to use as a static variable +static ENKI_THREAD_LOCAL uint32_t gtl_threadNum = enki::NO_THREAD_NUM; + +namespace enki +{ + struct SubTaskSet + { + ITaskSet* pTask; + TaskSetPartition partition; + }; + + // we derive class TaskPipe rather than typedef to get forward declaration working easily + class TaskPipe : public LockLessMultiReadPipe {}; + + enum ThreadState : int32_t + { + ENKI_THREAD_STATE_NONE, // shouldn't get this value + ENKI_THREAD_STATE_NOT_LAUNCHED, // for debug purposes - indicates enki task thread not yet launched + ENKI_THREAD_STATE_RUNNING, + ENKI_THREAD_STATE_PRIMARY_REGISTERED, // primary thread is the one enkiTS was initialized on + ENKI_THREAD_STATE_EXTERNAL_REGISTERED, + ENKI_THREAD_STATE_EXTERNAL_UNREGISTERED, + ENKI_THREAD_STATE_WAIT_TASK_COMPLETION, + ENKI_THREAD_STATE_WAIT_NEW_TASKS, + ENKI_THREAD_STATE_WAIT_NEW_PINNED_TASKS, + ENKI_THREAD_STATE_STOPPED, + }; + + struct ThreadArgs + { + uint32_t threadNum; + TaskScheduler* pTaskScheduler; + }; + + struct alignas(enki::gc_CacheLineSize) ThreadDataStore + { + semaphoreid_t* pWaitNewPinnedTaskSemaphore = nullptr; + std::atomic threadState = { ENKI_THREAD_STATE_NONE }; + uint32_t rndSeed = 0; + char prevent_false_Share[ enki::gc_CacheLineSize - sizeof(std::atomic) - sizeof(semaphoreid_t*) - sizeof( uint32_t ) ]; // required to prevent alignment padding warning + }; + constexpr size_t SIZEOFTHREADDATASTORE = sizeof( ThreadDataStore ); // for easier inspection + static_assert( SIZEOFTHREADDATASTORE == enki::gc_CacheLineSize, "ThreadDataStore may exhibit false sharing" ); + + class PinnedTaskList : public LocklessMultiWriteIntrusiveList {}; + + semaphoreid_t* SemaphoreCreate(); + void SemaphoreDelete( semaphoreid_t* pSemaphore_ ); + void SemaphoreWait( semaphoreid_t& semaphoreid ); + void SemaphoreSignal( semaphoreid_t& semaphoreid, int32_t countWaiting ); +} + +namespace +{ + SubTaskSet SplitTask( SubTaskSet& subTask_, uint32_t rangeToSplit_ ) + { + SubTaskSet splitTask = subTask_; + uint32_t rangeLeft = subTask_.partition.end - subTask_.partition.start; + rangeToSplit_ = std::min( rangeToSplit_, rangeLeft ); + splitTask.partition.end = subTask_.partition.start + rangeToSplit_; + subTask_.partition.start = splitTask.partition.end; + return splitTask; + } + + #if ( defined _WIN32 && ( defined _M_IX86 || defined _M_X64 ) ) || ( defined __i386__ || defined __x86_64__ ) + // Note: see https://software.intel.com/en-us/articles/a-common-construct-to-avoid-the-contention-of-threads-architecture-agnostic-spin-wait-loops + void SpinWait( uint32_t spinCount_ ) + { + uint64_t end = __rdtsc() + spinCount_; + while( __rdtsc() < end ) + { + _mm_pause(); + } + } + #else + void SpinWait( uint32_t spinCount_ ) + { + while( spinCount_ ) + { + // TODO: may have NOP or yield equiv + --spinCount_; + } + } + #endif + + void SafeCallback( ProfilerCallbackFunc func_, uint32_t threadnum_ ) + { + if( func_ != nullptr ) + { + func_( threadnum_ ); + } + } +} + + +ENKITS_API void* enki::DefaultAllocFunc( size_t align_, size_t size_, void* userData_, const char* file_, int line_ ) +{ + (void)userData_; (void)file_; (void)line_; + void* pRet; +#ifdef _WIN32 + pRet = (void*)_aligned_malloc( size_, align_ ); +#else + pRet = nullptr; + if( align_ <= size_ && align_ <= alignof(int64_t) ) + { + // no need for alignment, use malloc + pRet = malloc( size_ ); + } + else + { + int retval = posix_memalign( &pRet, align_, size_ ); + (void)retval; // unused + } +#endif + return pRet; +} + +ENKITS_API void enki::DefaultFreeFunc( void* ptr_, size_t size_, void* userData_, const char* file_, int line_ ) +{ + (void)size_; (void)userData_; (void)file_; (void)line_; +#ifdef _WIN32 + _aligned_free( ptr_ ); +#else + free( ptr_ ); +#endif +} + +bool TaskScheduler::RegisterExternalTaskThread() +{ + bool bRegistered = false; + while( !bRegistered && m_NumExternalTaskThreadsRegistered < (int32_t)m_Config.numExternalTaskThreads ) + { + for(uint32_t thread = GetNumFirstExternalTaskThread(); thread < GetNumFirstExternalTaskThread() + m_Config.numExternalTaskThreads; ++thread ) + { + ThreadState threadStateExpected = ENKI_THREAD_STATE_EXTERNAL_UNREGISTERED; + if( m_pThreadDataStore[thread].threadState.compare_exchange_strong( + threadStateExpected, ENKI_THREAD_STATE_EXTERNAL_REGISTERED ) ) + { + ++m_NumExternalTaskThreadsRegistered; + gtl_threadNum = thread; + bRegistered = true; + break; + } + } + } + return bRegistered; +} + +bool TaskScheduler::RegisterExternalTaskThread( uint32_t threadNumToRegister_ ) +{ + ENKI_ASSERT( threadNumToRegister_ >= GetNumFirstExternalTaskThread() ); + ENKI_ASSERT( threadNumToRegister_ < ( GetNumFirstExternalTaskThread() + m_Config.numExternalTaskThreads ) ); + ThreadState threadStateExpected = ENKI_THREAD_STATE_EXTERNAL_UNREGISTERED; + if( m_pThreadDataStore[threadNumToRegister_].threadState.compare_exchange_strong( + threadStateExpected, ENKI_THREAD_STATE_EXTERNAL_REGISTERED ) ) + { + ++m_NumExternalTaskThreadsRegistered; + gtl_threadNum = threadNumToRegister_; + return true; + } + return false; +} + + +void TaskScheduler::DeRegisterExternalTaskThread() +{ + ENKI_ASSERT( gtl_threadNum != enki::NO_THREAD_NUM ); + ENKI_ASSERT( gtl_threadNum >= GetNumFirstExternalTaskThread() ); + ThreadState threadState = m_pThreadDataStore[gtl_threadNum].threadState.load( std::memory_order_acquire ); + ENKI_ASSERT( threadState == ENKI_THREAD_STATE_EXTERNAL_REGISTERED ); + if( threadState == ENKI_THREAD_STATE_EXTERNAL_REGISTERED ) + { + --m_NumExternalTaskThreadsRegistered; + m_pThreadDataStore[gtl_threadNum].threadState.store( ENKI_THREAD_STATE_EXTERNAL_UNREGISTERED, std::memory_order_release ); + gtl_threadNum = enki::NO_THREAD_NUM; + } +} + +uint32_t TaskScheduler::GetNumRegisteredExternalTaskThreads() +{ + return m_NumExternalTaskThreadsRegistered; +} + +void TaskScheduler::TaskingThreadFunction( const ThreadArgs& args_ ) +{ + uint32_t threadNum = args_.threadNum; + TaskScheduler* pTS = args_.pTaskScheduler; + gtl_threadNum = threadNum; + + pTS->m_pThreadDataStore[threadNum].threadState.store( ENKI_THREAD_STATE_RUNNING, std::memory_order_release ); + SafeCallback( pTS->m_Config.profilerCallbacks.threadStart, threadNum ); + + uint32_t spinCount = 0; + uint32_t hintPipeToCheck_io = threadNum + 1; // does not need to be clamped. + while( pTS->GetIsRunningInt() ) + { + if( !pTS->TryRunTask( threadNum, hintPipeToCheck_io ) ) + { + // no tasks, will spin then wait + ++spinCount; + if( spinCount > gc_SpinCount ) + { + pTS->WaitForNewTasks( threadNum ); + } + else + { + uint32_t spinBackoffCount = spinCount * gc_SpinBackOffMultiplier; + SpinWait( spinBackoffCount ); + } + } + else + { + spinCount = 0; // have run a task so reset spin count. + } + } + + pTS->m_NumInternalTaskThreadsRunning.fetch_sub( 1, std::memory_order_release ); + pTS->m_pThreadDataStore[threadNum].threadState.store( ENKI_THREAD_STATE_STOPPED, std::memory_order_release ); + SafeCallback( pTS->m_Config.profilerCallbacks.threadStop, threadNum ); +} + + +void TaskScheduler::StartThreads() +{ + if( m_bHaveThreads ) + { + return; + } + + m_NumThreads = m_Config.numTaskThreadsToCreate + m_Config.numExternalTaskThreads + 1; + + for( int priority = 0; priority < TASK_PRIORITY_NUM; ++priority ) + { + m_pPipesPerThread[ priority ] = NewArray( m_NumThreads, ENKI_FILE_AND_LINE ); + m_pPinnedTaskListPerThread[ priority ] = NewArray( m_NumThreads, ENKI_FILE_AND_LINE ); + } + + m_pNewTaskSemaphore = SemaphoreNew(); + m_pTaskCompleteSemaphore = SemaphoreNew(); + + // we create one less thread than m_NumThreads as the main thread counts as one + m_pThreadDataStore = NewArray( m_NumThreads, ENKI_FILE_AND_LINE ); + m_pThreads = NewArray( m_NumThreads, ENKI_FILE_AND_LINE ); + m_bRunning = true; + m_bWaitforAllCalled = false; + m_bShutdownRequested = false; + + // current thread is primary enkiTS thread + m_pThreadDataStore[0].threadState = ENKI_THREAD_STATE_PRIMARY_REGISTERED; + gtl_threadNum = 0; + + for( uint32_t thread = GetNumFirstExternalTaskThread(); thread < m_Config.numExternalTaskThreads + GetNumFirstExternalTaskThread(); ++thread ) + { + m_pThreadDataStore[thread].threadState = ENKI_THREAD_STATE_EXTERNAL_UNREGISTERED; + } + for( uint32_t thread = m_Config.numExternalTaskThreads + GetNumFirstExternalTaskThread(); thread < m_NumThreads; ++thread ) + { + m_pThreadDataStore[thread].threadState = ENKI_THREAD_STATE_NOT_LAUNCHED; + } + + + // Create Wait New Pinned Task Semaphores and init rndSeed + for( uint32_t threadNum = 0; threadNum < m_NumThreads; ++threadNum ) + { + m_pThreadDataStore[threadNum].pWaitNewPinnedTaskSemaphore = SemaphoreNew(); + m_pThreadDataStore[threadNum].rndSeed = threadNum; + } + + // only launch threads once all thread states are set + for( uint32_t thread = m_Config.numExternalTaskThreads + GetNumFirstExternalTaskThread(); thread < m_NumThreads; ++thread ) + { + m_pThreads[thread] = std::thread( TaskingThreadFunction, ThreadArgs{ thread, this } ); + ++m_NumInternalTaskThreadsRunning; + } + + // ensure we have sufficient tasks to equally fill either all threads including main + // or just the threads we've launched, this is outside the first init as we want to be able + // to runtime change it + if( 1 == m_NumThreads ) + { + m_NumPartitions = 1; + m_NumInitialPartitions = 1; + } + else + { + // There could be more threads than hardware threads if external threads are + // being intended for blocking functionality such as io etc. + // We only need to partition for a maximum of the available processor parallelism. + uint32_t numThreadsToPartitionFor = std::min( m_NumThreads, GetNumHardwareThreads() ); + m_NumPartitions = numThreadsToPartitionFor * (numThreadsToPartitionFor - 1); + // ensure m_NumPartitions, m_NumInitialPartitions non zero, can happen if m_NumThreads > 1 && GetNumHardwareThreads() == 1 + m_NumPartitions = std::max( m_NumPartitions, (uint32_t)1 ); + m_NumInitialPartitions = std::max( numThreadsToPartitionFor - 1, (uint32_t)1 ); + m_NumInitialPartitions = std::min( m_NumInitialPartitions, gc_MaxNumInitialPartitions ); + } + +#ifdef ENKI_USE_WINDOWS_PROCESSOR_API + // x64 bit Windows may support >64 logical processors using processor groups, and only allocate threads to a default group. + // We need to detect this and distribute threads accordingly + if( GetNumHardwareThreads() > 64 && // only have processor groups if > 64 hardware threads + std::thread::hardware_concurrency() < GetNumHardwareThreads() && // if std::thread sees > 64 hardware threads no need to distribute + std::thread::hardware_concurrency() < m_NumThreads ) // no need to distribute if number of threads requested lower than std::thread sees + { + uint32_t numProcessorGroups = GetActiveProcessorGroupCount(); + GROUP_AFFINITY mainThreadAffinity; + BOOL success = GetThreadGroupAffinity( GetCurrentThread(), &mainThreadAffinity ); + ENKI_ASSERT( success ); + if( success ) + { + uint32_t mainProcessorGroup = mainThreadAffinity.Group; + uint32_t currLogicalProcess = GetActiveProcessorCount( (WORD)mainProcessorGroup ); // we start iteration at end of current process group's threads + + // If more threads are created than there are logical processors then we still want to distribute them evenly amongst groups + // so we iterate continuously around the groups until we reach m_NumThreads + uint32_t group = 0; + while( currLogicalProcess < m_NumThreads ) + { + ++group; // start at group 1 since we set currLogicalProcess to start of next group + uint32_t currGroup = ( group + mainProcessorGroup ) % numProcessorGroups; // we start at mainProcessorGroup, go round in circles + uint32_t groupNumLogicalProcessors = GetActiveProcessorCount( (WORD)currGroup ); + ENKI_ASSERT( groupNumLogicalProcessors <= 64 ); + uint64_t GROUPMASK = 0xFFFFFFFFFFFFFFFFULL >> (64-groupNumLogicalProcessors); // group mask should not have 1's where there are no processors + for( uint32_t groupLogicalProcess = 0; ( groupLogicalProcess < groupNumLogicalProcessors ) && ( currLogicalProcess < m_NumThreads ); ++groupLogicalProcess, ++currLogicalProcess ) + { + if( currLogicalProcess > m_Config.numExternalTaskThreads + GetNumFirstExternalTaskThread() ) + { + auto thread_handle = m_pThreads[currLogicalProcess].native_handle(); + + // From https://learn.microsoft.com/en-us/windows/win32/procthread/processor-groups + // If a thread is assigned to a different group than the process, the process's affinity is updated to include the thread's affinity + // and the process becomes a multi-group process. + GROUP_AFFINITY threadAffinity; + success = GetThreadGroupAffinity( thread_handle, &threadAffinity ); + ENKI_ASSERT(success); (void)success; + if( threadAffinity.Group != currGroup ) + { + threadAffinity.Group = (WORD)currGroup; + threadAffinity.Mask = GROUPMASK; + success = SetThreadGroupAffinity( thread_handle, &threadAffinity, nullptr ); + ENKI_ASSERT( success ); (void)success; + } + } + } + } + } + } +#endif + + m_bHaveThreads = true; +} + +void TaskScheduler::StopThreads( bool bWait_ ) +{ + // we set m_bWaitforAllCalled to true to ensure any task which loop using this status exit + m_bWaitforAllCalled.store( true, std::memory_order_release ); + + // set status + m_bShutdownRequested.store( true, std::memory_order_release ); + m_bRunning.store( false, std::memory_order_release ); + + if( m_bHaveThreads ) + { + + // wait for threads to quit before deleting data + while( bWait_ && m_NumInternalTaskThreadsRunning ) + { + // keep firing event to ensure all threads pick up state of m_bRunning + WakeThreadsForNewTasks(); + + for( uint32_t threadId = 0; threadId < m_NumThreads; ++threadId ) + { + // send wait for new pinned tasks signal to ensure any waiting are awoken + SemaphoreSignal( *m_pThreadDataStore[ threadId ].pWaitNewPinnedTaskSemaphore, 1 ); + } + } + + // detach threads starting with thread GetNumFirstExternalTaskThread() (as 0 is initialization thread). + for( uint32_t thread = m_Config.numExternalTaskThreads + GetNumFirstExternalTaskThread(); thread < m_NumThreads; ++thread ) + { + ENKI_ASSERT( m_pThreads[thread].joinable() ); + m_pThreads[thread].join(); + } + + // delete any Wait New Pinned Task Semaphores + for( uint32_t threadNum = 0; threadNum < m_NumThreads; ++threadNum ) + { + SemaphoreDelete( m_pThreadDataStore[threadNum].pWaitNewPinnedTaskSemaphore ); + } + + DeleteArray( m_pThreadDataStore, m_NumThreads, ENKI_FILE_AND_LINE ); + DeleteArray( m_pThreads, m_NumThreads, ENKI_FILE_AND_LINE ); + m_pThreadDataStore = 0; + m_pThreads = 0; + + SemaphoreDelete( m_pNewTaskSemaphore ); + m_pNewTaskSemaphore = 0; + SemaphoreDelete( m_pTaskCompleteSemaphore ); + m_pTaskCompleteSemaphore = 0; + + m_bHaveThreads = false; + m_NumThreadsWaitingForNewTasks = 0; + m_NumThreadsWaitingForTaskCompletion = 0; + m_NumInternalTaskThreadsRunning = 0; + m_NumExternalTaskThreadsRegistered = 0; + + for( int priority = 0; priority < TASK_PRIORITY_NUM; ++priority ) + { + DeleteArray( m_pPipesPerThread[ priority ], m_NumThreads, ENKI_FILE_AND_LINE ); + m_pPipesPerThread[ priority ] = NULL; + DeleteArray( m_pPinnedTaskListPerThread[ priority ], m_NumThreads, ENKI_FILE_AND_LINE ); + m_pPinnedTaskListPerThread[ priority ] = NULL; + } + m_NumThreads = 0; + } +} + +bool TaskScheduler::TryRunTask( uint32_t threadNum_, uint32_t& hintPipeToCheck_io_ ) +{ + for( int priority = 0; priority < TASK_PRIORITY_NUM; ++priority ) + { + if( TryRunTask( threadNum_, priority, hintPipeToCheck_io_ ) ) + { + return true; + } + } + return false; +} + +static inline uint32_t RotateLeft( uint32_t value, int32_t count ) +{ + return ( value << count ) | ( value >> ( 32 - count )); +} +/* xxHash variant based on documentation on + https://github.com/Cyan4973/xxHash/blob/eec5700f4d62113b47ee548edbc4746f61ffb098/doc/xxhash_spec.md + + Copyright (c) Yann Collet + + Permission is granted to copy and distribute this document for any purpose and without charge, including translations into other languages and incorporation into compilations, provided that the copyright notice and this notice are preserved, and that any substantive changes or deletions from the original are clearly marked. Distribution of this document is unlimited. +*/ +static inline uint32_t Hash32( uint32_t in_ ) +{ + static const uint32_t PRIME32_1 = 2654435761U; // 0b10011110001101110111100110110001 + static const uint32_t PRIME32_2 = 2246822519U; // 0b10000101111010111100101001110111 + static const uint32_t PRIME32_3 = 3266489917U; // 0b11000010101100101010111000111101 + static const uint32_t PRIME32_4 = 668265263U; // 0b00100111110101001110101100101111 + static const uint32_t PRIME32_5 = 374761393U; // 0b00010110010101100110011110110001 + static const uint32_t SEED = 0; // can configure seed if needed + + // simple hash of nodes, does not check if nodePool is compressed or not. + uint32_t acc = SEED + PRIME32_5; + + // add node types to map, and also ensure that fully empty nodes are well distributed by hashing the pointer. + acc += in_; + acc = acc ^ (acc >> 15); + acc = acc * PRIME32_2; + acc = acc ^ (acc >> 13); + acc = acc * PRIME32_3; + acc = acc ^ (acc >> 16); + return acc; +} + +bool TaskScheduler::TryRunTask( uint32_t threadNum_, uint32_t priority_, uint32_t& hintPipeToCheck_io_ ) +{ + // Run any tasks for this thread + RunPinnedTasks( threadNum_, priority_ ); + + // check for tasks + SubTaskSet subTask; + bool bHaveTask = m_pPipesPerThread[ priority_ ][ threadNum_ ].WriterTryReadFront( &subTask ); + + uint32_t threadToCheckStart = hintPipeToCheck_io_ % m_NumThreads; + uint32_t threadToCheck = threadToCheckStart; + uint32_t checkCount = 0; + if( !bHaveTask ) + { + bHaveTask = m_pPipesPerThread[ priority_ ][ threadToCheck ].ReaderTryReadBack( &subTask ); + if( !bHaveTask ) + { + // To prevent many threads checking the same task pipe for work we pseudorandomly distribute + // the starting thread which we start checking for tasks to run + uint32_t& rndSeed = m_pThreadDataStore[threadNum_].rndSeed; + ++rndSeed; + uint32_t threadToCheckOffset = Hash32( rndSeed * threadNum_ ); + while( !bHaveTask && checkCount < m_NumThreads ) + { + threadToCheck = ( threadToCheckOffset + checkCount ) % m_NumThreads; + if( threadToCheck != threadNum_ && threadToCheckOffset != threadToCheckStart ) + { + bHaveTask = m_pPipesPerThread[ priority_ ][ threadToCheck ].ReaderTryReadBack( &subTask ); + } + ++checkCount; + } + } + } + + if( bHaveTask ) + { + // update hint, will preserve value unless actually got task from another thread. + hintPipeToCheck_io_ = threadToCheck; + + uint32_t partitionSize = subTask.partition.end - subTask.partition.start; + if( subTask.pTask->m_RangeToRun < partitionSize ) + { + SubTaskSet taskToRun = SplitTask( subTask, subTask.pTask->m_RangeToRun ); + uint32_t rangeToSplit = subTask.pTask->m_RangeToRun; + if( threadNum_ != threadToCheck ) + { + // task was stolen from another thread + // in order to ensure other threads can get enough work we need to split into larger ranges + // these larger splits are then stolen and split themselves + // otherwise other threads must keep stealing from this thread, which may stall when pipe is full + rangeToSplit = std::max( rangeToSplit, (subTask.partition.end - subTask.partition.start) / gc_MaxStolenPartitions ); + } + SplitAndAddTask( threadNum_, subTask, rangeToSplit ); + taskToRun.pTask->ExecuteRange( taskToRun.partition, threadNum_ ); + int prevCount = taskToRun.pTask->m_RunningCount.fetch_sub(1,std::memory_order_acq_rel ); + if( gc_TaskStartCount == prevCount ) + { + TaskComplete( taskToRun.pTask, true, threadNum_ ); + } + } + else + { + // the task has already been divided up by AddTaskSetToPipe, so just run it + subTask.pTask->ExecuteRange( subTask.partition, threadNum_ ); + int prevCount = subTask.pTask->m_RunningCount.fetch_sub(1,std::memory_order_acq_rel ); + if( gc_TaskStartCount == prevCount ) + { + TaskComplete( subTask.pTask, true, threadNum_ ); + } + } + } + + return bHaveTask; + +} + +void TaskScheduler::TaskComplete( ICompletable* pTask_, bool bWakeThreads_, uint32_t threadNum_ ) +{ + // It must be impossible for a thread to enter the sleeping wait prior to the load of m_WaitingForTaskCount + // in this function, so we introduce a gc_TaskAlmostCompleteCount to prevent this. + ENKI_ASSERT( gc_TaskAlmostCompleteCount == pTask_->m_RunningCount.load( std::memory_order_acquire ) ); + bool bCallWakeThreads = bWakeThreads_ && pTask_->m_WaitingForTaskCount.load( std::memory_order_acquire ); + + Dependency* pDependent = pTask_->m_pDependents; + + // Do not access pTask_ below this line unless we have dependencies. + pTask_->m_RunningCount.store( 0, std::memory_order_release ); + + if( bCallWakeThreads ) + { + WakeThreadsForTaskCompletion(); + } + + while( pDependent ) + { + // access pTaskToRunOnCompletion member data before incrementing m_DependenciesCompletedCount so + // they do not get deleted when another thread completes the pTaskToRunOnCompletion + int32_t dependenciesCount = pDependent->pTaskToRunOnCompletion->m_DependenciesCount; + // get temp copy of pDependent so OnDependenciesComplete can delete task if needed. + Dependency* pDependentCurr = pDependent; + pDependent = pDependent->pNext; + int32_t prevDeps = pDependentCurr->pTaskToRunOnCompletion->m_DependenciesCompletedCount.fetch_add( 1, std::memory_order_release ); + ENKI_ASSERT( prevDeps < dependenciesCount ); + if( dependenciesCount == ( prevDeps + 1 ) ) + { + // reset dependencies + // only safe to access pDependentCurr here after above fetch_add because this is the thread + // which calls OnDependenciesComplete after store with memory_order_release + pDependentCurr->pTaskToRunOnCompletion->m_DependenciesCompletedCount.store( + 0, + std::memory_order_release ); + pDependentCurr->pTaskToRunOnCompletion->OnDependenciesComplete( this, threadNum_ ); + } + } +} + +bool TaskScheduler::HaveTasks( uint32_t threadNum_ ) +{ + for( int priority = 0; priority < TASK_PRIORITY_NUM; ++priority ) + { + for( uint32_t thread = 0; thread < m_NumThreads; ++thread ) + { + if( !m_pPipesPerThread[ priority ][ thread ].IsPipeEmpty() ) + { + return true; + } + } + if( !m_pPinnedTaskListPerThread[ priority ][ threadNum_ ].IsListEmpty() ) + { + return true; + } + } + return false; +} + +void TaskScheduler::WaitForNewTasks( uint32_t threadNum_ ) +{ + // We don't want to suspend this thread if there are task threads + // with pinned tasks suspended, as it could result in this thread + // being unsuspended and not the thread with pinned tasks + if( WakeSuspendedThreadsWithPinnedTasks( threadNum_ ) ) + { + return; + } + + // We increment the number of threads waiting here in order + // to ensure that the check for tasks occurs after the increment + // to prevent a task being added after a check, then the thread waiting. + // This will occasionally result in threads being mistakenly awoken, + // but they will then go back to sleep. + m_NumThreadsWaitingForNewTasks.fetch_add( 1, std::memory_order_acquire ); + ThreadState prevThreadState = m_pThreadDataStore[threadNum_].threadState.load( std::memory_order_relaxed ); + m_pThreadDataStore[threadNum_].threadState.store( ENKI_THREAD_STATE_WAIT_NEW_TASKS, std::memory_order_seq_cst ); + + if( HaveTasks( threadNum_ ) ) + { + m_NumThreadsWaitingForNewTasks.fetch_sub( 1, std::memory_order_release ); + } + else + { + SafeCallback( m_Config.profilerCallbacks.waitForNewTaskSuspendStart, threadNum_ ); + SemaphoreWait( *m_pNewTaskSemaphore ); + SafeCallback( m_Config.profilerCallbacks.waitForNewTaskSuspendStop, threadNum_ ); + } + + m_pThreadDataStore[threadNum_].threadState.store( prevThreadState, std::memory_order_release ); +} + +void TaskScheduler::WaitForTaskCompletion( const ICompletable* pCompletable_, uint32_t threadNum_ ) +{ + // We don't want to suspend this thread if there are task threads + // with pinned tasks suspended, as the completable could be a pinned task + // or it could be waiting on one. + if( WakeSuspendedThreadsWithPinnedTasks( threadNum_ ) ) + { + return; + } + + m_NumThreadsWaitingForTaskCompletion.fetch_add( 1, std::memory_order_acq_rel ); + pCompletable_->m_WaitingForTaskCount.fetch_add( 1, std::memory_order_acq_rel ); + ThreadState prevThreadState = m_pThreadDataStore[threadNum_].threadState.load( std::memory_order_relaxed ); + m_pThreadDataStore[threadNum_].threadState.store( ENKI_THREAD_STATE_WAIT_TASK_COMPLETION, std::memory_order_seq_cst ); + + // do not wait on semaphore if task in gc_TaskAlmostCompleteCount state. + if( gc_TaskAlmostCompleteCount >= pCompletable_->m_RunningCount.load( std::memory_order_acquire ) || HaveTasks( threadNum_ ) ) + { + m_NumThreadsWaitingForTaskCompletion.fetch_sub( 1, std::memory_order_acq_rel ); + } + else + { + SafeCallback( m_Config.profilerCallbacks.waitForTaskCompleteSuspendStart, threadNum_ ); + std::atomic_thread_fence(std::memory_order_acquire); + + SemaphoreWait( *m_pTaskCompleteSemaphore ); + if( !pCompletable_->GetIsComplete() ) + { + // This thread which may not the one which was supposed to be awoken + WakeThreadsForTaskCompletion(); + } + SafeCallback( m_Config.profilerCallbacks.waitForTaskCompleteSuspendStop, threadNum_ ); + } + + m_pThreadDataStore[threadNum_].threadState.store( prevThreadState, std::memory_order_release ); + pCompletable_->m_WaitingForTaskCount.fetch_sub( 1, std::memory_order_acq_rel ); +} + +void TaskScheduler::WakeThreadsForNewTasks() +{ + int32_t waiting = m_NumThreadsWaitingForNewTasks.load( std::memory_order_relaxed ); + while( waiting > 0 && !m_NumThreadsWaitingForNewTasks.compare_exchange_weak(waiting, 0, std::memory_order_release, std::memory_order_relaxed ) ) {} + + if( waiting > 0 ) + { + SemaphoreSignal( *m_pNewTaskSemaphore, waiting ); + } + + // We also wake tasks waiting for completion as they can run tasks + WakeThreadsForTaskCompletion(); +} + +void TaskScheduler::WakeThreadsForTaskCompletion() +{ + // m_NumThreadsWaitingForTaskCompletion can go negative as this indicates that + // we signalled more threads than the number which ended up waiting + int32_t waiting = m_NumThreadsWaitingForTaskCompletion.load( std::memory_order_relaxed ); + while( waiting > 0 && !m_NumThreadsWaitingForTaskCompletion.compare_exchange_weak(waiting, 0, std::memory_order_release, std::memory_order_relaxed ) ) {} + + if( waiting > 0 ) + { + SemaphoreSignal( *m_pTaskCompleteSemaphore, waiting ); + } +} + +bool TaskScheduler::WakeSuspendedThreadsWithPinnedTasks( uint32_t threadNum_ ) +{ + for( uint32_t t = 1; t < m_NumThreads; ++t ) + { + // distribute thread checks more evenly by starting at our thread number rather than 0. + uint32_t thread = ( threadNum_ + t ) % m_NumThreads; + + ThreadState state = m_pThreadDataStore[ thread ].threadState.load( std::memory_order_acquire ); + + ENKI_ASSERT( state != ENKI_THREAD_STATE_NONE ); + + if( state == ENKI_THREAD_STATE_WAIT_NEW_TASKS || state == ENKI_THREAD_STATE_WAIT_TASK_COMPLETION ) + { + // thread is suspended, check if it has pinned tasks + for( int priority = 0; priority < TASK_PRIORITY_NUM; ++priority ) + { + if( !m_pPinnedTaskListPerThread[ priority ][ thread ].IsListEmpty() ) + { + WakeThreadsForNewTasks(); + return true; + } + } + } + } + return false; +} + +void TaskScheduler::SplitAndAddTask( uint32_t threadNum_, SubTaskSet subTask_, uint32_t rangeToSplit_ ) +{ + int32_t numAdded = 0; + int32_t numNewTasksSinceNotification = 0; + int32_t numRun = 0; + + int32_t upperBoundNumToAdd = 2 + (int32_t)( ( subTask_.partition.end - subTask_.partition.start ) / rangeToSplit_ ); + + // ensure that an artificial completion is not registered whilst adding tasks by incrementing count + subTask_.pTask->m_RunningCount.fetch_add( upperBoundNumToAdd, std::memory_order_acquire ); + while( subTask_.partition.start != subTask_.partition.end ) + { + SubTaskSet taskToAdd = SplitTask( subTask_, rangeToSplit_ ); + + // add the partition to the pipe + ++numAdded; ++numNewTasksSinceNotification; + if( !m_pPipesPerThread[ subTask_.pTask->m_Priority ][ threadNum_ ].WriterTryWriteFront( taskToAdd ) ) + { + --numAdded; // we were unable to add the task + if( numNewTasksSinceNotification > 1 ) + { + WakeThreadsForNewTasks(); + } + numNewTasksSinceNotification = 0; + // alter range to run the appropriate fraction + if( taskToAdd.pTask->m_RangeToRun < taskToAdd.partition.end - taskToAdd.partition.start ) + { + taskToAdd.partition.end = taskToAdd.partition.start + taskToAdd.pTask->m_RangeToRun; + ENKI_ASSERT( taskToAdd.partition.end <= taskToAdd.pTask->m_SetSize ); + subTask_.partition.start = taskToAdd.partition.end; + } + taskToAdd.pTask->ExecuteRange( taskToAdd.partition, threadNum_ ); + ++numRun; + } + } + int32_t countToRemove = upperBoundNumToAdd - numAdded; + ENKI_ASSERT( countToRemove > 0 ); + int prevCount = subTask_.pTask->m_RunningCount.fetch_sub( countToRemove, std::memory_order_acq_rel ); + if( countToRemove-1 + gc_TaskStartCount == prevCount ) + { + TaskComplete( subTask_.pTask, false, threadNum_ ); + } + + // WakeThreadsForNewTasks also calls WakeThreadsForTaskCompletion() so do not need to do so above + WakeThreadsForNewTasks(); +} + +TaskSchedulerConfig TaskScheduler::GetConfig() const +{ + return m_Config; +} + +void TaskScheduler::AddTaskSetToPipeInt( ITaskSet* pTaskSet_, uint32_t threadNum_ ) +{ + ENKI_ASSERT( pTaskSet_->m_RunningCount == gc_TaskStartCount ); + ThreadState prevThreadState = m_pThreadDataStore[threadNum_].threadState.load( std::memory_order_relaxed ); + m_pThreadDataStore[threadNum_].threadState.store( ENKI_THREAD_STATE_RUNNING, std::memory_order_relaxed ); + std::atomic_thread_fence(std::memory_order_acquire); + + + // divide task up and add to pipe + pTaskSet_->m_RangeToRun = pTaskSet_->m_SetSize / m_NumPartitions; + pTaskSet_->m_RangeToRun = std::max( pTaskSet_->m_RangeToRun, pTaskSet_->m_MinRange ); + // Note: if m_SetSize is < m_RangeToRun this will be handled by SplitTask and so does not need to be handled here + + uint32_t rangeToSplit = pTaskSet_->m_SetSize / m_NumInitialPartitions; + rangeToSplit = std::max( rangeToSplit, pTaskSet_->m_MinRange ); + + SubTaskSet subTask; + subTask.pTask = pTaskSet_; + subTask.partition.start = 0; + subTask.partition.end = pTaskSet_->m_SetSize; + SplitAndAddTask( threadNum_, subTask, rangeToSplit ); + int prevCount = pTaskSet_->m_RunningCount.fetch_sub(1, std::memory_order_acq_rel ); + if( gc_TaskStartCount == prevCount ) + { + TaskComplete( pTaskSet_, true, threadNum_ ); + } + + m_pThreadDataStore[threadNum_].threadState.store( prevThreadState, std::memory_order_release ); +} + +void TaskScheduler::AddTaskSetToPipe( ITaskSet* pTaskSet_ ) +{ + ENKI_ASSERT( pTaskSet_->m_RunningCount == 0 ); + InitDependencies( pTaskSet_ ); + pTaskSet_->m_RunningCount.store( gc_TaskStartCount, std::memory_order_relaxed ); + AddTaskSetToPipeInt( pTaskSet_, gtl_threadNum ); +} + +void TaskScheduler::AddPinnedTaskInt( IPinnedTask* pTask_ ) +{ + ENKI_ASSERT( pTask_->m_RunningCount == gc_TaskStartCount ); + m_pPinnedTaskListPerThread[ pTask_->m_Priority ][ pTask_->threadNum ].WriterWriteFront( pTask_ ); + + ThreadState statePinnedTaskThread = m_pThreadDataStore[ pTask_->threadNum ].threadState.load( std::memory_order_acquire ); + if( statePinnedTaskThread == ENKI_THREAD_STATE_WAIT_NEW_PINNED_TASKS ) + { + SemaphoreSignal( *m_pThreadDataStore[ pTask_->threadNum ].pWaitNewPinnedTaskSemaphore, 1 ); + } + else + { + WakeThreadsForNewTasks(); + } +} + +void TaskScheduler::AddPinnedTask( IPinnedTask* pTask_ ) +{ + ENKI_ASSERT( pTask_->m_RunningCount == 0 ); + InitDependencies( pTask_ ); + pTask_->m_RunningCount = gc_TaskStartCount; + AddPinnedTaskInt( pTask_ ); +} + +void TaskScheduler::InitDependencies( ICompletable* pCompletable_ ) +{ + // go through any dependencies and set their running count so they show as not complete + // and increment dependency count + if( pCompletable_->m_RunningCount.load( std::memory_order_relaxed ) ) + { + // already initialized + return; + } + Dependency* pDependent = pCompletable_->m_pDependents; + while( pDependent ) + { + InitDependencies( pDependent->pTaskToRunOnCompletion ); + pDependent->pTaskToRunOnCompletion->m_RunningCount.store( gc_TaskStartCount, std::memory_order_relaxed ); + pDependent = pDependent->pNext; + } +} + + +void TaskScheduler::RunPinnedTasks() +{ + ENKI_ASSERT( gtl_threadNum != enki::NO_THREAD_NUM ); + uint32_t threadNum = gtl_threadNum; + ThreadState prevThreadState = m_pThreadDataStore[threadNum].threadState.load( std::memory_order_relaxed ); + m_pThreadDataStore[threadNum].threadState.store( ENKI_THREAD_STATE_RUNNING, std::memory_order_relaxed ); + std::atomic_thread_fence(std::memory_order_acquire); + for( int priority = 0; priority < TASK_PRIORITY_NUM; ++priority ) + { + RunPinnedTasks( threadNum, priority ); + } + m_pThreadDataStore[threadNum].threadState.store( prevThreadState, std::memory_order_release ); +} + +void TaskScheduler::RunPinnedTasks( uint32_t threadNum_, uint32_t priority_ ) +{ + IPinnedTask* pPinnedTaskSet = NULL; + do + { + pPinnedTaskSet = m_pPinnedTaskListPerThread[ priority_ ][ threadNum_ ].ReaderReadBack(); + if( pPinnedTaskSet ) + { + pPinnedTaskSet->Execute(); + pPinnedTaskSet->m_RunningCount.fetch_sub(1,std::memory_order_acq_rel); + TaskComplete( pPinnedTaskSet, true, threadNum_ ); + } + } while( pPinnedTaskSet ); +} + +void TaskScheduler::WaitforTask( const ICompletable* pCompletable_, enki::TaskPriority priorityOfLowestToRun_ ) +{ + ENKI_ASSERT( gtl_threadNum != enki::NO_THREAD_NUM ); + uint32_t threadNum = gtl_threadNum; + uint32_t hintPipeToCheck_io = threadNum + 1; // does not need to be clamped. + + // waiting for a task is equivalent to 'running' for thread state purpose as we may run tasks whilst waiting + ThreadState prevThreadState = m_pThreadDataStore[threadNum].threadState.load( std::memory_order_relaxed ); + m_pThreadDataStore[threadNum].threadState.store( ENKI_THREAD_STATE_RUNNING, std::memory_order_relaxed ); + std::atomic_thread_fence(std::memory_order_acquire); + + + if( pCompletable_ && !pCompletable_->GetIsComplete() ) + { + SafeCallback( m_Config.profilerCallbacks.waitForTaskCompleteStart, threadNum ); + // We need to ensure that the task we're waiting on can complete even if we're the only thread, + // so we clamp the priorityOfLowestToRun_ to no smaller than the task we're waiting for + priorityOfLowestToRun_ = std::max( priorityOfLowestToRun_, pCompletable_->m_Priority ); + uint32_t spinCount = 0; + while( !pCompletable_->GetIsComplete() && GetIsRunningInt() ) + { + ++spinCount; + for( int priority = 0; priority <= priorityOfLowestToRun_; ++priority ) + { + if( TryRunTask( threadNum, priority, hintPipeToCheck_io ) ) + { + spinCount = 0; // reset spin as ran a task + break; + } + } + if( spinCount > gc_SpinCount ) + { + WaitForTaskCompletion( pCompletable_, threadNum ); + spinCount = 0; + } + else + { + uint32_t spinBackoffCount = spinCount * gc_SpinBackOffMultiplier; + SpinWait( spinBackoffCount ); + } + } + SafeCallback( m_Config.profilerCallbacks.waitForTaskCompleteStop, threadNum ); + } + else if( nullptr == pCompletable_ ) + { + for( int priority = 0; priority <= priorityOfLowestToRun_; ++priority ) + { + if( TryRunTask( threadNum, priority, hintPipeToCheck_io ) ) + { + break; + } + } + } + + m_pThreadDataStore[threadNum].threadState.store( prevThreadState, std::memory_order_release ); + +} + +class TaskSchedulerWaitTask : public IPinnedTask +{ + void Execute() override + { + // do nothing + } +}; + +void TaskScheduler::WaitforAll() +{ + ENKI_ASSERT( gtl_threadNum != enki::NO_THREAD_NUM ); + m_bWaitforAllCalled.store( true, std::memory_order_release ); + + bool bHaveTasks = true; + uint32_t ourThreadNum = gtl_threadNum; + uint32_t hintPipeToCheck_io = ourThreadNum + 1; // does not need to be clamped. + bool otherThreadsRunning = false; // account for this thread + uint32_t spinCount = 0; + TaskSchedulerWaitTask dummyWaitTask; + dummyWaitTask.threadNum = 0; + while( GetIsRunningInt() && ( bHaveTasks || otherThreadsRunning ) ) + { + bHaveTasks = TryRunTask( ourThreadNum, hintPipeToCheck_io ); + ++spinCount; + if( bHaveTasks ) + { + spinCount = 0; // reset spin as ran a task + } + if( spinCount > gc_SpinCount ) + { + // find a running thread and add a dummy wait task + int32_t countThreadsToCheck = m_NumThreads - 1; + bool bHaveThreadToWaitOn = false; + do + { + --countThreadsToCheck; + dummyWaitTask.threadNum = ( dummyWaitTask.threadNum + 1 ) % m_NumThreads; + + // We can only add a pinned task to wait on if we find an enki Task Thread which isn't this thread. + // Otherwise, we have to busy wait. + if( dummyWaitTask.threadNum != ourThreadNum && dummyWaitTask.threadNum > m_Config.numExternalTaskThreads ) + { + ThreadState state = m_pThreadDataStore[ dummyWaitTask.threadNum ].threadState.load( std::memory_order_acquire ); + if( state == ENKI_THREAD_STATE_RUNNING || state == ENKI_THREAD_STATE_WAIT_TASK_COMPLETION ) + { + bHaveThreadToWaitOn = true; + break; + } + } + } while( countThreadsToCheck ); + + if( bHaveThreadToWaitOn ) + { + ENKI_ASSERT( dummyWaitTask.threadNum != ourThreadNum ); + AddPinnedTask( &dummyWaitTask ); + WaitforTask( &dummyWaitTask ); + } + spinCount = 0; + } + else + { + uint32_t spinBackoffCount = spinCount * gc_SpinBackOffMultiplier; + SpinWait( spinBackoffCount ); + } + + // count threads running + otherThreadsRunning = false; + for(uint32_t thread = 0; thread < m_NumThreads && !otherThreadsRunning; ++thread ) + { + // ignore our thread + if( thread != ourThreadNum ) + { + switch( m_pThreadDataStore[thread].threadState.load( std::memory_order_acquire ) ) + { + case ENKI_THREAD_STATE_NONE: + ENKI_ASSERT(false); + break; + case ENKI_THREAD_STATE_NOT_LAUNCHED: + case ENKI_THREAD_STATE_RUNNING: + case ENKI_THREAD_STATE_WAIT_TASK_COMPLETION: + otherThreadsRunning = true; + break; + case ENKI_THREAD_STATE_WAIT_NEW_PINNED_TASKS: + otherThreadsRunning = true; + SemaphoreSignal( *m_pThreadDataStore[thread].pWaitNewPinnedTaskSemaphore, 1 ); + break; + case ENKI_THREAD_STATE_PRIMARY_REGISTERED: + case ENKI_THREAD_STATE_EXTERNAL_REGISTERED: + case ENKI_THREAD_STATE_EXTERNAL_UNREGISTERED: + case ENKI_THREAD_STATE_WAIT_NEW_TASKS: + case ENKI_THREAD_STATE_STOPPED: + break; + } + } + } + if( !otherThreadsRunning ) + { + // check there are no tasks + for(uint32_t thread = 0; thread < m_NumThreads && !otherThreadsRunning; ++thread ) + { + // ignore our thread + if( thread != ourThreadNum ) + { + otherThreadsRunning = HaveTasks( thread ); + } + } + } + } + + m_bWaitforAllCalled.store( false, std::memory_order_release ); +} + +void TaskScheduler::WaitforAllAndShutdown() +{ + m_bWaitforAllCalled.store( true, std::memory_order_release ); + m_bShutdownRequested.store( true, std::memory_order_release ); + if( m_bHaveThreads ) + { + WaitforAll(); + StopThreads(true); + } +} + +void TaskScheduler::ShutdownNow() +{ + m_bWaitforAllCalled.store( true, std::memory_order_release ); + m_bShutdownRequested.store( true, std::memory_order_release ); + if( m_bHaveThreads ) + { + StopThreads(true); + } +} + +void TaskScheduler::WaitForNewPinnedTasks() +{ + ENKI_ASSERT( gtl_threadNum != enki::NO_THREAD_NUM ); + uint32_t threadNum = gtl_threadNum; + ThreadState prevThreadState = m_pThreadDataStore[threadNum].threadState.load( std::memory_order_relaxed ); + m_pThreadDataStore[threadNum].threadState.store( ENKI_THREAD_STATE_WAIT_NEW_PINNED_TASKS, std::memory_order_seq_cst ); + + // check if have tasks inside threadState change but before waiting + bool bHavePinnedTasks = false; + for( int priority = 0; priority < TASK_PRIORITY_NUM; ++priority ) + { + if( !m_pPinnedTaskListPerThread[ priority ][ threadNum ].IsListEmpty() ) + { + bHavePinnedTasks = true; + break; + } + } + + if( !bHavePinnedTasks ) + { + SafeCallback( m_Config.profilerCallbacks.waitForNewTaskSuspendStart, threadNum ); + SemaphoreWait( *m_pThreadDataStore[threadNum].pWaitNewPinnedTaskSemaphore ); + SafeCallback( m_Config.profilerCallbacks.waitForNewTaskSuspendStop, threadNum ); + } + + m_pThreadDataStore[threadNum].threadState.store( prevThreadState, std::memory_order_release ); +} + + +uint32_t TaskScheduler::GetNumTaskThreads() const +{ + return m_NumThreads; +} + + +uint32_t TaskScheduler::GetThreadNum() const +{ + return gtl_threadNum; +} + +template +T* TaskScheduler::NewArray( size_t num_, const char* file_, int line_ ) +{ + T* pRet = (T*)m_Config.customAllocator.alloc( alignof(T), num_*sizeof(T), m_Config.customAllocator.userData, file_, line_ ); + if( !std::is_trivial::value ) + { + T* pCurr = pRet; + for( size_t i = 0; i < num_; ++i ) + { + void* pBuffer = pCurr; + pCurr = new(pBuffer) T; + ++pCurr; + } + } + return pRet; +} + +template +void TaskScheduler::DeleteArray( T* p_, size_t num_, const char* file_, int line_ ) +{ + if( !std::is_trivially_destructible::value ) + { + size_t i = num_; + while(i) + { + p_[--i].~T(); + } + } + m_Config.customAllocator.free( p_, sizeof(T)*num_, m_Config.customAllocator.userData, file_, line_ ); +} + +template +T* TaskScheduler::New( const char* file_, int line_, Args&&... args_ ) +{ + T* pRet = this->Alloc( file_, line_ ); + return new(pRet) T( std::forward(args_)... ); +} + +template< typename T > +void TaskScheduler::Delete( T* p_, const char* file_, int line_ ) +{ + p_->~T(); + this->Free(p_, file_, line_ ); +} + +template< typename T > +T* TaskScheduler::Alloc( const char* file_, int line_ ) +{ + T* pRet = (T*)m_Config.customAllocator.alloc( alignof(T), sizeof(T), m_Config.customAllocator.userData, file_, line_ ); + return pRet; +} + +template< typename T > +void TaskScheduler::Free( T* p_, const char* file_, int line_ ) +{ + m_Config.customAllocator.free( p_, sizeof(T), m_Config.customAllocator.userData, file_, line_ ); +} + +TaskScheduler::TaskScheduler() + : m_pPipesPerThread() + , m_pPinnedTaskListPerThread() + , m_NumThreads(0) + , m_pThreadDataStore(NULL) + , m_pThreads(NULL) + , m_bRunning(false) + , m_NumInternalTaskThreadsRunning(0) + , m_NumThreadsWaitingForNewTasks(0) + , m_NumThreadsWaitingForTaskCompletion(0) + , m_NumPartitions(0) + , m_pNewTaskSemaphore(NULL) + , m_pTaskCompleteSemaphore(NULL) + , m_NumInitialPartitions(0) + , m_bHaveThreads(false) + , m_NumExternalTaskThreadsRegistered(0) +{ +} + +TaskScheduler::~TaskScheduler() +{ + StopThreads( true ); // Stops threads, waiting for them. +} + +void TaskScheduler::Initialize( uint32_t numThreadsTotal_ ) +{ + ENKI_ASSERT( numThreadsTotal_ >= 1 ); + StopThreads( true ); // Stops threads, waiting for them. + m_Config.numTaskThreadsToCreate = numThreadsTotal_ - 1; + m_Config.numExternalTaskThreads = 0; + StartThreads();} + +void TaskScheduler::Initialize( TaskSchedulerConfig config_ ) +{ + StopThreads( true ); // Stops threads, waiting for them. + m_Config = config_; + StartThreads(); +} + +void TaskScheduler::Initialize() +{ + Initialize( std::thread::hardware_concurrency() ); +} + +// Semaphore implementation +#ifdef _WIN32 + +#ifndef WIN32_LEAN_AND_MEAN +#define WIN32_LEAN_AND_MEAN +#endif +#ifndef NOMINMAX +#define NOMINMAX +#endif +#include + +namespace enki +{ + struct semaphoreid_t + { + HANDLE sem; + }; + + inline void SemaphoreCreate( semaphoreid_t& semaphoreid ) + { +#ifdef _XBOX_ONE + semaphoreid.sem = CreateSemaphoreExW( NULL, 0, MAXLONG, NULL, 0, SEMAPHORE_ALL_ACCESS ); +#else + semaphoreid.sem = CreateSemaphore( NULL, 0, MAXLONG, NULL ); +#endif + } + + inline void SemaphoreClose( semaphoreid_t& semaphoreid ) + { + CloseHandle( semaphoreid.sem ); + } + + inline void SemaphoreWait( semaphoreid_t& semaphoreid ) + { + DWORD retval = WaitForSingleObject( semaphoreid.sem, INFINITE ); + ENKI_ASSERT( retval != WAIT_FAILED ); + (void)retval; // only needed for ENKI_ASSERT + } + + inline void SemaphoreSignal( semaphoreid_t& semaphoreid, int32_t countWaiting ) + { + if( countWaiting ) + { + ReleaseSemaphore( semaphoreid.sem, countWaiting, NULL ); + } + } +} +#elif defined(__MACH__) + + +// OS X does not have POSIX semaphores +// Mach semaphores can now only be created by the kernel +// Named semaphores work, but would require unique name construction to ensure +// they are isolated to this process. +// Dispatch semaphores appear to be the way other developers use OSX Semaphores, e.g. Boost +// However the API could change +// OSX below 10.6 does not support dispatch, but I do not have an earlier OSX version +// to test alternatives +#include + +namespace enki +{ + + struct semaphoreid_t + { + dispatch_semaphore_t sem; + }; + + inline void SemaphoreCreate( semaphoreid_t& semaphoreid ) + { + semaphoreid.sem = dispatch_semaphore_create(0); + } + + inline void SemaphoreClose( semaphoreid_t& semaphoreid ) + { + dispatch_release( semaphoreid.sem ); + } + + inline void SemaphoreWait( semaphoreid_t& semaphoreid ) + { + dispatch_semaphore_wait( semaphoreid.sem, DISPATCH_TIME_FOREVER ); + } + + inline void SemaphoreSignal( semaphoreid_t& semaphoreid, int32_t countWaiting ) + { + while( countWaiting-- > 0 ) + { + dispatch_semaphore_signal( semaphoreid.sem ); + } + } +} + +#else // POSIX + +#include +#include + +namespace enki +{ + + struct semaphoreid_t + { + sem_t sem; + }; + + inline void SemaphoreCreate( semaphoreid_t& semaphoreid ) + { + int err = sem_init( &semaphoreid.sem, 0, 0 ); + ENKI_ASSERT( err == 0 ); + (void)err; + } + + inline void SemaphoreClose( semaphoreid_t& semaphoreid ) + { + sem_destroy( &semaphoreid.sem ); + } + + inline void SemaphoreWait( semaphoreid_t& semaphoreid ) + { + while( sem_wait( &semaphoreid.sem ) == -1 && errno == EINTR ) {} + } + + inline void SemaphoreSignal( semaphoreid_t& semaphoreid, int32_t countWaiting ) + { + while( countWaiting-- > 0 ) + { + sem_post( &semaphoreid.sem ); + } + } +} +#endif + +semaphoreid_t* TaskScheduler::SemaphoreNew() +{ + semaphoreid_t* pSemaphore = this->Alloc( ENKI_FILE_AND_LINE ); + SemaphoreCreate( *pSemaphore ); + return pSemaphore; +} + +void TaskScheduler::SemaphoreDelete( semaphoreid_t* pSemaphore_ ) +{ + SemaphoreClose( *pSemaphore_ ); + this->Free( pSemaphore_, ENKI_FILE_AND_LINE ); +} + +void TaskScheduler::SetCustomAllocator( CustomAllocator customAllocator_ ) +{ + m_Config.customAllocator = customAllocator_; +} + +Dependency::Dependency( const ICompletable* pDependencyTask_, ICompletable* pTaskToRunOnCompletion_ ) + : pTaskToRunOnCompletion( pTaskToRunOnCompletion_ ) + , pDependencyTask( pDependencyTask_ ) + , pNext( pDependencyTask->m_pDependents ) +{ + ENKI_ASSERT( pDependencyTask->GetIsComplete() ); + ENKI_ASSERT( pTaskToRunOnCompletion->GetIsComplete() ); + pDependencyTask->m_pDependents = this; + ++pTaskToRunOnCompletion->m_DependenciesCount; +} + +Dependency::Dependency( Dependency&& rhs_ ) noexcept +{ + pDependencyTask = rhs_.pDependencyTask; + pTaskToRunOnCompletion = rhs_.pTaskToRunOnCompletion; + pNext = rhs_.pNext; + if( rhs_.pDependencyTask ) + { + ENKI_ASSERT( rhs_.pTaskToRunOnCompletion ); + ENKI_ASSERT( rhs_.pDependencyTask->GetIsComplete() ); + ENKI_ASSERT( rhs_.pTaskToRunOnCompletion->GetIsComplete() ); + Dependency** ppDependent = &(pDependencyTask->m_pDependents); + while( *ppDependent ) + { + if( &rhs_ == *ppDependent ) + { + *ppDependent = this; + break; + } + ppDependent = &((*ppDependent)->pNext); + } + } +} + + +Dependency::~Dependency() +{ + ClearDependency(); +} + +void Dependency::SetDependency( const ICompletable* pDependencyTask_, ICompletable* pTaskToRunOnCompletion_ ) +{ + ClearDependency(); + ENKI_ASSERT( pDependencyTask_->GetIsComplete() ); + ENKI_ASSERT( pTaskToRunOnCompletion_->GetIsComplete() ); + pDependencyTask = pDependencyTask_; + pTaskToRunOnCompletion = pTaskToRunOnCompletion_; + pNext = pDependencyTask->m_pDependents; + pDependencyTask->m_pDependents = this; + ++pTaskToRunOnCompletion->m_DependenciesCount; +} + +void Dependency::ClearDependency() +{ + if( pDependencyTask ) + { + ENKI_ASSERT( pTaskToRunOnCompletion ); + ENKI_ASSERT( pDependencyTask->GetIsComplete() ); + ENKI_ASSERT( pTaskToRunOnCompletion->GetIsComplete() ); + ENKI_ASSERT( pTaskToRunOnCompletion->m_DependenciesCount > 0 ); + Dependency* pDependent = pDependencyTask->m_pDependents; + --pTaskToRunOnCompletion->m_DependenciesCount; + if( this == pDependent ) + { + pDependencyTask->m_pDependents = pDependent->pNext; + } + else + { + while( pDependent ) + { + Dependency* pPrev = pDependent; + pDependent = pDependent->pNext; + if( this == pDependent ) + { + pPrev->pNext = pDependent->pNext; + break; + } + } + } + } + pDependencyTask = NULL; + pDependencyTask = NULL; + pNext = NULL; +} diff --git a/tests/cpp-tests/Source/Box2DTestBed/samples/TaskScheduler.h b/tests/cpp-tests/Source/Box2DTestBed/samples/TaskScheduler.h new file mode 100644 index 000000000000..9d1d962ff7f5 --- /dev/null +++ b/tests/cpp-tests/Source/Box2DTestBed/samples/TaskScheduler.h @@ -0,0 +1,583 @@ +// Copyright (c) 2013 Doug Binks +// +// This software is provided 'as-is', without any express or implied +// warranty. In no event will the authors be held liable for any damages +// arising from the use of this software. +// +// Permission is granted to anyone to use this software for any purpose, +// including commercial applications, and to alter it and redistribute it +// freely, subject to the following restrictions: +// +// 1. The origin of this software must not be misrepresented; you must not +// claim that you wrote the original software. If you use this software +// in a product, an acknowledgement in the product documentation would be +// appreciated but is not required. +// 2. Altered source versions must be plainly marked as such, and must not be +// misrepresented as being the original software. +// 3. This notice may not be removed or altered from any source distribution. + +#pragma once + +#include +#include +#include +#include +#include + +// ENKITS_TASK_PRIORITIES_NUM can be set from 1 to 5. +// 1 corresponds to effectively no priorities. +#ifndef ENKITS_TASK_PRIORITIES_NUM + #define ENKITS_TASK_PRIORITIES_NUM 3 +#endif + +#ifndef ENKITS_API +#if defined(_WIN32) && defined(ENKITS_BUILD_DLL) + // Building enkiTS as a DLL + #define ENKITS_API __declspec(dllexport) +#elif defined(_WIN32) && defined(ENKITS_DLL) + // Using enkiTS as a DLL + #define ENKITS_API __declspec(dllimport) +#elif defined(__GNUC__) && defined(ENKITS_BUILD_DLL) + // Building enkiTS as a shared library + #define ENKITS_API __attribute__((visibility("default"))) +#else + #define ENKITS_API +#endif +#endif + +// Define ENKI_CUSTOM_ALLOC_FILE_AND_LINE (at project level) to get file and line report in custom allocators, +// this is default in Debug - to turn off define ENKI_CUSTOM_ALLOC_NO_FILE_AND_LINE +#ifndef ENKI_CUSTOM_ALLOC_FILE_AND_LINE +#if defined(_DEBUG ) && !defined(ENKI_CUSTOM_ALLOC_NO_FILE_AND_LINE) +#define ENKI_CUSTOM_ALLOC_FILE_AND_LINE +#endif +#endif + +#ifndef ENKI_ASSERT +#include +#define ENKI_ASSERT(x) assert(x) +#endif + +#if (!defined(_MSVC_LANG) && __cplusplus >= 201402L) || (defined(_MSVC_LANG) && _MSVC_LANG >= 201402L) +#define ENKI_DEPRECATED [[deprecated]] +#else +#define ENKI_DEPRECATED +#endif + +namespace enki +{ + struct TaskSetPartition + { + uint32_t start; + uint32_t end; + }; + + class TaskScheduler; + class TaskPipe; + class PinnedTaskList; + class Dependency; + struct ThreadArgs; + struct ThreadDataStore; + struct SubTaskSet; + struct semaphoreid_t; + + static constexpr uint32_t NO_THREAD_NUM = 0xFFFFFFFF; + + ENKITS_API uint32_t GetNumHardwareThreads(); + + enum TaskPriority + { + TASK_PRIORITY_HIGH = 0, +#if ( ENKITS_TASK_PRIORITIES_NUM > 3 ) + TASK_PRIORITY_MED_HI, +#endif +#if ( ENKITS_TASK_PRIORITIES_NUM > 2 ) + TASK_PRIORITY_MED, +#endif +#if ( ENKITS_TASK_PRIORITIES_NUM > 4 ) + TASK_PRIORITY_MED_LO, +#endif +#if ( ENKITS_TASK_PRIORITIES_NUM > 1 ) + TASK_PRIORITY_LOW, +#endif + TASK_PRIORITY_NUM + }; + + // ICompletable is a base class used to check for completion. + // Can be used with dependencies to wait for their completion. + // Derive from ITaskSet or IPinnedTask for running parallel tasks. + class ICompletable + { + public: + bool GetIsComplete() const { + return 0 == m_RunningCount.load( std::memory_order_acquire ); + } + + virtual ~ICompletable(); + + // Dependency helpers, see Dependencies.cpp + void SetDependency( Dependency& dependency_, const ICompletable* pDependencyTask_ ); + template void SetDependenciesArr( D& dependencyArray_ , const T(&taskArray_)[SIZE] ); + template void SetDependenciesArr( D& dependencyArray_, std::initializer_list taskpList_ ); + template void SetDependenciesArr( D(&dependencyArray_)[SIZE], const T(&taskArray_)[SIZE] ); + template void SetDependenciesArr( D(&dependencyArray_)[SIZE], std::initializer_list taskpList_ ); + template void SetDependenciesVec( D& dependencyVec_, const T(&taskArray_)[SIZE] ); + template void SetDependenciesVec( D& dependencyVec_, std::initializer_list taskpList_ ); + + TaskPriority m_Priority = TASK_PRIORITY_HIGH; + protected: + // Deriving from an ICompletable and overriding OnDependenciesComplete is advanced use. + // If you do override OnDependenciesComplete() call: + // ICompletable::OnDependenciesComplete( pTaskScheduler_, threadNum_ ); + // in your implementation. + virtual void OnDependenciesComplete( TaskScheduler* pTaskScheduler_, uint32_t threadNum_ ); + private: + friend class TaskScheduler; + friend class Dependency; + std::atomic m_RunningCount = {0}; + std::atomic m_DependenciesCompletedCount = {0}; + int32_t m_DependenciesCount = 0; + mutable std::atomic m_WaitingForTaskCount = {0}; + mutable Dependency* m_pDependents = NULL; + }; + + // Subclass ITaskSet to create tasks. + // TaskSets can be re-used, but check completion first. + class ITaskSet : public ICompletable + { + public: + ITaskSet() = default; + ITaskSet( uint32_t setSize_ ) + : m_SetSize( setSize_ ) + {} + + ITaskSet( uint32_t setSize_, uint32_t minRange_ ) + : m_SetSize( setSize_ ) + , m_MinRange( minRange_ ) + , m_RangeToRun(minRange_) + {} + + // Execute range should be overloaded to process tasks. It will be called with a + // range_ where range.start >= 0; range.start < range.end; and range.end < m_SetSize; + // The range values should be mapped so that linearly processing them in order is cache friendly + // i.e. neighbouring values should be close together. + // threadnum_ should not be used for changing processing of data, its intended purpose + // is to allow per-thread data buckets for output. + virtual void ExecuteRange( TaskSetPartition range_, uint32_t threadnum_ ) = 0; + + // Set Size - usually the number of data items to be processed, see ExecuteRange. Defaults to 1 + uint32_t m_SetSize = 1; + + // Min Range - Minimum size of TaskSetPartition range when splitting a task set into partitions. + // Designed for reducing scheduling overhead by preventing set being + // divided up too small. Ranges passed to ExecuteRange will *not* be a multiple of this, + // only attempts to deliver range sizes larger than this most of the time. + // This should be set to a value which results in computation effort of at least 10k + // clock cycles to minimize task scheduler overhead. + // NOTE: The last partition will be smaller than m_MinRange if m_SetSize is not a multiple + // of m_MinRange. + // Also known as grain size in literature. + uint32_t m_MinRange = 1; + + private: + friend class TaskScheduler; + void OnDependenciesComplete( TaskScheduler* pTaskScheduler_, uint32_t threadNum_ ) final; + uint32_t m_RangeToRun = 1; + }; + + // Subclass IPinnedTask to create tasks which can be run on a given thread only. + class IPinnedTask : public ICompletable + { + public: + IPinnedTask() = default; + IPinnedTask( uint32_t threadNum_ ) : threadNum(threadNum_) {} // default is to run a task on main thread + + // IPinnedTask needs to be non-abstract for intrusive list functionality. + // Should never be called as is, should be overridden. + virtual void Execute() { ENKI_ASSERT(false); } + + uint32_t threadNum = 0; // thread to run this pinned task on + std::atomic pNext = {NULL}; + private: + void OnDependenciesComplete( TaskScheduler* pTaskScheduler_, uint32_t threadNum_ ) final; + }; + + // TaskSet - a utility task set for creating tasks based on std::function. + typedef std::function TaskSetFunction; + class TaskSet : public ITaskSet + { + public: + TaskSet() = default; + TaskSet( TaskSetFunction func_ ) : m_Function( std::move(func_) ) {} + TaskSet( uint32_t setSize_, TaskSetFunction func_ ) : ITaskSet( setSize_ ), m_Function( std::move(func_) ) {} + + void ExecuteRange( TaskSetPartition range_, uint32_t threadnum_ ) override { m_Function( range_, threadnum_ ); } + TaskSetFunction m_Function; + }; + + // LambdaPinnedTask - a utility pinned task for creating tasks based on std::func. + typedef std::function PinnedTaskFunction; + class LambdaPinnedTask : public IPinnedTask + { + public: + LambdaPinnedTask() = default; + LambdaPinnedTask( PinnedTaskFunction func_ ) : m_Function( std::move(func_) ) {} + LambdaPinnedTask( uint32_t threadNum_, PinnedTaskFunction func_ ) : IPinnedTask( threadNum_ ), m_Function( std::move(func_) ) {} + + void Execute() override { m_Function(); } + PinnedTaskFunction m_Function; + }; + + class Dependency + { + public: + Dependency() = default; + Dependency( const Dependency& ) = delete; + ENKITS_API Dependency( Dependency&& ) noexcept; + ENKITS_API Dependency( const ICompletable* pDependencyTask_, ICompletable* pTaskToRunOnCompletion_ ); + ENKITS_API ~Dependency(); + + ENKITS_API void SetDependency( const ICompletable* pDependencyTask_, ICompletable* pTaskToRunOnCompletion_ ); + ENKITS_API void ClearDependency(); + ICompletable* GetTaskToRunOnCompletion() { return pTaskToRunOnCompletion; } + const ICompletable* GetDependencyTask() { return pDependencyTask; } + private: + friend class TaskScheduler; friend class ICompletable; + ICompletable* pTaskToRunOnCompletion = NULL; + const ICompletable* pDependencyTask = NULL; + Dependency* pNext = NULL; + }; + + // TaskScheduler implements several callbacks intended for profilers + typedef void (*ProfilerCallbackFunc)( uint32_t threadnum_ ); + struct ProfilerCallbacks + { + ProfilerCallbackFunc threadStart; + ProfilerCallbackFunc threadStop; + ProfilerCallbackFunc waitForNewTaskSuspendStart; // thread suspended waiting for new tasks + ProfilerCallbackFunc waitForNewTaskSuspendStop; // thread unsuspended + ProfilerCallbackFunc waitForTaskCompleteStart; // thread waiting for task completion + ProfilerCallbackFunc waitForTaskCompleteStop; // thread stopped waiting + ProfilerCallbackFunc waitForTaskCompleteSuspendStart; // thread suspended waiting task completion + ProfilerCallbackFunc waitForTaskCompleteSuspendStop; // thread unsuspended + }; + + // Custom allocator, set in TaskSchedulerConfig. Also see ENKI_CUSTOM_ALLOC_FILE_AND_LINE for file_ and line_ + typedef void* (*AllocFunc)( size_t align_, size_t size_, void* userData_, const char* file_, int line_ ); + typedef void (*FreeFunc)( void* ptr_, size_t size_, void* userData_, const char* file_, int line_ ); + ENKITS_API void* DefaultAllocFunc( size_t align_, size_t size_, void* userData_, const char* file_, int line_ ); + ENKITS_API void DefaultFreeFunc( void* ptr_, size_t size_, void* userData_, const char* file_, int line_ ); + struct CustomAllocator + { + AllocFunc alloc = DefaultAllocFunc; + FreeFunc free = DefaultFreeFunc; + void* userData = nullptr; + }; + + // TaskSchedulerConfig - configuration struct for advanced Initialize + struct TaskSchedulerConfig + { + // numTaskThreadsToCreate - Number of tasking threads the task scheduler will create. Must be > 0. + // Defaults to GetNumHardwareThreads()-1 threads as thread which calls initialize is thread 0. + uint32_t numTaskThreadsToCreate = GetNumHardwareThreads()-1; + + // numExternalTaskThreads - Advanced use. Number of external threads which need to use TaskScheduler API. + // See TaskScheduler::RegisterExternalTaskThread() for usage. + // Defaults to 0. The thread used to initialize the TaskScheduler can also use the TaskScheduler API. + // Thus there are (numTaskThreadsToCreate + numExternalTaskThreads + 1) able to use the API, with this + // defaulting to the number of hardware threads available to the system. + uint32_t numExternalTaskThreads = 0; + + ProfilerCallbacks profilerCallbacks = {}; + + CustomAllocator customAllocator; + }; + + class TaskScheduler + { + public: + ENKITS_API TaskScheduler(); + ENKITS_API ~TaskScheduler(); + + // Call an Initialize function before adding tasks. + + // Initialize() will create GetNumHardwareThreads()-1 tasking threads, which is + // sufficient to fill the system when including the main thread. + // Initialize can be called multiple times - it will wait for completion + // before re-initializing. + ENKITS_API void Initialize(); + + // Initialize( numThreadsTotal_ ) + // will create numThreadsTotal_-1 threads, as thread 0 is + // the thread on which the initialize was called. + // numThreadsTotal_ must be > 0 + ENKITS_API void Initialize( uint32_t numThreadsTotal_ ); + + // Initialize with advanced TaskSchedulerConfig settings. See TaskSchedulerConfig. + ENKITS_API void Initialize( TaskSchedulerConfig config_ ); + + // Get config. Can be called before Initialize to get the defaults. + ENKITS_API TaskSchedulerConfig GetConfig() const; + + // while( !GetIsShutdownRequested() ) {} can be used in tasks which loop, to check if enkiTS has been requested to shutdown. + // If GetIsShutdownRequested() returns true should then exit. Not required for finite tasks + // Safe to use with WaitforAllAndShutdown() and ShutdownNow() where this will be set + // Not safe to use with WaitforAll(), use GetIsWaitforAllCalled() instead. + inline bool GetIsShutdownRequested() const { return m_bShutdownRequested.load( std::memory_order_acquire ); } + + // while( !GetIsWaitforAllCalled() ) {} can be used in tasks which loop, to check if WaitforAll() has been called. + // If GetIsWaitforAllCalled() returns false should then exit. Not required for finite tasks + // This is intended to be used with code which calls WaitforAll(). + // This is also set when the task manager is shutting down, so no need to have an additional check for GetIsShutdownRequested() + inline bool GetIsWaitforAllCalled() const { return m_bWaitforAllCalled.load( std::memory_order_acquire ); } + + // Adds the TaskSet to pipe and returns if the pipe is not full. + // If the pipe is full, pTaskSet is run. + // should only be called from main thread, or within a task + ENKITS_API void AddTaskSetToPipe( ITaskSet* pTaskSet_ ); + + // Thread 0 is main thread, otherwise use threadNum + // Pinned tasks can be added from any thread + ENKITS_API void AddPinnedTask( IPinnedTask* pTask_ ); + + // This function will run any IPinnedTask* for current thread, but not run other + // Main thread should call this or use a wait to ensure its tasks are run. + ENKITS_API void RunPinnedTasks(); + + // Runs the TaskSets in pipe until true == pTaskSet->GetIsComplete(); + // Should only be called from thread which created the task scheduler, or within a task. + // If called with 0 it will try to run tasks, and return if none are available. + // To run only a subset of tasks, set priorityOfLowestToRun_ to a high priority. + // Default is lowest priority available. + // Only wait for child tasks of the current task otherwise a deadlock could occur. + // WaitforTask will exit if ShutdownNow() is called even if pCompletable_ is not complete. + ENKITS_API void WaitforTask( const ICompletable* pCompletable_, enki::TaskPriority priorityOfLowestToRun_ = TaskPriority(TASK_PRIORITY_NUM - 1) ); + + // Waits for all task sets to complete - not guaranteed to work unless we know we + // are in a situation where tasks aren't being continuously added. + // If you are running tasks which loop, make sure to check GetIsWaitforAllCalled() and exit + // WaitforAll will exit if ShutdownNow() is called even if there are still tasks to run or currently running + ENKITS_API void WaitforAll(); + + // Waits for all task sets to complete and shutdown threads - not guaranteed to work unless we know we + // are in a situation where tasks aren't being continuously added. + // This function can be safely called even if TaskScheduler::Initialize() has not been called. + ENKITS_API void WaitforAllAndShutdown(); + + // Shutdown threads without waiting for all tasks to complete. + // Intended to be used to exit an application quickly. + // This function can be safely called even if TaskScheduler::Initialize() has not been called. + // This function will still wait for any running tasks to exit before the task threads exit. + // ShutdownNow will cause tasks which have been added to the scheduler but not completed + // to be in an undefined state in which should not be re-launched. + ENKITS_API void ShutdownNow(); + + // Waits for the current thread to receive a PinnedTask. + // Will not run any tasks - use with RunPinnedTasks(). + // Can be used with both ExternalTaskThreads or with an enkiTS tasking thread to create + // a thread which only runs pinned tasks. If enkiTS threads are used can create + // extra enkiTS task threads to handle non-blocking computation via normal tasks. + ENKITS_API void WaitForNewPinnedTasks(); + + // Returns the number of threads created for running tasks + number of external threads + // plus 1 to account for the thread used to initialize the task scheduler. + // Equivalent to config values: numTaskThreadsToCreate + numExternalTaskThreads + 1. + // It is guaranteed that GetThreadNum() < GetNumTaskThreads() + ENKITS_API uint32_t GetNumTaskThreads() const; + + // Returns the current task threadNum. + // Will return 0 for thread which initialized the task scheduler, + // and NO_THREAD_NUM for all other non-enkiTS threads which have not been registered ( see RegisterExternalTaskThread() ), + // and < GetNumTaskThreads() for all registered and internal enkiTS threads. + // It is guaranteed that GetThreadNum() < GetNumTaskThreads() unless it is NO_THREAD_NUM + ENKITS_API uint32_t GetThreadNum() const; + + // Call on a thread to register the thread to use the TaskScheduling API. + // This is implicitly done for the thread which initializes the TaskScheduler + // Intended for developers who have threads who need to call the TaskScheduler API + // Returns true if successful, false if not. + // Can only have numExternalTaskThreads registered at any one time, which must be set + // at initialization time. + ENKITS_API bool RegisterExternalTaskThread(); + + // As RegisterExternalTaskThread() but explicitly requests a given thread number. + // threadNumToRegister_ must be >= GetNumFirstExternalTaskThread() + // and < ( GetNumFirstExternalTaskThread() + numExternalTaskThreads ). + ENKITS_API bool RegisterExternalTaskThread( uint32_t threadNumToRegister_ ); + + // Call on a thread on which RegisterExternalTaskThread has been called to deregister that thread. + ENKITS_API void DeRegisterExternalTaskThread(); + + // Get the number of registered external task threads. + ENKITS_API uint32_t GetNumRegisteredExternalTaskThreads(); + + // Get the thread number of the first external task thread. This thread + // is not guaranteed to be registered, but threads are registered in order + // from GetNumFirstExternalTaskThread() up to ( GetNumFirstExternalTaskThread() + numExternalTaskThreads ) + // Note that if numExternalTaskThreads == 0 a for loop using this will be valid: + // for( uint32_t externalThreadNum = GetNumFirstExternalTaskThread(); + // externalThreadNum < ( GetNumFirstExternalTaskThread() + numExternalTaskThreads + // ++externalThreadNum ) { // do something with externalThreadNum } + inline static constexpr uint32_t GetNumFirstExternalTaskThread() { return 1; } + + // ------------- Start DEPRECATED Functions ------------- + // DEPRECATED: use GetIsShutdownRequested() instead of GetIsRunning() in external code + // while( GetIsRunning() ) {} can be used in tasks which loop, to check if enkiTS has been shutdown. + // If GetIsRunning() returns false should then exit. Not required for finite tasks. + ENKI_DEPRECATED inline bool GetIsRunning() const { return !GetIsShutdownRequested(); } + + // DEPRECATED - WaitforTaskSet, deprecated interface use WaitforTask. + ENKI_DEPRECATED inline void WaitforTaskSet( const ICompletable* pCompletable_ ) { WaitforTask( pCompletable_ ); } + + // DEPRECATED - GetProfilerCallbacks. Use TaskSchedulerConfig instead. + // Returns the ProfilerCallbacks structure so that it can be modified to + // set the callbacks. Should be set prior to initialization. + ENKI_DEPRECATED inline ProfilerCallbacks* GetProfilerCallbacks() { return &m_Config.profilerCallbacks; } + // ------------- End DEPRECATED Functions ------------- + + private: + friend class ICompletable; friend class ITaskSet; friend class IPinnedTask; + static void TaskingThreadFunction( const ThreadArgs& args_ ); + bool HaveTasks( uint32_t threadNum_ ); + void WaitForNewTasks( uint32_t threadNum_ ); + void WaitForTaskCompletion( const ICompletable* pCompletable_, uint32_t threadNum_ ); + void RunPinnedTasks( uint32_t threadNum_, uint32_t priority_ ); + bool TryRunTask( uint32_t threadNum_, uint32_t& hintPipeToCheck_io_ ); + bool TryRunTask( uint32_t threadNum_, uint32_t priority_, uint32_t& hintPipeToCheck_io_ ); + void StartThreads(); + void StopThreads( bool bWait_ ); + void SplitAndAddTask( uint32_t threadNum_, SubTaskSet subTask_, uint32_t rangeToSplit_ ); + void WakeThreadsForNewTasks(); + void WakeThreadsForTaskCompletion(); + bool WakeSuspendedThreadsWithPinnedTasks( uint32_t threadNum_ ); + void InitDependencies( ICompletable* pCompletable_ ); + inline bool GetIsRunningInt() const { return m_bRunning.load( std::memory_order_acquire ); } + + ENKITS_API void TaskComplete( ICompletable* pTask_, bool bWakeThreads_, uint32_t threadNum_ ); + ENKITS_API void AddTaskSetToPipeInt( ITaskSet* pTaskSet_, uint32_t threadNum_ ); + ENKITS_API void AddPinnedTaskInt( IPinnedTask* pTask_ ); + + template< typename T > T* NewArray( size_t num_, const char* file_, int line_ ); + template< typename T > void DeleteArray( T* p_, size_t num_, const char* file_, int line_ ); + template T* New( const char* file_, int line_, Args&&... args_ ); + template< typename T > void Delete( T* p_, const char* file_, int line_ ); + template< typename T > T* Alloc( const char* file_, int line_ ); + template< typename T > void Free( T* p_, const char* file_, int line_ ); + semaphoreid_t* SemaphoreNew(); + void SemaphoreDelete( semaphoreid_t* pSemaphore_ ); + + TaskPipe* m_pPipesPerThread[ TASK_PRIORITY_NUM ]; + PinnedTaskList* m_pPinnedTaskListPerThread[ TASK_PRIORITY_NUM ]; + + uint32_t m_NumThreads; + ThreadDataStore* m_pThreadDataStore; + std::thread* m_pThreads; + std::atomic m_bRunning; + std::atomic m_bShutdownRequested; + std::atomic m_bWaitforAllCalled; + std::atomic m_NumInternalTaskThreadsRunning; + std::atomic m_NumThreadsWaitingForNewTasks; + std::atomic m_NumThreadsWaitingForTaskCompletion; + uint32_t m_NumPartitions; + semaphoreid_t* m_pNewTaskSemaphore; + semaphoreid_t* m_pTaskCompleteSemaphore; + uint32_t m_NumInitialPartitions; + bool m_bHaveThreads; + TaskSchedulerConfig m_Config; + std::atomic m_NumExternalTaskThreadsRegistered; + + TaskScheduler( const TaskScheduler& nocopy_ ); + TaskScheduler& operator=( const TaskScheduler& nocopy_ ); + + protected: + void SetCustomAllocator( CustomAllocator customAllocator_ ); // for C interface + }; + + inline void ICompletable::OnDependenciesComplete( TaskScheduler* pTaskScheduler_, uint32_t threadNum_ ) + { + m_RunningCount.fetch_sub( 1, std::memory_order_acq_rel ); + pTaskScheduler_->TaskComplete( this, true, threadNum_ ); + } + + inline void ITaskSet::OnDependenciesComplete( TaskScheduler* pTaskScheduler_, uint32_t threadNum_ ) + { + pTaskScheduler_->AddTaskSetToPipeInt( this, threadNum_ ); + } + + inline void IPinnedTask::OnDependenciesComplete( TaskScheduler* pTaskScheduler_, uint32_t threadNum_ ) + { + (void)threadNum_; + pTaskScheduler_->AddPinnedTaskInt( this ); + } + + inline ICompletable::~ICompletable() + { + ENKI_ASSERT( GetIsComplete() ); // this task is still waiting to run + Dependency* pDependency = m_pDependents; + while( pDependency ) + { + Dependency* pNext = pDependency->pNext; + pDependency->pDependencyTask = NULL; + pDependency->pNext = NULL; + pDependency = pNext; + } + } + + inline void ICompletable::SetDependency( Dependency& dependency_, const ICompletable* pDependencyTask_ ) + { + ENKI_ASSERT( pDependencyTask_ != this ); + dependency_.SetDependency( pDependencyTask_, this ); + } + + template + void ICompletable::SetDependenciesArr( D& dependencyArray_ , const T(&taskArray_)[SIZE] ) { + static_assert( std::tuple_size::value >= SIZE, "Size of dependency array too small" ); + for( int i = 0; i < SIZE; ++i ) + { + dependencyArray_[i].SetDependency( &taskArray_[i], this ); + } + } + template + void ICompletable::SetDependenciesArr( D& dependencyArray_, std::initializer_list taskpList_ ) { + ENKI_ASSERT( std::tuple_size::value >= taskpList_.size() ); + int i = 0; + for( auto pTask : taskpList_ ) + { + dependencyArray_[i++].SetDependency( pTask, this ); + } + } + template + void ICompletable::SetDependenciesArr( D(&dependencyArray_)[SIZE], const T(&taskArray_)[SIZE] ) { + for( int i = 0; i < SIZE; ++i ) + { + dependencyArray_[i].SetDependency( &taskArray_[i], this ); + } + } + template + void ICompletable::SetDependenciesArr( D(&dependencyArray_)[SIZE], std::initializer_list taskpList_ ) { + ENKI_ASSERT( SIZE >= taskpList_.size() ); + int i = 0; + for( auto pTask : taskpList_ ) + { + dependencyArray_[i++].SetDependency( pTask, this ); + } + } + template + void ICompletable::SetDependenciesVec( D& dependencyVec_, const T(&taskArray_)[SIZE] ) { + dependencyVec_.resize( SIZE ); + for( int i = 0; i < SIZE; ++i ) + { + dependencyVec_[i].SetDependency( &taskArray_[i], this ); + } + } + + template + void ICompletable::SetDependenciesVec( D& dependencyVec_, std::initializer_list taskpList_ ) { + dependencyVec_.resize( taskpList_.size() ); + int i = 0; + for( auto pTask : taskpList_ ) + { + dependencyVec_[i++].SetDependency( pTask, this ); + } + } +} diff --git a/tests/cpp-tests/Source/Box2DTestBed/samples/car.cpp b/tests/cpp-tests/Source/Box2DTestBed/samples/car.cpp new file mode 100644 index 000000000000..e54679e0cf32 --- /dev/null +++ b/tests/cpp-tests/Source/Box2DTestBed/samples/car.cpp @@ -0,0 +1,295 @@ +// SPDX-FileCopyrightText: 2022 Erin Catto +// SPDX-License-Identifier: MIT + +#include "car.h" + +#include "box2d/box2d.h" +#include "box2d/math_functions.h" + +#include + +Car::Car() +{ + m_chassisId = {}; + m_rearWheelId = {}; + m_frontWheelId = {}; + m_rearAxleId = {}; + m_frontAxleId = {}; + m_isSpawned = false; +} + +void Car::Spawn( b2WorldId worldId, b2Vec2 position, float scale, float hertz, float dampingRatio, float torque, void* userData ) +{ + assert( m_isSpawned == false ); + + assert( B2_IS_NULL( m_chassisId ) ); + assert( B2_IS_NULL( m_frontWheelId ) ); + assert( B2_IS_NULL( m_rearWheelId ) ); + + b2Vec2 vertices[6] = { + { -1.5f, -0.5f }, { 1.5f, -0.5f }, { 1.5f, 0.0f }, { 0.0f, 0.9f }, { -1.15f, 0.9f }, { -1.5f, 0.2f }, + }; + + for ( int i = 0; i < 6; ++i ) + { + vertices[i].x *= 0.85f * scale; + vertices[i].y *= 0.85f * scale; + } + + b2Hull hull = b2ComputeHull( vertices, 6 ); + b2Polygon chassis = b2MakePolygon( &hull, 0.15f * scale ); + + b2ShapeDef shapeDef = b2DefaultShapeDef(); + shapeDef.density = 1.0f / scale; + shapeDef.friction = 0.2f; + + b2Circle circle = { { 0.0f, 0.0f }, 0.4f * scale }; + + b2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.type = b2_dynamicBody; + bodyDef.position = b2Add( { 0.0f, 1.0f * scale }, position ); + m_chassisId = b2CreateBody( worldId, &bodyDef ); + b2CreatePolygonShape( m_chassisId, &shapeDef, &chassis ); + + shapeDef.density = 2.0f / scale; + shapeDef.friction = 1.5f; + + bodyDef.position = b2Add( { -1.0f * scale, 0.35f * scale }, position ); + bodyDef.allowFastRotation = true; + m_rearWheelId = b2CreateBody( worldId, &bodyDef ); + b2CreateCircleShape( m_rearWheelId, &shapeDef, &circle ); + + bodyDef.position = b2Add( { 1.0f * scale, 0.4f * scale }, position ); + bodyDef.allowFastRotation = true; + m_frontWheelId = b2CreateBody( worldId, &bodyDef ); + b2CreateCircleShape( m_frontWheelId, &shapeDef, &circle ); + + b2Vec2 axis = { 0.0f, 1.0f }; + b2Vec2 pivot = b2Body_GetPosition( m_rearWheelId ); + + // float throttle = 0.0f; + // float speed = 35.0f; + // float torque = 2.5f * scale; + // float hertz = 5.0f; + // float dampingRatio = 0.7f; + + b2WheelJointDef jointDef = b2DefaultWheelJointDef(); + + jointDef.bodyIdA = m_chassisId; + jointDef.bodyIdB = m_rearWheelId; + jointDef.localAxisA = b2Body_GetLocalVector( jointDef.bodyIdA, axis ); + jointDef.localAnchorA = b2Body_GetLocalPoint( jointDef.bodyIdA, pivot ); + jointDef.localAnchorB = b2Body_GetLocalPoint( jointDef.bodyIdB, pivot ); + jointDef.motorSpeed = 0.0f; + jointDef.maxMotorTorque = torque; + jointDef.enableMotor = true; + jointDef.hertz = hertz; + jointDef.dampingRatio = dampingRatio; + jointDef.lowerTranslation = -0.25f * scale; + jointDef.upperTranslation = 0.25f * scale; + jointDef.enableLimit = true; + m_rearAxleId = b2CreateWheelJoint( worldId, &jointDef ); + + pivot = b2Body_GetPosition( m_frontWheelId ); + jointDef.bodyIdA = m_chassisId; + jointDef.bodyIdB = m_frontWheelId; + jointDef.localAxisA = b2Body_GetLocalVector( jointDef.bodyIdA, axis ); + jointDef.localAnchorA = b2Body_GetLocalPoint( jointDef.bodyIdA, pivot ); + jointDef.localAnchorB = b2Body_GetLocalPoint( jointDef.bodyIdB, pivot ); + jointDef.motorSpeed = 0.0f; + jointDef.maxMotorTorque = torque; + jointDef.enableMotor = true; + jointDef.hertz = hertz; + jointDef.dampingRatio = dampingRatio; + jointDef.lowerTranslation = -0.25f * scale; + jointDef.upperTranslation = 0.25f * scale; + jointDef.enableLimit = true; + m_frontAxleId = b2CreateWheelJoint( worldId, &jointDef ); +} + +void Car::Despawn() +{ + assert( m_isSpawned == true ); + + b2DestroyJoint( m_rearAxleId ); + b2DestroyJoint( m_frontAxleId ); + b2DestroyBody( m_rearWheelId ); + b2DestroyBody( m_frontWheelId ); + b2DestroyBody( m_chassisId ); + + m_isSpawned = false; +} + +void Car::SetSpeed( float speed ) +{ + b2WheelJoint_SetMotorSpeed( m_rearAxleId, speed ); + b2WheelJoint_SetMotorSpeed( m_frontAxleId, speed ); + b2Joint_WakeBodies( m_rearAxleId ); +} + +void Car::SetTorque( float torque ) +{ + b2WheelJoint_SetMaxMotorTorque( m_rearAxleId, torque ); + b2WheelJoint_SetMaxMotorTorque( m_frontAxleId, torque ); +} + +void Car::SetHertz( float hertz ) +{ + b2WheelJoint_SetSpringHertz( m_rearAxleId, hertz ); + b2WheelJoint_SetSpringHertz( m_frontAxleId, hertz ); +} + +void Car::SetDampingRadio( float dampingRatio ) +{ + b2WheelJoint_SetSpringDampingRatio( m_rearAxleId, dampingRatio ); + b2WheelJoint_SetSpringDampingRatio( m_frontAxleId, dampingRatio ); +} + +Truck::Truck() +{ + m_chassisId = {}; + m_rearWheelId = {}; + m_frontWheelId = {}; + m_rearAxleId = {}; + m_frontAxleId = {}; + m_isSpawned = false; +} + +void Truck::Spawn( b2WorldId worldId, b2Vec2 position, float scale, float hertz, float dampingRatio, float torque, float density, + void* userData ) +{ + assert( m_isSpawned == false ); + + assert( B2_IS_NULL( m_chassisId ) ); + assert( B2_IS_NULL( m_frontWheelId ) ); + assert( B2_IS_NULL( m_rearWheelId ) ); + + // b2Vec2 vertices[6] = { + // { -1.5f, -0.5f }, { 1.5f, -0.5f }, { 1.5f, 0.0f }, { 0.0f, 0.9f }, { -1.15f, 0.9f }, { -1.5f, 0.2f }, + // }; + + b2Vec2 vertices[5] = { + { -0.65f, -0.4f }, { 1.5f, -0.4f }, { 1.5f, 0.0f }, { 0.0f, 0.9f }, { -0.65f, 0.9f }, + }; + + for ( int i = 0; i < 5; ++i ) + { + vertices[i].x *= 0.85f * scale; + vertices[i].y *= 0.85f * scale; + } + + b2Hull hull = b2ComputeHull( vertices, 5 ); + b2Polygon chassis = b2MakePolygon( &hull, 0.15f * scale ); + + b2ShapeDef shapeDef = b2DefaultShapeDef(); + shapeDef.density = density; + shapeDef.friction = 0.2f; + shapeDef.customColor = b2_colorHotPink; + + b2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.type = b2_dynamicBody; + bodyDef.position = b2Add( { 0.0f, 1.0f * scale }, position ); + m_chassisId = b2CreateBody( worldId, &bodyDef ); + b2CreatePolygonShape( m_chassisId, &shapeDef, &chassis ); + + b2Polygon box = b2MakeOffsetBox( 1.25f * scale, 0.1f * scale, { -2.05f * scale, -0.275f * scale }, b2Rot_identity ); + box.radius = 0.1f * scale; + b2CreatePolygonShape( m_chassisId, &shapeDef, &box ); + + box = b2MakeOffsetBox( 0.05f * scale, 0.35f * scale, { -3.25f * scale, 0.375f * scale }, b2Rot_identity ); + box.radius = 0.1f * scale; + b2CreatePolygonShape( m_chassisId, &shapeDef, &box ); + + shapeDef.density = 2.0f * density; + shapeDef.friction = 2.5f; + shapeDef.customColor = b2_colorSilver; + + b2Circle circle = { { 0.0f, 0.0f }, 0.4f * scale }; + bodyDef.position = b2Add( { -2.75f * scale, 0.3f * scale }, position ); + m_rearWheelId = b2CreateBody( worldId, &bodyDef ); + b2CreateCircleShape( m_rearWheelId, &shapeDef, &circle ); + + bodyDef.position = b2Add( { 0.8f * scale, 0.3f * scale }, position ); + m_frontWheelId = b2CreateBody( worldId, &bodyDef ); + b2CreateCircleShape( m_frontWheelId, &shapeDef, &circle ); + + b2Vec2 axis = { 0.0f, 1.0f }; + b2Vec2 pivot = b2Body_GetPosition( m_rearWheelId ); + + // float throttle = 0.0f; + // float speed = 35.0f; + // float torque = 2.5f * scale; + // float hertz = 5.0f; + // float dampingRatio = 0.7f; + + b2WheelJointDef jointDef = b2DefaultWheelJointDef(); + + jointDef.bodyIdA = m_chassisId; + jointDef.bodyIdB = m_rearWheelId; + jointDef.localAxisA = b2Body_GetLocalVector( jointDef.bodyIdA, axis ); + jointDef.localAnchorA = b2Body_GetLocalPoint( jointDef.bodyIdA, pivot ); + jointDef.localAnchorB = b2Body_GetLocalPoint( jointDef.bodyIdB, pivot ); + jointDef.motorSpeed = 0.0f; + jointDef.maxMotorTorque = torque; + jointDef.enableMotor = true; + jointDef.hertz = hertz; + jointDef.dampingRatio = dampingRatio; + jointDef.lowerTranslation = -0.25f * scale; + jointDef.upperTranslation = 0.25f * scale; + jointDef.enableLimit = true; + m_rearAxleId = b2CreateWheelJoint( worldId, &jointDef ); + + pivot = b2Body_GetPosition( m_frontWheelId ); + jointDef.bodyIdA = m_chassisId; + jointDef.bodyIdB = m_frontWheelId; + jointDef.localAxisA = b2Body_GetLocalVector( jointDef.bodyIdA, axis ); + jointDef.localAnchorA = b2Body_GetLocalPoint( jointDef.bodyIdA, pivot ); + jointDef.localAnchorB = b2Body_GetLocalPoint( jointDef.bodyIdB, pivot ); + jointDef.motorSpeed = 0.0f; + jointDef.maxMotorTorque = torque; + jointDef.enableMotor = true; + jointDef.hertz = hertz; + jointDef.dampingRatio = dampingRatio; + jointDef.lowerTranslation = -0.25f * scale; + jointDef.upperTranslation = 0.25f * scale; + jointDef.enableLimit = true; + m_frontAxleId = b2CreateWheelJoint( worldId, &jointDef ); +} + +void Truck::Despawn() +{ + assert( m_isSpawned == true ); + + b2DestroyJoint( m_rearAxleId ); + b2DestroyJoint( m_frontAxleId ); + b2DestroyBody( m_rearWheelId ); + b2DestroyBody( m_frontWheelId ); + b2DestroyBody( m_chassisId ); + + m_isSpawned = false; +} + +void Truck::SetSpeed( float speed ) +{ + b2WheelJoint_SetMotorSpeed( m_rearAxleId, speed ); + b2WheelJoint_SetMotorSpeed( m_frontAxleId, speed ); + b2Joint_WakeBodies( m_rearAxleId ); +} + +void Truck::SetTorque( float torque ) +{ + b2WheelJoint_SetMaxMotorTorque( m_rearAxleId, torque ); + b2WheelJoint_SetMaxMotorTorque( m_frontAxleId, torque ); +} + +void Truck::SetHertz( float hertz ) +{ + b2WheelJoint_SetSpringHertz( m_rearAxleId, hertz ); + b2WheelJoint_SetSpringHertz( m_frontAxleId, hertz ); +} + +void Truck::SetDampingRadio( float dampingRatio ) +{ + b2WheelJoint_SetSpringDampingRatio( m_rearAxleId, dampingRatio ); + b2WheelJoint_SetSpringDampingRatio( m_frontAxleId, dampingRatio ); +} diff --git a/tests/cpp-tests/Source/Box2DTestBed/samples/car.h b/tests/cpp-tests/Source/Box2DTestBed/samples/car.h new file mode 100644 index 000000000000..8013d1c051dc --- /dev/null +++ b/tests/cpp-tests/Source/Box2DTestBed/samples/car.h @@ -0,0 +1,49 @@ +// SPDX-FileCopyrightText: 2023 Erin Catto +// SPDX-License-Identifier: MIT + +#pragma once + +#include "box2d/types.h" + +class Car +{ +public: + Car(); + + void Spawn( b2WorldId worldId, b2Vec2 position, float scale, float hertz, float dampingRatio, float torque, void* userData ); + void Despawn(); + + void SetSpeed( float speed ); + void SetTorque( float torque ); + void SetHertz( float hertz ); + void SetDampingRadio( float dampingRatio ); + + b2BodyId m_chassisId; + b2BodyId m_rearWheelId; + b2BodyId m_frontWheelId; + b2JointId m_rearAxleId; + b2JointId m_frontAxleId; + bool m_isSpawned; +}; + +class Truck +{ +public: + Truck(); + + void Spawn( b2WorldId worldId, b2Vec2 position, float scale, float hertz, float dampingRatio, float torque, float density, + void* userData ); + void Despawn(); + + void SetSpeed( float speed ); + void SetTorque( float torque ); + void SetHertz( float hertz ); + void SetDampingRadio( float dampingRatio ); + + b2BodyId m_chassisId; + b2BodyId m_rearWheelId; + b2BodyId m_frontWheelId; + b2JointId m_rearAxleId; + b2JointId m_frontAxleId; + bool m_isSpawned; +}; diff --git a/tests/cpp-tests/Source/Box2DTestBed/samples/data/background.fs b/tests/cpp-tests/Source/Box2DTestBed/samples/data/background.fs new file mode 100644 index 000000000000..bd0a8e4141a2 --- /dev/null +++ b/tests/cpp-tests/Source/Box2DTestBed/samples/data/background.fs @@ -0,0 +1,33 @@ +// SPDX-FileCopyrightText: 2024 Erin Catto +// SPDX-License-Identifier: MIT + +#version 330 + +out vec4 FragColor; + +uniform float time; +uniform vec2 resolution; +uniform vec3 baseColor; + +// A simple pseudo-random function +float random(vec2 st) +{ + return fract(sin(dot(st.xy, vec2(12.9898, 78.233))) * 43758.5453123); +} + +void main() +{ + vec2 uv = gl_FragCoord.xy / resolution.xy; + + // Create some noise + float noise = random(uv + time * 0.1); + + // Adjust these values to control the intensity and color of the grain + float grainIntensity = 0.03; + + // Mix the base color with the noise + vec3 color = baseColor + vec3(noise * grainIntensity); + + FragColor = vec4(color, 1.0); +} + diff --git a/tests/cpp-tests/Source/Box2DTestBed/samples/data/background.vs b/tests/cpp-tests/Source/Box2DTestBed/samples/data/background.vs new file mode 100644 index 000000000000..1ea9a85021e4 --- /dev/null +++ b/tests/cpp-tests/Source/Box2DTestBed/samples/data/background.vs @@ -0,0 +1,10 @@ +// SPDX-FileCopyrightText: 2024 Erin Catto +// SPDX-License-Identifier: MIT +#version 330 + +layout(location = 0) in vec2 v_position; + +void main(void) +{ + gl_Position = vec4(v_position, 0.0f, 1.0f); +} diff --git a/tests/cpp-tests/Source/Box2DTestBed/samples/data/circle.fs b/tests/cpp-tests/Source/Box2DTestBed/samples/data/circle.fs new file mode 100644 index 000000000000..7586a2a8bc85 --- /dev/null +++ b/tests/cpp-tests/Source/Box2DTestBed/samples/data/circle.fs @@ -0,0 +1,23 @@ +// SPDX-FileCopyrightText: 2024 Erin Catto +// SPDX-License-Identifier: MIT + +#version 330 + +in vec2 f_position; +in vec4 f_color; +in float f_thickness; + +out vec4 fragColor; + +void main() +{ + // radius in unit quad + float radius = 1.0; + + // distance to circle + vec2 w = f_position; + float dw = length(w); + float d = abs(dw - radius); + + fragColor = vec4(f_color.rgb, smoothstep(f_thickness, 0.0, d)); +} diff --git a/tests/cpp-tests/Source/Box2DTestBed/samples/data/circle.vs b/tests/cpp-tests/Source/Box2DTestBed/samples/data/circle.vs new file mode 100644 index 000000000000..df10674bce0a --- /dev/null +++ b/tests/cpp-tests/Source/Box2DTestBed/samples/data/circle.vs @@ -0,0 +1,29 @@ +// SPDX-FileCopyrightText: 2024 Erin Catto +// SPDX-License-Identifier: MIT + +#version 330 + +uniform mat4 projectionMatrix; +uniform float pixelScale; + +layout(location = 0) in vec2 v_localPosition; +layout(location = 1) in vec2 v_instancePosition; +layout(location = 2) in float v_instanceRadius; +layout(location = 3) in vec4 v_instanceColor; + +out vec2 f_position; +out vec4 f_color; +out float f_thickness; + +void main() +{ + f_position = v_localPosition; + f_color = v_instanceColor; + float radius = v_instanceRadius; + + // resolution.y = pixelScale * radius + f_thickness = 3.0f / (pixelScale * radius); + + vec2 p = vec2(radius * v_localPosition.x, radius * v_localPosition.y) + v_instancePosition; + gl_Position = projectionMatrix * vec4(p, 0.0f, 1.0f); +} diff --git a/tests/cpp-tests/Source/Box2DTestBed/samples/data/droid_sans.ttf b/tests/cpp-tests/Source/Box2DTestBed/samples/data/droid_sans.ttf new file mode 100644 index 0000000000000000000000000000000000000000..767c63ad000e3eea20f3cb7a43ba9f4154ed7a5d GIT binary patch literal 190044 zcmeFaX<$@Uwl=)?IaR0Ts#9|n2}vcDk%S~91QLd%2uTQGp2H9bn1q>t0TC5J5fK~_ z1Vuy|(MF}AKpRnUpl!to(6+sOZ`-z{yKS%CinQ(22$k92#39~wDvLfsn=K8vzyGGp?sV7OH;@L}2Ie)u{vB}pm zX4rMrHOoWgH&%>fY@QWmF0?M4ySV+maFnr`0p#oF&Td}{It=K~gZkFFi&nM%YRhZm z7+abLV+W;g%+HFGK2B6|AG!vpIC6Y}_S+-J;Ny!_hV z{H`FIF$wM4b&J}rntkv2T=c)di2jttv#(vM^pe)2{QW2&S~7ca%P*(iGnq-jBF2CJygb2HWwLYtp4;vM#xJ%4$8mSfP@EQn^2j_)WD z=hu?%kiXw}@qN=Y;~YFtm?R#HI)=ruo{X8Azuy?!W11#fif7VEUQ18po7ryWW7jb~ zYO1V|O+&A#yqw9%m30sBdyr$)@6e+=o}!)QcUUX`66Gw4A{%v*q&vf;FCr{79nI!Y z+kJ*i7|QUV^P-X(^RQbCi}{~Jj6Y0yx_bQ)oI*^}on*(w6G%d$J)3Qj`?K}>JXWHc z$F5g)uvYypwwRx0>!o>YJodqOJ|253hFK|X!Lv-ZMY_N|cs2+7VeC!Vsf-`{I_%4^ zPs6@|c2Pc)RpPoevCH#VrgD}o)_n--zGEkJ@3GbTRXCTk6Y^PhLcbpOJJ|{82>Q$G ztkdn$o-5xV?|tm&*lHc}^=ENDih9{>9(Ip@8#}6lrW#zJuL*7U$37NqEM|K_vmfUY zT@}lf>pL&#cJYn6N;Xp$We4P^L30#)9a|>tVLf%3EK_%c9gtjXx8&+PCVwZc4;b>; z0eY_6CCVS5I&vS>-^r$d)->epmEUJf{~h{gEG&P@%s}3F>3!lO{}gB9b8CY4xITux z8vFIwdr}$s&ukTFD^d2bX6Z3jEq@&6KX^~ibgc8dyjYZPMSdUbsbVb0*#Z4v&=2|W zy)2C9mC{L8h58fqAG1pA`Pfsz`)a{=;{6r7@@{90WI=N1zGG|^LKa7u6XytIRhWHHewd<486jPu`dF2ebbLIy5Y9X}t?bwD>Vp`*YE@{b9eKvzj$2}j`B zlkszF{H#5175P0;=Npov==;iZZD(g!9^nl<7P#wnHl(uE1{yywNH~a}o8srI373S8 ze{=R=ENO(@?q@yW81nnyo(b;)bHKZha~$`CJ;HzYvosQP&SP&SWKTLzdKVvCdl#*{ zxCC5Dx)nSL=QOefI4|bc3%ntZeT4h(uzPeLc795@)4kF8iQ@14L_UmjRp%%Ai8$AH zej@Ga{EO%hwvOZg8J>`=bE7thF+ZGg={Ino)Ozw=8S?1(N5 zK2ko(+RTDD?-lp3lk)w{MQuVh%XED@KhuB4_9}{K!z=nCEFzzt(HQl+nMaJ7>S?TmOB!#SU+{%?YD-zqbh;U=4tn_p?Nmn(|0(zt=cA}k zb+!IHI<%SKm3{}x*RjV8=h+F`VJ|((Hk76E$Jsz*Pn?fpykr~hW-D|X!Lt(Zqlx7b z&h@V_oqPcH^HV&-j&U8(&19YMD5@GqJ#I!icL)-hH27-gvpXb^IR+=$OqCxM46 z&!iusEMwyl)rg&ZA#Y`V+R4Y`9F#u6HFo(XRtbM}J#^!GSz_CiHns(EO)JF*6gNnt zAxrG?H}E;;*ly`N;qxgTs8$%;hI6F?e}J*CG<*Vn$6y1YtGcgPt72ypu$N#TgMFFq zF~k$MgCFGot*jn(TXoGSzmKhygG?3o-C_vc6Nn$4hR>vZ7dtKNALAQzjJIK54gZ&k z-HLr+{JIcl@|S6#hf!3QI7^5=F?Pl$;n_S~XX1JX9-PD;Pbfc;cIth08xP&v z&0M5^A~uNo6QwHlp7aE3hb*=jCbOQj6OKXiGW}QZiG^%0V;Amb>|!0xwK#*6i+u!M zNUur$&@aOWY&Ce!^qIi?hs20nfX{v>FFJtUk7-A3%y zb$$=mgIQSF2;V?@1>Eb7u^r+J4B~u3hjx{fY_Ps3^e}!V`$Tqzu&%F1ncnDg0oqyy zobLopkc<8;oH6D^TLuMkfqkXA3BQs!C;SV^R@jicL3)Em`Rv3wK$(L9f@7987no_>z*mepStKaXk-l?nD`HuV8`IYPJRn zycUbAm!%+R>EUw)132YSdjT)kd9p2B$5o5diPk*aY0gHM$yp+C4=CY5Bug-zk|o`X zl;!q0Me-%ntrf{zKv!Oiw*bj!@ntz_^CW8<^~Jr3&4SiwL!E-4HL-d9UaQw^_E^Q1 z;jsn_XbS`j(!I3x%JI2qOX?NhEJ@g?jl`x}vV2)Ck%}x?PVP(hCiUVxgT_E9%j4~B z_1e6?tn{FtyS(W^Z!kINO!vA&Nm)TPm>$fs_zHY3i`D1#`3n3#b8lB5;0?F~0qzRA z-2tD=>k4pt(32DldUJwyZxL9L9!%u{7apj|pwjJ0DhjGzPcZ0LgT4%((-ZXgyulv9 zpxNgRdR>;}pf5-=2>Js-XVB>_@Fe#F{{n#mv(M#+9B9k(5xriLt(UkT^yCGCK`Reh zEKUpOy@KhSn4aZNfqwY?#NMFCg8~q{mZ(1G1=@*otJmoZBnQA0&OM%Vj9beA2YUKL zS)3;a0zS~>;XQ)xbZ@|ijeER7-ZS7ywh;#lxG%{4Zi@>~gF#mhNHgd)TLPd9Vsg1X z$w|R1Y{6hJuiNK=l9_#GA5<+A2%t(J=y72zq!iqTks$@pipz-#K~2CuXOWLK)b*$V zm&eD|9&U@@i!OZrKsHEq`&~WMxN2z%*8P}Juf&-YOKbUuN1=MjkiOWUHrT+}OGyM3 zZeZ~i#h_!dge4tWuj7&=(>0EXq+RQENV2TM(vOUCi90 ztq~N~Q~Oj435{`4lgd%jqqNq&b{~IEB}LNe5qX#AC+g#@ZOEpUwAB46)zy+Vk~nol zVZ4iY%R0SYC()K@MH@yUm(CisA|KhC+%WaV`n*%aUTi1xA6g7=f&y1wE4JPP9OyA!%Pq7>`8KXZ1TjPq20}Dx(4u3!SfVt9 z5N|Dh7Bv&Obk>wh6av~vdV&EUP{*lvr~px~pk-1N8VBi^HXbFeNhnN-xFN1+gw!PR zH4H)#sDNZJ#M>Y_Q6p$8(Ue|qG!pX+R1(FBPhA)k$P^d_2B{|v(NvC9oe~}!3^HL* zQxC-`tR_bh>dZjBQD>sRxG6v#!N0gd8FU6<5JnXkG#GFN`f0Edy1+?OIGhEWNYBY= z(2%rjz0QOiK?^z+^dOHCZiOi5N(ff3GZ4wdB$053c145K0$plb{E~(z@~KW&CR&fz zCv3)hKzo8}^gt#7RK|NuG}-;E_1^6$l&X7)pSq6M5oRflL8Dn8$DjeW42*>U;`C8z zP2oWkNdUw`rHmLVXvCcn{Rj+d{Xy_3E*VK;f@MPL7_HU_SYSZQvO$a=W$>6x4=Efa zl8KgJhbU>!HQ6IW=$ODDb$}#PgStjZY#I^T(?m<4GO-cO0!efv3{rPp4`@i(;wL?H zUO0iSc1Y-CAiFjr4NmJ)LKg-S8uKf#NZGrM56uB19zgBUBTOVgsof7^yOK zkIg`xU=zb6zQ?zCi^M88;W*m#1`HOXF_=WFs3|0%wRHJ0PQaxTG%3;czdy$*7c8Lz z+a2I%_MAAYMb#XAF4gojbTH-Oz+Muaf+UV|5>dugE zkfTTp6loYF=d3lN2w@uZCQ|5bo5r~AZ3)%XiV)h2#)Q8B2dH-iF`eFMFrsCm2O&S< z217!xG%5q=B*0Q4Zip*_4)sa}=z%sA;KF3WrCl&OeT6vo{cbQ5s?{3Ge)q(WHg`yf-&`P5W0a&*fG%@RYBJ# zz!tIr22Cc4Kv4xm$~qG~TpWW$3fh965&mc=PEfX@x1t!BMXEuGhAR@$Ab>@8M1Ugb zp>YUIpb}cbQ(;Uc$j#Sz@Wwm(SuNUvV|moAH^Wq5TaN7ksySC zNit#>q*6lP(VRjsl7)H`gW?#3Hj1S2on-p|4TD{Viu&zJ-5JmggBnfo2T)9{7&Js< z;s0w4LQ}y28V9CEVmMtGq^7UHAT;KGi9u2+fk9kQ!fiJUV#p*KvzZLInZ`_9$1Zq} zy95SF%}6d_?vG*6Z0?3ZleHTLjd~NbT*Dw~dlv?UjUZtm>CJjGf?Nf@kTJ8l3xmcV zV9<;<;A9jNAZjL>5*Wmd*@#?C7tCf1l%xnXxg3KA#Z34`r$XHSBL*QzA<`=`h}vK! zZ6E@Kh^qt!jU-KyX9CcPvX{=p;_m7H2!j^8fgxsqfi@Z8)1X6IfG8x6JDN1WGQeGd z9VkJ(G7%p!d=vbY7NU`I5dR2+W~xd?NBBv)5RH%`nxGRpGU)RJaxR7Q9-ZR zpw)qmWHeENX&!7owPg~14^j{cOm`NG5lFETr!bij7)3eJUL2$3 zl|UfD6Z`|M=nb>9yAh+AhyUq*unyoeMF#y~^CjuPb5kd4i5p^lI%WDI&V5dsBJ0x(Py zE_Bdju$VDcaEw?1r2zfZ0BAJGLMEa;LO2*qa?wVic{Wi}j4Iv{3PT~`6{szgG(+pq zrUk`8CH4Cgn<(DxK&qvw&JT~&D>->>(^|ww#_#!Ga@+fIviWR1A=vOi!V`Fd_nNP-W2w6*21&gMsy^h5BOI2I7rlkZhzT zRt zf}kxL2D=&(h)l4N&K5*Z8oU&u$BcX80k~j*3W_r!?GgqtVw8cJAmOZK5T_|VC5GV) ztfCPOqm)JRBo@mSYF=Pas2eB+?BY!c3_?OyY8kpoRdA#`*gd)v7=>0+HX$0Lqu%t; zdTe-TwRXp#1Cb4u&p?3LU{-95Hc$hD^CgK<(NnjA7A#rFJB#(_=KrbfZ z0)vDcn0T^9Wb|oTAcmqCEfkYM0bs|_AiS?09>**~9cYpmF*$gX2~Qy+SS2BQjS-hH zNF5+SDUdE18M?wIY2uZ^DQLVXof{K>qv9KEHg$7$wJ0R7@C=;bXdy}10d@r@Tc7( zC^K6q%Yu>+nqI~yuwWRTP*+eWBlHzb;TDq@lg$PfVzf~vL`R+xiig7RAW*wFMiqd9 zT%^Dt8B}1R+3^s#JU{FRZYO~1_ z@dM&C69!S+Boqz`W3*uyR-4iL5*76UJqF0DZ+XUT;W|oLAkcZs~48rWez?gwSCi;1+Ts|bjwuP_)t$XI8l2XT4uZE@$%GI|SR$m8NoPcaX0p(#1Vm}J=#3_5 z7G#Wa=)wY4fL|zyGhvWggCoYR@<=!pem?F8EO;d zae_<59h?!UBSQ)Vfq$?t)EitJZ5W5$PS7OJ2D4&y(#IGE2~AmH1?+I~LK@H^&>(b! z21<5SgP`7EH`uYhg(wLawA-=Z3T-COrdMn>j5UrykYoc)$gu*0CL5@L4G?2;IxNUT zSlHZA3S?-{l9vcZbWw z2s*I+7R&~~TTI{t1H?hYAZ($Dl)!@E8D@eS!_Z82@CFkm;d($X0a#!VfP$d{htXMg z3}OwIFbLvdZH;DuL5P>EA(9QP0q;l}1Th#*_To9@N0LVmI-18&5{?0k60D`5MBB*R zP)RK%a6_y?&>7!AEJo4|gBs_^x(HU;>{?h)?4qP0QY)k#?GRR6k~&Bhx`y_MCY=eZ znDFE}lNG9F#f*sXfKi&vq>p5)sS}GrU;%uIXkd>ph_Psh1+c8hA`GAuVm`YA1|NYE zwS|YM4=D&AfJ?L84k(#aVhBWM5p+?kN+YB@AQ;qI(J?Sa7=-bJ3SkyT8^+;qBrpgX ztS$&KfkCSgIxjG20v6G@&i zCL9@XXg1k342luER0}Za!15@lHklltiuyn(1+`3I0rNCKONAo@=fnnO6q-rSb{&x$>+j*QFyVme%r* z9J_j@yg23EF{paX8V2FyfkE&V7^LxQ391Sv*%4BJgHRmkzg;5`CJ7ic0fR(6u?|`T z$m;A=6Br~F!U&7l>{3E>EpM?y$INr)c85FsfTB*Y>x zNIfJHbWIy}f=yze=uF!*nP{v}Ado(fA|wORVna+wGPIg3_!FDmfj|d6%CuIh4P%Rr}(}!|8 zI~h+XxE%%`NH7{z6_XC(2ML27`WOQrs=!s5Y(~;~`qmoyM+X>&mMF*;!h*bvs!;_7 z5!e8Os!AWFScxIFI0nr^;eZnp8Z&`%;Lie}0YW&F+}ut)v#G$X5Vi_5Qo_W?h<0dF z0(zl?Cac8>bVE}#3<7H83DFg-5Nt|9LO{5PLNIg++o@A?zB_?IfCSyytv`uD2kL?=Rx2j3 zMhBPxRw5d5K!XGZg@J~E1qK~7o1^d@vFha*G}{S-V7<2ZBrs^R*(e@GvJ3VT24O@g z!7>Wji|5@ji0&w9;DiL}3L9NvgY${kATe5D3B*Q*vda%z5SJ1+x-f{&uvu^r@fmcU z7W>59Qt(C83?uk=tHJhj^3?NJ9a9S~)Q z!U2w)r?_LSZIRNgI{x4(G(g6BJn+3K+^1@ zjsOuWwInLi20CwmLAV|>({ zmP}5Q6CWL8wE!4&I(1-x&EWv~ia`agtQrO}8j}-rk}q^1=ySpn*sTO8v(e+Sfjdr^ zVGPM^cB*zEVp=SvxKINs>7dQxf|C@8p|A%tM+D^(`gF#|x zK+G58PejbxWndB>&q_1vu9O%$(TpXE_-4U}X#@uPbQw#BaL&lahCd-9cosx)}1O`EZ0|r#6CVf;!om+GkJ5SyH5j~(BGzJ(1!N^4>A`F@o z!;?bKNim?eHtd^ zagm=+V9;f9;oBOJGcf3K>1FT(9E8a=I6=4#|HzL7##&r}2{~33XF{C7AjtN)ZDyz4 zg%FemVRkuD27QnxqPZa+g40 zHfn_e)P#VkBif)U26D)RHZaPmBH(a2+$1SrpC;Wuz7dt(6Dbz;^P?N}3X`dAq&DEb zU~D8ga#oATdIfPRTcM; zB6JhzL$U&c&=3dK{UJsS1ravtfOn`yXJE1mgJ2%qBG5^`2F*ZcgbL9-&kC04oh|?l zdP^f!sS)@L9MLqIA`Gfl{4)qz#wv&X5(Y6sC_en84PHrL5N0QVL9@{bgD=M6c4I{w zpF4>;sl!j-$%tdnVRmDVM1ny7AU%#jxG|V(AuPzt>^8f*Fz9v@232qnCfDe45rIPC zFlvh%Gc7n7i_>AZxt+iuVH=yzV~2x7E`WyQ1{x{R;td!}Sb&8B0)aIT@JrEw*f2XJ zz|nJHPzBP!I^0q(ieQ#1xGh?t02P}MFrs@QMjjMrRk?Hikr-0?<31h!qg5g9TQsJJRTE4)D#1 z*#?noQ^6fXVk&OQTL+32T*ZH!f_D0(Awq z1PsE1QFjhFZ@1f~T5Vox8RpOdn+9wm57P(8z-o0uWDp{(J_1ut3SkicrAC&_faNjKmYFbU zHDLOUkCP!%1ijrcNT!~`C!^6$GN)d|k+hVO3px)Ifh1Z>96&>yMe#sDV33N6l)xYv zN^NC@jue-YMe4#JWjm~P?SzqeFqy-qfegM$e7=#$r8D#eJ!=@WLaAJ|mPRxsFsRs6 zSYKd}47J4pWY{2Cj0(broCqlK^VCL z7d2vs6fh;ErW|&U$BvZ>FLeZUb)YpfC`1CNF%{G>i1{K=Zx$GILFJ&}ZfHG?!{YT~ zCNGx2VKrSr@`J!2g(gl5blyf55&aWILN^E}WSlh!8Z7vyPJG|XiZ78e=Jo1zHmlR6 zVbBfVrp7S{QarHc2qtM#XY)W2RII)bAA9&zU=q0i8j{z8?vN;Y1k>OF2{Cq?17^(U zf@ubsR1KaB4cS2%Ids?*ayUYeu#o~mE{vMENUe~{CN^*m_2EImGAA%dx(qHzEdKaN4SjmA%kgoRu; z=GlnpVcy(e6V|5?{y`hyW-K;1QpH2w3t9_12t!5gL;#s=xC<~fVC?`H1gPO~jhL&b zaX;u5>?aIjGC|3z3Jj9@r{ogLb0Suy9?1MrA_PKp1qlL$WNfq=bR+?Y4NtX?W$9=G zO~iJw@B-tZXgtD{T-!WeF@28*EHGWd4^qpCO<<5T1F<4HcSAE^sX>Ke(+Rp1n-kX8 zX@H7=9EXZe2vA3**$i}BRe?!Fc47bs@oexq)PRVl;MK5Fi7a>^XaO+j@xtEYkL(Kq zPt}IF4NYO_syZ=Z)Nu;hTsD`JFo;zf#20{y9WECGus~Wg8HE2) zDT_#_4-XVC`~>8Pj-XC9n%{Y_et=t>%cc?rF>4Fpjuu{3EmlQgihFPe45|n+fI&M= zJQW~X%h6G-r4=#AMNLhG}Zaxk<kP#&if9~D}?m}i5Pz@ZmqfI%n?EoLBg0|u>%kC1@{2eR0R z&!83>2J!U+4;0UcwF6)fpe78G`=>b}lE5x~h-Ff>w?#lvN%oMpNFEp}kcUJaAZbfC zV2oz@Gz0>J7_wG_j$JGP`>_BT-yHa|4nZ5_VuPoqA-U|Tc5>OhK5c5P2?|FMnFFppZc0fP5UVbFz8CxFQj z$pihq!cvvjxWHtXFfJf$*aeab&l{*^i^ zWC?jgDWUXGR;Vac5*iR16siub3GEI2JhVSepXN^UrKP83r4^*jOqbG?bVs@?y+?XV zdR}^6dQ*B!=9}N^Vx65AJ3B#Hi0uYNyZJ#-bY2R8qSrvte}W?V=u@NU7EpAT{D}N6 zC}KLhZmh0Jcbo3s6qZtvvNmOR%9o&sh1?;3C?qH<4wc6#dIS_bc^O61;}ki&QPd2I zI4HW%+1dH$&R=y#*-Pv&o839Qvrp%?*!tKlv1>Z#bzapur?ZjG?X14|&c)LgPhC72 zyE(Q7?+?Qp%VL-t#7uu__)Fl=*ZpNRV}G{&sjB0PjVW^fNuq1kbq6IL^o)ee%&eAD#Tee28?6G(nRl zN%Vi6notsN2xeQm)xi5L|4q8<-{f7&TBKb{^Y~8w5Z}cghRlA(_wYyfUj8V5jQ?Dk zFD>AIMNB>jnYh}Na8a%sI<`POT^$F6~(=F?3Q)KrB+kMOSbet zfB`R{Gchx>Fe|e$J97XNPUd26=3!pEAK1?VEXaDWB$mv2vJ{pI{Y+!&EX*=kCd*>k zc<*m6>&5a|KI_d2@E+kJyx*yqm9W07l=Wl%Ss5#51K2<|hz({Htdd385LU&8vT9bt zYS}PW$A+_dHiC_0qu6LRhK*(8*myR9O=OeUWHyC0u&H=A@pLwWHL{s(7HeX&F``@9 z2DXvi&bF}o*)H}7+k-dRJ;okoKWC4#C)pEhKl=qc$PTb5JH(!1zhqCdBkU-9h8<(i zvlrNll8s%>npq2*FFBwcKVwVTLMg*llcw)SIhv@*b*t9S4o9Z4qJ@(W~yusyP2(L+wnFjFTS)cOJ>O;DUyLbE0wV0 ze1PPKBt4Q>NYl<%Nk+*kIoU1j4z`Zn$u_aOVU;$syV!kf8%DU5?PL$J2iZUQ2tJ-I zZ)K9*g_C-8B6L-?cYaQGR%S*xJuMYe1xybx3&NMQ@b7W-uQd5J;o7E9 zG^;79%L>=k^``sqY&@9V?LkvCgh#cPmx+d`-fU!DRx*N;tyh$cXeA?ECAk_J#0K@w z57mT2(UaBT&`~~ZTm!D}tPW2NML!qUqr|l?OWfFSla_`Wp_;(F>QIz7g=(U;*UZ~g z(^TC%pC7WAhlYo?n0x25LuLyuEx3y2gqI%TITc)7NI5kF4oS>tqi&;eX3gy8=-6=$ zHPuOJX;XXW50BczXbD+tsAw{(42>E@lcD)hkj`!o9m;=s(;Y`)q?+=q&Ee+RGa91u zY}DN(*KFFfA?nPF=7y`Ix!3(A06w-v^TX9O(L8E(#Q3g$MqKKWNA;O%IJD_&hT()i z{|w{k_GEVaiIS;)%_y@LlG?PXHXN$m)U;{#(ayDV!XY)h>5$dBX=x2O%*HlEIi5dv zds4Laj;T?#X&xUCA8hUT5mEQJ=?zgSvoQ~d9mc>(ko>YX30%hN}-O>IhQ2GUwX4M}Oy$W#b; zYPg|gDk&N`lzRr1&(VpqX%@sw1xYEYxi~YUwRv4UB<4nkiTebvp);3f z4^L!=HW|YsCTt=}gyRh|P(D1$NQEP1P7E7ZCp?F1XNN;75H0Y$=};s>Se{3)-4q_) zyeT}PVUQ>d3$i-tI_lfSM(`06t9s|dm{lDL^V`N9ityVeOlx=+?*k3pHnHI_UOqRp zscPz>4CFOD8)7UXo=WtT9#WAI-B3&8abpzalb($*wpQfo!~=1E)ltsGGp!Jwady>F zNqeRy%1L;p)1F1dv#CgsQeYkw5{A7d)J*cZVd}h1O;bt7nGYPnA100A3UDV}afnNb zHEIsGR7EY}DtcZ?&nva(3OzT3tD<-%WbgdzHmNn?uLHdq#-^ISlRH!I{`TQ-r8WQL zO<(e(o&O!l`@2=G{ix)_Nq_A8!=$1Q`G;Jd^auHTYSFKF+sj|PEL}JfNG*DXH$AiV z8R=-}%aO8Y9PZjr4)N53g$FAS%3}`BI@oqljvU0({o?|usXJ%wl+HZJ<-8p{bqBv^ z$L<|c`;K)xB>mCOzeR#OOqSY%+h5u)$$8s&>NfuHwu9TG8@6$gyM7yT3-7JGcg($V z%fk{DkXR1NhnW|vym2-LO+`_q)b35niBCTUvceh4cb(JmL z(Q>dw((i6Q(=5p}$T7`|xpwxfyws*y!%}CBO;7!HI{$1spE&)N>C*7&eBgBcuW9^? zX?)={K4KalIE^dr64xX>Sf?wIC#A~VAy>)=@h>z}Ql}J7k*Uq6CcC_~M?23vHQ8Xn z`Q@i3iu;!%izh0U+VNxaQY*)f89QsNe7=rJ1xsDI0G|_V$_W%U9&Z6Q9-Il~{Wor^mC}S^ILF zFUzBQS#aVyRYN1n&>+L!ZoI_BZjo|lI|o`de75XE1tM+9HZ{YS|{=+&yqsNzyF;DEWa{s3{@qeeW=$4}FSo8Ea*2n1OC%FSGZdHCl@C0qFT!7K6q3CQnX~f?a0v22!f(EGWiob*KOs*P_s7@~{yTmjuD4-{;eh@~>{zTVwvM$)zreFz z@vV*hjOX${Nz3G1d6ayEykGv4&Y`Q;P1oJ0dm3Mzs?={)TuO`bwxQB6*XT6fZ2Y}x zuxXX)dDFM%1(v>+-S~WBx%DaQXSQ?(7eb&qoIa=+{T*5mgK^i1_E_1xz*dgpr&`0T!gzEl1^{yzsifmwlrf!BlO!Ii<6 zd!+YR*W-<(@}#?xPA6w4|F&mK&r>N$DJxPQNclRo4%d(#`!e-^F} zKb>LBD9sq0Y0V60-kAAZmN{!p*50hM*?HM5*-vKwDW_M?bvcjZyq@!I?&93_xjS>8 z&V4KQFTG5?uI{zI*F(LY?)7%Bk9%Fr^8jqsdHeES%KKg3*?fI|&-{M*5B5&(-M9CM z-dFX$q4$>F9~N9&aCgC@1%tET|50QpN-gSNG`gsz=*B+wJ{f%m z_8He_UY|96?k_eK-(37a@xkI(i~m^sRms$nWhEO+9xgdj@^;Ce`?9|NzJ+~j`_AmU zqVHXOAMN`>-}n3eci(fR^Gny3ZY%vo=_{qbFa1Y9eZQ=JgZoYBx1itJe%t#U==W;B z5Br_#uk=spKd}ED{Xgj+EAy7+mklkOR<^9{ma_ZH4wk)M_K$LJd4BoO^2z1%%MX{o zS^i1+`2qF;SpzBtj2p0g!100hftdpb4jeOZ&cIaz?-_V_kbBUgLF)!RFep0c#Go^S zz8gG$@V6C?ip+|#ijft~6*p9Dsd&8N<%<8P_*=!rN>634%E6W6D(6*RSNU#aU}QpM ze&nXe_Q-+AuOlBuz8+#9k~O4!$jBjA4Y_W}<{>{H^74@14f(uESCv#%TvbcQvuv(Y4pt{%V+hSn;q&h8-LBOm!xs!cGW^BiuMIy{Ke)c8epLMf^*^hBy#AZ|*ofQ_eMYPranp#6 zBLgEtBkvlej>;WXHL79Ml2Nye+Bxdb=ZcUs}J+G&l`9-DSxI-hQu?t}}xV@7bs&Kch~x*MlAzSMZ4@pR+w8b50M zr16W!Z)eV(**0_K%>6U}Icwmo&9gpf8r*by)7jZAv;Q>vZ*yAa{BBO?RV`O7zUoiS zP0f3opKt!UCDQU}%iml6*_zy%(VE}7u(iGQTUfe#QeR2Dq_NUu_ zv)sFU>GE$^+;olinpaj1S^4d?H(q<|wV$qXts1gw>8b~>^IkXSy3N;pa6P}i_w{ew z(Bplf_P%lHjgQ>;#*Js!xYtZx^YEGrYp1W>bW`?Cn}2${`Ga+% z*G*eD=N8K?x2?}uKXCo{^{wl#TmSR*U)?(F*5$YE-r(JE!-fxTtGMl|+itt<^hW!} zf{nEsXKq}v@s3TooBncp%k96vW5gZD?u^{|-dzjt_TPQ7H!MlR<-T*?X~yUKTz_(zjplf!MX>x@0`5z!JU76$o5dhuAaM| ze|Yx8pYE>Sz3*okKRdj~xo7sC(~p!q^6uVS_wL+#_|dFKH$U3>*wv5y=g+Hue%H^> z>?_!}Vc$ECr#!y#@e5D<;>jsbE_`y$lc)Cg+@HDsw*CM5#k>QK1E&x6JGk}WKci!! zM-BxK%{}ys!-a=mdTQiTe|_rg)B2~ApWgKJzka#sm%l#ZJF@b~i_iFSznJ-A z{};!;*!topFYS2gFE4%ea_h@q9>4upJ%6?5m7G_e|8>r)EeIUZ3`Q`|EeU{@Cj;zy4pZUwk9wjs9YX7NMPW}GWm!}PZ?*!i| zdZ+fCY41Gr&NuIlc=x&A6#nMA-@N`_?tAyX_tJZx{FXjAm*{uzbY?_BGFIO6BJ^q} zlWwWJ7OyCxC>Ni4)zNpkkzH9_Si%eQ8cUodg(ZE8+-c6V{@9Pp!!JBDPg-?xgZ{|* z`gyv)qRlOxf99J-n|?OnS;jlhMcigjof=#lyeGIjsFUqG@ESKAb%5^x*6FzELAQh+ z3co&u9!d(;j^f5XMR`0dm&ko9q>|zk$?LI8;q(HDk6l&r(K%0gK~j&xbi7f&ut!os zx`*%51n@GX zk`l0}SezP*i$N^WT-d1o#a~|Rbot96BDySzUzZfpDAK$bg4&mpn1Ja7D#@jMgWfGZdN&cl(|hw|#!(T%Z*d_V0QdEG`H zjg6&!W9%regQ8sDxl#Tv{Q6FBeA-gUo{p6CEzIS)g@{s=rG zB*Ou&Nu8JnGj3o*Y_|SO&IEaoRHMjfsSB2PJh_7gxeNLm2j{e>Wu~P@?9RH>)ReJl z+>w@=CK=QEy4#KT@(sRgfJ#r9ZFNQl9V#hw5|0XJHWF|0#0{9|0&Aeaq{ki5v;RQ$ zvf>VBIdQPDq!D_D4a|bJ8NA_AC|q`Fid^C>kV~P6{Yy*m0~LNlR#vt%MK<(BuCrf1 z?n#mSPP`-Lzls~zPux`8+B7kgHl?YhaO%3rz4}bQY0ReP$A?yx-BI7LVR}K-lUG~P zI6dZa=2Z2WI)T4fd-cTr*1vvea`kZTSJQep`QenRNlWU7woNECJss8eO5GgHRbtap zt0pY0OY7e|%N4ubK1X^aqr4Em=&($kitkym*3QqA68!d#37?2%C^^Z6@h4Ph^0s9_bNwHH6rz`q6EQvb=Q~uLAJd9cD*quAa4&x^> z>f6T*Upg`?Jbc;c5z9tp^ZoIh*TTrDUpgY5GcvoAU3h<&oUfA}eU#VpxvhBPhu4{h^&2v*DpbY$RrRatsUJ9K7=93E7|U1lC1ZYmdZ>=qP3l>vribu}^pGKa zT;s<0L|w_0Vwn0`Bd%sPb~y1|09Nf#JDdWIq+3vz z3g}Y-44UFDhFWpE?8VbeQZY@_5+={UE9CwqcG+hNz{k|Iz8M22mn8=at9ngt z-Y{U!#JW&yb>Ff5lfzX_vDG=F+J>bL>YZFXb5p~Fb<>NACoUeuo09U1nsQb7h?Q6tn6Pq0WO3t|jMyDL2UU+QtZJJxDL3}P{@%m-B$dv+4?u>^<-~fK z^@=3Ftba#;MlToY+wk?RZpIcLN0O-({Az@;CnGBitY%wb4cPeDF)0u2xLJ>gO|gGz zI#-FO_J83A4J(CW{sl;N6VFN%_y=$`Tp*9ns9!dE{~en%hb^diV)U~545`oRe}4R4 zTS}7ZG3&~ z`fc>Z3C=2G-|XyW1-}z1+O*W%dq} z`r@6&IEvG~zII(kB{@m(uD^t=Q%Lo*+C zNoOosc6iM-2iwa_+78{gGP-=gMR!Wo!cn6Z)ug7@EE+XxVO5Is;i1@n|Ejh1S3Gd& z5Dy%0Z8;wM@59^9+&7|rYsdC&XSUYYZ#_dYI1EDRcl1?eYra~GEKnIo>5ke^;T zxpdY8HLW=_7F;u=>A^(ZLay-9mBcXR;{_9z4?A>p$SrSfT6}!cl(!mZ+&U$_ z)wpDJ?V?APlvVc0bGP}2 z2nU5cvcl;I60_YDR`=^CrCs&pn!4&+UtL>r)%5ZB#Bk14y~f-yxgfePsj{-%JFUNT z_~K{Tq51Mn{TJX$tF7LnizFvK7^Z5_JlLg=!gO$z3H@xZ* z>rF@r7cX&`bLy77qJK{>{gQtA!!c4F$#a$jE0(+;=5L4jbEF$;Fcg%6=9GC@Hk788 z@tm@pQV7l4>9HpgeIw zOZoUnNx;{)YGS{Z1*D0@Qynxh7vGmFjii)&+gPQF0Bng>4TVBdsPBrTBtx(EOoO_@ z&?CVd(!&zr$Fv|yNBWj9X8i$jy6>7b#Pu&>-E_KyA{OM6B5pX)ny0sU%0@N`HLPrZ zw6*2H+WJ^#LJg~z2sM;z=fC^nqt`+W75z?|b>`3hV`WA~fu@AdkP`lr56hX@T?rRf z(FjE}p=%YsxgwIR=B~}XCwF(QF2%gf^TU85f){+&vG9XIRf{Sr5$_G!1XPS^Rj zy=;5OCNBixJ*9%ige zD^l-Zl}2AFBiiDM%!CJBhPgcgT37kt!jcjmz%_-6nYN@p&Hs9SzFui}e#$5LBk z4H$c7=bz@)=`C^~xPuGp4j?OnGYG^v%5nmNeZyW$65A6AOC}t*pv- z#ooO#QkO8}uUBq@SDC`*v9?HMgTE;|)z8;tbANViZMI(?-6sSYEQl=N10$pP=zQyR zm%-JX>~N*JUUI?ZBy)o+*_9mWH$jfn49CLT@yg?3B0sZnW}|u>i%G{tICC6+cG+>v z@(ZyjhYa<&h~G3@cZH4f0o)^gY6mVMV%8}XvuSd%YFHOi&cIgY{e?ihX7hxdBCcf-0F(loG`eVSH zhusp%Q1rDDuk~=rBM&!X0f!qgwYSTNZ47*^;U2>~cr81{Y5~PuXHX3WpTn2xEA-v% zd)W7p53jAq;xS?coixiHGTZCydc34S*5Pa7mBp3ig|6~Eps}oECe07Z@*0<6=>;%> z2r2%=p@f16TGZtwKwN>0wG}j(y8GfsdyYxLC!`*+xmy({{zb=t7YHbu`Kz&k`eWy- z@%8zAuD*kYq^4F4E^z`-upuWfE-ZZ`Mo!0f&F4faEGg6U3jbL7T9N*PK%CKQ`cmgw zr{tWAPeJeQBdL9OAY+=pxyTZ3S{2}y05=8#EThTqN^e4Y@!$d$rK6my-(arZsD48c z1vG3~2U(T01j@Yer~-?)@rX3+bfzU@x)c2FwXdwJ%C28rQ?qJX-yzpW+hR%2eKqmw z+SDD7eZd8@x5~kV2d*DF@S5l~Q+JQf z9zALHz`f@l8ad_h^LrNkdd%p`t@`Y7_y2z5ogZu)9ro{W`0a*8uW{ewk8#gyOY(c| z3>85CNKO|ZCo3d{`L}I4n~R?w=ALOvbHLqXkUdRUvKI4iq7FMj89zrE#%CNf@ut<@ zaM)QALNiTAV&CqI-OIN>@ZEt~uU6f7bj2Hcpukq?(`RF!K0i}`WX3OJUmdyk{Y^u^ z-^eqFZ|Gfq5xrYjRV2$}L1HO(5MaEEeG!4HdIp?{Vnn`d#281QE% zxfW6c^{*8F(li?+VcACP@{1Qu(zh31k^C1wl?LmN?2QfDbMd=wJ(}3kh!fp8WZo(B zwemgiqvG2}kuroa_-O%$rO<)G#N;qA=>qpZ&T@%OyTOlBstXJ#@pnaoTk$z-1- zla(aQ8@7xjWPz|42nh&b5d>U7>(EHjy$JnwVPdCqg5{XwlP0oWJ~16IN&q1&VzrF!(L zFObx!HdhihE5eNqS%T=Xv~(Obe$cR;TktXPgGQ&}$9A0@Lk_5oTsSgDLx1I(O}(01 zTAEqhmnE(#x0SQt(#)Z<(g8up_YK(P`~f`?41W|~0Bj03)njAA_DMtnZV8)w1*k^T zV(2i`+ryHD{ic5N)*IWu^4EtK4IF2tZ*KbWl4SmZCH)oGKfPl{{k6~BRCdYqdSB{I zNgjzia-7C>&oR&Ak1_Xi>&i;LX3zClh^TJ-j|UeisKL*M)B7mD$@j5EjTl&;u-bgu z%ubnE$jmb2K4n#=G+?FeMO7+E(fCoE+k(?)nJ5c~n3f2NFYIsjxcuhHk)e|(#e*ls zM@JUH{&}CcmYy{qY;zTV)?&5LWG*#}+szM{ziB>imV3>t)ZAolH%mb?6Hy|jPZ3{% zCVpb{BNZ9S@WMbuGEgraBjNCDHU5&RrnLXr>c2c&JwS5G79ynDdOHL&m)`CXSB<>) z*khvOvB!pr<@<_%oJ9s##fDnl$1E&H=16VT%_53_B;f%1Kt&>X z5}m!U{DOk1Yd$lCebKrDExHhZBmR%u8}S`&1=I7bsK;d>f9;J_%l?xmuVc@@KJt-x z^2(7D*pPxpplI$+HH%N(VE@0If9^0gs{wRaL@-fRpgjNjjLH zeDJV#Ds?f;%`5CzopoLkFg?f&wI&~}hE+batG5{(NoFxg_Drn6aeR}R!AY;Wv>M(_IKTMptNU?a={A9- z$sR9_mtsLmL5uzUVHRJYXv!<@Tu08^*d}=llKV{`Hmc<^_A*(FwBz4Zn zNVO`$+)EPs8!z0W96!HAe&mz6(zcqLTG}o-FKY8tESRT9=uwL;MUd-VI4j;`UUtCC z1TbDPifxq)ea2PUZqcU8PKqYUOxWYCnG{s{F*h`G=5(JFB6$QinX)BqkF8NdxwsFC zPZzUO#cXpiixm$Pi&;yJLq(ARtLz<6G)|=LEG=RBcKE-nv&xp9!+(C@U*G>qN5?}S z{ObdMJlu2s^_t87?aC|v?egmC^+&F}^5G3N;xE6E`ooJGaASEMeU!PLzx?tSQh)sB zzPIo1>AC;yeYgDQy*)km{wHBfH&`5NY611kQOjIQi;&7j4%apQGk&IW0?}FZeHp8K z*1<}hN==va-a+(^;vAdq4(%e*sGy(E0Y(XKfrCuJ#-#{3UjKzN_uX@nt-t99UuYib z+||E-X~mJp56i}-kL;Q^0uyoL#>1&Kp|&kOU)o1&nOe!$GJs6G1~sn;YZ*TsXQ$$9 zGhD6lfw-8vG;653e84M51{AB)AjG&K^rH2IIW6s7{Xfm#%9g5uFZGx1>$`5vx8n9< z$N#m&yZ`HZUo2dHp!rLeytqRY|GBk3jJ38R%kV~Zk-_SPP8cyuyD+$Ic9Ad8L}N+J>5uxu4GrmA|@ z*0V3Yo%-s#NBX|dQ$VJ+!{V}$Bg*lgyuSFx{^?mGTf}7#MP{#=om`~`a6;F@TKWoZ z;W#9cD49p?eZrdBQ?;YD;$SUH)UuL3gMX;TUZO+?@aefa*sCD->~x_e=< zdi7UVY$RKfx@RT|A`8}jv3JQW z1J&h`2Lq*B1{)X5YzpUh41NJ~f`>+SVAu4*a<$rOV6Pcim*I>-v>8H%N`rJ%_r6a2 zBFW_XK2b%~hX|nvwF(!DYs76xk0jZZY&!@88IaR*4z5jvykLv^DVhK?)-FzL2d>Sq zxaFzTaw(M>P#j0NY;ywg@h_DV;Cr|&-F=L+yiMMtUJ+D}Gxof4^Umk@%zEluM=#wuzX(zA7dozd{`UFvzVy=d(&HDB zM^D$Sytzj@f!ULWwI8$36gH?fG#Eqg_CdCpCz8vE<3Fvsp-X`nnI&e~VL4@yEGnXV z6w;)IQ5!%qQ0TeQp2X`1DoRKu;ofoxFi6lP8ZQMmCY`F3U!hnN&G3M|kAouP3@5j) zu3mcK#gyl%l=lS|4V2ie6+z7bc45WRZ_J(hjTPD+=~BS5L^ylgmI3X-2Aljx-@_ZL z9$0>)I`EbW^b^4ypqD@neH_EPoPsR)2K2M^!_W@pf~{N<1&ZEXP~g@}xyTo0 zo8e_Wjk$3}1e=L~Tua%evK?h&sI0O~w3e}qU@pzjnd=T=wr(3{>NX3A=O`#huK-?g z5gC#Nwg4Sc%-v2)fLBec)WitVE8+HUA>|*EFuzhK{-cF)=1TDN*E_L75@JRQqdL^1 z)sxnl{)trl!n@Xh*NGBCHmB+3mr{2;owE2)djwS)yq3Q{o%+HHXHfyc0;6@7$BfA5 zM;bP-TV7b$ciEaM=}KI=kH$B|>TBZlvCVZCpvhg>TXEUSrG*76H*Bg(Q^X+F+9d2! z0|wOkGz#^Q8!{ST2Ip;P*=AuU;=Cp9#mXYHCS)rVL?D~t4l}AH>97*tMQ96Gw{3Ef z$f7XJBaR6lrKMZ0P-9)H$@w{cP!C!gsjnqS!EA;BV7g-1E zd?vM2+&;2Lyda(&d0mW+bdan?^jr_@GlZ*D@B46*s)o#D#y9osK0Q)1q5ko&eNdzE_?sVi-_(<^m=$z+FKqi9DX_q~&v4a9M*A*}IN zyyGe<@C4U|8`IBvUrx<vG_HX*l`wUg z4pAd2S!P4%3^@RAeuk#cAw&e6^S9NpYM7CJiv{B%xaQ0VOQ{RJ;!7j*q}?Oa#JBH| zcRYN@`CassBZyRJ#8dRpTVav=AKhQb6MKxEW-?Pg6hG2^$PluS_8k8i0ultcunWqC z*qFMU-F^0K>S^64cYnfVYk2a+-vgE;gr5*0I5I_m((u7BWv}9(#}JPJ!8qPP(yJ5u z^@7rK9mufL5SFX)_;yL@x<*B zgc*e6XV0>GQtKaA&fNVmtcy%277Jvr@+@q>j^k)5hL3atd6TgR^k8Bsh|2%b5IgnN z)X~(xiUnlL6wi)?p^;ojJuP*PlHlWjXI)|Jq{zbr`0vtUN}U%Tmk>RI^a#fG$!|#x zT^O|l+^SK~W$d&fWgq6KSikSbEcPl^3sED{TEupEB*(Rp`STLCw{4{rl`i?MoCQmI z{I0y7o{j*?U(0c0J}57T?W0=gRqZxb+!k3_W3Aa#BkC%XnZ6UFuc^6ad(vkEo%(#4 zi{f5)cc5gx%@(lbcSE*8{Q`JH&%r*B-BRr#XFfse;g_Xbi(}_TRPtAe@Gt{}qR}J00qB0e0S=&*1pzr!i zLM!eoZ!O7+_H6Gxax6nXduCsKc4gkaqK4v2Z&^8F+fg|0ny$NQ>b$$U&ePC5&G(ht@3^aHcU|4~ti--I@A=Bx`=;l44;oRS z=9Qnm@mzuTYk^!^OY-50d@U=58Z}TT_}g+9R@W5XZ+X?Ml)+m;t=EBX4X8vfEbtYj|y1vAy6 zNYZOtT>MTk+f>Z*>{!hrd$%7YV?=8n)8*x51-p&5tg*G_maDYFL2@GbqbC*_egaom zi)Q%ced^N-W6M(J#KUc9A9(hI5MQSAG~|kmhLjp03Fy5bV0=AZdNrfq_U(7SpNiW zn&raRRj;{PSA+JS#Dl5-)PKR*413rY4tF@$J1<^-IeEOP<#^4 zVxFX&Jj<04}I;W8o`WWMh2HGc@M) zg-@dvCWZ+mfubUD+&I%CLmm$?B@O}ALAvllPGM}|)_!-H#UA$Cvx^&o$NG2ni)|Qudj{HSR5;Y`BSQ|X+`qnliBrst;ZrAiQ;7&E?c!`-Kxvh z%X7E*BO7OT>{?#SB%?P}lK7DU2)~SCVi4&(ejH~V%T_{kqY|fBV%oN&W z*6YwOup-q{yL)2qa>b1t)v!P!xIXQ&BRd9Lb^Q4AE#>ZLz@8{8Z;|J+;^sPYPH8Tx zUm!yJ=~Nbff>Q_!6{;7d79wJfwJkGCk}`YqJPU$BvsK@X0|%;s%TN<@a4_&F=cf#T z&dCp+QJL`g>LUJ%i1T<}a-(wshZG9A--RwM(bl;9xAWQawl9w92PTlxR$lsmM_De1 z>V}^2=z%l~(k-0_51j{_qS&mqm`&814Q<9=D@vu4Hi6=zNpy9Di4ARWjV=k~&V%cr ze$BNVVhY0C|j2*7`cOeG(Xj+ z%NnUm{aM40tpa`$VpuIUC4?Z@Hr6Xc{vbZW^*?PZX!S9U(~@p4&U~4$T1`ZZZBLrm zJ*J0E;yx2wW7=jCO(wU{R<^L>g9>)0f`ux&D@1Dr%ka9|+`Z)ypHH@S7X@t|E4rZ& zwSn9y_9|@xB+C!oEo2MA7q>)DBELz5=;5J4?(~bxuDrf#-6cbn-gpLv|xtWLZzA` zv}G?u_}HLtr%&RC#cP$jQK%RjHRZ4n)>x@Y$i~NU8$T*Q>QhH0jRAkQb|aVL*-l6Y zm1@A36c&fo@N40B!jd)2bXiH`qEMjQYV&j}6CwgB|C+@wt)Zu5IutG;Xgo?TpLxS~ zHm!MlN88MQ`7VDQ5%wKCxR?Hk(S2{;(a~|ooBM8l+*=juw!3YppzSa1R!C}ruthtn30sKsBE0BYjgDJz z)9a#n;w3N^z2;-q#4SzC;&Q=VJ}zc8rzNmmlZAvV60^8qkSf*H-4$sm_m7DT(%Tiw zZyiXB2@tqQPQV;Q%)zG3p){lvwB>U_$C7j}s>=80Swp67zpYHbY``GM2{>8Mf$V#c zIO7=?#}K_1uZO&own$qfv34?|LlXi|N3$oxf9G-H0huHVO%-SE9mC&W-V(X%;~%m& zozwiGJQ9Y|d?W)ia0{)f={wR7B#~}^EeFc4H^EPqU76jTy)#>~o7*hC9@KnvImi={ zcs4zgYO;&K1qN0z>Cz{jPIM~n3nYEnIZeLA+V`;Sy`LmP~d1l3@oZEEbexe zDS0-7MWfj21nyu`F8+=^nW$bC8ljWIJoh_reH_!rDd&48+}=| z%aX~fx=ZU<-MVPSSCfg>eQiT4Jq^p}B)4>zu=4I}dx|1?50i<`UEGu(tgfmK2Ak)u zm^o+T+`^K)I|HRNr$=fkVu9S|_CeD9!lQo@4=8gWqji7V%Q?X>MRn@(r*Il53g)hcv@0n#0l7QkL4NsO;O=E zY)6UA8-EALw;zBu@FMRm}mRQc+{)Dd!pI`%bCb$*j(pv&)GN_b{!&sK?q2 z%m6dCCJm4Uq~Te*6F|BRrKQhulO7yHV6>Wv3Y1IME4D4Q@9CFJxw&367R=7gHNEt7 zYM(rJ=5K_@eM;GA3jgfNvDR0Wk03Z74_| z1PwNt2e<#JXeE`jX7@%}6gD{3g{ymMHp>p-HEPJtHbf|L4F$xK%M8-{J^ci01iL1Z6^><=Pq zgfWa=u54Gt#R{9I%vHoy>}n=1p%}5*h_7W>D0KPIWw3u_uy-?99X!{1)$L8{Da-D1 z{dT>$SkI>E=fcYYw??H|yxn}vELxFa@|KakY-A^m%mBk&ld0V#7MYlWo&};w59crm zHzZ;}Tj+;zw0{foX%JdS+5>_F$Y`2PAyf2aBm+H?1&Lgq zgPPADOS;jcVb2t!+|&l|0@1r zn~*Jui;J!=ykceu3mCIW){X?k?36(~Hu5(y_XcU!zAs;RoiL;9De}=r1ywEZDat=W z(Z58T*1y}})s0v`e9H#bG(g&QvF}6gN6Lpf%|Z#I0FP^m_5U^=|4a3M#lR^kqs8Nb zCP=bfEmT0B1^_1RBAXFqLa@JOm$w$p+dWXX`-U5K*A48RSJYabz3r;2w%ICMB4&0E z3%6Z?IKDND9$2)7&Fj3PEzIs_=15CrYIxoDtjw(I5y#As@c* z5=}8>sKG9H%g7s~uSIm_IhyC#{KKt-(-KUz9?QLmF|8Io8r*`6WZc;E-8dc+K=yGTD zUxmg?;j0u;;mjt}>PT7@t0JKlb zxi7xhwC>BwE9H^f1Ev0^d*r zkDKld~>a9A*pi>f@JFEw+B3wAHk?nr&2rKPrbp)Q*V4nd-+3Z&uQ|~LHT}YU+NR4!wZ6nky13l{q;hlS|Au$CF1@! z8+ICCqJI>*05-j<->E;1zF&HZaMEeF08ucv^Fqm>Kn&7E)As@02p9gA@?Q8K(t)oV z{kyar7U$W>X}V5rR~BXT%n28E%xRueTx=T+v%}$IVKKby^fGp88Jjal@XX1`;4V3? zHB*+!2k=e_OA9-cMV>_+#j?4+yTU~#XVP}a_=@L3!*~&{W>}jk*$AEsBtPIi)`{m2 zLMXv0;u@@c{Lg?r7TZz~BZ=ODvt;U+=NUmv>xpRd?~hoW$y6B&#^=^0jW5?~N6D zq~iDjwcJ{GWpB&MSp`L*{W;O5Mt?@=v6u2yGjBQy+B~hh ziJ}BmRPB~^f>pZ1bBH29Dk168X+@F_XTH8)@bD)xL&8)|N3dYBNfN2NMbT-~qD2jC zTSJjE;m}`gYfa{P88y>U;2ZNh~JA(>750k2 zB5iM3u7(3G12a8!jSb%Hs@mqr(0!}pb?fe5mH1-Q^qLhHo~c`L%hyk@U3>cLx2&j} z{C?x=qkq`5=MP6$kG;x)vIW<+cV0WcRG(=#>_#I7{fz4$U9F)?HrP92)>tss726Rz8dLJ$F?l?L z2xq7$|Bcn^W6|FTy3@|z_2;G2mY1%U^$eY8q>l9J!@|c)6>rwX6zrnlD@>-6_2UkGEMERPCZ{ z{-d!QT@pkYP0rEnk8Yqa9LMN0Zari@YL#?a^J!VU`@+XkVC45~*F6F{Nu)FUMe1^aD`))NWMH4Cq0;^j)EkbthuzP+ zrD4zW9?|1w^IaB?#r=ZIlI3!t2f5&V!Do0ucFAzZ3BJ?5*L;%xfXsZdPY%w0PAF%m z%VCacdM;?menk-6ez(h)@roaVt-hcy=G);r;5+2Qk>lQ(&G64CKEKajsJ>j|ocnSi zojD4})6U!$T1ZM8Ay56}v&Tby@G0G1jeBjSHsRU5N~!I6hMTG%A(Y%_6`{H)=;=Z{ zH6oEi5PatUI*;P-l1sZ5Tsnt+*y{mH;re*efr=l?kQ^i6*Msgo!P z$=P$8c2%}F7MZVDT+r5Fx3j_2Vd;h&Q_ua=XONA4qRT?(%ERDq|4WpGtULcVbVkcU zlm#R%X^}`V{Z!pxrdpQ!O=IyPR5y6E5}a5RypP+gA*=X~^#iN8$;t$%;UN&X)f#zR z(NfGvzvdZqbW%YNAzXJS%Vxzbg_*Ejiv{@3M$?9^?34-gKu6m&QK($y+h(@Cp z)f-A$B=2vu^EKc*>r*m(K1X5X-LhXf-nE*}@}P!uU|6I}Yp4_Alt751g@i0D9+dn^ zLJ7qLXh#UO4)CUeisX!yIuJX1W-nP%wBQSi=4@|Fe6e=rlFJ8Iu3Xle6^Z$qR>Wrq zT4r^W&0aSrBLA*)eYR(PXY;DIB5!a{Ug3gE=FZ&GrG{d;mg0gzPl3;x*NomRQ=@~B z-?G!1Ie^M05EXogMxy8-SU}^rTlEpP&mD`EAUG&7uP1Vy{I@_yPfwo9-?L;v4oT6E z!w2GkttNnM{XFtDF2v0q8oS%G0&O|Hxjw%H2Z+g)V}N-HegiF6fO~Q1YD(WdBwxbj z$n}D;u>3e%wSLq5D`w^#zteB5ynRh_JxYv-?D%oEcfw$N=YrmflER|M!jjZ!9Ywos zNnMbKVDWGQr?7m#ppeO(adQQ;oKDvc#{swz96IwC z1;5VcxD|zEEJ-wgGTw&t;d3zxUVwan*^+674m77mQpb4F4NiU0`Z$vJ*?qU&<1V*a zt88uEzKTRa-*t_2|EJEa*r1a(NK$E4cEsm29J?yRR}9${BD*-3orF$hLEKNc%eDii zFgi09i+E1hAsvvA*;@(slK4xr;a%YuINGmrdubdeg9jgynn9vG!bPc+%HBIDo551( zUXE%q7w!^eZ*y~l+g@b9WRSS;49H9SAuo|vv*J6xcepy6Wq@}(46kO^JI0XbH_GY2 zZ>*YZgjxTL_-rX%)fX8@Ci^tS{l)V(&PuHBEG_QXIBWXlon@(M?TZ$+w=Z1OE^l0L z-Ll0*^+8>C3-LS8vIiat1Vv1xj-S-ddv1o?#}(VZ=c zr`!ydg9MBROtmw?&g{6&@Y-K%@8|#eYior>Z7m>{0pei|;-3Y9AW~5yy>PV@2gD*Z zTg=K91?VU2n4N&xh0OxffQ>;TP=FK!q@RHI77EvZmI=}$8g5bokebz~BsKuI%?+%( zfiqWRA%bjUo%&Mrq(UE%5}h!1?V%@rpko*;>m2=UOER&OCmn-a~v# z;}vu-)n06T6T5BE*0C(``rdnHtMe%fd`*MDwufhdpZv6Cu3*~Io{otu@Ob3QCDY=A zw=9{pmS=%apIPc;v5Oc2RuW(GFZEZ6g+e~xiVIqHU*zc0{1+27I zz~9dpzg4&muN}h%V|8ih0YGQDWuF1PbZoS+ zUI=5fM>R`gu2P6-U01O-o{*n}y`26H9qSI>AXKR#NqpbO&iYulkEuQu@-dX^fkf<+ zL?M_)uoe`ZCKalgIf?@3I;0`O1>!`ec8ShuwDd7MPOI$vhLhzxYn;7K326jbnas&D z@t>q*2y0w2T)7{pA;sVv&`AR}^MJ9-$gIYrMll0DyOED4TTmT{;_1)D5+r5AkU?TJ z7qA=Rl--(6r0?&)Vej+L!;;7?MC6Y^HfMy9e8vA!f|_p8NJgsQM<>Mt(ksY*b&vc( zY&m}oyK@82)lR3uvKMi9u(LPdZbsHJ)~1kC!u9L|h{JT8b(Q32hd2pf6wlG+^iq_j zmnZm+f+-b3Ap60v12(To^m6g|twKWEQI?Hgn49VNywR+E^e>a4j|^iuKMkEIK*MJJ z=!Y5wVDE`wSY}8YKTZMDv3aXF{@_O7Ju_3^oBV`FF2H>;;BQTjrsWYY!d}JbZF<69!)5)bc?$NzRP4c-VmPgopKSvN>{qAxd=~rCj5Xm< zm}TemS!_0DSrziz2C`9~0>TqY)E^bj3ZrB)qXcCZonnch)HA8) z)`)Mg#JbcE*z`52Q>oKKtR?lr8nXG_FglEk#|wxIp?zMaPAd+UL7|O3LgjAu9U20OP5pH>G$_^?(18zlrm43e|1Lpg(oU1#hH~? zwo%flw0_CRGqEdYQzEMJ_}!PKe)R;;LT%g8dB-YN@(fQzEh*e_;oR=mJEd<2LdZeA zg|T}t^ycIuEfsXmGu=4lHl!)Hg;NV>evl%DbF)p8DgEN{TZI=e9=9vY$GJ#m=YHO3 zQ0{e;p^prQJ_|b0qK3_SPP?E*k1eV6xD$9i&%L!Fz_nQodywH8I22&E0JEDY<2J`WsPhk+(4gF;Pbb`R=NDaXTC5)J z>6pCD4MHxmZ+trE_T;gtIk%F9a&A-4LGB+?&=XAuo_WjFqWq?YX_2B?Gv}1l zbtUSi%`7XKUhJe$`tzOgv9-4?EGX=_8g$FEcywV<3Ou4rC(OJHhK*q{ZYP=W-QmV~ zX5QGof5zyokdR0sOknt)c;n}dW$pi^jU~|Wrq6(@{CW%vHvjwD*d~lEZl3#TxXN2& zV}qaFCZ~&#imsXQ>&NW%NQ*-GJf1|dd}=<_?uSd`53gBqa6{wBpmOJ(RSP@k7DU^- z7FONz!|8b)16R*&+um36{S{r~8B@}pD|VRJ&wcg1EIu`7YGMDv{)>}NuDq&lVN($K zRr8lEnnAim(!Yt#rpqD(^?hLj!@LBK^~*E zQGjA=IM#52a+z}>=H?n^vYlk+gTqF7id6eJ1hpBH#%~?1#CW;|Z1{z%W#i=0qc{}e z!imwWJ@r(2G=Uc~r@$V452Fh>?iw!Z`DlIzm@!pUazmUKtznw-MPU=-JIi$uXv2-d z)#@Ba5z6|7++H~+FFPkMFDDz-Qx-R=O|Lb*(;>rw%=+h4t)fP^kVA^0t1jGtWYRGv>y4? z2oun%%=7dRO~KaU%-0^>(U#cy$hx6}r5V9%`$mPiZxt9>DP0go4Kj0?CHEY zSG3RDI42x#+t`5!^p-FE;=ZmKJD%LO?Zg*ao0{dVr+eZ+j?NcFUG)S|5Ds zm&Y0|pI=?EWcR|38~UnVBE1Q_g?xJAQawbIZ2rgEUWKvxVqT@mGnFE-mc&;SR{WR5e(gUtsQCV86U zF*00S|7qBqW;CqVV}{=2l2}aSeq!|F;ZV&K8CDX8gpa`O9)k?%L2cGS)m2%}@*d63 zwmiB>8j{2!sR4F99!^+=ob4$IIUJ!9Pj+$8aq;(XLfn&j zFA~c&n{#84a4cxC1Y_dc7kxmo?Ny`0a!kGhvTZ*6uglb?Xs`&fZ9&qL?RM!i(C^kP zERLzM*JAI)Bx{W6{7LhoilW?TP%h2u)*G{73UE7YrQI2nL-FFd#EX-JTRH`kWLqvj zXwffYo@*W)12d#1+3J)UO|Y$#W0YZW!^&B^yAyLL$s)0(3h$@S9ub^wc3Ysm9CSV_ znz}NWq<1{$qITol-AOI&B9OD;l3ka)epB!6xVYEuIP?0U%SN`_ZEp~r^Ehu#yGu0M z{a2zH=I1=@f#I+}(>saMFCM*hP&*Y-3}5UIf8JQuo}rB;&_d)h;Bs2juvjk=Es89k znAeHX^TYm_278SA3OwSK{{-drhM=!BsKHp$xHxW$ zvuG&j^20Y#9T5EfvTm2v9qvY*OHC;m&+4sCXPUqpGTAFPDe1(nFPNHY!d6iZ?^tq4 zI=z?YogB|#!O1Bc((%dZC6f|QGK5s!UG}NjCfF@LYram}tk-{=-%%nX=D|#ndTrk0 ztAr~<{-y3&a3*dCWb=eL*CkN?pJcw#VV8S~?TyP%$|j8G5+a-TTa7o>0(>N1GetJz zu=VOV>`rc{qubR7;NvE?w1rWF{N6a^&YxLrdt?vYmFVOh{< zc1BrclnK$OK(`qz(O8q^sTtAOVS21%|=Rc$0km3B+KZ8tuUN>R0}OVvxV5# zLxo(T>8Hi#mN)SE`zD}6j~h-yC#`Q=*_iol0{Vdo=&b!i4LU9nW-DR~iZprYBIxy4 zBaS=jKL}~)F+ks$U-oI-m3J|kaMz=IeH?2<*q3V9&kGfOKK32LBWd`>fPZ=H)7T%L zgnfrjgI++;OW7)s=7x>y>CxK6gFc;8qSCK*Z#OM>lk!;;$4H|2&%-3ZB z|27&gHi&Nlf75Ys`Sj_50B2Cx&$Jy4Q5IjuPqmDC?h(TnZFv%;c!!v&&L@MIS=@0*JeQ; z7i|_DqaRONAKCL6>%-5Bwmu!dnOGk~eCqlTm-;I|D8*9ok5$vQ{ zYcLa@H_suHUr)!{U*wRb0Z4Vynt|x#{nsE92iG8X3a_Lg!-&ZS>OKRqLVKz~?!?{W z8kSZ8@QKm)hu3`;Y~oKE>@M_7(QIDz1iP%}(^zxj6oM08LGRGSEHw_It}>KYavc6*Vcki&RIPbU+ka!CSPUH6EJ8Ob^u>ULYfC; z6g~=N^4f9zK>sK(XitwXMMIjPnzh-?HY12;GaOA~A@k<>OxfD@^w;+H6Ia5Q@*u4J zPDs)45NFAmeR>lwmUYz2P%6tKS^EUrs%*=E$8AJlv0id|@Q2w<4zy5)bdqj`2JbL& zA#P+tUmL3MNEQWROHMGKaR!7sn_xq=6rF=|j0aD?^b*zFd`b3?49U;^55Dd>{{~gx zl*{i)?LT`K)-{|9p3@%&*Q{}xgtqX)h%GW0*%^_L%Vlh{ntJnG<}Sx6%NbODv)DXx zH^nH1P;J3xgM}LzHpsn(7!SV5i$!R7aY2gWUM8n9BmA9^#h*{7|3CQPakf3RXQBwe z!E_OT59BwA0iGDE1F#JD?If2KlMLpipv7UF;`?V9~p(1D_+TUC9W}J z^jEPlq>X3d&SGZNftYlvS}^)wIA7IZ&l}B4!zRAUrt;O%KVozV=c^j*d8yaZuyq6* zO+6;Vh7|z5C_052?9OrT5^+{>DrY6DgV?}Np$5A%1xo?P+W^?2Wr|$@eWYek9o2Ez zJ;H++jdLP+$6RzcC$myB*lvFMIylehDL@>tY7{pW08zXrPDfmFKAycwPiI=gv`gqt zuL5yuVKS#C8!ce1LR8Sj`Ttr5Cb#y36c8Rr`Tz1Vs0~pl(9HD0 zJQ1`lRFH2t_e?H3TgwjBvYoYTpq8n%EL6(`oD&tWH8=oerx*vS1e^9Rf`t%wNd0gU zZ3#J9X3GS%P`wCoG4WurK0SJDkFu~w7mXHP0x$a--=hkQ)`^W*rEjQj51XAD&%sO{ zPc=!X_JFnT)JEYVB0}ZiV-gbfy$_lq3{E$j?24Q{8cIi z!G7P9S}V^@{jPbauq^)=+m&*U940Av;$DPwMl$y3YOEaisski*Vs!JcE>n;v)=u~) zXsky680OiP9-qhdbG|CE&2VOHM(arM)u9P<8Rx6?B&?8j*VB`*j@T-`Lcbh;21X5~ zn%D-ujvPMfg#7~)!Nz# zhsWXS#D*q8$VRk_jM^7TnW_ygmt{%9ZMXkTA3ET<(6eh}!!;aM)ca<3UN zjnyWdf}2c|&8Tdv7&9_p#$z@sSeRKS>HAEAag{}p&6zU(F#UkhL01kHqUPa8l!fv4 zYPw_)i%ofx6v`O4-LO}F{&^b$7jX69vwDxhJ<9s%ODBKbnK~o3j2Kp2A|8<(BbObx z;h9w9!|bEx8-Us9-!Z=c=4TStsZFZ6+bkM%3i^FYvdL&NWk3sLPboiAM6E=Tgy20O z*=7(jdNWN%g~G>WhMZfFn#S9YOTkoa?mxfiIinxhjngg}3)fyW?=jpzuUroeN@3-zhbQaEMFo+y5@E2dN&DmT z{OO9@BM5D#Le5%sWJ!6ve$|0V6}Tr?O1`GGwc&;uDKt@&TZ?X84!M4snkPw~Ow{*4 zhTvey&JxMzZu9gO`)u7ITej8X8Y==cUgwCMh`@R(#?DXInsG_MSbT4y8jzT-fHbMB z3+e++DhecR94m-4UghP&-BcwAyT&6cxU^DDvNNNZVBFy2VUm%=*%#TbdEIN>5!Ai* zLMK_O)}b;6oW5DvebK1FA*?A2mZ1#4VW`wI(3QokSx2+PjI1oLW56!?1}vz_FFU>I zGUtf0;kEe@V!>V9RM8UKOlc8XlL8*WtTi-2nw4iFd<%xcv2a0qVWIBGh_Q5OrwsDW;OqBJ2|6HJ##R5aIgyPYUJ zQqe7G)(D}R6zc+*h}FKhG0jhn}c3T;}y zZu7;SNwMkmH(q)D7e6_(l8WLzcz0S7kq>i@UQ4G!*g6((1cMgn@x9?dMATf8nneyg zUCQ9pwUEw*u=OVzPmh3Es-A#_?(#8PAIWftfd%anY{(wlIIk8*kB>tIy$(V`L8Tpx zB8Ym(9)B6*{29Yz5HXH)(Mgzsr(19maB}iErr#JY3QWQDN?>Xn#}wAI(arp%u^i6V ze?6RsKBSYd<@K-mO4OxSqA7i6sJJsW*G$Wpte^0jJRJHWZ5@80t-}le@2_i~GWH_K zh?M(t8ZwSB%WJNgGWsIOto;EEavdP&Os|_Vb}b}%)a=IEX*gfcu+rLn_{s3JG+eA* zAI71A5XKRBrd1p`QcO()d2#QiwR4yaTJ1@Htrw8x%rNAgfq|Hekk`T9^MfTgnyl#@B zx7qW$vVuXAE4K?>SEm$Dnm{uif1a8Gr>iL0;{pW4DV3BcKOg_n)s(oQ;?otC_U|97 zsl>!o>Y}gH#D{eG)l+?`Oa!e~$)x9<)k=9RO^_pYv(h z{O;Fack(zhJ{rJ;K={ts6CloutnO1Wa`XfX=X%-`7Ided z09U8TlArQNp5Q*K6XSE}5xpAUF&B)5_hIjkV3b>)o5q`bRVTDjmT;I`l({V#XH=Ks zr=AjOU~OPy{JE4?I$oMP$=X0CTKP4{6n-xnOq@M(&`@cwn8f?>qsg&<;LxvO zC1__1fGye$6S055_aZMVYyZXs?TMw42`?+gYLF=ov6=8d@64Z!tub_csP2Y<277#0 zAhk)_JX`p0Y!w>QxVVAKaw9s2Pr{r=*YbIiQj7ceXBsYU%yRHE#-_NXibICpT%3kX z5soHkwV#5_?Oz0$tk18eSI`C6S)Va_64v0!@1$W{0bA#q630W5?_{i5$4wg6RzS8* z9t&=VeG{KSRHIWUP+bOti?Y;0F3H&@^k$j6b!gE}syMftAzp%d?C|J_i8{OX$F&N( zg|T1~6?UUe`Ph`oy5~Fblpo=S{Jlcmb)k_Vf}=PXWNi+&BP+{+BKZ!Rqtc-|WWB>F z47!Vp&5`aLJs!@Bn^JCEn!i&MYW#%-I!1L)nbyK#QrDx;ECcKk50o$5-Wh7i@ihjE znhKp`HG#)5{<}2mLV}Ps=GqjQ>JkTI={=D-hQcnRB#Is=;+fKQRIOzbsi;9y_>>AH$h|;er9C##x~<( z<4Z6nyd|*)DJdLK;4s?%ri z#t~Hp{Xir@P5A;wso4ut;;jx_GdtjT-ysef=z!)Ljhi#4mT?PA|H(MM;m!FKOQuLeZW;qxr3BXigpW>FhVtkwKQUB ztjC{oJe`v(pj2PSgFI8IyE)D-Jacg5jR&7*AH}DIn&wwm^-hmuPVNG@$5lCz|M_&H zx+)Q`%B)T(56&6%o{zA-sq5soy@PY&T@8Uieb zZ0^>!8Sd2IYnq#D;!RRB@-BH)0QS&~9aND!Hz+))Mi%b2wgy{ctx{|2(%tPkwEAi{ z`*N-IZB+|bCRVcPNna7wo1cbY%p$5cUsU8v|)a3ZmF%h+v;*#Tvn9GMUE>~5=~d5$10MNju-N!phW>yrYFrEwPVTbWqZ|z zh$LP#V|)qOKVPr@;xt1&PgvGUth1NK7j}LGDJcJRWqWp0q@}m2dVW(ljQy&_eqEEO zLjT8xs)X*uq}Bd-+5d+c_)12+2OHSX6;E$uO>J{?E&jm*o}$bseka|m@oI#KbDacI zi-^YL!&$y5{xKd|%enO<+IV)2##5z!DCb*x77IC(cstK2;Ls1IN2hZ;r0G!omsB^{ zC)P8V>sNqMyP+^i#+Jf3{mKvMij=kS#j}YsNr^m;s`S`+f}J*Y{&;074nH z5kxa0jWvQoPxZXZ!olm}8*1v-$FK9vn%RWvo=r1nd*!$CR@B$8$n*28>O9_Z;i6~h z6!_Ino1EsIHmhy`Jtz3HUY+tR%F{;4#@aZdC((^2E*vTFi zq&pJnMqdYi-in5X6?w=6_hVjt3Nb?$yas}Bz$y(;Bp+yAO9Y4g0^LOtBhMWk*}RQymu$QwDs7N+Drc7Ex5o2i<-L~GbBn3DcWJd(Z}3+Y;Sq-wt zdj;u^L#8vP_f1kI3ZdXh{Yauli2juN+J`3!YAaVws-7*fjhI7~ZZGcIASw}*XW>3b zz~wN2i^aFQl8VFKN>#!V)-LM+Oyr|hz0MlAbz)jSMbIJ=YtH?2yqDNmnmAG$CLeLB zDt%Hz#961DROLJW%X!0WgYd~M2d!oaZq=aMDSXlFkiKN3+-+LSAEUnrg@5xN4G0M* zW?vuI4cFv(%Pl@NuxeFqtiiWy|9j=7io7KwV{KnaJ6ySYa9lVQzpnfm*&GdO#OjHm z+=zfCQKH`9Rt{KH)WWl6(v0#vjgQ7)^+rq{C1=i=b(Az?1D&1q-J{airIw_cZ9Rp zhi7?qL3i5EL3XRryIr%=SS7VA=R7nMQeUA zU!?!eY8< ztm*&n6%v5u?||hz<%AH09iMy|de%GgKJcs!@|U%7T8gN zA)|g!U^q7_9UUg1Wu~9p;rP#x1nT1%@rYJRySB$>gUGt)U=xSceg4F3+SE$1ToMmg zu$>iba|P?JK)HQpt6)L}%de;~8W9yG!hHdFX^H(&gm==x&sV<18Ytt7>wVD z?9-4Z+{f_#4XkO~bi;5qLuJE0FXT}i*{T&O=%@3Un$JS{(_AEL;CFZ>1h8v<5_A-8 z3#sBIe2@OvxUNA4SSoza{jdt^1R0+f{w9dW1Sax|6~PQABEj@leN5k_-=vrIsy#a? z^~tMXTSEaXC<>b5$6L^qqKPdIIdXO*b~HhL4%Idu{^SD~^}sUmBzY~~DyYA)M75*f ziB2>v9h}k(FA61wF?5C)ux&zpjzee$qeG1jL(;fZ)KOzNos2fj2$g`FBaSM$=d`C% zTSiT=DL`})i|ZlUK-nlPl-HjBKKsE_%k3qBl=%3CV8snL>`|&-o%_hr5u^CQ3Z93a zo;%i}h_4OT{rTEJLr5`s3#enF93Qj3RNm@U{ z0W~LPdEYWhEelcC*rKAvF>hX&X;Kt}k?IcQ8Ci^rE{CuNXN30!y$y?H6BN=!6_eHn z8IBz4;D@4#&@A~jA`k&o6eeAfgjAH8j?4RBOuZefu+m1p^#c3dQ_{f|kF>Wxvh>0r zNJ_(#diVm*$}zmTFIz`ma_k!I>U$W9nv(G8U#3n+TMs{(j0I*w5A6(gI)f!pubJ1_ z6)Ta^tlwJrKz&aUab_R#9sQ?b>DuI;VOZu!Cw_RM|CM~!@`H*Dimjf;2m7F8_W-|tg? zG`#%&=Z9K$ZNFlE<+*c}^RL*xt7Yi<`(6sKZC7t`%HaD-`-?`+jbxrBj!a=}l zA-D%Ly%v%HRXQpVHmhyHHK?=Mmvhj__8QssP#}!X2vVitq(GDddo9Y2Mo&e>+j+bM zau}@9U{ut(mpX?EgE@m4CX*pQkR`znO+gQ7Guvk`4ORvVLxn9Vmzu8#Vs40O#qmm& z!_X1>r{BjZ7XY}z6%e(_-*!U znj0urc^J283|X-~mZhe=r3E2VrAd@cELM;xI9u?3fov*ZdQ*X^z*)Lf7jeVD7FbgS zU-OwNwzG;Qsy0=L2D8^JW|$RBvr-tymJ9QY=7EAtvU^cfhjyp&3iHI7tH}-n-puJC zop7DrTn~>e07##XAEx+{7P|LGw0oUZ7;qLr;2 zIX1AnCxWiiO7Z_=?M>jDy3X|Bd+yb~@3JggvMkH;F5AKzUUadIC2X)|V+@$ZX5V87 zTL=L|NT6hcgoGqyb17xAf=j5AkTj4qOVc!g{FAg_CT)Rf=}f0-U^K zP5PhteV@ULWcljcbKdiwcX{6DZQOoh{lZtDdw9aWL&dwEez$M`SI^I#`}|k?``&$e zSMi~J6CQr<)rIR%Y;UByLxh(8Uod%$?kC3m@js1$Qw+oXG?qcOV%im?6KMm)GWces z860(uj?kDW3C)+zX0N?ndhO4pSaK6$$*abTDTQNR%TO!qwRcFbZIoV15lQn$@k7bH zA#xqCbm$l)OQ$pOA>F{B8A5ZMFzt?$6pC=N(&7^d z&HSDHaS*?xkq$60v97^MXP7nw`qoxCwQCM8T)nqMr}4S=AeN#k%`^m5#tE6(kRyHDUPd( z$yIVMVzPW#9+uk+qPppkj7V{W&!lat>g;Y60+C^&3WvGOASm(Ds1Z%iISL-3RDtZ~ zlUQkN29`p_hG%L}Y6_5K9>DI0+s~Rxny(QW`;o9ravSp-dw;N@;gLOquS9NnVO8&; znxToCo?cM7u%#fQwWA{|-q++U>%F6G@sy5@t%2;u9)-7L{)6)$dcmo%|NO+tgR3es zyd!^9jN5!{ZN+}?%18MX#WW}&U{BYHB?|- z8`DFy+n_QPwmaoIxg?^Yq?WT?!!5c673fIyI{F)pE>&l=a$36_i3Vsuso`>{ z5;gOZ(MenBo~P|J;ClzK;s_z|0Ha>(MZ)(wGE;P~8{GkWqW34C89b>rnL=)JxzU5d zQun=`TgT;X+I??CRyT4YXHIwAo!T*{+s99)HnUUX?0IEZ>lEan*4rwG^K02hmW7EBAcpqSMgO1F)m+wZzFx2d-x zJi9&Wt$uRVn!_v0vLhV@`Ay-p@yqshm34Pa$%(XVkFH!Bj4iC?cRf05Zg^sz#hBtX z4P@q)bggfwn^Bi*)Ee^Z;-$1hliHeQ+n1R=WlEtKM7M8L0rd&Xfkq8;U{Nd*1aDJn z26*erOl7Cacug!9*vGDMt|9@jcOXu2fQ4APK%!WtF)~U&3`@))r#F^dU>YP#5{)zV znhM%c$A5$$+V=9+$vOJM&RbfdJ>y;W%zVeQDfyW;L-DLBGk47{ntkBCmHDBZ-jICr zqxWw3;Kc)X_7%@uDf-b_ygHUwIHRM*n-PmOq}u|SUZc5wB>oszn?`;WRcxjkJ8u}QDtSJr>c?I zMw`*WBw}U(GsIy5Q+pAEWv0;I`*4Kzj?b$MZ7hxYaLi-EeeWWw`H0TQ>}@ zAD_sdv~;3h7sBz`sgO7nwf zAKG{Jp%%Gm;_eT2gAvY(gOAou^BZL`#K93|pfu}hC+9RdGn~E7)y}=nQ_eF^m5M}d zoeo*t=m6v<>BKYpoqF)2&F)7D%*~`b5<|1DEVVwjcw7o?)+LJ#56_!_U862R220}z zCK*V7*rJ!IEqdiIY`&@8zEsT1wA)iH7Lu#MKMwSF<5C+_c@Dh>Gg5`r)Z*Uay~Vt- zm?(-^vA&TjvgZM?UQXK9963pvorJ$ZTNfcgiTooa4D}N)k1 zbhksU(3g2H$if4;4JB@+{C=e-Z|1H!!{y~wICOOebB$OY}?2ibPXgq zr`)pq}miZe0zjM%HVT9AD#;|eKqQeHx0}c)J zraMdyGUy;i2dq6h|L{R=c0H92_~@8%D%@1GM}sZq>~2pF`|rRbwz7RE8|sp+@!RbY0J`t$~2tQ60a%V$%I8=xq%A?!t8@}1P0gzvHnBkTV5WRS=25T zX4$+SY-~tW@4j6q-_vr><}DHmZF#jpP4CIRhm=SUZEj5_DXxqF4#3*PG-z98JSE{ zc|3nj7U{tZ@%xSwnch#b?Kwhyg=v$v&ilLLc*-5~D<c~#Wc)>7SbcXvMdy6@Dz@us`pzw_~bzN_v)?wpmkl~3Edth!?U z&epxZnpv>m`&8B~S1t&PG2%2-KA>Y9#nUMC(>*2+$xp3LMSdr0N;#Lpr|_RIfJb0P?d!Se&@WRalp0$;-*o>w`sQZgKt0afx5;lruPIn-nhLb@kZifx{?W-JODWj0PQ_jtSJdZ9s9Y%_ywoPj(Mk7 zWU!3LxeC%((NEi!iuAF!f>)TMimvkfxHi>P;y@2R2Q$9l^umRp8&9%oMx$VOo|*`f z_fP=@H!~{H3-)RN#7vzArX|g2v$}ZIOFLUKDq9Qh4MYlE;Th}1AKu^nz~YLoy{|7N zZJzQ;`RFIAJ6IEN|HT>BCbWxob;BZ+Pj=TcUlB&MQ{(b6 zvpkV43(S9TQP^2FeZt6qvUnyDQ&Kz%Fc9Vz=Zd*|a|d%zfnOolo69SFQBP-%)@vl$ zafi#{bmN#|eU7O$APEJ&Qgcj_hom<_1sK|Zma#!XB;j_Ng|5~eJ>yi6;vo|xdHTM$ zRvqhmZTIA%^6p#e6_(3u)io<0UlQH6b;qRGj;&i8pZN7X6P1Z<^3Spf-}~hO-aGQy z%?IWc$>n>fHbyLXKXzI*qMNbGCCHXVKTRbsrIKe-$$?Y?LsA}y6;H2~)!J(Lw`#wt zjwM zC6bO~v7MyyVE4#aKAAU_ynjlE2OE?8vHPChQlmGNaUvt+HKv!h6c#lWIF2o7FKj7K zr_aAipD*pbw|m9iUXAx|TgLu|6p8CZB#vTv!QesCJz3I~lTkjlo-E3@AJ;|Rn)@ln;uF;*hFAC2kQ%x})oN|5 zwx(WQjJvpwuSIyB#Wj(=BNW^F4;h&uJvK;>g2l$~6uV_ItF9X*+Gl-NkPDa=GzR;E z{Xs#I`c(=UNEuAw*P*sM#nNr%?8SD`9=D&e%kB0-D_LUQXyxZxNd%pI0S6&gb5Clj z(q->)2v%Aq$TEDeq^KQAp&|HyN}!-r`~*W9J)qZ6(5h6X#9AAmWRSKDVF?_XH31b57EOJtha{ zfJUHb(tzg4u&qY>aZ4E@93spMrse`ugA{``CgVwiEP|_N;Fc+Re%;7UxwzrEH8TfV zgRRr%RUf+a#N_GETt2w+t+v+4gYw{nTaWe~y#2kqVt(gAqtl>7QJUkK!^HN^O$B+6 zXB1%$SY-|BbQH8s2ah4c*JV1z|7}d2*4~q~EbD@TZhFXG`J>`2Br6Z6@4Y_`h?eCA+S*VoMJEXZ0G#sxc@ExCw_tR$NhW#1AduO;aB+WA*5z1P6M_; z)82M_d0CG!a5TWH0)c#0k6&>0TCDlKP~S9)l`Qo;PmdfB!BSf{9AEhZ9y-_PTdo^?lAYIVcte2%ET}cZ?I*_PIihIU61a5Jb zfEs&~N}Y6*BQKvg!N*Q~>)lZD5&6)_BmC0q2T!#KvQSiu*io%CMb)jG2Cm6IXFp8C zuWV$%HfZD5AwXy24BbYK6)?rkr_6G*dC*9f7&jXExkf?}d^`tqiQU{|(f43FEP_!< zZ;N@aSh(vH2h$u>anLvzQw=oCK1N^gen}ym7^5PD?Nkkn{Du5=R53`}Kz5L6gX2kU zup5Y3stqVxMyA$$O&j3V)UQ0ETn63o6Z&etobxAN9mK2k1(_%cNY)!-Ufxagv+Om^ zu}^r1|0U1!-ERw@3jEUo*$;Mb6M9%j1u`B#3UW2F+vr_Q`t(m>ZQ+^{6|&npv?Wjh zq=|AFbXKEX^^zttbNz4}Q`Td89wXn)l%=%3vNsalKT6Cw!Qc66qMvM+4*_^$!uauV z4f`U=-Bs4;7O|)YcA&~^r`*xT^q%zo^wa4wh1T4ySeWMSF=~%$d89zinjWP9Hfd?w zjnQXpJ@A;u%#!j3#eMyDl7*S1?H6n#hxfBh(>fq+n(fI=Lpvjk-5kj^x{A<|9y1OJ z1=ZlV3qztGbOb}38J7CpqBg~y5zfgXo)VXA;Ekf+&t41&5H=?_JZz=71AiX(UwXh`{AkW7H0VVtSI z2NoT?(-PPLxe#a?XdF5VKp)T@S0|@xg{mC6Qmz>sUO2cW)HP*7ak`9O^k!m5;*sE%gm7Spge-635vI5JpS-1Yln z>tgi=^-i@=p^mDjQzKsy)&C7872V6YP26rysN`a(!4fz@c-ru?fq&FMb{fcZ!xFeATw(W7A2v85SkjoQ3qj^902s*gp}d~QKm|uA6XKdiV8lE4W|MO3Veo_f+xWK zjKKcK zoY_@~vPOclIJu82_Q8lM7OZKrv`ea=swS(g+pIjbS9+|1)jD3D(fcg=`7(wYmCxr2 z74(+LT)j5ab;mY_Sw(LMgNEjjDnbwrN>YyVO1ot3Q)`#?E@h?E*Ajg+2pg2JxWY%_ zSzB7B-!-?i;d>wKzUkDo{N4o%ijEDQK5=qz_I>k81C2f7Z+fO>cJ};rJEE%&uc?yt zPP%1Al}@3pnz=F7JFrg7EPU3Pm7;%e*RJgk2V;wB%cqX_G?Z`gmglCF&%2Y(!h6ul zCdh}ATOqn_T0^&f5wNKQ^i>(IVi)gF^WDycDNfYH{M(DRlo-CB6a5-%OrdtDvmrxz zi!BAu+cNN++UBz5+61Ld3n7AYFc;)KkubhU2?>eS3i|@?96%1(T?!?d{7{`-UQu4* zC;s=2AnL@+WJ>-QpFU4n9#kVGzXD_YxzxIbhK%OPvg4PV1y|L56DCjjrin^B8$Hvs z$C8?s>SbiRDPhLTSN?Q7GlkQQ)wR%Uz-Dn8`1(GTS`Bm%ZL`WN@rBb3>PSdO(EI&i z4e0{vgX#sFN*B|BVF&O%t&*3iK$HVcEgIg1MaGU$GwnK$4=S`cSi_Yxo(?D$-P4c* zB0Ko`$O)X7Wv7v#fb8LTq!L+AXCL;DY#+ws<>voL1?si0#cJ9nqI);`uA5xvJje&W z<@v-TaGtnl8%UJFZAkG=KBLd;>-PyN6!;509>LaY)?4YYnDt17k7-#be!8Jx*1)Qf zpibXm&dBziNZk1kOqVnNz)DGKog1-ava1L{tkZRfElEq!GY{aYo0w$@twm25Sen3E zNv~&}ZWOSn>Er9LPM9}#!eZWhT%QOqt=nz&^`dXaAa@B0kLo28V3Bl9VHoxMd7Iiw zb-K=K&HUHYzoqiO1fd%hWD`Jj7Bms!(B($~5@Ra0m_kIsv7||*lvGAcPtnjffUa!o zN?j@_hmuEdhU?J8OLipDnLTy&<9*@oKw##$vX1)7w2?1=0m!RHjqc2XRtvfzAhf=@ zJm=8nn9=x(&E+iXipV0vztW_nI@M~gFI_35(ECkdw=h3z+5OWKc`RB9HUs*0P%K)t z=o7jDFze>1S#VTO#+U>gvkBzvK*D)+%_jKdJPsuUr|I6Kb{;-uZww9&E2t@Oc;vHD zTL3eC81GEa4?7nV)2&qakPjPh@*yWNIf=fzSF>6(pczED9i4UqJr1m<9=Y76MaNAl zkhH@V?TE)x#w^JPp0H$!7;i0AKH&YgpIH0T`P4guG1mRc>JxldK~YK1T{HRU$VY3} z<>c`1U^LVMhMeHOd&T-L*dg6cIoT=S4|tQXM81($y92ld6N&W$%m4||YW+6-Dg7D! zIla;dh@z~UP-}??cS8@s_~@*RhGs^Cb#OB!LrF4%F*(Zce?NGF_tO88kwgt8x__|_ z%HLr;MsAJR(xoH1Zl{{;RPR?GRtq$g#;etGeK$u}h+Aalz}bc3{J43wc^hooGv;$< zrO`~3)Pyq`V8iKdSh4E{2LpoH4>(xZ0LQK#T|s4daPaCd$1I{WR&;H{N@7o9SGc(j zF${xkuYxEPs-z*uARdwQp7f*Xa)nAY)|6BAsBbjoBpow4V`CkrG7RN9-$?c;Gv*#j zJkJ{+dgOF}#R!bK9~92MYtE=KXE}7evmnrQmY>F04d8c42me{Vbrw^Xiq!7(MP8%# zDKD>38)0#JTs=m>JbCDHw^A_nrjB)TBnNs_6<#gw!2oXb}3I{m~+6bJhTZx!hbU-?4$B=C!=$cQ6G@1CXEiyWs}AAk!|jNEsmxUaas z_+s&uVtKJ>w#7`kZvDbgUXKIdW1|T{Ym?p}=q(vNaw*0nc_R{Fu@rVD3j#l8heu6(lXu?zbjKL1EukvMYV1nJUFd*t)`AG&a$?VHObOP*M{>WRh0 z#f$L$$t5NHZy1t%;Y&mqklf=3zWAr^6>lg0^cA_n9S9I6~Xi{A|Js)U6Khxz)%vCqktiUlnNBj1qq zufEp2wk4D({j%m9dg`$XVwRr;z!7)6 z9@6V$eJ!e?0}2XbYR~jqpp^S-2_nqMPsO6^TRdiH61`@_2?&U|4c@nCSqo@Jdod&0$ylTsJHwr?g( z5THS1F2w5XTn6eV(34AW`?{yc<0rGp zrtIC>e0H`w5JxH)CDJV9#T+OJxOEMV5JDuCN;^P^Nq0mFo(e2H;=>$BKT-Ih@Yl^N zD(6LW0l{9As3LDqy>ChN-rKjKR8$6JTefG_%EW(tQ8zo{70Q;bw$-|% z&1vF_k#}du5>H$5Ci1!|V&YfEEDEM&h!+j+5#b6kjalG0*awtkyOP9}gpUfH==>-% z^0)(On@aXI^_Hoy`2JUaVMGXYgnqeS@X|AKL(=NW)MM5qjYm&=TENwc3L=|ztF6X|X zMj^bGq%V(=n0}ASi>to;928>E1}PZZs3*^}2q8ua@sd>>8EHMzHDN(RCgGhACyt#` zW{qzjm)JqJBzEx5p0oc+xPB!3vzGkoqc^8w7a+FJ_^TE0#_3$agO|UKdo!b+-U|m> z(_{qEftI|YeNW3D){=)&Z=p3P!2qekjh=cDfKB=x@n&rd;S2>j-z%-OV+ViMN{X$k zt$VG4(F!6^surncFd7UNXcVGXY3ybhL2B;X9rHDkhXZ#G#}WfLQ0Ic`19tOx4#x1P zKTb%P-*bozyq%aqKKwAza46Aunw(C|c$+Wd?IVAppKyl?M?Pkscn){kh&%6KSj8T3 zyu1mW@2l`nH!kX%&hUz7s)fF4l2J`^bzS4#mGRS6WOvo0RlKo^sH&>Wp?J1K6{oG5 z5at~{w&0`vA?bLs5PH%PXPcbj5l4M(mVg`G3!|<%)dbl9;gU7A3uD2xE2G<6A~{n! zy34xuE*qZ~Zpts{h-8rrV($f~i*TAb8wRIQ+5OwlBt#thjE z{Q$FJFX@$fy21zuGL8x}UZv)mG@Uw?pfxIZ4YmtgI_8%m<%5Hb)wi0J%+0n!KuI?? z!5^R3jwJpaK%<4j!Xw`d@wb7I)ScM5|EsO~>3Qz&*Q*8d0TyDZHF_(htbv*(IH z#5Hl#GEZj`F%wlc#!MVkrY3Wzk2h8uTa2@eg4*aZ<{AZ~*J2#!YIJqF1cl4t%0|mO zH8HbQM3@U}NuvsErjjTg$%}_pVP|iUh8U>(Nxjdo-1L2g*=A8DzTTDio1Ff1@Ha%O zcUX-w1=9C=V)#thZm+5?w~WO3L48SeUFvW@<}{kwP*Z9$msC&i@jtmv5@(4ZScx(r z1oKL(+57HO^6Cux#R|sz;UJ@b#Oc9OTE+lbqNdr<6%Bx6E z=6Fg9lPu(YLqj8H!5oeA<=tqsrIE^ z&J-LD2h0Y5!opnkjx)rX3^h5cCiKnt9yJst`33FbdXmGdIUHKO^40P)a={CXJZNuC z!k};p%ritDS0qm|nx3G6LD8B**>EAt6+6K}kfcta?o$C@yB0J|}X#mDt$ zV>CeG0*T-Us@cV5Vr1*YmLR8XN_Xn?)=97E-qZ09>;6f{>vR@>Og=q7BcB-a$y50k z^Z9N0q%WU{^a*c1uSktqy7E%EY&5-dnu6(ZjozVUSbgda@`DV5e#Q(&ym8qcUz;vd7hXt_q*RupT2HBd(PAZ(|nL_w53hI;;paSM=8nNdSe zbArR1`TUY-fbfBJtV$l;$)uO;*wFB?A@cRm06nB-&2%w_X=MlaOoRm9GX*&8G?gx@ zody_gZ|b&Gfq**Jr4r&6i`K4|#Gb`Hk^)HciDnzMr7&5LLmEN?D#A?SFBirqPstk^ zDu_>;JHGYSj(lu^#Yc>31!?3{x(__X8Rk&u+D39%+L?+bcF&8&1}-|Jen-teqb9;S z^&RR5X~2Of5N{yZ7MK8FyvR0eR=!V9z;sOu6L|#%Rw2j`m)j8okq&wZUB^~?bg=>6 zz<+F;0Ckm7fWTLYlT?IwZ$461X*JO z#UCJPG><|tCekpO-F0=K=CIlGgv1`cI5!NEK$S_81KqO?gSZ9pNrl-f# zd(-!(pG%iT(h28?YB-%q#}Dc*(d|T$i*Zda;#_i=Nb!^OE*!eJ-sp(aBUef)QwkA; zf6@93yPLX7m_H;P9n7zQab4zP2rp`8*YSLe*gpA%WKbs_0WBl%o6sv#i@IV?V>Z9* z@yDjzHX{TbF#L5%eU3Ttv^f{?V&)sjsl}^96ek@F;9^RYwyn&Ntp=-=91(MkA*!E- z00}wiJ?%Z~z2cP(P}jcSix9U3y4quwhHumwEpY=g7+6L)2{xc&kaUNEKfn#Z_f6hF z7_&^1F3fU5&YLX!B6$b%hlDSNhK6mQQ*VIY$LRQ}mc;>chz-A(-U#*TM*SNdw3sO7 z+A#ZW4lNo`Mzq?!*FESy<3=}Z0|uL_mjkXmq^{fj`4bO(a#sVJWv~Z>pB6EbZ0=^U#U0aN%eAqvi4XYU9Ai!> zhrgI}C5MmZ5ca^Ni|RW;vE?vn;}=cj8Ph8!zQ9DZCbx-KP+!5Qw|kOUR+?^SDg(05 z*5jJo&~D{em!vm6Hj3gwPiJaaQJXpwfk*L{=a$buxV}l5xHvRKteV({fd%vbX;b4! z?za2(Y~8l!zO540xPJTVw^4f_QMK+zx74-V|MB-e`k11OPt*IP=feHi5oRD;r;0Y- zY*wZiqlRf7qo)ty5dq;7>f^Wwu{0@ZF`Ke0oj1i(QaBejFza6dO98jT^dNX~SeXPB zF&gSXLADg+0JNB%(td#=*r38Q6@Pq(_zOEH;Mze4s0y zyfWIlsXY%Ge2{b&Hx)U^;vRa6G2f2$e1mU?6YC%LC54)?z&f$uyf2}64=?D-3#Xym zU2k|BaP*lTN7U`?EcLnCJgKQ^ep8&wHRWdH3c1EKZY+Pf)Vu|!H&%*PVP5UsGM0LM zOqtNPv~j^|@|UhLnkNa(Z|v!E1FLysvYhCK4ll#6Q)=_1uOV?A6L2(QHa2`BA>J%- zHYO@`!dR3<(k_GD(Fa?$nPyHbUwTaV#qFNzN)HS7XfvDz_G_p5sAdSyTLz5Dcrb*M- zJkYcS#)42mq@bsuzu;uS#R8?xmY*dl*?=Z9iF z?$^dpP{s1JC~q`c?TC)~FZ#(gKZ*E>35AZ9sI@cGZH~)N5`1uJZ~?EnB-DIJb~B=) zz>aG?g*^h*#Vnp{H%IsVnA^e8a(d|M(U&M0lG}X~=0*OWU(=IYa$pj*()lg9F=I!M z9ZfwL>}ZDIS_<&VceiA^z8jPA#%@eEY-n>8OcJ*spK2QNRGAbRxPJv{8;yM8()7ZJyD?*o`HAK*3x1NrL&6)w5m_^dK+7PvH!a-1y;g_#Gf4t-$@eHHOF_aOaJd|YkPFqSKsYq)Z=G((gs`QfLEgafdLQ zeDw6wkOusf@_+nS%KtBa#a|gtlnAX?KE?=@T%MSMurho_`H72)D-b&VM6!gCP@YF> zWrq-yW3_%TQ|UjUb@+(TK|d2Eci(e2-8D|(4HAb`>$rgE>@g69QP~5+tCPypO1V<^ z2+=*R0HT6YIYQZDFIr*e(lP*Lz+ea#jdSfbrl&eo!W)kATW{N3S$oHh+seM}s<@Pq z^pHMslAI=TqIev)K(C5%GW;CP ziLc2f?n{ie>=n*{6|!kkVxyPtXP1VjipbVWK=~>0E>E*}LEHoHM41Y%zRz+ad!M_E z^nJc`j*fl{9lZ;4(!u5=Urb|j0`|;4dB6N5653~h?2~hkXs_M^%uQ)#5X7U5pA}!K z9I5HX-L7#(L2w5SmECs79W|A=ZrN1fz)6q!=09TBc5(B?7%1)bQdYYjcb{9Z8f`tc zK3l)-r0ulrtPKad>k)(Q5st)(L>P98oI#<$^=stPJ#M8-1oA-|nW8rev<4j#1KEu( zmv8h3a(L{*jl4Ye(RIr>c18a$QXQXi(4LycF1MNsC$1O-}-_KV6 zEV~7zRq?o<3PC9yw=`@T$r&B@=tB8Mf4E*Uc+fI_hn$QR8J)3l+ERXWZx9 zm)yc3_i^`IZUIxzXSj<&ebEcQb(>pZ9laBJ9|}7EdkyCdmka{E9e&J>oF->!vX;r3 zs4<~dfZY$ojoptVa5%uEJ3@$q9%G}oa_ye3lfvuoX{=0+6;H_LW}H7uVSi}B1=O(@ ziG_2B5y=oEGU;i6S5PWW$y6%g_u8+tJd#WswPd-LbZW^&Ey+fr>6BW7()e;DmS3l* z2nb6P zKd$(!BJm!nz&Fw&{Kh*h6NbIVJ>>g|9uM8SbktigYCAV5uHl?yz&Yr= z<4!@PKV{Gw!3Qm~kgqJ{52FlM3oT@tg_K*GEWG8E$%dR4Y(UxTa*YOwb@Xlo_Avtt zkOT5TfL`P_t5$c?Vm9dCG;%-&s!l2dUil`Hl%w}F=H?KA0KK==Jovi1T!TjtN|tq^ zY4U)5^j=4A7*FseWOCwo#b=d?<79FuM88(z-tk^=5Ad%yj({i6MfU1_qD9!(#Jdyi_)YA$LNnltzV8m!j= zn8l$k&1izohl+oD4#i}>jYXjeaj6iP1$q#d+@BTcDlO>}0|BwCq_iuN#eWf97)eWu zEQ~fSjCeeeg;?i(SAHk|9DD^j)Vvr5S~rQ}>G`J|M*T1pO;lDkXE&81{fe3tMa0250F8N~>ej|mu{~5*CEE@4=`jzgNXYt}IU#PwUMn??~E{F=f2seqF!|fJli^n^E-0V4C`eXm`oPj!0TqD*D z)(FKpVh&$dlao_ZC)g$)Z|;fowDt7#^!FSE>h8Ga$J}v~EyGr96Kv6;nJ+aAwYegS zx|G0B#Y?`SLYFXPd`UUvQodgEnRp zLLTyDP-m2mo^^uy_TL~s_1(X{9F0Hzcsxqp%>puH((1|C{z+?ETh>eolIN4ZdB>06 zG_P(+{$_IU3OD@elR^RiaJ2og$J+6yV_(pG^W>n6|KSfwGl~7+hby1o5Q%Jfe&xz% z*Vompe|Dt#hlj`{_KWA}FPN|zQ1TTTFjxtc`F@KCbhFOP= z)xoVL!@|29825f81IXr-=mi5XuZpOiQXN%ESQ~}nPfww8Ljh1dY!@yRlBggk`yb}} z9g54fqD`<>NjgZ``Y*p+pLn0oB~P{{4kZq?lEm2nM8&Y@2~&!pVyNz-jG<-N@kPUtqI!A276kV5Bnm>$;d~zK?TJg zaOxk2O3?3su>O;Y0fZbVCn@aQ9}xyDQu6{fC6GfnkSWUuC8ebA)l^iDhsS z?Z^k%edMPkEAi_W2M75}BR{1KrDV_U#ATuYEySUb;X`!PjTp6G{vNnGcZnvm-rkoV z^c!)8`cV+=rju5uHpJZSQd23(4R}PYA?C3K)@PfuO}Xpy^m%>R^#nsx-LCMxfW`OrZEr@!w9kY*`Skm0egg=+_WGYzl3Kynh2!vGAV#gy}-Z7J@kK?OzPd-T&847>R+hJ6K-$~^Djqk7o`^7jh; zd(-pX)pP4l?S68%wQi=EZ#sNHo7*_A!Hy?>GkteYDFkglDpH5AhHh3lxJgVY=}WVi zj5np&Q_}D+>k3Xe8ti2qrV21Ka(M+MHWmR7%lF@ubz8YHWxd^E&tIEq$G@zywE;y| z3b$VEqVtek%F%VC!C;9sH(y#pX-R1eOlmJRie{EtONAM~2Md5>kw=lHNhpcD+L-0{ zD4p?0*$gr3@y92u_~@f?iF?M~_KQbC$G+Kla`X6+pD=dILr1E%Go~P&EwgEPjGXM4 z@{^BuC%PuL3cF`Ly?x5ayYiu`RRf778PPTE_w9y+M1YoSX7wyPk6V;Ex(RyulI>6n^iZ7P$IYa+q|giI(P8&^dIDuAXoAQm>I!e+PGXy-&mGcw8$ z7Q_z79#=N5oJtCVS+ep~)x-^R8!TinF#-JcD0O-u^Oc8Mc2!s3Icx3Et{L`25Aytj z1C|-PUcPzWKwCV#Q63z(V9%`GMuRR@ZOwJ;KKgP->cKScq+M@p+U@kDJBn1hxBPr> zYktWCUeFQCxO=aBE&p8p2BgRdd|iy&i9QK@Ra44wgTOI`*1WoDZ_@y{;$&w*pk(5k z3}_(H#LG%Dj~iv=qU?%{mo$<|@rq%jAO#SpBsjmV>Ih71-Wnlnu-En4jtO*T|Lo8^j{vYeY-HWMMwi!>p_BnpPf+X!@;e)$HTp#-^X$03Hm z66j?LMp#uL^-hv7oJz`2VjXvtN~15Ms>mZOlw~(9YZ|}2y|`#z$+(`vG;wK5`Se;} z+3ee*=B$bZi!*Ya>hjib%0>qou}t03T^#8A-hzKMr|C8sJVx?m^~$Lwd2M|Y7aZQX zG&}KeaeK8-=6lUuIx#P?+}ApP6Xfb$;16~~CsC=YfE{;2yxBnP-CR{w!TC%akb@j~ ziX+9Ghj(MpjHIpx^6Ov=$`lW9{wHXnT8X0l74`U+?6}%^3|1b*Vze z@GDbqpBW0xynX7_?K6ssW^A9jX6@QFH?LYX;@!6o^B3SRz-yz7&l%hg#5vqsNFSQs zGM`eMQJhln5EXHS;8hTr0)6Vvn0cZwn-wUb^|`M=BvZ(w zApJ}<3y7UY;FlM?YITMV&^o2?Gg%6%L(hpiJhYiXuVuGLg^Sd~!^Gm459cGW9HMRN z!Ied4R@&W?(}CYhxe|KZgZPOEe!yqwsbED39OL|~@C#&V|69D~U0yr^>?1odjqGl4*=2GeS|8+Z_LxgT)@JJZrKhJ@TJuBIAwCpJ$!*mcQ(C#JDQng`iMj%% zrr%PeG~?PCS0|(O957ToQlA$@BCOykVTo%qzv=dOc1|pqym-U5$eK6rYaf2wAx@c5 z)Vyx)l+0Mx)SLGF!dg9LPFZ;Rq?#1}+$Vl<(}IDmvuo1Z_Irahx(dPQ#4 zqA3OZ-NbIo^E<4*JZIvMxT~o+<3Gk4XLFko@I`BB9z9f0O)U!ZM_xqq+$$6Kpo@HE zCcih6GiEYqCIe=Ku!%V|lNe{hUNuXLP=@9OjMG2NuJ|6VxITGB8pa&GVvU%CD^7Be zOJQ;*Ox_BU<6$xoCJ%;5Z`F=40oGq6CCxF!dg0GZW|x4*bAG4!^_Wq^4h%}^*8OATyyWeI~LWpO|<4|Uw=>M@LG@PoMz3; z{cjDgEts?tjl2hNj%Wupm zq>TKR`NM1E6EDBgH%iJsLU}&l1-dWDv|5E|=DL-O)SS9lElTJ{AbwAYMwHspQdAY~ zBq4+OI|_4TSYNCIOFQ|@vQ;Ct6SCffO)>o6GUdUq=i=(kz!K0}H7zhtTGg!8V0c5| zpQCkqFTp|vdA*WV_o|IheFDm18OqZLE|W0?_IO8jI-_?4f#OSn8qBBhE{VaOI%d`} z4ED|Z>X8AkUcSbU9~l%LOW4*54+!};9~w~)Kf%13#r!T5MyA6SD;2%G91W@N<`ASO zT^woQNF_(mb3l!;b1#q=@y^gq$q&eLN^fDu`v0CjZ*F&bxG_7czSKjOPoFnu zHXa4D8%jV($(hibQh}JQpQ{l4rofp1Z<7&)jL7uQyX{16Cw7x3!^3-AM=WVdq(zxz zVTBPOW;uCCGc<`kd1rsulahLsJ2G1wmfEaveb7`o^S0)!7N@PwPkDkv(>Ez>YV|&~ zIXI!S@ba(b?(E9ds`jZ&0kN}K9>l{OdQOtfFy#XnVUz=}#f0=prb(oTvJ~bEVXh;srn=I#@WJ%biiS|2B9z0PNLlbu@`>+! z#k_~=6}S$rl7pU3bI*2mMTcSf>fcd zRIO6!WiKeL7K@pC!Qd4_0^ctXmQklQESHqTh*Tj^iKEg_abqx^m`TMfs^m^*bINSg zN_b0KcD;oU5;n2#m}3(ErE#pntw!WO#p$T#=7>o2sO2&xEMdj-oc4;Ah}t-M zybNkjY2`+32uv@}nkX-gic_fYk5|5c7r(%xwH=G0)JpLpDcA{H0pVrT>CkeIWQ34} z)q*UNsF|FoBX3S7y2RBxvjv-KM}n?S<_&iP}BfWd0yj zMwXbulcTKvlb2Dlq#q3taP%2Nuy&GtrOJ|jtr+vyZS1czMduBF>?0y$_qh7USo+iG zC-;HEiQH2}%;mrc%^!81cJjrjg9q>gqQPm3QR$-5^y2}gphAw~KIB-Kc7Rr3pkN8; zFKO><9TPuLl+3wz;gWl1+DxwW})w*Xm1D^Xsqdz_rlzPzM*~P6fSEh~fcC8M5Hoh5EFqDOTAr zg}0zQeSV_LHMw)D&*qvky~~Hi1_u8#eh2I5=R|V>b|rMvlg5<16dexh%*pT2rKVV9 z$K>9^inRF(1-leF_;uk@)Nicf`w@ZGQnVZxEYIt_Z4?~s6)9iDd58Lf`?tac^V#HC z1hWn@i|HJLTfIIB;&?gQ-inVKQxXpyKtkgCPyN#mpL*(t$wb7;mCvlJ`GW3s9pB9F zk-r9=V@D1zPtBj#Dh0vWo63i}XItH69H`8`|qBzBWc_ijmi1?ni9BSrm~pgDD_k8c?H;HP;D zO0_aGcq9 z!SieL1(h^Y%ppKmNjh89J4hCEQMb{CQ*SKM4UGu~X`?4)c{!=CUC~x}`1_Gf1*tW) z)hW|@dS>ba!GNK`2bKN0#a1O-n;k)2rjamvV-j1e zEEv5==+OrIK!XgFYyA;Ul4xHF$sX7!A6ecW+h~k;P|tXdx@Bs&zB`m z;)?&VdExNauv|+Z?Q0Lm&tx(J5Ua*}lyTJXHZ4$r^Q3^^w ztB_HaHVjJ?z<5oz{b6E&WtR$vAPzKu1ZR+b6Oucthz}y1WtI9vpq>rWPCCZTZOU^5 z$9XdAiqrC@EQ{Q|-<4PCbr<=e6MN=2$C_Il!3v*oLj8l45lcp4YDz(db%Oj|cSUDq zpsqB>Y%Q#qm{&EYK5NI?yor_hW?Oz)U7&JmMcT;H>ZYb@PjN8AsK}f`{;i_iotKra z*QNOj(#k6!RX)tzW@K9^D{!@#Sz`f zkI=k*&(EDPd4?-DoSGG@hTrSpIuqNKZ^*yI$*x82%ZVey3(GeM-#-M;hDIzS@Nc5r zR5Z@*<8I-$b9=epi!;X+WTaK5OmDw+b>F%r3m4wLb;auTQ|)Kkh4#J`EBe}nIlJ5L z>A8o$=bpx#EVXk^yrs8=Z)urOTf*t`>YM7aOLy+s)3<5K>eaXQ-rCn0i>>b4*2mB5 zBYjlV0!m%1udlbewe_xByuG2`NN-=SthbjGq@`GNRb@V<5im<^6KFgNwxD02Ug!e` zPXkL2`+*iCQ=Nmp-|Qa+1<}{g-zI;Mc({Ifv=l5r2a+3g_NEG{I4g~6cUGMNO<6{5ihw&Yl4!XDB`_@qTx^v9B7wIHV} zQjSTM5-3Wv(`LwS+L*v@AJskxRVjpo@ceM+aF_5W-{b{TN@ne7&+nezk=fC>wK7r{ z?3mc>jy5$$qm4~bg>ATNc)IY+H=%;M@soGVoVcPnXma~)$1;n?he{eJlx>PvC;nVF zX;PgyX%fZw1F_FfqxfmQ0@k7uD4$muo*;>zM4STA31}S+qH)L2XGO=gaqGqC6n%rK zj;mHh8yZrpqivjtm`v>h?IhAp!qL$VAer&py0*4f#DG!!LHVfTxpGBrYi?_$JRT#J zarZI0Y)BGS5ncAMED{lTIi7& zF0~*JkJcfhIRR(bE-cqqO_^0ZxvOaTbEJmn#qDqH=zE|mQ=`>*y(#6UKwx2c>BIsn z9@~ps#~(Vx^GE)&zaiM(KXd8#*SA>q2p_F{c3tg^Smm;g@Li7a-lq~@zPGlbXUoK# zIWt>Q3VlXRzd1Fnv@yruHD_L7UURvZ05IgNv+*sr9B6M?-BHx}_}RO6Q@aKqvKYLX zOm2_pK8h44&zdx6KIbEkFk9cv%zYm%5o+}LIIo-4P3ki&ju<7_Ap7{4@pb2Y*NqDe{sjyP1ht2R=T;s}+Ax&C}JN;$P@8Gfri2PNsH)>jkt z@AIFxJUy_xAg4BHQ>*S5^{k*4Q8)GS~|BDlW=10GPZg5OdW9&uDmQQ4QuUwzPMN=jx5_tc zx^?{g9j!h}5^)ufEl0Kwd&u7!FsPHp4kWFCMrs#Fb-P)3x zp*D{43F6fVTPb$YosAC4(yQ_J?A7A|6Th`E?8qPUYHsAyvE8-#S;UQ3>t8~e%xjk^ z=_?t}_}r0Cg~w4dluc0|i1E+jUQ52tRvXr3j=nDOF>(T9`aScr6Cz$084}R%S>VgC zSjW_~54KNCEh}Svo{@vx#t}HGagK-_@nVaNCP~nL8Sjov3DrbvM?U2%;JaC2Ba9bw zY&%dZ45ieAG-v9LV$@it-$Su6#d48>CojpB3c2$hpz+RsLV2sP;3%ygHGf8P)*fET zD*xEBmh%<<+RhRrTQf4MJ4%Z?>-_7)lF~?3Wr>(L5^k?dPp@nXhuiSIqHSDRO-&g- zF|vKKrTk~GY4sRcy{NVq8$^I>&8bu})6}L|YHGe7Ax0Z^szR=l>muehbB|dtr*olS z%%OQu68eBP&dm>hK-uZ(Hk6!MG*qX+1S)=#Jd(kyP1WnX9f{xFa%)YMH{dM|jh`sn zJ*JiVEv;+4lO07VR*NY$Ef8e9l!#U+UKx!yz{Z+H;|(-wA;*fsPstYeJym3N6%nyj zst5>7s>nGn8T1nG(H640g@`T0*g|6%G>B0qtr5#%(JMdwEmv-1S1uO=-+kd)vU-#_ z>4pnqUcu0)2;d~nfUnRZ768ty&X`3={w(#9@>AvHWI0(~PD159<-89KK+6mxl8R;_txIkRozw9)qC~6FF+S-H`vhCZ3EIRRD!tR`T z|FZdD`s3GEgTdT?@F4S^Ts6GZ);ME+!^JnR!LNl(n*5pkDMnfWZ@0ofBM``USxP8u zLqwoLl~%Q@YG2jiDmjWCE|so)r9A0ZwZcs4>+7Q<#l{9@$GAC};#eP&wsj1zULX6| zs#m#k@?Fmj^KO4%cz$^CGaV>{Fubm6l^t_hBZVWvjy-#})2Ckf+Wp`=JE*-*=M;PM zb@S`}Gj3ln*)#E}1yuif=FAzW6u`Z|G0eie5yM-ZVbElX4Y0iqcG$rVq0Zh>ysmgl z@%dsYRm_B9tyl~W#ABed&}UYZExPY<>a<0FMs#&ye)m8xIcQ*P&aFFi?8!bD`u!858ixo zSKrL9zHz7Y?8UCb$D0=2+baUGF08#25$}O1D`^E?^ zk4oAQv;?O1-B>et-;LErR}S~Cot$@c%&5K=?Osu zLLD>v3ht0U%j@n<#=ODa8?F!0Jx!C}yW~;+9bR5bBBuJQ5m^#Il_DC=K4mA+i*9rt z2SJp>;ofQHQOxT3)Lbc!3TU!jenKV+(iN}Tv_8-u0$JW zRF(D4oSIizrP8D6^82wk;&9^@PA!g=qpc_XK;uGIHGPL+C+?;=;Zl$q5WM*6QUl?D z7}#PEs&jubzYdXMR{azDrk@-}V+@5Sy6K*2Oy^7A5v&|nU|H_~mdWoetjh~EXj)y_ zwr$E=C2eIv%oA02Vo zkbcZCy5ssc5u(&MTRdkahF3jFzdnaw|BWO0V_1oCJ+BJqRs=R7hHAWrsnSeD$t3V- zFCD2WRj#r*z|aJnLuY6bCZlsO>e6hyF6|q*mn#^Wu&)~zAd)c^45eKjxpkoEwI;T^ z=|GdXqlq;&;r z>={>|a^_&ypj-{Oo#AeMNERRM9y%ohA@1Roe^^I{E0pIdhDhGw1tFkf;MhRJM;m1p zL(0#BCL%}~lfGO&Zb5l>`_A5tFWi;r*!udt!w08}*L{2Ky{~WWNZj?p#@?On-DRn9 zQ`aZsRX5&uW9w~IFIU~xe&hW&R#ha|D@B}I@}?Kle>`w7efFO=*45P1ZTu$|J$Qhb zUc8BSxtMm}>3bje#r}DP;Ob9LE-sw6{}&J3d-}d<*qiX7dGV!MkZv(1B(MhGIh#a@_^AAELV7Nwn&qFpg8pNAmLMl=34Zgq$}W z&-hr+|A5&e9yeTx&@HmeQS{*Tp%F9o7`K#d$D3_b2rX#DQ7*BK=^fuc7b-Rzjb4+2ke;?-=^|<(Y!r~VE12(VMprNzyiG)43q$^@$;x5}h z?27o!^FSx?zYyTrW^)7w4a;IetOgB2hR}FLLC=uAF_w%mM>Hk)&|$@`!2||cwg<#b z8kVvqXo!UkQ5W+Mc{h7HLLf6_;kla(Q9-q#)R0hWJz=1E#Se6ww#3WaF3?&8ivaf3 zLk%&}!f9mnHU{aFY}_$+M|yj)C?h6p`CS;J zs)aStNI3V+Iy%aiv_VfkgevH;!1aV($&Qys3ylDdoz^f#Fp01x%K-zNF1)fRP$pRx z81diJ?=bIMr*tZt-~H|=i{lq^^8%d-2BrM(j&j+2@WEBAE8ulC5CeZD`Y%?Sl)+$u z5%e7eQhs@Pbu@1ve{oe@NR`MzQz%uSpyd!a_rp>?8h>OP<#Z4W3CD?Mo8BTrTn$~% z0lXIXPg{_VL8cr$?0d2Xu=k8*CE**8vSv)roDJXAd*;kp*!I-NPxdM4!^gny>bU7! zA3ef*=RESKy>k}7$fUhGwd>B0zjY(uTCkN1&%(Z04;Gd(VR520lGmm5U?Rh|u&D&Z z3sz&NslPn8*Xz_GiXoY2d&c?_GCg|I2ds5w)#Q!!7VExs2On!@ut;BEo{%iRs~Q?f zqovu>0ee@CjR$nDAheMC7e2GK$C&<4B3-e~-ulf28Dfj}z*ip35?l0rsj2Y`p0-V| zY}c7rwtVmAhCiXVp!G_-WVJ%5q8#Db#B`H^{fpsO2xuAEyT)G{#REq6b;dRd-x5T- z#cTtT06goaZ$UJSDP{pgtpMmmOzTWrOec^*Mtiy3UAghS>_Y_7H<>Gd*lS=^! zJX}7LXm$l$Xd~!zis)h^I!$IWh>5#5h~wlX)LNS8Sg{^~#@G}EjeeL$X82Zfp2rz*6LeG zLcG}?uw0OvE+xJX;u;P+p{IwA{eT@=uLNU7_VbRS zSWqU?CnCPk$uI5*%pO`$P>>qx2d%NlG16tw7B{@F=GOGcNoGoK#^&+Lf9l$F$4Ag6cgX9c{yDlf zU2d3kEv=&X637a99OmOh?KgF@d$g^?`HI#Y0kKS+F0O#4DOM4E48(xRV|H2H7AKk$ zzn)PiKh)?=uwrs`a-T%el?UpiYl*ws(t%o0u<$@f9Q_fIX;-LN1y`}?nJ7CRWkb<* z(OprgC&~ou#a8H-%LC$NQ3bcA*+5*q5GT=lLdm#5b@4%>@pucWfOVjG3nV#4+i>u3 zx$3gz@+Q~gvc|cy0?ull+&`*$uckl3Cf5JGb$>?xbR8s4$vXo5L#fMph%;7M2|bJf zPjVvR{Bi;NaxRWmUfhi#+VQYEiAP57?^4}WDrdT zfR2pCDU%d*@s1+d=qWEOrbViE!=d;o@?6-+c-06$tNCt*z9aZO)@QvN1{+UI!NGLL z2w}!oD;<3jV^PnhoTmOSq|&?O>FI3?x6EnIx$}pcJjG2t<8xll8Q;@X?AgRVW-~^H z*=w`!UDRe^zr6o`__HmGcFak)vsb!Ot^N^ki2GYpUFi;KfW4UA?}+t}B=|M07A&$98^0B#rYh*p0m`OPL4ucz#7DfvT#OyiIfzlq z$Z)81SE(2-?I{&wB(J0?5bE<478f=Z9w?N-hi(WPdJHK84idF;iANGK?d+&7Nk3B+2pI3wCwuyPo>l8H?Lnlxx&Bc*d|Y5&6Gy@ zfZRB(rpR+AJ905?Vg2Km%&axD#Vb~%pD@*SFB_l!E=x9b#hoL+7cI_sS5x}1csJwc z2I9rMYNQl-*dHgnUkG2O{O9jd9Y(di+qX#%+SCsSy>y5L+Z-Qzc*N zVc~=T98#FhVxwO`M{;qUZwsm_B_D~p0ZQrfRi3M4dn=Duiit{AUCAtl0psEd)fP|L z^OfkrGEKE&L0e5I|My3D-wr=m7e|dNE<-XnXnAsV zS-F2fHO2Z(?y^vN6WZhKPd@_oBJWJD1>^tk{u?wvY|@WQ^|zNno7|^l*c66B#E}~Kp7+HhOMHJfdoBAcLF~hF;q;4ND=o_2zWw%OXk!D=#W^xacZMlmHLGX zaRqx}HnGGpxn8o1#}?Zm;Y2}bcsN>OzDI10yC zPh5C?Z(V#+ZOmp0H_dFU?VNGrwB`5qL~55lxq9A~-s09}58iNP^RD&-%W|5V>O6PU zFKo(fpL|_;xY}VYaQbR$syuyPoE;x+0e1IEk1JA^<15T{pi04LQH@89E)5t!v8-#T znfebDcdeMxxuhcxt-cmd$h7*Zxz3qCv7^l&ELhl5IISgSMwc9;zpN!{@3`@{sWUcS z(JlBaC)wfJoyyDe)CkO~|7w=XwlkJOeq( zP7}@eWi3GF^;$|meDAsr5GsJ;S7TH_JWaC`b$Y9+D&5WVI$~o<;IErktvU+YD(Y^U ze|>aPr`=poTH%h&UOe1-!@5LH!JJ)dn-_OifF6F~Wj6WNRTKTSH5INq>K8TTwoSfH z>EjdREv^qcYNy`NI7%BoIDX2lb7~#A#eQEb#|~O)pZie@EPD{JRCUfNtDZNl+#Tm+ z@}t!Ap#oE=y}8btoq9wXm&5Ks3nNsDZQuxm{d#9>Jbna@oRsa;g|(IspS34ZR}?^x zmH3%BI~`{rk?x2O$Hl_jLCeze+#$d-R8TOUEtE`!g`RLql|7jbD+nR*1*)6WI5cQ) zgH<=$TqPU)^SP+qMl-tEy2)CJR!PeGv8kUuzuDJ2vVG-Kt6B{g_ZZq%J+)@V6RXFY zq{XK86<-%WxGL*I9aTn*onQXZop=7|%boI`$y;9Bu;IlmlWAm>^{Ip>?gk@j6eFt- z`$2;mOT^9~b|RsDYJLu~Jq0<*Px9F_`7h;*YJNDs8vLR8`5v^=&^(#+k3KZ0WM*vH zk&WF;P=tYC1B#u2XETE1^~$6VC4bVf2ky0u{LuWBuk5(jAx^O0cV)f@9H%31KZg%# z%G3MzKTVMY^0P%C8A8H9qReQp+Svs=@;Ihl=+-A9gC*p%!}(0V>7M*l{^opx(TrZY z=z3-nhJ3QwB-stTRVtPjRnl-bnJ4GDHNKuhvS|{yONJ9~9#kw_tc+E0kYEjGs8@(0Y*k^e-K$%`*k`Tzw|5kLSn^jsx17c@9tm{5cVw zx9;I14H3b2`8l*A(;o8xdLI$b&J5x2kY5_iZ<$eD-CR~mvWS(ICa|L>#PbOjPE;p4 z66h7k@)P+9OH;FUruiHIi@F(T?P_lBR=b(n9qz8~?&;p#eZ2c@_b1&ZqX*0Rx|Zr0 zGfJl<#|N8A)fkp?AiI=FTZjsmX9#iXasjuz0)LG&MbKkAo1NUbuVNvfV>fRYMOiTmI<6YI*Vx-nHii z+_FLT2lK3Nd_O%aFEQuVWJ7n*n~2p+Yba7%uemu~-EQgMN4{Qq!wuIL1m-MU5EXB_ zP|~|~pw?=55GQ0sqHG`GLBP0gnuUEs@8E6}Zj^^HXU@MD#TRAIkw~>gPyBGy#9f!} zR$evKLdQF(!&(it&WT4Ro}4J|ng~42#4QuW$e^|~5y)ZI944DQCehf^*q#dJc%{;! zlu5Djnr+^a4n`q)&$P)9qY=K*_KDR=h^#E&3cxm6p83Fb=ZiYBjYDUv#1j27Q1ZVz zeaCBGaLj0K@3L=wWBb&p+uzt~?`m(J;n?)rju{s=Q<=YWa%1gOXWteo`rp#0O{;C3 z?4;6v@q&N@=I(}jIttTE@P zt8PpNMXy>`97{qRFbDMDF8`X92mFk`CJoxWNHPTcE6&KmV35Dvy6W+id0b0dyJ`O8 zYsa^)-m}Qm-qt+cJowm}aTm7@Z?d*F)V12~ShMzaTWeiIt98?`SaRF(uMW6ct14Su z17AI{al`Qk@Tjc3%{BPI_cwg^RJbA%DG&eTJJ0+>Bp!)WhW{aRiW(bD&=Y~y{gQ2BsZ(82jdE@flL|?LR zs88zaO9{)h<&ouEmhW1w7?+=$$z~p(b9#`Y| zISte&DOrP+RyQu?^lWb4Nswi1sLjo2>{cX6r9vu7&D{{8fzt*7lL-wE5BUJ(V5`^H zYp3RGAIv`mf-s7$SI_EoK=2=?ZK!T`8Tfk++{pa#~IP57DuUp%a zmvZw2I7IORWnz(JR?w2FQ3q`DU-^`dt5Sf=o}SvdZ`q&80G7|0HOcRvG;2}~M*+B7X%)+uHA9)9!-*w0XM_83&-2SE6QTobn0!bY4bq`HeiGphLL$RB`|_*$v3mXLDliKRdB- zvSfW;WvzEjJiaoSdS>&Cjx}HF4=2VocsiVA@wjW)Un-0jRy8AK)~3XyB=319uLayVxlSY?ge%tmicV_zmSK&zptR~#oAcyi**G_DRk9} z{NE(yjdJk2%kc5tU^@E$Aw5sgE?C5$Df=PqHWo`j=7POkeW`|hsfJakh8+~^N z`FEbBe+HN+4Zm<`2#VJPLk&ywSYC(>sq58HIMfl6dP2jY&7r-a;~^y!YN)A3c9q>4 zW2<7UFxC(g+Z>)<4Xn`9;F;pN)$=gA@icfm4R9dJI)!qrTr`%KkJH9Q#?_3IG?iYr zQO(StAytieiakwWN33!1P&$HYn05p;;&Ixd z^~vX{8<6lUU7-`w-;(Q>k7cL5HkOqC z_PoEcKRd~v-}t}A05Gz7srxfx%l!$zbAS@{8dY9vTF$Wu8;Y!ph!L_Oa#4vBi4+|w zV#;97(!!{a%0r(YpxQK)L-J6O4r}8kE7i&oC>e`mOc0Wov@=z>~Wke0ak0*bAjtNEc|-=mW5rlmaI zD*vtm&FMo$K67PlT~jM)P?+$U{0r%<@*dqu!nwt?iyFfE1rITZN=7grok+OsT9?NI z0EoxzNE`=)&=xb$dLICfh&wn%H-klmA1P@Evks;pH|7vKLqr7iRN|~{8`rh1`NFog zs`4dwJUClZmg1CMvKrh+RrrA%VfLZ?uK1Rr5mjlCgv;vaKtcz^Bpp(ZltOq5sPdn{ z7(E4mAl_pXum=-_$RXH=Q`)QRfXi+jcjxV(#6uJ`nCo=916AJcFq)@des$!W6D}jc z*hN8dWO3nu0@ABz(BPxSXU0eo-y+oas9m8PB_q#U6&Pz3n9zOn6;#$e$bIy7y#7tX zM&W*8FWy`aFdAbtF>ukcQ zvVTtL)S{kQ)AKL>T8#CKK0+mHSxKQf*| zecvl?f*sWGdPV9KdOhF8N5(!9|CMfIcIh*wMq$9eq#o2-hw53RzNTJun{0q*)D`bVgBl`E z2!$%X8dY*HxL>^VtLKp;!uNBDVvD=W;P$zT-ICeuPYEtV4*q0_;MZnDz))t8j0PVP z^01XaaR)XgSVQ=R@3iUjC@tkJL1kQ5S5KwilvBVj)>CE({ZddK*bD#=P6QraF@od^ zEjD6qlp>LLI!e|cFIHp_a*ou#`z#1Am<*`UPu_WT=g$v3nI&;3mk-JzkT}GrH=~>| z(6V!4_sk2^5D`6>J7Mya@QiNx$mi!r-b0h)> zjUi2~;~K{TIN8=Q0M5rAJI316uU!+q`P_4t!JD{i>hBW(ewEOh3lr^(0jg=58yk)t zkDZNOih(`Q*9p>${sM1OmeeGmA9BK2*rmD{H+76?hyyB3Uy^))EFyJ1n6_}!oV)QP zA!{feh=E`b1UkJ6pLt~)JaFk<#%EH)u2F2J65TQKi4M+`AKUR?7k)mbO`k|eEiQex zP$1l!=m{#hhTI%ez-Yvp3m9zqgQY@?fZ{-5S>fqIhAv!cAv1Vv1NOzSKu+*bF3ZjJ z6c`SZJ!ex)o}n6R!Yab5D;RP}gtn!iGRnjVkq6I~uSpcr@a0Mtm-c&JS;P5b5NhTa zAqn5#ir(xWg$@;uG~8R3%if8m0$kR7srjv+FCp% zeB+vR^$q)<-6vbxh8NF?e0|o@;3$KxqHOElS9r@@u;`+@l->^(zDFzi<|P^$v1yb? z^26cgu4s91ZhK8TE9af^+UgtYs?~;Y!*Ijt2HDuqP+geRe40ZIMR}j+ zk$`ttWc4)W1q86m?n~P5!4m-BVm20m_sM3ELfhpCSn z0Z#F$_Mw^Me3S{6k3O7h^0m(#8n@u_wc`f*)~wtRo;a<0#l)}}oVdEz8C6%VS=+za z6R2p56ohk|wRGIj^l-t1%3#;dx4&}e(u%K+oldGB~<+1LkR{rkH zPsv)uij;nc6=?@My_c|fB6^N<=*cc&pKuP!Ko%p}v1nHxRy)+a>TwktXjrwhmj5O;+*kknODP5|qVeGm0rdQMi^sas-GXJXC!C>)3{X zLV>v%pTC|I2P#1e0H06@lBQRNK;IbALBtLnEIe?5DbhOW^TQqN+@AEY9ce1W zd?PKu>m|O+rHP54M>86YO2MG3^h7D!j8>GQM#TT2kOlPQi%aq%DSt>9qOPtTc1gvT zvmhvwO?JCtP8lR+jO++BfpzttbtmZ9K?}UjFe~o_#q*J^6it1-3vq>=-=%{zYM|5AXTP%WVvPw=6L+WrsI@qbzdIvda zEfgHW#!5xldJ1nn45ez|GHGk2R=NlsQk0mk;*ld5M-1A5Y%mbgu#WH*r#_gRpB?$# z^CQ1|ON@^E_LOLOUbLJV`7QhQtB1v3j0E}Tr{c+xaoNw2BV_TA4#?@d(V%vQ;APIl z>JqJ46U!W}4zbwWWEM*-%@z@cvYh$Cnwj{Q#Y@ti2I-!X*h><7Mq;}qww(O=0y2Cgc~st9OVslD^yK~NDW}=#^bN1Ez5Cg|*U~qzp4Xlg zD@WeqpN*n-*)OzyH{=JJ`vejr{QdDW`bs=8+ebqa#J%7VAgu=DXG1G8%CU4&m{{ zN)sQa>6GaMlVsu}GL@S|iT_qQC4B&m4B@qUULRw-%nDi;9kBr;d*)ii{ReL!&)a%=bptUU{QP8kA-nFCXP2h0J8}du*Z)kXNE=8?@aSWjJy>*4WAp zcS2^iP^U{1_VionXSZ!Uc5LG|HW3rXK0C!0FB*9l(`DGtZhD>_7+FFyhOxykV{YMz z?>Q4zXBWYYpT~Dzfj8HLaG|Bx#azZ6?#JEFxTUbW+TG!ntboP4RRc9&^jK{+P1PdM zPP7yC3SP}C3ZS?g@^10&^PcdY^BRo~bdv`WrrRW1WmV&Vcdm=n)P?3uM@K*lv6GT= zl7*5PB?5Jb*0B;@6EQY7_<^yvf)f1nnj=Tn-1YHm=_Gsibo!5XvFA4FbGC_1dx_c8 zzUi3Al2aHZc|W9D-((2h2ugvDf9++~B_*ohRR6AFK8; z%?I3h%4RkT02gar8Z9#${U9nXi=<>GjIS)*H(4@crVQtY_WRfIOFkw{C?*pJgub+N z?}^%8@Fj*B!vcfL^v*l_BID}|g>LYa(wnktjI6Qqyw2~QgzAKME1S%cyVE0pa**Z@ z$xQm5XhbRuMw*X#`u#S$W>>XFg8gt1v`JNS)Es6;;}?%=^!9)#Q&hb+>@nCVxXn%H znXZ0Z`La_}_d13ckAv~vpK0^#=7nvrgrs^J1ZIZRFy?yV2-}q2rpen`(OcE9k?E7CS_{sVr&4`643n zno!TtYG8Pd(VP?jIh80i1xJPy9tuJm6x5x*;}!{%OBAlK%DD(3LENR$DShx-dWgMo zI^F#}-t3;Xr;*oaYeI*6apboow_hNVv>$R)g_{x`sH9@-R(212n4y5dX;%gW=YKdq zbBZUNOw>?#fnc>$=I4cGOsSx3MTwNdf-!)bn;#Rv2}6juqYO@uA{ze>X#?yAV<+wMB%)AfzslpwJK9kTXQ+`72JZXCy zRj4+n>@0MOZ`17^#_iSM=90L*UCw<@z|1=3e$9SCOnz)+&lq1aimEYetOlJb;BIDw zVUl_jhM#G4g3TuuPde9d!(>G8CV;d-!pq@%2W}Ss&{}re$XS58qeu7gYAe0*upUhz zX(EZx1!>xa`H6Nj!#S%UCvy(>jp6~meCQA!g6DG>Q;fnpaw;5XWduNJN;NT)$pj}t z-xIhLaVxqcF~{2LxHW2+PCLNpZNZI@Kg-BaRH2Te65LPoiyd0o&vkHlB5IO56}0pf z1gp~TfG7prmADJ{azZ_$o>vuQPSHyXPzEXav57rnddVcJrm(5nBpFO5!EQ0A3OB(z z>QC}XCO9TMLfj-jStwNUr#IJrRF{|-PCt6NFlZ0%4W5_reMqZBNG00LR%Qg}abzGG zEy@`*m4SewaG+>$Y2J_u5@Vn=RD3y$_P{cT#fxI8VkuhWa{E&D5CS&Px{QOeo{Njz zEiNkZ1Igw3bBDGDZ1E?Tw{wbW~d0)^TlrOgelq2}Lilje2B+$oQ_Z8~Dfx=*D(uBB{*M z>SSU1pb*g_Vq>H+lG6bcUccHO?(gYO^>6Oq+kdFvXpAFYeEpQ6S&Go6wME*7+Savg zX*0C7p&l!mCu?;zDYr2NZVbX-RHJE1T`S-7s!=k{hl?g(6iy`ThRO~yktrXqF{XT+ zjyW3(g3>cPdgP{={y@1hVniYUlnzS=c25~DnK-+$skdlaIIpMp!2NY!7`OXZPxj7y z>eB<0ZZB%Il-AUHZ+m8S{kV1e7S|N_3^otl-F@A*MXj&-%Ey6pLBu-Se*xJpA)r(~K*a@9W!M+0@>$ z>GenYcihy}yl8u}Yujy0N}`K4-m4?|UX%ADlWj*{mlfOL1#gCo;ZLz{sC*JRk64=P-rHKbsB^>fcXo`s{m31Y`f|(MGs^qdO%CNxUYjhclUirg z&c1nKVz{p=zhP>Lr#UaTH58b{K5M-3x>i@|?p4d~ymMrEd6CmU_tBqhxci?Uy3t$0~ke!9tB>P z2-{9@yujFox{92L?s9ST=_EoC;5xvMxF@$hb6p=;LF^gV3|2>av+TmEw+h3F*07zO zv$GT;LSjU3>M9SYEX@6^L+A=BS}fK9L&A8DP9ui1f39JfVWUCf_LhpsgTYG1$mO#X zv~CoA<1QA~0#*SXtT?PtM4dfJ!G#$W*W(mTFObsd6hA~^gz&w;EPc1M0DC{(-OTfL zcF4{W_M}~`wlizzA=7cwX}DH|Q8by(D(p#x?L?kgF?I?=;yUpR`thKzA#6HLpjtPS zd|xn#Nr_mmd5j!*SrCrls3cE<3~rcq{O%AP#HmR)#!pPPJpGE)n0{sNQCJ;&F8-R_ zBSQG%LvH3>9mnBECI~=oyo0-SqeHG6QI*aE;KgQ-QfD5?>C&zUCq#HS!F%4z&Y}UU znDp*~vmo_&S&bLG95AKG%GM;Oq$(D#MKroC@EQo5LS9`52d(3V@qwd@ie#}4aFzj& zhaH8u07tRw^-P@hLVEr8-_RmKk7AU)!H5RjGf*b%SMscp(^ZQX^aKJu3l>+2wHKbF zQRt(B(Do(*pTPA22REVsGJI^idAFHuG;cSHe8%mlvbUNI2E=_u9kbN(9hL1;nMY+X z{WMgaeX(+X&4I zhQ~1&gh`@&28PL*5MeBOX_C0L2nuU4$ol^wJ&Bd1-+MXzb5=}@hi{4he(~<{`_tcM zH|(zm!Qq&B3cwrkIvjr1X*eLV?c$>d;UY$kLP+u>vL$;oQ$8R1H?y61mMY!1HSLmE3>7%ZPKMIXQwlNR;T zwEZ4s;J5tA)G^DB^potS9k7P4oW7&TXF&@$%SW)HRc1)s+o>|@Hlqqw^m#Knj5fh) zRxBpF$%bDEX8ckyqZrg+aF{F#JBSjWA1l9BK8Lqa16~xwS(x3SSsm;=|~x4^-+KvILviqHjYS_qC%#W2<=gb%NhSh&~2MH^r3M^gvK z37IfC4tya9j{cS5dD)GIJTr1G>Y0>2dU#{{sq};2XRoCPUT58G)(_Kzy79hgWSe+V zJT~$(v3jJNbR`B0K8!W3verb=JUg2sG2&H-0F*V}#>#BWY74@aw@yO~Yl8%JMi-#- ziS<|+-;>$n@-woyQod8ZPnM?2vt^N*m?-ayABz`6=}nQnEVA9=0a08cZV|jaENBW8QazCRIyZLit2^0CfS+82!a>$ z1(_kB#523(Ow#(JZqM{V%8MDmuXzOregiu3%pWt~(awiY2pkdmD{NQHBQzUP3#P}^ zFb|KV_n!Xz^V3jb_29Eh(x0V2JuTLY{`69|XXK;wGTL9+ku;Y-&L_hVX|wQf0(7b} zwjp#QJA={Fh^H7`Xwd0A>!b!=sFyJ9hZ+MMLH=Q#3ts&kr>4(WUQJ){r&{TaH>g9S*UR9G2%D2sqPl>ao?R2t7N) zd??2;Fa!F>o->`tF=w)LO5u!ZDCB1kp{WR+6QChLKvgm;Nt;1+Sd#FxufBWzfa7p0 z;%TEh$0?CLq)q&VlH+v|207nel>?mt92l3Ak=j0Yy({Iu&AaNOaB zm*e4n@QydhMh`%*wgKysKy@G$7!K?V>Crn{JQi?V;WbrFgC<>gTK)RGod;*K1wtgKyd z-@K6*#p+u&kfx8c;@&8p?Sh2SK=viUgIIk5GyrNoJNJ`ZrsWRhZpoGG=1xn0z$7JI zE>QgF@etBwJ${tAmpBkk98!oIsdr|;1WxJ3jNTp{)nv8@I$IG+Bkz&+g{z^;*WWTF zJpe?+c(8OC;9!md*V!u zABta!OQ0<@#f#!aiZkWObyuVS*#)SjUiX}_39qM(8ErE`9gQ9172Ir9Jud4L26dUZWAKr+6OEFqXwLmo>z>cPGf36K-(#@efXy(I!%V!*Kw zTgAc5-C!=B*Ktf?dCz|ZM;Fd5T}ITJvxK}{aB`A7eDztRh~8uwyA{aJBxLwWMpEOd z=f8BFyzQ$l^CJdFZEeueed}WbH$AnseWY7?{L%Wkz1=0HGkfO_-Mr1peiLk+=b3iz z8(YQUn@c*pj<$m8+SSi&n9#ZPdu!Lfa@69^b3N@2xlGdgPdqYZeX@dL1mkhOla5qz z)XcQtKqyquNQ%;~wujrr3V5_dMGd4V1zW()Bo(w3*oGQ*s#0SlCZ``^8=U?3 zJU9I3RZ2Xc(I$B0eNZWM#Q_E!%7w==qS;%GE0Ci;jE&nUHVL1h)|xok(SU^sArSBL zNJyM#v~jiaLxUezpUGVPH8{ikYEY!&Y6oXp7jQM50zln-h?BVpxsi@wP>BvY&0u;W zUO^O$-9!ujEWLS!5= zr5xnz;8=#T>aZJ*sL-)pK+4l{rY0a9bt6ujcw;FjaT-sL{LFA=B$%UazVNI#>~xGA z#_>ToiyLNt<2P5qZl1rbjZP7uZa%^(5(AR0(`pKu#DbU>2G}*0r=X94r%M6)hNX&z zfxg*WbiC*+FgQjb8qMp>?=Oso0=-tmDk#;PUl3OFz0O{TB?zAaRVp-0gn%-6YK)OU zI2auqxoUWIp-@de+weQL+>?z!D7sw-AZzHLKn#?7-L zv&)VgVUL{@S1O5n&OA)8n+zmp<)cHDnmtVcU<>;_Mh6dBnS+^&3Ag;c5k$FvOiZJ~ zs(~HorLEk0sA|F>24Uka( zlDrdn?06oFHoi zE|BU-k*c=uSYO8Z_5WXTRC^oX5y~_X5Mhi?@YR=1juHy}a<1>Pmck3p_+#@DV4BU% z^$7!kD_k(gz>syFbst(?%KQ-azyP=UoGF9BmK(f~m&$edZ9LxjVFzt`FuTXNUv)gF zFxG#>y3!p;@`t52{?zk5cK850d`-W1MtZYY0x84y&3IjjL{ShfuO|ex&AM22I0Jcs z3m!EI10$Hs@p_R~&1569{w*mZzco&}kzw;6dt>#|HC)@H`>yHRUO1Y*8=UfVf^If^ z3SA5fdyb+N58Rh9ZN?QgwJR(jLfM7zuuU_&+-?UwYK4)L=N<4kwP+-IA}SKEFC_fO zgoZyBBSS3`3@oChbRd}7)g^#8*6VUz$Y%^M)5VbJ$H?jditp&~*T?m>251yaW?hT% zv%X^kq<}~loL~U#MnTn41P}F18on7Og~~sqSbD07>N~!R*yj@l_7wBHy6e3I$B=gr zk?2iNRD>DRJMM42xLY}T;RgBJ`u;3!42+*RDZg|21yNrG${Sb(g~Im4fXSGXqeTj0 zv3y}#Vf0Yscm(w@=Ze|sVpda}C{7mdE0)z_*#GJ@t-q+?0&90%=NNQIR!7hgcSsIL zzEh1P8*--P^yf%QjyI<$M>1(S9C_%68$jSr#PsyQ1*ZzZ*s}`)3JEw8Eejk`2-Ly4 zqeB-bGE$6yx9}569yT~VE@S$S+tYtg=%=SYU{<@&?NAI3w+~^&_h|7*TfMFAO|Fro zc-UUsJ|XwwW`wId3nq`N)10;K6Qkk}^hI?ItBo=cACT1siRe~aRJYp1TrlJa*^W14 zhvC@^F8xOOJF?n%&l`i-^ftHI#1=5)6lJ^KWUkd;Ii7uGVI}xT`viZ&E{XXHTB|T` zS+<)_wxbSS{R!p0?5hha|5ebhALqpN)%Bg(t~&blm(s8}VOzigMG92EJHr&@;S9pJ z`QtYb0r>|$wo-j;mo8*1QYS1@gYU|I06M=vjSX=%hOzxxA6q5Hrq+#N_lGBiYZVtf zDdOLno~Q=bmSb65jqij*eLOD56IQjWXiy3)EiXz@pkgxuUIB9$HP6V`naRE8bUlbk z-~_6pc8pl_y3L99s84w9$0Z#_rA}*+28x*uXaEF?)C}N z`^LmA&rBYCWX&Yw#ords{)$q@-DS6B_3P=6zVmGQ;~y_&)hSR!mNm6*#+;xn+mTSVtL)bp^CJrLp8*?T~g_JFCf>CWMm~U%=Cg zl+{(E;DHqy1rp|2j5_XJPx%@#E;rF_VF>J(YmeSNebVjUT1vhxXqWfEw@t#g?S^lA z%QkD;6KuUfUNGaU|Fr$ye|}^Z_h?VRqYb(K4xi?WMVEaLJ`YCdnD`;hFm!AhW;lS} zO7)~-FJ5}63oB0&yOSi=l9Ii6>9ZrH<5cB}XTVp1 zXM=aqJsZ3Wc{b2wkp+oHN^lkHzRl=+;2Fvf={Ksy+j<||-4`=i_ zU6_a8@OR;OU}5E@<9sY4o2+wHeRia6mJSLYea;j-r;oqS&-C$Mnw@!{$@=@ehv(?v zVEsAR0r~sjB#6M@s1yc+p}|Nbq2+Yt1tSpzF73WSWm&=A0=CY+#V&T(d+ez4ut)Mz z`9aFIOnG!WHgV15b~lLO!b*IqhtIH z9yX$mbtpil1Tg#TmSgvHPQLfljO|$gBVi~+29R)S4Zqim=TW5C@f5%>p(PIScs-?_MA>-Zt-yO z-eM7@kJ!XbTtNN!J3^oy@vUa1V(nixTQIAS}7Dl+yXBVewS{L5E^X@EIH#uhru+NXu!lxomkjJ3ibS?6!?^yR zNT=D%R^DyI!MlxII${I9am>|v=M&Or{_4=qCo-BA)in*TU!lMLsHW-fKpM^8L0DO& z>$5kj&$fvE9;43y3yZRz^jWCWbyGN~uNo{hzWBp7RJo61uNn#X@qbi zjf2fZGfRrD6hS3etWZEm0Y|!is`J<8W8kliF}pHji0fk*Ia1K1zxwDH^w+1en!moV z@>^U^BCDVwuvoMH4x`uW?*S%0rr-bd!ctuuM8*Zxd+m4N{dV*bT(8drTA$K84E)JE z@azpA#Vfj0h5qb2glQP6ul%aH;~5e@4|IB9t__(n6wo`E0+oIqvC_w&+t9qS&MYiW zSUm#!4*}ad>UzlqZ8W)IHlPuB5AvQn;f^GnIbFtqG2WbWp16KqEbB7Nj5pm$cZmNaA?oDx{yLqLYS(ufm zi-W28o(QJr2xa>fb$b$W)hMZnw&mgiYGY-HLvXCv;zg=?{!0{SS z6GIB5DG}n@$^41qTR1NXm>NL#uCPBS>=z1a0(8KD@-G=vqS?(7`=i8uEwNUVtca@& zn+)PygwYH$4B}$(R#Ch`WR2of1i3_N3n{4v8~cNe{ldnY00OZlz(#K+n8qsWCaXBt z%G#_ms8-pnxoh0wF85&y`7mHD{^(%8b}({aSE-v+@dlMOs#5{Fuv@eq+4u=n`Jhz= z8)U0+b5x69^(vrd&j=I8J?Amx*2^nc6YKF``zhHn@i>1BOMthXz;E!O@!ugy4fgVb;#8crWIMK9nG6hKlaGW=|`v*TYPTo{QCy0zHw+T`|jMw)=v_b ze?DXUx~EtwIBwyzd+#Ani%v<=01{E56UzYii0orK?lFe59+d$(<~l@coCO1HNrT$J zh8y-ae9|B#8j=kn9vaFAolENrQ~p@2!{6f{_8<43^(%gVj+9q9WQJc$aRcB%5cvdD zoNrNNgk@Sheb`DqhTiJ17`>Cp}lhOD+_LZJf73hx2)sc z|Co5+ySwjvdD+5>uKLi(v90s(AFTZ1mnRRl_@!hzr}zG)Z3|}>cT@%b>7SQ<_&&Fj{_&zT|g*=}Qb4%oUF*MgQBgLd zIYO1^54^K$dahoR-sEIQE<7}0<*dr81z$<=BJ`jCj2_!j(sp9 z45FpDF%pkQjFPW@plQj(%@cP{+&l4;iI)(tnHZT^Gf|>m2x5n?$0z!Hi6X<0QD|Td z4QGw)ppo5fWI6ygSd(gZdtF1p%MLV&ia@34X+Tg$dISMFxyM0_ zW7}u0nHC+(>K<+?%V}D;t!E5>`;6ri^EYl~Z>B3rxYdj4@Dh@+l%IwN-Y#rS#JrtC zWLj-gZ9u3E1ZoAT#MEh>JzgEp&WvZr$FrL8tb`7UGC(F%C9I@mTvH&KYc`+FWsm1R zlPhKmvK5fCR%i;y4tSzOPX&>zvZ4aMQ(sbCSU4my($X9Jjgf`!q3LX(AdE7MrH~}) zNqyehWc!Mtqt&UMLpSbP+;}B-dg;u*fyOzzRRB-O z_$VQ|JI`${p0Q?{PO5G$n?TNhFblEZC!kfe!rh5}b+E)-fPXn`U|rRgDzU0c7(gpA zp`M)pbB(ySo(c8DT_df_*^(o6uWhyxg+vk-w%%hYz!TlNV2n z{1xT-eB?dS7TA5C@?7-;iCo*Xyn;Lf{<($Dva-s~NO?_pqI{@aE+-{%sO~OwX**GU zrutm91frf3iAvHNDyXcAUno$LXASIe!!rhvnzM*jLy%jFf=dO~p)8w{8WqY%8KLM# zDmJI;tVn2`0mDMB{GV48FA2mcKC$(_vE2Gs=to6gM=?-Z@dZo%oIW@zCJl~%kfXvO z3*pj7=uLMNTJca~$-9PMf+_-K%I~s200%x0@`H-7>wZu?X+}qvKU6!A5AyYMA$BBm zGW2#x5<WAw$*YB-ARDZVK5UFP>sIQBPTr3rmqOPF| zD-Uy^2M}%5nfL~;f8ZMtbvQsxvjz&@I)Xy|+1w+YQ)9QAM(M94B5zuo-DnrpjrDEvu)wFgr?8pM_v{E{!*feKpr+c59z3*fvos0_L#6l<2oXqKVy3N7P z{MqH3%TJeoQZAW8Asb6N)f`(-%s}T94&}_qX_vi>OwDW^*GN$Q>27k(bmiC`yGiz=Tf`=a@{0PQO{>g-0PG!mOEE-;W)6tftyXM{S z;PSSSrGNzNImWhMJrS%jo8iPgqT`_9e$2@KVed@tJNFWIc4uEK!=c#%ftF%>Kn>xgy&h3b zlK*Gzz0bKf1E_u0U*G>}{Ibuv_uRequ-4jZt-a6QBb@3P^W4EKHyPf2UazA^I?T4X zGe#~Rxt%k-?i#sgWC{~6N@n^qhc0H>$)A(C=c^>xF?hTX^Z=5J#Xt_ z^t^T}<93ran`HTE$3B@V%lg%7-cyv0Sa*_E<;PuFS;S1Id1tY+4ycg{Pw54TvMfP2 z=Q#6bWas3hXCJ3BX7EOEMuu85@$rdj!$dXWxb(^iIVU;Jn87FY>zy>_q*NqH7MaPc zoHbB?&jygahgNWe&+*N${gcb(GB51n=13+K;Sc+Kw1YJYIp zK?h!aT=_K>h1pBbJMoBy1*7*q=q{ME<0lu)JN&AjowMU-S1%qr{*qzY)7Ia#@`M{U z95iY124(m9t>zK~>us&jENZzds1Vo-@FRZTO? zx;sjecg;@mKC>xxEetrsSwCZeNYAjI!_+Io-XGRF%##HID~5SJhm9Dbr{l?PK8`cE7spuk=%GY{k{lBkLCax$qu<4%Bnm8{8Qef^F5%PZ zca=H{QSWSdZ%J#ilOE8v5CSC2x<89`R9>z`5NfGGI&xF$oY36&xWd6YkTiL~;Q8!K zGk8i`+V}%*IY4C|H>Gk~;gA)(2CB^i70b%H3>=s|dd0Y$zCBj3ajR%${-?acO>dQI zS?cw2K5u&C_49@RBeQn;WM|9Ng!k{*3BY=POv~`nonE&qGp$o6dp90Xf9Lx7r&lc> zcl@EZttmZe&bW!k>^LU+xGam^wX^!gBaeJy`}XMBv+_>5;FtxQkD8RU;_$}uB6+Lv z%%a)DryW^z(CYc)zcq4p#q0|%&6}`m+}HzaZ(K3|^b@CdOGz(XcB=5)@x(yaA#26X z`sVz(!_{apPNq|L7Cxzg6MB~ZXX$TB_mp~#ek+?i z-b{Xkxa=5t&RvuAGoB;z>kiBq&t4fr%$%!@{GPb?I`2mP=AtM9dAimK1Dj~&)-R=SU_N1V#$IF$o5mDvZhotK6P?bL<7GJW7}390PAE-5J=hjrN! zr?S7gbXgfvIW45J@4DzdN$nL788Dw6Lh35%sCwDj8IELz8cGGlat^p zsh5+JOUvmt40=k>Fqp`y<}@A6hZ~%6>d;r?{5EPvfA3KEO)A=b{vuk_9UBzHW+9TI zTeWXk-_vz{b#IT8&U3Oyw(B{=fy|;A4)jh>I-N}PFXl+k4WD;#r#%;^amJUXamE*< zu`H~|nQ7G6+~ps~VVn6`@Z#?~)LBhDFH+tXNlV*9qz zoN=6~2b{5p^&RU&&X7Kx3};A{oC$ft8R24*c%N#nNPf@M46azzjw=?z6;j3Sha4_k zF+5me63^jTG?<<%z-lubwTsHjOjpm0@r29!sKi;ZjXeF9)_1bTL-gHa=4p2Vlyb8s zCnwH}G1uWeruCi4yE5qtqwiE7Cf&M^>}>sr=sS~lWzuyv@5$)9Nz)Tnkig9*@2Nbk z^Hxh*UnOs5cr)~T-Y3oMIn<`)XrZ@~-?q7@s3pHLT3u!ghvyyKzT8R2r;3)-d7~F^ z*1FVH>A`$nhAn%@F=G1~U8+l;s7tei$Gluy{_q#$b?d~bY!0bV1vy*mv*w{0rVin6 zM3=s$HMXIWpi8L;R7!G9O~PN$P8=;Sy42KVOGst^buqfsP)X3G`>2cV4XsNJmD55h z`>qQu=DnqLsiBgfOZQb5;Z?0mp(47p^#iL*g=f*F?ihGh>gA0bFD)l+81$57c^!Qh zF4npf%A!ku73X}ByBYAZRCMU{*|cW7>*7*FNX=cXIh!yva!~2X`T~&};q1QcIC~|} z#Pmr1SNrsdhO<=~v&nr2v8Xj+++V`(LGoU}IC(EXWXuy8gR?~-WrS@flQ-|ZPHjql zhvygC=hy4!-F{R<);jbVXy(tY6>{d@>ykGz=Wo{)xNDM`AJ$gL1hH_kCYxDa`!o_S zCkpbkgb_vJ?=iWL*15MfXO3(0yjd|IHcq~&Yw{w~GOkQh_nV&8=4p|dNxEfB?v+V9 zZCS=4mwl&Zwt3p*EfS;i_D)S+M&5cn$`Z|=oNIFrxnJ^MtK~)Kk5me$h*iW{bYidk zqdCjc2!GR_%H}wgTur4U*OV&!g}gU4DS^u7kct);q0*sbnu?Z7Q9>72~~FmPp3Gi@b+-OW_KFQ_h9Rvn9Y+3$!VAOY`sE) z=ckhOgRMt2A24;lxzGNE&RvGH2U}y!g_GKIHzh}MOv&9!Eji?Vpw9h~<^`SQ&UmTI zjt(=+&Y* zZA-uRx|FfZXa=P>T7~CJ==uj+zt`Np<?jqivv&=l5^dL{8Lx&F#9lE!9$jJ1>o_!ELFrJyo zy)t>2;TpwKTh)8gz_z*bbQi6)OzxFQvc5vfG7^2>BX>yqyi*owjx0AE={&95U9pOb zvRvLgcEZ3e2|T%zXy6vjlQM3=ldz`Q)%2F;tjX!YPNmpDl)6pMvz_TC=dn8HF3l6h z?lz!$7j>E8MQIOHiesCn1=(uP+LTxLP|7QOD6L_hUD`aolYE5- zb-v`e!jyQDF7dqP$p>~UvF1VD7HSCtsL6RzbKb#iN(}!k()?#iZ1{s7&(fpj{0^;* z&f22+&y;#iz#$m2^h#O0snIRsJ;713f6h4PK+93v3`eEgQMqf+R5zg)_4+Q&fsC%2 zUoQ$;Mdde-&bAB4Oy1aN#oC%Mc~>TFq*b&HPP1eT@7ghE&3z_kd2h;aAM5;7M#<`aqlBZH%p5J#MJL&bk52cJ`XRRU5q#1)Udh|%0vDi`Oh?;(>S8iwP z#gmwCa)=iW{)=ArDks3bM4WRpYx<`a4EF{khgW+aT0Q!R!w&BCOs||Nx&8KDyL$8z z?w-|mNv{t*a@i3>`i)+E#KK`i60RNh({)L&w?3A#jb+zEd7CJ&dwNnYCbniw;WV_v zhk8Skl7>!kPI4sXoa8*wrT>s2N!!vdW3!a)>^FT98@lV#adadFQ&R@N$QEg`h=l`! zSUJQ?2x{~=*{mydG^aWi%R%aLNJ;_WvDDFfUte8NU0AfPprT+??i0BU1&cC94jb01 zdD6P#ilo;^9hIAV)Tki`oKseI&H+yjnl!m^WWNK~OmN&!E}-4sv?X3JBPYpAAHm7N zGlr(851m4hF6(kjm!vLFIQ{$dN#EvOhV8$-S3*(cAfYxz4NGbZ*e@t?v0Ug@{lK!MdWUCl{y?x5)gGV1e+8dpeG`dam4jMFK+`xhRY~mO5W#i2@&75D#n=qZU zb6?qAB6Tzen9Dpnv+HyKlN}uzeP_m~$%9gsrWMaDozQ=Lw{GJ`tXkc!_0=iECJgVF za>UtPM~odihE2DNiw)an|y{O$pIqQOcNO!Wb;$7f8F?g^uzW=4KjQI74_t_ml z_7@&8V$7w3diUN($KVkF;pER4l(M~Bw-HlVJN87sjASk|wh!*v`y@6<(z5UpFIl}v`H=-U zZLo-I$ns;&8>2|Be(&nD6EiZ0TaH(?$rBE%nS13GDb-0sa_3AOGqYf1a`MN6CQfEr z;J`H#Qx05JHtyoB6Y>Y8b{&$J)vi<}GlnN`?>3`v|M>|;V;7f>MMFw$(I)7!g-p&d zMl5*>54`#8r*R(tw6mhUIZ-%$>g0)o`n4-sP`1<*c2Uc5X3ZEmXy{OiGIng=DMaYI zcHwqZ))Pa9^zE{}N6+Mw*eAiZ9PMRUyuh@czM}=(mTi1T-WJw=sgq((uo( zT&$ctHdWimNR;6WoYA%CcBe}hm-GG{*0+eXQP8~H0fo~4a)ey)UUSjzi_RKcI%Rmp z>R~em_n0(jRN;G7jeQGe9GbIyZI><^laeRUqs$?wQ&~&f+v#(ESJkU~%0D=-PBu&{ zc#2hK`b9|l`YVeQ)TwH4>9GfoJGg(314d2G|L)$`XI0J})4S`nsccQ5WH7e*MgFP$ zB75IS%rBP;^WPA)fPBq=g(T)$IcZLzy>^{c=OBA6@7a~v>!gHdk~t-8x&55O3!O@P zo$4%dYVCCw+V@d=o#w1@erm6~JCjw3z3$=UsNlDKbpD>BlDbl^wd!*Am62;^Pu0iv z+I4!k>Gqm%xI5ThCnY?S>`Zj$*w0g(KJEs4o$B1^?y%QgoGfp+y-ssp_T-deq1DYP zNy@O->6~%C&|Y`v`VM>D!#OHBX-?Ies)nl5DpqH%E^jE$TvfYaOI_8PwGEkfX6EPS z7EjEaS6jQLx*~ISZQX|2y7Gpq+M3C8>T0W2XC7HzQ@^BQ&8F(|x|#K>Dr#0&)MZY} zjDNSZqOP8&GxH|r=IT%72j=Ul`pojohPv|A73<6EPR^{Y45*aXtj=6tz9n;IMP^;a znyUJSiaJVGRg<}@qOPHwbls-9s`}MctE3$DlS8@*rHdCJQS}AMR?#%;*Kgv& zw86qEs$9=6H|dHutgXnLxuJX&iT!3`X3zlnlXKTLG;EkQWy-0io;tZ)KQ+0wZq1Zx zlSloOg@?>8TXbaEB%Wmcp$ZnNkNXFd5;@omsX_0*zc8+6{DiFF9-m8tVIwOI=dDPfKNOnR+e-$`xjb$P0E-5RLF z2JS2QR>~=TU%?Z?6E$2(O)GU@W|E`%rO>Id^$dPzTG6yjh2{jQm-M)lU=_ItC#~mi zll>%-H-xVmD91EL$|;SKNXwbKw!abCXwRwnP{`e9x#M|F)^%M&YgD%>M?Ie`bPhqv z$`~<@}h)4;#dKk0JPV!*~aG1oau|jAAAI7~kphT=d3#YO=sNlo}mIZ4P%9J4c`sj&zQ4 zj;7^~A;NSlqvdhD$8~~pB0l5_+WPy>?anz)BO~Os&L5p~ol7|>^(J)EPUSflswC%Z z=PFL1`Kxn@^DXCmmF)b=`M|l^+2#DV^O4iy+~)j%HyEE~f3%hK+U4};3-sepoadca zoEJHD=4IzkwBFBHg!5D9hseMl-j4mb^Q!Y2efDSPFTC-vj{Z0qKCXrvZl#wt!U4iL zo8aJ6;M;ed&FIlB&gssX&Kb^w&bOUy&Q@o;bC%;f?>P_Q`LkuR>Z;OIH|H~FuS!?E zDzADvt%|i0s<-N+`m%*UKh<9iPy^K8v+^$2iK#r5&xnW;i`5atnvYaRaT3i^b&Oi(eBylS{LXnt z9jlh}lFISw1a%_ug%zq?tyHVjYE_{s)f%-{RjGCAWL3?aYmM`;s^t};jjGQ1z4HfE z?|heY88&go&8cd$+M-TVr#ruJ-co0*QjgRU->$9y^5$C)Q##U_FuR~-KxH= zZd13bo$5R4yXt%D4i+fxQvad8uYRDK)DP8N>TY$9x>q%`zU2mWzj{DD$O6WP)Whl# z^{9GGJd(qoe^KwL_thTtf%;JW zRei*{87-^~{#gB8{X>1CK2@Koy{Z*q)%!ZjD>Z*4`W4I=7zpvo^V>xTm_C-7W5EtUNr! zJ=5LlZgaQ0XSqAvv)yypRQFu>JokL}0{24qBKKnV68BQ~GWT-#3inF)D)(yl8uwau zeZJ1U-i^37xHq~txi`DFxVO6Bc5icUcXztqVRzo|xp%mCy1U%}aKG>Vz-@AW=-%bt z?cU?w>o&Xhx%aydxDUENavyRZb{}ycbsuvdcb{;7>^|v6-KX5A-Dliq-JiJ6xzD>V zxG%adxi7n~urc{h-T!ib=DzB_=KkD`xxa8yT5VYa)0Z-?f%Yv z$Njzg2ltQepWJuZG}w3l;=ad0gL~W$+z;Kqx*xg!?Y6job3bd-Vkr7 zH_T)0u9xYJ^hSB3y)oWcZ=5&Y%kr{0qiTXT(VN6M)Kk1%FVD;O3cNyZs#nCh#U);; zH_bc1JCIl658{lA8Qx59mN(m*hb57UVQ}7yA)z($?*np*8yBZ@~`v9pk*O#xt4o@vNY0Au%brq*nq?GF< zb!P3F+M0@!Q_D@-ZT70Fx>cLjS5{YS?zSp^oiuxOZ9~#3{z;j$s+{anR-0rFd6zeE zCrN6VrG9zWqj8ngj9b(H_Wzw`m!Ya6jEO4vVbz2Z8(QRG)I{nauGNqr~=4MS@MMX_D?o!pNl!fK1 zHZ@eFRO=*tVd8Vu2{$PVP5V^qBx#|vLpA@TEHb~ZF~47w@RRhKgqxH_=Jz$GeQU}$ z)YjvtZCG33mDQ~ADr(lG9&X#A*0#gprX6Z^(*5wYn`+jS*KJx~UB0QIdu`%f$`VtC zI#Y%v31rjj5^hqKm@?Fvf_hxMca!PeqiwD5YBg^jZECeiC&@?GRn@FX#z(GM)BWhgI(6Tac$a##<$_Hn?RHGO zgxyX}xIQ-FdQ1E|Wx1*AX*x+;9`aDyY2jsZb#2X>`m~u+7V}tnc$qr0On;PGUZK-& zhu2q^*RM6#wejopBNG}Z9la2WNQ$X`azkxRZGHFERm2JEaq{IZZD#d`wdMM8;wo>++S=qJtJbV9_l_#xlxj=kEnZvY%?69>s|=Z$rRkDiP@#CVT&YJDkY;reOrPiAId39@RQ;52(FV9QXiPu0KEq!kT^;-ofPI9d@e7a;C z6su>uVy4?XO|RK6^-Y@ovA*nDURhO@mz$ek5L`{o3$OCStN6Et;b&9BtD^9#IJ_ze zuS$cfqTKK*q)`;m$SVx<%L{YK3vBl z%()=UxggBBAk4WSY=eT3MnOoUAf!MQ5e!F3QJWK{-P-SMN#;RqVN~RVX2D4vJ{79DGtk09Ohgc=3E@+TpZ?H9OhgS z=3Eliw&8fJRXDZl(#Q*G#LGN2_$;@y zB;}Yjb>*0Kr<&v#JtCx>s*|qA1Ztq`slipsvF5=oI?*LW&~r<=>!_?YBv-Gj?!Ivo z!+_X}bwq4a)>qYtiBey&sP5GeZK|u) zpBCliTYluq_D@kBC&zcKsBggHYN%M<6-%^2Lg;JL8`feOnd|y)l~tz%*Xi|?rN(}d zJV$F)Ea17ba`QxKlIC|`MI64G2(cd6yD+R)SJc<9>!q!Ogn_VI!gcS2>pD^G73rr{ z)YS&nO0TTlR2RRYIKfS~`l`*rb$4C1@J<&pxJ$38stJE2C1r{r*QUDU+~?c-0-F}v zbgE5@Y+7v75}TIVbf!&bNjkS|?p&SDojcp6b0p2po2%*O=Fiff=N8P-^hq^+QcWkX zte{MuC)N4nm6`IA>gV(37MlF>=N5EbU0z>RUc0$U590KJt^sLb^5JW1>uQo~^}k2! ze>cg$x+DcA$J_#wBWXd>+S=NarOQ@URM(!WpDMIZ73Laxg;P!W3#Z!qB70wC>QPu^ z>QPvvOOabxq}!2HwpAE|CfQr(WEx*bV%JCf>lBsJ|+SQOCH?ZW4}U2+SHE&XCk zzu3|*w)BfF{bEbM*wQby^ouS1VoSf+(l563i!J?POTXCCFShhcEd3Hozr@lnvGhwU z{Sr&R#L_RZbW1GV5=*zl(k-!cODx?IOSi<*Ewyw?EgeiZ;jh9{OQ+P*DYf-1we(6Y zy;4iB)Y2=p^hzzgQcJJY(wk}PIn&m2rlmjA(w}MR&$RSsTKY3B{h5~jOiO>Jr9acs zpK0mOwDf0M`ZF#4S(g4ROMjN7Kg-geW$Dkd^k-T6vn>5tmi{bDf0m^`%hI>@?B)4#mr9a2gpJVCIvGnIy`g1J(IhOt$OMi}~UuNl- z+4`5+`j=VyWtM)KrC(<2UuNl-S^8y`ewn3TX6ct%`el}WnWbN5>07;7IM>pjYw6Fm z^ygaob1nV3mi}Bzf3BrJ*V3PB>Cd(Fjh@aeoNMXNwe*c1&z)-Z;8d$Er<(DC&kg;l zxrYAKTtk0quAx6Q*U+DuYv@nSHT0+E8v0Xn4gIOPrv6i{o}HR&=ugcx^`Dw&+J9=E zssGeGQ~#-XntooHkxx?nJ*oblH27YZk9%D{Qe8e$T|QD>K2lviQe8e$T|QD>K2qI2 zq`G{hx_o(MMm|X`eIuXTTlz*mxwrI%%quhUNownF-nPGyPws8|8~MyDGxAAl+uO(^_qM%_JaTXAYveIM*Ny`P zxpVu*<2BlT#yDJ=@R_!l<+DEV=frMl_n3}4$!9&ZAsXgnZo^Ev>u-bKn=iwk_6lrS z`M$cMvY~hI!Ak68K0A7L`%f0mmKjr*c~O@Y$*gJNE*(S^(|Yddy|vHx`|ilNE8|y#Rt$P?*ySUOMz0;eBXwccTK;Ol_Uysgx!FZ> zpFKZ&X;w}4tJ&{nznkOaypgjfb>W1ECj4Z=&nJ(WJZ8erb1U<|-)&Uk-l@MU+*`P} z#~AZh^kngz;IH@AlAFz6=^dJaP~k6oepbyvEA3zPL8k?O+4B#&+Wy@`M1>rtBb1VR4)hB)fZMjRsHkq`RgaHKS;{B{^FXN+SPUU z*B90=sBdn#a?|Qla!$E*^U^IJoiX@~r_Nlk?fmUmp0(_(56>QR&eq1O&%ODgKVEXm zrJ0w$aoLTRUwcKbE3UjU^U7oNuMC-(-|Wqv33(l@S;$ZyZtdTrLZH-GEIQ*z{!YqR9vARkHP-h3~=6-shCG3WU~%p3F{!1uo9w@ zwGi2?hFHRSh-_zWYt)$s=7R;TF=q+sQQ#Q9I~FWw9mesbJILp3a1Lk$=YsRV`K`yZ z`XQTH*KAfYWV2Esn>F@JoIA+lPOuC72lzht0cZyIf&0M&;6d;Zc%0w;7(7XyPl0E^ zbA0{+cnQ1$UIQ`kI@k@~1U|pn13q-JRnOL_>J9pWeqfBVMDbpuDsn1SF>4Gw)S>bsTc-Y$Mqw8_bB(&S3Siw^-{k8)JOf2&)x*T;`&|i z7w`|t{t3VRl=L&wy`-&92Fq-gxa{%bX0DqACW#p`Y_Kv%JuW4FSj0T;GYmIv8pa-Az0=-)sz5d(} zaF%!jxgX5+5I!5$+T@J@Bf)4e7K{hkU;>x~rhq(90E+ls377^B1P6f`U>2AI%E)^j zSito{um~&$@Vy7$drQGGupAr@P6V{AS4+7z@H_a!t0QIIpw|H3d8|4*oi#nDtJ_IW zbjHDxIUpYtg5uVQQ-WNTl1_uC4}~Kq_yGKs zwnG-Z0j&{l5a}V{P;eMH92^0T1V@8oz_H*sZ~~w#9%b=XfeNq&RDqMhdQeNA)W@r* zjRr_d>)O>(v%9GMeL!l6HgQj(Mz;Vdd18ql<})ev^Q13>cgQsvq=Mc+N^si6<`cQ48e|`ElO8>@e|N8W=PyhP#uTTH_^si6<`t+|)|N8W=PyhP1 ze|`Gbr+!e$bBw=K7H)d$FYPy_UU7vR%oP;efrp^ z1sdt&7=0Y0k7K%zQ)!WI;G|ZczV_*BpT73#YoEUM>1&_9_UUV%7Hg!hefrv`}kb+NaeT>1&_9_UUV%zV_*BpT73#YoEUM>1&_9_UUV% zzV_*BpT73#YoEUM>1&_9_UUV%zV_Sn^+1PKpyj32eTN<9X?1BypH}i|C7%}Y;d*Ha zX#t;FOHHH(J~i;Eflm#jq`p@M=4lE(+~mVeeQs9HXCO^mB}Uj?vFC`ZtM}Q;2 z(cl60-3{KPUk9K~hLMf{Bf)4e z7K{hkU;>x~rhq(90OGt6gEwOE2AUk+m=14D*Syga-P0S4f*;1{Z!@s5GO({QLO$8S z=Vt@@lU9q-YB5?Zrd#b6o_mt(r@%9`LXoypGO$rHXoVQ95Tg}hv_K47K7_AVa6=KvlMvY_CI7V$_)HX(KW7IZAZDZ6nMr~u%Hb!k@)HX(KW7HOX z>SQp!WiYm7=vo>HK%UxYybHMBmv;LJ-|b(M`Eb6`QdUDHaQ^-^(cSdpy22jr%f%}%J~08wnbn6JIS|0PSYMbs(`lb_mPGvGaFJXLM0_Q~7+c|N3ZtUQnVSx}E$#CXi$Ik`x zz`*lVgd=@yek+Hs!vA&V9zLBxM zk+Hs!v0gZG-+te1JaaqP3BCiq3%&>L0NCKfQ5uP(G!jQ?B#zSP+zb6?QfzW!DUHNZ z8i}Pe5=&`x9_G78z@y+X@HljT44&k>D0m7y4W0qd0`V`OBYgq91jN^Th4eKL1FwVK z;7!op2Tegzx&VA5?T_|E)8ory<4PQ+5#L2(F^y^{y*7+=1jyuiB;H_h3nIyxs`M~-_h=h*p$LYC05f&tfrAzO{2Pk z`>ROtw~5s>602z>R@10%g*I)9{cU|=@_e3q>Pu z4}8LBpMh3JBn4bFgU5)JN_?an$ig>T295>dPp=?d2gIkQ{Mc!Y+PB_DdKUL*1M#!r z7wmnB(==l9ORT0*``j0CEq?bU+~0}aw~On$!9CoI4-Ov?x5;qt=lTIs>O|Zo!+nVK zQBwNPeFIP@_m_ZvBxWQ2TijRQ!~F-`e@KeYP7J5f{h0J0fYFzDO(XG|Mvs_-*9(YG z-=F&d*oyy3CV|Od3djX{ zARiQfLQn)sz%+0mI0(!DvjDuJV+#vNCBCqbRALN^NF~m&nDhwJC8QE>IEqwa4ogWT z?m&Ox7fIZw5#LB+KaKcD68~w`aR~a*TSFL5(ud*wv_yRv_f?`D{MtE`eJ+T{gQMuc7&_3zh4CSE zY#2Wh9T-Cg$_N}q-$l`PQS@CDeHTUFMbUSS`}L1+YG#MyM58UL_NocSs~Cq5X)my!D*UVyHOwuv31m(fusX8a!hUtAYO(M3^oQ50Pi z3**ERV}M_EoM9U&v~`U5TvGZ^#~c{1!nngFT;I)i_mI*TI#x`3p;w~ll_+{8ie8E8 zSa4!&LSn)n@cf6QeKsLw$ z6Tn0;2}}l4KrYAw`Jez4f+A1?rhx;&L4a{4j0;Q5WC8aQH(5w3v6Drl5kjN=I7zveVn~#LXkWe2>B-TMfXYl!KKBs-Hl#2H*u_uYp`ABIDOT$M>V@PQX zE5k=hV^|n5EQ}b|MGPs8A*C@a3lpiau??g&hLrM7CZOC{5;3GSh9wchk`O7a#A2un zBRI$MnfSFUNZ}AH28nG+G|+dqkwP0Om8hVPluBgK$4Zdspo!LA!nMRn?je;}3H_lX zLT`{lA1T$5A*56yi1Fy{9_~Nj{zKBgl70kdrh#RHB_eQYsNoA1Rfnr;n6MLaBwMtC17^^sB^DP;$KKp!BbKJT3dF=1-xP%DR$JCqu!^pQ#*sT4^RDfIU{_T9nv zcrQqhj|BNhkdFk3^!P}RkM#IRkB{{DNRMyFEgz}ykqU{h$++bs6+TkoBNaYU;Ug6? zhtM(J+!K!N4f?=!eMvJ&=~cMg#Gtou4Tr(q!r4BY?ZeqVobAKeKAi2t*?u_oKpT$s z;b$R6lMgrfaFY)=`EV0w%Yr%J z5T0K^YIMHLQ;5!&xeC$wGG8G&U*;@C=gYi>=zN*G5S=gc7cscWw>n?uF+}IfT!!d; zpS>MBx4a|{*Or4QDIRD9PKv-u5jZJ=eisikihhs6RT1>N%*;p7?@{!76V{h_o-yZm zev74wo{wOOMXH{XmDx-PL05+5jZu1o{ysEqv-i4dY)N*%JyUMB;Q5BQ{ZXv40sm& zgz`T}`T}?f7|xEs*%3H90%u3y>A=}3SUX(+)&%-qEV2k(9a*O!9J z!ByZ|a6PyI+{*W~T_`orbNveV1)xrNmLfe7q$h&(L>Ld6@Fqoy0xvRxC4y!XjG2F5>%(dHxcfH&Pa{QWn8u^zjyb z#)=3QU4*eB!dMZ(qKhDbGOL>?fe|Z#5i5ZaJVhT*(Z^F1X^bF^5j;g7Pce{6nf(#7B_$2ofJb;v-h#BUa)gR^lU8;v-h#BUa)gR^lU8;v-h#BUa)gR^lU8 z;vG|ITo&5FB zuCOl|@-6$k==n0rqUWpFwI#53k0tVERv2Zl0w{wOKtzE+DcBA!1XqBo!7ss^Kvu)Z zN*GxM!|r0_YHX=oM(*(6*Y|+;FZKSt@o!~p*yWo2u3Z4ef@=Kn@ zN))L#wK$gRH^499l;L0$7z4(EERX{xg2^BkN`vAGjYR*zM?4V?*~P726s8u}`~KY+S65pmoq!#)cKURcuzVSH)Hp8}%Ll zO>KvY?fD`22>k%RXq!{)O|dl>0I~DL#uNLF@-h<3C@dqejK1P+$*3zMZcNwL%*~qm z8h*>yR+(5`&7czti+UOhNvtEWjP_>*{ethYdZ?eVaKy3^t41suv1Y`I5eo)d+IrbT z`XTs;o=O8^sfd*_7%Tt_fmj`j!4hy35G!LDSPmH9wN(KLx-N^B-SUvFHRvS=9v=GQohI9cF>kc=>ZNFwu7}yY&+&a z9qU43JhAD5Gmte)kMo-!gP(B!GN8p0*S4@?>O@9uu}O_RN_p^Q}B{7U|jG9Inp==YtEuMSKR===CzYxE3$$ zULb2`kSe`y=0VaQasLqMqoj!*n0Q}*1t0U>KLBN81dTC*8V^j?)C}Z$5Zpi?X%FZy z(!+sxJx7A00ez>voa0E*OpHvQzpiEj`NBzzPO`!w!Uz>(EoZReXQFNTStP79(wIxq=$A}J+#~Eq227| zmCRm(X`p-SH(b!l zW>uFu>2MFTfXvs#*Ue69ZE+-)$4Uk)>y9hBX7HK#DLZ)PY#=63R+Ww4O{J=WLGYBPI|kLyeabHM&u?KZx@9qa^& zzV?&cM}hEz_}uV>_PY%i;a4X_*FpA#MPiDnr; zeFW)9Krax@GCuoQ(s5usATOd>#($qcil0L?%lPqANX3`W1Lz{6S=y&plab*iPFHwI zRx_pJpLNIU>$s{Z9#5Xs`k2G)CtjYJnb5q*9=W{rq*o=+AP?eT&Rj4LNSyf)EUN{i zpBHNmJi^47^9u4K9{PVN?u^VmroM{U^B7K28wX^?w|Kavz^r`QcPx4%&u_s4?mQkX z@tJr$TD)M1NsA}kJ}!Mbb=bvsc%xf<7>5O-chb?iWcnteKKG&W$DI+i|pvLQ%_M@c#%DDCbXT0^yzlm?7C3Sq8 zci3C6)6riqxWe{~vEO%cYb~GGqIGt+iJr2KRyc!2{qy@DO+oyZ~MT zuYlJ;47?6@gEtu!dxGAeFX#uhfURH$xENdtE(ceEYr*y420(ty^0Y9^(?X=Mg&Cd} z^_SKbq+`0|DTA0acA(GfaB(a4^VhfSP7C5|xNMZ|- z#1?lC_z-;5+Cn6;g-Buxk;E1vi7m{0v@rA0!puhtGaoI?e6%q0(Zb9}3o{=rL>gO& zG`0|FY$4Lv;?Z_Q8e51owh(D-A=21Fq_M?25l|lXWpCdLi4oP+^j6#V;U+n6G3x9) zw)kZuKY9|jh2eK=Ubp72e?VDDnL`Mbd%`0P(S|7VrP{_-!en|2F3)@QR{{dDJV^z7gH z{!_4*Hv9=u*>}tIQlZU`7&lmCb4>HTIh3N^*A|T62A$!Vt*X)`Mv$`_1@5| zu%@}_B4hE(JH9@#^!Pg**p0-j%$snJll~Y!90~$!9Xk;oj$yIOySnC0II#kV$Ll!% zcCPV7i6i-p;=Z0yeuVF^J&7Or9o~jBBd~do5S}MiZDxgK=J!rw%Hsbp%fl!sZ$ZY) zC`tMd_m7f(O!^P-DOw!AR?prJASOJJ>srzcq|86)Smbk2mJXf zvc8DtD`V^aR%Wp(f)%^}y46|Y;oc36_TKFS{|`LdL5%a?tlBbO?bD&v*NYfiVB-e% zZ4v%$30B@*Fb~WJOTbaU#Li@`=9gPZaR6JWW0p_Rgkt1h17H!pyffULlH1-u4g z;B`P06EC;J$_(vc^+W=CgFc`y5N}J?#SA6Q1g!b)uu`L#&xm=49@iG0XU0Rxsth8x zSh5nIimxs<@hP!vB|a5jWqd35#BYd8`FPUtmBuC>C2Ner702omp8X88A}xC5M!1WH zS)WsmHM`SnMV;k3rt2l_i>y;d`e&>9EpVDSU$0ghcD?0As`d-isB-SJ=I%GA+ zZ@7O8ybaz5d%y?aL-1Gd5lCFwk%lFm4iZ;*^yfZU;W3zNJOVuW{jFX$>xT~InT6mm zum~Iu7K0-I{HIrjz=L{q$Wl^SA+ih{3zmc90A7k-Epj61Nniz_?e@7Fi>w%t)gle> zbAje^;Z$1XD_dRFjrD4jLie8ZnDo-uvBIjGTFDMv5;dqI#f#FB1J>FqJRY?PZ07oW zzPkb);Dy{?Pu;ue8HCmRW(|Pn6dn`5k1EsO`Bg-*a+JKx zyJ14<6d;k_(||;K&jee!K8qE~vI3bkNO~1AE9=y`e8)Rmiubj`2=HY*Bde0HRa;Bv0TXT6g2DsVN|*N}>o8`?2w>$hcyMzoJ4<smz;A8@ zH-VdhtU~($U7qi?w}j4#M;2l)B%T?#hRH)h|E#5t$- z<6gX<;(Lg!=XYlkA^ht5OKP{Tm4flTMC6U}mGq64V^WdhdN|)mRU=Z>_U-Yrc*eXz z9&IB{7t*)#HaTxU>UI8?aUHx>ZY0aRS#BgNqP~ShT*tHWmbsBG*(d+Ac|92%bWuoJ;L>Lv!%H8;=5=GxewN8~4((y<~hxd1x zbN;{io@|%I(N>QxGO8Gv(<2J!xwc=Cv9FaFSfpRY3JkQ-7h7?$l9GxZt^%D|24WQu zd+ls@Grh)O1bTTSkhKM}u3$XK1{1&}Fa_j+0#F9#0g2g4ycQmFlH0W1Sx{p1=G@$1 zl@Kv#BKGlj8d<6TPw#D)L0tkq7p*InKD`7_hkM-VH*w^>yTicY;0SOeI2s%S@R;;l zHYbptL|I-5?F6&aT1WpYJ`TlKw9mn+nG5ED`CtK9Om0xaYK=Nj#6OGjPTd#X(O}=} zraH4fZX@s8!A|h??Q$URP$%wkAim$eb~$Kk7p2fsT|gRO27npRV3z~&1NXhl!M?}j z`0kX6J0Zxp+|jlU>?yNCzcYJ^H~(3Y6YORnKA!AWDywk*Uus10!(`_G@xx^20P(|Q z=K%4;Waj|!!(`_GpBY!#IY8d;kljwj50jk(#1E651H=!Lode{357{|D{4m)8)#tr3 z*#*`2R+CncB3*ii%(bLw6mK2r$)we!w5#kYfb_?AM~(X_BK7~iT5^9HGVq=Lm0Gfh z@#E{=ZHzr-lz5D?_q5pLn}PUQyc;cVT8T|gtWodr43~eoy^s@a`RuPj(`%3Lf28GQ zCye-P;@7=*m+@L;b>=sFc>OE(@G>4*+kL$18C6aJTkx0yuME$Tn1k8Z%fvg(-d^lE zPn@F(%O!(YN87!=+InV9#2o_9tckcoBlgF1JhLY2nKcpj=xiS^>zRdbV0V~hLsMqLIW2#D>n$G40E^z8r|9YocE5BVN9Zc=@d3wY&UKo~SNQ6{+Za^u5mIDsqxo z+4VrIN_o%I#1QckIL#q$v-{ZWGN)-GX8Vh_e10S>zQRU)((TYTUIK4mGX}+N_b>ha zq4S>B>i=%DwM1SWc=4O{%k(z#%zV$6+dCszzfAP8({;=7i1z2{0~6U6ePFmwVsY^Z zcVcw=LTH(2bA*_jNKAx?vx#&Y*=w&85@T|KPLQ{Jg`>?r8~d2eMbm$?PleJ^f)qUy zu+?gWi%CV-p{3A%iP|yH8FB4s<_F{#mw>ifQZ%Hmz6GuWqU$2$sARUl>4wG#-{aSM z=n^8Um-0-o8^pDGKLhiAzbhI_W;o)%(Hu(zA^h?Z3w~q9z zp1aa{n%&!kHw{mi-P^9@UdCHF%g4-7$=q1Hz1z~EbWUIL7Ar>jE|?b!+E&g95?&8@ zF0crMJB>wFruGBy<}8J?|tbv5+8)(!{D(vU+DKftGS2k zzP2|JrxUs34JM@HCTwo@HP)xI$bJlu(oT<4!XNY9Q{4ZO^iA%64cMDj{SLo3l?YEa zKvctJ_QpMfYvw}T?EpS;caSnC$^Hwzdkf#+$-WA^_{{9PAg4hz+I6n7?}G30wu^q} z`Ej0k0zlh+5zBaGt;e%2qfz}Q*FOdS1%3u-J@p#j{hXBDRCxoUQL#g)dIPlIP5IY+ z_8TC*{9Eug_#Jo${2u%P{1N<#a=lAFKG%OCWsfYi2QBm=bpFa`{|){I{tnvDvjyWj zaWws|^Ex=;WO|Pn8Qf%cVHQq8ih1)QaVKWkuSRxamOX1^CuZ5VMs{M3!DX@&v+QBh z=w8cp1l$UU@4DXw!g=2ZP2etYH_w_~nq|Km*$E^HH%8&asLT77ngfOVo&rz9?a#Ds zbe|=Cf%}(v{*~6_UE0{CeKlv&uDr{%uic!@9$mDbOMAKRf%lf9k@3IbOBvkkLT@@Mz0%b;*QTm1iU_cmccXq;C$Kr8BVwRdNu0Px`y)oAxMM7 zLM0A*IFR_K#6BhNDKXDufy6o)6LgHToK#|)64xaDx6f05VR|v&KyRjlD z1}Dj!nVDY@4Toj_xmxZCo_P|y2HxT~Z-e*22jH(@FSTUVlk9cC{K>6QVhxgB3&h$V zS*zm=rY!nXt+yxxTr4Y%41Lj0fo|HxFZq?$MWTlS+E4S@vp~uq`bKn(=oz6cdZiZ- zO7Zu(+v}43>5VUr2FE>JTJT;g z#sL`vME^JGzKQGqCUk!jr-hkwujnfz!FTe)8U>@CsZ~&;7^7aiHovB(a3D2;i^CfE zjCN8ZpV2O;QGAD?#GQpqjpQwkM)z@^eS+VZ8u`>HsEto;d_A(ss3s$tsg2KQ<};f4 zjAo`b1GH?%oMDXFoM#Bk_$>P;NlAszqpF46{uR8wK9F?A&98P=QZ&a-EWP`{gIvP} zXpknxH($MsmG}zxzW}>Ie7&J?n~YE4-o2rwIf*u02h5lfqpyPTB<5n5BmJ^dl-d8x zj3?$aFWCppj3>d)r$(=s@xq+&)yy+SW0*CDVy(zn@iH@F=&UCF=%+6B1ImyuIa;?v-E{B8p{1Be|GBdPm0*fcXn%Ia8N9-HM z*|c zH0AuvY|dcF4)y(0Jo7RVZbtSPBfHW4)X|RYa;~PF(;#PS$~g^9jOH;$a~a8-7{!|y z!Ho`b=%F+uHeKtVh#j3}WNzYI2025s30)NN7?qt1&_`|G*bID(&gRYcH>Uh^w&)jo zTT`@+j7K}M@x)VT3T4&!GBO$n7a83dQLpj6$f=P}>_+b8B-nrOS!eUdGPYrFAg_UL z6?u*8SNtu#7WjNpe6EC@lsezhP4sSD=ZZHWqex)OMYSy_^7;xeBTBSWOT*~lXsCZh zF8gTNjE^cb|Hs=1GYvfV;TV`YuKP!e+>SLA4$Q0E+)&M?Z zjCDq`KJ@cfKHf#Cp-yeI1)*nCPk|CVMrqrgAM`NyJg3J?Gfsx1emFi3pd__mBd8aj zJkWW}h8%A=7Y^PfoC^X^D)6HI%{{0DJ@}2MWJb_`si)-3($aRj)1qw>Mia%JwVmiZ zI#25~_OZog6g$LtKif%(WnzPLrq>d6n(R<%q}51j z%u1^9ke=bYxE?bj*6YyP4cL=+A380jCAd2h+Y_|aX=W!&u`|$TVlCTvFHtb<-*vRo zqB;r@*Z57k*1;K=)s&EVW9`k0#dR(a&t0sni@`1+*44d0tg0uprf$OPj^cGk(A37V z5>5RY-z8dBVpR!0a7J=ME#ekaPz%}D*m$USqi@iHR+H)&93G5}TRwg0t7qwtz*{|o z>+OITuKR6pCzM2kw(SqG=Y1>(iBkFQAHbi$e}lh)xOXC&Qle45J-=AIkr?m)w~aFX zE3E42omkb_aB%2b*k8Y+_QIt>t;KTwygm#1E9k3$1HWkh41()y-+XCnb}6&8ZTI&2 zcUVa(qw`Lnbp?FX1jJJ&mZUvo(GNrrI$K4GRcj)s_(1Sj+p}`0BCGf}p zyES0nto{1x&u_d1?Sy}T<`Nkc+t|n;k#J%IQFb58z^h{pi1c~BkK4%`JIpkgS%!`3 zpPgk0W*E%uf}EMwsQ%0tMa10BFf`gYL1Tv*hEI5g`5)dkHRnDCaf5xG_h@Dt;%7dZ z=uouXY{M5j_p!6thC8vmI?gzJv2!71_Cd~u45CVA210D1Mr{k383@sR8^6R1ggH;L z?F>ZJ&Oq$*Y)KuNYC8kbXlEeWo;hjs?M6ET(fRq4Cf>0TUHr{DJM=saSV-3d9Pdj_f-cpJ|g=Toe33L^EO5 z&G<6J-9j6}#METwD~PEXpQbb46aPrRNB=mfXdv-zg4nkBH^#2$I3xEN-$~ntK8}r% z+1Qwq%U%667pu1@EpzU=W zi~GcyXkU_Ay-Xkdl+<{~!3>SxCO%My_a(lj#0G-eeVY+LYPbt9+lFT>JA<)zCg`lD zrgjp`ls=N$WfKF)CI*o0{(-#y1pXWRjr+fYPeA9j&epXa7}h+S)kW}R!a9+S$cT70 zjr3WtK1Al7WQNSF2|0swJCOLJS@$8HbNsxGcy!jR^N?60<99+VQJ<;daALTfeY^>3 z5_jG=Wi#9OWhp-mWr;BCs|2HUB*^H7n5G`+1rtfYMxrg~5v?C+d2~WtAGD7cU{le1 zGM6d3Ac`(9@}CeXNRa<1K1__>kVruk`8TtEaX(Wul_^b31#yj_Bxo8mR8X3jo^cgl z@_AsiP=^`R(4XuLMWJl8PRtoZ-v_g9;cO^#jIvroxYlPzZGZFWB8$S6veQQ5JbHXD zI~_Mht4cIGpcG@Zk<1_HxmQa~B1uBc%->x^dMl6!QsR6ZJ%a8sR9PRRDgU0d?VOuW z1Up6qJARsrlwwRUGKX<>baaQ31S8>ka_v|mvnxs45{WhqN@UtX;_4ksWMb+)C>8NG z9ZyfS-msiS=fhoM`H6Qcvq!A;SI=uL6U?@l`QHzm)3r7UT1H~~q7Owgynz z2a(dmy5CBR?gEBGP5py@euZa^tb~#p_It4Er4M5RvI774thsDU-SoikpbYg+ImuE8 zb2Yj7I=2B~pYjx36`Wog*quHc^iS_N4+ZA$1b6BDUo~X+893e{CPeZ9{1(u?;hVy8P2Q4?+{jNv1M(8IAZm z)>pK^8S8aFz`vxj3PwgzcATfYa@uIHq9s0Fv>l@oN6QGkZ~fTLsD(WERZ{F7S_y&1 zllozMT5V@6f_B=h4Ihct>b1{YWZV!A{k$B)-P5IgouO8fh|MME$ciR0qv5+;2jjMA zkNu5+jO*>SLpbWmcpujU5yvGqHeG#06k(*bnWVSH-;a-hzZ2~RO<7YbqwiUyX9HO) zYosvf^g_|SJh&H8&^IYU>p{}H!nj4LG zEUU)a+a;za0?WXxNBz7W`3psT~)usy*lyyxj(GkO^PD_TzU&oKOF-v3 zHy9PZxXyhe9LN4QuV6PThRiDVcwG6L73}+3!5+Lnkr+vnnV-Z}?8e6q-mQodEs%FB z%!+nd4J8pBiRPG?^8db7?Pm4s*SE6WV=YsJcYO7ZP>!4?)x=xA*{u0q!pS{L*jK-j zlUcK!1G%0~Iv0y*9+(dnppll49tDo!yJNv}-cn-yGy5AgIp+Y@FgmnT7))p?G4 z<_nyc02Us*j7HgIG|DccQFa-Pvdd`H@u9N^um&L9Ri-Z^Nk1@#b(!Np5i42b^yd<; zrvd8BIj`BA^P0^WO>#1Olinv|Bj3qhSz7_IAN#JD?9gW4#N+v^xR!Uih!Csm!42Rh zekX6be3$eN#x*k^%=<`O-wj<^uk{GuJ<9##{ProXsShKqyulL1^37)ClQ|=qE8>k4 z$}6W)e@ZGRQtu^|Q>i&2A)G^AfgG#@GM9V`*77#48^L+t0&oeqh0pKgH$?l8vnYNl zv-8~F&;0|WKLQW)%%h~wlggVRuj@#)M5&t?&3R{u&-ajuRsIq98~1+)pE%P!tS1jk zopB|Kg)K4PMn-vJzRnV?Y*{BNvERloJ%3%q?@GWla3DAc%mA~%98gBy^S}bG7lK7# zF@Rt7PB=?Rmx1NrcyJ=1y}Vk=MIU?h@MSO73rxokspK4s=?UjpOlRlF>2Q1{9A62? zS31YRUB{E20L2YXC1-k9dJWuv(Hxc}x-`X_<4I^8@m@m-9Q0aUSB# z^boZRsWdHj^W4bE%)ZFFM%)j6*_yPbOHP5w?n5`E`XWO0wP8EO2` z+2VZ6-)YW2oKKw7ozM6?(`i-8*{VF{IcKR9mE!DBT~rt6Y?a3OAm^was;9#&vFhub zr~0XW&V_228s=Q2MyL_a#cCw-e2E&T#yeN2iE5&AmC9AQ&ebYki}>_f6_xHN&}C%~o@qTh#)!z`0E=RZE@Q)pB-X-Kkcq)y@x8CA+aUscN;}xkuHi zGn{6%Rh{EJsm|qn*5}lP>Qd)Lbvf^w{#0GXP9Hy0*Q@KDU#J_@ZO-dzC$j#Q+C?1w z_v$X>{LiXcH9LP%52^>9_tZn`3Fmz!bI&d68TAwAWA!|d^iR~Q{CVmZ{3WS3)NfR> zdRx7#(v`2?QyFRxe*@HqsznV{AG@9!=BBtQD%(wS(^QU|i}2Dd>KyQjEks1kRpd$Bs$y~e#sEpl&hZ&k;*x4A!5$GUgB zPpCC+)Ma-H_i6WOwb^~veO7I8pL1VSr@1e?uc)o=f4M(X+ufhLud5yI?*FeMb4f`k z48Z9B{ilK8777s&5fKqVL_|aiaRVVDk_aMlapW4baVv;M?IG>`eo*t^s-ae+HV|B| zlQa0{XYg_Cr-s(Gu10ode|2d8?7xnzhtV+*&b&^H0I= 0.0) + { + side = 1.0; + } + + j = i; + } + + return side * sqrt(d); +} + +void main() +{ + vec4 borderColor = f_color; + vec4 fillColor = 0.6f * borderColor; + + float dw = sdConvexPolygon(f_position, f_points, f_count); + float d = abs(dw - f_radius); + + // roll the fill alpha down at the border + vec4 back = vec4(fillColor.rgb, fillColor.a * smoothstep(f_radius + f_thickness, f_radius, dw)); + + // roll the border alpha down from 1 to 0 across the border thickness + vec4 front = vec4(borderColor.rgb, smoothstep(f_thickness, 0.0f, d)); + + fragColor = blend_colors(front, back); + + // todo debugging + // float resy = 3.0f / f_thickness; + + // if (resy < 539.9f) + // { + // fragColor = vec4(1, 0, 0, 1); + // } + // else if (resy > 540.1f) + // { + // fragColor = vec4(0, 1, 0, 1); + // } + // else + // { + // fragColor = vec4(0, 0, 1, 1); + // } +} diff --git a/tests/cpp-tests/Source/Box2DTestBed/samples/data/solid_polygon.vs b/tests/cpp-tests/Source/Box2DTestBed/samples/data/solid_polygon.vs new file mode 100644 index 000000000000..62dce4431361 --- /dev/null +++ b/tests/cpp-tests/Source/Box2DTestBed/samples/data/solid_polygon.vs @@ -0,0 +1,79 @@ +// SPDX-FileCopyrightText: 2024 Erin Catto +// SPDX-License-Identifier: MIT + +#version 330 + +uniform mat4 projectionMatrix; +uniform float pixelScale; + +layout(location = 0) in vec2 v_localPosition; +layout(location = 1) in vec4 v_instanceTransform; +layout(location = 2) in vec4 v_instancePoints12; +layout(location = 3) in vec4 v_instancePoints34; +layout(location = 4) in vec4 v_instancePoints56; +layout(location = 5) in vec4 v_instancePoints78; +layout(location = 6) in int v_instanceCount; +layout(location = 7) in float v_instanceRadius; +layout(location = 8) in vec4 v_instanceColor; + +out vec2 f_position; +out vec4 f_color; +out vec2 f_points[8]; +flat out int f_count; +out float f_radius; +out float f_thickness; + +void main() +{ + f_position = v_localPosition; + f_color = v_instanceColor; + + f_radius = v_instanceRadius; + f_count = v_instanceCount; + + f_points[0] = v_instancePoints12.xy; + f_points[1] = v_instancePoints12.zw; + f_points[2] = v_instancePoints34.xy; + f_points[3] = v_instancePoints34.zw; + f_points[4] = v_instancePoints56.xy; + f_points[5] = v_instancePoints56.zw; + f_points[6] = v_instancePoints78.xy; + f_points[7] = v_instancePoints78.zw; + + // Compute polygon AABB + vec2 lower = f_points[0]; + vec2 upper = f_points[0]; + for (int i = 1; i < v_instanceCount; ++i) + { + lower = min(lower, f_points[i]); + upper = max(upper, f_points[i]); + } + + vec2 center = 0.5 * (lower + upper); + vec2 width = upper - lower; + float maxWidth = max(width.x, width.y); + + float scale = f_radius + 0.5 * maxWidth; + float invScale = 1.0 / scale; + + // Shift and scale polygon points so they fit in 2x2 quad + for (int i = 0; i < f_count; ++i) + { + f_points[i] = invScale * (f_points[i] - center); + } + + // Scale radius as well + f_radius = invScale * f_radius; + + // resolution.y = pixelScale * scale + f_thickness = 3.0f / (pixelScale * scale); + + // scale up and transform quad to fit polygon + float x = v_instanceTransform.x; + float y = v_instanceTransform.y; + float c = v_instanceTransform.z; + float s = v_instanceTransform.w; + vec2 p = vec2(scale * v_localPosition.x, scale * v_localPosition.y) + center; + p = vec2((c * p.x - s * p.y) + x, (s * p.x + c * p.y) + y); + gl_Position = projectionMatrix * vec4(p, 0.0f, 1.0f); +} diff --git a/tests/cpp-tests/Source/Box2DTestBed/samples/donut.cpp b/tests/cpp-tests/Source/Box2DTestBed/samples/donut.cpp new file mode 100644 index 000000000000..48c3f65af86b --- /dev/null +++ b/tests/cpp-tests/Source/Box2DTestBed/samples/donut.cpp @@ -0,0 +1,96 @@ +// SPDX-FileCopyrightText: 2023 Erin Catto +// SPDX-License-Identifier: MIT + +#include "donut.h" + +#include "box2d/box2d.h" +#include "box2d/math_functions.h" + +#include + +Donut::Donut() +{ + for ( int i = 0; i < e_sides; ++i ) + { + m_bodyIds[i] = b2_nullBodyId; + m_jointIds[i] = b2_nullJointId; + } + + m_isSpawned = false; +} + +void Donut::Spawn( b2WorldId worldId, b2Vec2 position, float scale, int groupIndex, void* userData ) +{ + assert( m_isSpawned == false ); + + for ( int i = 0; i < e_sides; ++i ) + { + assert( B2_IS_NULL( m_bodyIds[i] ) ); + assert( B2_IS_NULL( m_jointIds[i] ) ); + } + + float radius = 1.0f * scale; + float deltaAngle = 2.0f * b2_pi / e_sides; + float length = 2.0f * b2_pi * radius / e_sides; + + b2Capsule capsule = { { 0.0f, -0.5f * length }, { 0.0f, 0.5f * length }, 0.25f * scale }; + + b2Vec2 center = position; + + b2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.type = b2_dynamicBody; + bodyDef.userData = userData; + + b2ShapeDef shapeDef = b2DefaultShapeDef(); + shapeDef.density = 1.0f; + shapeDef.filter.groupIndex = -groupIndex; + shapeDef.friction = 0.3f; + + // Create bodies + float angle = 0.0f; + for ( int i = 0; i < e_sides; ++i ) + { + bodyDef.position = { radius * cosf( angle ) + center.x, radius * sinf( angle ) + center.y }; + bodyDef.rotation = b2MakeRot( angle ); + + m_bodyIds[i] = b2CreateBody( worldId, &bodyDef ); + b2CreateCapsuleShape( m_bodyIds[i], &shapeDef, &capsule ); + + angle += deltaAngle; + } + + // Create joints + b2WeldJointDef weldDef = b2DefaultWeldJointDef(); + weldDef.angularHertz = 5.0f; + weldDef.angularDampingRatio = 0.0f; + weldDef.localAnchorA = { 0.0f, 0.5f * length }; + weldDef.localAnchorB = { 0.0f, -0.5f * length }; + + b2BodyId prevBodyId = m_bodyIds[e_sides - 1]; + for ( int i = 0; i < e_sides; ++i ) + { + weldDef.bodyIdA = prevBodyId; + weldDef.bodyIdB = m_bodyIds[i]; + b2Rot rotA = b2Body_GetRotation( prevBodyId ); + b2Rot rotB = b2Body_GetRotation( m_bodyIds[i] ); + weldDef.referenceAngle = b2RelativeAngle( rotB, rotA ); + m_jointIds[i] = b2CreateWeldJoint( worldId, &weldDef ); + prevBodyId = weldDef.bodyIdB; + } + + m_isSpawned = true; +} + +void Donut::Despawn() +{ + assert( m_isSpawned == true ); + + for ( int i = 0; i < e_sides; ++i ) + { + b2DestroyBody( m_bodyIds[i] ); + m_bodyIds[i] = b2_nullBodyId; + m_jointIds[i] = b2_nullJointId; + } + + m_isSpawned = false; +} diff --git a/tests/cpp-tests/Source/Box2DTestBed/samples/donut.h b/tests/cpp-tests/Source/Box2DTestBed/samples/donut.h new file mode 100644 index 000000000000..aecc016c5143 --- /dev/null +++ b/tests/cpp-tests/Source/Box2DTestBed/samples/donut.h @@ -0,0 +1,24 @@ +// SPDX-FileCopyrightText: 2023 Erin Catto +// SPDX-License-Identifier: MIT + +#pragma once + +#include "box2d/types.h" + +class Donut +{ + enum + { + e_sides = 7 + }; + +public: + Donut(); + + void Spawn( b2WorldId worldId, b2Vec2 position, float scale, int groupIndex, void* userData ); + void Despawn(); + + b2BodyId m_bodyIds[e_sides]; + b2JointId m_jointIds[e_sides]; + bool m_isSpawned; +}; diff --git a/tests/cpp-tests/Source/Box2DTestBed/samples/doohickey.cpp b/tests/cpp-tests/Source/Box2DTestBed/samples/doohickey.cpp new file mode 100644 index 000000000000..89ac2dafd2a1 --- /dev/null +++ b/tests/cpp-tests/Source/Box2DTestBed/samples/doohickey.cpp @@ -0,0 +1,101 @@ +// SPDX-FileCopyrightText: 2022 Erin Catto +// SPDX-License-Identifier: MIT + +#include "doohickey.h" + +#include "box2d/box2d.h" +#include "box2d/math_functions.h" + +#include + +Doohickey::Doohickey() +{ + m_wheelId1 = {}; + m_wheelId2 = {}; + m_barId1 = {}; + m_barId2 = {}; + + m_axleId1 = {}; + m_axleId2 = {}; + m_sliderId = {}; + + m_isSpawned = false; +} + +void Doohickey::Spawn( b2WorldId worldId, b2Vec2 position, float scale ) +{ + assert( m_isSpawned == false ); + + b2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.type = b2_dynamicBody; + + b2ShapeDef shapeDef = b2DefaultShapeDef(); + b2Circle circle = { { 0.0f, 0.0f }, 1.0f * scale }; + b2Capsule capsule = { { -3.5f * scale, 0.0f }, { 3.5f * scale, 0.0f }, 0.15f * scale }; + + bodyDef.position = b2MulAdd( position, scale, { -5.0f, 3.0f } ); + m_wheelId1 = b2CreateBody( worldId, &bodyDef ); + b2CreateCircleShape( m_wheelId1, &shapeDef, &circle ); + + bodyDef.position = b2MulAdd( position, scale, { 5.0f, 3.0f } ); + m_wheelId2 = b2CreateBody( worldId, &bodyDef ); + b2CreateCircleShape( m_wheelId2, &shapeDef, &circle ); + + bodyDef.position = b2MulAdd( position, scale, { -1.5f, 3.0f } ); + m_barId1 = b2CreateBody( worldId, &bodyDef ); + b2CreateCapsuleShape( m_barId1, &shapeDef, &capsule ); + + bodyDef.position = b2MulAdd( position, scale, { 1.5f, 3.0f } ); + m_barId2 = b2CreateBody( worldId, &bodyDef ); + b2CreateCapsuleShape( m_barId2, &shapeDef, &capsule ); + + b2RevoluteJointDef revoluteDef = b2DefaultRevoluteJointDef(); + + revoluteDef.bodyIdA = m_wheelId1; + revoluteDef.bodyIdB = m_barId1; + revoluteDef.localAnchorA = { 0.0f, 0.0f }; + revoluteDef.localAnchorB = { -3.5f * scale, 0.0f }; + revoluteDef.enableMotor = true; + revoluteDef.maxMotorTorque = 2.0f * scale; + b2CreateRevoluteJoint( worldId, &revoluteDef ); + + revoluteDef.bodyIdA = m_wheelId2; + revoluteDef.bodyIdB = m_barId2; + revoluteDef.localAnchorA = { 0.0f, 0.0f }; + revoluteDef.localAnchorB = { 3.5f * scale, 0.0f }; + revoluteDef.enableMotor = true; + revoluteDef.maxMotorTorque = 2.0f * scale; + b2CreateRevoluteJoint( worldId, &revoluteDef ); + + b2PrismaticJointDef prismaticDef = b2DefaultPrismaticJointDef(); + prismaticDef.bodyIdA = m_barId1; + prismaticDef.bodyIdB = m_barId2; + prismaticDef.localAxisA = { 1.0f, 0.0f }; + prismaticDef.localAnchorA = { 2.0f * scale, 0.0f }; + prismaticDef.localAnchorB = { -2.0f * scale, 0.0f }; + prismaticDef.lowerTranslation = -2.0f * scale; + prismaticDef.upperTranslation = 2.0f * scale; + prismaticDef.enableLimit = true; + prismaticDef.enableMotor = true; + prismaticDef.maxMotorForce = 2.0f * scale; + prismaticDef.enableSpring = true; + prismaticDef.hertz = 1.0f; + prismaticDef.dampingRatio = 0.5; + b2CreatePrismaticJoint( worldId, &prismaticDef ); +} + +void Doohickey::Despawn() +{ + assert( m_isSpawned == true ); + + b2DestroyJoint( m_axleId1 ); + b2DestroyJoint( m_axleId2 ); + b2DestroyJoint( m_sliderId ); + + b2DestroyBody( m_wheelId1 ); + b2DestroyBody( m_wheelId2 ); + b2DestroyBody( m_barId1 ); + b2DestroyBody( m_barId2 ); + + m_isSpawned = false; +} diff --git a/tests/cpp-tests/Source/Box2DTestBed/samples/doohickey.h b/tests/cpp-tests/Source/Box2DTestBed/samples/doohickey.h new file mode 100644 index 000000000000..0db7345ffd7d --- /dev/null +++ b/tests/cpp-tests/Source/Box2DTestBed/samples/doohickey.h @@ -0,0 +1,26 @@ +// SPDX-FileCopyrightText: 2023 Erin Catto +// SPDX-License-Identifier: MIT + +#pragma once + +#include "box2d/types.h" + +class Doohickey +{ +public: + Doohickey(); + + void Spawn( b2WorldId worldId, b2Vec2 position, float scale ); + void Despawn(); + + b2BodyId m_wheelId1; + b2BodyId m_wheelId2; + b2BodyId m_barId1; + b2BodyId m_barId2; + + b2JointId m_axleId1; + b2JointId m_axleId2; + b2JointId m_sliderId; + + bool m_isSpawned; +}; diff --git a/tests/cpp-tests/Source/Box2DTestBed/samples/draw.h b/tests/cpp-tests/Source/Box2DTestBed/samples/draw.h new file mode 100644 index 000000000000..1d73d6a5684e --- /dev/null +++ b/tests/cpp-tests/Source/Box2DTestBed/samples/draw.h @@ -0,0 +1,3 @@ +#pragma once + +#include "../Box2DTestDebugDrawNode.h" diff --git a/tests/cpp-tests/Source/Box2DTestBed/samples/human.cpp b/tests/cpp-tests/Source/Box2DTestBed/samples/human.cpp new file mode 100644 index 000000000000..70912a929436 --- /dev/null +++ b/tests/cpp-tests/Source/Box2DTestBed/samples/human.cpp @@ -0,0 +1,573 @@ +// SPDX-FileCopyrightText: 2023 Erin Catto +// SPDX-License-Identifier: MIT + +#include "human.h" + +#include "sample.h" + +#include "box2d/box2d.h" +#include "box2d/math_functions.h" + +#include + +Human::Human() +{ + for ( int i = 0; i < Bone::e_count; ++i ) + { + m_bones[i].bodyId = b2_nullBodyId; + m_bones[i].jointId = b2_nullJointId; + m_bones[i].frictionScale = 1.0f; + m_bones[i].parentIndex = -1; + } + + m_scale = 1.0f; + m_isSpawned = false; +} + +void Human::Spawn( b2WorldId worldId, b2Vec2 position, float scale, float frictionTorque, float hertz, float dampingRatio, + int groupIndex, void* userData, bool colorize ) +{ + assert( m_isSpawned == false ); + + b2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.type = b2_dynamicBody; + bodyDef.sleepThreshold = 0.1f; + bodyDef.userData = userData; + + b2ShapeDef shapeDef = b2DefaultShapeDef(); + shapeDef.friction = 0.2f; + shapeDef.filter.groupIndex = -groupIndex; + shapeDef.filter.maskBits = 1; + + b2ShapeDef footShapeDef = shapeDef; + footShapeDef.friction = 0.05f; + + if ( colorize ) + { + footShapeDef.customColor = b2_colorSaddleBrown; + } + + m_scale = scale; + float s = scale; + float maxTorque = frictionTorque * s; + bool enableMotor = true; + bool enableLimit = true; + float drawSize = 0.05f; + + b2HexColor shirtColor = b2_colorMediumTurquoise; + b2HexColor pantColor = b2_colorDodgerBlue; + + b2HexColor skinColors[4] = { b2_colorNavajoWhite, b2_colorLightYellow, b2_colorPeru, b2_colorTan }; + b2HexColor skinColor = skinColors[groupIndex % 4]; + + // hip + { + Bone* bone = m_bones + Bone::e_hip; + bone->parentIndex = -1; + + bodyDef.position = b2Add( { 0.0f, 0.95f * s }, position ); + bodyDef.linearDamping = 0.0f; + bone->bodyId = b2CreateBody( worldId, &bodyDef ); + + if ( colorize ) + { + shapeDef.customColor = pantColor; + } + + b2Capsule capsule = { { 0.0f, -0.02f * s }, { 0.0f, 0.02f * s }, 0.095f * s }; + b2CreateCapsuleShape( bone->bodyId, &shapeDef, &capsule ); + } + + // torso + { + Bone* bone = m_bones + Bone::e_torso; + bone->parentIndex = Bone::e_hip; + + bodyDef.position = b2Add( { 0.0f, 1.2f * s }, position ); + bodyDef.linearDamping = 0.0f; + // bodyDef.type = b2_staticBody; + bone->bodyId = b2CreateBody( worldId, &bodyDef ); + bone->frictionScale = 0.5f; + bodyDef.type = b2_dynamicBody; + + if ( colorize ) + { + shapeDef.customColor = shirtColor; + } + + b2Capsule capsule = { { 0.0f, -0.135f * s }, { 0.0f, 0.135f * s }, 0.09f * s }; + b2CreateCapsuleShape( bone->bodyId, &shapeDef, &capsule ); + + b2Vec2 pivot = b2Add( { 0.0f, 1.0f * s }, position ); + b2RevoluteJointDef jointDef = b2DefaultRevoluteJointDef(); + jointDef.bodyIdA = m_bones[bone->parentIndex].bodyId; + jointDef.bodyIdB = bone->bodyId; + jointDef.localAnchorA = b2Body_GetLocalPoint( jointDef.bodyIdA, pivot ); + jointDef.localAnchorB = b2Body_GetLocalPoint( jointDef.bodyIdB, pivot ); + jointDef.enableLimit = enableLimit; + jointDef.lowerAngle = -0.25f * b2_pi; + jointDef.upperAngle = 0.0f; + jointDef.enableMotor = enableMotor; + jointDef.maxMotorTorque = bone->frictionScale * maxTorque; + jointDef.enableSpring = hertz > 0.0f; + jointDef.hertz = hertz; + jointDef.dampingRatio = dampingRatio; + jointDef.drawSize = drawSize; + + bone->jointId = b2CreateRevoluteJoint( worldId, &jointDef ); + } + + // head + { + Bone* bone = m_bones + Bone::e_head; + bone->parentIndex = Bone::e_torso; + + bodyDef.position = b2Add( { 0.0f * s, 1.5f * s }, position ); + bodyDef.linearDamping = 0.1f; + + bone->bodyId = b2CreateBody( worldId, &bodyDef ); + bone->frictionScale = 0.25f; + + if ( colorize ) + { + shapeDef.customColor = skinColor; + } + + b2Capsule capsule = { { 0.0f, -0.0325f * s }, { 0.0f, 0.0325f * s }, 0.08f * s }; + b2CreateCapsuleShape( bone->bodyId, &shapeDef, &capsule ); + + // neck + capsule = { { 0.0f, -0.12f * s }, { 0.0f, -0.08f * s }, 0.05f * s }; + b2CreateCapsuleShape( bone->bodyId, &shapeDef, &capsule ); + + b2Vec2 pivot = b2Add( { 0.0f, 1.4f * s }, position ); + b2RevoluteJointDef jointDef = b2DefaultRevoluteJointDef(); + jointDef.bodyIdA = m_bones[bone->parentIndex].bodyId; + jointDef.bodyIdB = bone->bodyId; + jointDef.localAnchorA = b2Body_GetLocalPoint( jointDef.bodyIdA, pivot ); + jointDef.localAnchorB = b2Body_GetLocalPoint( jointDef.bodyIdB, pivot ); + jointDef.enableLimit = enableLimit; + jointDef.lowerAngle = -0.3f * b2_pi; + jointDef.upperAngle = 0.1f * b2_pi; + jointDef.enableMotor = enableMotor; + jointDef.maxMotorTorque = bone->frictionScale * maxTorque; + jointDef.enableSpring = hertz > 0.0f; + jointDef.hertz = hertz; + jointDef.dampingRatio = dampingRatio; + jointDef.drawSize = drawSize; + + bone->jointId = b2CreateRevoluteJoint( worldId, &jointDef ); + } + + // upper left leg + { + Bone* bone = m_bones + Bone::e_upperLeftLeg; + bone->parentIndex = Bone::e_hip; + + bodyDef.position = b2Add( { 0.0f, 0.775f * s }, position ); + bodyDef.linearDamping = 0.0f; + bone->bodyId = b2CreateBody( worldId, &bodyDef ); + bone->frictionScale = 1.0f; + + if ( colorize ) + { + shapeDef.customColor = pantColor; + } + + b2Capsule capsule = { { 0.0f, -0.125f * s }, { 0.0f, 0.125f * s }, 0.06f * s }; + b2CreateCapsuleShape( bone->bodyId, &shapeDef, &capsule ); + + b2Vec2 pivot = b2Add( { 0.0f, 0.9f * s }, position ); + b2RevoluteJointDef jointDef = b2DefaultRevoluteJointDef(); + jointDef.bodyIdA = m_bones[bone->parentIndex].bodyId; + jointDef.bodyIdB = bone->bodyId; + jointDef.localAnchorA = b2Body_GetLocalPoint( jointDef.bodyIdA, pivot ); + jointDef.localAnchorB = b2Body_GetLocalPoint( jointDef.bodyIdB, pivot ); + jointDef.enableLimit = enableLimit; + jointDef.lowerAngle = -0.05f * b2_pi; + jointDef.upperAngle = 0.4f * b2_pi; + jointDef.enableMotor = enableMotor; + jointDef.maxMotorTorque = bone->frictionScale * maxTorque; + jointDef.enableSpring = hertz > 0.0f; + jointDef.hertz = hertz; + jointDef.dampingRatio = dampingRatio; + jointDef.drawSize = drawSize; + + bone->jointId = b2CreateRevoluteJoint( worldId, &jointDef ); + } + + // lower left leg + { + Bone* bone = m_bones + Bone::e_lowerLeftLeg; + bone->parentIndex = Bone::e_upperLeftLeg; + + bodyDef.position = b2Add( { 0.0f, 0.475f * s }, position ); + bodyDef.linearDamping = 0.0f; + bone->bodyId = b2CreateBody( worldId, &bodyDef ); + bone->frictionScale = 0.5f; + + if ( colorize ) + { + shapeDef.customColor = pantColor; + } + + b2Capsule capsule = { { 0.0f, -0.14f * s }, { 0.0f, 0.125f * s }, 0.05f * s }; + b2CreateCapsuleShape( bone->bodyId, &shapeDef, &capsule ); + + // b2Polygon box = b2MakeOffsetBox(0.1f * s, 0.03f * s, {0.05f * s, -0.175f * s}, 0.0f); + // b2CreatePolygonShape(bone->bodyId, &shapeDef, &box); + + capsule = { { -0.02f * s, -0.175f * s }, { 0.13f * s, -0.175f * s }, 0.03f * s }; + b2CreateCapsuleShape( bone->bodyId, &footShapeDef, &capsule ); + + b2Vec2 pivot = b2Add( { 0.0f, 0.625f * s }, position ); + b2RevoluteJointDef jointDef = b2DefaultRevoluteJointDef(); + jointDef.bodyIdA = m_bones[bone->parentIndex].bodyId; + jointDef.bodyIdB = bone->bodyId; + jointDef.localAnchorA = b2Body_GetLocalPoint( jointDef.bodyIdA, pivot ); + jointDef.localAnchorB = b2Body_GetLocalPoint( jointDef.bodyIdB, pivot ); + jointDef.enableLimit = enableLimit; + jointDef.lowerAngle = -0.5f * b2_pi; + jointDef.upperAngle = -0.02f * b2_pi; + jointDef.enableMotor = enableMotor; + jointDef.maxMotorTorque = bone->frictionScale * maxTorque; + jointDef.enableSpring = hertz > 0.0f; + jointDef.hertz = hertz; + jointDef.dampingRatio = dampingRatio; + jointDef.drawSize = drawSize; + + bone->jointId = b2CreateRevoluteJoint( worldId, &jointDef ); + } + + // upper right leg + { + Bone* bone = m_bones + Bone::e_upperRightLeg; + bone->parentIndex = Bone::e_hip; + + bodyDef.position = b2Add( { 0.0f, 0.775f * s }, position ); + bodyDef.linearDamping = 0.0f; + bone->bodyId = b2CreateBody( worldId, &bodyDef ); + bone->frictionScale = 1.0f; + + if ( colorize ) + { + shapeDef.customColor = pantColor; + } + + b2Capsule capsule = { { 0.0f, -0.125f * s }, { 0.0f, 0.125f * s }, 0.06f * s }; + b2CreateCapsuleShape( bone->bodyId, &shapeDef, &capsule ); + + b2Vec2 pivot = b2Add( { 0.0f, 0.9f * s }, position ); + b2RevoluteJointDef jointDef = b2DefaultRevoluteJointDef(); + jointDef.bodyIdA = m_bones[bone->parentIndex].bodyId; + jointDef.bodyIdB = bone->bodyId; + jointDef.localAnchorA = b2Body_GetLocalPoint( jointDef.bodyIdA, pivot ); + jointDef.localAnchorB = b2Body_GetLocalPoint( jointDef.bodyIdB, pivot ); + jointDef.enableLimit = enableLimit; + jointDef.lowerAngle = -0.05f * b2_pi; + jointDef.upperAngle = 0.4f * b2_pi; + jointDef.enableMotor = enableMotor; + jointDef.maxMotorTorque = bone->frictionScale * maxTorque; + jointDef.enableSpring = hertz > 0.0f; + jointDef.hertz = hertz; + jointDef.dampingRatio = dampingRatio; + jointDef.drawSize = drawSize; + + bone->jointId = b2CreateRevoluteJoint( worldId, &jointDef ); + } + + // lower right leg + { + Bone* bone = m_bones + Bone::e_lowerRightLeg; + bone->parentIndex = Bone::e_upperRightLeg; + + bodyDef.position = b2Add( { 0.0f, 0.475f * s }, position ); + bodyDef.linearDamping = 0.0f; + bone->bodyId = b2CreateBody( worldId, &bodyDef ); + bone->frictionScale = 0.5f; + + if ( colorize ) + { + shapeDef.customColor = pantColor; + } + + b2Capsule capsule = { { 0.0f, -0.14f * s }, { 0.0f, 0.125f * s }, 0.05f * s }; + b2CreateCapsuleShape( bone->bodyId, &shapeDef, &capsule ); + + // b2Polygon box = b2MakeOffsetBox(0.1f * s, 0.03f * s, {0.05f * s, -0.175f * s}, 0.0f); + // b2CreatePolygonShape(bone->bodyId, &shapeDef, &box); + + capsule = { { -0.02f * s, -0.175f * s }, { 0.13f * s, -0.175f * s }, 0.03f * s }; + b2CreateCapsuleShape( bone->bodyId, &footShapeDef, &capsule ); + + b2Vec2 pivot = b2Add( { 0.0f, 0.625f * s }, position ); + b2RevoluteJointDef jointDef = b2DefaultRevoluteJointDef(); + jointDef.bodyIdA = m_bones[bone->parentIndex].bodyId; + jointDef.bodyIdB = bone->bodyId; + jointDef.localAnchorA = b2Body_GetLocalPoint( jointDef.bodyIdA, pivot ); + jointDef.localAnchorB = b2Body_GetLocalPoint( jointDef.bodyIdB, pivot ); + jointDef.enableLimit = enableLimit; + jointDef.lowerAngle = -0.5f * b2_pi; + jointDef.upperAngle = -0.02f * b2_pi; + jointDef.enableMotor = enableMotor; + jointDef.maxMotorTorque = bone->frictionScale * maxTorque; + jointDef.enableSpring = hertz > 0.0f; + jointDef.hertz = hertz; + jointDef.dampingRatio = dampingRatio; + jointDef.drawSize = drawSize; + + bone->jointId = b2CreateRevoluteJoint( worldId, &jointDef ); + } + + // upper left arm + { + Bone* bone = m_bones + Bone::e_upperLeftArm; + bone->parentIndex = Bone::e_torso; + bone->frictionScale = 0.5f; + + bodyDef.position = b2Add( { 0.0f, 1.225f * s }, position ); + bodyDef.linearDamping = 0.0f; + bone->bodyId = b2CreateBody( worldId, &bodyDef ); + + if ( colorize ) + { + shapeDef.customColor = shirtColor; + } + + b2Capsule capsule = { { 0.0f, -0.125f * s }, { 0.0f, 0.125f * s }, 0.035f * s }; + b2CreateCapsuleShape( bone->bodyId, &shapeDef, &capsule ); + + b2Vec2 pivot = b2Add( { 0.0f, 1.35f * s }, position ); + b2RevoluteJointDef jointDef = b2DefaultRevoluteJointDef(); + jointDef.bodyIdA = m_bones[bone->parentIndex].bodyId; + jointDef.bodyIdB = bone->bodyId; + jointDef.localAnchorA = b2Body_GetLocalPoint( jointDef.bodyIdA, pivot ); + jointDef.localAnchorB = b2Body_GetLocalPoint( jointDef.bodyIdB, pivot ); + jointDef.enableLimit = enableLimit; + jointDef.lowerAngle = -0.1f * b2_pi; + jointDef.upperAngle = 0.8f * b2_pi; + jointDef.enableMotor = enableMotor; + jointDef.maxMotorTorque = bone->frictionScale * maxTorque; + jointDef.enableSpring = hertz > 0.0f; + jointDef.hertz = hertz; + jointDef.dampingRatio = dampingRatio; + jointDef.drawSize = drawSize; + + bone->jointId = b2CreateRevoluteJoint( worldId, &jointDef ); + } + + // lower left arm + { + Bone* bone = m_bones + Bone::e_lowerLeftArm; + bone->parentIndex = Bone::e_upperLeftArm; + + bodyDef.position = b2Add( { 0.0f, 0.975f * s }, position ); + bodyDef.linearDamping = 0.1f; + bone->bodyId = b2CreateBody( worldId, &bodyDef ); + bone->frictionScale = 0.1f; + + if ( colorize ) + { + shapeDef.customColor = skinColor; + } + + b2Capsule capsule = { { 0.0f, -0.125f * s }, { 0.0f, 0.125f * s }, 0.03f * s }; + b2CreateCapsuleShape( bone->bodyId, &shapeDef, &capsule ); + + b2Vec2 pivot = b2Add( { 0.0f, 1.1f * s }, position ); + b2RevoluteJointDef jointDef = b2DefaultRevoluteJointDef(); + jointDef.bodyIdA = m_bones[bone->parentIndex].bodyId; + jointDef.bodyIdB = bone->bodyId; + jointDef.localAnchorA = b2Body_GetLocalPoint( jointDef.bodyIdA, pivot ); + jointDef.localAnchorB = b2Body_GetLocalPoint( jointDef.bodyIdB, pivot ); + jointDef.enableLimit = enableLimit; + jointDef.lowerAngle = 0.01f * b2_pi; + jointDef.upperAngle = 0.5f * b2_pi; + jointDef.enableMotor = enableMotor; + jointDef.maxMotorTorque = bone->frictionScale * maxTorque; + jointDef.enableSpring = hertz > 0.0f; + jointDef.hertz = hertz; + jointDef.dampingRatio = dampingRatio; + jointDef.drawSize = drawSize; + + bone->jointId = b2CreateRevoluteJoint( worldId, &jointDef ); + } + + // upper right arm + { + Bone* bone = m_bones + Bone::e_upperRightArm; + bone->parentIndex = Bone::e_torso; + + bodyDef.position = b2Add( { 0.0f, 1.225f * s }, position ); + bodyDef.linearDamping = 0.0f; + bone->bodyId = b2CreateBody( worldId, &bodyDef ); + bone->frictionScale = 0.5f; + + if ( colorize ) + { + shapeDef.customColor = shirtColor; + } + + b2Capsule capsule = { { 0.0f, -0.125f * s }, { 0.0f, 0.125f * s }, 0.035f * s }; + b2CreateCapsuleShape( bone->bodyId, &shapeDef, &capsule ); + + b2Vec2 pivot = b2Add( { 0.0f, 1.35f * s }, position ); + b2RevoluteJointDef jointDef = b2DefaultRevoluteJointDef(); + jointDef.bodyIdA = m_bones[bone->parentIndex].bodyId; + jointDef.bodyIdB = bone->bodyId; + jointDef.localAnchorA = b2Body_GetLocalPoint( jointDef.bodyIdA, pivot ); + jointDef.localAnchorB = b2Body_GetLocalPoint( jointDef.bodyIdB, pivot ); + jointDef.enableLimit = enableLimit; + jointDef.lowerAngle = -0.1f * b2_pi; + jointDef.upperAngle = 0.8f * b2_pi; + jointDef.enableMotor = enableMotor; + jointDef.maxMotorTorque = bone->frictionScale * maxTorque; + jointDef.enableSpring = hertz > 0.0f; + jointDef.hertz = hertz; + jointDef.dampingRatio = dampingRatio; + jointDef.drawSize = drawSize; + + bone->jointId = b2CreateRevoluteJoint( worldId, &jointDef ); + } + + // lower right arm + { + Bone* bone = m_bones + Bone::e_lowerRightArm; + bone->parentIndex = Bone::e_upperRightArm; + + bodyDef.position = b2Add( { 0.0f, 0.975f * s }, position ); + bodyDef.linearDamping = 0.1f; + bone->bodyId = b2CreateBody( worldId, &bodyDef ); + bone->frictionScale = 0.1f; + + if ( colorize ) + { + shapeDef.customColor = skinColor; + } + + b2Capsule capsule = { { 0.0f, -0.125f * s }, { 0.0f, 0.125f * s }, 0.03f * s }; + b2CreateCapsuleShape( bone->bodyId, &shapeDef, &capsule ); + + b2Vec2 pivot = b2Add( { 0.0f, 1.1f * s }, position ); + b2RevoluteJointDef jointDef = b2DefaultRevoluteJointDef(); + jointDef.bodyIdA = m_bones[bone->parentIndex].bodyId; + jointDef.bodyIdB = bone->bodyId; + jointDef.localAnchorA = b2Body_GetLocalPoint( jointDef.bodyIdA, pivot ); + jointDef.localAnchorB = b2Body_GetLocalPoint( jointDef.bodyIdB, pivot ); + jointDef.enableLimit = enableLimit; + jointDef.lowerAngle = 0.01f * b2_pi; + jointDef.upperAngle = 0.5f * b2_pi; + jointDef.enableMotor = enableMotor; + jointDef.maxMotorTorque = bone->frictionScale * maxTorque; + jointDef.enableSpring = hertz > 0.0f; + jointDef.hertz = hertz; + jointDef.dampingRatio = dampingRatio; + jointDef.drawSize = drawSize; + + bone->jointId = b2CreateRevoluteJoint( worldId, &jointDef ); + } + + m_isSpawned = true; +} + +void Human::Despawn() +{ + assert( m_isSpawned == true ); + + for ( int i = 0; i < Bone::e_count; ++i ) + { + if ( B2_IS_NULL( m_bones[i].jointId ) ) + { + continue; + } + + b2DestroyJoint( m_bones[i].jointId ); + m_bones[i].jointId = b2_nullJointId; + } + + for ( int i = 0; i < Bone::e_count; ++i ) + { + if ( B2_IS_NULL( m_bones[i].bodyId ) ) + { + continue; + } + + b2DestroyBody( m_bones[i].bodyId ); + m_bones[i].bodyId = b2_nullBodyId; + } + + m_isSpawned = false; +} + +void Human::ApplyRandomAngularImpulse( float magnitude ) +{ + if ( m_isSpawned == false ) + { + return; + } + + float impulse = RandomFloat( -magnitude, magnitude ); + b2Body_ApplyAngularImpulse( m_bones[Bone::e_torso].bodyId, impulse, true ); +} + +void Human::SetJointFrictionTorque( float torque ) +{ + if ( m_isSpawned == false ) + { + return; + } + + if ( torque == 0.0f ) + { + for ( int i = 1; i < Bone::e_count; ++i ) + { + b2RevoluteJoint_EnableMotor( m_bones[i].jointId, false ); + } + } + else + { + for ( int i = 1; i < Bone::e_count; ++i ) + { + b2RevoluteJoint_EnableMotor( m_bones[i].jointId, true ); + float scale = m_scale * m_bones[i].frictionScale; + b2RevoluteJoint_SetMaxMotorTorque( m_bones[i].jointId, scale * torque ); + } + } +} + +void Human::SetJointSpringHertz( float hertz ) +{ + if ( m_isSpawned == false ) + { + return; + } + + if ( hertz == 0.0f ) + { + for ( int i = 1; i < Bone::e_count; ++i ) + { + b2RevoluteJoint_EnableSpring( m_bones[i].jointId, false ); + } + } + else + { + for ( int i = 1; i < Bone::e_count; ++i ) + { + b2RevoluteJoint_EnableSpring( m_bones[i].jointId, true ); + b2RevoluteJoint_SetSpringHertz( m_bones[i].jointId, hertz ); + } + } +} + +void Human::SetJointDampingRatio( float dampingRatio ) +{ + if ( m_isSpawned == false ) + { + return; + } + + for ( int i = 1; i < Bone::e_count; ++i ) + { + b2RevoluteJoint_SetSpringDampingRatio( m_bones[i].jointId, dampingRatio ); + } +} diff --git a/tests/cpp-tests/Source/Box2DTestBed/samples/human.h b/tests/cpp-tests/Source/Box2DTestBed/samples/human.h new file mode 100644 index 000000000000..306a867da7b2 --- /dev/null +++ b/tests/cpp-tests/Source/Box2DTestBed/samples/human.h @@ -0,0 +1,49 @@ +// SPDX-FileCopyrightText: 2023 Erin Catto +// SPDX-License-Identifier: MIT + +#pragma once + +#include "box2d/types.h" + +struct Bone +{ + enum + { + e_hip = 0, + e_torso = 1, + e_head = 2, + e_upperLeftLeg = 3, + e_lowerLeftLeg = 4, + e_upperRightLeg = 5, + e_lowerRightLeg = 6, + e_upperLeftArm = 7, + e_lowerLeftArm = 8, + e_upperRightArm = 9, + e_lowerRightArm = 10, + e_count = 11, + }; + + b2BodyId bodyId; + b2JointId jointId; + float frictionScale; + int parentIndex; +}; + +class Human +{ +public: + Human(); + + void Spawn( b2WorldId worldId, b2Vec2 position, float scale, float frictionTorque, float hertz, float dampingRatio, + int groupIndex, void* userData, bool colorize ); + void Despawn(); + + void ApplyRandomAngularImpulse( float magnitude ); + void SetJointFrictionTorque( float torque ); + void SetJointSpringHertz( float hertz ); + void SetJointDampingRatio( float dampingRatio ); + + Bone m_bones[Bone::e_count]; + float m_scale; + bool m_isSpawned; +}; diff --git a/tests/cpp-tests/Source/Box2DTestBed/samples/jsmn/jsmn.h b/tests/cpp-tests/Source/Box2DTestBed/samples/jsmn/jsmn.h new file mode 100644 index 000000000000..8ac14c1bdec9 --- /dev/null +++ b/tests/cpp-tests/Source/Box2DTestBed/samples/jsmn/jsmn.h @@ -0,0 +1,471 @@ +/* + * MIT License + * + * Copyright (c) 2010 Serge Zaitsev + * + * 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 JSMN_H +#define JSMN_H + +#include + +#ifdef __cplusplus +extern "C" { +#endif + +#ifdef JSMN_STATIC +#define JSMN_API static +#else +#define JSMN_API extern +#endif + +/** + * JSON type identifier. Basic types are: + * o Object + * o Array + * o String + * o Other primitive: number, boolean (true/false) or null + */ +typedef enum { + JSMN_UNDEFINED = 0, + JSMN_OBJECT = 1 << 0, + JSMN_ARRAY = 1 << 1, + JSMN_STRING = 1 << 2, + JSMN_PRIMITIVE = 1 << 3 +} jsmntype_t; + +enum jsmnerr { + /* Not enough tokens were provided */ + JSMN_ERROR_NOMEM = -1, + /* Invalid character inside JSON string */ + JSMN_ERROR_INVAL = -2, + /* The string is not a full JSON packet, more bytes expected */ + JSMN_ERROR_PART = -3 +}; + +/** + * JSON token description. + * type type (object, array, string etc.) + * start start position in JSON data string + * end end position in JSON data string + */ +typedef struct jsmntok { + jsmntype_t type; + int start; + int end; + int size; +#ifdef JSMN_PARENT_LINKS + int parent; +#endif +} jsmntok_t; + +/** + * JSON parser. Contains an array of token blocks available. Also stores + * the string being parsed now and current position in that string. + */ +typedef struct jsmn_parser { + unsigned int pos; /* offset in the JSON string */ + unsigned int toknext; /* next token to allocate */ + int toksuper; /* superior token node, e.g. parent object or array */ +} jsmn_parser; + +/** + * Create JSON parser over an array of tokens + */ +JSMN_API void jsmn_init(jsmn_parser *parser); + +/** + * Run JSON parser. It parses a JSON data string into and array of tokens, each + * describing + * a single JSON object. + */ +JSMN_API int jsmn_parse(jsmn_parser *parser, const char *js, const size_t len, + jsmntok_t *tokens, const unsigned int num_tokens); + +#ifndef JSMN_HEADER +/** + * Allocates a fresh unused token from the token pool. + */ +static jsmntok_t *jsmn_alloc_token(jsmn_parser *parser, jsmntok_t *tokens, + const size_t num_tokens) { + jsmntok_t *tok; + if (parser->toknext >= num_tokens) { + return NULL; + } + tok = &tokens[parser->toknext++]; + tok->start = tok->end = -1; + tok->size = 0; +#ifdef JSMN_PARENT_LINKS + tok->parent = -1; +#endif + return tok; +} + +/** + * Fills token type and boundaries. + */ +static void jsmn_fill_token(jsmntok_t *token, const jsmntype_t type, + const int start, const int end) { + token->type = type; + token->start = start; + token->end = end; + token->size = 0; +} + +/** + * Fills next available token with JSON primitive. + */ +static int jsmn_parse_primitive(jsmn_parser *parser, const char *js, + const size_t len, jsmntok_t *tokens, + const size_t num_tokens) { + jsmntok_t *token; + int start; + + start = parser->pos; + + for (; parser->pos < len && js[parser->pos] != '\0'; parser->pos++) { + switch (js[parser->pos]) { +#ifndef JSMN_STRICT + /* In strict mode primitive must be followed by "," or "}" or "]" */ + case ':': +#endif + case '\t': + case '\r': + case '\n': + case ' ': + case ',': + case ']': + case '}': + goto found; + default: + /* to quiet a warning from gcc*/ + break; + } + if (js[parser->pos] < 32 || js[parser->pos] >= 127) { + parser->pos = start; + return JSMN_ERROR_INVAL; + } + } +#ifdef JSMN_STRICT + /* In strict mode primitive must be followed by a comma/object/array */ + parser->pos = start; + return JSMN_ERROR_PART; +#endif + +found: + if (tokens == NULL) { + parser->pos--; + return 0; + } + token = jsmn_alloc_token(parser, tokens, num_tokens); + if (token == NULL) { + parser->pos = start; + return JSMN_ERROR_NOMEM; + } + jsmn_fill_token(token, JSMN_PRIMITIVE, start, parser->pos); +#ifdef JSMN_PARENT_LINKS + token->parent = parser->toksuper; +#endif + parser->pos--; + return 0; +} + +/** + * Fills next token with JSON string. + */ +static int jsmn_parse_string(jsmn_parser *parser, const char *js, + const size_t len, jsmntok_t *tokens, + const size_t num_tokens) { + jsmntok_t *token; + + int start = parser->pos; + + /* Skip starting quote */ + parser->pos++; + + for (; parser->pos < len && js[parser->pos] != '\0'; parser->pos++) { + char c = js[parser->pos]; + + /* Quote: end of string */ + if (c == '\"') { + if (tokens == NULL) { + return 0; + } + token = jsmn_alloc_token(parser, tokens, num_tokens); + if (token == NULL) { + parser->pos = start; + return JSMN_ERROR_NOMEM; + } + jsmn_fill_token(token, JSMN_STRING, start + 1, parser->pos); +#ifdef JSMN_PARENT_LINKS + token->parent = parser->toksuper; +#endif + return 0; + } + + /* Backslash: Quoted symbol expected */ + if (c == '\\' && parser->pos + 1 < len) { + int i; + parser->pos++; + switch (js[parser->pos]) { + /* Allowed escaped symbols */ + case '\"': + case '/': + case '\\': + case 'b': + case 'f': + case 'r': + case 'n': + case 't': + break; + /* Allows escaped symbol \uXXXX */ + case 'u': + parser->pos++; + for (i = 0; i < 4 && parser->pos < len && js[parser->pos] != '\0'; + i++) { + /* If it isn't a hex character we have an error */ + if (!((js[parser->pos] >= 48 && js[parser->pos] <= 57) || /* 0-9 */ + (js[parser->pos] >= 65 && js[parser->pos] <= 70) || /* A-F */ + (js[parser->pos] >= 97 && js[parser->pos] <= 102))) { /* a-f */ + parser->pos = start; + return JSMN_ERROR_INVAL; + } + parser->pos++; + } + parser->pos--; + break; + /* Unexpected symbol */ + default: + parser->pos = start; + return JSMN_ERROR_INVAL; + } + } + } + parser->pos = start; + return JSMN_ERROR_PART; +} + +/** + * Parse JSON string and fill tokens. + */ +JSMN_API int jsmn_parse(jsmn_parser *parser, const char *js, const size_t len, + jsmntok_t *tokens, const unsigned int num_tokens) { + int r; + int i; + jsmntok_t *token; + int count = parser->toknext; + + for (; parser->pos < len && js[parser->pos] != '\0'; parser->pos++) { + char c; + jsmntype_t type; + + c = js[parser->pos]; + switch (c) { + case '{': + case '[': + count++; + if (tokens == NULL) { + break; + } + token = jsmn_alloc_token(parser, tokens, num_tokens); + if (token == NULL) { + return JSMN_ERROR_NOMEM; + } + if (parser->toksuper != -1) { + jsmntok_t *t = &tokens[parser->toksuper]; +#ifdef JSMN_STRICT + /* In strict mode an object or array can't become a key */ + if (t->type == JSMN_OBJECT) { + return JSMN_ERROR_INVAL; + } +#endif + t->size++; +#ifdef JSMN_PARENT_LINKS + token->parent = parser->toksuper; +#endif + } + token->type = (c == '{' ? JSMN_OBJECT : JSMN_ARRAY); + token->start = parser->pos; + parser->toksuper = parser->toknext - 1; + break; + case '}': + case ']': + if (tokens == NULL) { + break; + } + type = (c == '}' ? JSMN_OBJECT : JSMN_ARRAY); +#ifdef JSMN_PARENT_LINKS + if (parser->toknext < 1) { + return JSMN_ERROR_INVAL; + } + token = &tokens[parser->toknext - 1]; + for (;;) { + if (token->start != -1 && token->end == -1) { + if (token->type != type) { + return JSMN_ERROR_INVAL; + } + token->end = parser->pos + 1; + parser->toksuper = token->parent; + break; + } + if (token->parent == -1) { + if (token->type != type || parser->toksuper == -1) { + return JSMN_ERROR_INVAL; + } + break; + } + token = &tokens[token->parent]; + } +#else + for (i = parser->toknext - 1; i >= 0; i--) { + token = &tokens[i]; + if (token->start != -1 && token->end == -1) { + if (token->type != type) { + return JSMN_ERROR_INVAL; + } + parser->toksuper = -1; + token->end = parser->pos + 1; + break; + } + } + /* Error if unmatched closing bracket */ + if (i == -1) { + return JSMN_ERROR_INVAL; + } + for (; i >= 0; i--) { + token = &tokens[i]; + if (token->start != -1 && token->end == -1) { + parser->toksuper = i; + break; + } + } +#endif + break; + case '\"': + r = jsmn_parse_string(parser, js, len, tokens, num_tokens); + if (r < 0) { + return r; + } + count++; + if (parser->toksuper != -1 && tokens != NULL) { + tokens[parser->toksuper].size++; + } + break; + case '\t': + case '\r': + case '\n': + case ' ': + break; + case ':': + parser->toksuper = parser->toknext - 1; + break; + case ',': + if (tokens != NULL && parser->toksuper != -1 && + tokens[parser->toksuper].type != JSMN_ARRAY && + tokens[parser->toksuper].type != JSMN_OBJECT) { +#ifdef JSMN_PARENT_LINKS + parser->toksuper = tokens[parser->toksuper].parent; +#else + for (i = parser->toknext - 1; i >= 0; i--) { + if (tokens[i].type == JSMN_ARRAY || tokens[i].type == JSMN_OBJECT) { + if (tokens[i].start != -1 && tokens[i].end == -1) { + parser->toksuper = i; + break; + } + } + } +#endif + } + break; +#ifdef JSMN_STRICT + /* In strict mode primitives are: numbers and booleans */ + case '-': + case '0': + case '1': + case '2': + case '3': + case '4': + case '5': + case '6': + case '7': + case '8': + case '9': + case 't': + case 'f': + case 'n': + /* And they must not be keys of the object */ + if (tokens != NULL && parser->toksuper != -1) { + const jsmntok_t *t = &tokens[parser->toksuper]; + if (t->type == JSMN_OBJECT || + (t->type == JSMN_STRING && t->size != 0)) { + return JSMN_ERROR_INVAL; + } + } +#else + /* In non-strict mode every unquoted value is a primitive */ + default: +#endif + r = jsmn_parse_primitive(parser, js, len, tokens, num_tokens); + if (r < 0) { + return r; + } + count++; + if (parser->toksuper != -1 && tokens != NULL) { + tokens[parser->toksuper].size++; + } + break; + +#ifdef JSMN_STRICT + /* Unexpected char in strict mode */ + default: + return JSMN_ERROR_INVAL; +#endif + } + } + + if (tokens != NULL) { + for (i = parser->toknext - 1; i >= 0; i--) { + /* Unmatched opened object or array */ + if (tokens[i].start != -1 && tokens[i].end == -1) { + return JSMN_ERROR_PART; + } + } + } + + return count; +} + +/** + * Creates a new parser based over a given buffer with an array of tokens + * available. + */ +JSMN_API void jsmn_init(jsmn_parser *parser) { + parser->pos = 0; + parser->toknext = 0; + parser->toksuper = -1; +} + +#endif /* JSMN_HEADER */ + +#ifdef __cplusplus +} +#endif + +#endif /* JSMN_H */ diff --git a/tests/cpp-tests/Source/Box2DTestBed/samples/sample.cpp b/tests/cpp-tests/Source/Box2DTestBed/samples/sample.cpp new file mode 100644 index 000000000000..dada65b29fd1 --- /dev/null +++ b/tests/cpp-tests/Source/Box2DTestBed/samples/sample.cpp @@ -0,0 +1,511 @@ +// SPDX-FileCopyrightText: 2023 Erin Catto +// SPDX-License-Identifier: MIT + +#include "sample.h" + +#include "draw.h" +#include "settings.h" + +#include "box2d/box2d.h" +#include "box2d/collision.h" +#include "box2d/math_functions.h" + +#include +#include +#include + +static void* EnqueueTask( b2TaskCallback* task, int32_t itemCount, int32_t minRange, void* taskContext, void* userContext ) +{ + Sample* sample = static_cast( userContext ); + if ( sample->m_taskCount < maxTasks ) + { + SampleTask& sampleTask = sample->m_tasks[sample->m_taskCount]; + sampleTask.m_SetSize = itemCount; + sampleTask.m_MinRange = minRange; + sampleTask.m_task = task; + sampleTask.m_taskContext = taskContext; + sample->m_scheduler.AddTaskSetToPipe( &sampleTask ); + ++sample->m_taskCount; + return &sampleTask; + } + else + { + // This is not fatal but the maxTasks should be increased + assert( false ); + task( 0, itemCount, 0, taskContext ); + return nullptr; + } +} + +static void FinishTask( void* taskPtr, void* userContext ) +{ + if ( taskPtr != nullptr ) + { + SampleTask* sampleTask = static_cast( taskPtr ); + Sample* sample = static_cast( userContext ); + sample->m_scheduler.WaitforTask( sampleTask ); + } +} + +static void TestMathCpp() +{ + b2Vec2 a = { 1.0f, 2.0f }; + b2Vec2 b = { 3.0f, 4.0f }; + + b2Vec2 c = a; + c += b; + c -= b; + c *= 2.0f; + c = -a; + c = c + b; + c = c - a; + c = 2.0f * a; + c = a * 2.0f; + + if ( b == a ) + { + c = a; + } + + if ( b != a ) + { + c = b; + } + + c += c; +} + +Sample::Sample( Settings& settings ) +{ + m_scheduler.Initialize( settings.workerCount ); + m_taskCount = 0; + + m_threadCount = 1 + settings.workerCount; + + b2WorldDef worldDef = b2DefaultWorldDef(); + worldDef.workerCount = settings.workerCount; + worldDef.enqueueTask = EnqueueTask; + worldDef.finishTask = FinishTask; + worldDef.userTaskContext = this; + worldDef.enableSleep = settings.enableSleep; + + m_worldId = b2CreateWorld( &worldDef ); + m_textLine = 30; + m_textIncrement = 18; + m_mouseJointId = b2_nullJointId; + + m_stepCount = 0; + + m_groundBodyId = b2_nullBodyId; + + m_maxProfile = {}; + m_totalProfile = {}; + + g_seed = RAND_SEED; + + TestMathCpp(); +} + +Sample::~Sample() +{ + // By deleting the world, we delete the bomb, mouse joint, etc. + b2DestroyWorld( m_worldId ); +} + +void Sample::DrawTitle( const char* string ) +{ + g_draw.DrawString( 5, 5, string ); + m_textLine = int( 26.0f ); +} + +struct QueryContext +{ + b2Vec2 point; + b2BodyId bodyId = b2_nullBodyId; +}; + +bool QueryCallback( b2ShapeId shapeId, void* context ) +{ + QueryContext* queryContext = static_cast( context ); + + b2BodyId bodyId = b2Shape_GetBody( shapeId ); + b2BodyType bodyType = b2Body_GetType( bodyId ); + if ( bodyType != b2_dynamicBody ) + { + // continue query + return true; + } + + bool overlap = b2Shape_TestPoint( shapeId, queryContext->point ); + if ( overlap ) + { + // found shape + queryContext->bodyId = bodyId; + return false; + } + + return true; +} + +void Sample::MouseDown( b2Vec2 p, int button, int mod ) +{ + if ( B2_IS_NON_NULL( m_mouseJointId ) ) + { + return; + } + + if ( button == GLFW_MOUSE_BUTTON_1 ) + { + // Make a small box. + b2AABB box; + b2Vec2 d = { 0.001f, 0.001f }; + box.lowerBound = b2Sub( p, d ); + box.upperBound = b2Add( p, d ); + + // Query the world for overlapping shapes. + QueryContext queryContext = { p, b2_nullBodyId }; + b2World_OverlapAABB( m_worldId, box, b2DefaultQueryFilter(), QueryCallback, &queryContext ); + + if ( B2_IS_NON_NULL( queryContext.bodyId ) ) + { + b2BodyDef bodyDef = b2DefaultBodyDef(); + m_groundBodyId = b2CreateBody( m_worldId, &bodyDef ); + + b2MouseJointDef mouseDef = b2DefaultMouseJointDef(); + mouseDef.bodyIdA = m_groundBodyId; + mouseDef.bodyIdB = queryContext.bodyId; + mouseDef.target = p; + mouseDef.hertz = 5.0f; + mouseDef.dampingRatio = 0.7f; + mouseDef.maxForce = 1000.0f * b2Body_GetMass( queryContext.bodyId ); + m_mouseJointId = b2CreateMouseJoint( m_worldId, &mouseDef ); + + b2Body_SetAwake( queryContext.bodyId, true ); + } + } +} + +void Sample::MouseUp( b2Vec2 p, int button ) +{ + if ( b2Joint_IsValid( m_mouseJointId ) == false ) + { + // The world or attached body was destroyed. + m_mouseJointId = b2_nullJointId; + } + + if ( B2_IS_NON_NULL( m_mouseJointId ) && button == GLFW_MOUSE_BUTTON_1 ) + { + b2DestroyJoint( m_mouseJointId ); + m_mouseJointId = b2_nullJointId; + + b2DestroyBody( m_groundBodyId ); + m_groundBodyId = b2_nullBodyId; + } +} + +void Sample::MouseMove( b2Vec2 p ) +{ + if ( b2Joint_IsValid( m_mouseJointId ) == false ) + { + // The world or attached body was destroyed. + m_mouseJointId = b2_nullJointId; + } + + if ( B2_IS_NON_NULL( m_mouseJointId ) ) + { + b2MouseJoint_SetTarget( m_mouseJointId, p ); + b2BodyId bodyIdB = b2Joint_GetBodyB( m_mouseJointId ); + b2Body_SetAwake( bodyIdB, true ); + } +} + +void Sample::ResetProfile() +{ + m_totalProfile = {}; + m_maxProfile = {}; + m_stepCount = 0; +} + +void Sample::Step( Settings& settings ) +{ + float timeStep = settings.hertz > 0.0f ? 1.0f / settings.hertz : 0.0f; + + if ( settings.pause ) + { + if ( settings.singleStep ) + { + settings.singleStep = false; + } + else + { + timeStep = 0.0f; + } + + g_draw.DrawString( 5, m_textLine, "****PAUSED****" ); + m_textLine += m_textIncrement; + } + + g_draw.m_debugDraw.drawingBounds = g_camera.GetViewBounds(); + g_draw.m_debugDraw.useDrawingBounds = settings.useCameraBounds; + + // todo testing + // b2Transform t1 = {g_draw.m_debugDraw.drawingBounds.lowerBound, b2Rot_identity}; + // b2Transform t2 = {g_draw.m_debugDraw.drawingBounds.upperBound, b2Rot_identity}; + // g_draw.DrawSolidCircle(t1, b2Vec2_zero, 1.0f, {1.0f, 0.0f, 0.0f, 1.0f}); + // g_draw.DrawSolidCircle(t2, b2Vec2_zero, 1.0f, {1.0f, 0.0f, 0.0f, 1.0f}); + + g_draw.m_debugDraw.drawShapes = settings.drawShapes; + g_draw.m_debugDraw.drawJoints = settings.drawJoints; + g_draw.m_debugDraw.drawJointExtras = settings.drawJointExtras; + g_draw.m_debugDraw.drawAABBs = settings.drawAABBs; + g_draw.m_debugDraw.drawMass = settings.drawMass; + g_draw.m_debugDraw.drawContacts = settings.drawContactPoints; + g_draw.m_debugDraw.drawGraphColors = settings.drawGraphColors; + g_draw.m_debugDraw.drawContactNormals = settings.drawContactNormals; + g_draw.m_debugDraw.drawContactImpulses = settings.drawContactImpulses; + g_draw.m_debugDraw.drawFrictionImpulses = settings.drawFrictionImpulses; + + b2World_EnableSleeping( m_worldId, settings.enableSleep ); + b2World_EnableWarmStarting( m_worldId, settings.enableWarmStarting ); + b2World_EnableContinuous( m_worldId, settings.enableContinuous ); + + for ( int i = 0; i < 1; ++i ) + { + b2World_Step( m_worldId, timeStep, settings.subStepCount ); + m_taskCount = 0; + } + + b2World_Draw( m_worldId, &g_draw.m_debugDraw ); + + if ( timeStep > 0.0f ) + { + ++m_stepCount; + } + + if ( settings.drawCounters ) + { + b2Counters s = b2World_GetCounters( m_worldId ); + + g_draw.DrawString( 5, m_textLine, "bodies/shapes/contacts/joints = %d/%d/%d/%d", s.bodyCount, s.shapeCount, + s.contactCount, s.jointCount ); + m_textLine += m_textIncrement; + + g_draw.DrawString( 5, m_textLine, "islands/tasks = %d/%d", s.islandCount, s.taskCount ); + m_textLine += m_textIncrement; + + g_draw.DrawString( 5, m_textLine, "tree height static/movable = %d/%d", s.staticTreeHeight, s.treeHeight ); + m_textLine += m_textIncrement; + + int totalCount = 0; + char buffer[256] = { 0 }; + static_assert( std::size( s.colorCounts ) == 12 ); + + int offset = snprintf( buffer, 256, "colors: " ); + for ( int i = 0; i < 12; ++i ) + { + offset += snprintf( buffer + offset, 256 - offset, "%d/", s.colorCounts[i] ); + totalCount += s.colorCounts[i]; + } + snprintf( buffer + offset, 256 - offset, "[%d]", totalCount ); + g_draw.DrawString( 5, m_textLine, buffer ); + m_textLine += m_textIncrement; + + g_draw.DrawString( 5, m_textLine, "stack allocator size = %d K", s.stackUsed / 1024 ); + m_textLine += m_textIncrement; + + g_draw.DrawString( 5, m_textLine, "total allocation = %d K", s.byteCount / 1024 ); + m_textLine += m_textIncrement; + } + + // Track maximum profile times + { + b2Profile p = b2World_GetProfile( m_worldId ); + m_maxProfile.step = b2MaxFloat( m_maxProfile.step, p.step ); + m_maxProfile.pairs = b2MaxFloat( m_maxProfile.pairs, p.pairs ); + m_maxProfile.collide = b2MaxFloat( m_maxProfile.collide, p.collide ); + m_maxProfile.solve = b2MaxFloat( m_maxProfile.solve, p.solve ); + m_maxProfile.buildIslands = b2MaxFloat( m_maxProfile.buildIslands, p.buildIslands ); + m_maxProfile.solveConstraints = b2MaxFloat( m_maxProfile.solveConstraints, p.solveConstraints ); + m_maxProfile.prepareTasks = b2MaxFloat( m_maxProfile.prepareTasks, p.prepareTasks ); + m_maxProfile.solverTasks = b2MaxFloat( m_maxProfile.solverTasks, p.solverTasks ); + m_maxProfile.prepareConstraints = b2MaxFloat( m_maxProfile.prepareConstraints, p.prepareConstraints ); + m_maxProfile.integrateVelocities = b2MaxFloat( m_maxProfile.integrateVelocities, p.integrateVelocities ); + m_maxProfile.warmStart = b2MaxFloat( m_maxProfile.warmStart, p.warmStart ); + m_maxProfile.solveVelocities = b2MaxFloat( m_maxProfile.solveVelocities, p.solveVelocities ); + m_maxProfile.integratePositions = b2MaxFloat( m_maxProfile.integratePositions, p.integratePositions ); + m_maxProfile.relaxVelocities = b2MaxFloat( m_maxProfile.relaxVelocities, p.relaxVelocities ); + m_maxProfile.applyRestitution = b2MaxFloat( m_maxProfile.applyRestitution, p.applyRestitution ); + m_maxProfile.storeImpulses = b2MaxFloat( m_maxProfile.storeImpulses, p.storeImpulses ); + m_maxProfile.finalizeBodies = b2MaxFloat( m_maxProfile.finalizeBodies, p.finalizeBodies ); + m_maxProfile.sleepIslands = b2MaxFloat( m_maxProfile.sleepIslands, p.sleepIslands ); + m_maxProfile.splitIslands = b2MaxFloat( m_maxProfile.splitIslands, p.splitIslands ); + m_maxProfile.hitEvents = b2MaxFloat( m_maxProfile.hitEvents, p.hitEvents ); + m_maxProfile.broadphase = b2MaxFloat( m_maxProfile.broadphase, p.broadphase ); + m_maxProfile.continuous = b2MaxFloat( m_maxProfile.continuous, p.continuous ); + + m_totalProfile.step += p.step; + m_totalProfile.pairs += p.pairs; + m_totalProfile.collide += p.collide; + m_totalProfile.solve += p.solve; + m_totalProfile.buildIslands += p.buildIslands; + m_totalProfile.solveConstraints += p.solveConstraints; + m_totalProfile.prepareTasks += p.prepareTasks; + m_totalProfile.solverTasks += p.solverTasks; + m_totalProfile.prepareConstraints += p.prepareConstraints; + m_totalProfile.integrateVelocities += p.integrateVelocities; + m_totalProfile.warmStart += p.warmStart; + m_totalProfile.solveVelocities += p.solveVelocities; + m_totalProfile.integratePositions += p.integratePositions; + m_totalProfile.relaxVelocities += p.relaxVelocities; + m_totalProfile.applyRestitution += p.applyRestitution; + m_totalProfile.storeImpulses += p.storeImpulses; + m_totalProfile.finalizeBodies += p.finalizeBodies; + m_totalProfile.sleepIslands += p.sleepIslands; + m_totalProfile.splitIslands += p.splitIslands; + m_totalProfile.hitEvents += p.hitEvents; + m_totalProfile.broadphase += p.broadphase; + m_totalProfile.continuous += p.continuous; + } + + if ( settings.drawProfile ) + { + b2Profile p = b2World_GetProfile( m_worldId ); + + b2Profile aveProfile; + memset( &aveProfile, 0, sizeof( b2Profile ) ); + if ( m_stepCount > 0 ) + { + float scale = 1.0f / m_stepCount; + aveProfile.step = scale * m_totalProfile.step; + aveProfile.pairs = scale * m_totalProfile.pairs; + aveProfile.collide = scale * m_totalProfile.collide; + aveProfile.solve = scale * m_totalProfile.solve; + aveProfile.buildIslands = scale * m_totalProfile.buildIslands; + aveProfile.solveConstraints = scale * m_totalProfile.solveConstraints; + aveProfile.prepareTasks = scale * m_totalProfile.prepareTasks; + aveProfile.solverTasks = scale * m_totalProfile.solverTasks; + aveProfile.prepareConstraints = scale * m_totalProfile.prepareConstraints; + aveProfile.integrateVelocities = scale * m_totalProfile.integrateVelocities; + aveProfile.warmStart = scale * m_totalProfile.warmStart; + aveProfile.solveVelocities = scale * m_totalProfile.solveVelocities; + aveProfile.integratePositions = scale * m_totalProfile.integratePositions; + aveProfile.relaxVelocities = scale * m_totalProfile.relaxVelocities; + aveProfile.applyRestitution = scale * m_totalProfile.applyRestitution; + aveProfile.storeImpulses = scale * m_totalProfile.storeImpulses; + aveProfile.finalizeBodies = scale * m_totalProfile.finalizeBodies; + aveProfile.sleepIslands = scale * m_totalProfile.sleepIslands; + aveProfile.splitIslands = scale * m_totalProfile.splitIslands; + aveProfile.hitEvents = scale * m_totalProfile.hitEvents; + aveProfile.broadphase = scale * m_totalProfile.broadphase; + aveProfile.continuous = scale * m_totalProfile.continuous; + } + + g_draw.DrawString( 5, m_textLine, "step [ave] (max) = %5.2f [%6.2f] (%6.2f)", p.step, aveProfile.step, + m_maxProfile.step ); + m_textLine += m_textIncrement; + g_draw.DrawString( 5, m_textLine, "pairs [ave] (max) = %5.2f [%6.2f] (%6.2f)", p.pairs, aveProfile.pairs, + m_maxProfile.pairs ); + m_textLine += m_textIncrement; + g_draw.DrawString( 5, m_textLine, "collide [ave] (max) = %5.2f [%6.2f] (%6.2f)", p.collide, aveProfile.collide, + m_maxProfile.collide ); + m_textLine += m_textIncrement; + g_draw.DrawString( 5, m_textLine, "solve [ave] (max) = %5.2f [%6.2f] (%6.2f)", p.solve, aveProfile.solve, + m_maxProfile.solve ); + m_textLine += m_textIncrement; + g_draw.DrawString( 5, m_textLine, "builds island [ave] (max) = %5.2f [%6.2f] (%6.2f)", p.buildIslands, + aveProfile.buildIslands, m_maxProfile.buildIslands ); + m_textLine += m_textIncrement; + g_draw.DrawString( 5, m_textLine, "solve constraints [ave] (max) = %5.2f [%6.2f] (%6.2f)", p.solveConstraints, + aveProfile.solveConstraints, m_maxProfile.solveConstraints ); + m_textLine += m_textIncrement; + g_draw.DrawString( 5, m_textLine, "prepare tasks [ave] (max) = %5.2f [%6.2f] (%6.2f)", p.prepareTasks, + aveProfile.prepareTasks, m_maxProfile.prepareTasks ); + m_textLine += m_textIncrement; + g_draw.DrawString( 5, m_textLine, "solver tasks [ave] (max) = %5.2f [%6.2f] (%6.2f)", p.solverTasks, + aveProfile.solverTasks, m_maxProfile.solverTasks ); + m_textLine += m_textIncrement; + g_draw.DrawString( 5, m_textLine, "prepare constraints [ave] (max) = %5.2f [%6.2f] (%6.2f)", p.prepareConstraints, + aveProfile.prepareConstraints, m_maxProfile.prepareConstraints ); + m_textLine += m_textIncrement; + g_draw.DrawString( 5, m_textLine, "integrate velocities [ave] (max) = %5.2f [%6.2f] (%6.2f)", p.integrateVelocities, + aveProfile.integrateVelocities, m_maxProfile.integrateVelocities ); + m_textLine += m_textIncrement; + g_draw.DrawString( 5, m_textLine, "warm start [ave] (max) = %5.2f [%6.2f] (%6.2f)", p.warmStart, aveProfile.warmStart, + m_maxProfile.warmStart ); + m_textLine += m_textIncrement; + g_draw.DrawString( 5, m_textLine, "solve velocities [ave] (max) = %5.2f [%6.2f] (%6.2f)", p.solveVelocities, + aveProfile.solveVelocities, m_maxProfile.solveVelocities ); + m_textLine += m_textIncrement; + g_draw.DrawString( 5, m_textLine, "integrate positions [ave] (max) = %5.2f [%6.2f] (%6.2f)", p.integratePositions, + aveProfile.integratePositions, m_maxProfile.integratePositions ); + m_textLine += m_textIncrement; + g_draw.DrawString( 5, m_textLine, "relax velocities [ave] (max) = %5.2f [%6.2f] (%6.2f)", p.relaxVelocities, + aveProfile.relaxVelocities, m_maxProfile.relaxVelocities ); + m_textLine += m_textIncrement; + g_draw.DrawString( 5, m_textLine, "apply restitution [ave] (max) = %5.2f [%6.2f] (%6.2f)", p.applyRestitution, + aveProfile.applyRestitution, m_maxProfile.applyRestitution ); + m_textLine += m_textIncrement; + g_draw.DrawString( 5, m_textLine, "store impulses [ave] (max) = %5.2f [%6.2f] (%6.2f)", p.storeImpulses, + aveProfile.storeImpulses, m_maxProfile.storeImpulses ); + m_textLine += m_textIncrement; + g_draw.DrawString( 5, m_textLine, "finalize bodies [ave] (max) = %5.2f [%6.2f] (%6.2f)", p.finalizeBodies, + aveProfile.finalizeBodies, m_maxProfile.finalizeBodies ); + m_textLine += m_textIncrement; + g_draw.DrawString( 5, m_textLine, "sleep islands [ave] (max) = %5.2f [%6.2f] (%6.2f)", p.sleepIslands, + aveProfile.sleepIslands, m_maxProfile.sleepIslands ); + m_textLine += m_textIncrement; + g_draw.DrawString( 5, m_textLine, "split islands [ave] (max) = %5.2f [%6.2f] (%6.2f)", p.splitIslands, + aveProfile.splitIslands, m_maxProfile.splitIslands ); + m_textLine += m_textIncrement; + g_draw.DrawString( 5, m_textLine, "hit events [ave] (max) = %5.2f [%6.2f] (%6.2f)", p.hitEvents, aveProfile.hitEvents, + m_maxProfile.hitEvents ); + m_textLine += m_textIncrement; + g_draw.DrawString( 5, m_textLine, "broad-phase [ave] (max) = %5.2f [%6.2f] (%6.2f)", p.broadphase, aveProfile.broadphase, + m_maxProfile.broadphase ); + m_textLine += m_textIncrement; + g_draw.DrawString( 5, m_textLine, "continuous collision [ave] (max) = %5.2f [%6.2f] (%6.2f)", p.continuous, + aveProfile.continuous, m_maxProfile.continuous ); + m_textLine += m_textIncrement; + } +} + +void Sample::ShiftOrigin( b2Vec2 newOrigin ) +{ + // m_world->ShiftOrigin(newOrigin); +} + +SampleEntry g_sampleEntries[MAX_SAMPLES] = {}; +int g_sampleCount = 0; + +int RegisterSample( const char* category, const char* name, SampleCreateFcn* fcn ) +{ + int index = g_sampleCount; + if ( index < MAX_SAMPLES ) + { + g_sampleEntries[index] = { category, name, fcn }; + ++g_sampleCount; + return index; + } + + return -1; +} + +uint32_t g_seed = RAND_SEED; + +b2Polygon RandomPolygon( float extent ) +{ + b2Vec2 points[b2_maxPolygonVertices]; + int count = 3 + RandomInt() % 6; + for ( int i = 0; i < count; ++i ) + { + points[i] = RandomVec2( -extent, extent ); + } + + b2Hull hull = b2ComputeHull( points, count ); + if ( hull.count > 0 ) + { + return b2MakePolygon( &hull, 0.0f ); + } + + return b2MakeSquare( extent ); +} diff --git a/tests/cpp-tests/Source/Box2DTestBed/samples/sample.h b/tests/cpp-tests/Source/Box2DTestBed/samples/sample.h new file mode 100644 index 000000000000..f697213ee879 --- /dev/null +++ b/tests/cpp-tests/Source/Box2DTestBed/samples/sample.h @@ -0,0 +1,169 @@ +// SPDX-FileCopyrightText: 2023 Erin Catto +// SPDX-License-Identifier: MIT + +#pragma once + +#include "box2d/collision.h" +#include "box2d/id.h" +#include "box2d/types.h" +#include "../Box2DTestDebugDrawNode.h" + +// todo this include is slow +#include "TaskScheduler.h" + +#define ARRAY_COUNT( A ) (int)( sizeof( A ) / sizeof( A[0] ) ) + +struct Settings; + +#ifdef NDEBUG +constexpr bool g_sampleDebug = false; +#else +constexpr bool g_sampleDebug = true; +#endif + +constexpr int32_t k_maxContactPoints = 12 * 2048; + +struct ContactPoint +{ + b2ShapeId shapeIdA; + b2ShapeId shapeIdB; + b2Vec2 normal; + b2Vec2 position; + bool persisted; + float normalImpulse; + float tangentImpulse; + float separation; + int32_t constraintIndex; + int32_t color; +}; + +class SampleTask : public enki::ITaskSet +{ +public: + SampleTask() = default; + + void ExecuteRange( enki::TaskSetPartition range, uint32_t threadIndex ) override + { + m_task( range.start, range.end, threadIndex, m_taskContext ); + } + + b2TaskCallback* m_task = nullptr; + void* m_taskContext = nullptr; +}; + +constexpr int32_t maxTasks = 64; +constexpr int32_t maxThreads = 64; + +class Sample +{ +public: + explicit Sample( Settings& settings ); + virtual ~Sample(); + + void DrawTitle( const char* string ); + virtual void Step( Settings& settings ); + virtual void UpdateUI() + { + } + virtual void Keyboard( int ) + { + } + virtual void MouseDown( b2Vec2 p, int button, int mod ); + virtual void MouseUp( b2Vec2 p, int button ); + virtual void MouseMove( b2Vec2 p ); + + void ResetProfile(); + void ShiftOrigin( b2Vec2 newOrigin ); + + friend class DestructionListener; + friend class BoundaryListener; + friend class ContactListener; + + enki::TaskScheduler m_scheduler; + SampleTask m_tasks[maxTasks]; + int32_t m_taskCount; + int m_threadCount; + + b2BodyId m_groundBodyId; + + // DestructionListener m_destructionListener; + int32_t m_textLine; + b2WorldId m_worldId; + b2JointId m_mouseJointId; + int32_t m_stepCount; + int32_t m_textIncrement; + b2Profile m_maxProfile; + b2Profile m_totalProfile; +}; + +typedef Sample* SampleCreateFcn( Settings& settings ); + +int RegisterSample( const char* category, const char* name, SampleCreateFcn* fcn ); + +struct SampleEntry +{ + const char* category; + const char* name; + SampleCreateFcn* createFcn; +}; + +#define MAX_SAMPLES 256 +extern SampleEntry g_sampleEntries[MAX_SAMPLES]; +extern int g_sampleCount; + +#define RAND_LIMIT 32767 +#define RAND_SEED 12345 + +// Global seed for simple random number generator. This is reset +// for each sample. +extern uint32_t g_seed; + +// Simple random number generator. Using this instead of rand() +// for cross platform determinism. +inline int RandomInt() +{ + // XorShift32 algorithm + uint32_t x = g_seed; + x ^= x << 13; + x ^= x >> 17; + x ^= x << 5; + g_seed = x; + + // Map the 32-bit value to the range 0 to RAND_LIMIT + return (int)( x % ( RAND_LIMIT + 1 ) ); +} + +// Random integer in range [lo, hi] +inline float RandomInt( int lo, int hi ) +{ + return lo + RandomInt() % ( hi - lo + 1 ); +} + +// Random number in range [-1,1] +inline float RandomFloat() +{ + float r = (float)( RandomInt() & ( RAND_LIMIT ) ); + r /= RAND_LIMIT; + r = 2.0f * r - 1.0f; + return r; +} + +// Random floating point number in range [lo, hi] +inline float RandomFloat( float lo, float hi ) +{ + float r = (float)( RandomInt() & ( RAND_LIMIT ) ); + r /= RAND_LIMIT; + r = ( hi - lo ) * r + lo; + return r; +} + +// Random vector with coordinates in range [lo, hi] +inline b2Vec2 RandomVec2( float lo, float hi ) +{ + b2Vec2 v; + v.x = RandomFloat( lo, hi ); + v.y = RandomFloat( lo, hi ); + return v; +} + +b2Polygon RandomPolygon( float extent ); diff --git a/tests/cpp-tests/Source/Box2DTestBed/samples/sample_benchmark.cpp b/tests/cpp-tests/Source/Box2DTestBed/samples/sample_benchmark.cpp new file mode 100644 index 000000000000..d761803b3937 --- /dev/null +++ b/tests/cpp-tests/Source/Box2DTestBed/samples/sample_benchmark.cpp @@ -0,0 +1,1531 @@ +// SPDX-FileCopyrightText: 2022 Erin Catto +// SPDX-License-Identifier: MIT + +#include "draw.h" +#include "human.h" +#include "sample.h" +#include "settings.h" + +#include "box2d/box2d.h" +#include "box2d/math_functions.h" + +#include +#include + +// Note: resetting the scene is non-deterministic because the world uses freelists +class BenchmarkBarrel : public Sample +{ +public: + enum ShapeType + { + e_circleShape = 0, + e_caspuleShape, + e_mixShape, + e_compoundShape, + e_humanShape, + }; + + enum + { + e_maxColumns = 26, + e_maxRows = 130, + }; + + explicit BenchmarkBarrel( Settings& settings ) + : Sample( settings ) + { + if ( settings.restart == false ) + { + g_camera.m_center = { 8.0f, 53.0f }; + g_camera.m_zoom = 25.0f * 2.35f; + } + + settings.drawJoints = false; + + float groundSize = 25.0f; + + { + b2BodyDef bodyDef = b2DefaultBodyDef(); + b2BodyId groundId = b2CreateBody( m_worldId, &bodyDef ); + + b2Polygon box = b2MakeBox( groundSize, 1.2f ); + b2ShapeDef shapeDef = b2DefaultShapeDef(); + b2CreatePolygonShape( groundId, &shapeDef, &box ); + + box = b2MakeOffsetBox( 1.2f, 2.0f * groundSize, { -groundSize, 2.0f * groundSize }, b2Rot_identity ); + b2CreatePolygonShape( groundId, &shapeDef, &box ); + + box = b2MakeOffsetBox( 1.2f, 2.0f * groundSize, { groundSize, 2.0f * groundSize }, b2Rot_identity ); + b2CreatePolygonShape( groundId, &shapeDef, &box ); + + box = b2MakeOffsetBox( 800.0f, 10.0f, { 0.0f, -80.0f }, b2Rot_identity ); + b2CreatePolygonShape( groundId, &shapeDef, &box ); + } + + for ( int i = 0; i < e_maxRows * e_maxColumns; ++i ) + { + m_bodies[i] = b2_nullBodyId; + } + + m_shapeType = e_circleShape; + + CreateScene(); + } + + void CreateScene() + { + g_seed = 42; + + for ( int i = 0; i < e_maxRows * e_maxColumns; ++i ) + { + if ( B2_IS_NON_NULL( m_bodies[i] ) ) + { + b2DestroyBody( m_bodies[i] ); + m_bodies[i] = b2_nullBodyId; + } + + if ( m_humans[i].m_isSpawned ) + { + m_humans[i].Despawn(); + } + } + + m_columnCount = g_sampleDebug ? 10 : e_maxColumns; + m_rowCount = g_sampleDebug ? 40 : e_maxRows; + + if ( m_shapeType == e_compoundShape ) + { + if constexpr ( g_sampleDebug == false ) + { + m_columnCount = 20; + } + } + else if ( m_shapeType == e_humanShape ) + { + if constexpr ( g_sampleDebug ) + { + m_rowCount = 5; + m_columnCount = 10; + } + else + { + m_columnCount = 15; + m_rowCount = 50; + } + } + + float rad = 0.5f; + + float shift = 1.15f; + float centerx = shift * m_columnCount / 2.0f; + float centery = shift / 2.0f; + + b2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.type = b2_dynamicBody; + + b2ShapeDef shapeDef = b2DefaultShapeDef(); + shapeDef.density = 1.0f; + shapeDef.friction = 0.5f; + + b2Capsule capsule = { { 0.0f, -0.25f }, { 0.0f, 0.25f }, rad }; + b2Circle circle = { { 0.0f, 0.0f }, rad }; + + b2Vec2 points[3] = { { -0.1f, -0.5f }, { 0.1f, -0.5f }, { 0.0f, 0.5f } }; + b2Hull wedgeHull = b2ComputeHull( points, 3 ); + b2Polygon wedge = b2MakePolygon( &wedgeHull, 0.0f ); + + b2Vec2 vertices[3]; + vertices[0] = { -1.0f, 0.0f }; + vertices[1] = { 0.5f, 1.0f }; + vertices[2] = { 0.0f, 2.0f }; + b2Hull hull = b2ComputeHull( vertices, 3 ); + b2Polygon left = b2MakePolygon( &hull, 0.0f ); + + vertices[0] = { 1.0f, 0.0f }; + vertices[1] = { -0.5f, 1.0f }; + vertices[2] = { 0.0f, 2.0f }; + hull = b2ComputeHull( vertices, 3 ); + b2Polygon right = b2MakePolygon( &hull, 0.0f ); + + // b2Polygon top = b2MakeOffsetBox(0.8f, 0.2f, {0.0f, 0.8f}, 0.0f); + // b2Polygon leftLeg = b2MakeOffsetBox(0.2f, 0.5f, {-0.6f, 0.5f}, 0.0f); + // b2Polygon rightLeg = b2MakeOffsetBox(0.2f, 0.5f, {0.6f, 0.5f}, 0.0f); + + float side = -0.1f; + float extray = 0.5f; + + if ( m_shapeType == e_compoundShape ) + { + extray = 0.25f; + side = 0.25f; + shift = 2.0f; + centerx = shift * m_columnCount / 2.0f - 1.0f; + } + else if ( m_shapeType == e_humanShape ) + { + extray = 0.5f; + side = 0.55f; + shift = 2.5f; + centerx = shift * m_columnCount / 2.0f; + } + + int index = 0; + + for ( int i = 0; i < m_columnCount; ++i ) + { + float x = i * shift - centerx; + + for ( int j = 0; j < m_rowCount; ++j ) + { + float y = j * ( shift + extray ) + centery + 2.0f; + + bodyDef.position = { x + side, y }; + side = -side; + + if ( m_shapeType == e_circleShape ) + { + m_bodies[index] = b2CreateBody( m_worldId, &bodyDef ); + circle.radius = RandomFloat( 0.25f, 0.75f ); + b2CreateCircleShape( m_bodies[index], &shapeDef, &circle ); + } + else if ( m_shapeType == e_caspuleShape ) + { + m_bodies[index] = b2CreateBody( m_worldId, &bodyDef ); + capsule.radius = RandomFloat( 0.25f, 0.5f ); + float length = RandomFloat( 0.25f, 1.0f ); + capsule.center1 = { 0.0f, -0.5f * length }; + capsule.center2 = { 0.0f, 0.5f * length }; + b2CreateCapsuleShape( m_bodies[index], &shapeDef, &capsule ); + } + else if ( m_shapeType == e_mixShape ) + { + m_bodies[index] = b2CreateBody( m_worldId, &bodyDef ); + + int mod = index % 3; + if ( mod == 0 ) + { + circle.radius = RandomFloat( 0.25f, 0.75f ); + b2CreateCircleShape( m_bodies[index], &shapeDef, &circle ); + } + else if ( mod == 1 ) + { + capsule.radius = RandomFloat( 0.25f, 0.5f ); + float length = RandomFloat( 0.25f, 1.0f ); + capsule.center1 = { 0.0f, -0.5f * length }; + capsule.center2 = { 0.0f, 0.5f * length }; + b2CreateCapsuleShape( m_bodies[index], &shapeDef, &capsule ); + } + else if ( mod == 2 ) + { + float width = RandomFloat( 0.1f, 0.5f ); + float height = RandomFloat( 0.5f, 0.75f ); + b2Polygon box = b2MakeBox( width, height ); + + // Don't put a function call into a macro. + float value = RandomFloat( -1.0f, 1.0f ); + box.radius = 0.25f * b2MaxFloat( 0.0f, value ); + b2CreatePolygonShape( m_bodies[index], &shapeDef, &box ); + } + else + { + wedge.radius = RandomFloat( 0.1f, 0.25f ); + b2CreatePolygonShape( m_bodies[index], &shapeDef, &wedge ); + } + } + else if ( m_shapeType == e_compoundShape ) + { + m_bodies[index] = b2CreateBody( m_worldId, &bodyDef ); + + b2CreatePolygonShape( m_bodies[index], &shapeDef, &left ); + b2CreatePolygonShape( m_bodies[index], &shapeDef, &right ); + // b2CreatePolygonShape(m_bodies[index], &shapeDef, &top); + // b2CreatePolygonShape(m_bodies[index], &shapeDef, &leftLeg); + // b2CreatePolygonShape(m_bodies[index], &shapeDef, &rightLeg); + } + else if ( m_shapeType == e_humanShape ) + { + m_humans[index].Spawn( m_worldId, bodyDef.position, 3.5f, 0.05f, 0.0f, 0.0f, index + 1, nullptr, false ); + } + + index += 1; + } + } + } + + void UpdateUI() override + { + float height = 80.0f; + ImGui::SetNextWindowPos( ImVec2( 10.0f, g_camera.m_height - height - 50.0f ), ImGuiCond_Once ); + ImGui::SetNextWindowSize( ImVec2( 220.0f, height ) ); + ImGui::Begin( "Benchmark: Barrel", nullptr, ImGuiWindowFlags_NoResize ); + + bool changed = false; + const char* shapeTypes[] = { "Circle", "Capsule", "Mix", "Compound", "Human" }; + + int shapeType = int( m_shapeType ); + changed = changed || ImGui::Combo( "Shape", &shapeType, shapeTypes, IM_ARRAYSIZE( shapeTypes ) ); + m_shapeType = ShapeType( shapeType ); + + changed = changed || ImGui::Button( "Reset Scene" ); + + if ( changed ) + { + CreateScene(); + } + + ImGui::End(); + } + + static Sample* Create( Settings& settings ) + { + return new BenchmarkBarrel( settings ); + } + + b2BodyId m_bodies[e_maxRows * e_maxColumns]; + Human m_humans[e_maxRows * e_maxColumns]; + int m_columnCount; + int m_rowCount; + + ShapeType m_shapeType; +}; + +static int benchmarkBarrel = RegisterSample( "Benchmark", "Barrel", BenchmarkBarrel::Create ); + +class BenchmarkTumbler : public Sample +{ +public: + explicit BenchmarkTumbler( Settings& settings ) + : Sample( settings ) + { + if ( settings.restart == false ) + { + g_camera.m_center = { 1.5f, 10.0f }; + g_camera.m_zoom = 25.0f * 0.6f; + } + + b2BodyId groundId; + { + b2BodyDef bodyDef = b2DefaultBodyDef(); + groundId = b2CreateBody( m_worldId, &bodyDef ); + } + + { + b2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.type = b2_dynamicBody; + bodyDef.enableSleep = true; + bodyDef.position = { 0.0f, 10.0f }; + b2BodyId bodyId = b2CreateBody( m_worldId, &bodyDef ); + + b2ShapeDef shapeDef = b2DefaultShapeDef(); + shapeDef.density = 50.0f; + + b2Polygon polygon; + polygon = b2MakeOffsetBox( 0.5f, 10.0f, { 10.0f, 0.0f }, b2Rot_identity ); + b2CreatePolygonShape( bodyId, &shapeDef, &polygon ); + polygon = b2MakeOffsetBox( 0.5f, 10.0f, { -10.0f, 0.0f }, b2Rot_identity ); + b2CreatePolygonShape( bodyId, &shapeDef, &polygon ); + polygon = b2MakeOffsetBox( 10.0f, 0.5f, { 0.0f, 10.0f }, b2Rot_identity ); + b2CreatePolygonShape( bodyId, &shapeDef, &polygon ); + polygon = b2MakeOffsetBox( 10.0f, 0.5f, { 0.0f, -10.0f }, b2Rot_identity ); + b2CreatePolygonShape( bodyId, &shapeDef, &polygon ); + + // m_motorSpeed = 9.0f; + m_motorSpeed = 25.0f; + + b2RevoluteJointDef jd = b2DefaultRevoluteJointDef(); + jd.bodyIdA = groundId; + jd.bodyIdB = bodyId; + jd.localAnchorA = { 0.0f, 10.0f }; + jd.localAnchorB = { 0.0f, 0.0f }; + jd.referenceAngle = 0.0f; + jd.motorSpeed = ( b2_pi / 180.0f ) * m_motorSpeed; + jd.maxMotorTorque = 1e8f; + jd.enableMotor = true; + + m_jointId = b2CreateRevoluteJoint( m_worldId, &jd ); + } + + int gridCount = g_sampleDebug ? 20 : 45; + b2Polygon polygon = b2MakeBox( 0.125f, 0.125f ); + b2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.type = b2_dynamicBody; + b2ShapeDef shapeDef = b2DefaultShapeDef(); + + float y = -0.2f * gridCount + 10.0f; + for ( int i = 0; i < gridCount; ++i ) + { + float x = -0.2f * gridCount; + + for ( int j = 0; j < gridCount; ++j ) + { + bodyDef.position = { x, y }; + b2BodyId bodyId = b2CreateBody( m_worldId, &bodyDef ); + + b2CreatePolygonShape( bodyId, &shapeDef, &polygon ); + + x += 0.4f; + } + + y += 0.4f; + } + } + + void UpdateUI() override + { + float height = 60.0f; + ImGui::SetNextWindowPos( ImVec2( 10.0f, g_camera.m_height - height - 50.0f ), ImGuiCond_Once ); + ImGui::SetNextWindowSize( ImVec2( 200.0f, height ) ); + ImGui::Begin( "Benchmark: Tumbler", nullptr, ImGuiWindowFlags_NoResize ); + ImGui::PushItemWidth( 120.0f ); + + if ( ImGui::SliderFloat( "Speed", &m_motorSpeed, 0.0f, 100.0f, "%.f" ) ) + { + b2RevoluteJoint_SetMotorSpeed( m_jointId, ( b2_pi / 180.0f ) * m_motorSpeed ); + + if ( m_motorSpeed > 0.0f ) + { + b2Joint_WakeBodies( m_jointId ); + } + } + + ImGui::PopItemWidth(); + ImGui::End(); + } + + static Sample* Create( Settings& settings ) + { + return new BenchmarkTumbler( settings ); + } + + b2JointId m_jointId; + float m_motorSpeed; +}; + +static int benchmarkTumbler = RegisterSample( "Benchmark", "Tumbler", BenchmarkTumbler::Create ); + +// todo try removing kinematics from graph coloring +class BenchmarkManyTumblers : public Sample +{ +public: + explicit BenchmarkManyTumblers( Settings& settings ) + : Sample( settings ) + { + if ( settings.restart == false ) + { + g_camera.m_center = { 1.0f, -5.5 }; + g_camera.m_zoom = 25.0f * 3.4f; + settings.drawJoints = false; + } + + b2BodyDef bodyDef = b2DefaultBodyDef(); + m_groundId = b2CreateBody( m_worldId, &bodyDef ); + + m_rowCount = g_sampleDebug ? 2 : 19; + m_columnCount = g_sampleDebug ? 2 : 19; + + m_tumblerIds = nullptr; + m_positions = nullptr; + m_tumblerCount = 0; + + m_bodyIds = nullptr; + m_bodyCount = 0; + m_bodyIndex = 0; + + m_angularSpeed = 25.0f; + + CreateScene(); + } + + ~BenchmarkManyTumblers() override + { + free( m_tumblerIds ); + free( m_positions ); + free( m_bodyIds ); + } + + void CreateTumbler( b2Vec2 position, int index ) + { + b2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.type = b2_kinematicBody; + bodyDef.position = { position.x, position.y }; + bodyDef.angularVelocity = ( b2_pi / 180.0f ) * m_angularSpeed; + b2BodyId bodyId = b2CreateBody( m_worldId, &bodyDef ); + m_tumblerIds[index] = bodyId; + + b2ShapeDef shapeDef = b2DefaultShapeDef(); + shapeDef.density = 50.0f; + + b2Polygon polygon; + polygon = b2MakeOffsetBox( 0.25f, 2.0f, { 2.0f, 0.0f }, b2Rot_identity ); + b2CreatePolygonShape( bodyId, &shapeDef, &polygon ); + polygon = b2MakeOffsetBox( 0.25f, 2.0f, { -2.0f, 0.0f }, b2Rot_identity ); + b2CreatePolygonShape( bodyId, &shapeDef, &polygon ); + polygon = b2MakeOffsetBox( 2.0f, 0.25f, { 0.0f, 2.0f }, b2Rot_identity ); + b2CreatePolygonShape( bodyId, &shapeDef, &polygon ); + polygon = b2MakeOffsetBox( 2.0f, 0.25f, { 0.0f, -2.0f }, b2Rot_identity ); + b2CreatePolygonShape( bodyId, &shapeDef, &polygon ); + } + + void CreateScene() + { + for ( int i = 0; i < m_bodyCount; ++i ) + { + if ( B2_IS_NON_NULL( m_bodyIds[i] ) ) + { + b2DestroyBody( m_bodyIds[i] ); + } + } + + for ( int i = 0; i < m_tumblerCount; ++i ) + { + b2DestroyBody( m_tumblerIds[i] ); + } + + free( m_tumblerIds ); + free( m_positions ); + + m_tumblerCount = m_rowCount * m_columnCount; + m_tumblerIds = static_cast( malloc( m_tumblerCount * sizeof( b2BodyId ) ) ); + m_positions = static_cast( malloc( m_tumblerCount * sizeof( b2Vec2 ) ) ); + + int index = 0; + float x = -4.0f * m_rowCount; + for ( int i = 0; i < m_rowCount; ++i ) + { + float y = -4.0f * m_columnCount; + for ( int j = 0; j < m_columnCount; ++j ) + { + m_positions[index] = { x, y }; + CreateTumbler( m_positions[index], index ); + ++index; + y += 8.0f; + } + + x += 8.0f; + } + + free( m_bodyIds ); + + int bodiesPerTumbler = g_sampleDebug ? 8 : 50; + m_bodyCount = bodiesPerTumbler * m_tumblerCount; + + m_bodyIds = static_cast( malloc( m_bodyCount * sizeof( b2BodyId ) ) ); + + memset( m_bodyIds, 0, m_bodyCount * sizeof( b2BodyId ) ); + m_bodyIndex = 0; + } + + void UpdateUI() override + { + float height = 110.0f; + ImGui::SetNextWindowPos( ImVec2( 10.0f, g_camera.m_height - height - 50.0f ), ImGuiCond_Once ); + ImGui::SetNextWindowSize( ImVec2( 200.0f, height ) ); + ImGui::Begin( "Benchmark: Many Tumblers", nullptr, ImGuiWindowFlags_NoResize ); + ImGui::PushItemWidth( 100.0f ); + + bool changed = false; + changed = changed || ImGui::SliderInt( "Row Count", &m_rowCount, 1, 32 ); + changed = changed || ImGui::SliderInt( "Column Count", &m_columnCount, 1, 32 ); + + if ( changed ) + { + CreateScene(); + } + + if ( ImGui::SliderFloat( "Speed", &m_angularSpeed, 0.0f, 100.0f, "%.f" ) ) + { + for ( int i = 0; i < m_tumblerCount; ++i ) + { + b2Body_SetAngularVelocity( m_tumblerIds[i], ( b2_pi / 180.0f ) * m_angularSpeed ); + b2Body_SetAwake( m_tumblerIds[i], true ); + } + } + + ImGui::PopItemWidth(); + ImGui::End(); + } + + void Step( Settings& settings ) override + { + Sample::Step( settings ); + + if ( m_bodyIndex < m_bodyCount && ( m_stepCount & 0x7 ) == 0 ) + { + b2ShapeDef shapeDef = b2DefaultShapeDef(); + + b2Capsule capsule = { { -0.1f, 0.0f }, { 0.1f, 0.0f }, 0.075f }; + + for ( int i = 0; i < m_tumblerCount; ++i ) + { + assert( m_bodyIndex < m_bodyCount ); + + b2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.type = b2_dynamicBody; + bodyDef.position = m_positions[i]; + m_bodyIds[m_bodyIndex] = b2CreateBody( m_worldId, &bodyDef ); + b2CreateCapsuleShape( m_bodyIds[m_bodyIndex], &shapeDef, &capsule ); + + m_bodyIndex += 1; + } + } + } + + static Sample* Create( Settings& settings ) + { + return new BenchmarkManyTumblers( settings ); + } + + b2BodyId m_groundId; + + int m_rowCount; + int m_columnCount; + + b2BodyId* m_tumblerIds; + b2Vec2* m_positions; + int m_tumblerCount; + + b2BodyId* m_bodyIds; + int m_bodyCount; + int m_bodyIndex; + + float m_angularSpeed; +}; + +static int benchmarkManyTumblers = RegisterSample( "Benchmark", "Many Tumblers", BenchmarkManyTumblers::Create ); + +class BenchmarkLargePyramid : public Sample +{ +public: + explicit BenchmarkLargePyramid( Settings& settings ) + : Sample( settings ) + { + if ( settings.restart == false ) + { + g_camera.m_center = { 0.0f, 50.0f }; + g_camera.m_zoom = 25.0f * 2.2f; + } + +#ifdef NDEBUG + int baseCount = 100; +#else + int baseCount = 40; +#endif + + { + b2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.position = { 0.0f, -1.0f }; + b2BodyId groundId = b2CreateBody( m_worldId, &bodyDef ); + + b2Polygon box = b2MakeBox( 100.0f, 1.0f ); + b2ShapeDef shapeDef = b2DefaultShapeDef(); + b2CreatePolygonShape( groundId, &shapeDef, &box ); + } + + b2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.type = b2_dynamicBody; + + b2ShapeDef shapeDef = b2DefaultShapeDef(); + shapeDef.density = 1.0f; + + float h = 0.5f; + b2Polygon box = b2MakeRoundedBox( h - 0.05f, h - 0.05f, 0.05f ); + + float shift = 1.0f * h; + + for ( int i = 0; i < baseCount; ++i ) + { + float y = ( 2.0f * i + 1.0f ) * shift; + + for ( int j = i; j < baseCount; ++j ) + { + float x = ( i + 1.0f ) * shift + 2.0f * ( j - i ) * shift - h * baseCount; + + bodyDef.position = { x, y }; + + b2BodyId bodyId = b2CreateBody( m_worldId, &bodyDef ); + b2CreatePolygonShape( bodyId, &shapeDef, &box ); + } + } + } + + static Sample* Create( Settings& settings ) + { + return new BenchmarkLargePyramid( settings ); + } +}; + +static int benchmarkLargePyramid = RegisterSample( "Benchmark", "Large Pyramid", BenchmarkLargePyramid::Create ); + +class BenchmarkManyPyramids : public Sample +{ +public: + explicit BenchmarkManyPyramids( Settings& settings ) + : Sample( settings ) + { + if ( settings.restart == false ) + { + g_camera.m_center = { 16.0f, 110.0f }; + g_camera.m_zoom = 25.0f * 5.0f; + } + + m_extent = 0.5f; + m_round = 0.0f; + m_baseCount = 10; + m_rowCount = g_sampleDebug ? 4 : 20; + m_columnCount = g_sampleDebug ? 4 : 20; + m_groundId = b2_nullBodyId; + m_bodyIds = nullptr; + m_bodyCount = 0; + m_bodyIndex = 0; + + CreateScene(); + } + + ~BenchmarkManyPyramids() override + { + free( m_bodyIds ); + } + + void CreatePyramid( float centerX, float baseY ) + { + b2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.type = b2_dynamicBody; + + b2ShapeDef shapeDef = b2DefaultShapeDef(); + shapeDef.density = 1.0f; + + float h = m_extent - m_round; + b2Polygon box = b2MakeRoundedBox( h, h, m_round ); + + float shift = 1.0f * h; + + for ( int i = 0; i < m_baseCount; ++i ) + { + float y = ( 2.0f * i + 1.0f ) * shift + baseY; + + for ( int j = i; j < m_baseCount; ++j ) + { + float x = ( i + 1.0f ) * shift + 2.0f * ( j - i ) * shift + centerX - 0.5f; + + bodyDef.position = { x, y }; + + assert( m_bodyIndex < m_bodyCount ); + m_bodyIds[m_bodyIndex] = b2CreateBody( m_worldId, &bodyDef ); + b2CreatePolygonShape( m_bodyIds[m_bodyIndex], &shapeDef, &box ); + + m_bodyIndex += 1; + } + } + } + + void CreateScene() + { + if ( B2_IS_NON_NULL( m_groundId ) ) + { + b2DestroyBody( m_groundId ); + } + + for ( int i = 0; i < m_bodyCount; ++i ) + { + b2DestroyBody( m_bodyIds[i] ); + } + + free( m_bodyIds ); + + m_bodyCount = m_rowCount * m_columnCount * m_baseCount * ( m_baseCount + 1 ) / 2; + m_bodyIds = (b2BodyId*)malloc( m_bodyCount * sizeof( b2BodyId ) ); + m_bodyIndex = 0; + + b2BodyDef bodyDef = b2DefaultBodyDef(); + m_groundId = b2CreateBody( m_worldId, &bodyDef ); + + float groundDeltaY = 2.0f * m_extent * ( m_baseCount + 1.0f ); + float groundWidth = 2.0f * m_extent * m_columnCount * ( m_baseCount + 1.0f ); + b2ShapeDef shapeDef = b2DefaultShapeDef(); + + float groundY = 0.0f; + + for ( int i = 0; i < m_rowCount; ++i ) + { + // b2Segment segment = {{-0.5f * groundWidth, groundY}, {0.5f * groundWidth, groundY}}; + b2Segment segment = { { -0.5f * 2.0f * groundWidth, groundY }, { 0.5f * 2.0f * groundWidth, groundY } }; + b2CreateSegmentShape( m_groundId, &shapeDef, &segment ); + groundY += groundDeltaY; + } + + float baseWidth = 2.0f * m_extent * m_baseCount; + float baseY = 0.0f; + + for ( int i = 0; i < m_rowCount; ++i ) + { + for ( int j = 0; j < m_columnCount; ++j ) + { + float centerX = -0.5f * groundWidth + j * ( baseWidth + 2.0f * m_extent ) + m_extent; + CreatePyramid( centerX, baseY ); + } + + baseY += groundDeltaY; + } + + assert( m_bodyIndex == m_bodyCount ); + } + + void UpdateUI() override + { + float height = 160.0f; + ImGui::SetNextWindowPos( ImVec2( 10.0f, g_camera.m_height - height - 50.0f ), ImGuiCond_Once ); + ImGui::SetNextWindowSize( ImVec2( 200.0f, height ) ); + ImGui::Begin( "Benchmark: Many Pyramids", nullptr, ImGuiWindowFlags_NoResize ); + ImGui::PushItemWidth( 100.0f ); + + bool changed = false; + changed = changed || ImGui::SliderInt( "Row Count", &m_rowCount, 1, 32 ); + changed = changed || ImGui::SliderInt( "Column Count", &m_columnCount, 1, 32 ); + changed = changed || ImGui::SliderInt( "Base Count", &m_baseCount, 1, 30 ); + + changed = changed || ImGui::SliderFloat( "Round", &m_round, 0.0f, 0.4f, "%.1f" ); + changed = changed || ImGui::Button( "Reset Scene" ); + + if ( changed ) + { + CreateScene(); + } + + ImGui::PopItemWidth(); + ImGui::End(); + } + + static Sample* Create( Settings& settings ) + { + return new BenchmarkManyPyramids( settings ); + } + + b2BodyId m_groundId; + b2BodyId* m_bodyIds; + int m_bodyCount; + int m_bodyIndex; + int m_baseCount; + int m_rowCount; + int m_columnCount; + float m_round; + float m_extent; +}; + +static int benchmarkManyPyramids = RegisterSample( "Benchmark", "Many Pyramids", BenchmarkManyPyramids::Create ); + +class BenchmarkCreateDestroy : public Sample +{ +public: + enum + { + e_maxBaseCount = 100, + e_maxBodyCount = e_maxBaseCount * ( e_maxBaseCount + 1 ) / 2 + }; + + explicit BenchmarkCreateDestroy( Settings& settings ) + : Sample( settings ) + { + if ( settings.restart == false ) + { + g_camera.m_center = { 0.0f, 50.0f }; + g_camera.m_zoom = 25.0f * 2.2f; + } + + float groundSize = 100.0f; + + b2BodyDef bodyDef = b2DefaultBodyDef(); + b2BodyId groundId = b2CreateBody( m_worldId, &bodyDef ); + + b2Polygon box = b2MakeBox( groundSize, 1.0f ); + b2ShapeDef shapeDef = b2DefaultShapeDef(); + b2CreatePolygonShape( groundId, &shapeDef, &box ); + + for ( int i = 0; i < e_maxBodyCount; ++i ) + { + m_bodies[i] = b2_nullBodyId; + } + + m_baseCount = g_sampleDebug ? 40 : 100; + m_iterations = g_sampleDebug ? 1 : 10; + m_bodyCount = 0; + } + + void CreateScene() + { + for ( int i = 0; i < e_maxBodyCount; ++i ) + { + if ( B2_IS_NON_NULL( m_bodies[i] ) ) + { + b2DestroyBody( m_bodies[i] ); + m_bodies[i] = b2_nullBodyId; + } + } + + int count = m_baseCount; + float rad = 0.5f; + float shift = rad * 2.0f; + float centerx = shift * count / 2.0f; + float centery = shift / 2.0f + 1.0f; + + b2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.type = b2_dynamicBody; + + b2ShapeDef shapeDef = b2DefaultShapeDef(); + shapeDef.density = 1.0f; + shapeDef.friction = 0.5f; + + float h = 0.5f; + b2Polygon box = b2MakeRoundedBox( h, h, 0.0f ); + + int index = 0; + + for ( int i = 0; i < count; ++i ) + { + float y = i * shift + centery; + + for ( int j = i; j < count; ++j ) + { + float x = 0.5f * i * shift + ( j - i ) * shift - centerx; + bodyDef.position = { x, y }; + + assert( index < e_maxBodyCount ); + m_bodies[index] = b2CreateBody( m_worldId, &bodyDef ); + b2CreatePolygonShape( m_bodies[index], &shapeDef, &box ); + + index += 1; + } + } + + m_bodyCount = index; + } + + void Step( Settings& settings ) override + { + b2Timer timer = b2CreateTimer(); + + for ( int i = 0; i < m_iterations; ++i ) + { + CreateScene(); + } + + float ms = b2GetMilliseconds( &timer ); + + g_draw.DrawString( 5, m_textLine, "milliseconds = %g", ms ); + m_textLine += m_textIncrement; + + Sample::Step( settings ); + } + + static Sample* Create( Settings& settings ) + { + return new BenchmarkCreateDestroy( settings ); + } + + b2BodyId m_bodies[e_maxBodyCount]; + int m_bodyCount; + int m_baseCount; + int m_iterations; +}; + +static int benchmarkCreateDestroy = RegisterSample( "Benchmark", "CreateDestroy", BenchmarkCreateDestroy::Create ); + +class BenchmarkSleep : public Sample +{ +public: + enum + { + e_maxBaseCount = 100, + e_maxBodyCount = e_maxBaseCount * ( e_maxBaseCount + 1 ) / 2 + }; + + explicit BenchmarkSleep( Settings& settings ) + : Sample( settings ) + { + if ( settings.restart == false ) + { + g_camera.m_center = { 0.0f, 50.0f }; + g_camera.m_zoom = 25.0f * 2.2f; + } + + float groundSize = 100.0f; + + b2BodyDef bodyDef = b2DefaultBodyDef(); + b2BodyId groundId = b2CreateBody( m_worldId, &bodyDef ); + + b2Polygon box = b2MakeBox( groundSize, 1.0f ); + b2ShapeDef shapeDef = b2DefaultShapeDef(); + b2CreatePolygonShape( groundId, &shapeDef, &box ); + + for ( int i = 0; i < e_maxBodyCount; ++i ) + { + m_bodies[i] = b2_nullBodyId; + } + + m_baseCount = g_sampleDebug ? 40 : 100; + m_iterations = g_sampleDebug ? 1 : 41; + m_bodyCount = 0; + m_awake = false; + + m_wakeTotal = 0.0f; + m_wakeCount = 0; + + m_sleepTotal = 0.0f; + m_sleepCount = 0; + + CreateScene(); + } + + void CreateScene() + { + for ( int i = 0; i < e_maxBodyCount; ++i ) + { + if ( B2_IS_NON_NULL( m_bodies[i] ) ) + { + b2DestroyBody( m_bodies[i] ); + m_bodies[i] = b2_nullBodyId; + } + } + + int count = m_baseCount; + float rad = 0.5f; + float shift = rad * 2.0f; + float centerx = shift * count / 2.0f; + float centery = shift / 2.0f + 1.0f; + + b2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.type = b2_dynamicBody; + + b2ShapeDef shapeDef = b2DefaultShapeDef(); + shapeDef.density = 1.0f; + shapeDef.friction = 0.5f; + + float h = 0.5f; + b2Polygon box = b2MakeRoundedBox( h, h, 0.0f ); + + int index = 0; + + for ( int i = 0; i < count; ++i ) + { + float y = i * shift + centery; + + for ( int j = i; j < count; ++j ) + { + float x = 0.5f * i * shift + ( j - i ) * shift - centerx; + bodyDef.position = { x, y }; + + assert( index < e_maxBodyCount ); + m_bodies[index] = b2CreateBody( m_worldId, &bodyDef ); + b2CreatePolygonShape( m_bodies[index], &shapeDef, &box ); + + index += 1; + } + } + + m_bodyCount = index; + } + + void Step( Settings& settings ) override + { + float timeStep = settings.hertz > 0.0f ? 1.0f / settings.hertz : float( 0.0f ); + + b2Timer timer = b2CreateTimer(); + + for ( int i = 0; i < m_iterations; ++i ) + { + b2Body_SetAwake( m_bodies[0], m_awake ); + if ( m_awake ) + { + m_wakeTotal += b2GetMillisecondsAndReset( &timer ); + m_wakeCount += 1; + } + else + { + m_sleepTotal += b2GetMillisecondsAndReset( &timer ); + m_sleepCount += 1; + } + m_awake = !m_awake; + } + + if ( m_wakeCount > 0 ) + { + g_draw.DrawString( 5, m_textLine, "wake ave = %g ms", m_wakeTotal / m_wakeCount ); + m_textLine += m_textIncrement; + } + + if ( m_sleepCount > 0 ) + { + g_draw.DrawString( 5, m_textLine, "sleep ave = %g ms", m_sleepTotal / m_sleepCount ); + m_textLine += m_textIncrement; + } + + Sample::Step( settings ); + } + + static Sample* Create( Settings& settings ) + { + return new BenchmarkSleep( settings ); + } + + b2BodyId m_bodies[e_maxBodyCount]; + int m_bodyCount; + int m_baseCount; + int m_iterations; + float m_wakeTotal; + float m_sleepTotal; + int m_wakeCount; + int m_sleepCount; + bool m_awake; +}; + +static int benchmarkSleep = RegisterSample( "Benchmark", "Sleep", BenchmarkSleep::Create ); + +class BenchmarkJointGrid : public Sample +{ +public: + explicit BenchmarkJointGrid( Settings& settings ) + : Sample( settings ) + { + if ( settings.restart == false ) + { + g_camera.m_center = { 60.0f, -57.0f }; + g_camera.m_zoom = 25.0f * 2.5f; + } + + constexpr int N = g_sampleDebug ? 10 : 100; + + // Allocate to avoid huge stack usage + b2BodyId* bodies = static_cast( malloc( N * N * sizeof( b2BodyId ) ) ); + int index = 0; + + b2ShapeDef shapeDef = b2DefaultShapeDef(); + shapeDef.density = 1.0f; + shapeDef.filter.categoryBits = 2; + shapeDef.filter.maskBits = ~2u; + + b2Circle circle = { { 0.0f, 0.0f }, 0.4f }; + + b2RevoluteJointDef jd = b2DefaultRevoluteJointDef(); + b2BodyDef bodyDef = b2DefaultBodyDef(); + + for ( int k = 0; k < N; ++k ) + { + for ( int i = 0; i < N; ++i ) + { + float fk = (float)k; + float fi = (float)i; + + if ( k >= N / 2 - 3 && k <= N / 2 + 3 && i == 0 ) + { + bodyDef.type = b2_staticBody; + } + else + { + bodyDef.type = b2_dynamicBody; + } + + bodyDef.position = { fk, -fi }; + + b2BodyId body = b2CreateBody( m_worldId, &bodyDef ); + + b2CreateCircleShape( body, &shapeDef, &circle ); + + if ( i > 0 ) + { + jd.bodyIdA = bodies[index - 1]; + jd.bodyIdB = body; + jd.localAnchorA = { 0.0f, -0.5f }; + jd.localAnchorB = { 0.0f, 0.5f }; + b2CreateRevoluteJoint( m_worldId, &jd ); + } + + if ( k > 0 ) + { + jd.bodyIdA = bodies[index - N]; + jd.bodyIdB = body; + jd.localAnchorA = { 0.5f, 0.0f }; + jd.localAnchorB = { -0.5f, 0.0f }; + b2CreateRevoluteJoint( m_worldId, &jd ); + } + + bodies[index++] = body; + } + } + + free( bodies ); + + m_gravity = 10.0f; + } + + void UpdateUI() override + { + float height = 60.0f; + ImGui::SetNextWindowPos( ImVec2( 10.0f, g_camera.m_height - height - 50.0f ), ImGuiCond_Once ); + ImGui::SetNextWindowSize( ImVec2( 240.0f, height ) ); + ImGui::Begin( "Benchmark: Joint Grid", nullptr, ImGuiWindowFlags_NoResize ); + + if ( ImGui::SliderFloat( "gravity", &m_gravity, 0.0f, 20.0f, "%.1f" ) ) + { + b2World_SetGravity( m_worldId, { 0.0f, -m_gravity } ); + } + + ImGui::End(); + } + + static Sample* Create( Settings& settings ) + { + return new BenchmarkJointGrid( settings ); + } + + float m_gravity; +}; + +static int benchmarkJointGridIndex = RegisterSample( "Benchmark", "Joint Grid", BenchmarkJointGrid::Create ); + +class BenchmarkSmash : public Sample +{ +public: + explicit BenchmarkSmash( Settings& settings ) + : Sample( settings ) + { + if ( settings.restart == false ) + { + g_camera.m_center = { 60.0f, 6.0f }; + g_camera.m_zoom = 25.0f * 1.6f; + } + + b2World_SetGravity( m_worldId, b2Vec2_zero ); + + { + b2Polygon box = b2MakeBox( 4.0f, 4.0f ); + + b2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.type = b2_dynamicBody; + bodyDef.position = { -20.0f, 0.0f }; + bodyDef.linearVelocity = { 40.0f, 0.0f }; + b2BodyId bodyId = b2CreateBody( m_worldId, &bodyDef ); + + b2ShapeDef shapeDef = b2DefaultShapeDef(); + shapeDef.density = 8.0f; + b2CreatePolygonShape( bodyId, &shapeDef, &box ); + } + + m_created = false; + } + + void CreateScene1() + { + ImGuiIO& io = ImGui::GetIO(); + if ( io.Fonts->Fonts.size() == 0 ) + { + return; + } + + const ImFont* font = io.Fonts->Fonts[0]; + const unsigned char* pixels = font->ContainerAtlas->TexPixelsAlpha8; + int width = font->ContainerAtlas->TexWidth; + int height = font->ContainerAtlas->TexHeight; + + float scale = 0.1f; + + b2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.type = b2_dynamicBody; + bodyDef.isAwake = false; + + b2ShapeDef shapeDef = b2DefaultShapeDef(); + + for ( int i = 0; i < height; ++i ) + { + for ( int j = 0; j < width; ++j ) + { + unsigned char value = pixels[i * width + j]; + if ( value != 0 && value != 0xFF ) + { + value += 0; + } + + if ( value > 50 ) + { + b2Polygon square = b2MakeSquare( 0.95f * scale * ( value / 255.0f ) ); + bodyDef.position = { 2.0f * j * scale, 2.0f * ( height - i ) * scale - 10.0f }; + b2BodyId bodyId = b2CreateBody( m_worldId, &bodyDef ); + b2CreatePolygonShape( bodyId, &shapeDef, &square ); + } + } + } + + m_created = true; + } + + void CreateScene2() + { + ImGuiIO& io = ImGui::GetIO(); + if ( io.Fonts->Fonts.size() == 0 ) + { + return; + } + + const ImFont* font = io.Fonts->Fonts.back(); + const unsigned char* pixels = font->ContainerAtlas->TexPixelsAlpha8; + int width = font->ContainerAtlas->TexWidth; + int height = font->ContainerAtlas->TexHeight; + int fontSize = font->Ascent; + + float scale = 0.1f; + + b2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.type = b2_dynamicBody; + bodyDef.isAwake = false; + + b2ShapeDef shapeDef = b2DefaultShapeDef(); + + const char* text = "I"; + int n = (int)strlen( text ); + float zoom = 1.0f; + + float x = 0.0f; + for ( int k = 0; k < n; ++k ) + { + const ImFontGlyph* glyph = font->FindGlyph( text[k] ); + float x1 = glyph->X0; + float x2 = glyph->X1; + float y1 = glyph->Y0; + float y2 = glyph->Y1; + float u1 = glyph->U0; + float v1 = glyph->V0; + float u2 = glyph->U1; + float v2 = glyph->V1; + + float w = zoom * ( x2 - x1 ); + float h = zoom * ( y2 - y1 ); + + int gridx = int( w ); + int gridy = int( h ); + for ( int i = 0; i < gridy; ++i ) + { + float v = v1 + i / h * ( v2 - v1 ); + int iy = int( v * height ); + + for ( int j = 0; j < gridx; ++j ) + { + float u = u1 + j / w * ( u2 - u1 ); + int ix = int( u * width ); + + unsigned char value = pixels[iy * width + ix]; + if ( value > 50 ) + { + b2Polygon square = b2MakeSquare( 0.9f * scale * value / 255.0f ); + bodyDef.position = { x + 2.0f * ( zoom * x1 + j ) * scale, -2.0f * ( zoom * y1 + i ) * scale + 13.0f }; + b2BodyId bodyId = b2CreateBody( m_worldId, &bodyDef ); + b2CreatePolygonShape( bodyId, &shapeDef, &square ); + } + } + } + + x += 2.0f * zoom * scale * glyph->AdvanceX; + } + + m_created = true; + } + + void CreateScene3() + { + float d = 0.4f; + b2Polygon box = b2MakeSquare( 0.5f * d ); + + b2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.type = b2_dynamicBody; + bodyDef.isAwake = false; + + b2ShapeDef shapeDef = b2DefaultShapeDef(); + + int columns = g_sampleDebug ? 20 : 120; + int rows = g_sampleDebug ? 10 : 80; + + for ( int i = 0; i < columns; ++i ) + { + for ( int j = 0; j < rows; ++j ) + { + bodyDef.position.x = i * d + 30.0f; + bodyDef.position.y = ( j - rows / 2.0f ) * d; + b2BodyId bodyId = b2CreateBody( m_worldId, &bodyDef ); + b2CreatePolygonShape( bodyId, &shapeDef, &box ); + } + } + + m_created = true; + } + + void Step( Settings& settings ) override + { + Sample::Step( settings ); + + if ( m_created == false ) + { + CreateScene3(); + } + } + + static Sample* Create( Settings& settings ) + { + return new BenchmarkSmash( settings ); + } + + bool m_created; +}; + +static int sampleSmash = RegisterSample( "Benchmark", "Smash", BenchmarkSmash::Create ); + +class BenchmarkCompound : public Sample +{ +public: + explicit BenchmarkCompound( Settings& settings ) + : Sample( settings ) + { + if ( settings.restart == false ) + { + g_camera.m_center = { 18.0f, 115.0f }; + g_camera.m_zoom = 25.0f * 5.5f; + } + + float grid = 1.0f; +#ifdef NDEBUG + int height = 200; + int width = 200; +#else + int height = 100; + int width = 100; +#endif + { + + b2BodyDef bodyDef = b2DefaultBodyDef(); + b2BodyId groundId = b2CreateBody( m_worldId, &bodyDef ); + b2ShapeDef shapeDef = b2DefaultShapeDef(); + + for ( int i = 0; i < height; ++i ) + { + float y = grid * i; + for ( int j = i; j < width; ++j ) + { + float x = grid * j; + b2Polygon square = b2MakeOffsetBox( 0.5f * grid, 0.5f * grid, { x, y }, b2Rot_identity ); + b2CreatePolygonShape( groundId, &shapeDef, &square ); + } + } + + for ( int i = 0; i < height; ++i ) + { + float y = grid * i; + for ( int j = i; j < width; ++j ) + { + float x = -grid * j; + b2Polygon square = b2MakeOffsetBox( 0.5f * grid, 0.5f * grid, { x, y }, b2Rot_identity ); + b2CreatePolygonShape( groundId, &shapeDef, &square ); + } + } + } + + { +#ifdef NDEBUG + int span = 20; + int count = 5; +#else + int span = 5; + int count = 5; +#endif + + b2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.type = b2_dynamicBody; + // defer mass properties to avoid n-squared mass computations + bodyDef.automaticMass = false; + b2ShapeDef shapeDef = b2DefaultShapeDef(); + + for ( int m = 0; m < count; ++m ) + { + float ybody = ( 100.0f + m * span ) * grid; + + for ( int n = 0; n < count; ++n ) + { + float xbody = -0.5f * grid * count * span + n * span * grid; + bodyDef.position = { xbody, ybody }; + b2BodyId bodyId = b2CreateBody( m_worldId, &bodyDef ); + + for ( int i = 0; i < span; ++i ) + { + float y = i * grid; + for ( int j = 0; j < span; ++j ) + { + float x = j * grid; + b2Polygon square = b2MakeOffsetBox( 0.5f * grid, 0.5f * grid, { x, y }, b2Rot_identity ); + b2CreatePolygonShape( bodyId, &shapeDef, &square ); + } + } + + // All shapes have been added so I can efficiently compute the mass properties. + b2Body_ApplyMassFromShapes( bodyId ); + } + } + } + } + + static Sample* Create( Settings& settings ) + { + return new BenchmarkCompound( settings ); + } +}; + +static int sampleCompound = RegisterSample( "Benchmark", "Compound", BenchmarkCompound::Create ); + +class BenchmarkKinematic : public Sample +{ +public: + explicit BenchmarkKinematic( Settings& settings ) + : Sample( settings ) + { + if ( settings.restart == false ) + { + g_camera.m_center = { 0.0f, 0.0f }; + g_camera.m_zoom = 150.0f; + } + + float grid = 1.0f; + +#ifdef NDEBUG + int span = 100; +#else + int span = 20; +#endif + + b2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.type = b2_kinematicBody; + bodyDef.angularVelocity = 1.0f; + // defer mass properties to avoid n-squared mass computations + bodyDef.automaticMass = false; + + b2ShapeDef shapeDef = b2DefaultShapeDef(); + shapeDef.filter.categoryBits = 1; + shapeDef.filter.maskBits = 2; + + b2BodyId bodyId = b2CreateBody( m_worldId, &bodyDef ); + + for ( int i = -span; i < span; ++i ) + { + float y = i * grid; + for ( int j = -span; j < span; ++j ) + { + float x = j * grid; + b2Polygon square = b2MakeOffsetBox( 0.5f * grid, 0.5f * grid, { x, y }, b2Rot_identity ); + b2CreatePolygonShape( bodyId, &shapeDef, &square ); + } + } + + // All shapes have been added so I can efficiently compute the mass properties. + b2Body_ApplyMassFromShapes( bodyId ); + } + + static Sample* Create( Settings& settings ) + { + return new BenchmarkKinematic( settings ); + } +}; + +static int sampleKinematic = RegisterSample( "Benchmark", "Kinematic", BenchmarkKinematic::Create ); diff --git a/tests/cpp-tests/Source/Box2DTestBed/samples/sample_bodies.cpp b/tests/cpp-tests/Source/Box2DTestBed/samples/sample_bodies.cpp new file mode 100644 index 000000000000..cc441a50510e --- /dev/null +++ b/tests/cpp-tests/Source/Box2DTestBed/samples/sample_bodies.cpp @@ -0,0 +1,839 @@ +// SPDX-FileCopyrightText: 2022 Erin Catto +// SPDX-License-Identifier: MIT + +#include "draw.h" +#include "sample.h" +#include "settings.h" + +#include "box2d/box2d.h" + +#include + +class BodyType : public Sample +{ +public: + explicit BodyType( Settings& settings ) + : Sample( settings ) + { + if ( settings.restart == false ) + { + g_camera.m_center = { 0.8f, 6.4f }; + g_camera.m_zoom = 25.0f * 0.4f; + } + + m_type = b2_dynamicBody; + m_isEnabled = true; + + b2BodyId groundId = b2_nullBodyId; + { + b2BodyDef bodyDef = b2DefaultBodyDef(); + groundId = b2CreateBody( m_worldId, &bodyDef ); + + b2Segment segment = { { -20.0f, 0.0f }, { 20.0f, 0.0f } }; + b2ShapeDef shapeDef = b2DefaultShapeDef(); + b2CreateSegmentShape( groundId, &shapeDef, &segment ); + } + + // Define attachment + { + b2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.type = b2_dynamicBody; + bodyDef.position = { -2.0f, 3.0f }; + m_attachmentId = b2CreateBody( m_worldId, &bodyDef ); + + b2Polygon box = b2MakeBox( 0.5f, 2.0f ); + b2ShapeDef shapeDef = b2DefaultShapeDef(); + shapeDef.density = 1.0f; + b2CreatePolygonShape( m_attachmentId, &shapeDef, &box ); + } + + // Define second attachment + { + b2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.type = m_type; + bodyDef.isEnabled = m_isEnabled; + bodyDef.position = { 3.0f, 3.0f }; + m_secondAttachmentId = b2CreateBody( m_worldId, &bodyDef ); + + b2Polygon box = b2MakeBox( 0.5f, 2.0f ); + b2ShapeDef shapeDef = b2DefaultShapeDef(); + shapeDef.density = 1.0f; + b2CreatePolygonShape( m_secondAttachmentId, &shapeDef, &box ); + } + + // Define platform + { + b2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.type = m_type; + bodyDef.isEnabled = m_isEnabled; + bodyDef.position = { -4.0f, 5.0f }; + m_platformId = b2CreateBody( m_worldId, &bodyDef ); + + b2Polygon box = b2MakeOffsetBox( 0.5f, 4.0f, { 4.0f, 0.0f }, b2MakeRot(0.5f * b2_pi) ); + + b2ShapeDef shapeDef = b2DefaultShapeDef(); + shapeDef.friction = 0.6f; + shapeDef.density = 2.0f; + b2CreatePolygonShape( m_platformId, &shapeDef, &box ); + + b2RevoluteJointDef revoluteDef = b2DefaultRevoluteJointDef(); + b2Vec2 pivot = { -2.0f, 5.0f }; + revoluteDef.bodyIdA = m_attachmentId; + revoluteDef.bodyIdB = m_platformId; + revoluteDef.localAnchorA = b2Body_GetLocalPoint( m_attachmentId, pivot ); + revoluteDef.localAnchorB = b2Body_GetLocalPoint( m_platformId, pivot ); + revoluteDef.maxMotorTorque = 50.0f; + revoluteDef.enableMotor = true; + b2CreateRevoluteJoint( m_worldId, &revoluteDef ); + + pivot = { 3.0f, 5.0f }; + revoluteDef.bodyIdA = m_secondAttachmentId; + revoluteDef.bodyIdB = m_platformId; + revoluteDef.localAnchorA = b2Body_GetLocalPoint( m_secondAttachmentId, pivot ); + revoluteDef.localAnchorB = b2Body_GetLocalPoint( m_platformId, pivot ); + revoluteDef.maxMotorTorque = 50.0f; + revoluteDef.enableMotor = true; + b2CreateRevoluteJoint( m_worldId, &revoluteDef ); + + b2PrismaticJointDef prismaticDef = b2DefaultPrismaticJointDef(); + b2Vec2 anchor = { 0.0f, 5.0f }; + prismaticDef.bodyIdA = groundId; + prismaticDef.bodyIdB = m_platformId; + prismaticDef.localAnchorA = b2Body_GetLocalPoint( groundId, anchor ); + prismaticDef.localAnchorB = b2Body_GetLocalPoint( m_platformId, anchor ); + prismaticDef.localAxisA = { 1.0f, 0.0f }; + prismaticDef.maxMotorForce = 1000.0f; + prismaticDef.motorSpeed = 0.0f; + prismaticDef.enableMotor = true; + prismaticDef.lowerTranslation = -10.0f; + prismaticDef.upperTranslation = 10.0f; + prismaticDef.enableLimit = true; + + b2CreatePrismaticJoint( m_worldId, &prismaticDef ); + + m_speed = 3.0f; + } + + // Create a payload + { + b2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.type = b2_dynamicBody; + bodyDef.position = { -3.0f, 8.0f }; + b2BodyId bodyId = b2CreateBody( m_worldId, &bodyDef ); + + b2Polygon box = b2MakeBox( 0.75f, 0.75f ); + + b2ShapeDef shapeDef = b2DefaultShapeDef(); + shapeDef.friction = 0.6f; + shapeDef.density = 2.0f; + + b2CreatePolygonShape( bodyId, &shapeDef, &box ); + } + + // Create a second payload + { + b2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.type = m_type; + bodyDef.isEnabled = m_isEnabled; + bodyDef.position = { 2.0f, 8.0f }; + m_secondPayloadId = b2CreateBody( m_worldId, &bodyDef ); + + b2Polygon box = b2MakeBox( 0.75f, 0.75f ); + + b2ShapeDef shapeDef = b2DefaultShapeDef(); + shapeDef.friction = 0.6f; + shapeDef.density = 2.0f; + + b2CreatePolygonShape( m_secondPayloadId, &shapeDef, &box ); + } + + // Create a separate body on the ground + { + b2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.type = m_type; + bodyDef.isEnabled = m_isEnabled; + bodyDef.position = { 8.0f, 0.2f }; + m_touchingBodyId = b2CreateBody( m_worldId, &bodyDef ); + + b2Capsule capsule = { { 0.0f, 0.0f }, { 1.0f, 0.0f }, 0.25f }; + + b2ShapeDef shapeDef = b2DefaultShapeDef(); + shapeDef.friction = 0.6f; + shapeDef.density = 2.0f; + + b2CreateCapsuleShape( m_touchingBodyId, &shapeDef, &capsule ); + } + + // Create a separate floating body + { + b2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.type = m_type; + bodyDef.isEnabled = m_isEnabled; + bodyDef.position = { -8.0f, 12.0f }; + bodyDef.gravityScale = 0.0f; + m_floatingBodyId = b2CreateBody( m_worldId, &bodyDef ); + + b2Circle circle = { { 0.0f, 0.5f }, 0.25f }; + + b2ShapeDef shapeDef = b2DefaultShapeDef(); + shapeDef.friction = 0.6f; + shapeDef.density = 2.0f; + + b2CreateCircleShape( m_floatingBodyId, &shapeDef, &circle ); + } + } + + void UpdateUI() override + { + float height = 140.0f; + ImGui::SetNextWindowPos( ImVec2( 10.0f, g_camera.m_height - height - 50.0f ), ImGuiCond_Once ); + ImGui::SetNextWindowSize( ImVec2( 180.0f, height ) ); + ImGui::Begin( "Body Type", nullptr, ImGuiWindowFlags_NoMove | ImGuiWindowFlags_NoResize ); + + if ( ImGui::RadioButton( "Static", m_type == b2_staticBody ) ) + { + m_type = b2_staticBody; + b2Body_SetType( m_platformId, b2_staticBody ); + b2Body_SetType( m_secondAttachmentId, b2_staticBody ); + b2Body_SetType( m_secondPayloadId, b2_staticBody ); + b2Body_SetType( m_touchingBodyId, b2_staticBody ); + b2Body_SetType( m_floatingBodyId, b2_staticBody ); + } + + if ( ImGui::RadioButton( "Kinematic", m_type == b2_kinematicBody ) ) + { + m_type = b2_kinematicBody; + b2Body_SetType( m_platformId, b2_kinematicBody ); + b2Body_SetLinearVelocity( m_platformId, { -m_speed, 0.0f } ); + b2Body_SetAngularVelocity( m_platformId, 0.0f ); + b2Body_SetType( m_secondAttachmentId, b2_kinematicBody ); + b2Body_SetType( m_secondPayloadId, b2_kinematicBody ); + b2Body_SetType( m_touchingBodyId, b2_kinematicBody ); + b2Body_SetType( m_floatingBodyId, b2_kinematicBody ); + } + + if ( ImGui::RadioButton( "Dynamic", m_type == b2_dynamicBody ) ) + { + m_type = b2_dynamicBody; + b2Body_SetType( m_platformId, b2_dynamicBody ); + b2Body_SetType( m_secondAttachmentId, b2_dynamicBody ); + b2Body_SetType( m_secondPayloadId, b2_dynamicBody ); + b2Body_SetType( m_touchingBodyId, b2_dynamicBody ); + b2Body_SetType( m_floatingBodyId, b2_dynamicBody ); + } + + if ( ImGui::Checkbox( "Enable", &m_isEnabled ) ) + { + if ( m_isEnabled ) + { + b2Body_Enable( m_platformId ); + b2Body_Enable( m_secondAttachmentId ); + b2Body_Enable( m_secondPayloadId ); + b2Body_Enable( m_touchingBodyId ); + b2Body_Enable( m_floatingBodyId ); + + if ( m_type == b2_kinematicBody ) + { + b2Body_SetLinearVelocity( m_platformId, { -m_speed, 0.0f } ); + b2Body_SetAngularVelocity( m_platformId, 0.0f ); + } + } + else + { + b2Body_Disable( m_platformId ); + b2Body_Disable( m_secondAttachmentId ); + b2Body_Disable( m_secondPayloadId ); + b2Body_Disable( m_touchingBodyId ); + b2Body_Disable( m_floatingBodyId ); + } + } + + ImGui::End(); + } + + void Step( Settings& settings ) override + { + // Drive the kinematic body. + if ( m_type == b2_kinematicBody ) + { + b2Vec2 p = b2Body_GetPosition( m_platformId ); + b2Vec2 v = b2Body_GetLinearVelocity( m_platformId ); + + if ( ( p.x < -14.0f && v.x < 0.0f ) || ( p.x > 6.0f && v.x > 0.0f ) ) + { + v.x = -v.x; + b2Body_SetLinearVelocity( m_platformId, v ); + } + } + + Sample::Step( settings ); + } + + static Sample* Create( Settings& settings ) + { + return new BodyType( settings ); + } + + b2BodyId m_attachmentId; + b2BodyId m_secondAttachmentId; + b2BodyId m_platformId; + b2BodyId m_secondPayloadId; + b2BodyId m_touchingBodyId; + b2BodyId m_floatingBodyId; + b2BodyType m_type; + float m_speed; + bool m_isEnabled; +}; + +static int sampleBodyType = RegisterSample( "Bodies", "Body Type", BodyType::Create ); + +/// This is a test of typical character collision scenarios. This does not +/// show how you should implement a character in your application. +/// Instead this is used to test smooth collision on chain shapes. +class Character : public Sample +{ +public: + explicit Character( Settings& settings ) + : Sample( settings ) + { + if ( settings.restart == false ) + { + g_camera.m_center = { -2.0f, 7.0f }; + g_camera.m_zoom = 25.0f * 0.4f; + } + + // Ground body + { + b2BodyDef bodyDef = b2DefaultBodyDef(); + b2BodyId groundId = b2CreateBody( m_worldId, &bodyDef ); + + b2ShapeDef shapeDef = b2DefaultShapeDef(); + b2Segment segment = { { -20.0f, 0.0f }, { 20.0f, 0.0f } }; + b2CreateSegmentShape( groundId, &shapeDef, &segment ); + } + + // Collinear edges with no adjacency information. + // This shows the problematic case where a box shape can hit + // an internal vertex. + { + b2BodyDef bodyDef = b2DefaultBodyDef(); + b2BodyId groundId = b2CreateBody( m_worldId, &bodyDef ); + + b2ShapeDef shapeDef = b2DefaultShapeDef(); + b2Segment segment1 = { { -8.0f, 1.0f }, { -6.0f, 1.0f } }; + b2CreateSegmentShape( groundId, &shapeDef, &segment1 ); + + b2Segment segment2 = { { -6.0f, 1.0f }, { -4.0f, 1.0f } }; + b2CreateSegmentShape( groundId, &shapeDef, &segment2 ); + + b2Segment segment3 = { { -4.0f, 1.0f }, { -2.0f, 1.0f } }; + b2CreateSegmentShape( groundId, &shapeDef, &segment3 ); + } + + // Chain shape + { + b2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.rotation = b2MakeRot( 0.25f * b2_pi ); + b2BodyId groundId = b2CreateBody( m_worldId, &bodyDef ); + + b2Vec2 points[4] = { { 8.0f, 7.0f }, { 7.0f, 8.0f }, { 6.0f, 8.0f }, { 5.0f, 7.0f } }; + b2ChainDef chainDef = b2DefaultChainDef(); + chainDef.points = points; + chainDef.count = 4; + chainDef.isLoop = true; + + b2CreateChain( groundId, &chainDef ); + } + + // Square tiles. This shows that adjacency shapes may have non-smooth collision. Box2D has no solution + // to this problem. + // todo_erin try this: https://briansemrau.github.io/dealing-with-ghost-collisions/ + { + b2BodyDef bodyDef = b2DefaultBodyDef(); + b2BodyId groundId = b2CreateBody( m_worldId, &bodyDef ); + + b2ShapeDef shapeDef = b2DefaultShapeDef(); + b2Polygon box = b2MakeOffsetBox( 1.0f, 1.0f, { 4.0f, 3.0f }, b2Rot_identity ); + b2CreatePolygonShape( groundId, &shapeDef, &box ); + + box = b2MakeOffsetBox( 1.0f, 1.0f, { 6.0f, 3.0f }, b2Rot_identity ); + b2CreatePolygonShape( groundId, &shapeDef, &box ); + + box = b2MakeOffsetBox( 1.0f, 1.0f, { 8.0f, 3.0f }, b2Rot_identity ); + b2CreatePolygonShape( groundId, &shapeDef, &box ); + } + + // Square made from a chain loop. Collision should be smooth. + { + b2BodyDef bodyDef = b2DefaultBodyDef(); + b2BodyId groundId = b2CreateBody( m_worldId, &bodyDef ); + + b2Vec2 points[4] = { { -1.0f, 3.0 }, { 1.0f, 3.0f }, { 1.0f, 5.0f }, { -1.0f, 5.0 } }; + b2ChainDef chainDef = b2DefaultChainDef(); + chainDef.points = points; + chainDef.count = 4; + chainDef.isLoop = true; + b2CreateChain( groundId, &chainDef ); + } + + // Chain loop. Collision should be smooth. + { + b2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.position = { -10.0f, 4.0f }; + b2BodyId groundId = b2CreateBody( m_worldId, &bodyDef ); + + b2Vec2 points[10] = { { 0.0f, 0.0f }, { 6.0f, 0.0f }, { 6.0f, 2.0f }, { 4.0f, 1.0f }, { 2.0f, 2.0f }, + { 0.0f, 2.0f }, { -2.0f, 2.0f }, { -4.0f, 3.0f }, { -6.0f, 2.0f }, { -6.0f, 0.0f } }; + b2ChainDef chainDef = b2DefaultChainDef(); + chainDef.points = points; + chainDef.count = 10; + chainDef.isLoop = true; + b2CreateChain( groundId, &chainDef ); + } + + // Circle character + { + b2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.position = { -7.0f, 6.0f }; + bodyDef.type = b2_dynamicBody; + bodyDef.fixedRotation = true; + bodyDef.enableSleep = false; + + m_circleCharacterId = b2CreateBody( m_worldId, &bodyDef ); + + b2Circle circle = { { 0.0f, 0.0f }, 0.25f }; + + b2ShapeDef shapeDef = b2DefaultShapeDef(); + shapeDef.density = 20.0f; + shapeDef.friction = 0.2f; + b2CreateCircleShape( m_circleCharacterId, &shapeDef, &circle ); + } + + // Capsule character + { + b2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.position = { 3.0f, 5.0f }; + bodyDef.type = b2_dynamicBody; + bodyDef.fixedRotation = true; + bodyDef.enableSleep = false; + + m_capsuleCharacterId = b2CreateBody( m_worldId, &bodyDef ); + + b2Capsule capsule = { { 0.0f, 0.25f }, { 0.0f, 0.75f }, 0.25f }; + + b2ShapeDef shapeDef = b2DefaultShapeDef(); + shapeDef.density = 20.0f; + shapeDef.friction = 0.2f; + b2CreateCapsuleShape( m_capsuleCharacterId, &shapeDef, &capsule ); + } + + // Square character + { + b2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.position = { -3.0f, 8.0f }; + bodyDef.type = b2_dynamicBody; + bodyDef.fixedRotation = true; + bodyDef.enableSleep = false; + + m_boxCharacterId = b2CreateBody( m_worldId, &bodyDef ); + + b2Polygon box = b2MakeBox( 0.4f, 0.4f ); + + b2ShapeDef shapeDef = b2DefaultShapeDef(); + shapeDef.density = 20.0f; + shapeDef.friction = 0.2f; + b2CreatePolygonShape( m_boxCharacterId, &shapeDef, &box ); + } + } + + void Step( Settings& settings ) override + { + Sample::Step( settings ); + + g_draw.DrawString( 5, m_textLine, "This tests various character collision shapes." ); + m_textLine += m_textIncrement; + g_draw.DrawString( 5, m_textLine, "Limitation: square and hexagon can snag on aligned boxes." ); + m_textLine += m_textIncrement; + } + + static Sample* Create( Settings& settings ) + { + return new Character( settings ); + } + + b2BodyId m_circleCharacterId; + b2BodyId m_capsuleCharacterId; + b2BodyId m_boxCharacterId; +}; + +static int sampleCharacter = RegisterSample( "Bodies", "Character", Character::Create ); + +class Weeble : public Sample +{ +public: + explicit Weeble( Settings& settings ) + : Sample( settings ) + { + if ( settings.restart == false ) + { + g_camera.m_center = { 2.3f, 10.0f }; + g_camera.m_zoom = 25.0f * 0.5f; + } + + b2BodyId groundId = b2_nullBodyId; + { + b2BodyDef bodyDef = b2DefaultBodyDef(); + groundId = b2CreateBody( m_worldId, &bodyDef ); + + b2Segment segment = { { -20.0f, 0.0f }, { 20.0f, 0.0f } }; + b2ShapeDef shapeDef = b2DefaultShapeDef(); + b2CreateSegmentShape( groundId, &shapeDef, &segment ); + } + + // Build weeble + { + b2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.type = b2_dynamicBody; + bodyDef.position = { 0.0f, 3.0f }; + bodyDef.rotation = b2MakeRot( 0.25f * b2_pi ); + m_weebleId = b2CreateBody( m_worldId, &bodyDef ); + + b2Capsule capsule = { { 0.0f, -1.0f }, { 0.0f, 1.0f }, 1.0f }; + b2ShapeDef shapeDef = b2DefaultShapeDef(); + shapeDef.density = 1.0f; + b2CreateCapsuleShape( m_weebleId, &shapeDef, &capsule ); + + float mass = b2Body_GetMass( m_weebleId ); + float inertiaTensor = b2Body_GetRotationalInertia( m_weebleId ); + + float offset = 1.5f; + + // See: https://en.wikipedia.org/wiki/Parallel_axis_theorem + inertiaTensor += mass * offset * offset; + + b2MassData massData = { mass, { 0.0f, -offset }, inertiaTensor }; + b2Body_SetMassData( m_weebleId, massData ); + } + + m_explosionPosition = { 0.0f, 0.0f }; + m_explosionRadius = 2.0f; + m_explosionMagnitude = 8.0f; + } + + void UpdateUI() override + { + float height = 120.0f; + ImGui::SetNextWindowPos( ImVec2( 10.0f, g_camera.m_height - height - 50.0f ), ImGuiCond_Once ); + ImGui::SetNextWindowSize( ImVec2( 200.0f, height ) ); + ImGui::Begin( "Weeble", nullptr, ImGuiWindowFlags_NoMove | ImGuiWindowFlags_NoResize ); + if ( ImGui::Button( "Teleport" ) ) + { + b2Body_SetTransform( m_weebleId, { 0.0f, 5.0f }, b2MakeRot( 0.95 * b2_pi ) ); + } + + if ( ImGui::Button( "Explode" ) ) + { + b2World_Explode( m_worldId, m_explosionPosition, m_explosionRadius, m_explosionMagnitude ); + } + ImGui::PushItemWidth( 100.0f ); + + ImGui::SliderFloat( "Magnitude", &m_explosionMagnitude, -100.0f, 100.0f, "%.1f" ); + + ImGui::PopItemWidth(); + ImGui::End(); + } + + void Step( Settings& settings ) override + { + Sample::Step( settings ); + + g_draw.DrawCircle( m_explosionPosition, m_explosionRadius, b2_colorAzure ); + } + + static Sample* Create( Settings& settings ) + { + return new Weeble( settings ); + } + + b2BodyId m_weebleId; + b2Vec2 m_explosionPosition; + float m_explosionRadius; + float m_explosionMagnitude; +}; + +static int sampleWeeble = RegisterSample( "Bodies", "Weeble", Weeble::Create ); + +class Sleep : public Sample +{ +public: + explicit Sleep( Settings& settings ) + : Sample( settings ) + { + if ( settings.restart == false ) + { + g_camera.m_center = { 3.0f, 50.0f }; + g_camera.m_zoom = 25.0f * 2.2f; + } + + b2BodyId groundId = b2_nullBodyId; + { + b2BodyDef bodyDef = b2DefaultBodyDef(); + groundId = b2CreateBody( m_worldId, &bodyDef ); + + b2Segment segment = { { -20.0f, 0.0f }, { 20.0f, 0.0f } }; + b2ShapeDef shapeDef = b2DefaultShapeDef(); + m_groundShapeId = b2CreateSegmentShape( groundId, &shapeDef, &segment ); + } + + // Sleeping body with sensors + for ( int i = 0; i < 2; ++i ) + { + b2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.type = b2_dynamicBody; + bodyDef.position = { -4.0f, 3.0f + 2.0f * i }; + bodyDef.isAwake = false; + bodyDef.enableSleep = true; + b2BodyId bodyId = b2CreateBody( m_worldId, &bodyDef ); + + b2Capsule capsule = { { 0.0f, 1.0f }, { 1.0f, 1.0f }, 0.75f }; + b2ShapeDef shapeDef = b2DefaultShapeDef(); + b2CreateCapsuleShape( bodyId, &shapeDef, &capsule ); + + shapeDef.isSensor = true; + capsule.radius = 1.0f; + m_sensorIds[i] = b2CreateCapsuleShape( bodyId, &shapeDef, &capsule ); + m_sensorTouching[i] = false; + } + + // Sleeping body but sleep is disabled + { + b2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.type = b2_dynamicBody; + bodyDef.position = { 0.0f, 3.0f }; + bodyDef.isAwake = false; + bodyDef.enableSleep = false; + b2BodyId bodyId = b2CreateBody( m_worldId, &bodyDef ); + + b2Circle circle = { { 1.0f, 1.0f }, 1.0f }; + b2ShapeDef shapeDef = b2DefaultShapeDef(); + b2CreateCircleShape( bodyId, &shapeDef, &circle ); + } + + // Awake body and sleep is disabled + { + b2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.type = b2_dynamicBody; + bodyDef.position = { 5.0f, 3.0f }; + bodyDef.isAwake = true; + bodyDef.enableSleep = false; + b2BodyId bodyId = b2CreateBody( m_worldId, &bodyDef ); + + b2Polygon box = b2MakeOffsetBox( 1.0f, 1.0f, { 0.0f, 1.0f }, b2MakeRot(0.25f * b2_pi) ); + b2ShapeDef shapeDef = b2DefaultShapeDef(); + b2CreatePolygonShape( bodyId, &shapeDef, &box ); + } + + // A sleeping body to test waking on collision + { + b2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.type = b2_dynamicBody; + bodyDef.position = { 5.0f, 1.0f }; + bodyDef.isAwake = false; + bodyDef.enableSleep = true; + b2BodyId bodyId = b2CreateBody( m_worldId, &bodyDef ); + + b2Polygon box = b2MakeSquare( 1.0f ); + b2ShapeDef shapeDef = b2DefaultShapeDef(); + b2CreatePolygonShape( bodyId, &shapeDef, &box ); + } + + // A long pendulum + { + b2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.type = b2_dynamicBody; + bodyDef.position = { 0.0f, 100.0f }; + bodyDef.angularDamping = 0.5f; + bodyDef.sleepThreshold = 0.05f; + m_pendulumId = b2CreateBody( m_worldId, &bodyDef ); + + b2Capsule capsule = { { 0.0f, 0.0f }, { 90.0f, 0.0f }, 0.25f }; + b2ShapeDef shapeDef = b2DefaultShapeDef(); + b2CreateCapsuleShape( m_pendulumId, &shapeDef, &capsule ); + + b2Vec2 pivot = bodyDef.position; + b2RevoluteJointDef jointDef = b2DefaultRevoluteJointDef(); + jointDef.bodyIdA = groundId; + jointDef.bodyIdB = m_pendulumId; + jointDef.localAnchorA = b2Body_GetLocalPoint( jointDef.bodyIdA, pivot ); + jointDef.localAnchorB = b2Body_GetLocalPoint( jointDef.bodyIdB, pivot ); + b2CreateRevoluteJoint( m_worldId, &jointDef ); + } + } + + void UpdateUI() override + { + float height = 100.0f; + ImGui::SetNextWindowPos( ImVec2( 10.0f, g_camera.m_height - height - 50.0f ), ImGuiCond_Once ); + ImGui::SetNextWindowSize( ImVec2( 240.0f, height ) ); + ImGui::Begin( "Sleep", nullptr, ImGuiWindowFlags_NoMove | ImGuiWindowFlags_NoResize ); + + ImGui::PushItemWidth( 120.0f ); + + ImGui::Text( "Pendulum Tuning" ); + + float sleepVelocity = b2Body_GetSleepThreshold( m_pendulumId ); + if ( ImGui::SliderFloat( "sleep velocity", &sleepVelocity, 0.0f, 1.0f, "%.2f" ) ) + { + b2Body_SetSleepThreshold( m_pendulumId, sleepVelocity ); + b2Body_SetAwake( m_pendulumId, true ); + } + + float angularDamping = b2Body_GetAngularDamping( m_pendulumId ); + if ( ImGui::SliderFloat( "angular damping", &angularDamping, 0.0f, 2.0f, "%.2f" ) ) + { + b2Body_SetAngularDamping( m_pendulumId, angularDamping ); + } + + ImGui::PopItemWidth(); + + ImGui::End(); + } + + void Step( Settings& settings ) override + { + Sample::Step( settings ); + + // Detect sensors touching the ground + b2SensorEvents sensorEvents = b2World_GetSensorEvents( m_worldId ); + + for ( int i = 0; i < sensorEvents.beginCount; ++i ) + { + b2SensorBeginTouchEvent* event = sensorEvents.beginEvents + i; + if ( B2_ID_EQUALS( event->visitorShapeId, m_groundShapeId ) ) + { + if ( B2_ID_EQUALS( event->sensorShapeId, m_sensorIds[0] ) ) + { + m_sensorTouching[0] = true; + } + else if ( B2_ID_EQUALS( event->sensorShapeId, m_sensorIds[1] ) ) + { + m_sensorTouching[1] = true; + } + } + } + + for ( int i = 0; i < sensorEvents.endCount; ++i ) + { + b2SensorEndTouchEvent* event = sensorEvents.endEvents + i; + if ( B2_ID_EQUALS( event->visitorShapeId, m_groundShapeId ) ) + { + if ( B2_ID_EQUALS( event->sensorShapeId, m_sensorIds[0] ) ) + { + m_sensorTouching[0] = false; + } + else if ( B2_ID_EQUALS( event->sensorShapeId, m_sensorIds[1] ) ) + { + m_sensorTouching[1] = false; + } + } + } + + for ( int i = 0; i < 2; ++i ) + { + g_draw.DrawString( 5, m_textLine, "sensor touch %d = %s", i, m_sensorTouching[i] ? "true" : "false" ); + m_textLine += m_textIncrement; + } + } + + static Sample* Create( Settings& settings ) + { + return new Sleep( settings ); + } + + b2BodyId m_pendulumId; + b2ShapeId m_groundShapeId; + b2ShapeId m_sensorIds[2]; + bool m_sensorTouching[2]; +}; + +static int sampleSleep = RegisterSample( "Bodies", "Sleep", Sleep::Create ); + +class BadBody : public Sample +{ +public: + explicit BadBody( Settings& settings ) + : Sample( settings ) + { + if ( settings.restart == false ) + { + g_camera.m_center = { 2.3f, 10.0f }; + g_camera.m_zoom = 25.0f * 0.5f; + } + + b2BodyId groundId = b2_nullBodyId; + { + b2BodyDef bodyDef = b2DefaultBodyDef(); + groundId = b2CreateBody( m_worldId, &bodyDef ); + + b2Segment segment = { { -20.0f, 0.0f }, { 20.0f, 0.0f } }; + b2ShapeDef shapeDef = b2DefaultShapeDef(); + b2CreateSegmentShape( groundId, &shapeDef, &segment ); + } + + // Build a bad body + { + b2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.type = b2_dynamicBody; + bodyDef.position = { 0.0f, 3.0f }; + bodyDef.angularVelocity = 0.5f; + bodyDef.rotation = b2MakeRot( 0.25f * b2_pi ); + + m_badBodyId = b2CreateBody( m_worldId, &bodyDef ); + + b2Capsule capsule = { { 0.0f, -1.0f }, { 0.0f, 1.0f }, 1.0f }; + b2ShapeDef shapeDef = b2DefaultShapeDef(); + + // density set to zero intentionally to create a bad body + shapeDef.density = 0.0f; + b2CreateCapsuleShape( m_badBodyId, &shapeDef, &capsule ); + } + + // Build a normal body + { + b2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.type = b2_dynamicBody; + bodyDef.position = { 2.0f, 3.0f }; + bodyDef.rotation = b2MakeRot( 0.25f * b2_pi ); + + b2BodyId bodyId = b2CreateBody( m_worldId, &bodyDef ); + + b2Capsule capsule = { { 0.0f, -1.0f }, { 0.0f, 1.0f }, 1.0f }; + b2ShapeDef shapeDef = b2DefaultShapeDef(); + + b2CreateCapsuleShape( bodyId, &shapeDef, &capsule ); + } + } + + void Step( Settings& settings ) override + { + Sample::Step( settings ); + + g_draw.DrawString( 5, m_textLine, "A bad body is a dynamic body with no mass and behaves like a kinematic body." ); + m_textLine += m_textIncrement; + + g_draw.DrawString( 5, m_textLine, "Bad bodies are considered invalid and a user bug. Behavior is not guaranteed." ); + m_textLine += m_textIncrement; + + // For science + b2Body_ApplyForceToCenter( m_badBodyId, { 0.0f, 10.0f }, true ); + } + + static Sample* Create( Settings& settings ) + { + return new BadBody( settings ); + } + + b2BodyId m_badBodyId; +}; + +static int sampleBadBody = RegisterSample( "Bodies", "Bad", BadBody::Create ); diff --git a/tests/cpp-tests/Source/Box2DTestBed/samples/sample_collision.cpp b/tests/cpp-tests/Source/Box2DTestBed/samples/sample_collision.cpp new file mode 100644 index 000000000000..9241152e40f4 --- /dev/null +++ b/tests/cpp-tests/Source/Box2DTestBed/samples/sample_collision.cpp @@ -0,0 +1,3478 @@ +// SPDX-FileCopyrightText: 2023 Erin Catto +// SPDX-License-Identifier: MIT + +#include "draw.h" +#include "sample.h" +#include "settings.h" + +#include "box2d/box2d.h" +#include "box2d/collision.h" +#include "box2d/math_functions.h" + +#include +#include + +constexpr int SIMPLEX_CAPACITY = 20; + +class ShapeDistance : public Sample +{ +public: + enum ShapeType + { + e_point, + e_segment, + e_triangle, + e_box + }; + + explicit ShapeDistance( Settings& settings ) + : Sample( settings ) + { + if ( settings.restart == false ) + { + g_camera.m_center = { 0.0f, 0.0f }; + g_camera.m_zoom = 3.0f; + } + + m_point = b2Vec2_zero; + m_segment = { { -0.5f, 0.0f }, { 0.5f, 0.0f } }; + + { + b2Vec2 points[3] = { { -0.5f, 0.0f }, { 0.5f, 0.0f }, { 0.0f, 1.0f } }; + b2Hull hull = b2ComputeHull( points, 3 ); + m_triangle = b2MakePolygon( &hull, 0.0f ); + } + + m_box = b2MakeBox( 0.5f, 0.5f ); + + m_transform = { { 1.5f, -1.5f }, b2Rot_identity }; + m_angle = 0.0f; + + m_cache = b2_emptyDistanceCache; + m_simplexCount = 0; + m_startPoint = { 0.0f, 0.0f }; + m_basePosition = { 0.0f, 0.0f }; + m_baseAngle = 0.0f; + + m_dragging = false; + m_rotating = false; + m_showIndices = false; + m_useCache = false; + m_drawSimplex = false; + + m_typeA = e_box; + m_typeB = e_box; + m_radiusA = 0.0f; + m_radiusB = 0.0f; + + m_proxyA = MakeProxy( m_typeA, m_radiusA ); + m_proxyB = MakeProxy( m_typeB, m_radiusB ); + } + + b2DistanceProxy MakeProxy( ShapeType type, float radius ) + { + b2DistanceProxy proxy = {}; + proxy.radius = radius; + + switch ( type ) + { + case e_point: + proxy.points[0] = b2Vec2_zero; + proxy.count = 1; + break; + + case e_segment: + proxy.points[0] = m_segment.point1; + proxy.points[1] = m_segment.point2; + proxy.count = 2; + break; + + case e_triangle: + proxy.points[0] = m_triangle.vertices[0]; + proxy.points[1] = m_triangle.vertices[1]; + proxy.points[2] = m_triangle.vertices[2]; + proxy.count = 3; + break; + + case e_box: + proxy.points[0] = m_box.vertices[0]; + proxy.points[1] = m_box.vertices[1]; + proxy.points[2] = m_box.vertices[2]; + proxy.points[3] = m_box.vertices[3]; + proxy.count = 4; + break; + + default: + assert( false ); + } + + return proxy; + } + + void DrawShape( ShapeType type, b2Transform transform, float radius, b2HexColor color ) + { + switch ( type ) + { + case e_point: + { + b2Vec2 p = b2TransformPoint( transform, m_point ); + if ( radius > 0.0f ) + { + g_draw.DrawSolidCircle( transform, m_point, radius, color ); + } + else + { + g_draw.DrawPoint( p, 5.0f, color ); + } + } + break; + + case e_segment: + { + b2Vec2 p1 = b2TransformPoint( transform, m_segment.point1 ); + b2Vec2 p2 = b2TransformPoint( transform, m_segment.point2 ); + + if ( radius > 0.0f ) + { + g_draw.DrawSolidCapsule( p1, p2, radius, color ); + } + else + { + g_draw.DrawSegment( p1, p2, color ); + } + } + break; + + case e_triangle: + g_draw.DrawSolidPolygon( transform, m_triangle.vertices, 3, radius, color ); + break; + + case e_box: + g_draw.DrawSolidPolygon( transform, m_box.vertices, 4, radius, color ); + break; + + default: + assert( false ); + } + } + + void UpdateUI() override + { + float height = 310.0f; + ImGui::SetNextWindowPos( ImVec2( 10.0f, g_camera.m_height - height - 50.0f ), ImGuiCond_Once ); + ImGui::SetNextWindowSize( ImVec2( 240.0f, height ) ); + + ImGui::Begin( "Shape Distance", nullptr, ImGuiWindowFlags_NoMove | ImGuiWindowFlags_NoResize ); + + const char* shapeTypes[] = { "point", "segment", "triangle", "box" }; + int shapeType = int( m_typeA ); + if ( ImGui::Combo( "shape A", &shapeType, shapeTypes, IM_ARRAYSIZE( shapeTypes ) ) ) + { + m_typeA = ShapeType( shapeType ); + m_proxyA = MakeProxy( m_typeA, m_radiusA ); + } + + if ( ImGui::SliderFloat( "radius A", &m_radiusA, 0.0f, 0.5f, "%.2f" ) ) + { + m_proxyA.radius = m_radiusA; + } + + shapeType = int( m_typeB ); + if ( ImGui::Combo( "shape B", &shapeType, shapeTypes, IM_ARRAYSIZE( shapeTypes ) ) ) + { + m_typeB = ShapeType( shapeType ); + m_proxyB = MakeProxy( m_typeB, m_radiusB ); + } + + if ( ImGui::SliderFloat( "radius B", &m_radiusB, 0.0f, 0.5f, "%.2f" ) ) + { + m_proxyB.radius = m_radiusB; + } + + ImGui::Separator(); + + ImGui::SliderFloat( "x offset", &m_transform.p.x, -2.0f, 2.0f, "%.2f" ); + ImGui::SliderFloat( "y offset", &m_transform.p.y, -2.0f, 2.0f, "%.2f" ); + + if ( ImGui::SliderFloat( "angle", &m_angle, -b2_pi, b2_pi, "%.2f" ) ) + { + m_transform.q = b2MakeRot( m_angle ); + } + + ImGui::Separator(); + + ImGui::Checkbox( "show indices", &m_showIndices ); + ImGui::Checkbox( "use cache", &m_useCache ); + + ImGui::Separator(); + + if ( ImGui::Checkbox( "draw simplex", &m_drawSimplex ) ) + { + m_simplexIndex = 0; + } + + if ( m_drawSimplex ) + { + ImGui::SliderInt( "index", &m_simplexIndex, 0, m_simplexCount - 1 ); + m_simplexIndex = b2ClampInt( m_simplexIndex, 0, m_simplexCount - 1 ); + } + + ImGui::End(); + } + + void MouseDown( b2Vec2 p, int button, int mods ) override + { + if ( button == GLFW_MOUSE_BUTTON_1 ) + { + if ( mods == 0 && m_rotating == false ) + { + m_dragging = true; + m_startPoint = p; + m_basePosition = m_transform.p; + } + else if ( mods == GLFW_MOD_SHIFT && m_dragging == false ) + { + m_rotating = true; + m_startPoint = p; + m_baseAngle = m_angle; + } + } + } + + void MouseUp( b2Vec2, int button ) override + { + if ( button == GLFW_MOUSE_BUTTON_1 ) + { + m_dragging = false; + m_rotating = false; + } + } + + void MouseMove( b2Vec2 p ) override + { + if ( m_dragging ) + { + m_transform.p.x = m_basePosition.x + 0.5f * ( p.x - m_startPoint.x ); + m_transform.p.y = m_basePosition.y + 0.5f * ( p.y - m_startPoint.y ); + } + else if ( m_rotating ) + { + float dx = p.x - m_startPoint.x; + m_angle = b2ClampFloat( m_baseAngle + 1.0f * dx, -b2_pi, b2_pi ); + m_transform.q = b2MakeRot( m_angle ); + } + } + + static b2Vec2 Weight2( float a1, b2Vec2 w1, float a2, b2Vec2 w2 ) + { + return { a1 * w1.x + a2 * w2.x, a1 * w1.y + a2 * w2.y }; + } + + static b2Vec2 Weight3( float a1, b2Vec2 w1, float a2, b2Vec2 w2, float a3, b2Vec2 w3 ) + { + return { a1 * w1.x + a2 * w2.x + a3 * w3.x, a1 * w1.y + a2 * w2.y + a3 * w3.y }; + } + + void ComputeSimplexWitnessPoints( b2Vec2* a, b2Vec2* b, const b2Simplex* s ) + { + switch ( s->count ) + { + case 0: + assert( false ); + break; + + case 1: + *a = s->v1.wA; + *b = s->v1.wB; + break; + + case 2: + *a = Weight2( s->v1.a, s->v1.wA, s->v2.a, s->v2.wA ); + *b = Weight2( s->v1.a, s->v1.wB, s->v2.a, s->v2.wB ); + break; + + case 3: + *a = Weight3( s->v1.a, s->v1.wA, s->v2.a, s->v2.wA, s->v3.a, s->v3.wA ); + *b = *a; + break; + + default: + assert( false ); + break; + } + } + + void Step( Settings& ) override + { + b2DistanceInput input; + input.proxyA = m_proxyA; + input.proxyB = m_proxyB; + input.transformA = b2Transform_identity; + input.transformB = m_transform; + input.useRadii = m_radiusA > 0.0f || m_radiusB > 0.0f; + + if ( m_useCache == false ) + { + m_cache.count = 0; + } + + b2DistanceOutput output = b2ShapeDistance( &m_cache, &input, m_simplexes, SIMPLEX_CAPACITY ); + + m_simplexCount = output.simplexCount; + + DrawShape( m_typeA, b2Transform_identity, m_radiusA, b2_colorCyan ); + DrawShape( m_typeB, m_transform, m_radiusB, b2_colorBisque ); + + if ( m_drawSimplex ) + { + b2Simplex* simplex = m_simplexes + m_simplexIndex; + b2SimplexVertex* vertices[3] = { &simplex->v1, &simplex->v2, &simplex->v3 }; + + if ( m_simplexIndex > 0 ) + { + // The first recorded simplex does not have valid barycentric coordinates + b2Vec2 pointA, pointB; + ComputeSimplexWitnessPoints( &pointA, &pointB, simplex ); + + g_draw.DrawSegment( pointA, pointB, b2_colorWhite ); + g_draw.DrawPoint( pointA, 5.0f, b2_colorWhite ); + g_draw.DrawPoint( pointB, 5.0f, b2_colorWhite ); + } + + b2HexColor colors[3] = { b2_colorRed, b2_colorGreen, b2_colorBlue }; + + for ( int i = 0; i < simplex->count; ++i ) + { + b2SimplexVertex* vertex = vertices[i]; + g_draw.DrawPoint( vertex->wA, 5.0f, colors[i] ); + g_draw.DrawPoint( vertex->wB, 5.0f, colors[i] ); + } + } + else + { + g_draw.DrawSegment( output.pointA, output.pointB, b2_colorWhite ); + g_draw.DrawPoint( output.pointA, 5.0f, b2_colorWhite ); + g_draw.DrawPoint( output.pointB, 5.0f, b2_colorWhite ); + } + + if ( m_showIndices ) + { + for ( int i = 0; i < m_proxyA.count; ++i ) + { + b2Vec2 p = m_proxyA.points[i]; + g_draw.DrawString( p, " %d", i ); + } + + for ( int i = 0; i < m_proxyB.count; ++i ) + { + b2Vec2 p = b2TransformPoint( m_transform, m_proxyB.points[i] ); + g_draw.DrawString( p, " %d", i ); + } + } + + g_draw.DrawString( 5, m_textLine, "mouse button 1: drag" ); + m_textLine += m_textIncrement; + g_draw.DrawString( 5, m_textLine, "mouse button 1 + shift: rotate" ); + m_textLine += m_textIncrement; + g_draw.DrawString( 5, m_textLine, "distance = %.2f, iterations = %d", output.distance, output.iterations ); + m_textLine += m_textIncrement; + + if ( m_cache.count == 1 ) + { + g_draw.DrawString( 5, m_textLine, "cache = {%d}, {%d}", m_cache.indexA[0], m_cache.indexB[0] ); + } + else if ( m_cache.count == 2 ) + { + g_draw.DrawString( 5, m_textLine, "cache = {%d, %d}, {%d, %d}", m_cache.indexA[0], m_cache.indexA[1], + m_cache.indexB[0], m_cache.indexB[1] ); + } + else if ( m_cache.count == 3 ) + { + g_draw.DrawString( 5, m_textLine, "cache = {%d, %d, %d}, {%d, %d, %d}", m_cache.indexA[0], m_cache.indexA[1], + m_cache.indexA[2], m_cache.indexB[0], m_cache.indexB[1], m_cache.indexB[2] ); + } + m_textLine += m_textIncrement; + } + + static Sample* Create( Settings& settings ) + { + return new ShapeDistance( settings ); + } + + b2Polygon m_box; + b2Polygon m_triangle; + b2Vec2 m_point; + b2Segment m_segment; + + ShapeType m_typeA; + ShapeType m_typeB; + float m_radiusA; + float m_radiusB; + b2DistanceProxy m_proxyA; + b2DistanceProxy m_proxyB; + + b2DistanceCache m_cache; + b2Simplex m_simplexes[SIMPLEX_CAPACITY]; + int m_simplexCount; + int m_simplexIndex; + + b2Transform m_transform; + float m_angle; + + b2Vec2 m_basePosition; + b2Vec2 m_startPoint; + float m_baseAngle; + + bool m_dragging; + bool m_rotating; + bool m_showIndices; + bool m_useCache; + bool m_drawSimplex; +}; + +static int sampleShapeDistance = RegisterSample( "Collision", "Shape Distance", ShapeDistance::Create ); + +enum UpdateType +{ + Update_Incremental = 0, + Update_FullRebuild = 1, + Update_PartialRebuild = 2, +}; + +struct Proxy +{ + b2AABB box; + b2AABB fatBox; + b2Vec2 position; + b2Vec2 width; + int proxyId; + int rayStamp; + int queryStamp; + bool moved; +}; + +static bool QueryCallback( int32_t proxyId, int32_t userData, void* context ); +static float RayCallback( const b2RayCastInput* input, int32_t proxyId, int32_t userData, void* context ); + +// Tests the Box2D bounding volume hierarchy (BVH). The dynamic tree +// can be used independently as a spatial data structure. +class DynamicTree : public Sample +{ +public: + explicit DynamicTree( Settings& settings ) + : Sample( settings ) + { + if ( settings.restart == false ) + { + g_camera.m_center = { 500.0f, 500.0f }; + g_camera.m_zoom = 25.0f * 21.0f; + } + + m_fill = 0.25f; + m_moveFraction = 0.05f; + m_moveDelta = 0.1f; + m_proxies = nullptr; + m_proxyCount = 0; + m_proxyCapacity = 0; + m_ratio = 5.0f; + m_grid = 1.0f; + + m_moveBuffer = nullptr; + m_moveCount = 0; + + m_rowCount = g_sampleDebug ? 100 : 1000; + m_columnCount = g_sampleDebug ? 100 : 1000; + memset( &m_tree, 0, sizeof( m_tree ) ); + BuildTree(); + m_timeStamp = 0; + m_updateType = Update_Incremental; + + m_startPoint = { 0.0f, 0.0f }; + m_endPoint = { 0.0f, 0.0f }; + m_queryDrag = false; + m_rayDrag = false; + m_validate = true; + } + + ~DynamicTree() override + { + free( m_proxies ); + free( m_moveBuffer ); + b2DynamicTree_Destroy( &m_tree ); + } + + void BuildTree() + { + b2DynamicTree_Destroy( &m_tree ); + free( m_proxies ); + free( m_moveBuffer ); + + m_proxyCapacity = m_rowCount * m_columnCount; + m_proxies = static_cast( malloc( m_proxyCapacity * sizeof( Proxy ) ) ); + m_proxyCount = 0; + + m_moveBuffer = static_cast( malloc( m_proxyCapacity * sizeof( int ) ) ); + m_moveCount = 0; + + float y = -4.0f; + + bool isStatic = false; + m_tree = b2DynamicTree_Create(); + + const b2Vec2 aabbMargin = { 0.1f, 0.1f }; + + for ( int i = 0; i < m_rowCount; ++i ) + { + float x = -40.0f; + + for ( int j = 0; j < m_columnCount; ++j ) + { + float fillTest = RandomFloat( 0.0f, 1.0f ); + if ( fillTest <= m_fill ) + { + assert( m_proxyCount <= m_proxyCapacity ); + Proxy* p = m_proxies + m_proxyCount; + p->position = { x, y }; + + float ratio = RandomFloat( 1.0f, m_ratio ); + float width = RandomFloat( 0.1f, 0.5f ); + if ( RandomFloat() > 0.0f ) + { + p->width.x = ratio * width; + p->width.y = width; + } + else + { + p->width.x = width; + p->width.y = ratio * width; + } + + p->box.lowerBound = { x, y }; + p->box.upperBound = { x + p->width.x, y + p->width.y }; + p->fatBox.lowerBound = b2Sub( p->box.lowerBound, aabbMargin ); + p->fatBox.upperBound = b2Add( p->box.upperBound, aabbMargin ); + + p->proxyId = b2DynamicTree_CreateProxy( &m_tree, p->fatBox, b2_defaultCategoryBits, m_proxyCount ); + p->rayStamp = -1; + p->queryStamp = -1; + p->moved = false; + ++m_proxyCount; + } + + x += m_grid; + } + + y += m_grid; + } + } + + void UpdateUI() override + { + float height = 320.0f; + ImGui::SetNextWindowPos( ImVec2( 10.0f, g_camera.m_height - height - 50.0f ), ImGuiCond_Once ); + ImGui::SetNextWindowSize( ImVec2( 200.0f, height ) ); + + ImGui::Begin( "Dynamic Tree", nullptr, ImGuiWindowFlags_NoMove | ImGuiWindowFlags_NoResize ); + + ImGui::PushItemWidth( 100.0f ); + + bool changed = false; + if ( ImGui::SliderInt( "rows", &m_rowCount, 0, 1000, "%d" ) ) + { + changed = true; + } + + if ( ImGui::SliderInt( "columns", &m_columnCount, 0, 1000, "%d" ) ) + { + changed = true; + } + + if ( ImGui::SliderFloat( "fill", &m_fill, 0.0f, 1.0f, "%.2f" ) ) + { + changed = true; + } + + if ( ImGui::SliderFloat( "grid", &m_grid, 0.5f, 2.0f, "%.2f" ) ) + { + changed = true; + } + + if ( ImGui::SliderFloat( "ratio", &m_ratio, 1.0f, 10.0f, "%.2f" ) ) + { + changed = true; + } + + if ( ImGui::SliderFloat( "move", &m_moveFraction, 0.0f, 1.0f, "%.2f" ) ) + { + } + + if ( ImGui::SliderFloat( "delta", &m_moveDelta, 0.0f, 1.0f, "%.2f" ) ) + { + } + + if ( ImGui::RadioButton( "Incremental", m_updateType == Update_Incremental ) ) + { + m_updateType = Update_Incremental; + changed = true; + } + + if ( ImGui::RadioButton( "Full Rebuild", m_updateType == Update_FullRebuild ) ) + { + m_updateType = Update_FullRebuild; + changed = true; + } + + if ( ImGui::RadioButton( "Partial Rebuild", m_updateType == Update_PartialRebuild ) ) + { + m_updateType = Update_PartialRebuild; + changed = true; + } + + ImGui::Separator(); + + ImGui::Text( "mouse button 1: ray cast" ); + ImGui::Text( "mouse button 1 + shift: query" ); + + ImGui::PopItemWidth(); + ImGui::End(); + + if ( changed ) + { + BuildTree(); + } + } + + void MouseDown( b2Vec2 p, int button, int mods ) override + { + if ( button == GLFW_MOUSE_BUTTON_1 ) + { + if ( mods == 0 && m_queryDrag == false ) + { + m_rayDrag = true; + m_startPoint = p; + m_endPoint = p; + } + else if ( mods == GLFW_MOD_SHIFT && m_rayDrag == false ) + { + m_queryDrag = true; + m_startPoint = p; + m_endPoint = p; + } + } + } + + void MouseUp( b2Vec2, int button ) override + { + if ( button == GLFW_MOUSE_BUTTON_1 ) + { + m_queryDrag = false; + m_rayDrag = false; + } + } + + void MouseMove( b2Vec2 p ) override + { + m_endPoint = p; + } + + void Step( Settings& ) override + { + if ( m_queryDrag ) + { + b2AABB box = { b2Min( m_startPoint, m_endPoint ), b2Max( m_startPoint, m_endPoint ) }; + b2DynamicTree_Query( &m_tree, box, b2_defaultMaskBits, QueryCallback, this ); + + g_draw.DrawAABB( box, b2_colorWhite ); + } + + // m_startPoint = {-1.0f, 0.5f}; + // m_endPoint = {7.0f, 0.5f}; + + if ( m_rayDrag ) + { + b2RayCastInput input = { m_startPoint, b2Sub( m_endPoint, m_startPoint ), 1.0f }; + b2DynamicTree_RayCast( &m_tree, &input, b2_defaultMaskBits, RayCallback, this ); + + g_draw.DrawSegment( m_startPoint, m_endPoint, b2_colorWhite ); + g_draw.DrawPoint( m_startPoint, 5.0f, b2_colorGreen ); + g_draw.DrawPoint( m_endPoint, 5.0f, b2_colorRed ); + } + + b2HexColor c = b2_colorBlue; + b2HexColor qc = b2_colorGreen; + + const b2Vec2 aabbMargin = { 0.1f, 0.1f }; + + for ( int i = 0; i < m_proxyCount; ++i ) + { + Proxy* p = m_proxies + i; + + if ( p->queryStamp == m_timeStamp || p->rayStamp == m_timeStamp ) + { + g_draw.DrawAABB( p->box, qc ); + } + else + { + g_draw.DrawAABB( p->box, c ); + } + + float moveTest = RandomFloat( 0.0f, 1.0f ); + if ( m_moveFraction > moveTest ) + { + float dx = m_moveDelta * RandomFloat(); + float dy = m_moveDelta * RandomFloat(); + + p->position.x += dx; + p->position.y += dy; + + p->box.lowerBound.x = p->position.x + dx; + p->box.lowerBound.y = p->position.y + dy; + p->box.upperBound.x = p->position.x + dx + p->width.x; + p->box.upperBound.y = p->position.y + dy + p->width.y; + + if ( b2AABB_Contains( p->fatBox, p->box ) == false ) + { + p->fatBox.lowerBound = b2Sub( p->box.lowerBound, aabbMargin ); + p->fatBox.upperBound = b2Add( p->box.upperBound, aabbMargin ); + p->moved = true; + } + else + { + p->moved = false; + } + } + else + { + p->moved = false; + } + } + + switch ( m_updateType ) + { + case Update_Incremental: + { + b2Timer timer = b2CreateTimer(); + for ( int i = 0; i < m_proxyCount; ++i ) + { + Proxy* p = m_proxies + i; + if ( p->moved ) + { + b2DynamicTree_MoveProxy( &m_tree, p->proxyId, p->fatBox ); + } + } + float ms = b2GetMilliseconds( &timer ); + g_draw.DrawString( 5, m_textLine, "incremental : %.3f ms", ms ); + m_textLine += m_textIncrement; + } + break; + + case Update_FullRebuild: + { + for ( int i = 0; i < m_proxyCount; ++i ) + { + Proxy* p = m_proxies + i; + if ( p->moved ) + { + b2DynamicTree_EnlargeProxy( &m_tree, p->proxyId, p->fatBox ); + } + } + + b2Timer timer = b2CreateTimer(); + int boxCount = b2DynamicTree_Rebuild( &m_tree, true ); + float ms = b2GetMilliseconds( &timer ); + g_draw.DrawString( 5, m_textLine, "full build %d : %.3f ms", boxCount, ms ); + m_textLine += m_textIncrement; + } + break; + + case Update_PartialRebuild: + { + for ( int i = 0; i < m_proxyCount; ++i ) + { + Proxy* p = m_proxies + i; + if ( p->moved ) + { + b2DynamicTree_EnlargeProxy( &m_tree, p->proxyId, p->fatBox ); + } + } + + b2Timer timer = b2CreateTimer(); + int boxCount = b2DynamicTree_Rebuild( &m_tree, false ); + float ms = b2GetMilliseconds( &timer ); + g_draw.DrawString( 5, m_textLine, "partial rebuild %d : %.3f ms", boxCount, ms ); + m_textLine += m_textIncrement; + } + break; + + default: + break; + } + + int height = b2DynamicTree_GetHeight( &m_tree ); + float areaRatio = b2DynamicTree_GetAreaRatio( &m_tree ); + + int hmin = (int)( ceilf( logf( (float)m_proxyCount ) / logf( 2.0f ) - 1.0f ) ); + g_draw.DrawString( 5, m_textLine, "proxies = %d, height = %d, hmin = %d, area ratio = %.1f", m_proxyCount, height, hmin, + areaRatio ); + m_textLine += m_textIncrement; + + b2DynamicTree_Validate( &m_tree ); + + m_timeStamp += 1; + } + + static Sample* Create( Settings& settings ) + { + return new DynamicTree( settings ); + } + + b2DynamicTree m_tree; + int m_rowCount, m_columnCount; + Proxy* m_proxies; + int* m_moveBuffer; + int m_moveCount; + int m_proxyCapacity; + int m_proxyCount; + int m_timeStamp; + int m_updateType; + float m_fill; + float m_moveFraction; + float m_moveDelta; + float m_ratio; + float m_grid; + + b2Vec2 m_startPoint; + b2Vec2 m_endPoint; + + bool m_rayDrag; + bool m_queryDrag; + bool m_validate; +}; + +static bool QueryCallback( int proxyId, int userData, void* context ) +{ + DynamicTree* sample = static_cast( context ); + Proxy* proxy = sample->m_proxies + userData; + assert( proxy->proxyId == proxyId ); + proxy->queryStamp = sample->m_timeStamp; + return true; +} + +static float RayCallback( const b2RayCastInput* input, int proxyId, int userData, void* context ) +{ + DynamicTree* sample = static_cast( context ); + Proxy* proxy = sample->m_proxies + userData; + assert( proxy->proxyId == proxyId ); + proxy->rayStamp = sample->m_timeStamp; + return input->maxFraction; +} + +static int sampleDynamicTree = RegisterSample( "Collision", "Dynamic Tree", DynamicTree::Create ); + +class RayCast : public Sample +{ +public: + explicit RayCast( Settings& settings ) + : Sample( settings ) + { + if ( settings.restart == false ) + { + g_camera.m_center = { 0.0f, 20.0f }; + g_camera.m_zoom = 17.5f; + } + + m_circle = { { 0.0f, 0.0f }, 2.0f }; + m_capsule = { { -1.0f, 1.0f }, { 1.0f, -1.0f }, 1.5f }; + m_box = b2MakeBox( 2.0f, 2.0f ); + + b2Vec2 vertices[3] = { { -2.0f, 0.0f }, { 2.0f, 0.0f }, { 2.0f, 3.0f } }; + b2Hull hull = b2ComputeHull( vertices, 3 ); + m_triangle = b2MakePolygon( &hull, 0.0f ); + + m_segment = { { -3.0f, 0.0f }, { 3.0f, 0.0 } }; + + m_transform = b2Transform_identity; + m_angle = 0.0f; + + m_basePosition = { 0.0f, 0.0f }; + m_baseAngle = 0.0f; + m_startPosition = { 0.0f, 0.0f }; + + m_rayStart = { 0.0f, 30.0f }; + m_rayEnd = { 0.0f, 0.0f }; + + m_rayDrag = false; + m_translating = false; + m_rotating = false; + + m_showFraction = false; + } + + void UpdateUI() override + { + float height = 230.0f; + ImGui::SetNextWindowPos( ImVec2( 10.0f, g_camera.m_height - height - 50.0f ), ImGuiCond_Once ); + ImGui::SetNextWindowSize( ImVec2( 200.0f, height ) ); + + ImGui::Begin( "Ray-cast", nullptr, ImGuiWindowFlags_NoMove | ImGuiWindowFlags_NoResize ); + + ImGui::PushItemWidth( 100.0f ); + + ImGui::SliderFloat( "x offset", &m_transform.p.x, -2.0f, 2.0f, "%.2f" ); + ImGui::SliderFloat( "y offset", &m_transform.p.y, -2.0f, 2.0f, "%.2f" ); + + if ( ImGui::SliderFloat( "angle", &m_angle, -b2_pi, b2_pi, "%.2f" ) ) + { + m_transform.q = b2MakeRot( m_angle ); + } + + // if (ImGui::SliderFloat("ray radius", &m_rayRadius, 0.0f, 1.0f, "%.1f")) + //{ + // } + + ImGui::Checkbox( "show fraction", &m_showFraction ); + + if ( ImGui::Button( "Reset" ) ) + { + m_transform = b2Transform_identity; + m_angle = 0.0f; + } + + ImGui::Separator(); + + ImGui::Text( "mouse btn 1: ray cast" ); + ImGui::Text( "mouse btn 1 + shft: translate" ); + ImGui::Text( "mouse btn 1 + ctrl: rotate" ); + + ImGui::PopItemWidth(); + + ImGui::End(); + } + + void MouseDown( b2Vec2 p, int button, int mods ) override + { + if ( button == GLFW_MOUSE_BUTTON_1 ) + { + m_startPosition = p; + + if ( mods == 0 ) + { + m_rayStart = p; + m_rayDrag = true; + } + else if ( mods == GLFW_MOD_SHIFT ) + { + m_translating = true; + m_basePosition = m_transform.p; + } + else if ( mods == GLFW_MOD_CONTROL ) + { + m_rotating = true; + m_baseAngle = m_angle; + } + } + } + + void MouseUp( b2Vec2, int button ) override + { + if ( button == GLFW_MOUSE_BUTTON_1 ) + { + m_rayDrag = false; + m_rotating = false; + m_translating = false; + } + } + + void MouseMove( b2Vec2 p ) override + { + if ( m_rayDrag ) + { + m_rayEnd = p; + } + else if ( m_translating ) + { + m_transform.p.x = m_basePosition.x + 0.5f * ( p.x - m_startPosition.x ); + m_transform.p.y = m_basePosition.y + 0.5f * ( p.y - m_startPosition.y ); + } + else if ( m_rotating ) + { + float dx = p.x - m_startPosition.x; + m_angle = b2ClampFloat( m_baseAngle + 0.5f * dx, -b2_pi, b2_pi ); + m_transform.q = b2MakeRot( m_angle ); + } + } + + void DrawRay( const b2CastOutput* output ) + { + b2Vec2 p1 = m_rayStart; + b2Vec2 p2 = m_rayEnd; + b2Vec2 d = b2Sub( p2, p1 ); + + if ( output->hit ) + { + b2Vec2 p = b2MulAdd( p1, output->fraction, d ); + g_draw.DrawSegment( p1, p, b2_colorWhite ); + g_draw.DrawPoint( p1, 5.0f, b2_colorGreen ); + g_draw.DrawPoint( output->point, 5.0f, b2_colorWhite ); + + b2Vec2 n = b2MulAdd( p, 1.0f, output->normal ); + g_draw.DrawSegment( p, n, b2_colorViolet ); + + // if (m_rayRadius > 0.0f) + //{ + // g_draw.DrawCircle(p1, m_rayRadius, b2_colorGreen); + // g_draw.DrawCircle(p, m_rayRadius, b2_colorRed); + // } + + if ( m_showFraction ) + { + b2Vec2 ps = { p.x + 0.05f, p.y - 0.02f }; + g_draw.DrawString( ps, "%.2f", output->fraction ); + } + } + else + { + g_draw.DrawSegment( p1, p2, b2_colorWhite ); + g_draw.DrawPoint( p1, 5.0f, b2_colorGreen ); + g_draw.DrawPoint( p2, 5.0f, b2_colorRed ); + + // if (m_rayRadius > 0.0f) + //{ + // g_draw.DrawCircle(p1, m_rayRadius, b2_colorGreen); + // g_draw.DrawCircle(p2, m_rayRadius, b2_colorRed); + // } + } + } + + void Step( Settings& ) override + { + b2Vec2 offset = { -20.0f, 20.0f }; + b2Vec2 increment = { 10.0f, 0.0f }; + + b2HexColor color1 = b2_colorYellow; + + b2CastOutput output = { 0 }; + float maxFraction = 1.0f; + + // circle + { + b2Transform transform = { b2Add( m_transform.p, offset ), m_transform.q }; + g_draw.DrawSolidCircle( transform, m_circle.center, m_circle.radius, color1 ); + + b2Vec2 start = b2InvTransformPoint( transform, m_rayStart ); + b2Vec2 translation = b2InvRotateVector( transform.q, b2Sub( m_rayEnd, m_rayStart ) ); + b2RayCastInput input = { start, translation, maxFraction }; + + b2CastOutput localOutput = b2RayCastCircle( &input, &m_circle ); + if ( localOutput.hit ) + { + output = localOutput; + output.point = b2TransformPoint( transform, localOutput.point ); + output.normal = b2RotateVector( transform.q, localOutput.normal ); + maxFraction = localOutput.fraction; + } + + offset = b2Add( offset, increment ); + } + + // capsule + { + b2Transform transform = { b2Add( m_transform.p, offset ), m_transform.q }; + b2Vec2 v1 = b2TransformPoint( transform, m_capsule.center1 ); + b2Vec2 v2 = b2TransformPoint( transform, m_capsule.center2 ); + g_draw.DrawSolidCapsule( v1, v2, m_capsule.radius, color1 ); + + b2Vec2 start = b2InvTransformPoint( transform, m_rayStart ); + b2Vec2 translation = b2InvRotateVector( transform.q, b2Sub( m_rayEnd, m_rayStart ) ); + b2RayCastInput input = { start, translation, maxFraction }; + + b2CastOutput localOutput = b2RayCastCapsule( &input, &m_capsule ); + if ( localOutput.hit ) + { + output = localOutput; + output.point = b2TransformPoint( transform, localOutput.point ); + output.normal = b2RotateVector( transform.q, localOutput.normal ); + maxFraction = localOutput.fraction; + } + + offset = b2Add( offset, increment ); + } + + // box + { + b2Transform transform = { b2Add( m_transform.p, offset ), m_transform.q }; + g_draw.DrawSolidPolygon( transform, m_box.vertices, m_box.count, 0.0f, color1 ); + + b2Vec2 start = b2InvTransformPoint( transform, m_rayStart ); + b2Vec2 translation = b2InvRotateVector( transform.q, b2Sub( m_rayEnd, m_rayStart ) ); + b2RayCastInput input = { start, translation, maxFraction }; + + b2CastOutput localOutput = b2RayCastPolygon( &input, &m_box ); + if ( localOutput.hit ) + { + output = localOutput; + output.point = b2TransformPoint( transform, localOutput.point ); + output.normal = b2RotateVector( transform.q, localOutput.normal ); + maxFraction = localOutput.fraction; + } + + offset = b2Add( offset, increment ); + } + + // triangle + { + b2Transform transform = { b2Add( m_transform.p, offset ), m_transform.q }; + g_draw.DrawSolidPolygon( transform, m_triangle.vertices, m_triangle.count, 0.0f, color1 ); + + b2Vec2 start = b2InvTransformPoint( transform, m_rayStart ); + b2Vec2 translation = b2InvRotateVector( transform.q, b2Sub( m_rayEnd, m_rayStart ) ); + b2RayCastInput input = { start, translation, maxFraction }; + + b2CastOutput localOutput = b2RayCastPolygon( &input, &m_triangle ); + if ( localOutput.hit ) + { + output = localOutput; + output.point = b2TransformPoint( transform, localOutput.point ); + output.normal = b2RotateVector( transform.q, localOutput.normal ); + maxFraction = localOutput.fraction; + } + + offset = b2Add( offset, increment ); + } + + // segment + { + b2Transform transform = { b2Add( m_transform.p, offset ), m_transform.q }; + + b2Vec2 p1 = b2TransformPoint( transform, m_segment.point1 ); + b2Vec2 p2 = b2TransformPoint( transform, m_segment.point2 ); + g_draw.DrawSegment( p1, p2, color1 ); + + b2Vec2 start = b2InvTransformPoint( transform, m_rayStart ); + b2Vec2 translation = b2InvRotateVector( transform.q, b2Sub( m_rayEnd, m_rayStart ) ); + b2RayCastInput input = { start, translation, maxFraction }; + + b2CastOutput localOutput = b2RayCastSegment( &input, &m_segment, false ); + if ( localOutput.hit ) + { + output = localOutput; + output.point = b2TransformPoint( transform, localOutput.point ); + output.normal = b2RotateVector( transform.q, localOutput.normal ); + maxFraction = localOutput.fraction; + } + + offset = b2Add( offset, increment ); + } + + DrawRay( &output ); + } + + static Sample* Create( Settings& settings ) + { + return new RayCast( settings ); + } + + b2Polygon m_box; + b2Polygon m_triangle; + b2Circle m_circle; + b2Capsule m_capsule; + b2Segment m_segment; + + b2Transform m_transform; + float m_angle; + + b2Vec2 m_rayStart; + b2Vec2 m_rayEnd; + + b2Vec2 m_basePosition; + float m_baseAngle; + + b2Vec2 m_startPosition; + + bool m_rayDrag; + bool m_translating; + bool m_rotating; + bool m_showFraction; +}; + +static int sampleIndex = RegisterSample( "Collision", "Ray Cast", RayCast::Create ); + +// This shows how to filter a specific shape using using data. +struct ShapeUserData +{ + int index; + bool ignore; +}; + +// Context for ray cast callbacks. Do what you want with this. +struct RayCastContext +{ + b2Vec2 points[3]; + b2Vec2 normals[3]; + float fractions[3]; + int count; +}; + +// This callback finds the closest hit. This is the most common callback used in games. +static float RayCastClosestCallback( b2ShapeId shapeId, b2Vec2 point, b2Vec2 normal, float fraction, void* context ) +{ + RayCastContext* rayContext = (RayCastContext*)context; + + ShapeUserData* userData = (ShapeUserData*)b2Shape_GetUserData( shapeId ); + if ( userData != nullptr && userData->ignore ) + { + // By returning -1, we instruct the calling code to ignore this shape and + // continue the ray-cast to the next shape. + return -1.0f; + } + + rayContext->points[0] = point; + rayContext->normals[0] = normal; + rayContext->fractions[0] = fraction; + rayContext->count = 1; + + // By returning the current fraction, we instruct the calling code to clip the ray and + // continue the ray-cast to the next shape. WARNING: do not assume that shapes + // are reported in order. However, by clipping, we can always get the closest shape. + return fraction; +} + +// This callback finds any hit. For this type of query we are usually just checking for obstruction, +// so the hit data is not relevant. +// NOTE: shape hits are not ordered, so this may not return the closest hit +static float RayCastAnyCallback( b2ShapeId shapeId, b2Vec2 point, b2Vec2 normal, float fraction, void* context ) +{ + RayCastContext* rayContext = (RayCastContext*)context; + + ShapeUserData* userData = (ShapeUserData*)b2Shape_GetUserData( shapeId ); + if ( userData != nullptr && userData->ignore ) + { + // By returning -1, we instruct the calling code to ignore this shape and + // continue the ray-cast to the next shape. + return -1.0f; + } + + rayContext->points[0] = point; + rayContext->normals[0] = normal; + rayContext->fractions[0] = fraction; + rayContext->count = 1; + + // At this point we have a hit, so we know the ray is obstructed. + // By returning 0, we instruct the calling code to terminate the ray-cast. + return 0.0f; +} + +// This ray cast collects multiple hits along the ray. +// The shapes are not necessary reported in order, so we might not capture +// the closest shape. +// NOTE: shape hits are not ordered, so this may return hits in any order. This means that +// if you limit the number of results, you may discard the closest hit. You can see this +// behavior in the sample. +static float RayCastMultipleCallback( b2ShapeId shapeId, b2Vec2 point, b2Vec2 normal, float fraction, void* context ) +{ + RayCastContext* rayContext = (RayCastContext*)context; + + ShapeUserData* userData = (ShapeUserData*)b2Shape_GetUserData( shapeId ); + if ( userData != nullptr && userData->ignore ) + { + // By returning -1, we instruct the calling code to ignore this shape and + // continue the ray-cast to the next shape. + return -1.0f; + } + + int count = rayContext->count; + assert( count < 3 ); + + rayContext->points[count] = point; + rayContext->normals[count] = normal; + rayContext->fractions[count] = fraction; + rayContext->count = count + 1; + + if ( rayContext->count == 3 ) + { + // At this point the buffer is full. + // By returning 0, we instruct the calling code to terminate the ray-cast. + return 0.0f; + } + + // By returning 1, we instruct the caller to continue without clipping the ray. + return 1.0f; +} + +// This ray cast collects multiple hits along the ray and sorts them. +static float RayCastSortedCallback( b2ShapeId shapeId, b2Vec2 point, b2Vec2 normal, float fraction, void* context ) +{ + RayCastContext* rayContext = (RayCastContext*)context; + + ShapeUserData* userData = (ShapeUserData*)b2Shape_GetUserData( shapeId ); + if ( userData != nullptr && userData->ignore ) + { + // By returning -1, we instruct the calling code to ignore this shape and + // continue the ray-cast to the next shape. + return -1.0f; + } + + int count = rayContext->count; + assert( count <= 3 ); + + int index = 3; + while ( fraction < rayContext->fractions[index - 1] ) + { + index -= 1; + + if ( index == 0 ) + { + break; + } + } + + if ( index == 3 ) + { + // not closer, continue but tell the caller not to consider fractions further than the largest fraction acquired + // this only happens once the buffer is full + assert( rayContext->count == 3 ); + assert( rayContext->fractions[2] <= 1.0f ); + return rayContext->fractions[2]; + } + + for ( int j = 2; j > index; --j ) + { + rayContext->points[j] = rayContext->points[j - 1]; + rayContext->normals[j] = rayContext->normals[j - 1]; + rayContext->fractions[j] = rayContext->fractions[j - 1]; + } + + rayContext->points[index] = point; + rayContext->normals[index] = normal; + rayContext->fractions[index] = fraction; + rayContext->count = count < 3 ? count + 1 : 3; + + if ( rayContext->count == 3 ) + { + return rayContext->fractions[2]; + } + + // By returning 1, we instruct the caller to continue without clipping the ray. + return 1.0f; +} + +class RayCastWorld : public Sample +{ +public: + enum Mode + { + e_any = 0, + e_closest = 1, + e_multiple = 2, + e_sorted = 3 + }; + + enum CastType + { + e_rayCast = 0, + e_circleCast = 1, + e_capsuleCast = 2, + e_polygonCast = 3 + }; + + enum + { + e_maxCount = 64 + }; + + explicit RayCastWorld( Settings& settings ) + : Sample( settings ) + { + if ( settings.restart == false ) + { + g_camera.m_center = { 2.0f, 14.0f }; + g_camera.m_zoom = 25.0f * 0.75f; + } + + // Ground body + { + b2BodyDef bodyDef = b2DefaultBodyDef(); + b2BodyId groundId = b2CreateBody( m_worldId, &bodyDef ); + + b2ShapeDef shapeDef = b2DefaultShapeDef(); + b2Segment segment = { { -40.0f, 0.0f }, { 40.0f, 0.0f } }; + b2CreateSegmentShape( groundId, &shapeDef, &segment ); + } + + { + b2Vec2 vertices[3] = { { -0.5f, 0.0f }, { 0.5f, 0.0f }, { 0.0f, 1.5f } }; + b2Hull hull = b2ComputeHull( vertices, 3 ); + m_polygons[0] = b2MakePolygon( &hull, 0.0f ); + } + + { + b2Vec2 vertices[3] = { { -0.1f, 0.0f }, { 0.1f, 0.0f }, { 0.0f, 1.5f } }; + b2Hull hull = b2ComputeHull( vertices, 3 ); + m_polygons[1] = b2MakePolygon( &hull, 0.0f ); + m_polygons[1].radius = 0.5f; + } + + { + float w = 1.0f; + float b = w / ( 2.0f + sqrtf( 2.0f ) ); + float s = sqrtf( 2.0f ) * b; + + b2Vec2 vertices[8] = { { 0.5f * s, 0.0f }, { 0.5f * w, b }, { 0.5f * w, b + s }, { 0.5f * s, w }, + { -0.5f * s, w }, { -0.5f * w, b + s }, { -0.5f * w, b }, { -0.5f * s, 0.0f } }; + + b2Hull hull = b2ComputeHull( vertices, 8 ); + m_polygons[2] = b2MakePolygon( &hull, 0.0f ); + } + + m_polygons[3] = b2MakeBox( 0.5f, 0.5f ); + m_capsule = { { -0.5f, 0.0f }, { 0.5f, 0.0f }, 0.25f }; + m_circle = { { 0.0f, 0.0f }, 0.5f }; + m_segment = { { -1.0f, 0.0f }, { 1.0f, 0.0f } }; + + m_bodyIndex = 0; + + for ( int i = 0; i < e_maxCount; ++i ) + { + m_bodyIds[i] = b2_nullBodyId; + } + + m_mode = e_closest; + m_ignoreIndex = 7; + + m_castType = e_rayCast; + m_castRadius = 0.5f; + + m_rayStart = { -20.0f, 10.0f }; + m_rayEnd = { 20.0f, 10.0f }; + m_dragging = false; + + m_angle = 0.0f; + m_baseAngle = 0.0f; + m_angleAnchor = { 0.0f, 0.0f }; + m_rotating = false; + + m_simple = false; + } + + void Create( int index ) + { + if ( B2_IS_NON_NULL( m_bodyIds[m_bodyIndex] ) ) + { + b2DestroyBody( m_bodyIds[m_bodyIndex] ); + m_bodyIds[m_bodyIndex] = b2_nullBodyId; + } + + float x = RandomFloat( -20.0f, 20.0f ); + float y = RandomFloat( 0.0f, 20.0f ); + + b2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.position = { x, y }; + bodyDef.rotation = b2MakeRot( RandomFloat( -b2_pi, b2_pi ) ); + + m_bodyIds[m_bodyIndex] = b2CreateBody( m_worldId, &bodyDef ); + + b2ShapeDef shapeDef = b2DefaultShapeDef(); + shapeDef.userData = m_userData + m_bodyIndex; + m_userData[m_bodyIndex].ignore = false; + if ( m_bodyIndex == m_ignoreIndex ) + { + m_userData[m_bodyIndex].ignore = true; + } + + if ( index < 4 ) + { + b2CreatePolygonShape( m_bodyIds[m_bodyIndex], &shapeDef, m_polygons + index ); + } + else if ( index == 4 ) + { + b2CreateCircleShape( m_bodyIds[m_bodyIndex], &shapeDef, &m_circle ); + } + else if ( index == 5 ) + { + b2CreateCapsuleShape( m_bodyIds[m_bodyIndex], &shapeDef, &m_capsule ); + } + else + { + b2CreateSegmentShape( m_bodyIds[m_bodyIndex], &shapeDef, &m_segment ); + } + + m_bodyIndex = ( m_bodyIndex + 1 ) % e_maxCount; + } + + void CreateN( int index, int count ) + { + for ( int i = 0; i < count; ++i ) + { + Create( index ); + } + } + + void DestroyBody() + { + for ( int i = 0; i < e_maxCount; ++i ) + { + if ( B2_IS_NON_NULL( m_bodyIds[i] ) ) + { + b2DestroyBody( m_bodyIds[i] ); + m_bodyIds[i] = b2_nullBodyId; + return; + } + } + } + + void MouseDown( b2Vec2 p, int button, int mods ) override + { + if ( button == GLFW_MOUSE_BUTTON_1 ) + { + if ( mods == 0 && m_rotating == false ) + { + m_rayStart = p; + m_rayEnd = p; + m_dragging = true; + } + else if ( mods == GLFW_MOD_SHIFT && m_dragging == false ) + { + m_rotating = true; + m_angleAnchor = p; + m_baseAngle = m_angle; + } + } + } + + void MouseUp( b2Vec2, int button ) override + { + if ( button == GLFW_MOUSE_BUTTON_1 ) + { + m_dragging = false; + m_rotating = false; + } + } + + void MouseMove( b2Vec2 p ) override + { + if ( m_dragging ) + { + m_rayEnd = p; + } + else if ( m_rotating ) + { + float dx = p.x - m_angleAnchor.x; + m_angle = m_baseAngle + 1.0f * dx; + } + } + + void UpdateUI() override + { + float height = 300.0f; + ImGui::SetNextWindowPos( ImVec2( 10.0f, g_camera.m_height - height - 50.0f ), ImGuiCond_Once ); + ImGui::SetNextWindowSize( ImVec2( 200.0f, height ) ); + + ImGui::Begin( "Ray-cast World", nullptr, ImGuiWindowFlags_NoMove | ImGuiWindowFlags_NoResize ); + + ImGui::Checkbox( "Simple", &m_simple ); + + if ( m_simple == false ) + { + const char* castTypes[] = { "Ray", "Circle", "Capsule", "Polygon" }; + int castType = int( m_castType ); + if ( ImGui::Combo( "Type", &castType, castTypes, IM_ARRAYSIZE( castTypes ) ) ) + { + m_castType = CastType( castType ); + } + + if ( m_castType != e_rayCast ) + { + ImGui::SliderFloat( "Radius", &m_castRadius, 0.0f, 2.0f, "%.1f" ); + } + + const char* modes[] = { "Any", "Closest", "Multiple", "Sorted" }; + int mode = int( m_mode ); + if ( ImGui::Combo( "Mode", &mode, modes, IM_ARRAYSIZE( modes ) ) ) + { + m_mode = Mode( mode ); + } + } + + if ( ImGui::Button( "Polygon 1" ) ) + Create( 0 ); + ImGui::SameLine(); + if ( ImGui::Button( "10x##Poly1" ) ) + CreateN( 0, 10 ); + + if ( ImGui::Button( "Polygon 2" ) ) + Create( 1 ); + ImGui::SameLine(); + if ( ImGui::Button( "10x##Poly2" ) ) + CreateN( 1, 10 ); + + if ( ImGui::Button( "Polygon 3" ) ) + Create( 2 ); + ImGui::SameLine(); + if ( ImGui::Button( "10x##Poly3" ) ) + CreateN( 2, 10 ); + + if ( ImGui::Button( "Box" ) ) + Create( 3 ); + ImGui::SameLine(); + if ( ImGui::Button( "10x##Box" ) ) + CreateN( 3, 10 ); + + if ( ImGui::Button( "Circle" ) ) + Create( 4 ); + ImGui::SameLine(); + if ( ImGui::Button( "10x##Circle" ) ) + CreateN( 4, 10 ); + + if ( ImGui::Button( "Capsule" ) ) + Create( 5 ); + ImGui::SameLine(); + if ( ImGui::Button( "10x##Capsule" ) ) + CreateN( 5, 10 ); + + if ( ImGui::Button( "Segment" ) ) + Create( 6 ); + ImGui::SameLine(); + if ( ImGui::Button( "10x##Segment" ) ) + CreateN( 6, 10 ); + + if ( ImGui::Button( "Destroy Shape" ) ) + { + DestroyBody(); + } + + ImGui::End(); + } + + void Step( Settings& settings ) override + { + Sample::Step( settings ); + + g_draw.DrawString( 5, m_textLine, "Click left mouse button and drag to modify ray cast" ); + m_textLine += m_textIncrement; + g_draw.DrawString( 5, m_textLine, "Shape 7 is intentionally ignored by the ray" ); + m_textLine += m_textIncrement; + + m_textLine += m_textIncrement; + + b2HexColor color1 = b2_colorGreen; + b2HexColor color2 = b2_colorGray8; + b2HexColor color3 = b2_colorMagenta; + + b2Vec2 rayTranslation = b2Sub( m_rayEnd, m_rayStart ); + + if ( m_simple ) + { + g_draw.DrawString( 5, m_textLine, "Simple closest point ray cast" ); + m_textLine += m_textIncrement; + + // This version doesn't have a callback, but it doesn't skip the ignored shape + b2RayResult result = b2World_CastRayClosest( m_worldId, m_rayStart, rayTranslation, b2DefaultQueryFilter() ); + + if ( result.hit == true ) + { + b2Vec2 c = b2MulAdd( m_rayStart, result.fraction, rayTranslation ); + g_draw.DrawPoint( result.point, 5.0f, color1 ); + g_draw.DrawSegment( m_rayStart, c, color2 ); + b2Vec2 head = b2MulAdd( result.point, 0.5f, result.normal ); + g_draw.DrawSegment( result.point, head, color3 ); + } + else + { + g_draw.DrawSegment( m_rayStart, m_rayEnd, color2 ); + } + } + else + { + switch ( m_mode ) + { + case e_any: + g_draw.DrawString( 5, m_textLine, "Cast mode: any - check for obstruction - unsorted" ); + break; + + case e_closest: + g_draw.DrawString( 5, m_textLine, "Cast mode: closest - find closest shape along the cast" ); + break; + + case e_multiple: + g_draw.DrawString( 5, m_textLine, "Cast mode: multiple - gather up to 3 shapes - unsorted" ); + break; + + case e_sorted: + g_draw.DrawString( 5, m_textLine, "Cast mode: sorted - gather up to 3 shapes sorted by closeness" ); + break; + } + + m_textLine += m_textIncrement; + + b2CastResultFcn* fcns[] = { RayCastAnyCallback, RayCastClosestCallback, RayCastMultipleCallback, + RayCastSortedCallback }; + b2CastResultFcn* modeFcn = fcns[m_mode]; + + RayCastContext context = { 0 }; + + // Must initialize fractions for sorting + context.fractions[0] = FLT_MAX; + context.fractions[1] = FLT_MAX; + context.fractions[2] = FLT_MAX; + + b2Circle circle = { { 0.0f, 0.0f }, m_castRadius }; + b2Capsule capsule = { { -0.25f, 0.0f }, { 0.25f, 0.0f }, m_castRadius }; + b2Polygon box = b2MakeRoundedBox( 0.25f, 0.5f, m_castRadius ); + b2Transform transform = { m_rayStart, b2MakeRot( m_angle ) }; + + switch ( m_castType ) + { + case e_rayCast: + b2World_CastRay( m_worldId, m_rayStart, rayTranslation, b2DefaultQueryFilter(), modeFcn, &context ); + break; + + case e_circleCast: + b2World_CastCircle( m_worldId, &circle, transform, rayTranslation, b2DefaultQueryFilter(), modeFcn, + &context ); + break; + + case e_capsuleCast: + b2World_CastCapsule( m_worldId, &capsule, transform, rayTranslation, b2DefaultQueryFilter(), modeFcn, + &context ); + break; + + case e_polygonCast: + b2World_CastPolygon( m_worldId, &box, transform, rayTranslation, b2DefaultQueryFilter(), modeFcn, &context ); + break; + } + + if ( context.count > 0 ) + { + assert( context.count <= 3 ); + b2HexColor colors[3] = { b2_colorRed, b2_colorGreen, b2_colorBlue }; + for ( int i = 0; i < context.count; ++i ) + { + b2Vec2 c = b2MulAdd( m_rayStart, context.fractions[i], rayTranslation ); + b2Vec2 p = context.points[i]; + b2Vec2 n = context.normals[i]; + g_draw.DrawPoint( p, 5.0f, colors[i] ); + g_draw.DrawSegment( m_rayStart, c, color2 ); + b2Vec2 head = b2MulAdd( p, 0.5f, n ); + g_draw.DrawSegment( p, head, color3 ); + + b2Vec2 t = b2MulSV( context.fractions[i], rayTranslation ); + b2Transform shiftedTransform = { b2Add( transform.p, t ), transform.q }; + + if ( m_castType == e_circleCast ) + { + g_draw.DrawSolidCircle( shiftedTransform, b2Vec2_zero, m_castRadius, b2_colorYellow ); + } + else if ( m_castType == e_capsuleCast ) + { + b2Vec2 p1 = b2Add( b2TransformPoint( transform, capsule.center1 ), t ); + b2Vec2 p2 = b2Add( b2TransformPoint( transform, capsule.center2 ), t ); + g_draw.DrawSolidCapsule( p1, p2, m_castRadius, b2_colorYellow ); + } + else if ( m_castType == e_polygonCast ) + { + g_draw.DrawSolidPolygon( shiftedTransform, box.vertices, box.count, box.radius, b2_colorYellow ); + } + } + } + else + { + b2Transform shiftedTransform = { b2Add( transform.p, rayTranslation ), transform.q }; + g_draw.DrawSegment( m_rayStart, m_rayEnd, color2 ); + + if ( m_castType == e_circleCast ) + { + g_draw.DrawSolidCircle( shiftedTransform, b2Vec2_zero, m_castRadius, b2_colorGray ); + } + else if ( m_castType == e_capsuleCast ) + { + b2Vec2 p1 = b2Add( b2TransformPoint( transform, capsule.center1 ), rayTranslation ); + b2Vec2 p2 = b2Add( b2TransformPoint( transform, capsule.center2 ), rayTranslation ); + g_draw.DrawSolidCapsule( p1, p2, m_castRadius, b2_colorYellow ); + } + else if ( m_castType == e_polygonCast ) + { + g_draw.DrawSolidPolygon( shiftedTransform, box.vertices, box.count, box.radius, b2_colorYellow ); + } + } + } + + g_draw.DrawPoint( m_rayStart, 5.0f, b2_colorGreen ); + + if ( B2_IS_NON_NULL( m_bodyIds[m_ignoreIndex] ) ) + { + b2Vec2 p = b2Body_GetPosition( m_bodyIds[m_ignoreIndex] ); + p.x -= 0.2f; + g_draw.DrawString( p, "ign" ); + } + } + + static Sample* Create( Settings& settings ) + { + return new RayCastWorld( settings ); + } + + int m_bodyIndex; + b2BodyId m_bodyIds[e_maxCount]; + ShapeUserData m_userData[e_maxCount]; + b2Polygon m_polygons[4]; + b2Capsule m_capsule; + b2Circle m_circle; + b2Segment m_segment; + + bool m_simple; + + int m_mode; + int m_ignoreIndex; + + CastType m_castType; + float m_castRadius; + + b2Vec2 m_angleAnchor; + float m_baseAngle; + float m_angle; + bool m_rotating; + + b2Vec2 m_rayStart; + b2Vec2 m_rayEnd; + bool m_dragging; +}; + +static int sampleRayCastWorld = RegisterSample( "Collision", "Ray Cast World", RayCastWorld::Create ); + +class OverlapWorld : public Sample +{ +public: + enum + { + e_circleShape = 0, + e_capsuleShape = 1, + e_boxShape = 2 + }; + + enum + { + e_maxCount = 64, + e_maxDoomed = 16, + }; + + static bool OverlapResultFcn( b2ShapeId shapeId, void* context ) + { + ShapeUserData* userData = (ShapeUserData*)b2Shape_GetUserData( shapeId ); + if ( userData != nullptr && userData->ignore ) + { + // continue the query + return true; + } + + OverlapWorld* sample = (OverlapWorld*)context; + + if ( sample->m_doomCount < e_maxDoomed ) + { + int index = sample->m_doomCount; + sample->m_doomIds[index] = shapeId; + sample->m_doomCount += 1; + } + + // continue the query + return true; + } + + explicit OverlapWorld( Settings& settings ) + : Sample( settings ) + { + if ( settings.restart == false ) + { + g_camera.m_center = { 0.0f, 10.0f }; + g_camera.m_zoom = 25.0f * 0.7f; + } + + { + b2Vec2 vertices[3] = { { -0.5f, 0.0f }, { 0.5f, 0.0f }, { 0.0f, 1.5f } }; + b2Hull hull = b2ComputeHull( vertices, 3 ); + m_polygons[0] = b2MakePolygon( &hull, 0.0f ); + } + + { + b2Vec2 vertices[3] = { { -0.1f, 0.0f }, { 0.1f, 0.0f }, { 0.0f, 1.5f } }; + b2Hull hull = b2ComputeHull( vertices, 3 ); + m_polygons[1] = b2MakePolygon( &hull, 0.0f ); + } + + { + float w = 1.0f; + float b = w / ( 2.0f + sqrtf( 2.0f ) ); + float s = sqrtf( 2.0f ) * b; + + b2Vec2 vertices[8] = { { 0.5f * s, 0.0f }, { 0.5f * w, b }, { 0.5f * w, b + s }, { 0.5f * s, w }, + { -0.5f * s, w }, { -0.5f * w, b + s }, { -0.5f * w, b }, { -0.5f * s, 0.0f } }; + + b2Hull hull = b2ComputeHull( vertices, 8 ); + m_polygons[2] = b2MakePolygon( &hull, 0.0f ); + } + + m_polygons[3] = b2MakeBox( 0.5f, 0.5f ); + m_capsule = { { -0.5f, 0.0f }, { 0.5f, 0.0f }, 0.25f }; + m_circle = { { 0.0f, 0.0f }, 0.5f }; + m_segment = { { -1.0f, 0.0f }, { 1.0f, 0.0f } }; + + m_bodyIndex = 0; + + for ( int i = 0; i < e_maxCount; ++i ) + { + m_bodyIds[i] = b2_nullBodyId; + } + + m_ignoreIndex = 7; + + m_shapeType = e_circleShape; + + m_queryCircle = { { 0.0f, 0.0f }, 1.0f }; + m_queryCapsule = { { -1.0f, 0.0f }, { 1.0f, 0.0f }, 0.5f }; + m_queryBox = b2MakeBox( 2.0f, 0.5f ); + + m_position = { 0.0f, 10.0f }; + m_angle = 0.0f; + m_dragging = false; + m_rotating = false; + + m_doomCount = 0; + + CreateN( 0, 10 ); + } + + void Create( int index ) + { + if ( B2_IS_NON_NULL( m_bodyIds[m_bodyIndex] ) ) + { + b2DestroyBody( m_bodyIds[m_bodyIndex] ); + m_bodyIds[m_bodyIndex] = b2_nullBodyId; + } + + float x = RandomFloat( -20.0f, 20.0f ); + float y = RandomFloat( 0.0f, 20.0f ); + + b2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.position = { x, y }; + bodyDef.rotation = b2MakeRot( RandomFloat( -b2_pi, b2_pi ) ); + + m_bodyIds[m_bodyIndex] = b2CreateBody( m_worldId, &bodyDef ); + + b2ShapeDef shapeDef = b2DefaultShapeDef(); + shapeDef.userData = m_userData + m_bodyIndex; + m_userData[m_bodyIndex].index = m_bodyIndex; + m_userData[m_bodyIndex].ignore = false; + if ( m_bodyIndex == m_ignoreIndex ) + { + m_userData[m_bodyIndex].ignore = true; + } + + if ( index < 4 ) + { + b2CreatePolygonShape( m_bodyIds[m_bodyIndex], &shapeDef, m_polygons + index ); + } + else if ( index == 4 ) + { + b2CreateCircleShape( m_bodyIds[m_bodyIndex], &shapeDef, &m_circle ); + } + else if ( index == 5 ) + { + b2CreateCapsuleShape( m_bodyIds[m_bodyIndex], &shapeDef, &m_capsule ); + } + else + { + b2CreateSegmentShape( m_bodyIds[m_bodyIndex], &shapeDef, &m_segment ); + } + + m_bodyIndex = ( m_bodyIndex + 1 ) % e_maxCount; + } + + void CreateN( int index, int count ) + { + for ( int i = 0; i < count; ++i ) + { + Create( index ); + } + } + + void DestroyBody() + { + for ( int i = 0; i < e_maxCount; ++i ) + { + if ( B2_IS_NON_NULL( m_bodyIds[i] ) ) + { + b2DestroyBody( m_bodyIds[i] ); + m_bodyIds[i] = b2_nullBodyId; + return; + } + } + } + + void MouseDown( b2Vec2 p, int button, int mods ) override + { + if ( button == GLFW_MOUSE_BUTTON_1 ) + { + if ( mods == 0 && m_rotating == false ) + { + m_dragging = true; + m_position = p; + } + else if ( mods == GLFW_MOD_SHIFT && m_dragging == false ) + { + m_rotating = true; + m_startPosition = p; + m_baseAngle = m_angle; + } + } + } + + void MouseUp( b2Vec2, int button ) override + { + if ( button == GLFW_MOUSE_BUTTON_1 ) + { + m_dragging = false; + m_rotating = false; + } + } + + void MouseMove( b2Vec2 p ) override + { + if ( m_dragging ) + { + m_position = p; + } + else if ( m_rotating ) + { + float dx = p.x - m_startPosition.x; + m_angle = m_baseAngle + 1.0f * dx; + } + } + + void UpdateUI() override + { + float height = 330.0f; + ImGui::SetNextWindowPos( ImVec2( 10.0f, g_camera.m_height - height - 50.0f ), ImGuiCond_Once ); + ImGui::SetNextWindowSize( ImVec2( 140.0f, height ) ); + + ImGui::Begin( "Overlap World", nullptr, ImGuiWindowFlags_NoMove | ImGuiWindowFlags_NoResize ); + + if ( ImGui::Button( "Polygon 1" ) ) + Create( 0 ); + ImGui::SameLine(); + if ( ImGui::Button( "10x##Poly1" ) ) + CreateN( 0, 10 ); + + if ( ImGui::Button( "Polygon 2" ) ) + Create( 1 ); + ImGui::SameLine(); + if ( ImGui::Button( "10x##Poly2" ) ) + CreateN( 1, 10 ); + + if ( ImGui::Button( "Polygon 3" ) ) + Create( 2 ); + ImGui::SameLine(); + if ( ImGui::Button( "10x##Poly3" ) ) + CreateN( 2, 10 ); + + if ( ImGui::Button( "Box" ) ) + Create( 3 ); + ImGui::SameLine(); + if ( ImGui::Button( "10x##Box" ) ) + CreateN( 3, 10 ); + + if ( ImGui::Button( "Circle" ) ) + Create( 4 ); + ImGui::SameLine(); + if ( ImGui::Button( "10x##Circle" ) ) + CreateN( 4, 10 ); + + if ( ImGui::Button( "Capsule" ) ) + Create( 5 ); + ImGui::SameLine(); + if ( ImGui::Button( "10x##Capsule" ) ) + CreateN( 5, 10 ); + + if ( ImGui::Button( "Segment" ) ) + Create( 6 ); + ImGui::SameLine(); + if ( ImGui::Button( "10x##Segment" ) ) + CreateN( 6, 10 ); + + if ( ImGui::Button( "Destroy Shape" ) ) + { + DestroyBody(); + } + + ImGui::Separator(); + ImGui::Text( "Overlap Shape" ); + ImGui::RadioButton( "Circle##Overlap", &m_shapeType, e_circleShape ); + ImGui::RadioButton( "Capsule##Overlap", &m_shapeType, e_capsuleShape ); + ImGui::RadioButton( "Box##Overlap", &m_shapeType, e_boxShape ); + + ImGui::End(); + } + + void Step( Settings& settings ) override + { + Sample::Step( settings ); + + g_draw.DrawString( 5, m_textLine, "left mouse button: drag query shape" ); + m_textLine += m_textIncrement; + g_draw.DrawString( 5, m_textLine, "left mouse button + shift: rotate query shape" ); + m_textLine += m_textIncrement; + + m_doomCount = 0; + + b2Transform transform = { m_position, b2MakeRot( m_angle ) }; + + if ( m_shapeType == e_circleShape ) + { + b2World_OverlapCircle( m_worldId, &m_queryCircle, transform, b2DefaultQueryFilter(), OverlapWorld::OverlapResultFcn, + this ); + g_draw.DrawSolidCircle( transform, b2Vec2_zero, m_queryCircle.radius, b2_colorWhite ); + } + else if ( m_shapeType == e_capsuleShape ) + { + b2World_OverlapCapsule( m_worldId, &m_queryCapsule, transform, b2DefaultQueryFilter(), OverlapWorld::OverlapResultFcn, + this ); + b2Vec2 p1 = b2TransformPoint( transform, m_queryCapsule.center1 ); + b2Vec2 p2 = b2TransformPoint( transform, m_queryCapsule.center2 ); + g_draw.DrawSolidCapsule( p1, p2, m_queryCapsule.radius, b2_colorWhite ); + } + else if ( m_shapeType == e_boxShape ) + { + b2World_OverlapPolygon( m_worldId, &m_queryBox, transform, b2DefaultQueryFilter(), OverlapWorld::OverlapResultFcn, + this ); + b2Vec2 points[b2_maxPolygonVertices] = { 0 }; + for ( int i = 0; i < m_queryBox.count; ++i ) + { + points[i] = b2TransformPoint( transform, m_queryBox.vertices[i] ); + } + g_draw.DrawPolygon( points, m_queryBox.count, b2_colorWhite ); + } + + if ( B2_IS_NON_NULL( m_bodyIds[m_ignoreIndex] ) ) + { + b2Vec2 p = b2Body_GetPosition( m_bodyIds[m_ignoreIndex] ); + p.x -= 0.2f; + g_draw.DrawString( p, "skip" ); + } + + for ( int i = 0; i < m_doomCount; ++i ) + { + b2ShapeId shapeId = m_doomIds[i]; + ShapeUserData* userData = (ShapeUserData*)b2Shape_GetUserData( shapeId ); + if ( userData == nullptr ) + { + continue; + } + + int index = userData->index; + assert( 0 <= index && index < e_maxCount ); + assert( B2_IS_NON_NULL( m_bodyIds[index] ) ); + + b2DestroyBody( m_bodyIds[index] ); + m_bodyIds[index] = b2_nullBodyId; + } + } + + static Sample* Create( Settings& settings ) + { + return new OverlapWorld( settings ); + } + + int m_bodyIndex; + b2BodyId m_bodyIds[e_maxCount]; + ShapeUserData m_userData[e_maxCount]; + b2Polygon m_polygons[4]; + b2Capsule m_capsule; + b2Circle m_circle; + b2Segment m_segment; + int m_ignoreIndex; + + b2ShapeId m_doomIds[e_maxDoomed]; + int m_doomCount; + + b2Circle m_queryCircle; + b2Capsule m_queryCapsule; + b2Polygon m_queryBox; + + int m_shapeType; + b2Transform m_transform; + + b2Vec2 m_startPosition; + + b2Vec2 m_position; + b2Vec2 m_basePosition; + float m_angle; + float m_baseAngle; + + bool m_dragging; + bool m_rotating; +}; + +static int sampleOverlapWorld = RegisterSample( "Collision", "Overlap World", OverlapWorld::Create ); + +// Tests manifolds and contact points +class Manifold : public Sample +{ +public: + explicit Manifold( Settings& settings ) + : Sample( settings ) + { + if ( settings.restart == false ) + { + // g_camera.m_center = {1.8f, 15.0f}; + g_camera.m_center = { 1.8f, 0.0f }; + g_camera.m_zoom = 25.0f * 0.45f; + } + + m_smgroxCache1 = b2_emptyDistanceCache; + m_smgroxCache2 = b2_emptyDistanceCache; + m_smgcapCache1 = b2_emptyDistanceCache; + m_smgcapCache2 = b2_emptyDistanceCache; + + m_transform = b2Transform_identity; + m_transform.p.x = 1.0f; + m_transform.p.y = 0.0f; + //m_transform.q = b2MakeRot( 0.5f * b2_pi ); + m_angle = 0.0f; + m_round = 0.1f; + + m_startPoint = { 0.0f, 0.0f }; + m_basePosition = { 0.0f, 0.0f }; + m_baseAngle = 0.0f; + + m_dragging = false; + m_rotating = false; + m_showIds = false; + m_showSeparation = false; + m_showAnchors = false; + m_enableCaching = true; + + b2Vec2 points[3] = { { -0.1f, -0.5f }, { 0.1f, -0.5f }, { 0.0f, 0.5f } }; + m_wedge = b2ComputeHull( points, 3 ); + } + + void UpdateUI() override + { + float height = 300.0f; + ImGui::SetNextWindowPos( ImVec2( 10.0f, g_camera.m_height - height - 50.0f ), ImGuiCond_Once ); + ImGui::SetNextWindowSize( ImVec2( 240.0f, height ) ); + + ImGui::Begin( "Manifold", nullptr, ImGuiWindowFlags_NoMove | ImGuiWindowFlags_NoResize ); + + ImGui::SliderFloat( "x offset", &m_transform.p.x, -2.0f, 2.0f, "%.2f" ); + ImGui::SliderFloat( "y offset", &m_transform.p.y, -2.0f, 2.0f, "%.2f" ); + + if ( ImGui::SliderFloat( "angle", &m_angle, -b2_pi, b2_pi, "%.2f" ) ) + { + m_transform.q = b2MakeRot( m_angle ); + } + + ImGui::SliderFloat( "round", &m_round, 0.0f, 0.4f, "%.1f" ); + ImGui::Checkbox( "show ids", &m_showIds ); + ImGui::Checkbox( "show separation", &m_showSeparation ); + ImGui::Checkbox( "show anchors", &m_showAnchors ); + ImGui::Checkbox( "enable caching", &m_enableCaching ); + + if ( ImGui::Button( "Reset" ) ) + { + m_transform = b2Transform_identity; + m_angle = 0.0f; + } + + ImGui::Separator(); + + ImGui::Text( "mouse button 1: drag" ); + ImGui::Text( "mouse button 1 + shift: rotate" ); + + ImGui::End(); + } + + void MouseDown( b2Vec2 p, int button, int mods ) override + { + if ( button == GLFW_MOUSE_BUTTON_1 ) + { + if ( mods == 0 && m_rotating == false ) + { + m_dragging = true; + m_startPoint = p; + m_basePosition = m_transform.p; + } + else if ( mods == GLFW_MOD_SHIFT && m_dragging == false ) + { + m_rotating = true; + m_startPoint = p; + m_baseAngle = m_angle; + } + } + } + + void MouseUp( b2Vec2, int button ) override + { + if ( button == GLFW_MOUSE_BUTTON_1 ) + { + m_dragging = false; + m_rotating = false; + } + } + + void MouseMove( b2Vec2 p ) override + { + if ( m_dragging ) + { + m_transform.p.x = m_basePosition.x + 0.5f * ( p.x - m_startPoint.x ); + m_transform.p.y = m_basePosition.y + 0.5f * ( p.y - m_startPoint.y ); + } + else if ( m_rotating ) + { + float dx = p.x - m_startPoint.x; + m_angle = b2ClampFloat( m_baseAngle + 1.0f * dx, -b2_pi, b2_pi ); + m_transform.q = b2MakeRot( m_angle ); + } + } + + void DrawManifold( const b2Manifold* manifold, b2Vec2 origin1, b2Vec2 origin2 ) + { + for ( int i = 0; i < manifold->pointCount; ++i ) + { + const b2ManifoldPoint* mp = manifold->points + i; + + b2Vec2 p1 = mp->point; + b2Vec2 p2 = b2MulAdd( p1, 0.5f, manifold->normal ); + g_draw.DrawSegment( p1, p2, b2_colorWhite ); + + if ( m_showAnchors ) + { + g_draw.DrawPoint( b2Add( origin1, mp->anchorA ), 5.0f, b2_colorRed ); + g_draw.DrawPoint( b2Add( origin2, mp->anchorB ), 5.0f, b2_colorGreen ); + } + else + { + g_draw.DrawPoint( p1, 5.0f, b2_colorBlue ); + } + + if ( m_showIds ) + { + // uint32_t indexA = mp->id >> 8; + // uint32_t indexB = 0xFF & mp->id; + b2Vec2 p = { p1.x + 0.05f, p1.y - 0.02f }; + g_draw.DrawString( p, "0x%04x", mp->id ); + } + + if ( m_showSeparation ) + { + b2Vec2 p = { p1.x + 0.05f, p1.y + 0.03f }; + g_draw.DrawString( p, "%.3f", mp->separation ); + } + } + } + + void Step( Settings& ) override + { + b2Vec2 offset = { -10.0f, -5.0f }; + b2Vec2 increment = { 4.0f, 0.0f }; + + b2HexColor color1 = b2_colorAquamarine; + b2HexColor color2 = b2_colorPaleGoldenrod; + + if ( m_enableCaching == false ) + { + m_smgroxCache1 = b2_emptyDistanceCache; + m_smgroxCache2 = b2_emptyDistanceCache; + m_smgcapCache1 = b2_emptyDistanceCache; + m_smgcapCache2 = b2_emptyDistanceCache; + } + + // circle-circle + { + b2Circle circle1 = { { 0.0f, 0.0f }, 0.5f }; + b2Circle circle2 = { { 0.0f, 0.0f }, 1.0f }; + + b2Transform transform1 = { offset, b2Rot_identity }; + b2Transform transform2 = { b2Add( m_transform.p, offset ), m_transform.q }; + + b2Manifold m = b2CollideCircles( &circle1, transform1, &circle2, transform2 ); + + g_draw.DrawSolidCircle( transform1, circle1.center, circle1.radius, color1 ); + g_draw.DrawSolidCircle( transform2, circle2.center, circle2.radius, color2 ); + + DrawManifold( &m, transform1.p, transform2.p ); + + offset = b2Add( offset, increment ); + } + + // capsule-circle + { + b2Capsule capsule = { { -0.5f, 0.0f }, { 0.5f, 0.0 }, 0.25f }; + b2Circle circle = { { 0.0f, 0.0f }, 0.5f }; + + b2Transform transform1 = { offset, b2Rot_identity }; + b2Transform transform2 = { b2Add( m_transform.p, offset ), m_transform.q }; + + b2Manifold m = b2CollideCapsuleAndCircle( &capsule, transform1, &circle, transform2 ); + + b2Vec2 v1 = b2TransformPoint( transform1, capsule.center1 ); + b2Vec2 v2 = b2TransformPoint( transform1, capsule.center2 ); + g_draw.DrawSolidCapsule( v1, v2, capsule.radius, color1 ); + + g_draw.DrawSolidCircle( transform2, circle.center, circle.radius, color2 ); + + DrawManifold( &m, transform1.p, transform2.p ); + + offset = b2Add( offset, increment ); + } + + // segment-circle + { + b2Segment segment = { { -1.0f, 0.0f }, { 1.0f, 0.0 } }; + b2Circle circle = { { 0.0f, 0.0f }, 0.5f }; + + b2Transform transform1 = { offset, b2Rot_identity }; + b2Transform transform2 = { b2Add( m_transform.p, offset ), m_transform.q }; + + b2Manifold m = b2CollideSegmentAndCircle( &segment, transform1, &circle, transform2 ); + + b2Vec2 p1 = b2TransformPoint( transform1, segment.point1 ); + b2Vec2 p2 = b2TransformPoint( transform1, segment.point2 ); + g_draw.DrawSegment( p1, p2, color1 ); + + g_draw.DrawSolidCircle( transform2, circle.center, circle.radius, color2 ); + + DrawManifold( &m, transform1.p, transform2.p ); + + offset = b2Add( offset, increment ); + } + + // box-circle + { + b2Circle circle = { { 0.0f, 0.0f }, 0.5f }; + b2Polygon box = b2MakeSquare( 0.5f ); + box.radius = m_round; + + b2Transform transform1 = { offset, b2Rot_identity }; + b2Transform transform2 = { b2Add( m_transform.p, offset ), m_transform.q }; + + b2Manifold m = b2CollidePolygonAndCircle( &box, transform1, &circle, transform2 ); + + g_draw.DrawSolidPolygon( transform1, box.vertices, box.count, m_round, color1 ); + g_draw.DrawSolidCircle( transform2, circle.center, circle.radius, color2 ); + + DrawManifold( &m, transform1.p, transform2.p ); + + offset = b2Add( offset, increment ); + } + + // capsule-capsule + { + b2Capsule capsule1 = { { -0.5f, 0.0f }, { 0.5f, 0.0 }, 0.25f }; + b2Capsule capsule2 = { { 0.25f, 0.0f }, { 1.0f, 0.0 }, 0.1f }; + + b2Transform transform1 = { offset, b2Rot_identity }; + b2Transform transform2 = { b2Add( m_transform.p, offset ), m_transform.q }; + + b2Manifold m = b2CollideCapsules( &capsule1, transform1, &capsule2, transform2 ); + + b2Vec2 v1 = b2TransformPoint( transform1, capsule1.center1 ); + b2Vec2 v2 = b2TransformPoint( transform1, capsule1.center2 ); + g_draw.DrawSolidCapsule( v1, v2, capsule1.radius, color1 ); + + v1 = b2TransformPoint( transform2, capsule2.center1 ); + v2 = b2TransformPoint( transform2, capsule2.center2 ); + g_draw.DrawSolidCapsule( v1, v2, capsule2.radius, color2 ); + + DrawManifold( &m, transform1.p, transform2.p ); + + offset = b2Add( offset, increment ); + } + + // box-capsule + { + b2Capsule capsule = { { -0.4f, 0.0f }, { -0.1f, 0.0f }, 0.1f }; + b2Polygon box = b2MakeOffsetBox( 0.25f, 1.0f, { 1.0f, -1.0f }, b2MakeRot(0.25f * b2_pi) ); + + b2Transform transform1 = { offset, b2Rot_identity }; + b2Transform transform2 = { b2Add( m_transform.p, offset ), m_transform.q }; + + b2Manifold m = b2CollidePolygonAndCapsule( &box, transform1, &capsule, transform2 ); + + g_draw.DrawSolidPolygon( transform1, box.vertices, box.count, box.radius, color1 ); + + b2Vec2 v1 = b2TransformPoint( transform2, capsule.center1 ); + b2Vec2 v2 = b2TransformPoint( transform2, capsule.center2 ); + g_draw.DrawSolidCapsule( v1, v2, capsule.radius, color2 ); + + DrawManifold( &m, transform1.p, transform2.p ); + + offset = b2Add( offset, increment ); + } + + // segment-capsule + { + b2Segment segment = { { -1.0f, 0.0f }, { 1.0f, 0.0 } }; + b2Capsule capsule = { { -0.5f, 0.0f }, { 0.5f, 0.0 }, 0.25f }; + + b2Transform transform1 = { offset, b2Rot_identity }; + b2Transform transform2 = { b2Add( m_transform.p, offset ), m_transform.q }; + + b2Manifold m = b2CollideSegmentAndCapsule( &segment, transform1, &capsule, transform2 ); + + b2Vec2 p1 = b2TransformPoint( transform1, segment.point1 ); + b2Vec2 p2 = b2TransformPoint( transform1, segment.point2 ); + g_draw.DrawSegment( p1, p2, color1 ); + + p1 = b2TransformPoint( transform2, capsule.center1 ); + p2 = b2TransformPoint( transform2, capsule.center2 ); + g_draw.DrawSolidCapsule( p1, p2, capsule.radius, color2 ); + + DrawManifold( &m, transform1.p, transform2.p ); + + offset = b2Add( offset, increment ); + } + + offset = { -10.0f, 0.0f }; + + // box-box + { + b2Polygon box1 = b2MakeBox( 2.0f, 0.1f ); + b2Polygon box = b2MakeSquare( 0.25f ); + + b2Transform transform1 = { offset, b2Rot_identity }; + b2Transform transform2 = { b2Add( m_transform.p, offset ), m_transform.q }; + // b2Transform transform2 = {b2Add({0.0f, -0.1f}, offset), {0.0f, 1.0f}}; + + b2Manifold m = b2CollidePolygons( &box1, transform1, &box, transform2 ); + + g_draw.DrawSolidPolygon( transform1, box1.vertices, box1.count, box1.radius, color1 ); + g_draw.DrawSolidPolygon( transform2, box.vertices, box.count, box.radius, color2 ); + + DrawManifold( &m, transform1.p, transform2.p ); + + offset = b2Add( offset, increment ); + } + + // box-rox + { + b2Polygon box = b2MakeSquare( 0.5f ); + float h = 0.5f - m_round; + b2Polygon rox = b2MakeRoundedBox( h, h, m_round ); + + b2Transform transform1 = { offset, b2Rot_identity }; + b2Transform transform2 = { b2Add( m_transform.p, offset ), m_transform.q }; + // b2Transform transform2 = {b2Add({0.0f, -0.1f}, offset), {0.0f, 1.0f}}; + + b2Manifold m = b2CollidePolygons( &box, transform1, &rox, transform2 ); + + g_draw.DrawSolidPolygon( transform1, box.vertices, box.count, box.radius, color1 ); + g_draw.DrawSolidPolygon( transform2, rox.vertices, rox.count, rox.radius, color2 ); + + DrawManifold( &m, transform1.p, transform2.p ); + + offset = b2Add( offset, increment ); + } + + // rox-rox + { + float h = 0.5f - m_round; + b2Polygon rox = b2MakeRoundedBox( h, h, m_round ); + + b2Transform transform1 = { offset, b2Rot_identity }; + b2Transform transform2 = { b2Add( m_transform.p, offset ), m_transform.q }; + // b2Transform transform1 = {{6.48024225f, 2.07872653f}, {-0.938356698f, 0.345668465f}}; + // b2Transform transform2 = {{5.52862263f, 2.51146317f}, {-0.859374702f, -0.511346340f}}; + + b2Manifold m = b2CollidePolygons( &rox, transform1, &rox, transform2 ); + + g_draw.DrawSolidPolygon( transform1, rox.vertices, rox.count, rox.radius, color1 ); + g_draw.DrawSolidPolygon( transform2, rox.vertices, rox.count, rox.radius, color2 ); + + DrawManifold( &m, transform1.p, transform2.p ); + + offset = b2Add( offset, increment ); + } + + // segment-rox + { + b2Segment segment = { { -1.0f, 0.0f }, { 1.0f, 0.0 } }; + float h = 0.5f - m_round; + b2Polygon rox = b2MakeRoundedBox( h, h, m_round ); + + b2Transform transform1 = { offset, b2Rot_identity }; + b2Transform transform2 = { b2Add( m_transform.p, offset ), m_transform.q }; + // b2Transform transform2 = {b2Add({-1.44583416f, 0.397352695f}, offset), m_transform.q}; + + b2Manifold m = b2CollideSegmentAndPolygon( &segment, transform1, &rox, transform2 ); + + b2Vec2 p1 = b2TransformPoint( transform1, segment.point1 ); + b2Vec2 p2 = b2TransformPoint( transform1, segment.point2 ); + g_draw.DrawSegment( p1, p2, color1 ); + g_draw.DrawSolidPolygon( transform2, rox.vertices, rox.count, rox.radius, color2 ); + + DrawManifold( &m, transform1.p, transform2.p ); + + offset = b2Add( offset, increment ); + } + + // wox-wox + { + b2Polygon wox = b2MakePolygon( &m_wedge, m_round ); + + b2Transform transform1 = { offset, b2Rot_identity }; + b2Transform transform2 = { b2Add( m_transform.p, offset ), m_transform.q }; + // b2Transform transform2 = {b2Add({0.0f, -0.1f}, offset), {0.0f, 1.0f}}; + + b2Manifold m = b2CollidePolygons( &wox, transform1, &wox, transform2 ); + + g_draw.DrawSolidPolygon( transform1, wox.vertices, wox.count, wox.radius, color1 ); + g_draw.DrawSolidPolygon( transform1, wox.vertices, wox.count, 0.0f, color1 ); + g_draw.DrawSolidPolygon( transform2, wox.vertices, wox.count, wox.radius, color2 ); + g_draw.DrawSolidPolygon( transform2, wox.vertices, wox.count, 0.0f, color2 ); + + DrawManifold( &m, transform1.p, transform2.p ); + + offset = b2Add( offset, increment ); + } + + // wox-wox + { + b2Vec2 p1s[3] = { { 0.175740838, 0.224936664 }, { -0.301293969, 0.194021404 }, { -0.105151534, -0.432157338 } }; + b2Vec2 p2s[3] = { { -0.427884758, -0.225028217 }, { 0.0566576123, -0.128772855 }, { 0.176625848, 0.338923335 } }; + + b2Hull h1 = b2ComputeHull( p1s, 3 ); + b2Hull h2 = b2ComputeHull( p2s, 3 ); + b2Polygon w1 = b2MakePolygon( &h1, 0.158798501 ); + b2Polygon w2 = b2MakePolygon( &h2, 0.205900759 ); + + b2Transform transform1 = { offset, b2Rot_identity }; + b2Transform transform2 = { b2Add( m_transform.p, offset ), m_transform.q }; + // b2Transform transform2 = {b2Add({0.0f, -0.1f}, offset), {0.0f, 1.0f}}; + + b2Manifold m = b2CollidePolygons( &w1, transform1, &w2, transform2 ); + + g_draw.DrawSolidPolygon( transform1, w1.vertices, w1.count, w1.radius, color1 ); + g_draw.DrawSolidPolygon( transform1, w1.vertices, w1.count, 0.0f, color1 ); + g_draw.DrawSolidPolygon( transform2, w2.vertices, w2.count, w2.radius, color2 ); + g_draw.DrawSolidPolygon( transform2, w2.vertices, w2.count, 0.0f, color2 ); + + DrawManifold( &m, transform1.p, transform2.p ); + + offset = b2Add( offset, increment ); + } + + offset = { -10.0f, 5.0f }; + + // chain-segment vs circle + { + b2ChainSegment segment = { { 2.0f, 1.0f }, { { 1.0f, 1.0f }, { -1.0f, 0.0f } }, { -2.0f, 0.0f }, -1 }; + b2Circle circle = { { 0.0f, 0.0f }, 0.5f }; + + b2Transform transform1 = { offset, b2Rot_identity }; + b2Transform transform2 = { b2Add( m_transform.p, offset ), m_transform.q }; + + b2Manifold m = b2CollideChainSegmentAndCircle( &segment, transform1, &circle, transform2 ); + + b2Vec2 g1 = b2TransformPoint( transform1, segment.ghost1 ); + b2Vec2 g2 = b2TransformPoint( transform1, segment.ghost2 ); + b2Vec2 p1 = b2TransformPoint( transform1, segment.segment.point1 ); + b2Vec2 p2 = b2TransformPoint( transform1, segment.segment.point2 ); + g_draw.DrawSegment( g1, p1, b2_colorLightGray ); + g_draw.DrawSegment( p1, p2, color1 ); + g_draw.DrawSegment( p2, g2, b2_colorLightGray ); + g_draw.DrawSolidCircle( transform2, circle.center, circle.radius, color2 ); + + DrawManifold( &m, transform1.p, transform2.p ); + + offset.x += 2.0f * increment.x; + } + + // chain-segment vs rounded polygon + { + b2ChainSegment segment1 = { { 2.0f, 1.0f }, { { 1.0f, 1.0f }, { -1.0f, 0.0f } }, { -2.0f, 0.0f }, -1 }; + b2ChainSegment segment2 = { { 3.0f, 1.0f }, { { 2.0f, 1.0f }, { 1.0f, 1.0f } }, { -1.0f, 0.0f }, -1 }; + // b2ChainSegment segment1 = {{2.0f, 0.0f}, {{1.0f, 0.0f}, {-1.0f, 0.0f}}, {-2.0f, 0.0f}, -1}; + // b2ChainSegment segment2 = {{3.0f, 0.0f}, {{2.0f, 0.0f}, {1.0f, 0.0f}}, {-1.0f, 0.0f}, -1}; + // b2ChainSegment segment1 = {{0.5f, 1.0f}, {{0.0f, 2.0f}, {-0.5f, 1.0f}}, {-1.0f, 0.0f}, -1}; + // b2ChainSegment segment2 = {{1.0f, 0.0f}, {{0.5f, 1.0f}, {0.0f, 2.0f}}, {-0.5f, 1.0f}, -1}; + float h = 0.5f - m_round; + b2Polygon rox = b2MakeRoundedBox( h, h, m_round ); + + b2Transform transform1 = { offset, b2Rot_identity }; + b2Transform transform2 = { b2Add( m_transform.p, offset ), m_transform.q }; + + b2Manifold m1 = b2CollideChainSegmentAndPolygon( &segment1, transform1, &rox, transform2, &m_smgroxCache1 ); + b2Manifold m2 = b2CollideChainSegmentAndPolygon( &segment2, transform1, &rox, transform2, &m_smgroxCache2 ); + + { + b2Vec2 g1 = b2TransformPoint( transform1, segment1.ghost1 ); + b2Vec2 g2 = b2TransformPoint( transform1, segment1.ghost2 ); + b2Vec2 p1 = b2TransformPoint( transform1, segment1.segment.point1 ); + b2Vec2 p2 = b2TransformPoint( transform1, segment1.segment.point2 ); + g_draw.DrawSegment( p1, p2, color1 ); + g_draw.DrawPoint( p1, 4.0f, color1 ); + g_draw.DrawPoint( p2, 4.0f, color1 ); + g_draw.DrawSegment( p2, g2, b2_colorLightGray ); + } + + { + b2Vec2 g1 = b2TransformPoint( transform1, segment2.ghost1 ); + b2Vec2 g2 = b2TransformPoint( transform1, segment2.ghost2 ); + b2Vec2 p1 = b2TransformPoint( transform1, segment2.segment.point1 ); + b2Vec2 p2 = b2TransformPoint( transform1, segment2.segment.point2 ); + g_draw.DrawSegment( g1, p1, b2_colorLightGray ); + g_draw.DrawSegment( p1, p2, color1 ); + g_draw.DrawPoint( p1, 4.0f, color1 ); + g_draw.DrawPoint( p2, 4.0f, color1 ); + } + + g_draw.DrawSolidPolygon( transform2, rox.vertices, rox.count, rox.radius, color2 ); + g_draw.DrawPoint( b2TransformPoint( transform2, rox.centroid ), 5.0f, b2_colorGainsboro ); + + DrawManifold( &m1, transform1.p, transform2.p ); + DrawManifold( &m2, transform1.p, transform2.p ); + + offset.x += 2.0f * increment.x; + } + + // chain-segment vs capsule + { + b2ChainSegment segment1 = { { 2.0f, 1.0f }, { { 1.0f, 1.0f }, { -1.0f, 0.0f } }, { -2.0f, 0.0f }, -1 }; + b2ChainSegment segment2 = { { 3.0f, 1.0f }, { { 2.0f, 1.0f }, { 1.0f, 1.0f } }, { -1.0f, 0.0f }, -1 }; + b2Capsule capsule = { { -0.5f, 0.0f }, { 0.5f, 0.0 }, 0.25f }; + + b2Transform transform1 = { offset, b2Rot_identity }; + b2Transform transform2 = { b2Add( m_transform.p, offset ), m_transform.q }; + + b2Manifold m1 = b2CollideChainSegmentAndCapsule( &segment1, transform1, &capsule, transform2, &m_smgcapCache1 ); + b2Manifold m2 = b2CollideChainSegmentAndCapsule( &segment2, transform1, &capsule, transform2, &m_smgcapCache2 ); + + { + b2Vec2 g1 = b2TransformPoint( transform1, segment1.ghost1 ); + b2Vec2 g2 = b2TransformPoint( transform1, segment1.ghost2 ); + b2Vec2 p1 = b2TransformPoint( transform1, segment1.segment.point1 ); + b2Vec2 p2 = b2TransformPoint( transform1, segment1.segment.point2 ); + // g_draw.DrawSegment(g1, p1, b2_colorLightGray); + g_draw.DrawSegment( p1, p2, color1 ); + g_draw.DrawPoint( p1, 4.0f, color1 ); + g_draw.DrawPoint( p2, 4.0f, color1 ); + g_draw.DrawSegment( p2, g2, b2_colorLightGray ); + } + + { + b2Vec2 g1 = b2TransformPoint( transform1, segment2.ghost1 ); + b2Vec2 g2 = b2TransformPoint( transform1, segment2.ghost2 ); + b2Vec2 p1 = b2TransformPoint( transform1, segment2.segment.point1 ); + b2Vec2 p2 = b2TransformPoint( transform1, segment2.segment.point2 ); + g_draw.DrawSegment( g1, p1, b2_colorLightGray ); + g_draw.DrawSegment( p1, p2, color1 ); + g_draw.DrawPoint( p1, 4.0f, color1 ); + g_draw.DrawPoint( p2, 4.0f, color1 ); + // g_draw.DrawSegment(p2, g2, b2_colorLightGray); + } + + b2Vec2 p1 = b2TransformPoint( transform2, capsule.center1 ); + b2Vec2 p2 = b2TransformPoint( transform2, capsule.center2 ); + g_draw.DrawSolidCapsule( p1, p2, capsule.radius, color2 ); + + g_draw.DrawPoint( b2Lerp( p1, p2, 0.5f ), 5.0f, b2_colorGainsboro ); + + DrawManifold( &m1, transform1.p, transform2.p ); + DrawManifold( &m2, transform1.p, transform2.p ); + + offset.x += 2.0f * increment.x; + } + } + + static Sample* Create( Settings& settings ) + { + return new Manifold( settings ); + } + + b2DistanceCache m_smgroxCache1; + b2DistanceCache m_smgroxCache2; + b2DistanceCache m_smgcapCache1; + b2DistanceCache m_smgcapCache2; + + b2Hull m_wedge; + + b2Transform m_transform; + float m_angle; + float m_round; + + b2Vec2 m_basePosition; + b2Vec2 m_startPoint; + float m_baseAngle; + + bool m_dragging; + bool m_rotating; + bool m_showIds; + bool m_showAnchors; + bool m_showSeparation; + bool m_enableCaching; +}; + +static int sampleManifoldIndex = RegisterSample( "Collision", "Manifold", Manifold::Create ); + +class SmoothManifold : public Sample +{ +public: + enum ShapeType + { + e_circleShape = 0, + e_boxShape + }; + + explicit SmoothManifold( Settings& settings ) + : Sample( settings ) + { + if ( settings.restart == false ) + { + g_camera.m_center = { 2.0f, 20.0f }; + g_camera.m_zoom = 21.0f; + } + + m_shapeType = e_boxShape; + m_transform = { { 0.0f, 20.0f }, b2Rot_identity }; + m_angle = 0.0f; + m_round = 0.0f; + + m_startPoint = { 0.0f, 00.0f }; + m_basePosition = { 0.0f, 0.0f }; + m_baseAngle = 0.0f; + + m_dragging = false; + m_rotating = false; + m_showIds = false; + m_showAnchors = false; + m_showSeparation = false; + + // https://betravis.github.io/shape-tools/path-to-polygon/ + m_count = 36; + + b2Vec2 points[36]; + points[0] = { -20.58325, 14.54175 }; + points[1] = { -21.90625, 15.8645 }; + points[2] = { -24.552, 17.1875 }; + points[3] = { -27.198, 11.89575 }; + points[4] = { -29.84375, 15.8645 }; + points[5] = { -29.84375, 21.15625 }; + points[6] = { -25.875, 23.802 }; + points[7] = { -20.58325, 25.125 }; + points[8] = { -25.875, 29.09375 }; + points[9] = { -20.58325, 31.7395 }; + points[10] = { -11.0089998, 23.2290001 }; + points[11] = { -8.67700005, 21.15625 }; + points[12] = { -6.03125, 21.15625 }; + points[13] = { -7.35424995, 29.09375 }; + points[14] = { -3.38549995, 29.09375 }; + points[15] = { 1.90625, 30.41675 }; + points[16] = { 5.875, 17.1875 }; + points[17] = { 11.16675, 25.125 }; + points[18] = { 9.84375, 29.09375 }; + points[19] = { 13.8125, 31.7395 }; + points[20] = { 21.75, 30.41675 }; + points[21] = { 28.3644981, 26.448 }; + points[22] = { 25.71875, 18.5105 }; + points[23] = { 24.3957481, 13.21875 }; + points[24] = { 17.78125, 11.89575 }; + points[25] = { 15.1355, 7.92700005 }; + points[26] = { 5.875, 9.25 }; + points[27] = { 1.90625, 11.89575 }; + points[28] = { -3.25, 11.89575 }; + points[29] = { -3.25, 9.9375 }; + points[30] = { -4.70825005, 9.25 }; + points[31] = { -8.67700005, 9.25 }; + points[32] = { -11.323, 11.89575 }; + points[33] = { -13.96875, 11.89575 }; + points[34] = { -15.29175, 14.54175 }; + points[35] = { -19.2605, 14.54175 }; + + m_segments = (b2ChainSegment*)malloc( m_count * sizeof( b2ChainSegment ) ); + + for ( int i = 0; i < m_count; ++i ) + { + int i0 = i > 0 ? i - 1 : m_count - 1; + int i1 = i; + int i2 = i1 < m_count - 1 ? i1 + 1 : 0; + int i3 = i2 < m_count - 1 ? i2 + 1 : 0; + + b2Vec2 g1 = points[i0]; + b2Vec2 p1 = points[i1]; + b2Vec2 p2 = points[i2]; + b2Vec2 g2 = points[i3]; + + m_segments[i] = { g1, { p1, p2 }, g2, -1 }; + } + } + + virtual ~SmoothManifold() override + { + free( m_segments ); + } + + void UpdateUI() override + { + float height = 290.0f; + ImGui::SetNextWindowPos( ImVec2( 10.0f, g_camera.m_height - height - 50.0f ), ImGuiCond_Once ); + ImGui::SetNextWindowSize( ImVec2( 180.0f, height ) ); + + ImGui::Begin( "Smooth Manifold", nullptr, ImGuiWindowFlags_NoMove | ImGuiWindowFlags_NoResize ); + ImGui::PushItemWidth( 100.0f ); + + { + const char* shapeTypes[] = { "Circle", "Box" }; + int shapeType = int( m_shapeType ); + ImGui::Combo( "Shape", &shapeType, shapeTypes, IM_ARRAYSIZE( shapeTypes ) ); + m_shapeType = ShapeType( shapeType ); + } + + ImGui::SliderFloat( "x Offset", &m_transform.p.x, -2.0f, 2.0f, "%.2f" ); + ImGui::SliderFloat( "y Offset", &m_transform.p.y, -2.0f, 2.0f, "%.2f" ); + + if ( ImGui::SliderFloat( "Angle", &m_angle, -b2_pi, b2_pi, "%.2f" ) ) + { + m_transform.q = b2MakeRot( m_angle ); + } + + ImGui::SliderFloat( "Round", &m_round, 0.0f, 0.4f, "%.1f" ); + ImGui::Checkbox( "Show Ids", &m_showIds ); + ImGui::Checkbox( "Show Separation", &m_showSeparation ); + ImGui::Checkbox( "Show Anchors", &m_showAnchors ); + + if ( ImGui::Button( "Reset" ) ) + { + m_transform = b2Transform_identity; + m_angle = 0.0f; + } + + ImGui::Separator(); + + ImGui::Text( "mouse button 1: drag" ); + ImGui::Text( "mouse button 1 + shift: rotate" ); + + ImGui::PopItemWidth(); + ImGui::End(); + } + + void MouseDown( b2Vec2 p, int button, int mods ) override + { + if ( button == GLFW_MOUSE_BUTTON_1 ) + { + if ( mods == 0 && m_rotating == false ) + { + m_dragging = true; + m_startPoint = p; + m_basePosition = m_transform.p; + } + else if ( mods == GLFW_MOD_SHIFT && m_dragging == false ) + { + m_rotating = true; + m_startPoint = p; + m_baseAngle = m_angle; + } + } + } + + void MouseUp( b2Vec2, int button ) override + { + if ( button == GLFW_MOUSE_BUTTON_1 ) + { + m_dragging = false; + m_rotating = false; + } + } + + void MouseMove( b2Vec2 p ) override + { + if ( m_dragging ) + { + m_transform.p.x = m_basePosition.x + ( p.x - m_startPoint.x ); + m_transform.p.y = m_basePosition.y + ( p.y - m_startPoint.y ); + } + else if ( m_rotating ) + { + float dx = p.x - m_startPoint.x; + m_angle = b2ClampFloat( m_baseAngle + 1.0f * dx, -b2_pi, b2_pi ); + m_transform.q = b2MakeRot( m_angle ); + } + } + + void DrawManifold( const b2Manifold* manifold ) + { + for ( int i = 0; i < manifold->pointCount; ++i ) + { + const b2ManifoldPoint* mp = manifold->points + i; + + b2Vec2 p1 = mp->point; + b2Vec2 p2 = b2MulAdd( p1, 0.5f, manifold->normal ); + g_draw.DrawSegment( p1, p2, b2_colorWhite ); + + if ( m_showAnchors ) + { + g_draw.DrawPoint( p1, 5.0f, b2_colorGreen ); + } + else + { + g_draw.DrawPoint( p1, 5.0f, b2_colorGreen ); + } + + if ( m_showIds ) + { + // uint32_t indexA = mp->id >> 8; + // uint32_t indexB = 0xFF & mp->id; + b2Vec2 p = { p1.x + 0.05f, p1.y - 0.02f }; + g_draw.DrawString( p, "0x%04x", mp->id ); + } + + if ( m_showSeparation ) + { + b2Vec2 p = { p1.x + 0.05f, p1.y + 0.03f }; + g_draw.DrawString( p, "%.3f", mp->separation ); + } + } + } + + void Step( Settings& ) override + { + b2HexColor color1 = b2_colorYellow; + b2HexColor color2 = b2_colorMagenta; + + b2Transform transform1 = b2Transform_identity; + b2Transform transform2 = m_transform; + + for ( int i = 0; i < m_count; ++i ) + { + const b2ChainSegment* segment = m_segments + i; + b2Vec2 p1 = b2TransformPoint( transform1, segment->segment.point1 ); + b2Vec2 p2 = b2TransformPoint( transform1, segment->segment.point2 ); + g_draw.DrawSegment( p1, p2, color1 ); + g_draw.DrawPoint( p1, 4.0f, color1 ); + } + + // chain-segment vs circle + if ( m_shapeType == e_circleShape ) + { + b2Circle circle = { { 0.0f, 0.0f }, 0.5f }; + g_draw.DrawSolidCircle( transform2, circle.center, circle.radius, color2 ); + + for ( int i = 0; i < m_count; ++i ) + { + const b2ChainSegment* segment = m_segments + i; + b2Manifold m = b2CollideChainSegmentAndCircle( segment, transform1, &circle, transform2 ); + DrawManifold( &m ); + } + } + else if ( m_shapeType == e_boxShape ) + { + float h = 0.5f - m_round; + b2Polygon rox = b2MakeRoundedBox( h, h, m_round ); + g_draw.DrawSolidPolygon( transform2, rox.vertices, rox.count, rox.radius, color2 ); + + for ( int i = 0; i < m_count; ++i ) + { + const b2ChainSegment* segment = m_segments + i; + b2DistanceCache cache = {}; + b2Manifold m = b2CollideChainSegmentAndPolygon( segment, transform1, &rox, transform2, &cache ); + DrawManifold( &m ); + } + } + } + + static Sample* Create( Settings& settings ) + { + return new SmoothManifold( settings ); + } + + ShapeType m_shapeType; + + b2ChainSegment* m_segments; + int m_count; + + b2Transform m_transform; + float m_angle; + float m_round; + + b2Vec2 m_basePosition; + b2Vec2 m_startPoint; + float m_baseAngle; + + bool m_dragging; + bool m_rotating; + bool m_showIds; + bool m_showAnchors; + bool m_showSeparation; +}; + +static int sampleSmoothManifoldIndex = RegisterSample( "Collision", "Smooth Manifold", SmoothManifold::Create ); + +class ShapeCast : public Sample +{ +public: + enum + { + e_vertexCount = 8 + }; + + explicit ShapeCast( Settings& settings ) + : Sample( settings ) + { + if ( settings.restart == false ) + { + g_camera.m_center = { -1.5f, 1.0f }; + g_camera.m_zoom = 25.0f * 0.2f; + } + +#if 0 + // box swept against a triangle + m_vAs[0] = {-0.5f, 1.0f}; + m_vAs[1] = {0.5f, 1.0f}; + m_vAs[2] = {0.0f, 0.0f}; + m_countA = 3; + m_radiusA = 0.0f; + + m_vBs[0] = {-0.5f, -0.5f}; + m_vBs[1] = {0.5f, -0.5f}; + m_vBs[2] = {0.5f, 0.5f}; + m_vBs[3] = {-0.5f, 0.5f}; + m_countB = 4; + m_radiusB = 0.0f; + + m_transformA.p = {0.0f, 0.25f}; + m_transformA.q = b2Rot_identity; + m_transformB.p = {-4.0f, 0.0f}; + m_transformB.q = b2Rot_identity; + m_translationB = {8.0f, 0.0f}; +#elif 1 + // box swept against a segment + m_vAs[0] = { -2.0f, 0.0f }; + m_vAs[1] = { 2.0f, 0.0f }; + m_countA = 2; + m_radiusA = 0.0f; + + m_vBs[0] = { -0.25f, -0.25f }; + m_vBs[1] = { 0.25f, -0.25f }; + m_vBs[2] = { 0.25f, 0.25f }; + m_vBs[3] = { -0.25f, 0.25f }; + m_countB = 4; + m_radiusB = 0.25f; + + m_transformA.p = { 0.0f, 0.0 }; + m_transformA.q = b2MakeRot( 0.25f * b2_pi ); + m_transformB.p = { -8.0f, 0.0f }; + m_transformB.q = b2Rot_identity; + m_translationB = { 8.0f, 0.0f }; +#elif 0 + // A point swept against a box + m_vAs[0] = { -0.5f, -0.5f }; + m_vAs[1] = { 0.5f, -0.5f }; + m_vAs[2] = { 0.5f, 0.5f }; + m_vAs[3] = { -0.5f, 0.5f }; + m_countA = 4; + m_radiusA = 0.0f; + + m_vBs[0] = { 0.0f, 0.0f }; + m_countB = 1; + m_radiusB = 0.0f; + + m_transformA.p = { 0.0f, 0.0f }; + m_transformA.q = b2Rot_identity; + m_transformB.p = { -1.0f, 0.0f }; + m_transformB.q = b2Rot_identity; + m_translationB = { 1.0f, 0.0f }; +#elif 0 + m_vAs[0] = { 0.0f, 0.0f }; + m_countA = 1; + m_radiusA = 0.5f; + + m_vBs[0] = { 0.0f, 0.0f }; + m_countB = 1; + m_radiusB = 0.5f; + + m_transformA.p = { 0.0f, 0.25f }; + m_transformA.q = b2Rot_identity; + m_transformB.p = { -4.0f, 0.0f }; + m_transformB.q = b2Rot_identity; + m_translationB = { 8.0f, 0.0f }; +#else + m_vAs[0] = { 0.0f, 0.0f }; + m_vAs[1] = { 2.0f, 0.0f }; + m_countA = 2; + m_radiusA = 0.0f; + + m_vBs[0] = { 0.0f, 0.0f }; + m_countB = 1; + m_radiusB = 0.25f; + + // Initial overlap + m_transformA.p = b2Vec2_zero; + m_transformA.q = b2Rot_identity; + m_transformB.p = { -0.244360745f, 0.05999358f }; + m_transformB.q = b2Rot_identity; + m_translationB = { 0.0f, 0.0399999991f }; +#endif + + m_rayDrag = false; + } + + static Sample* Create( Settings& settings ) + { + return new ShapeCast( settings ); + } + + void MouseDown( b2Vec2 p, int button, int mods ) override + { + if ( button == GLFW_MOUSE_BUTTON_1 ) + { + m_transformB.p = p; + m_rayDrag = true; + } + } + + void MouseUp( b2Vec2, int button ) override + { + if ( button == GLFW_MOUSE_BUTTON_1 ) + { + m_rayDrag = false; + } + } + + void MouseMove( b2Vec2 p ) override + { + if ( m_rayDrag ) + { + m_translationB = b2Sub( p, m_transformB.p ); + } + } + + void Step( Settings& settings ) override + { + Sample::Step( settings ); + + b2ShapeCastPairInput input = { 0 }; + input.proxyA = b2MakeProxy( m_vAs, m_countA, m_radiusA ); + input.proxyB = b2MakeProxy( m_vBs, m_countB, m_radiusB ); + input.transformA = m_transformA; + input.transformB = m_transformB; + input.translationB = m_translationB; + input.maxFraction = 1.0f; + + b2CastOutput output = b2ShapeCast( &input ); + + b2Transform transformB2; + transformB2.q = m_transformB.q; + transformB2.p = b2MulAdd( m_transformB.p, output.fraction, input.translationB ); + + b2DistanceInput distanceInput; + distanceInput.proxyA = b2MakeProxy( m_vAs, m_countA, m_radiusA ); + distanceInput.proxyB = b2MakeProxy( m_vBs, m_countB, m_radiusB ); + distanceInput.transformA = m_transformA; + distanceInput.transformB = transformB2; + distanceInput.useRadii = false; + b2DistanceCache distanceCache; + distanceCache.count = 0; + b2DistanceOutput distanceOutput = b2ShapeDistance( &distanceCache, &distanceInput, nullptr, 0 ); + + g_draw.DrawString( 5, m_textLine, "hit = %s, iters = %d, lambda = %g, distance = %g", output.hit ? "true" : "false", + output.iterations, output.fraction, distanceOutput.distance ); + m_textLine += m_textIncrement; + + b2Vec2 vertices[b2_maxPolygonVertices]; + + for ( int i = 0; i < m_countA; ++i ) + { + vertices[i] = b2TransformPoint( m_transformA, m_vAs[i] ); + } + + if ( m_countA == 1 ) + { + if ( m_radiusA > 0.0f ) + { + g_draw.DrawSolidCircle( b2Transform_identity, vertices[0], m_radiusA, b2_colorGray9 ); + } + else + { + g_draw.DrawPoint( vertices[0], 5.0f, b2_colorGray9 ); + } + } + else + { + g_draw.DrawSolidPolygon( b2Transform_identity, vertices, m_countA, m_radiusA, b2_colorGray9 ); + } + + for ( int i = 0; i < m_countB; ++i ) + { + vertices[i] = b2TransformPoint( m_transformB, m_vBs[i] ); + } + + if ( m_countB == 1 ) + { + if ( m_radiusB > 0.0f ) + { + g_draw.DrawSolidCircle( b2Transform_identity, vertices[0], m_radiusB, b2_colorGreen ); + } + else + { + g_draw.DrawPoint( vertices[0], 5.0f, b2_colorGreen ); + } + } + else + { + g_draw.DrawSolidPolygon( b2Transform_identity, vertices, m_countB, m_radiusB, b2_colorGreen ); + } + + for ( int i = 0; i < m_countB; ++i ) + { + vertices[i] = b2TransformPoint( transformB2, m_vBs[i] ); + } + + if ( m_countB == 1 ) + { + if ( m_radiusB > 0.0f ) + { + g_draw.DrawSolidCircle( b2Transform_identity, vertices[0], m_radiusB, b2_colorOrange ); + } + else + { + g_draw.DrawPoint( vertices[0], 5.0f, b2_colorOrange ); + } + } + else + { + g_draw.DrawSolidPolygon( b2Transform_identity, vertices, m_countB, m_radiusB, b2_colorOrange ); + } + + if ( output.hit ) + { + b2Vec2 p1 = output.point; + g_draw.DrawPoint( p1, 10.0f, b2_colorRed ); + b2Vec2 p2 = b2MulAdd( p1, 1.0f, output.normal ); + g_draw.DrawSegment( p1, p2, b2_colorRed ); + } + + g_draw.DrawSegment( m_transformB.p, b2Add( m_transformB.p, m_translationB ), b2_colorGray ); + } + + b2Vec2 m_vAs[b2_maxPolygonVertices]; + int m_countA; + float m_radiusA; + + b2Vec2 m_vBs[b2_maxPolygonVertices]; + int m_countB; + float m_radiusB; + + b2Transform m_transformA; + b2Transform m_transformB; + b2Vec2 m_translationB; + bool m_rayDrag; +}; + +static int sampleShapeCast = RegisterSample( "Collision", "Shape Cast", ShapeCast::Create ); + +class TimeOfImpact : public Sample +{ +public: + explicit TimeOfImpact( Settings& settings ) + : Sample( settings ) + { + if ( settings.restart == false ) + { + g_camera.m_center = { 0.6f, 2.0f }; + g_camera.m_zoom = 25.0f * 0.18f; + } + } + + static Sample* Create( Settings& settings ) + { + return new TimeOfImpact( settings ); + } + + void Step( Settings& settings ) override + { + Sample::Step( settings ); + + b2Sweep sweepA = { b2Vec2_zero, { 0.0f, 0.0f }, { 0.0f, 0.0f }, b2Rot_identity, b2Rot_identity }; + b2Sweep sweepB = { b2Vec2_zero, { 2.0f, 4.0f }, { 2.0f, 4.0f }, b2Rot_identity, b2MakeRot( -0.25f * b2_pi ) }; + + b2TOIInput input; + input.proxyA = b2MakeProxy( m_verticesA, m_countA, 0.0f ); + input.proxyB = b2MakeProxy( m_verticesB, m_countB, 0.0f ); + input.sweepA = sweepA; + input.sweepB = sweepB; + input.tMax = 1.0f; + + b2TOIOutput output = b2TimeOfImpact( &input ); + + g_draw.DrawString( 5, m_textLine, "toi = %g", output.t ); + m_textLine += m_textIncrement; + + // g_draw.DrawString(5, m_textLine, "max toi iters = %d, max root iters = %d", b2_toiMaxIters, + // b2_toiMaxRootIters); + m_textLine += m_textIncrement; + + b2Vec2 vertices[b2_maxPolygonVertices]; + + // Draw A + b2Transform transformA = b2GetSweepTransform( &sweepA, 0.0f ); + for ( int i = 0; i < m_countA; ++i ) + { + vertices[i] = b2TransformPoint( transformA, m_verticesA[i] ); + } + g_draw.DrawPolygon( vertices, m_countA, b2_colorGray ); + + // Draw B at t = 0 + b2Transform transformB = b2GetSweepTransform( &sweepB, 0.0f ); + for ( int i = 0; i < m_countB; ++i ) + { + vertices[i] = b2TransformPoint( transformB, m_verticesB[i] ); + } + g_draw.DrawPolygon( vertices, m_countB, b2_colorGreen ); + + // Draw B at t = hit_time + transformB = b2GetSweepTransform( &sweepB, output.t ); + for ( int i = 0; i < m_countB; ++i ) + { + vertices[i] = b2TransformPoint( transformB, m_verticesB[i] ); + } + g_draw.DrawPolygon( vertices, m_countB, b2_colorOrange ); + + // Draw B at t = 1 + transformB = b2GetSweepTransform( &sweepB, 1.0f ); + for ( int i = 0; i < m_countB; ++i ) + { + vertices[i] = b2TransformPoint( transformB, m_verticesB[i] ); + } + g_draw.DrawPolygon( vertices, m_countB, b2_colorRed ); + + if ( output.state == b2_toiStateHit ) + { + b2DistanceInput dinput; + dinput.proxyA = input.proxyA; + dinput.proxyB = input.proxyB; + dinput.transformA = b2GetSweepTransform( &sweepA, output.t ); + dinput.transformB = b2GetSweepTransform( &sweepB, output.t ); + dinput.useRadii = false; + b2DistanceCache cache = { 0 }; + b2DistanceOutput doutput = b2ShapeDistance( &cache, &dinput, nullptr, 0 ); + g_draw.DrawString( 5, m_textLine, "distance = %g", doutput.distance ); + m_textLine += m_textIncrement; + } + +#if 0 + for (float t = 0.0f; t < 1.0f; t += 0.1f) + { + transformB = b2GetSweepTransform(&sweepB, t); + for (int i = 0; i < m_countB; ++i) + { + vertices[i] = b2TransformPoint(transformB, m_verticesB[i]); + } + g_draw.DrawPolygon(vertices, m_countB, {0.3f, 0.3f, 0.3f}); + } +#endif + } + + b2Vec2 m_verticesA[4] = { { -1.0f, -1.0f }, { 1.0f, -1.0f }, { 1.0f, 5.0f }, { -1.0f, 5.0f } }; + b2Vec2 m_verticesB[4] = { { -0.5f, -4.0f }, { 0.0f, -4.0f }, { 0.0f, 0.0f }, { -0.5f, 0.0f } }; + int m_countA = ARRAY_COUNT( m_verticesA ); + int m_countB = ARRAY_COUNT( m_verticesB ); +}; + +static int sampleTimeOfImpact = RegisterSample( "Collision", "Time of Impact", TimeOfImpact::Create ); diff --git a/tests/cpp-tests/Source/Box2DTestBed/samples/sample_continuous.cpp b/tests/cpp-tests/Source/Box2DTestBed/samples/sample_continuous.cpp new file mode 100644 index 000000000000..ac35e07c8974 --- /dev/null +++ b/tests/cpp-tests/Source/Box2DTestBed/samples/sample_continuous.cpp @@ -0,0 +1,955 @@ +// SPDX-FileCopyrightText: 2022 Erin Catto +// SPDX-License-Identifier: MIT + +#include "draw.h" +#include "sample.h" +#include "settings.h" + +#include "box2d/box2d.h" +#include "box2d/math_functions.h" + +#include +#include + +// This tests continuous collision robustness and also demonstrates the speed limits imposed +// by b2_maxTranslation and b2_maxRotation. +struct HitEvent +{ + b2Vec2 point; + float speed; + int stepIndex; +}; + +class BounceHouse : public Sample +{ +public: + enum ShapeType + { + e_circleShape = 0, + e_capsuleShape, + e_boxShape + }; + + explicit BounceHouse( Settings& settings ) + : Sample( settings ) + { + if ( settings.restart == false ) + { + g_camera.m_center = { 0.0f, 0.0f }; + g_camera.m_zoom = 25.0f * 0.45f; + } + + b2BodyDef bodyDef = b2DefaultBodyDef(); + b2BodyId groundId = b2CreateBody( m_worldId, &bodyDef ); + + b2ShapeDef shapeDef = b2DefaultShapeDef(); + { + b2Segment segment = { { -10.0f, -10.0f }, { 10.0f, -10.0f } }; + b2CreateSegmentShape( groundId, &shapeDef, &segment ); + } + + { + b2Segment segment = { { 10.0f, -10.0f }, { 10.0f, 10.0f } }; + b2CreateSegmentShape( groundId, &shapeDef, &segment ); + } + + { + b2Segment segment = { { 10.0f, 10.0f }, { -10.0f, 10.0f } }; + b2CreateSegmentShape( groundId, &shapeDef, &segment ); + } + + { + b2Segment segment = { { -10.0f, 10.0f }, { -10.0f, -10.0f } }; + b2CreateSegmentShape( groundId, &shapeDef, &segment ); + } + + m_shapeType = e_circleShape; + m_bodyId = b2_nullBodyId; + m_enableHitEvents = true; + + memset( m_hitEvents, 0, sizeof( m_hitEvents ) ); + + Launch(); + } + + void Launch() + { + if ( B2_IS_NON_NULL( m_bodyId ) ) + { + b2DestroyBody( m_bodyId ); + } + + b2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.type = b2_dynamicBody; + bodyDef.linearVelocity = { 10.0f, 20.0f }; + bodyDef.position = { 0.0f, 0.0f }; + bodyDef.gravityScale = 0.0f; + + // Circle shapes centered on the body can spin fast without risk of tunnelling. + bodyDef.allowFastRotation = m_shapeType == e_circleShape; + + m_bodyId = b2CreateBody( m_worldId, &bodyDef ); + + b2ShapeDef shapeDef = b2DefaultShapeDef(); + shapeDef.density = 1.0f; + shapeDef.restitution = 1.2f; + shapeDef.friction = 0.3f; + shapeDef.enableHitEvents = m_enableHitEvents; + + if ( m_shapeType == e_circleShape ) + { + b2Circle circle = { { 0.0f, 0.0f }, 0.5f }; + b2CreateCircleShape( m_bodyId, &shapeDef, &circle ); + } + else if ( m_shapeType == e_capsuleShape ) + { + b2Capsule capsule = { { -0.5f, 0.0f }, { 0.5f, 0.0 }, 0.25f }; + b2CreateCapsuleShape( m_bodyId, &shapeDef, &capsule ); + } + else + { + float h = 0.1f; + b2Polygon box = b2MakeBox( 20.0f * h, h ); + b2CreatePolygonShape( m_bodyId, &shapeDef, &box ); + } + } + + void UpdateUI() override + { + float height = 100.0f; + ImGui::SetNextWindowPos( ImVec2( 10.0f, g_camera.m_height - height - 50.0f ), ImGuiCond_Once ); + ImGui::SetNextWindowSize( ImVec2( 240.0f, height ) ); + + ImGui::Begin( "Bounce House", nullptr, ImGuiWindowFlags_NoResize ); + + const char* shapeTypes[] = { "Circle", "Capsule", "Box" }; + int shapeType = int( m_shapeType ); + if ( ImGui::Combo( "Shape", &shapeType, shapeTypes, IM_ARRAYSIZE( shapeTypes ) ) ) + { + m_shapeType = ShapeType( shapeType ); + Launch(); + } + + if ( ImGui::Checkbox( "hit events", &m_enableHitEvents ) ) + { + b2Body_EnableHitEvents( m_bodyId, m_enableHitEvents ); + } + + ImGui::End(); + } + + void Step( Settings& settings ) override + { + Sample::Step( settings ); + + b2ContactEvents events = b2World_GetContactEvents( m_worldId ); + for ( int i = 0; i < events.hitCount; ++i ) + { + b2ContactHitEvent* event = events.hitEvents + i; + + HitEvent* e = m_hitEvents + 0; + for ( int j = 1; j < 4; ++j ) + { + if ( m_hitEvents[j].stepIndex < e->stepIndex ) + { + e = m_hitEvents + j; + } + } + + e->point = event->point; + e->speed = event->approachSpeed; + e->stepIndex = m_stepCount; + } + + for ( int i = 0; i < 4; ++i ) + { + HitEvent* e = m_hitEvents + i; + if ( e->stepIndex > 0 && m_stepCount <= e->stepIndex + 30 ) + { + g_draw.DrawCircle( e->point, 0.1f, b2_colorOrangeRed ); + g_draw.DrawString( e->point, "%.1f", e->speed ); + } + } + } + + static Sample* Create( Settings& settings ) + { + return new BounceHouse( settings ); + } + + HitEvent m_hitEvents[4]; + b2BodyId m_bodyId; + ShapeType m_shapeType; + bool m_enableHitEvents; +}; + +static int sampleBounceHouse = RegisterSample( "Continuous", "Bounce House", BounceHouse::Create ); + +class FastChain : public Sample +{ +public: + explicit FastChain( Settings& settings ) + : Sample( settings ) + { + if ( settings.restart == false ) + { + g_camera.m_center = { 0.0f, 0.0f }; + g_camera.m_zoom = 25.0f * 0.35f; + } + + b2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.position = { 0.0f, -6.0f }; + b2BodyId groundId = b2CreateBody( m_worldId, &bodyDef ); + + b2Vec2 points[4] = { { -10.0f, -2.0f }, { 10.0f, -2.0f }, { 10.0f, 1.0f }, { -10.0f, 1.0f } }; + + b2ChainDef chainDef = b2DefaultChainDef(); + chainDef.points = points; + chainDef.count = 4; + chainDef.isLoop = true; + + b2CreateChain( groundId, &chainDef ); + + m_bodyId = b2_nullBodyId; + + Launch(); + } + + void Launch() + { + if ( B2_IS_NON_NULL( m_bodyId ) ) + { + b2DestroyBody( m_bodyId ); + } + + b2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.type = b2_dynamicBody; + bodyDef.linearVelocity = { 0.0f, -200.0f }; + bodyDef.position = { 0.0f, 10.0f }; + bodyDef.gravityScale = 1.0f; + m_bodyId = b2CreateBody( m_worldId, &bodyDef ); + + b2ShapeDef shapeDef = b2DefaultShapeDef(); + b2Circle circle = { { 0.0f, 0.0f }, 0.5f }; + b2CreateCircleShape( m_bodyId, &shapeDef, &circle ); + } + + void UpdateUI() override + { + float height = 70.0f; + ImGui::SetNextWindowPos( ImVec2( 10.0f, g_camera.m_height - height - 50.0f ), ImGuiCond_Once ); + ImGui::SetNextWindowSize( ImVec2( 240.0f, height ) ); + + ImGui::Begin( "Fast Chain", nullptr, ImGuiWindowFlags_NoResize ); + + if ( ImGui::Button( "Launch" ) ) + { + Launch(); + } + + ImGui::End(); + } + + static Sample* Create( Settings& settings ) + { + return new FastChain( settings ); + } + + b2BodyId m_bodyId; +}; + +static int sampleFastChainHouse = RegisterSample( "Continuous", "Fast Chain", FastChain::Create ); + +class SkinnyBox : public Sample +{ +public: + explicit SkinnyBox( Settings& settings ) + : Sample( settings ) + { + if ( settings.restart == false ) + { + g_camera.m_center = { 1.0f, 5.0f }; + g_camera.m_zoom = 25.0f * 0.25f; + } + + { + b2BodyDef bodyDef = b2DefaultBodyDef(); + b2BodyId groundId = b2CreateBody( m_worldId, &bodyDef ); + + b2Segment segment = { { -10.0f, 0.0f }, { 10.0f, 0.0f } }; + b2ShapeDef shapeDef = b2DefaultShapeDef(); + shapeDef.friction = 0.9f; + b2CreateSegmentShape( groundId, &shapeDef, &segment ); + + b2Polygon box = b2MakeOffsetBox( 0.1f, 1.0f, { 0.0f, 1.0f }, b2Rot_identity ); + b2CreatePolygonShape( groundId, &shapeDef, &box ); + } + + m_autoTest = false; + m_bullet = false; + m_capsule = false; + + m_bodyId = b2_nullBodyId; + m_bulletId = b2_nullBodyId; + + Launch(); + } + + void Launch() + { + if ( B2_IS_NON_NULL( m_bodyId ) ) + { + b2DestroyBody( m_bodyId ); + } + + if ( B2_IS_NON_NULL( m_bulletId ) ) + { + b2DestroyBody( m_bulletId ); + } + + m_angularVelocity = RandomFloat( -50.0f, 50.0f ); + // m_angularVelocity = -30.6695766f; + + b2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.type = b2_dynamicBody; + bodyDef.position = { 0.0f, 8.0f }; + bodyDef.angularVelocity = m_angularVelocity; + bodyDef.linearVelocity = { 0.0f, -100.0f }; + + b2ShapeDef shapeDef = b2DefaultShapeDef(); + shapeDef.density = 1.0f; + shapeDef.friction = 0.9f; + + m_bodyId = b2CreateBody( m_worldId, &bodyDef ); + + if ( m_capsule ) + { + b2Capsule capsule = { { 0.0f, -1.0f }, { 0.0f, 1.0f }, 0.1f }; + b2CreateCapsuleShape( m_bodyId, &shapeDef, &capsule ); + } + else + { + b2Polygon polygon = b2MakeBox( 2.0f, 0.05f ); + b2CreatePolygonShape( m_bodyId, &shapeDef, &polygon ); + } + + if ( m_bullet ) + { + b2Polygon polygon = b2MakeBox( 0.25f, 0.25f ); + m_x = RandomFloat( -1.0f, 1.0f ); + bodyDef.position = { m_x, 10.0f }; + bodyDef.linearVelocity = { 0.0f, -50.0f }; + m_bulletId = b2CreateBody( m_worldId, &bodyDef ); + b2CreatePolygonShape( m_bulletId, &shapeDef, &polygon ); + } + } + + void UpdateUI() override + { + float height = 110.0f; + ImGui::SetNextWindowPos( ImVec2( 10.0f, g_camera.m_height - height - 50.0f ), ImGuiCond_Once ); + ImGui::SetNextWindowSize( ImVec2( 140.0f, height ) ); + + ImGui::Begin( "Skinny Box", nullptr, ImGuiWindowFlags_NoResize ); + + ImGui::Checkbox( "Capsule", &m_capsule ); + + if ( ImGui::Button( "Launch" ) ) + { + Launch(); + } + + ImGui::Checkbox( "Auto Test", &m_autoTest ); + + ImGui::End(); + } + + void Step( Settings& settings ) override + { + Sample::Step( settings ); + + if ( m_autoTest && m_stepCount % 60 == 0 ) + { + Launch(); + } + } + + static Sample* Create( Settings& settings ) + { + return new SkinnyBox( settings ); + } + + b2BodyId m_bodyId, m_bulletId; + float m_angularVelocity; + float m_x; + bool m_capsule; + bool m_autoTest; + bool m_bullet; +}; + +static int sampleSkinnyBox = RegisterSample( "Continuous", "Skinny Box", SkinnyBox::Create ); + +class SpeculativeBug : public Sample +{ +public: + explicit SpeculativeBug( Settings& settings ) + : Sample( settings ) + { + if ( settings.restart == false ) + { + g_camera.m_center = { 1.0f, 5.0f }; + g_camera.m_zoom = 25.0f * 0.25f; + } + + { + b2BodyDef bodyDef = b2DefaultBodyDef(); + b2BodyId groundId = b2CreateBody( m_worldId, &bodyDef ); + + b2Segment segment = { { -10.0f, 0.0f }, { 10.0f, 0.0f } }; + b2ShapeDef shapeDef = b2DefaultShapeDef(); + b2CreateSegmentShape( groundId, &shapeDef, &segment ); + + shapeDef.friction = 0.0f; + b2Polygon box = b2MakeOffsetBox( 0.05f, 1.0f, { 0.0f, 1.0f }, b2Rot_identity ); + b2CreatePolygonShape( groundId, &shapeDef, &box ); + } + + b2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.type = b2_dynamicBody; + for (int i = 0; i < 2; ++i) + { + if (i == 0) + { + bodyDef.position = { -0.8f, 0.25f }; + bodyDef.isAwake = false; + } + else + { + bodyDef.position = { 0.8f, 2.0f }; + bodyDef.isAwake = true; + } + + b2BodyId bodyId = b2CreateBody( m_worldId, &bodyDef ); + + b2ShapeDef shapeDef = b2DefaultShapeDef(); + b2Capsule capsule = { { -0.5f, 0.0f }, { 0.5f, 0.0f }, 0.25f }; + b2CreateCapsuleShape( bodyId, &shapeDef, &capsule ); + } + } + + static Sample* Create( Settings& settings ) + { + return new SpeculativeBug( settings ); + } +}; + +static int sampleSpeculativeBug = RegisterSample( "Continuous", "Speculative Bug", SpeculativeBug::Create ); + +// This sample shows ghost collisions +class GhostCollision : public Sample +{ +public: + enum ShapeType + { + e_circleShape = 0, + e_capsuleShape, + e_boxShape + }; + + explicit GhostCollision( Settings& settings ) + : Sample( settings ) + { + if ( settings.restart == false ) + { + g_camera.m_center = { 1.5f, 16.0f }; + g_camera.m_zoom = 25.0f * 0.8f; + } + + m_groundId = b2_nullBodyId; + m_bodyId = b2_nullBodyId; + m_shapeId = b2_nullShapeId; + m_shapeType = e_circleShape; + m_round = 0.0f; + m_friction = 0.2f; + m_bevel = 0.0f; + m_useChain = true; + + CreateScene(); + Launch(); + } + + void CreateScene() + { + if ( B2_IS_NON_NULL( m_groundId ) ) + { + b2DestroyBody( m_groundId ); + } + + m_shapeId = b2_nullShapeId; + + b2BodyDef bodyDef = b2DefaultBodyDef(); + m_groundId = b2CreateBody( m_worldId, &bodyDef ); + + float m = 1.0f / sqrt( 2.0f ); + float mm = 2.0f * ( sqrt( 2.0f ) - 1.0f ); + float hx = 4.0f, hy = 0.25f; + + if ( m_useChain ) + { + b2Vec2 points[20]; + points[0] = { -3.0f * hx, hy }; + points[1] = b2Add( points[0], { -2.0f * hx * m, 2.0f * hx * m } ); + points[2] = b2Add( points[1], { -2.0f * hx * m, 2.0f * hx * m } ); + points[3] = b2Add( points[2], { -2.0f * hx * m, 2.0f * hx * m } ); + points[4] = b2Add( points[3], { -2.0f * hy * m, -2.0f * hy * m } ); + points[5] = b2Add( points[4], { 2.0f * hx * m, -2.0f * hx * m } ); + points[6] = b2Add( points[5], { 2.0f * hx * m, -2.0f * hx * m } ); + points[7] = + b2Add( points[6], { 2.0f * hx * m + 2.0f * hy * ( 1.0f - m ), -2.0f * hx * m - 2.0f * hy * ( 1.0f - m ) } ); + points[8] = b2Add( points[7], { 2.0f * hx + hy * mm, 0.0f } ); + points[9] = b2Add( points[8], { 2.0f * hx, 0.0f } ); + points[10] = b2Add( points[9], { 2.0f * hx + hy * mm, 0.0f } ); + points[11] = + b2Add( points[10], { 2.0f * hx * m + 2.0f * hy * ( 1.0f - m ), 2.0f * hx * m + 2.0f * hy * ( 1.0f - m ) } ); + points[12] = b2Add( points[11], { 2.0f * hx * m, 2.0f * hx * m } ); + points[13] = b2Add( points[12], { 2.0f * hx * m, 2.0f * hx * m } ); + points[14] = b2Add( points[13], { -2.0f * hy * m, 2.0f * hy * m } ); + points[15] = b2Add( points[14], { -2.0f * hx * m, -2.0f * hx * m } ); + points[16] = b2Add( points[15], { -2.0f * hx * m, -2.0f * hx * m } ); + points[17] = b2Add( points[16], { -2.0f * hx * m, -2.0f * hx * m } ); + points[18] = b2Add( points[17], { -2.0f * hx, 0.0f } ); + points[19] = b2Add( points[18], { -2.0f * hx, 0.0f } ); + + b2ChainDef chainDef = b2DefaultChainDef(); + chainDef.points = points; + chainDef.count = 20; + chainDef.isLoop = true; + chainDef.friction = m_friction; + + b2CreateChain( m_groundId, &chainDef ); + } + else + { + b2ShapeDef shapeDef = b2DefaultShapeDef(); + shapeDef.friction = m_friction; + + b2Hull hull = { 0 }; + + if ( m_bevel > 0.0f ) + { + float hb = m_bevel; + b2Vec2 vs[8] = { { hx + hb, hy - 0.05f }, { hx, hy }, { -hx, hy }, { -hx - hb, hy - 0.05f }, + { -hx - hb, -hy + 0.05f }, { -hx, -hy }, { hx, -hy }, { hx + hb, -hy + 0.05f } }; + hull = b2ComputeHull( vs, 8 ); + } + else + { + b2Vec2 vs[4] = { { hx, hy }, { -hx, hy }, { -hx, -hy }, { hx, -hy } }; + hull = b2ComputeHull( vs, 4 ); + } + + b2Transform transform; + float x, y; + + // Left slope + x = -3.0f * hx - m * hx - m * hy; + y = hy + m * hx - m * hy; + transform.q = b2MakeRot( -0.25f * b2_pi ); + + { + transform.p = { x, y }; + b2Polygon polygon = b2MakeOffsetPolygon( &hull, 0.0f, transform ); + b2CreatePolygonShape( m_groundId, &shapeDef, &polygon ); + x -= 2.0f * m * hx; + y += 2.0f * m * hx; + } + { + transform.p = { x, y }; + b2Polygon polygon = b2MakeOffsetPolygon( &hull, 0.0f, transform ); + b2CreatePolygonShape( m_groundId, &shapeDef, &polygon ); + x -= 2.0f * m * hx; + y += 2.0f * m * hx; + } + { + transform.p = { x, y }; + b2Polygon polygon = b2MakeOffsetPolygon( &hull, 0.0f, transform ); + b2CreatePolygonShape( m_groundId, &shapeDef, &polygon ); + x -= 2.0f * m * hx; + y += 2.0f * m * hx; + } + + x = -2.0f * hx; + y = 0.0f; + transform.q = b2MakeRot( 0.0f ); + + { + transform.p = { x, y }; + b2Polygon polygon = b2MakeOffsetPolygon( &hull, 0.0f, transform ); + b2CreatePolygonShape( m_groundId, &shapeDef, &polygon ); + x += 2.0f * hx; + } + { + transform.p = { x, y }; + b2Polygon polygon = b2MakeOffsetPolygon( &hull, 0.0f, transform ); + b2CreatePolygonShape( m_groundId, &shapeDef, &polygon ); + x += 2.0f * hx; + } + { + transform.p = { x, y }; + b2Polygon polygon = b2MakeOffsetPolygon( &hull, 0.0f, transform ); + b2CreatePolygonShape( m_groundId, &shapeDef, &polygon ); + x += 2.0f * hx; + } + + x = 3.0f * hx + m * hx + m * hy; + y = hy + m * hx - m * hy; + transform.q = b2MakeRot( 0.25f * b2_pi ); + + { + transform.p = { x, y }; + b2Polygon polygon = b2MakeOffsetPolygon( &hull, 0.0f, transform ); + b2CreatePolygonShape( m_groundId, &shapeDef, &polygon ); + x += 2.0f * m * hx; + y += 2.0f * m * hx; + } + { + transform.p = { x, y }; + b2Polygon polygon = b2MakeOffsetPolygon( &hull, 0.0f, transform ); + b2CreatePolygonShape( m_groundId, &shapeDef, &polygon ); + x += 2.0f * m * hx; + y += 2.0f * m * hx; + } + { + transform.p = { x, y }; + b2Polygon polygon = b2MakeOffsetPolygon( &hull, 0.0f, transform ); + b2CreatePolygonShape( m_groundId, &shapeDef, &polygon ); + x += 2.0f * m * hx; + y += 2.0f * m * hx; + } + } + } + + void Launch() + { + if ( B2_IS_NON_NULL( m_bodyId ) ) + { + b2DestroyBody( m_bodyId ); + m_shapeId = b2_nullShapeId; + } + + b2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.type = b2_dynamicBody; + bodyDef.position = { -28.0f, 18.0f }; + bodyDef.linearVelocity = { 0.0f, 0.0f }; + m_bodyId = b2CreateBody( m_worldId, &bodyDef ); + + b2ShapeDef shapeDef = b2DefaultShapeDef(); + shapeDef.density = 1.0f; + shapeDef.friction = m_friction; + + if ( m_shapeType == e_circleShape ) + { + b2Circle circle = { { 0.0f, 0.0f }, 0.5f }; + m_shapeId = b2CreateCircleShape( m_bodyId, &shapeDef, &circle ); + } + else if ( m_shapeType == e_capsuleShape ) + { + b2Capsule capsule = { { -0.5f, 0.0f }, { 0.5f, 0.0 }, 0.25f }; + m_shapeId = b2CreateCapsuleShape( m_bodyId, &shapeDef, &capsule ); + } + else + { + float h = 0.5f - m_round; + b2Polygon box = b2MakeRoundedBox( h, 2.0f * h, m_round ); + m_shapeId = b2CreatePolygonShape( m_bodyId, &shapeDef, &box ); + } + } + + void UpdateUI() override + { + float height = 140.0f; + ImGui::SetNextWindowPos( ImVec2( 10.0f, g_camera.m_height - height - 50.0f ), ImGuiCond_Once ); + ImGui::SetNextWindowSize( ImVec2( 180.0f, height ) ); + + ImGui::Begin( "Ghost Collision", nullptr, ImGuiWindowFlags_NoResize ); + ImGui::PushItemWidth( 100.0f ); + + if ( ImGui::Checkbox( "Chain", &m_useChain ) ) + { + CreateScene(); + } + + if ( m_useChain == false ) + { + if ( ImGui::SliderFloat( "Bevel", &m_bevel, 0.0f, 1.0f, "%.2f" ) ) + { + CreateScene(); + } + } + + { + const char* shapeTypes[] = { "Circle", "Capsule", "Box" }; + int shapeType = int( m_shapeType ); + ImGui::Combo( "Shape", &shapeType, shapeTypes, IM_ARRAYSIZE( shapeTypes ) ); + m_shapeType = ShapeType( shapeType ); + } + + if ( m_shapeType == e_boxShape ) + { + ImGui::SliderFloat( "Round", &m_round, 0.0f, 0.4f, "%.1f" ); + } + + if ( ImGui::SliderFloat( "Friction", &m_friction, 0.0f, 1.0f, "%.1f" ) ) + { + if ( B2_IS_NON_NULL( m_shapeId ) ) + { + b2Shape_SetFriction( m_shapeId, m_friction ); + } + + CreateScene(); + } + + if ( ImGui::Button( "Launch" ) ) + { + Launch(); + } + + ImGui::PopItemWidth(); + ImGui::End(); + } + + static Sample* Create( Settings& settings ) + { + return new GhostCollision( settings ); + } + + b2BodyId m_groundId; + b2BodyId m_bodyId; + b2ShapeId m_shapeId; + ShapeType m_shapeType; + float m_round; + float m_friction; + float m_bevel; + bool m_useChain; +}; + +static int sampleGhostCollision = RegisterSample( "Continuous", "Ghost Collision", GhostCollision::Create ); + +// Speculative collision failure case suggested by Dirk Gregorius. This uses +// a simple fallback scheme to prevent tunneling. +class SpeculativeFallback : public Sample +{ +public: + explicit SpeculativeFallback( Settings& settings ) + : Sample( settings ) + { + if ( settings.restart == false ) + { + g_camera.m_center = { 1.0f, 5.0f }; + g_camera.m_zoom = 25.0f * 0.25f; + } + + { + b2BodyDef bodyDef = b2DefaultBodyDef(); + b2BodyId groundId = b2CreateBody( m_worldId, &bodyDef ); + + b2ShapeDef shapeDef = b2DefaultShapeDef(); + b2Segment segment = { { -10.0f, 0.0f }, { 10.0f, 0.0f } }; + b2CreateSegmentShape( groundId, &shapeDef, &segment ); + + b2Vec2 points[5] = { { -2.0f, 4.0f }, { 2.0f, 4.0f }, { 2.0f, 4.1f }, { -0.5f, 4.2f }, { -2.0f, 4.2f } }; + b2Hull hull = b2ComputeHull( points, 5 ); + b2Polygon poly = b2MakePolygon( &hull, 0.0f ); + b2CreatePolygonShape( groundId, &shapeDef, &poly ); + } + + // Fast moving skinny box. Also testing a large shape offset. + { + float offset = 8.0f; + b2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.type = b2_dynamicBody; + bodyDef.position = { offset, 12.0f }; + bodyDef.linearVelocity = { 0.0f, -100.0f }; + b2BodyId bodyId = b2CreateBody( m_worldId, &bodyDef ); + + b2ShapeDef shapeDef = b2DefaultShapeDef(); + b2Polygon box = b2MakeOffsetBox( 2.0f, 0.05f, { -offset, 0.0f }, b2MakeRot(b2_pi) ); + b2CreatePolygonShape( bodyId, &shapeDef, &box ); + } + } + + static Sample* Create( Settings& settings ) + { + return new SpeculativeFallback( settings ); + } +}; + +static int sampleSpeculativeFallback = RegisterSample( "Continuous", "Speculative Fallback", SpeculativeFallback::Create ); + +// This shows a fast moving body that uses continuous collision versus static and dynamic bodies. +// This is achieved by setting the ball body as a *bullet*. +class Pinball : public Sample +{ +public: + explicit Pinball( Settings& settings ) + : Sample( settings ) + { + if ( settings.restart == false ) + { + g_camera.m_center = { 0.0f, 9.0f }; + g_camera.m_zoom = 25.0f * 0.5f; + } + + settings.drawJoints = false; + + // Ground body + b2BodyId groundId = {}; + { + b2BodyDef bodyDef = b2DefaultBodyDef(); + groundId = b2CreateBody( m_worldId, &bodyDef ); + + b2Vec2 vs[5] = { { -8.0f, 6.0f }, { -8.0f, 20.0f }, { 8.0f, 20.0f }, { 8.0f, 6.0f }, { 0.0f, -2.0f } }; + + b2ChainDef chainDef = b2DefaultChainDef(); + chainDef.points = vs; + chainDef.count = 5; + chainDef.isLoop = true; + b2CreateChain( groundId, &chainDef ); + } + + // Flippers + { + b2Vec2 p1 = { -2.0f, 0.0f }, p2 = { 2.0f, 0.0f }; + + b2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.type = b2_dynamicBody; + bodyDef.enableSleep = false; + + bodyDef.position = p1; + b2BodyId leftFlipperId = b2CreateBody( m_worldId, &bodyDef ); + + bodyDef.position = p2; + b2BodyId rightFlipperId = b2CreateBody( m_worldId, &bodyDef ); + + b2Polygon box = b2MakeBox( 1.75f, 0.2f ); + + b2ShapeDef shapeDef = b2DefaultShapeDef(); + + b2CreatePolygonShape( leftFlipperId, &shapeDef, &box ); + b2CreatePolygonShape( rightFlipperId, &shapeDef, &box ); + + b2RevoluteJointDef jointDef = b2DefaultRevoluteJointDef(); + jointDef.bodyIdA = groundId; + jointDef.localAnchorB = b2Vec2_zero; + jointDef.enableMotor = true; + jointDef.maxMotorTorque = 1000.0f; + jointDef.enableLimit = true; + + jointDef.motorSpeed = 0.0f; + jointDef.localAnchorA = p1; + jointDef.bodyIdB = leftFlipperId; + jointDef.lowerAngle = -30.0f * b2_pi / 180.0f; + jointDef.upperAngle = 5.0f * b2_pi / 180.0f; + m_leftJointId = b2CreateRevoluteJoint( m_worldId, &jointDef ); + + jointDef.motorSpeed = 0.0f; + jointDef.localAnchorA = p2; + jointDef.bodyIdB = rightFlipperId; + jointDef.lowerAngle = -5.0f * b2_pi / 180.0f; + jointDef.upperAngle = 30.0f * b2_pi / 180.0f; + m_rightJointId = b2CreateRevoluteJoint( m_worldId, &jointDef ); + } + + // Spinners + { + b2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.type = b2_dynamicBody; + bodyDef.position = { -4.0f, 17.0f }; + + b2BodyId bodyId = b2CreateBody( m_worldId, &bodyDef ); + + b2ShapeDef shapeDef = b2DefaultShapeDef(); + b2Polygon box1 = b2MakeBox( 1.5f, 0.125f ); + b2Polygon box2 = b2MakeBox( 0.125f, 1.5f ); + + b2CreatePolygonShape( bodyId, &shapeDef, &box1 ); + b2CreatePolygonShape( bodyId, &shapeDef, &box2 ); + + b2RevoluteJointDef jointDef = b2DefaultRevoluteJointDef(); + jointDef.bodyIdA = groundId; + jointDef.bodyIdB = bodyId; + jointDef.localAnchorA = bodyDef.position; + jointDef.localAnchorB = b2Vec2_zero; + jointDef.enableMotor = true; + jointDef.maxMotorTorque = 0.1f; + b2CreateRevoluteJoint( m_worldId, &jointDef ); + + bodyDef.position = { 4.0f, 8.0f }; + bodyId = b2CreateBody( m_worldId, &bodyDef ); + b2CreatePolygonShape( bodyId, &shapeDef, &box1 ); + b2CreatePolygonShape( bodyId, &shapeDef, &box2 ); + jointDef.localAnchorA = bodyDef.position; + jointDef.bodyIdB = bodyId; + b2CreateRevoluteJoint( m_worldId, &jointDef ); + } + + // Bumpers + { + b2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.position = { -4.0f, 8.0f }; + + b2BodyId bodyId = b2CreateBody( m_worldId, &bodyDef ); + + b2ShapeDef shapeDef = b2DefaultShapeDef(); + shapeDef.restitution = 1.5f; + + b2Circle circle = { { 0.0f, 0.0f }, 1.0f }; + b2CreateCircleShape( bodyId, &shapeDef, &circle ); + + bodyDef.position = { 4.0f, 17.0f }; + bodyId = b2CreateBody( m_worldId, &bodyDef ); + b2CreateCircleShape( bodyId, &shapeDef, &circle ); + } + + // Ball + { + b2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.position = { 1.0f, 15.0f }; + bodyDef.type = b2_dynamicBody; + bodyDef.isBullet = true; + + m_ballId = b2CreateBody( m_worldId, &bodyDef ); + + b2ShapeDef shapeDef = b2DefaultShapeDef(); + b2Circle circle = { { 0.0f, 0.0f }, 0.2f }; + b2CreateCircleShape( m_ballId, &shapeDef, &circle ); + } + } + + void Step( Settings& settings ) override + { + Sample::Step( settings ); + + if ( glfwGetKey( g_mainWindow, GLFW_KEY_SPACE ) == GLFW_PRESS ) + { + b2RevoluteJoint_SetMotorSpeed( m_leftJointId, 20.0f ); + b2RevoluteJoint_SetMotorSpeed( m_rightJointId, -20.0f ); + } + else + { + b2RevoluteJoint_SetMotorSpeed( m_leftJointId, -10.0f ); + b2RevoluteJoint_SetMotorSpeed( m_rightJointId, 10.0f ); + } + } + + static Sample* Create( Settings& settings ) + { + return new Pinball( settings ); + } + + b2JointId m_leftJointId; + b2JointId m_rightJointId; + b2BodyId m_ballId; +}; + +static int samplePinball = RegisterSample( "Continuous", "Pinball", Pinball::Create ); diff --git a/tests/cpp-tests/Source/Box2DTestBed/samples/sample_determinism.cpp b/tests/cpp-tests/Source/Box2DTestBed/samples/sample_determinism.cpp new file mode 100644 index 000000000000..29abdd43c5cf --- /dev/null +++ b/tests/cpp-tests/Source/Box2DTestBed/samples/sample_determinism.cpp @@ -0,0 +1,188 @@ +// SPDX-FileCopyrightText: 2022 Erin Catto +// SPDX-License-Identifier: MIT + +#include "draw.h" +#include "sample.h" +#include "settings.h" + +#include "box2d/box2d.h" +#include "box2d/math_functions.h" + +#include +#include +#include + +// This sample provides a visual representation of the cross platform determinism unit test. +// The scenario is designed to produce a chaotic result engaging: +// - continuous collision +// - joint limits (approximate atan2) +// - b2MakeRot (approximate sin/cos) +// Once all the bodies go to sleep the step counter and transform hash is emitted which +// can then be transferred to the unit test and tested in GitHub build actions. +// See CrossPlatformTest in the unit tests. +class FallingHinges : public Sample +{ +public: + enum + { + e_columns = 4, + e_rows = 30, + }; + + explicit FallingHinges( Settings& settings ) + : Sample( settings ) + { + if ( settings.restart == false ) + { + g_camera.m_center = { 0.0f, 7.5f }; + g_camera.m_zoom = 10.0f; + } + + { + b2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.position = { 0.0f, -1.0f }; + b2BodyId groundId = b2CreateBody( m_worldId, &bodyDef ); + + b2Polygon box = b2MakeBox( 20.0f, 1.0f ); + b2ShapeDef shapeDef = b2DefaultShapeDef(); + b2CreatePolygonShape( groundId, &shapeDef, &box ); + } + + for ( int i = 0; i < e_rows * e_columns; ++i ) + { + m_bodies[i] = b2_nullBodyId; + } + + float h = 0.25f; + float r = 0.1f * h; + b2Polygon box = b2MakeRoundedBox( h - r, h - r, r ); + + b2ShapeDef shapeDef = b2DefaultShapeDef(); + shapeDef.friction = 0.3f; + + float offset = 0.4f * h; + float dx = 10.0f * h; + float xroot = -0.5f * dx * ( e_columns - 1.0f ); + + b2RevoluteJointDef jointDef = b2DefaultRevoluteJointDef(); + jointDef.enableLimit = true; + jointDef.lowerAngle = -0.1f * b2_pi; + jointDef.upperAngle = 0.2f * b2_pi; + jointDef.enableSpring = true; + jointDef.hertz = 0.5f; + jointDef.dampingRatio = 0.5f; + jointDef.localAnchorA = { h, h }; + jointDef.localAnchorB = { offset, -h }; + jointDef.drawSize = 0.1f; + + int bodyIndex = 0; + int bodyCount = e_rows * e_columns; + + for ( int j = 0; j < e_columns; ++j ) + { + float x = xroot + j * dx; + + b2BodyId prevBodyId = b2_nullBodyId; + + for ( int i = 0; i < e_rows; ++i ) + { + b2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.type = b2_dynamicBody; + + bodyDef.position.x = x + offset * i; + bodyDef.position.y = h + 2.0f * h * i; + + // this tests the deterministic cosine and sine functions + bodyDef.rotation = b2MakeRot( 0.1f * i - 1.0f ); + + b2BodyId bodyId = b2CreateBody( m_worldId, &bodyDef ); + + if ((i & 1) == 0) + { + prevBodyId = bodyId; + } + else + { + jointDef.bodyIdA = prevBodyId; + jointDef.bodyIdB = bodyId; + b2CreateRevoluteJoint( m_worldId, &jointDef ); + prevBodyId = b2_nullBodyId; + } + + b2CreatePolygonShape( bodyId, &shapeDef, &box ); + + assert( bodyIndex < bodyCount ); + m_bodies[bodyIndex] = bodyId; + + bodyIndex += 1; + } + } + + m_hash = 0; + m_sleepStep = -1; + + //PrintTransforms(); + } + + void PrintTransforms() + { + uint32_t hash = B2_HASH_INIT; + int bodyCount = e_rows * e_columns; + for ( int i = 0; i < bodyCount; ++i ) + { + b2Transform xf = b2Body_GetTransform( m_bodies[i] ); + printf( "%d %.9f %.9f %.9f %.9f\n", i, xf.p.x, xf.p.y, xf.q.c, xf.q.s ); + hash = b2Hash( hash, reinterpret_cast( &xf ), sizeof( b2Transform ) ); + } + + printf( "hash = 0x%08x\n", hash ); + } + + void Step(Settings& settings) override + { + Sample::Step( settings ); + + if (m_hash == 0) + { + bool sleeping = true; + int bodyCount = e_rows * e_columns; + for (int i = 0; i < bodyCount; ++i) + { + if ( b2Body_IsAwake( m_bodies[i] ) == true ) + { + sleeping = false; + break; + } + } + + if (sleeping == true) + { + uint32_t hash = B2_HASH_INIT; + for ( int i = 0; i < bodyCount; ++i ) + { + b2Transform xf = b2Body_GetTransform( m_bodies[i] ); + //printf( "%d %.9f %.9f %.9f %.9f\n", i, xf.p.x, xf.p.y, xf.q.c, xf.q.s ); + hash = b2Hash( hash, reinterpret_cast( &xf ), sizeof( b2Transform ) ); + } + + m_sleepStep = m_stepCount - 1; + m_hash = hash; + printf( "sleep step = %d, hash = 0x%08x\n", m_sleepStep, m_hash ); + } + } + + g_draw.DrawString( 5, m_textLine, "sleep step = %d, hash = 0x%08x", m_sleepStep, m_hash ); + m_textLine += m_textIncrement; + } + + static Sample* Create( Settings& settings ) + { + return new FallingHinges( settings ); + } + + b2BodyId m_bodies[e_rows * e_columns]; + uint32_t m_hash; + int m_sleepStep; +}; + +static int sampleFallingHinges = RegisterSample( "Determinism", "Falling Hinges", FallingHinges::Create ); diff --git a/tests/cpp-tests/Source/Box2DTestBed/samples/sample_events.cpp b/tests/cpp-tests/Source/Box2DTestBed/samples/sample_events.cpp new file mode 100644 index 000000000000..e538ff2fdf7e --- /dev/null +++ b/tests/cpp-tests/Source/Box2DTestBed/samples/sample_events.cpp @@ -0,0 +1,1114 @@ +// SPDX-FileCopyrightText: 2022 Erin Catto +// SPDX-License-Identifier: MIT + +#include "donut.h" +#include "draw.h" +#include "human.h" +#include "sample.h" +#include "settings.h" + +#include "box2d/box2d.h" +#include "box2d/math_functions.h" + +#include +#include +#include + +class SensorEvent : public Sample +{ +public: + enum + { + e_donut = 1, + e_human = 2, + e_count = 32 + }; + + explicit SensorEvent( Settings& settings ) + : Sample( settings ) + { + if ( settings.restart == false ) + { + g_camera.m_center = { 0.0f, 0.0f }; + g_camera.m_zoom = 25.0f * 1.333f; + } + + settings.drawJoints = false; + + { + b2BodyDef bodyDef = b2DefaultBodyDef(); + b2BodyId groundId = b2CreateBody( m_worldId, &bodyDef ); + + // b2Vec2 points[] = { + //{42.333, 44.979}, {177.271, 44.979}, {177.271, 100.542}, {142.875, 121.708}, {177.271, 121.708}, + //{177.271, 171.979}, {142.875, 193.146}, {177.271, 193.146}, {177.271, 222.250}, {124.354, 261.938}, + //{124.354, 293.688}, {95.250, 293.688}, {95.250, 261.938}, {42.333, 222.250}, {42.333, 193.146}, + //{76.729, 193.146}, {42.333, 171.979}, {42.333, 121.708}, {76.729, 121.708}, {42.333, 100.542}, + //}; + + b2Vec2 points[] = { + { -16.8672504, 31.088623 }, { 16.8672485, 31.088623 }, { 16.8672485, 17.1978741 }, + { 8.26824951, 11.906374 }, { 16.8672485, 11.906374 }, { 16.8672485, -0.661376953 }, + { 8.26824951, -5.953125 }, { 16.8672485, -5.953125 }, { 16.8672485, -13.229126 }, + { 3.63799858, -23.151123 }, { 3.63799858, -31.088623 }, { -3.63800049, -31.088623 }, + { -3.63800049, -23.151123 }, { -16.8672504, -13.229126 }, { -16.8672504, -5.953125 }, + { -8.26825142, -5.953125 }, { -16.8672504, -0.661376953 }, { -16.8672504, 11.906374 }, + { -8.26825142, 11.906374 }, { -16.8672504, 17.1978741 }, + }; + + int count = sizeof( points ) / sizeof( points[0] ); + + // float scale = 0.25f; + // b2Vec2 lower = {FLT_MAX, FLT_MAX}; + // b2Vec2 upper = {-FLT_MAX, -FLT_MAX}; + // for (int i = 0; i < count; ++i) + //{ + // points[i].x = scale * points[i].x; + // points[i].y = -scale * points[i].y; + + // lower = b2Min(lower, points[i]); + // upper = b2Max(upper, points[i]); + //} + + // b2Vec2 center = b2MulSV(0.5f, b2Add(lower, upper)); + // for (int i = 0; i < count; ++i) + //{ + // points[i] = b2Sub(points[i], center); + // } + + // for (int i = 0; i < count / 2; ++i) + //{ + // b2Vec2 temp = points[i]; + // points[i] = points[count - 1 - i]; + // points[count - 1 - i] = temp; + // } + + // printf("{"); + // for (int i = 0; i < count; ++i) + //{ + // printf("{%.9g, %.9g},", points[i].x, points[i].y); + // } + // printf("};\n"); + + b2ChainDef chainDef = b2DefaultChainDef(); + chainDef.points = points; + chainDef.count = count; + chainDef.isLoop = true; + chainDef.friction = 0.2f; + b2CreateChain( groundId, &chainDef ); + + float sign = 1.0f; + float y = 14.0f; + for ( int i = 0; i < 3; ++i ) + { + b2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.position = { 0.0f, y }; + bodyDef.type = b2_dynamicBody; + + b2BodyId bodyId = b2CreateBody( m_worldId, &bodyDef ); + + b2Polygon box = b2MakeBox( 6.0f, 0.5f ); + b2ShapeDef shapeDef = b2DefaultShapeDef(); + shapeDef.friction = 0.1f; + shapeDef.restitution = 1.0f; + shapeDef.density = 1.0f; + + b2CreatePolygonShape( bodyId, &shapeDef, &box ); + + b2RevoluteJointDef revoluteDef = b2DefaultRevoluteJointDef(); + revoluteDef.bodyIdA = groundId; + revoluteDef.bodyIdB = bodyId; + revoluteDef.localAnchorA = bodyDef.position; + revoluteDef.localAnchorB = b2Vec2_zero; + revoluteDef.maxMotorTorque = 200.0f; + revoluteDef.motorSpeed = 2.0f * sign; + revoluteDef.enableMotor = true; + + b2CreateRevoluteJoint( m_worldId, &revoluteDef ); + + y -= 14.0f; + sign = -sign; + } + + { + b2Polygon box = b2MakeOffsetBox( 4.0f, 1.0f, { 0.0f, -30.5f }, b2Rot_identity ); + b2ShapeDef shapeDef = b2DefaultShapeDef(); + shapeDef.isSensor = true; + b2CreatePolygonShape( groundId, &shapeDef, &box ); + } + } + + m_wait = 0.5f; + m_side = -15.0f; + m_type = e_human; + + for ( int i = 0; i < e_count; ++i ) + { + m_isSpawned[i] = false; + } + + CreateElement(); + } + + void CreateElement() + { + int index = -1; + for ( int i = 0; i < e_count; ++i ) + { + if ( m_isSpawned[i] == false ) + { + index = i; + break; + } + } + + if ( index == -1 ) + { + return; + } + + b2Vec2 center = { m_side, 29.5f }; + + if ( m_type == e_donut ) + { + Donut* donut = m_donuts + index; + // donut->Spawn(m_worldId, center, index + 1, donut); + donut->Spawn( m_worldId, center, 1.0f, 0, donut ); + } + else + { + Human* human = m_humans + index; + human->Spawn( m_worldId, center, 2.0f, 0.05f, 0.0f, 0.0f, index + 1, human, false ); + } + + m_isSpawned[index] = true; + m_side = -m_side; + } + + void DestroyElement( int index ) + { + if ( m_type == e_donut ) + { + Donut* donut = m_donuts + index; + donut->Despawn(); + } + else + { + Human* human = m_humans + index; + human->Despawn(); + } + + m_isSpawned[index] = false; + } + + void Clear() + { + for ( int i = 0; i < e_count; ++i ) + { + if ( m_isSpawned[i] == true ) + { + if ( m_type == e_donut ) + { + m_donuts[i].Despawn(); + } + else + { + m_humans[i].Despawn(); + } + + m_isSpawned[i] = false; + } + } + } + + void UpdateUI() override + { + float height = 90.0f; + ImGui::SetNextWindowPos( ImVec2( 10.0f, g_camera.m_height - height - 50.0f ), ImGuiCond_Once ); + ImGui::SetNextWindowSize( ImVec2( 140.0f, height ) ); + + ImGui::Begin( "Sensor Event", nullptr, ImGuiWindowFlags_NoMove | ImGuiWindowFlags_NoResize ); + + if ( ImGui::RadioButton( "donut", m_type == e_donut ) ) + { + Clear(); + m_type = e_donut; + } + + if ( ImGui::RadioButton( "human", m_type == e_human ) ) + { + Clear(); + m_type = e_human; + } + + ImGui::End(); + } + + void Step( Settings& settings ) override + { + if ( m_stepCount == 832 ) + { + m_stepCount += 0; + } + + Sample::Step( settings ); + + // Discover rings that touch the bottom sensor + bool deferredDestructions[e_count] = {}; + b2SensorEvents sensorEvents = b2World_GetSensorEvents( m_worldId ); + for ( int i = 0; i < sensorEvents.beginCount; ++i ) + { + b2SensorBeginTouchEvent event = sensorEvents.beginEvents[i]; + b2ShapeId visitorId = event.visitorShapeId; + b2BodyId bodyId = b2Shape_GetBody( visitorId ); + + if ( m_type == e_donut ) + { + Donut* donut = (Donut*)b2Body_GetUserData( bodyId ); + if ( donut != nullptr ) + { + int index = (int)( donut - m_donuts ); + assert( 0 <= index && index < e_count ); + + // Defer destruction to avoid double destruction and event invalidation (orphaned shape ids) + deferredDestructions[index] = true; + } + } + else + { + Human* human = (Human*)b2Body_GetUserData( bodyId ); + if ( human != nullptr ) + { + int index = (int)( human - m_humans ); + assert( 0 <= index && index < e_count ); + + // Defer destruction to avoid double destruction and event invalidation (orphaned shape ids) + deferredDestructions[index] = true; + } + } + } + + // todo destroy mouse joint if necessary + + // Safely destroy rings that hit the bottom sensor + for ( int i = 0; i < e_count; ++i ) + { + if ( deferredDestructions[i] ) + { + DestroyElement( i ); + } + } + + if ( settings.hertz > 0.0f && settings.pause == false ) + { + m_wait -= 1.0f / settings.hertz; + if ( m_wait < 0.0f ) + { + CreateElement(); + m_wait += 0.5f; + } + } + } + + static Sample* Create( Settings& settings ) + { + return new SensorEvent( settings ); + } + + Human m_humans[e_count]; + Donut m_donuts[e_count]; + bool m_isSpawned[e_count]; + int m_type; + float m_wait; + float m_side; +}; + +static int sampleSensorEvent = RegisterSample( "Events", "Sensor", SensorEvent::Create ); + +struct BodyUserData +{ + int index; +}; + +class ContactEvent : public Sample +{ +public: + enum + { + e_count = 20 + }; + + explicit ContactEvent( Settings& settings ) + : Sample( settings ) + { + if ( settings.restart == false ) + { + g_camera.m_center = { 0.0f, 0.0f }; + g_camera.m_zoom = 25.0f * 1.75f; + } + + { + b2BodyDef bodyDef = b2DefaultBodyDef(); + b2BodyId groundId = b2CreateBody( m_worldId, &bodyDef ); + + b2Vec2 points[] = { { 40.0f, -40.0f }, { -40.0f, -40.0f }, { -40.0f, 40.0f }, { 40.0f, 40.0f } }; + + b2ChainDef chainDef = b2DefaultChainDef(); + chainDef.count = 4; + chainDef.points = points; + chainDef.isLoop = true; + + b2CreateChain( groundId, &chainDef ); + } + + // Player + { + b2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.type = b2_dynamicBody; + bodyDef.gravityScale = 0.0f; + bodyDef.linearDamping = 0.5f; + bodyDef.angularDamping = 0.5f; + bodyDef.isBullet = true; + m_playerId = b2CreateBody( m_worldId, &bodyDef ); + + b2Circle circle = { { 0.0f, 0.0f }, 1.0f }; + b2ShapeDef shapeDef = b2DefaultShapeDef(); + + // Enable contact events for the player shape + shapeDef.enableContactEvents = true; + + m_coreShapeId = b2CreateCircleShape( m_playerId, &shapeDef, &circle ); + } + + for ( int i = 0; i < e_count; ++i ) + { + m_debrisIds[i] = b2_nullBodyId; + m_bodyUserData[i].index = i; + } + + m_wait = 0.5f; + m_force = 200.0f; + } + + void SpawnDebris() + { + int index = -1; + for ( int i = 0; i < e_count; ++i ) + { + if ( B2_IS_NULL( m_debrisIds[i] ) ) + { + index = i; + break; + } + } + + if ( index == -1 ) + { + return; + } + + // Debris + b2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.type = b2_dynamicBody; + bodyDef.position = { RandomFloat( -38.0f, 38.0f ), RandomFloat( -38.0f, 38.0f ) }; + bodyDef.rotation = b2MakeRot( RandomFloat( -b2_pi, b2_pi ) ); + bodyDef.linearVelocity = { RandomFloat( -5.0f, 5.0f ), RandomFloat( -5.0f, 5.0f ) }; + bodyDef.angularVelocity = RandomFloat( -1.0f, 1.0f ); + bodyDef.gravityScale = 0.0f; + bodyDef.userData = m_bodyUserData + index; + m_debrisIds[index] = b2CreateBody( m_worldId, &bodyDef ); + + b2ShapeDef shapeDef = b2DefaultShapeDef(); + shapeDef.restitution = 0.8f; + + // No events when debris hits debris + shapeDef.enableContactEvents = false; + + if ( ( index + 1 ) % 3 == 0 ) + { + b2Circle circle = { { 0.0f, 0.0f }, 0.5f }; + b2CreateCircleShape( m_debrisIds[index], &shapeDef, &circle ); + } + else if ( ( index + 1 ) % 2 == 0 ) + { + b2Capsule capsule = { { 0.0f, -0.25f }, { 0.0f, 0.25f }, 0.25f }; + b2CreateCapsuleShape( m_debrisIds[index], &shapeDef, &capsule ); + } + else + { + b2Polygon box = b2MakeBox( 0.4f, 0.6f ); + b2CreatePolygonShape( m_debrisIds[index], &shapeDef, &box ); + } + } + + void UpdateUI() override + { + float height = 60.0f; + ImGui::SetNextWindowPos( ImVec2( 10.0f, g_camera.m_height - height - 50.0f ), ImGuiCond_Once ); + ImGui::SetNextWindowSize( ImVec2( 240.0f, height ) ); + + ImGui::Begin( "Contact Event", nullptr, ImGuiWindowFlags_NoMove | ImGuiWindowFlags_NoResize ); + + ImGui::SliderFloat( "force", &m_force, 100.0f, 500.0f, "%.1f" ); + + ImGui::End(); + } + + void Step( Settings& settings ) override + { + g_draw.DrawString( 5, m_textLine, "move using WASD" ); + m_textLine += m_textIncrement; + + b2Vec2 position = b2Body_GetPosition( m_playerId ); + + if ( glfwGetKey( g_mainWindow, GLFW_KEY_A ) == GLFW_PRESS ) + { + b2Body_ApplyForce( m_playerId, { -m_force, 0.0f }, position, true ); + } + + if ( glfwGetKey( g_mainWindow, GLFW_KEY_D ) == GLFW_PRESS ) + { + b2Body_ApplyForce( m_playerId, { m_force, 0.0f }, position, true ); + } + + if ( glfwGetKey( g_mainWindow, GLFW_KEY_W ) == GLFW_PRESS ) + { + b2Body_ApplyForce( m_playerId, { 0.0f, m_force }, position, true ); + } + + if ( glfwGetKey( g_mainWindow, GLFW_KEY_S ) == GLFW_PRESS ) + { + b2Body_ApplyForce( m_playerId, { 0.0f, -m_force }, position, true ); + } + + Sample::Step( settings ); + + // Discover rings that touch the bottom sensor + int debrisToAttach[e_count] = {}; + b2ShapeId shapesToDestroy[e_count] = { b2_nullShapeId }; + int attachCount = 0; + int destroyCount = 0; + + std::vector contactData; + + b2ContactEvents contactEvents = b2World_GetContactEvents( m_worldId ); + for ( int i = 0; i < contactEvents.beginCount; ++i ) + { + b2ContactBeginTouchEvent event = contactEvents.beginEvents[i]; + b2BodyId bodyIdA = b2Shape_GetBody( event.shapeIdA ); + b2BodyId bodyIdB = b2Shape_GetBody( event.shapeIdB ); + + int capacityA = b2Shape_GetContactCapacity( event.shapeIdA ); + contactData.resize( capacityA ); + int countA = b2Shape_GetContactData( event.shapeIdA, contactData.data(), capacityA ); + assert( countA >= 1 ); + + for (int j = 0; j < countA; ++j) + { + b2Manifold manifold = contactData[j].manifold; + b2Vec2 normal = manifold.normal; + assert( b2AbsFloat( b2Length( normal ) - 1.0f ) < 4.0f * FLT_EPSILON ); + + for (int k = 0; k < manifold.pointCount; ++k) + { + b2ManifoldPoint point = manifold.points[k]; + g_draw.DrawSegment( point.point, point.point + 4.0f * normal, b2_colorBlueViolet ); + g_draw.DrawPoint( point.point, 10.0f, b2_colorWhite ); + } + } + + int capacityB = b2Shape_GetContactCapacity( event.shapeIdB ); + contactData.resize( capacityB ); + int countB = b2Shape_GetContactData( event.shapeIdB, contactData.data(), capacityB ); + assert( countB >= 1 ); + + for (int j = 0; j < countB; ++j) + { + b2Manifold manifold = contactData[j].manifold; + b2Vec2 normal = manifold.normal; + assert( b2AbsFloat( b2Length( normal ) - 1.0f ) < 4.0f * FLT_EPSILON ); + + for (int k = 0; k < manifold.pointCount; ++k) + { + b2ManifoldPoint point = manifold.points[k]; + g_draw.DrawSegment( point.point, point.point + 4.0f * normal, b2_colorYellowGreen ); + g_draw.DrawPoint( point.point, 10.0f, b2_colorWhite ); + } + } + + if ( B2_ID_EQUALS( bodyIdA, m_playerId ) ) + { + BodyUserData* userDataB = static_cast( b2Body_GetUserData( bodyIdB ) ); + if ( userDataB == nullptr ) + { + if ( B2_ID_EQUALS( event.shapeIdA, m_coreShapeId ) == false && destroyCount < e_count ) + { + // player non-core shape hit the wall + + bool found = false; + for ( int j = 0; j < destroyCount; ++j ) + { + if ( B2_ID_EQUALS( event.shapeIdA, shapesToDestroy[j] ) ) + { + found = true; + break; + } + } + + // avoid double deletion + if ( found == false ) + { + shapesToDestroy[destroyCount] = event.shapeIdA; + destroyCount += 1; + } + } + } + else if ( attachCount < e_count ) + { + debrisToAttach[attachCount] = userDataB->index; + attachCount += 1; + } + } + else + { + // Only expect events for the player + assert( B2_ID_EQUALS( bodyIdB, m_playerId ) ); + BodyUserData* userDataA = static_cast( b2Body_GetUserData( bodyIdA ) ); + if ( userDataA == nullptr ) + { + if ( B2_ID_EQUALS( event.shapeIdB, m_coreShapeId ) == false && destroyCount < e_count ) + { + // player non-core shape hit the wall + + bool found = false; + for ( int j = 0; j < destroyCount; ++j ) + { + if ( B2_ID_EQUALS( event.shapeIdB, shapesToDestroy[j] ) ) + { + found = true; + break; + } + } + + // avoid double deletion + if ( found == false ) + { + shapesToDestroy[destroyCount] = event.shapeIdB; + destroyCount += 1; + } + } + } + else if ( attachCount < e_count ) + { + debrisToAttach[attachCount] = userDataA->index; + attachCount += 1; + } + } + } + + // Attach debris to player body + for ( int i = 0; i < attachCount; ++i ) + { + int index = debrisToAttach[i]; + b2BodyId debrisId = m_debrisIds[index]; + if ( B2_IS_NULL( debrisId ) ) + { + continue; + } + + b2Transform playerTransform = b2Body_GetTransform( m_playerId ); + b2Transform debrisTransform = b2Body_GetTransform( debrisId ); + b2Transform relativeTransform = b2InvMulTransforms( playerTransform, debrisTransform ); + + int shapeCount = b2Body_GetShapeCount( debrisId ); + if ( shapeCount == 0 ) + { + continue; + } + + b2ShapeId shapeId; + b2Body_GetShapes( debrisId, &shapeId, 1 ); + + b2ShapeType type = b2Shape_GetType( shapeId ); + + b2ShapeDef shapeDef = b2DefaultShapeDef(); + shapeDef.enableContactEvents = true; + + switch ( type ) + { + case b2_circleShape: + { + b2Circle circle = b2Shape_GetCircle( shapeId ); + circle.center = b2TransformPoint( relativeTransform, circle.center ); + + b2CreateCircleShape( m_playerId, &shapeDef, &circle ); + } + break; + + case b2_capsuleShape: + { + b2Capsule capsule = b2Shape_GetCapsule( shapeId ); + capsule.center1 = b2TransformPoint( relativeTransform, capsule.center1 ); + capsule.center2 = b2TransformPoint( relativeTransform, capsule.center2 ); + + b2CreateCapsuleShape( m_playerId, &shapeDef, &capsule ); + } + break; + + case b2_polygonShape: + { + b2Polygon originalPolygon = b2Shape_GetPolygon( shapeId ); + b2Polygon polygon = b2TransformPolygon( relativeTransform, &originalPolygon ); + + b2CreatePolygonShape( m_playerId, &shapeDef, &polygon ); + } + break; + + default: + assert( false ); + } + + b2DestroyBody( debrisId ); + m_debrisIds[index] = b2_nullBodyId; + } + + for ( int i = 0; i < destroyCount; ++i ) + { + b2DestroyShape( shapesToDestroy[i] ); + } + + if ( settings.hertz > 0.0f && settings.pause == false ) + { + m_wait -= 1.0f / settings.hertz; + if ( m_wait < 0.0f ) + { + SpawnDebris(); + m_wait += 0.5f; + } + } + } + + static Sample* Create( Settings& settings ) + { + return new ContactEvent( settings ); + } + + b2BodyId m_playerId; + b2ShapeId m_coreShapeId; + b2BodyId m_debrisIds[e_count]; + BodyUserData m_bodyUserData[e_count]; + float m_force; + float m_wait; +}; + +static int sampleWeeble = RegisterSample( "Events", "Contact", ContactEvent::Create ); + +// Shows how to make a rigid body character mover and use the pre-solve callback. +class Platformer : public Sample +{ +public: + explicit Platformer( Settings& settings ) + : Sample( settings ) + { + if ( settings.restart == false ) + { + g_camera.m_center = { 0.5f, 7.5f }; + g_camera.m_zoom = 25.0f * 0.4f; + } + + b2World_SetPreSolveCallback( m_worldId, PreSolveStatic, this ); + + // Ground + { + b2BodyDef bodyDef = b2DefaultBodyDef(); + b2BodyId groundId = b2CreateBody( m_worldId, &bodyDef ); + b2ShapeDef shapeDef = b2DefaultShapeDef(); + b2Segment segment = { { -20.0f, 0.0f }, { 20.0f, 0.0f } }; + b2CreateSegmentShape( groundId, &shapeDef, &segment ); + } + + // Platform + { + b2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.type = b2_kinematicBody; + bodyDef.position = { 0.0f, 6.0f }; + bodyDef.linearVelocity = { 2.0f, 0.0f }; + m_platformId = b2CreateBody( m_worldId, &bodyDef ); + + b2ShapeDef shapeDef = b2DefaultShapeDef(); + b2Polygon box = b2MakeBox( 3.0f, 0.5f ); + m_platformShapeId = b2CreatePolygonShape( m_platformId, &shapeDef, &box ); + } + + // Actor + { + b2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.type = b2_dynamicBody; + bodyDef.fixedRotation = true; + bodyDef.linearDamping = 0.5f; + bodyDef.position = { 0.0f, 1.0f }; + m_characterId = b2CreateBody( m_worldId, &bodyDef ); + + m_radius = 0.5f; + b2Capsule capsule = { { 0.0f, 0.0f }, { 0.0f, 1.0f }, m_radius }; + b2ShapeDef shapeDef = b2DefaultShapeDef(); + shapeDef.friction = 0.1f; + + // Need to turn this on to get the callback + shapeDef.enablePreSolveEvents = true; + + b2CreateCapsuleShape( m_characterId, &shapeDef, &capsule ); + } + + m_force = 25.0f; + m_impulse = 25.0f; + m_jumpDelay = 0.25f; + m_jumping = false; + } + + static bool PreSolveStatic( b2ShapeId shapeIdA, b2ShapeId shapeIdB, b2Manifold* manifold, void* context ) + { + Platformer* platformer = static_cast( context ); + return platformer->PreSolve( shapeIdA, shapeIdB, manifold ); + } + + // This callback must be thread-safe. It may be called multiple times simultaneously. + // Notice how this method is constant and doesn't change any data. It also + // does not try to access any values in the world that may be changing, such as contact data. + bool PreSolve( b2ShapeId shapeIdA, b2ShapeId shapeIdB, b2Manifold* manifold ) const + { + assert( b2Shape_IsValid( shapeIdA ) ); + assert( b2Shape_IsValid( shapeIdB ) ); + + b2ShapeId actorShapeId = b2_nullShapeId; + float sign = 0.0f; + if ( B2_ID_EQUALS( shapeIdA, m_platformShapeId ) ) + { + sign = 1.0f; + actorShapeId = shapeIdB; + } + else if ( B2_ID_EQUALS( shapeIdB, m_platformShapeId ) ) + { + sign = -1.0f; + actorShapeId = shapeIdA; + } + else + { + // not the platform, enable contact + return true; + } + + b2BodyId bodyId = b2Shape_GetBody( actorShapeId ); + if ( B2_ID_EQUALS( bodyId, m_characterId ) == false ) + { + // not the character, enable contact + return true; + } + + b2Vec2 normal = manifold->normal; + if ( sign * normal.y > 0.95f ) + { + return true; + } + + float separation = 0.0f; + for ( int i = 0; i < manifold->pointCount; ++i ) + { + float s = manifold->points[i].separation; + separation = separation < s ? separation : s; + } + + if ( separation > 0.1f * m_radius ) + { + // shallow overlap + return true; + } + + // normal points down, disable contact + return false; + } + + void UpdateUI() override + { + float height = 100.0f; + ImGui::SetNextWindowPos( ImVec2( 10.0f, g_camera.m_height - height - 50.0f ), ImGuiCond_Once ); + ImGui::SetNextWindowSize( ImVec2( 240.0f, height ) ); + + ImGui::Begin( "Platformer", nullptr, ImGuiWindowFlags_NoMove | ImGuiWindowFlags_NoResize ); + + ImGui::SliderFloat( "force", &m_force, 0.0f, 50.0f, "%.1f" ); + ImGui::SliderFloat( "impulse", &m_impulse, 0.0f, 50.0f, "%.1f" ); + + ImGui::End(); + } + + void Step( Settings& settings ) override + { + bool canJump = false; + b2Vec2 velocity = b2Body_GetLinearVelocity( m_characterId ); + if ( m_jumpDelay == 0.0f && m_jumping == false && velocity.y < 0.01f ) + { + int capacity = b2Body_GetContactCapacity( m_characterId ); + capacity = b2MinInt( capacity, 4 ); + b2ContactData contactData[4]; + int count = b2Body_GetContactData( m_characterId, contactData, capacity ); + for ( int i = 0; i < count; ++i ) + { + b2BodyId bodyIdA = b2Shape_GetBody( contactData[i].shapeIdA ); + float sign = 0.0f; + if ( B2_ID_EQUALS( bodyIdA, m_characterId ) ) + { + // normal points from A to B + sign = -1.0f; + } + else + { + sign = 1.0f; + } + + if ( sign * contactData[i].manifold.normal.y > 0.9f ) + { + canJump = true; + break; + } + } + } + + // A kinematic body is moved by setting its velocity. This + // ensure friction works correctly. + b2Vec2 platformPosition = b2Body_GetPosition( m_platformId ); + if ( platformPosition.x < -15.0f ) + { + b2Body_SetLinearVelocity( m_platformId, { 2.0f, 0.0f } ); + } + else if ( platformPosition.x > 15.0f ) + { + b2Body_SetLinearVelocity( m_platformId, { -2.0f, 0.0f } ); + } + + if ( glfwGetKey( g_mainWindow, GLFW_KEY_A ) == GLFW_PRESS ) + { + b2Body_ApplyForceToCenter( m_characterId, { -m_force, 0.0f }, true ); + } + + if ( glfwGetKey( g_mainWindow, GLFW_KEY_D ) == GLFW_PRESS ) + { + b2Body_ApplyForceToCenter( m_characterId, { m_force, 0.0f }, true ); + } + + int keyState = glfwGetKey( g_mainWindow, GLFW_KEY_SPACE ); + if ( keyState == GLFW_PRESS ) + { + if ( canJump ) + { + b2Body_ApplyLinearImpulseToCenter( m_characterId, { 0.0f, m_impulse }, true ); + m_jumpDelay = 0.5f; + m_jumping = true; + } + } + else + { + m_jumping = false; + } + + Sample::Step( settings ); + + g_draw.DrawString( 5, m_textLine, "Movement: A/D/Space" ); + m_textLine += m_textIncrement; + + g_draw.DrawString( 5, m_textLine, "Can jump = %s", canJump ? "true" : "false" ); + m_textLine += m_textIncrement; + + if ( settings.hertz > 0.0f ) + { + m_jumpDelay = b2MaxFloat( 0.0f, m_jumpDelay - 1.0f / settings.hertz ); + } + } + + static Sample* Create( Settings& settings ) + { + return new Platformer( settings ); + } + + bool m_jumping; + float m_radius; + float m_force; + float m_impulse; + float m_jumpDelay; + b2BodyId m_characterId; + b2BodyId m_platformId; + b2ShapeId m_platformShapeId; +}; + +static int samplePlatformer = RegisterSample( "Events", "Platformer", Platformer::Create ); + +// This shows how to process body events. +class BodyMove : public Sample +{ +public: + enum + { + e_count = 50 + }; + + explicit BodyMove( Settings& settings ) + : Sample( settings ) + { + if ( settings.restart == false ) + { + g_camera.m_center = { 2.0f, 8.0f }; + g_camera.m_zoom = 25.0f * 0.55f; + } + + { + b2BodyDef bodyDef = b2DefaultBodyDef(); + b2BodyId groundId = b2CreateBody( m_worldId, &bodyDef ); + + b2ShapeDef shapeDef = b2DefaultShapeDef(); + shapeDef.friction = 0.1f; + + b2Polygon box = b2MakeOffsetBox( 12.0f, 0.1f, { -10.0f, -0.1f }, b2MakeRot(-0.15f * b2_pi) ); + b2CreatePolygonShape( groundId, &shapeDef, &box ); + + box = b2MakeOffsetBox( 12.0f, 0.1f, { 10.0f, -0.1f }, b2MakeRot(0.15f * b2_pi) ); + b2CreatePolygonShape( groundId, &shapeDef, &box ); + + shapeDef.restitution = 0.8f; + + box = b2MakeOffsetBox( 0.1f, 10.0f, { 19.9f, 10.0f }, b2Rot_identity ); + b2CreatePolygonShape( groundId, &shapeDef, &box ); + + box = b2MakeOffsetBox( 0.1f, 10.0f, { -19.9f, 10.0f }, b2Rot_identity ); + b2CreatePolygonShape( groundId, &shapeDef, &box ); + + box = b2MakeOffsetBox( 20.0f, 0.1f, { 0.0f, 20.1f }, b2Rot_identity ); + b2CreatePolygonShape( groundId, &shapeDef, &box ); + } + + m_sleepCount = 0; + m_count = 0; + + m_explosionPosition = { 0.0f, -5.0f }; + m_explosionRadius = 10.0f; + m_explosionMagnitude = 6.0f; + } + + void CreateBodies() + { + b2Capsule capsule = { { -0.25f, 0.0f }, { 0.25f, 0.0f }, 0.25f }; + b2Circle circle = { { 0.0f, 0.0f }, 0.35f }; + b2Polygon square = b2MakeSquare( 0.35f ); + + b2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.type = b2_dynamicBody; + b2ShapeDef shapeDef = b2DefaultShapeDef(); + + float x = -5.0f, y = 10.0f; + for ( int32_t i = 0; i < 10 && m_count < e_count; ++i ) + { + bodyDef.position = { x, y }; + bodyDef.userData = m_bodyIds + m_count; + m_bodyIds[m_count] = b2CreateBody( m_worldId, &bodyDef ); + m_sleeping[m_count] = false; + + int remainder = m_count % 4; + if ( remainder == 0 ) + { + b2CreateCapsuleShape( m_bodyIds[m_count], &shapeDef, &capsule ); + } + else if ( remainder == 1 ) + { + b2CreateCircleShape( m_bodyIds[m_count], &shapeDef, &circle ); + } + else if ( remainder == 2 ) + { + b2CreatePolygonShape( m_bodyIds[m_count], &shapeDef, &square ); + } + else + { + b2Polygon poly = RandomPolygon( 0.75f ); + poly.radius = 0.1f; + b2CreatePolygonShape( m_bodyIds[m_count], &shapeDef, &poly ); + } + + m_count += 1; + x += 1.0f; + } + } + + void UpdateUI() override + { + float height = 100.0f; + ImGui::SetNextWindowPos( ImVec2( 10.0f, g_camera.m_height - height - 50.0f ), ImGuiCond_Once ); + ImGui::SetNextWindowSize( ImVec2( 240.0f, height ) ); + + ImGui::Begin( "Body Move", nullptr, ImGuiWindowFlags_NoMove | ImGuiWindowFlags_NoResize ); + + if ( ImGui::Button( "Explode" ) ) + { + b2World_Explode( m_worldId, m_explosionPosition, m_explosionRadius, m_explosionMagnitude ); + } + + ImGui::SliderFloat( "Magnitude", &m_explosionMagnitude, -8.0f, 8.0f, "%.1f" ); + + ImGui::End(); + } + + void Step( Settings& settings ) override + { + if ( settings.pause == false && ( m_stepCount & 15 ) == 15 && m_count < e_count ) + { + CreateBodies(); + } + + Sample::Step( settings ); + + // Process body events + b2BodyEvents events = b2World_GetBodyEvents( m_worldId ); + for ( int i = 0; i < events.moveCount; ++i ) + { + // draw the transform of every body that moved (not sleeping) + const b2BodyMoveEvent* event = events.moveEvents + i; + g_draw.DrawTransform( event->transform ); + + // this shows a somewhat contrived way to track body sleeping + b2BodyId* bodyId = static_cast( event->userData ); + ptrdiff_t diff = bodyId - m_bodyIds; + bool* sleeping = m_sleeping + diff; + + if ( event->fellAsleep ) + { + *sleeping = true; + m_sleepCount += 1; + } + else + { + if ( *sleeping ) + { + *sleeping = false; + m_sleepCount -= 1; + } + } + } + + g_draw.DrawCircle( m_explosionPosition, m_explosionRadius, b2_colorAzure ); + + g_draw.DrawString( 5, m_textLine, "sleep count: %d", m_sleepCount ); + m_textLine += m_textIncrement; + } + + static Sample* Create( Settings& settings ) + { + return new BodyMove( settings ); + } + + b2BodyId m_bodyIds[e_count]; + bool m_sleeping[e_count]; + int m_count; + int m_sleepCount; + b2Vec2 m_explosionPosition; + float m_explosionRadius; + float m_explosionMagnitude; +}; + +static int sampleBodyMove = RegisterSample( "Events", "Body Move", BodyMove::Create ); diff --git a/tests/cpp-tests/Source/Box2DTestBed/samples/sample_geometry.cpp b/tests/cpp-tests/Source/Box2DTestBed/samples/sample_geometry.cpp new file mode 100644 index 000000000000..7ed1a8dc77c2 --- /dev/null +++ b/tests/cpp-tests/Source/Box2DTestBed/samples/sample_geometry.cpp @@ -0,0 +1,220 @@ +// SPDX-FileCopyrightText: 2023 Erin Catto +// SPDX-License-Identifier: MIT + +#include "draw.h" +#include "sample.h" +#include "settings.h" + +#include "box2d/math_functions.h" + +#include + +class ConvexHull : public Sample +{ +public: + enum + { + e_count = b2_maxPolygonVertices + }; + + explicit ConvexHull( Settings& settings ) + : Sample( settings ) + { + if ( settings.restart == false ) + { + g_camera.m_center = { 0.5f, 0.0f }; + g_camera.m_zoom = 25.0f * 0.3f; + } + + m_generation = 0; + m_auto = false; + m_bulk = false; + Generate(); + } + + void Generate() + { +#if 0 + m_points[0] = { 5.65314484f, 0.204832315f }; + m_points[1] = {-5.65314484f, -0.204832315f }; + m_points[2] = {2.34463644f, 1.15731204f }; + m_points[3] = {0.0508846045f, 3.23230696f }; + m_points[4] = {-5.65314484f, -0.204832315f }; + m_points[5] = {-5.65314484f, -0.204832315f }; + m_points[6] = {3.73758054f, -1.11098099f }; + m_points[7] = {1.33504069f, -4.43795443f }; + + m_count = e_count; +#elif 0 + m_points[0] = { -0.328125, 0.179688 }; + m_points[1] = { -0.203125, 0.304688 }; + m_points[2] = { 0.171875, 0.304688 }; + m_points[3] = { 0.359375, 0.117188 }; + m_points[4] = { 0.359375, -0.195313 }; + m_points[5] = { 0.234375, -0.320313 }; + m_points[6] = { -0.265625, -0.257813 }; + m_points[7] = { -0.328125, -0.132813 }; + + b2Hull hull = b2ComputeHull( m_points, 8 ); + bool valid = b2ValidateHull( &hull ); + if ( valid == false ) + { + assert( valid ); + } + + m_count = e_count; +#else + + float angle = b2_pi * RandomFloat(); + b2Rot r = b2MakeRot( angle ); + + b2Vec2 lowerBound = { -4.0f, -4.0f }; + b2Vec2 upperBound = { 4.0f, 4.0f }; + + for ( int i = 0; i < e_count; ++i ) + { + float x = 10.0f * RandomFloat(); + float y = 10.0f * RandomFloat(); + + // Clamp onto a square to help create collinearities. + // This will stress the convex hull algorithm. + b2Vec2 v = b2Clamp( { x, y }, lowerBound, upperBound ); + m_points[i] = b2RotateVector( r, v ); + } + + m_count = e_count; +#endif + + m_generation += 1; + } + + void Keyboard( int key ) override + { + switch ( key ) + { + case GLFW_KEY_A: + m_auto = !m_auto; + break; + + case GLFW_KEY_B: + m_bulk = !m_bulk; + break; + + case GLFW_KEY_G: + Generate(); + break; + + default: + break; + } + } + + void Step( Settings& settings ) override + { + Sample::Step( settings ); + + g_draw.DrawString( 5, m_textLine, "Options: generate(g), auto(a), bulk(b)" ); + m_textLine += m_textIncrement; + + b2Hull hull; + bool valid = false; + float milliseconds = 0.0f; + + if ( m_bulk ) + { +#if 1 + // defect hunting + for ( int i = 0; i < 10000; ++i ) + { + Generate(); + hull = b2ComputeHull( m_points, m_count ); + if ( hull.count == 0 ) + { + // m_bulk = false; + // break; + continue; + } + + valid = b2ValidateHull( &hull ); + if ( valid == false || m_bulk == false ) + { + m_bulk = false; + break; + } + } +#else + // performance + Generate(); + b2Timer timer; + for ( int i = 0; i < 1000000; ++i ) + { + hull = b2ComputeHull( m_points, m_count ); + } + valid = hull.count > 0; + milliseconds = timer.GetMilliseconds(); +#endif + } + else + { + if ( m_auto ) + { + Generate(); + } + + hull = b2ComputeHull( m_points, m_count ); + if ( hull.count > 0 ) + { + valid = b2ValidateHull( &hull ); + if ( valid == false ) + { + m_auto = false; + } + } + } + + if ( valid == false ) + { + g_draw.DrawString( 5, m_textLine, "generation = %d, FAILED", m_generation ); + m_textLine += m_textIncrement; + } + else + { + g_draw.DrawString( 5, m_textLine, "generation = %d, count = %d", m_generation, hull.count ); + m_textLine += m_textIncrement; + } + + if ( milliseconds > 0.0f ) + { + g_draw.DrawString( 5, m_textLine, "milliseconds = %g", milliseconds ); + m_textLine += m_textIncrement; + } + + m_textLine += m_textIncrement; + + g_draw.DrawPolygon( hull.points, hull.count, b2_colorGray ); + + for ( int32_t i = 0; i < m_count; ++i ) + { + g_draw.DrawPoint( m_points[i], 5.0f, b2_colorBlue ); + g_draw.DrawString( b2Add( m_points[i], { 0.1f, 0.1f } ), "%d", i ); + } + + for ( int32_t i = 0; i < hull.count; ++i ) + { + g_draw.DrawPoint( hull.points[i], 6.0f, b2_colorGreen ); + } + } + + static Sample* Create( Settings& settings ) + { + return new ConvexHull( settings ); + } + + b2Vec2 m_points[b2_maxPolygonVertices]; + int32_t m_count; + int32_t m_generation; + bool m_auto; + bool m_bulk; +}; + +static int sampleIndex = RegisterSample( "Geometry", "Convex Hull", ConvexHull::Create ); diff --git a/tests/cpp-tests/Source/Box2DTestBed/samples/sample_joints.cpp b/tests/cpp-tests/Source/Box2DTestBed/samples/sample_joints.cpp new file mode 100644 index 000000000000..ace8c28089a7 --- /dev/null +++ b/tests/cpp-tests/Source/Box2DTestBed/samples/sample_joints.cpp @@ -0,0 +1,2467 @@ +// SPDX-FileCopyrightText: 2022 Erin Catto +// SPDX-License-Identifier: MIT + +#include "car.h" +#include "donut.h" +#include "doohickey.h" +#include "draw.h" +#include "human.h" +#include "sample.h" +#include "settings.h" + +#include "box2d/box2d.h" +#include "box2d/math_functions.h" + +#include +#include + +// Test the distance joint and all options +class DistanceJoint : public Sample +{ +public: + enum + { + e_maxCount = 10 + }; + + explicit DistanceJoint( Settings& settings ) + : Sample( settings ) + { + if ( settings.restart == false ) + { + g_camera.m_center = { 0.0f, 12.0f }; + g_camera.m_zoom = 25.0f * 0.35f; + } + + { + b2BodyDef bodyDef = b2DefaultBodyDef(); + m_groundId = b2CreateBody( m_worldId, &bodyDef ); + } + + m_count = 0; + m_hertz = 2.0f; + m_dampingRatio = 0.5f; + m_length = 1.0f; + m_minLength = m_length; + m_maxLength = m_length; + m_enableSpring = false; + m_enableLimit = false; + + for ( int i = 0; i < e_maxCount; ++i ) + { + m_bodyIds[i] = b2_nullBodyId; + m_jointIds[i] = b2_nullJointId; + } + + CreateScene( 1 ); + } + + void CreateScene( int newCount ) + { + // Must destroy joints before bodies + for ( int i = 0; i < m_count; ++i ) + { + b2DestroyJoint( m_jointIds[i] ); + m_jointIds[i] = b2_nullJointId; + } + + for ( int i = 0; i < m_count; ++i ) + { + b2DestroyBody( m_bodyIds[i] ); + m_bodyIds[i] = b2_nullBodyId; + } + + m_count = newCount; + + float radius = 0.25f; + b2Circle circle = { { 0.0f, 0.0f }, radius }; + + b2ShapeDef shapeDef = b2DefaultShapeDef(); + shapeDef.density = 20.0f; + + float yOffset = 20.0f; + + b2DistanceJointDef jointDef = b2DefaultDistanceJointDef(); + jointDef.hertz = m_hertz; + jointDef.dampingRatio = m_dampingRatio; + jointDef.length = m_length; + jointDef.minLength = m_minLength; + jointDef.maxLength = m_maxLength; + jointDef.enableSpring = m_enableSpring; + jointDef.enableLimit = m_enableLimit; + + b2BodyId prevBodyId = m_groundId; + for ( int i = 0; i < m_count; ++i ) + { + b2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.type = b2_dynamicBody; + bodyDef.angularDamping = 0.1f; + bodyDef.position = { m_length * ( i + 1.0f ), yOffset }; + m_bodyIds[i] = b2CreateBody( m_worldId, &bodyDef ); + b2CreateCircleShape( m_bodyIds[i], &shapeDef, &circle ); + + b2Vec2 pivotA = { m_length * i, yOffset }; + b2Vec2 pivotB = { m_length * ( i + 1.0f ), yOffset }; + jointDef.bodyIdA = prevBodyId; + jointDef.bodyIdB = m_bodyIds[i]; + jointDef.localAnchorA = b2Body_GetLocalPoint( jointDef.bodyIdA, pivotA ); + jointDef.localAnchorB = b2Body_GetLocalPoint( jointDef.bodyIdB, pivotB ); + m_jointIds[i] = b2CreateDistanceJoint( m_worldId, &jointDef ); + + prevBodyId = m_bodyIds[i]; + } + } + + void UpdateUI() override + { + float height = 240.0f; + ImGui::SetNextWindowPos( ImVec2( 10.0f, g_camera.m_height - height - 50.0f ), ImGuiCond_Once ); + ImGui::SetNextWindowSize( ImVec2( 180.0f, height ) ); + + ImGui::Begin( "Distance Joint", nullptr, ImGuiWindowFlags_NoResize ); + ImGui::PushItemWidth( 100.0f ); + + if ( ImGui::SliderFloat( "Length", &m_length, 0.1f, 4.0f, "%3.1f" ) ) + { + for ( int i = 0; i < m_count; ++i ) + { + b2DistanceJoint_SetLength( m_jointIds[i], m_length ); + b2Joint_WakeBodies( m_jointIds[i] ); + } + } + + if ( ImGui::Checkbox( "Spring", &m_enableSpring ) ) + { + for ( int i = 0; i < m_count; ++i ) + { + b2DistanceJoint_EnableSpring( m_jointIds[i], m_enableSpring ); + b2Joint_WakeBodies( m_jointIds[i] ); + } + } + + if ( m_enableSpring ) + { + if ( ImGui::SliderFloat( "Hertz", &m_hertz, 0.0f, 15.0f, "%3.1f" ) ) + { + for ( int i = 0; i < m_count; ++i ) + { + b2DistanceJoint_SetSpringHertz( m_jointIds[i], m_hertz ); + b2Joint_WakeBodies( m_jointIds[i] ); + } + } + + if ( ImGui::SliderFloat( "Damping", &m_dampingRatio, 0.0f, 4.0f, "%3.1f" ) ) + { + for ( int i = 0; i < m_count; ++i ) + { + b2DistanceJoint_SetSpringDampingRatio( m_jointIds[i], m_dampingRatio ); + b2Joint_WakeBodies( m_jointIds[i] ); + } + } + } + + if ( ImGui::Checkbox( "Limit", &m_enableLimit ) ) + { + for ( int i = 0; i < m_count; ++i ) + { + b2DistanceJoint_EnableLimit( m_jointIds[i], m_enableLimit ); + b2Joint_WakeBodies( m_jointIds[i] ); + } + } + + if ( m_enableLimit ) + { + if ( ImGui::SliderFloat( "Min Length", &m_minLength, 0.1f, 4.0f, "%3.1f" ) ) + { + for ( int i = 0; i < m_count; ++i ) + { + b2DistanceJoint_SetLengthRange( m_jointIds[i], m_minLength, m_maxLength ); + b2Joint_WakeBodies( m_jointIds[i] ); + } + } + + if ( ImGui::SliderFloat( "Max Length", &m_maxLength, 0.1f, 4.0f, "%3.1f" ) ) + { + for ( int i = 0; i < m_count; ++i ) + { + b2DistanceJoint_SetLengthRange( m_jointIds[i], m_minLength, m_maxLength ); + b2Joint_WakeBodies( m_jointIds[i] ); + } + } + } + + int count = m_count; + if ( ImGui::SliderInt( "Count", &count, 1, e_maxCount ) ) + { + CreateScene( count ); + } + + ImGui::PopItemWidth(); + ImGui::End(); + } + + static Sample* Create( Settings& settings ) + { + return new DistanceJoint( settings ); + } + + b2BodyId m_groundId; + b2BodyId m_bodyIds[e_maxCount]; + b2JointId m_jointIds[e_maxCount]; + int m_count; + float m_hertz; + float m_dampingRatio; + float m_length; + float m_minLength; + float m_maxLength; + bool m_enableSpring; + bool m_enableLimit; +}; + +static int sampleDistanceJoint = RegisterSample( "Joints", "Distance Joint", DistanceJoint::Create ); + +/// This test shows how to use a motor joint. A motor joint +/// can be used to animate a dynamic body. With finite motor forces +/// the body can be blocked by collision with other bodies. +/// By setting the correction factor to zero, the motor joint acts +/// like top-down dry friction. +class MotorJoint : public Sample +{ +public: + explicit MotorJoint( Settings& settings ) + : Sample( settings ) + { + if ( settings.restart == false ) + { + g_camera.m_center = { 0.0f, 7.0f }; + g_camera.m_zoom = 25.0f * 0.4f; + } + + b2BodyId groundId; + { + b2BodyDef bodyDef = b2DefaultBodyDef(); + groundId = b2CreateBody( m_worldId, &bodyDef ); + b2ShapeDef shapeDef = b2DefaultShapeDef(); + b2Segment segment = { { -20.0f, 0.0f }, { 20.0f, 0.0f } }; + b2CreateSegmentShape( groundId, &shapeDef, &segment ); + } + + // Define motorized body + { + b2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.type = b2_dynamicBody; + bodyDef.position = { 0.0f, 8.0f }; + b2BodyId bodyId = b2CreateBody( m_worldId, &bodyDef ); + + b2Polygon box = b2MakeBox( 2.0f, 0.5f ); + b2ShapeDef shapeDef = b2DefaultShapeDef(); + shapeDef.density = 1.0f; + b2CreatePolygonShape( bodyId, &shapeDef, &box ); + + m_maxForce = 500.0f; + m_maxTorque = 500.0f; + m_correctionFactor = 0.3f; + + b2MotorJointDef jointDef = b2DefaultMotorJointDef(); + jointDef.bodyIdA = groundId; + jointDef.bodyIdB = bodyId; + jointDef.maxForce = m_maxForce; + jointDef.maxTorque = m_maxTorque; + jointDef.correctionFactor = m_correctionFactor; + + m_jointId = b2CreateMotorJoint( m_worldId, &jointDef ); + } + + m_go = true; + m_time = 0.0f; + } + + void UpdateUI() override + { + float height = 140.0f; + ImGui::SetNextWindowPos( ImVec2( 10.0f, g_camera.m_height - height - 50.0f ), ImGuiCond_Once ); + ImGui::SetNextWindowSize( ImVec2( 240.0f, height ) ); + + ImGui::Begin( "Motor Joint", nullptr, ImGuiWindowFlags_NoResize ); + + if ( ImGui::Checkbox( "Go", &m_go ) ) + { + } + + if ( ImGui::SliderFloat( "Max Force", &m_maxForce, 0.0f, 1000.0f, "%.0f" ) ) + { + b2MotorJoint_SetMaxForce( m_jointId, m_maxForce ); + } + + if ( ImGui::SliderFloat( "Max Torque", &m_maxTorque, 0.0f, 1000.0f, "%.0f" ) ) + { + b2MotorJoint_SetMaxTorque( m_jointId, m_maxTorque ); + } + + if ( ImGui::SliderFloat( "Correction", &m_correctionFactor, 0.0f, 1.0f, "%.1f" ) ) + { + b2MotorJoint_SetCorrectionFactor( m_jointId, m_correctionFactor ); + } + + ImGui::End(); + } + + void Step( Settings& settings ) override + { + if ( m_go && settings.hertz > 0.0f ) + { + m_time += 1.0f / settings.hertz; + } + + b2Vec2 linearOffset; + linearOffset.x = 6.0f * sinf( 2.0f * m_time ); + linearOffset.y = 8.0f + 4.0f * sinf( 1.0f * m_time ); + + float angularOffset = b2_pi * sinf( -0.5f * m_time ); + + b2MotorJoint_SetLinearOffset( m_jointId, linearOffset ); + b2MotorJoint_SetAngularOffset( m_jointId, angularOffset ); + + b2Transform transform = { linearOffset, b2MakeRot( angularOffset ) }; + g_draw.DrawTransform( transform ); + + Sample::Step( settings ); + + b2Vec2 force = b2Joint_GetConstraintForce( m_jointId ); + float torque = b2Joint_GetConstraintTorque( m_jointId ); + + g_draw.DrawString( 5, m_textLine, "force = {%3.f, %3.f}, torque = %3.f", force.x, force.y, torque ); + m_textLine += 15; + } + + static Sample* Create( Settings& settings ) + { + return new MotorJoint( settings ); + } + + b2JointId m_jointId; + float m_time; + float m_maxForce; + float m_maxTorque; + float m_correctionFactor; + bool m_go; +}; + +static int sampleMotorJoint = RegisterSample( "Joints", "Motor Joint", MotorJoint::Create ); + +class RevoluteJoint : public Sample +{ +public: + explicit RevoluteJoint( Settings& settings ) + : Sample( settings ) + { + if ( settings.restart == false ) + { + g_camera.m_center = { 0.0f, 15.5f }; + g_camera.m_zoom = 25.0f * 0.7f; + } + + b2BodyId groundId = b2_nullBodyId; + { + b2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.position = { 0.0f, -1.0f }; + groundId = b2CreateBody( m_worldId, &bodyDef ); + + b2Polygon box = b2MakeBox( 40.0f, 1.0f ); + + b2ShapeDef shapeDef = b2DefaultShapeDef(); + b2CreatePolygonShape( groundId, &shapeDef, &box ); + } + + m_enableSpring = false; + m_enableLimit = true; + m_enableMotor = false; + m_hertz = 1.0f; + m_dampingRatio = 0.5f; + m_motorSpeed = 1.0f; + m_motorTorque = 1000.0f; + + { + b2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.type = b2_dynamicBody; + bodyDef.position = { -10.0f, 20.0f }; + b2BodyId bodyId = b2CreateBody( m_worldId, &bodyDef ); + + b2ShapeDef shapeDef = b2DefaultShapeDef(); + shapeDef.density = 1.0f; + + b2Capsule capsule = { { 0.0f, -1.0f }, { 0.0f, 6.0f }, 0.5f }; + b2CreateCapsuleShape( bodyId, &shapeDef, &capsule ); + + b2Vec2 pivot = { -10.0f, 20.5f }; + b2RevoluteJointDef jointDef = b2DefaultRevoluteJointDef(); + jointDef.bodyIdA = groundId; + jointDef.bodyIdB = bodyId; + jointDef.localAnchorA = b2Body_GetLocalPoint( jointDef.bodyIdA, pivot ); + jointDef.localAnchorB = b2Body_GetLocalPoint( jointDef.bodyIdB, pivot ); + jointDef.enableSpring = m_enableSpring; + jointDef.hertz = m_hertz; + jointDef.dampingRatio = m_dampingRatio; + jointDef.motorSpeed = m_motorSpeed; + jointDef.maxMotorTorque = m_motorTorque; + jointDef.enableMotor = m_enableMotor; + jointDef.referenceAngle = 0.5f * b2_pi; + jointDef.lowerAngle = -0.5f * b2_pi; + jointDef.upperAngle = 0.75f * b2_pi; + jointDef.enableLimit = m_enableLimit; + + m_jointId1 = b2CreateRevoluteJoint( m_worldId, &jointDef ); + } + + { + b2Circle circle = { 0 }; + circle.radius = 2.0f; + + b2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.type = b2_dynamicBody; + bodyDef.position = { 5.0f, 30.0f }; + m_ball = b2CreateBody( m_worldId, &bodyDef ); + + b2ShapeDef shapeDef = b2DefaultShapeDef(); + shapeDef.density = 1.0f; + + b2CreateCircleShape( m_ball, &shapeDef, &circle ); + } + + { + b2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.position = { 20.0f, 10.0f }; + bodyDef.type = b2_dynamicBody; + b2BodyId body = b2CreateBody( m_worldId, &bodyDef ); + + b2Polygon box = b2MakeOffsetBox( 10.0f, 0.5f, { -10.0f, 0.0f }, b2Rot_identity ); + b2ShapeDef shapeDef = b2DefaultShapeDef(); + shapeDef.density = 1.0f; + b2CreatePolygonShape( body, &shapeDef, &box ); + + b2Vec2 pivot = { 19.0f, 10.0f }; + b2RevoluteJointDef jointDef = b2DefaultRevoluteJointDef(); + jointDef.bodyIdA = groundId; + jointDef.bodyIdB = body; + jointDef.localAnchorA = b2Body_GetLocalPoint( jointDef.bodyIdA, pivot ); + jointDef.localAnchorB = b2Body_GetLocalPoint( jointDef.bodyIdB, pivot ); + jointDef.lowerAngle = -0.25f * b2_pi; + jointDef.upperAngle = 0.0f * b2_pi; + jointDef.enableLimit = true; + jointDef.enableMotor = true; + jointDef.motorSpeed = 0.0f; + jointDef.maxMotorTorque = m_motorTorque; + + m_jointId2 = b2CreateRevoluteJoint( m_worldId, &jointDef ); + } + } + + void UpdateUI() override + { + float height = 220.0f; + ImGui::SetNextWindowPos( ImVec2( 10.0f, g_camera.m_height - height - 50.0f ), ImGuiCond_Once ); + ImGui::SetNextWindowSize( ImVec2( 240.0f, height ) ); + + ImGui::Begin( "Revolute Joint", nullptr, ImGuiWindowFlags_NoResize ); + + if ( ImGui::Checkbox( "Limit", &m_enableLimit ) ) + { + b2RevoluteJoint_EnableLimit( m_jointId1, m_enableLimit ); + b2Joint_WakeBodies( m_jointId1 ); + } + + if ( ImGui::Checkbox( "Motor", &m_enableMotor ) ) + { + b2RevoluteJoint_EnableMotor( m_jointId1, m_enableMotor ); + b2Joint_WakeBodies( m_jointId1 ); + } + + if ( m_enableMotor ) + { + if ( ImGui::SliderFloat( "Max Torque", &m_motorTorque, 0.0f, 5000.0f, "%.0f" ) ) + { + b2RevoluteJoint_SetMaxMotorTorque( m_jointId1, m_motorTorque ); + b2Joint_WakeBodies( m_jointId1 ); + } + + if ( ImGui::SliderFloat( "Speed", &m_motorSpeed, -20.0f, 20.0f, "%.0f" ) ) + { + b2RevoluteJoint_SetMotorSpeed( m_jointId1, m_motorSpeed ); + b2Joint_WakeBodies( m_jointId1 ); + } + } + + if ( ImGui::Checkbox( "Spring", &m_enableSpring ) ) + { + b2RevoluteJoint_EnableSpring( m_jointId1, m_enableSpring ); + b2Joint_WakeBodies( m_jointId1 ); + } + + if ( m_enableSpring ) + { + if ( ImGui::SliderFloat( "Hertz", &m_hertz, 0.0f, 10.0f, "%.1f" ) ) + { + b2RevoluteJoint_SetSpringHertz( m_jointId1, m_hertz ); + b2Joint_WakeBodies( m_jointId1 ); + } + + if ( ImGui::SliderFloat( "Damping", &m_dampingRatio, 0.0f, 2.0f, "%.1f" ) ) + { + b2RevoluteJoint_SetSpringDampingRatio( m_jointId1, m_dampingRatio ); + b2Joint_WakeBodies( m_jointId1 ); + } + } + + ImGui::End(); + } + + void Step( Settings& settings ) override + { + Sample::Step( settings ); + + float angle1 = b2RevoluteJoint_GetAngle( m_jointId1 ); + g_draw.DrawString( 5, m_textLine, "Angle (Deg) 1 = %2.1f", angle1 ); + m_textLine += m_textIncrement; + + float torque1 = b2RevoluteJoint_GetMotorTorque( m_jointId1 ); + g_draw.DrawString( 5, m_textLine, "Motor Torque 1 = %4.1f", torque1 ); + m_textLine += m_textIncrement; + + float torque2 = b2RevoluteJoint_GetMotorTorque( m_jointId2 ); + g_draw.DrawString( 5, m_textLine, "Motor Torque 2 = %4.1f", torque2 ); + m_textLine += m_textIncrement; + } + + static Sample* Create( Settings& settings ) + { + return new RevoluteJoint( settings ); + } + + b2BodyId m_ball; + b2JointId m_jointId1; + b2JointId m_jointId2; + float m_motorSpeed; + float m_motorTorque; + float m_hertz; + float m_dampingRatio; + bool m_enableSpring; + bool m_enableMotor; + bool m_enableLimit; +}; + +static int sampleRevolute = RegisterSample( "Joints", "Revolute", RevoluteJoint::Create ); + +class PrismaticJoint : public Sample +{ +public: + explicit PrismaticJoint( Settings& settings ) + : Sample( settings ) + { + if ( settings.restart == false ) + { + g_camera.m_center = { 0.0f, 8.0f }; + g_camera.m_zoom = 25.0f * 0.5f; + } + + b2BodyId groundId; + { + b2BodyDef bodyDef = b2DefaultBodyDef(); + groundId = b2CreateBody( m_worldId, &bodyDef ); + } + + m_enableSpring = false; + m_enableLimit = true; + m_enableMotor = false; + m_motorSpeed = 2.0f; + m_motorForce = 25.0f; + m_hertz = 1.0f; + m_dampingRatio = 0.5f; + + { + b2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.position = { 0.0f, 10.0f }; + bodyDef.type = b2_dynamicBody; + b2BodyId bodyId = b2CreateBody( m_worldId, &bodyDef ); + + b2ShapeDef shapeDef = b2DefaultShapeDef(); + b2Polygon box = b2MakeBox( 0.5f, 2.0f ); + b2CreatePolygonShape( bodyId, &shapeDef, &box ); + + b2Vec2 pivot = { 0.0f, 9.0f }; + // b2Vec2 axis = b2Normalize({1.0f, 0.0f}); + b2Vec2 axis = b2Normalize( { 1.0f, 1.0f } ); + b2PrismaticJointDef jointDef = b2DefaultPrismaticJointDef(); + jointDef.bodyIdA = groundId; + jointDef.bodyIdB = bodyId; + jointDef.localAxisA = b2Body_GetLocalVector( jointDef.bodyIdA, axis ); + jointDef.localAnchorA = b2Body_GetLocalPoint( jointDef.bodyIdA, pivot ); + jointDef.localAnchorB = b2Body_GetLocalPoint( jointDef.bodyIdB, pivot ); + jointDef.motorSpeed = m_motorSpeed; + jointDef.maxMotorForce = m_motorForce; + jointDef.enableMotor = m_enableMotor; + jointDef.lowerTranslation = -10.0f; + jointDef.upperTranslation = 10.0f; + jointDef.enableLimit = m_enableLimit; + jointDef.enableSpring = m_enableSpring; + jointDef.hertz = m_hertz; + jointDef.dampingRatio = m_dampingRatio; + + m_jointId = b2CreatePrismaticJoint( m_worldId, &jointDef ); + } + } + + void UpdateUI() override + { + float height = 220.0f; + ImGui::SetNextWindowPos( ImVec2( 10.0f, g_camera.m_height - height - 50.0f ), ImGuiCond_Once ); + ImGui::SetNextWindowSize( ImVec2( 240.0f, height ) ); + + ImGui::Begin( "Prismatic Joint", nullptr, ImGuiWindowFlags_NoResize ); + + if ( ImGui::Checkbox( "Limit", &m_enableLimit ) ) + { + b2PrismaticJoint_EnableLimit( m_jointId, m_enableLimit ); + b2Joint_WakeBodies( m_jointId ); + } + + if ( ImGui::Checkbox( "Motor", &m_enableMotor ) ) + { + b2PrismaticJoint_EnableMotor( m_jointId, m_enableMotor ); + b2Joint_WakeBodies( m_jointId ); + } + + if ( m_enableMotor ) + { + if ( ImGui::SliderFloat( "Max Force", &m_motorForce, 0.0f, 200.0f, "%.0f" ) ) + { + b2PrismaticJoint_SetMaxMotorForce( m_jointId, m_motorForce ); + b2Joint_WakeBodies( m_jointId ); + } + + if ( ImGui::SliderFloat( "Speed", &m_motorSpeed, -40.0f, 40.0f, "%.0f" ) ) + { + b2PrismaticJoint_SetMotorSpeed( m_jointId, m_motorSpeed ); + b2Joint_WakeBodies( m_jointId ); + } + } + + if ( ImGui::Checkbox( "Spring", &m_enableSpring ) ) + { + b2PrismaticJoint_EnableSpring( m_jointId, m_enableSpring ); + b2Joint_WakeBodies( m_jointId ); + } + + if ( m_enableSpring ) + { + if ( ImGui::SliderFloat( "Hertz", &m_hertz, 0.0f, 10.0f, "%.1f" ) ) + { + b2PrismaticJoint_SetSpringHertz( m_jointId, m_hertz ); + b2Joint_WakeBodies( m_jointId ); + } + + if ( ImGui::SliderFloat( "Damping", &m_dampingRatio, 0.0f, 2.0f, "%.1f" ) ) + { + b2PrismaticJoint_SetSpringDampingRatio( m_jointId, m_dampingRatio ); + b2Joint_WakeBodies( m_jointId ); + } + } + + ImGui::End(); + } + + void Step( Settings& settings ) override + { + Sample::Step( settings ); + + float force = b2PrismaticJoint_GetMotorForce( m_jointId ); + g_draw.DrawString( 5, m_textLine, "Motor Force = %4.1f", force ); + m_textLine += m_textIncrement; + } + + static Sample* Create( Settings& settings ) + { + return new PrismaticJoint( settings ); + } + + b2JointId m_jointId; + float m_motorSpeed; + float m_motorForce; + float m_hertz; + float m_dampingRatio; + bool m_enableSpring; + bool m_enableMotor; + bool m_enableLimit; +}; + +static int samplePrismatic = RegisterSample( "Joints", "Prismatic", PrismaticJoint::Create ); + +class WheelJoint : public Sample +{ +public: + explicit WheelJoint( Settings& settings ) + : Sample( settings ) + { + if ( settings.restart == false ) + { + g_camera.m_center = { 0.0f, 10.0f }; + g_camera.m_zoom = 25.0f * 0.15f; + } + + b2BodyId groundId; + + { + b2BodyDef bodyDef = b2DefaultBodyDef(); + groundId = b2CreateBody( m_worldId, &bodyDef ); + } + + m_enableSpring = true; + m_enableLimit = true; + m_enableMotor = true; + m_motorSpeed = 2.0f; + m_motorTorque = 5.0f; + m_hertz = 1.0f; + m_dampingRatio = 0.7f; + + { + b2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.position = { 0.0f, 10.25f }; + bodyDef.type = b2_dynamicBody; + b2BodyId bodyId = b2CreateBody( m_worldId, &bodyDef ); + + b2ShapeDef shapeDef = b2DefaultShapeDef(); + b2Capsule capsule = { { 0.0f, -0.5f }, { 0.0f, 0.5f }, 0.5f }; + b2CreateCapsuleShape( bodyId, &shapeDef, &capsule ); + + b2Vec2 pivot = { 0.0f, 10.0f }; + b2Vec2 axis = b2Normalize( { 1.0f, 1.0f } ); + b2WheelJointDef jointDef = b2DefaultWheelJointDef(); + jointDef.bodyIdA = groundId; + jointDef.bodyIdB = bodyId; + jointDef.localAxisA = b2Body_GetLocalVector( jointDef.bodyIdA, axis ); + jointDef.localAnchorA = b2Body_GetLocalPoint( jointDef.bodyIdA, pivot ); + jointDef.localAnchorB = b2Body_GetLocalPoint( jointDef.bodyIdB, pivot ); + jointDef.motorSpeed = m_motorSpeed; + jointDef.maxMotorTorque = m_motorTorque; + jointDef.enableMotor = m_enableMotor; + jointDef.lowerTranslation = -3.0f; + jointDef.upperTranslation = 3.0f; + jointDef.enableLimit = m_enableLimit; + jointDef.hertz = m_hertz; + jointDef.dampingRatio = m_dampingRatio; + + m_jointId = b2CreateWheelJoint( m_worldId, &jointDef ); + } + } + + void UpdateUI() override + { + float height = 220.0f; + ImGui::SetNextWindowPos( ImVec2( 10.0f, g_camera.m_height - height - 50.0f ), ImGuiCond_Once ); + ImGui::SetNextWindowSize( ImVec2( 240.0f, height ) ); + + ImGui::Begin( "Wheel Joint", nullptr, ImGuiWindowFlags_NoResize ); + + if ( ImGui::Checkbox( "Limit", &m_enableLimit ) ) + { + b2WheelJoint_EnableLimit( m_jointId, m_enableLimit ); + } + + if ( ImGui::Checkbox( "Motor", &m_enableMotor ) ) + { + b2WheelJoint_EnableMotor( m_jointId, m_enableMotor ); + } + + if ( m_enableMotor ) + { + if ( ImGui::SliderFloat( "Torque", &m_motorTorque, 0.0f, 20.0f, "%.0f" ) ) + { + b2WheelJoint_SetMaxMotorTorque( m_jointId, m_motorTorque ); + } + + if ( ImGui::SliderFloat( "Speed", &m_motorSpeed, -20.0f, 20.0f, "%.0f" ) ) + { + b2WheelJoint_SetMotorSpeed( m_jointId, m_motorSpeed ); + } + } + + if ( ImGui::Checkbox( "Spring", &m_enableSpring ) ) + { + b2WheelJoint_EnableSpring( m_jointId, m_enableSpring ); + } + + if ( m_enableSpring ) + { + if ( ImGui::SliderFloat( "Hertz", &m_hertz, 0.0f, 10.0f, "%.1f" ) ) + { + b2WheelJoint_SetSpringHertz( m_jointId, m_hertz ); + } + + if ( ImGui::SliderFloat( "Damping", &m_dampingRatio, 0.0f, 2.0f, "%.1f" ) ) + { + b2WheelJoint_SetSpringDampingRatio( m_jointId, m_dampingRatio ); + } + } + + ImGui::End(); + } + + void Step( Settings& settings ) override + { + Sample::Step( settings ); + + float torque = b2WheelJoint_GetMotorTorque( m_jointId ); + g_draw.DrawString( 5, m_textLine, "Motor Torque = %4.1f", torque ); + m_textLine += m_textIncrement; + } + + static Sample* Create( Settings& settings ) + { + return new WheelJoint( settings ); + } + + b2JointId m_jointId; + float m_hertz; + float m_dampingRatio; + float m_motorSpeed; + float m_motorTorque; + bool m_enableSpring; + bool m_enableMotor; + bool m_enableLimit; +}; + +static int sampleWheel = RegisterSample( "Joints", "Wheel", WheelJoint::Create ); + +// A suspension bridge +class Bridge : public Sample +{ +public: + enum + { + e_count = 160 + }; + + explicit Bridge( Settings& settings ) + : Sample( settings ) + { + if ( settings.restart == false ) + { + g_camera.m_zoom = 25.0f * 2.5f; + } + + b2BodyId groundId = b2_nullBodyId; + { + b2BodyDef bodyDef = b2DefaultBodyDef(); + groundId = b2CreateBody( m_worldId, &bodyDef ); + } + + { + b2Polygon box = b2MakeBox( 0.5f, 0.125f ); + + b2ShapeDef shapeDef = b2DefaultShapeDef(); + shapeDef.density = 20.0f; + + b2RevoluteJointDef jointDef = b2DefaultRevoluteJointDef(); + int jointIndex = 0; + m_frictionTorque = 200.0f; + m_gravityScale = 1.0f; + + float xbase = -80.0f; + + b2BodyId prevBodyId = groundId; + for ( int i = 0; i < e_count; ++i ) + { + b2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.type = b2_dynamicBody; + bodyDef.position = { xbase + 0.5f + 1.0f * i, 20.0f }; + bodyDef.linearDamping = 0.1f; + bodyDef.angularDamping = 0.1f; + m_bodyIds[i] = b2CreateBody( m_worldId, &bodyDef ); + b2CreatePolygonShape( m_bodyIds[i], &shapeDef, &box ); + + b2Vec2 pivot = { xbase + 1.0f * i, 20.0f }; + jointDef.bodyIdA = prevBodyId; + jointDef.bodyIdB = m_bodyIds[i]; + jointDef.localAnchorA = b2Body_GetLocalPoint( jointDef.bodyIdA, pivot ); + jointDef.localAnchorB = b2Body_GetLocalPoint( jointDef.bodyIdB, pivot ); + jointDef.enableMotor = true; + jointDef.maxMotorTorque = m_frictionTorque; + m_jointIds[jointIndex++] = b2CreateRevoluteJoint( m_worldId, &jointDef ); + + prevBodyId = m_bodyIds[i]; + } + + b2Vec2 pivot = { xbase + 1.0f * e_count, 20.0f }; + jointDef.bodyIdA = prevBodyId; + jointDef.bodyIdB = groundId; + jointDef.localAnchorA = b2Body_GetLocalPoint( jointDef.bodyIdA, pivot ); + jointDef.localAnchorB = b2Body_GetLocalPoint( jointDef.bodyIdB, pivot ); + jointDef.enableMotor = true; + jointDef.maxMotorTorque = m_frictionTorque; + m_jointIds[jointIndex++] = b2CreateRevoluteJoint( m_worldId, &jointDef ); + + assert( jointIndex == e_count + 1 ); + } + + for ( int i = 0; i < 2; ++i ) + { + b2Vec2 vertices[3] = { { -0.5f, 0.0f }, { 0.5f, 0.0f }, { 0.0f, 1.5f } }; + + b2Hull hull = b2ComputeHull( vertices, 3 ); + b2Polygon triangle = b2MakePolygon( &hull, 0.0f ); + + b2ShapeDef shapeDef = b2DefaultShapeDef(); + shapeDef.density = 20.0f; + + b2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.type = b2_dynamicBody; + bodyDef.position = { -8.0f + 8.0f * i, 22.0f }; + b2BodyId bodyId = b2CreateBody( m_worldId, &bodyDef ); + b2CreatePolygonShape( bodyId, &shapeDef, &triangle ); + } + + for ( int i = 0; i < 3; ++i ) + { + b2Circle circle = { { 0.0f, 0.0f }, 0.5f }; + + b2ShapeDef shapeDef = b2DefaultShapeDef(); + shapeDef.density = 20.0f; + + b2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.type = b2_dynamicBody; + bodyDef.position = { -6.0f + 6.0f * i, 25.0f }; + b2BodyId bodyId = b2CreateBody( m_worldId, &bodyDef ); + b2CreateCircleShape( bodyId, &shapeDef, &circle ); + } + } + + void UpdateUI() override + { + float height = 80.0f; + ImGui::SetNextWindowPos( ImVec2( 10.0f, g_camera.m_height - height - 50.0f ), ImGuiCond_Once ); + ImGui::SetNextWindowSize( ImVec2( 240.0f, height ) ); + + ImGui::Begin( "Bridge", nullptr, ImGuiWindowFlags_NoResize ); + + // Slider takes half the window + ImGui::PushItemWidth( ImGui::GetWindowWidth() * 0.5f ); + bool updateFriction = ImGui::SliderFloat( "Joint Friction", &m_frictionTorque, 0.0f, 1000.0f, "%2.f" ); + if ( updateFriction ) + { + for ( int i = 0; i <= e_count; ++i ) + { + b2RevoluteJoint_SetMaxMotorTorque( m_jointIds[i], m_frictionTorque ); + } + } + + if ( ImGui::SliderFloat( "Gravity scale", &m_gravityScale, -1.0f, 1.0f, "%.1f" ) ) + { + for ( int i = 0; i < e_count; ++i ) + { + b2Body_SetGravityScale( m_bodyIds[i], m_gravityScale ); + } + } + + ImGui::End(); + } + + static Sample* Create( Settings& settings ) + { + return new Bridge( settings ); + } + + b2BodyId m_bodyIds[e_count]; + b2JointId m_jointIds[e_count + 1]; + float m_frictionTorque; + float m_gravityScale; +}; + +static int sampleBridgeIndex = RegisterSample( "Joints", "Bridge", Bridge::Create ); + +class BallAndChain : public Sample +{ +public: + enum + { + e_count = 30 + }; + + explicit BallAndChain( Settings& settings ) + : Sample( settings ) + { + if ( settings.restart == false ) + { + g_camera.m_center = { 0.0f, -8.0f }; + g_camera.m_zoom = 27.5f; + } + + b2BodyId groundId = b2_nullBodyId; + { + b2BodyDef bodyDef = b2DefaultBodyDef(); + groundId = b2CreateBody( m_worldId, &bodyDef ); + } + + m_frictionTorque = 100.0f; + + { + float hx = 0.5f; + b2Capsule capsule = { { -hx, 0.0f }, { hx, 0.0f }, 0.125f }; + + b2ShapeDef shapeDef = b2DefaultShapeDef(); + shapeDef.density = 20.0f; + + b2RevoluteJointDef jointDef = b2DefaultRevoluteJointDef(); + + int jointIndex = 0; + + b2BodyId prevBodyId = groundId; + for ( int i = 0; i < e_count; ++i ) + { + b2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.type = b2_dynamicBody; + bodyDef.position = { ( 1.0f + 2.0f * i ) * hx, e_count * hx }; + b2BodyId bodyId = b2CreateBody( m_worldId, &bodyDef ); + b2CreateCapsuleShape( bodyId, &shapeDef, &capsule ); + + b2Vec2 pivot = { ( 2.0f * i ) * hx, e_count * hx }; + jointDef.bodyIdA = prevBodyId; + jointDef.bodyIdB = bodyId; + jointDef.localAnchorA = b2Body_GetLocalPoint( jointDef.bodyIdA, pivot ); + jointDef.localAnchorB = b2Body_GetLocalPoint( jointDef.bodyIdB, pivot ); + // jointDef.enableMotor = true; + jointDef.maxMotorTorque = m_frictionTorque; + m_jointIds[jointIndex++] = b2CreateRevoluteJoint( m_worldId, &jointDef ); + + prevBodyId = bodyId; + } + + b2Circle circle = { { 0.0f, 0.0f }, 4.0f }; + + b2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.type = b2_dynamicBody; + bodyDef.position = { ( 1.0f + 2.0f * e_count ) * hx + circle.radius - hx, e_count * hx }; + + b2BodyId bodyId = b2CreateBody( m_worldId, &bodyDef ); + b2CreateCircleShape( bodyId, &shapeDef, &circle ); + + b2Vec2 pivot = { ( 2.0f * e_count ) * hx, e_count * hx }; + jointDef.bodyIdA = prevBodyId; + jointDef.bodyIdB = bodyId; + jointDef.localAnchorA = b2Body_GetLocalPoint( jointDef.bodyIdA, pivot ); + jointDef.localAnchorB = b2Body_GetLocalPoint( jointDef.bodyIdB, pivot ); + jointDef.enableMotor = true; + jointDef.maxMotorTorque = m_frictionTorque; + m_jointIds[jointIndex++] = b2CreateRevoluteJoint( m_worldId, &jointDef ); + assert( jointIndex == e_count + 1 ); + } + } + + void UpdateUI() override + { + float height = 60.0f; + ImGui::SetNextWindowPos( ImVec2( 10.0f, g_camera.m_height - height - 50.0f ), ImGuiCond_Once ); + ImGui::SetNextWindowSize( ImVec2( 240.0f, height ) ); + + ImGui::Begin( "Ball and Chain", nullptr, ImGuiWindowFlags_NoResize ); + + bool updateFriction = ImGui::SliderFloat( "Joint Friction", &m_frictionTorque, 0.0f, 1000.0f, "%2.f" ); + if ( updateFriction ) + { + for ( int i = 0; i <= e_count; ++i ) + { + b2RevoluteJoint_SetMaxMotorTorque( m_jointIds[i], m_frictionTorque ); + } + } + + ImGui::End(); + } + + static Sample* Create( Settings& settings ) + { + return new BallAndChain( settings ); + } + + b2JointId m_jointIds[e_count + 1]; + float m_frictionTorque; +}; + +static int sampleBallAndChainIndex = RegisterSample( "Joints", "Ball & Chain", BallAndChain::Create ); + +// This sample shows the limitations of an iterative solver. The cantilever sags even though the weld +// joint is stiff as possible. +class Cantilever : public Sample +{ +public: + enum + { + e_count = 8 + }; + + explicit Cantilever( Settings& settings ) + : Sample( settings ) + { + if ( settings.restart == false ) + { + g_camera.m_center = { 0.0f, 0.0f }; + g_camera.m_zoom = 25.0f * 0.35f; + } + + b2BodyId groundId = b2_nullBodyId; + { + b2BodyDef bodyDef = b2DefaultBodyDef(); + groundId = b2CreateBody( m_worldId, &bodyDef ); + } + + { + m_linearHertz = 15.0f; + m_linearDampingRatio = 0.5f; + m_angularHertz = 5.0f; + m_angularDampingRatio = 0.5f; + m_gravityScale = 1.0f; + m_collideConnected = false; + + float hx = 0.5f; + b2Capsule capsule = { { -hx, 0.0f }, { hx, 0.0f }, 0.125f }; + b2ShapeDef shapeDef = b2DefaultShapeDef(); + shapeDef.density = 20.0f; + + b2WeldJointDef jointDef = b2DefaultWeldJointDef(); + + b2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.type = b2_dynamicBody; + bodyDef.isAwake = false; + + b2BodyId prevBodyId = groundId; + for ( int i = 0; i < e_count; ++i ) + { + bodyDef.position = { ( 1.0f + 2.0f * i ) * hx, 0.0f }; + m_bodyIds[i] = b2CreateBody( m_worldId, &bodyDef ); + b2CreateCapsuleShape( m_bodyIds[i], &shapeDef, &capsule ); + + b2Vec2 pivot = { ( 2.0f * i ) * hx, 0.0f }; + jointDef.bodyIdA = prevBodyId; + jointDef.bodyIdB = m_bodyIds[i]; + jointDef.localAnchorA = b2Body_GetLocalPoint( jointDef.bodyIdA, pivot ); + jointDef.localAnchorB = b2Body_GetLocalPoint( jointDef.bodyIdB, pivot ); + jointDef.linearHertz = m_linearHertz; + jointDef.linearDampingRatio = m_linearDampingRatio; + jointDef.angularHertz = m_angularHertz; + jointDef.angularDampingRatio = m_angularDampingRatio; + jointDef.collideConnected = m_collideConnected; + m_jointIds[i] = b2CreateWeldJoint( m_worldId, &jointDef ); + + prevBodyId = m_bodyIds[i]; + } + + m_tipId = prevBodyId; + } + } + + void UpdateUI() override + { + float height = 180.0f; + ImGui::SetNextWindowPos( ImVec2( 10.0f, g_camera.m_height - height - 50.0f ), ImGuiCond_Once ); + ImGui::SetNextWindowSize( ImVec2( 240.0f, height ) ); + + ImGui::Begin( "Cantilever", nullptr, ImGuiWindowFlags_NoResize ); + ImGui::PushItemWidth( 100.0f ); + + if ( ImGui::SliderFloat( "Linear Hertz", &m_linearHertz, 0.0f, 20.0f, "%.0f" ) ) + { + for ( int i = 0; i < e_count; ++i ) + { + b2WeldJoint_SetLinearHertz( m_jointIds[i], m_linearHertz ); + } + } + + if ( ImGui::SliderFloat( "Linear Damping Ratio", &m_linearDampingRatio, 0.0f, 10.0f, "%.1f" ) ) + { + for ( int i = 0; i < e_count; ++i ) + { + b2WeldJoint_SetLinearDampingRatio( m_jointIds[i], m_linearDampingRatio ); + } + } + + if ( ImGui::SliderFloat( "Angular Hertz", &m_angularHertz, 0.0f, 20.0f, "%.0f" ) ) + { + for ( int i = 0; i < e_count; ++i ) + { + b2WeldJoint_SetAngularHertz( m_jointIds[i], m_angularHertz ); + } + } + + if ( ImGui::SliderFloat( "Angular Damping Ratio", &m_angularDampingRatio, 0.0f, 10.0f, "%.1f" ) ) + { + for ( int i = 0; i < e_count; ++i ) + { + b2WeldJoint_SetAngularDampingRatio( m_jointIds[i], m_angularDampingRatio ); + } + } + + if ( ImGui::Checkbox( "Collide Connected", &m_collideConnected ) ) + { + for ( int i = 0; i < e_count; ++i ) + { + b2Joint_SetCollideConnected( m_jointIds[i], m_collideConnected ); + } + } + + if ( ImGui::SliderFloat( "Gravity Scale", &m_gravityScale, -1.0f, 1.0f, "%.1f" ) ) + { + for ( int i = 0; i < e_count; ++i ) + { + b2Body_SetGravityScale( m_bodyIds[i], m_gravityScale ); + } + } + + ImGui::PopItemWidth(); + ImGui::End(); + } + + void Step( Settings& settings ) override + { + Sample::Step( settings ); + + b2Vec2 tipPosition = b2Body_GetPosition( m_tipId ); + g_draw.DrawString( 5, m_textLine, "tip-y = %.2f", tipPosition.y ); + m_textLine += m_textIncrement; + } + + static Sample* Create( Settings& settings ) + { + return new Cantilever( settings ); + } + + float m_linearHertz; + float m_linearDampingRatio; + float m_angularHertz; + float m_angularDampingRatio; + float m_gravityScale; + b2BodyId m_tipId; + b2BodyId m_bodyIds[e_count]; + b2JointId m_jointIds[e_count]; + bool m_collideConnected; +}; + +static int sampleCantileverIndex = RegisterSample( "Joints", "Cantilever", Cantilever::Create ); + +// This test ensures joints work correctly with bodies that have fixed rotation +class FixedRotation : public Sample +{ +public: + enum + { + e_count = 6 + }; + + explicit FixedRotation( Settings& settings ) + : Sample( settings ) + { + if ( settings.restart == false ) + { + g_camera.m_center = { 0.0f, 8.0f }; + g_camera.m_zoom = 25.0f * 0.7f; + } + + b2BodyDef bodyDef = b2DefaultBodyDef(); + m_groundId = b2CreateBody( m_worldId, &bodyDef ); + m_fixedRotation = true; + + for ( int i = 0; i < e_count; ++i ) + { + m_bodyIds[i] = b2_nullBodyId; + m_jointIds[i] = b2_nullJointId; + } + + CreateScene(); + } + + void CreateScene() + { + for ( int i = 0; i < e_count; ++i ) + { + if ( B2_IS_NON_NULL( m_jointIds[i] ) ) + { + b2DestroyJoint( m_jointIds[i] ); + m_jointIds[i] = b2_nullJointId; + } + + if ( B2_IS_NON_NULL( m_bodyIds[i] ) ) + { + b2DestroyBody( m_bodyIds[i] ); + m_bodyIds[i] = b2_nullBodyId; + } + } + + b2Vec2 position = { -12.5f, 10.0f }; + b2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.type = b2_dynamicBody; + bodyDef.fixedRotation = m_fixedRotation; + + b2Polygon box = b2MakeBox( 1.0f, 1.0f ); + + int index = 0; + + // distance joint + { + assert( index < e_count ); + + bodyDef.position = position; + m_bodyIds[index] = b2CreateBody( m_worldId, &bodyDef ); + b2ShapeDef shapeDef = b2DefaultShapeDef(); + b2CreatePolygonShape( m_bodyIds[index], &shapeDef, &box ); + + float length = 2.0f; + b2Vec2 pivot1 = { position.x, position.y + 1.0f + length }; + b2Vec2 pivot2 = { position.x, position.y + 1.0f }; + b2DistanceJointDef jointDef = b2DefaultDistanceJointDef(); + jointDef.bodyIdA = m_groundId; + jointDef.bodyIdB = m_bodyIds[index]; + jointDef.localAnchorA = b2Body_GetLocalPoint( jointDef.bodyIdA, pivot1 ); + jointDef.localAnchorB = b2Body_GetLocalPoint( jointDef.bodyIdB, pivot2 ); + jointDef.length = length; + m_jointIds[index] = b2CreateDistanceJoint( m_worldId, &jointDef ); + } + + position.x += 5.0f; + ++index; + + // motor joint + { + assert( index < e_count ); + + bodyDef.position = position; + m_bodyIds[index] = b2CreateBody( m_worldId, &bodyDef ); + b2ShapeDef shapeDef = b2DefaultShapeDef(); + b2CreatePolygonShape( m_bodyIds[index], &shapeDef, &box ); + + b2MotorJointDef jointDef = b2DefaultMotorJointDef(); + jointDef.bodyIdA = m_groundId; + jointDef.bodyIdB = m_bodyIds[index]; + jointDef.linearOffset = position; + jointDef.maxForce = 200.0f; + jointDef.maxTorque = 20.0f; + m_jointIds[index] = b2CreateMotorJoint( m_worldId, &jointDef ); + } + + position.x += 5.0f; + ++index; + + // prismatic joint + { + assert( index < e_count ); + + bodyDef.position = position; + m_bodyIds[index] = b2CreateBody( m_worldId, &bodyDef ); + b2ShapeDef shapeDef = b2DefaultShapeDef(); + b2CreatePolygonShape( m_bodyIds[index], &shapeDef, &box ); + + b2Vec2 pivot = { position.x - 1.0f, position.y }; + b2PrismaticJointDef jointDef = b2DefaultPrismaticJointDef(); + jointDef.bodyIdA = m_groundId; + jointDef.bodyIdB = m_bodyIds[index]; + jointDef.localAnchorA = b2Body_GetLocalPoint( jointDef.bodyIdA, pivot ); + jointDef.localAnchorB = b2Body_GetLocalPoint( jointDef.bodyIdB, pivot ); + jointDef.localAxisA = b2Body_GetLocalVector( jointDef.bodyIdA, { 1.0f, 0.0f } ); + m_jointIds[index] = b2CreatePrismaticJoint( m_worldId, &jointDef ); + } + + position.x += 5.0f; + ++index; + + // revolute joint + { + assert( index < e_count ); + + bodyDef.position = position; + m_bodyIds[index] = b2CreateBody( m_worldId, &bodyDef ); + b2ShapeDef shapeDef = b2DefaultShapeDef(); + b2CreatePolygonShape( m_bodyIds[index], &shapeDef, &box ); + + b2Vec2 pivot = { position.x - 1.0f, position.y }; + b2RevoluteJointDef jointDef = b2DefaultRevoluteJointDef(); + jointDef.bodyIdA = m_groundId; + jointDef.bodyIdB = m_bodyIds[index]; + jointDef.localAnchorA = b2Body_GetLocalPoint( jointDef.bodyIdA, pivot ); + jointDef.localAnchorB = b2Body_GetLocalPoint( jointDef.bodyIdB, pivot ); + m_jointIds[index] = b2CreateRevoluteJoint( m_worldId, &jointDef ); + } + + position.x += 5.0f; + ++index; + + // weld joint + { + assert( index < e_count ); + + bodyDef.position = position; + m_bodyIds[index] = b2CreateBody( m_worldId, &bodyDef ); + b2ShapeDef shapeDef = b2DefaultShapeDef(); + b2CreatePolygonShape( m_bodyIds[index], &shapeDef, &box ); + + b2Vec2 pivot = { position.x - 1.0f, position.y }; + b2WeldJointDef jointDef = b2DefaultWeldJointDef(); + jointDef.bodyIdA = m_groundId; + jointDef.bodyIdB = m_bodyIds[index]; + jointDef.localAnchorA = b2Body_GetLocalPoint( jointDef.bodyIdA, pivot ); + jointDef.localAnchorB = b2Body_GetLocalPoint( jointDef.bodyIdB, pivot ); + jointDef.angularHertz = 1.0f; + jointDef.angularDampingRatio = 0.5f; + jointDef.linearHertz = 1.0f; + jointDef.linearDampingRatio = 0.5f; + m_jointIds[index] = b2CreateWeldJoint( m_worldId, &jointDef ); + } + + position.x += 5.0f; + ++index; + + // wheel joint + { + assert( index < e_count ); + + bodyDef.position = position; + m_bodyIds[index] = b2CreateBody( m_worldId, &bodyDef ); + b2ShapeDef shapeDef = b2DefaultShapeDef(); + b2CreatePolygonShape( m_bodyIds[index], &shapeDef, &box ); + + b2Vec2 pivot = { position.x - 1.0f, position.y }; + b2WheelJointDef jointDef = b2DefaultWheelJointDef(); + jointDef.bodyIdA = m_groundId; + jointDef.bodyIdB = m_bodyIds[index]; + jointDef.localAnchorA = b2Body_GetLocalPoint( jointDef.bodyIdA, pivot ); + jointDef.localAnchorB = b2Body_GetLocalPoint( jointDef.bodyIdB, pivot ); + jointDef.localAxisA = b2Body_GetLocalVector( jointDef.bodyIdA, { 1.0f, 0.0f } ); + jointDef.hertz = 1.0f; + jointDef.dampingRatio = 0.7f; + jointDef.lowerTranslation = -1.0f; + jointDef.upperTranslation = 1.0f; + jointDef.enableLimit = true; + jointDef.enableMotor = true; + jointDef.maxMotorTorque = 10.0f; + jointDef.motorSpeed = 1.0f; + m_jointIds[index] = b2CreateWheelJoint( m_worldId, &jointDef ); + } + + position.x += 5.0f; + ++index; + } + + void UpdateUI() override + { + float height = 60.0f; + ImGui::SetNextWindowPos( ImVec2( 10.0f, g_camera.m_height - height - 50.0f ), ImGuiCond_Once ); + ImGui::SetNextWindowSize( ImVec2( 180.0f, height ) ); + + ImGui::Begin( "Fixed Rotation", nullptr, ImGuiWindowFlags_NoResize ); + + if ( ImGui::Checkbox( "Fixed Rotation", &m_fixedRotation ) ) + { + for ( int i = 0; i < e_count; ++i ) + { + b2Body_SetFixedRotation( m_bodyIds[i], m_fixedRotation ); + } + } + + ImGui::End(); + } + + static Sample* Create( Settings& settings ) + { + return new FixedRotation( settings ); + } + + b2BodyId m_groundId; + b2BodyId m_bodyIds[e_count]; + b2JointId m_jointIds[e_count]; + bool m_fixedRotation; +}; + +static int sampleFixedRotation = RegisterSample( "Joints", "Fixed Rotation", FixedRotation::Create ); + +// This sample shows how to break joints when the internal reaction force becomes large. +class BreakableJoint : public Sample +{ +public: + enum + { + e_count = 6 + }; + + explicit BreakableJoint( Settings& settings ) + : Sample( settings ) + { + if ( settings.restart == false ) + { + g_camera.m_center = { 0.0f, 8.0f }; + g_camera.m_zoom = 25.0f * 0.7f; + } + + b2BodyDef bodyDef = b2DefaultBodyDef(); + b2BodyId groundId = b2CreateBody( m_worldId, &bodyDef ); + + b2ShapeDef shapeDef = b2DefaultShapeDef(); + b2Segment segment = { { -40.0f, 0.0f }, { 40.0f, 0.0f } }; + b2CreateSegmentShape( groundId, &shapeDef, &segment ); + + for ( int i = 0; i < e_count; ++i ) + { + m_jointIds[i] = b2_nullJointId; + } + + b2Vec2 position = { -12.5f, 10.0f }; + bodyDef.type = b2_dynamicBody; + bodyDef.enableSleep = false; + + b2Polygon box = b2MakeBox( 1.0f, 1.0f ); + + int index = 0; + + // distance joint + { + assert( index < e_count ); + + bodyDef.position = position; + b2BodyId bodyId = b2CreateBody( m_worldId, &bodyDef ); + b2CreatePolygonShape( bodyId, &shapeDef, &box ); + + float length = 2.0f; + b2Vec2 pivot1 = { position.x, position.y + 1.0f + length }; + b2Vec2 pivot2 = { position.x, position.y + 1.0f }; + b2DistanceJointDef jointDef = b2DefaultDistanceJointDef(); + jointDef.bodyIdA = groundId; + jointDef.bodyIdB = bodyId; + jointDef.localAnchorA = b2Body_GetLocalPoint( jointDef.bodyIdA, pivot1 ); + jointDef.localAnchorB = b2Body_GetLocalPoint( jointDef.bodyIdB, pivot2 ); + jointDef.length = length; + jointDef.collideConnected = true; + m_jointIds[index] = b2CreateDistanceJoint( m_worldId, &jointDef ); + } + + position.x += 5.0f; + ++index; + + // motor joint + { + assert( index < e_count ); + + bodyDef.position = position; + b2BodyId bodyId = b2CreateBody( m_worldId, &bodyDef ); + b2CreatePolygonShape( bodyId, &shapeDef, &box ); + + b2MotorJointDef jointDef = b2DefaultMotorJointDef(); + jointDef.bodyIdA = groundId; + jointDef.bodyIdB = bodyId; + jointDef.linearOffset = position; + jointDef.maxForce = 1000.0f; + jointDef.maxTorque = 20.0f; + jointDef.collideConnected = true; + m_jointIds[index] = b2CreateMotorJoint( m_worldId, &jointDef ); + } + + position.x += 5.0f; + ++index; + + // prismatic joint + { + assert( index < e_count ); + + bodyDef.position = position; + b2BodyId bodyId = b2CreateBody( m_worldId, &bodyDef ); + b2CreatePolygonShape( bodyId, &shapeDef, &box ); + + b2Vec2 pivot = { position.x - 1.0f, position.y }; + b2PrismaticJointDef jointDef = b2DefaultPrismaticJointDef(); + jointDef.bodyIdA = groundId; + jointDef.bodyIdB = bodyId; + jointDef.localAnchorA = b2Body_GetLocalPoint( jointDef.bodyIdA, pivot ); + jointDef.localAnchorB = b2Body_GetLocalPoint( jointDef.bodyIdB, pivot ); + jointDef.localAxisA = b2Body_GetLocalVector( jointDef.bodyIdA, { 1.0f, 0.0f } ); + jointDef.collideConnected = true; + m_jointIds[index] = b2CreatePrismaticJoint( m_worldId, &jointDef ); + } + + position.x += 5.0f; + ++index; + + // revolute joint + { + assert( index < e_count ); + + bodyDef.position = position; + b2BodyId bodyId = b2CreateBody( m_worldId, &bodyDef ); + b2CreatePolygonShape( bodyId, &shapeDef, &box ); + + b2Vec2 pivot = { position.x - 1.0f, position.y }; + b2RevoluteJointDef jointDef = b2DefaultRevoluteJointDef(); + jointDef.bodyIdA = groundId; + jointDef.bodyIdB = bodyId; + jointDef.localAnchorA = b2Body_GetLocalPoint( jointDef.bodyIdA, pivot ); + jointDef.localAnchorB = b2Body_GetLocalPoint( jointDef.bodyIdB, pivot ); + jointDef.collideConnected = true; + m_jointIds[index] = b2CreateRevoluteJoint( m_worldId, &jointDef ); + } + + position.x += 5.0f; + ++index; + + // weld joint + { + assert( index < e_count ); + + bodyDef.position = position; + b2BodyId bodyId = b2CreateBody( m_worldId, &bodyDef ); + b2CreatePolygonShape( bodyId, &shapeDef, &box ); + + b2Vec2 pivot = { position.x - 1.0f, position.y }; + b2WeldJointDef jointDef = b2DefaultWeldJointDef(); + jointDef.bodyIdA = groundId; + jointDef.bodyIdB = bodyId; + jointDef.localAnchorA = b2Body_GetLocalPoint( jointDef.bodyIdA, pivot ); + jointDef.localAnchorB = b2Body_GetLocalPoint( jointDef.bodyIdB, pivot ); + jointDef.angularHertz = 2.0f; + jointDef.angularDampingRatio = 0.5f; + jointDef.linearHertz = 2.0f; + jointDef.linearDampingRatio = 0.5f; + jointDef.collideConnected = true; + m_jointIds[index] = b2CreateWeldJoint( m_worldId, &jointDef ); + } + + position.x += 5.0f; + ++index; + + // wheel joint + { + assert( index < e_count ); + + bodyDef.position = position; + b2BodyId bodyId = b2CreateBody( m_worldId, &bodyDef ); + b2CreatePolygonShape( bodyId, &shapeDef, &box ); + + b2Vec2 pivot = { position.x - 1.0f, position.y }; + b2WheelJointDef jointDef = b2DefaultWheelJointDef(); + jointDef.bodyIdA = groundId; + jointDef.bodyIdB = bodyId; + jointDef.localAnchorA = b2Body_GetLocalPoint( jointDef.bodyIdA, pivot ); + jointDef.localAnchorB = b2Body_GetLocalPoint( jointDef.bodyIdB, pivot ); + jointDef.localAxisA = b2Body_GetLocalVector( jointDef.bodyIdA, { 1.0f, 0.0f } ); + jointDef.hertz = 1.0f; + jointDef.dampingRatio = 0.7f; + jointDef.lowerTranslation = -1.0f; + jointDef.upperTranslation = 1.0f; + jointDef.enableLimit = true; + jointDef.enableMotor = true; + jointDef.maxMotorTorque = 10.0f; + jointDef.motorSpeed = 1.0f; + jointDef.collideConnected = true; + m_jointIds[index] = b2CreateWheelJoint( m_worldId, &jointDef ); + } + + position.x += 5.0f; + ++index; + + m_breakForce = 1000.0f; + } + + void UpdateUI() override + { + float height = 100.0f; + ImGui::SetNextWindowPos( ImVec2( 10.0f, g_camera.m_height - height - 50.0f ), ImGuiCond_Once ); + ImGui::SetNextWindowSize( ImVec2( 240.0f, height ) ); + + ImGui::Begin( "Breakable Joint", nullptr, ImGuiWindowFlags_NoResize ); + + ImGui::SliderFloat( "break force", &m_breakForce, 0.0f, 10000.0f, "%.1f" ); + + b2Vec2 gravity = b2World_GetGravity( m_worldId ); + if ( ImGui::SliderFloat( "gravity", &gravity.y, -50.0f, 50.0f, "%.1f" ) ) + { + b2World_SetGravity( m_worldId, gravity ); + } + + ImGui::End(); + } + + void Step( Settings& settings ) override + { + for ( int i = 0; i < e_count; ++i ) + { + if ( B2_IS_NULL( m_jointIds[i] ) ) + { + continue; + } + + b2Vec2 force = b2Joint_GetConstraintForce( m_jointIds[i] ); + if ( b2LengthSquared( force ) > m_breakForce * m_breakForce ) + { + b2DestroyJoint( m_jointIds[i] ); + m_jointIds[i] = b2_nullJointId; + } + else + { + b2Vec2 point = b2Joint_GetLocalAnchorA( m_jointIds[i] ); + g_draw.DrawString( point, "(%.1f, %.1f)", force.x, force.y ); + } + } + + Sample::Step( settings ); + } + + static Sample* Create( Settings& settings ) + { + return new BreakableJoint( settings ); + } + + b2JointId m_jointIds[e_count]; + float m_breakForce; +}; + +static int sampleBreakableJoint = RegisterSample( "Joints", "Breakable", BreakableJoint::Create ); + +// This shows how you can implement a constraint outside of Box2D +class UserConstraint : public Sample +{ +public: + explicit UserConstraint( Settings& settings ) + : Sample( settings ) + { + if ( settings.restart == false ) + { + g_camera.m_center = { 3.0f, -1.0f }; + g_camera.m_zoom = 25.0f * 0.15f; + } + + b2Polygon box = b2MakeBox( 1.0f, 0.5f ); + + b2ShapeDef shapeDef = b2DefaultShapeDef(); + shapeDef.density = 20.0f; + + b2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.type = b2_dynamicBody; + bodyDef.gravityScale = 1.0f; + bodyDef.angularDamping = 0.5f; + bodyDef.linearDamping = 0.2f; + m_bodyId = b2CreateBody( m_worldId, &bodyDef ); + b2CreatePolygonShape( m_bodyId, &shapeDef, &box ); + + m_impulses[0] = 0.0f; + m_impulses[1] = 0.0f; + } + + void Step( Settings& settings ) override + { + Sample::Step( settings ); + + b2Transform axes = b2Transform_identity; + g_draw.DrawTransform( axes ); + + if ( settings.pause ) + { + return; + } + + float timeStep = settings.hertz > 0.0f ? 1.0f / settings.hertz : 0.0f; + if ( timeStep == 0.0f ) + { + return; + } + + float invTimeStep = settings.hertz; + + static float hertz = 3.0f; + static float zeta = 0.7f; + static float maxForce = 1000.0f; + float omega = 2.0f * b2_pi * hertz; + float sigma = 2.0f * zeta + timeStep * omega; + float s = timeStep * omega * sigma; + float impulseCoefficient = 1.0f / ( 1.0f + s ); + float massCoefficient = s * impulseCoefficient; + float biasCoefficient = omega / sigma; + + b2Vec2 localAnchors[2] = { { 1.0f, -0.5f }, { 1.0f, 0.5f } }; + float mass = b2Body_GetMass( m_bodyId ); + float invMass = mass < 0.0001f ? 0.0f : 1.0f / mass; + float inertiaTensor = b2Body_GetRotationalInertia( m_bodyId ); + float invI = inertiaTensor < 0.0001f ? 0.0f : 1.0f / inertiaTensor; + + b2Vec2 vB = b2Body_GetLinearVelocity( m_bodyId ); + float omegaB = b2Body_GetAngularVelocity( m_bodyId ); + b2Vec2 pB = b2Body_GetWorldCenterOfMass( m_bodyId ); + + for ( int i = 0; i < 2; ++i ) + { + b2Vec2 anchorA = { 3.0f, 0.0f }; + b2Vec2 anchorB = b2Body_GetWorldPoint( m_bodyId, localAnchors[i] ); + + b2Vec2 deltaAnchor = b2Sub( anchorB, anchorA ); + + float slackLength = 1.0f; + float length = b2Length( deltaAnchor ); + float C = length - slackLength; + if ( C < 0.0f || length < 0.001f ) + { + g_draw.DrawSegment( anchorA, anchorB, b2_colorLightCyan ); + m_impulses[i] = 0.0f; + continue; + } + + g_draw.DrawSegment( anchorA, anchorB, b2_colorViolet ); + b2Vec2 axis = b2Normalize( deltaAnchor ); + + b2Vec2 rB = b2Sub( anchorB, pB ); + float Jb = b2Cross( rB, axis ); + float K = invMass + Jb * invI * Jb; + float invK = K < 0.0001f ? 0.0f : 1.0f / K; + + float Cdot = b2Dot( vB, axis ) + Jb * omegaB; + float impulse = -massCoefficient * invK * ( Cdot + biasCoefficient * C ); + float appliedImpulse = b2ClampFloat( impulse, -maxForce * timeStep, 0.0f ); + + vB = b2MulAdd( vB, invMass * appliedImpulse, axis ); + omegaB += appliedImpulse * invI * Jb; + + m_impulses[i] = appliedImpulse; + } + + b2Body_SetLinearVelocity( m_bodyId, vB ); + b2Body_SetAngularVelocity( m_bodyId, omegaB ); + + g_draw.DrawString( 5, m_textLine, "forces = %g, %g", m_impulses[0] * invTimeStep, m_impulses[1] * invTimeStep ); + m_textLine += m_textIncrement; + } + + static Sample* Create( Settings& settings ) + { + return new UserConstraint( settings ); + } + + b2BodyId m_bodyId; + float m_impulses[2]; +}; + +static int sampleUserConstraintIndex = RegisterSample( "Joints", "User Constraint", UserConstraint::Create ); + +// This is a fun demo that shows off the wheel joint +class Driving : public Sample +{ +public: + explicit Driving( Settings& settings ) + : Sample( settings ) + { + if ( settings.restart == false ) + { + g_camera.m_center.y = 5.0f; + g_camera.m_zoom = 25.0f * 0.4f; + settings.drawJoints = false; + } + + b2BodyId groundId; + { + b2BodyDef bodyDef = b2DefaultBodyDef(); + groundId = b2CreateBody( m_worldId, &bodyDef ); + + b2Vec2 points[25]; + int count = 24; + + // fill in reverse to match line list convention + points[count--] = { -20.0f, -20.0f }; + points[count--] = { -20.0f, 0.0f }; + points[count--] = { 20.0f, 0.0f }; + + float hs[10] = { 0.25f, 1.0f, 4.0f, 0.0f, 0.0f, -1.0f, -2.0f, -2.0f, -1.25f, 0.0f }; + float x = 20.0f, y1 = 0.0f, dx = 5.0f; + + for ( int j = 0; j < 2; ++j ) + { + for ( int i = 0; i < 10; ++i ) + { + float y2 = hs[i]; + points[count--] = { x + dx, y2 }; + y1 = y2; + x += dx; + } + } + + // flat before bridge + points[count--] = { x + 40.0f, 0.0f }; + points[count--] = { x + 40.0f, -20.0f }; + + assert( count == -1 ); + + b2ChainDef chainDef = b2DefaultChainDef(); + chainDef.points = points; + chainDef.count = 25; + chainDef.isLoop = true; + b2CreateChain( groundId, &chainDef ); + + // flat after bridge + x += 80.0f; + b2ShapeDef shapeDef = b2DefaultShapeDef(); + b2Segment segment = { { x, 0.0f }, { x + 40.0f, 0.0f } }; + b2CreateSegmentShape( groundId, &shapeDef, &segment ); + + // jump ramp + x += 40.0f; + segment = { { x, 0.0f }, { x + 10.0f, 5.0f } }; + b2CreateSegmentShape( groundId, &shapeDef, &segment ); + + // final corner + x += 20.0f; + segment = { { x, 0.0f }, { x + 40.0f, 0.0f } }; + b2CreateSegmentShape( groundId, &shapeDef, &segment ); + + x += 40.0f; + segment = { { x, 0.0f }, { x, 20.0f } }; + b2CreateSegmentShape( groundId, &shapeDef, &segment ); + } + + // Teeter + { + b2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.position = { 140.0f, 1.0f }; + bodyDef.angularVelocity = 1.0f; + bodyDef.type = b2_dynamicBody; + b2BodyId bodyId = b2CreateBody( m_worldId, &bodyDef ); + + b2ShapeDef shapeDef = b2DefaultShapeDef(); + b2Polygon box = b2MakeBox( 10.0f, 0.25f ); + b2CreatePolygonShape( bodyId, &shapeDef, &box ); + + b2Vec2 pivot = bodyDef.position; + b2RevoluteJointDef jointDef = b2DefaultRevoluteJointDef(); + jointDef.bodyIdA = groundId; + jointDef.bodyIdB = bodyId; + jointDef.localAnchorA = b2Body_GetLocalPoint( jointDef.bodyIdA, pivot ); + jointDef.localAnchorB = b2Body_GetLocalPoint( jointDef.bodyIdB, pivot ); + jointDef.lowerAngle = -8.0f * b2_pi / 180.0f; + jointDef.upperAngle = 8.0f * b2_pi / 180.0f; + jointDef.enableLimit = true; + b2CreateRevoluteJoint( m_worldId, &jointDef ); + } + + // Bridge + { + int N = 20; + b2ShapeDef shapeDef = b2DefaultShapeDef(); + b2Capsule capsule = { { -1.0f, 0.0f }, { 1.0f, 0.0f }, 0.125f }; + + b2RevoluteJointDef jointDef = b2DefaultRevoluteJointDef(); + + b2BodyId prevBodyId = groundId; + for ( int i = 0; i < N; ++i ) + { + b2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.type = b2_dynamicBody; + bodyDef.position = { 161.0f + 2.0f * i, -0.125f }; + b2BodyId bodyId = b2CreateBody( m_worldId, &bodyDef ); + b2CreateCapsuleShape( bodyId, &shapeDef, &capsule ); + + b2Vec2 pivot = { 160.0f + 2.0f * i, -0.125f }; + jointDef.bodyIdA = prevBodyId; + jointDef.bodyIdB = bodyId; + jointDef.localAnchorA = b2Body_GetLocalPoint( jointDef.bodyIdA, pivot ); + jointDef.localAnchorB = b2Body_GetLocalPoint( jointDef.bodyIdB, pivot ); + b2CreateRevoluteJoint( m_worldId, &jointDef ); + + prevBodyId = bodyId; + } + + b2Vec2 pivot = { 160.0f + 2.0f * N, -0.125f }; + jointDef.bodyIdA = prevBodyId; + jointDef.bodyIdB = groundId; + jointDef.localAnchorA = b2Body_GetLocalPoint( jointDef.bodyIdA, pivot ); + jointDef.localAnchorB = b2Body_GetLocalPoint( jointDef.bodyIdB, pivot ); + jointDef.enableMotor = true; + jointDef.maxMotorTorque = 50.0f; + b2CreateRevoluteJoint( m_worldId, &jointDef ); + } + + // Boxes + { + b2Polygon box = b2MakeBox( 0.5f, 0.5f ); + + b2BodyId bodyId; + b2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.type = b2_dynamicBody; + + b2ShapeDef shapeDef = b2DefaultShapeDef(); + shapeDef.friction = 0.25f; + shapeDef.restitution = 0.25f; + shapeDef.density = 0.25f; + + bodyDef.position = { 230.0f, 0.5f }; + bodyId = b2CreateBody( m_worldId, &bodyDef ); + b2CreatePolygonShape( bodyId, &shapeDef, &box ); + + bodyDef.position = { 230.0f, 1.5f }; + bodyId = b2CreateBody( m_worldId, &bodyDef ); + b2CreatePolygonShape( bodyId, &shapeDef, &box ); + + bodyDef.position = { 230.0f, 2.5f }; + bodyId = b2CreateBody( m_worldId, &bodyDef ); + b2CreatePolygonShape( bodyId, &shapeDef, &box ); + + bodyDef.position = { 230.0f, 3.5f }; + bodyId = b2CreateBody( m_worldId, &bodyDef ); + b2CreatePolygonShape( bodyId, &shapeDef, &box ); + + bodyDef.position = { 230.0f, 4.5f }; + bodyId = b2CreateBody( m_worldId, &bodyDef ); + b2CreatePolygonShape( bodyId, &shapeDef, &box ); + } + + // Car + + m_throttle = 0.0f; + m_speed = 35.0f; + m_torque = 2.5f; + m_hertz = 5.0f; + m_dampingRatio = 0.7f; + + m_car.Spawn( m_worldId, { 0.0f, 0.0f }, 1.0f, m_hertz, m_dampingRatio, m_torque, NULL ); + } + + void UpdateUI() override + { + float height = 140.0f; + ImGui::SetNextWindowPos( ImVec2( 10.0f, g_camera.m_height - height - 50.0f ), ImGuiCond_Once ); + ImGui::SetNextWindowSize( ImVec2( 200.0f, height ) ); + + ImGui::Begin( "Driving", nullptr, ImGuiWindowFlags_NoResize ); + + ImGui::PushItemWidth( 100.0f ); + if ( ImGui::SliderFloat( "Spring Hertz", &m_hertz, 0.0f, 20.0f, "%.0f" ) ) + { + m_car.SetHertz( m_hertz ); + } + + if ( ImGui::SliderFloat( "Damping Ratio", &m_dampingRatio, 0.0f, 10.0f, "%.1f" ) ) + { + m_car.SetDampingRadio( m_dampingRatio ); + } + + if ( ImGui::SliderFloat( "Speed", &m_speed, 0.0f, 50.0f, "%.0f" ) ) + { + m_car.SetSpeed( m_throttle * m_speed ); + } + + if ( ImGui::SliderFloat( "Torque", &m_torque, 0.0f, 5.0f, "%.1f" ) ) + { + m_car.SetTorque( m_torque ); + } + ImGui::PopItemWidth(); + + ImGui::End(); + } + + void Step( Settings& settings ) override + { + if ( glfwGetKey( g_mainWindow, GLFW_KEY_A ) == GLFW_PRESS ) + { + m_throttle = 1.0f; + m_car.SetSpeed( m_speed ); + } + + if ( glfwGetKey( g_mainWindow, GLFW_KEY_S ) == GLFW_PRESS ) + { + m_throttle = 0.0f; + m_car.SetSpeed( 0.0f ); + } + + if ( glfwGetKey( g_mainWindow, GLFW_KEY_D ) == GLFW_PRESS ) + { + m_throttle = -1.0f; + m_car.SetSpeed( -m_speed ); + } + + g_draw.DrawString( 5, m_textLine, "Keys: left = a, brake = s, right = d" ); + m_textLine += m_textIncrement; + + b2Vec2 linearVelocity = b2Body_GetLinearVelocity( m_car.m_chassisId ); + float kph = linearVelocity.x * 3.6f; + g_draw.DrawString( 5, m_textLine, "speed in kph: %.2g", kph ); + m_textLine += m_textIncrement; + + b2Vec2 carPosition = b2Body_GetPosition( m_car.m_chassisId ); + g_camera.m_center.x = carPosition.x; + + Sample::Step( settings ); + } + + static Sample* Create( Settings& settings ) + { + return new Driving( settings ); + } + + Car m_car; + + float m_throttle; + float m_hertz; + float m_dampingRatio; + float m_torque; + float m_speed; +}; + +static int sampleDriving = RegisterSample( "Joints", "Driving", Driving::Create ); + +class Ragdoll : public Sample +{ +public: + explicit Ragdoll( Settings& settings ) + : Sample( settings ) + { + if ( settings.restart == false ) + { + g_camera.m_center = { 0.0f, 3.0f }; + g_camera.m_zoom = 25.0f * 0.15f; + } + + { + b2BodyDef bodyDef = b2DefaultBodyDef(); + b2BodyId groundId = b2CreateBody( m_worldId, &bodyDef ); + b2ShapeDef shapeDef = b2DefaultShapeDef(); + b2Segment segment = { { -20.0f, 0.0f }, { 20.0f, 0.0f } }; + b2CreateSegmentShape( groundId, &shapeDef, &segment ); + } + + m_jointFrictionTorque = 0.05f; + m_jointHertz = 0.0f; + m_jointDampingRatio = 0.5f; + + m_human.Spawn( m_worldId, { 0.0f, 5.0f }, 1.0f, m_jointFrictionTorque, m_jointHertz, m_jointDampingRatio, 1, nullptr, + true ); + m_human.ApplyRandomAngularImpulse( 10.0f ); + } + + void UpdateUI() override + { + float height = 140.0f; + ImGui::SetNextWindowPos( ImVec2( 10.0f, g_camera.m_height - height - 50.0f ), ImGuiCond_Once ); + ImGui::SetNextWindowSize( ImVec2( 180.0f, height ) ); + + ImGui::Begin( "Ragdoll", nullptr, ImGuiWindowFlags_NoResize ); + ImGui::PushItemWidth( 100.0f ); + + if ( ImGui::SliderFloat( "Friction", &m_jointFrictionTorque, 0.0f, 1.0f, "%3.2f" ) ) + { + m_human.SetJointFrictionTorque( m_jointFrictionTorque ); + } + + if ( ImGui::SliderFloat( "Hertz", &m_jointHertz, 0.0f, 10.0f, "%3.1f" ) ) + { + m_human.SetJointSpringHertz( m_jointHertz ); + } + + if ( ImGui::SliderFloat( "Damping", &m_jointDampingRatio, 0.0f, 4.0f, "%3.1f" ) ) + { + m_human.SetJointDampingRatio( m_jointDampingRatio ); + } + + ImGui::PopItemWidth(); + ImGui::End(); + } + + static Sample* Create( Settings& settings ) + { + return new Ragdoll( settings ); + } + + Human m_human; + float m_jointFrictionTorque; + float m_jointHertz; + float m_jointDampingRatio; +}; + +static int sampleRagdoll = RegisterSample( "Joints", "Ragdoll", Ragdoll::Create ); + +class SoftBody : public Sample +{ +public: + explicit SoftBody( Settings& settings ) + : Sample( settings ) + { + if ( settings.restart == false ) + { + g_camera.m_center = { 0.0f, 5.0f }; + g_camera.m_zoom = 25.0f * 0.25f; + } + + { + b2BodyDef bodyDef = b2DefaultBodyDef(); + b2BodyId groundId = b2CreateBody( m_worldId, &bodyDef ); + b2ShapeDef shapeDef = b2DefaultShapeDef(); + b2Segment segment = { { -20.0f, 0.0f }, { 20.0f, 0.0f } }; + b2CreateSegmentShape( groundId, &shapeDef, &segment ); + } + + m_donut.Spawn( m_worldId, { 0.0f, 10.0f }, 2.0f, 0, nullptr ); + } + + static Sample* Create( Settings& settings ) + { + return new SoftBody( settings ); + } + + Donut m_donut; +}; + +static int sampleDonut = RegisterSample( "Joints", "Soft Body", SoftBody::Create ); + +class DoohickeyFarm : public Sample +{ +public: + explicit DoohickeyFarm( Settings& settings ) + : Sample( settings ) + { + if ( settings.restart == false ) + { + g_camera.m_center = { 0.0f, 5.0f }; + g_camera.m_zoom = 25.0f * 0.35f; + } + + { + b2BodyDef bodyDef = b2DefaultBodyDef(); + b2BodyId groundId = b2CreateBody( m_worldId, &bodyDef ); + + b2ShapeDef shapeDef = b2DefaultShapeDef(); + b2Segment segment = { { -20.0f, 0.0f }, { 20.0f, 0.0f } }; + b2CreateSegmentShape( groundId, &shapeDef, &segment ); + + b2Polygon box = b2MakeOffsetBox( 1.0f, 1.0f, { 0.0f, 1.0f }, b2Rot_identity ); + b2CreatePolygonShape( groundId, &shapeDef, &box ); + } + + float y = 4.0f; + for ( int i = 0; i < 4; ++i ) + { + Doohickey doohickey; + doohickey.Spawn( m_worldId, { 0.0f, y }, 0.5f ); + y += 2.0f; + } + } + + void Step( Settings& settings ) override + { + Sample::Step( settings ); + } + + static Sample* Create( Settings& settings ) + { + return new DoohickeyFarm( settings ); + } +}; + +static int sampleDoohickey = RegisterSample( "Joints", "Doohickey", DoohickeyFarm::Create ); + +class ScissorLift : public Sample +{ +public: + explicit ScissorLift( Settings& settings ) + : Sample( settings ) + { + if ( settings.restart == false ) + { + g_camera.m_center = { 0.0f, 9.0f }; + g_camera.m_zoom = 25.0f * 0.4f; + } + + // Need 8 sub-steps for smoother operation + settings.subStepCount = 8; + + b2BodyId groundId; + { + b2BodyDef bodyDef = b2DefaultBodyDef(); + groundId = b2CreateBody( m_worldId, &bodyDef ); + + b2ShapeDef shapeDef = b2DefaultShapeDef(); + b2Segment segment = { { -20.0f, 0.0f }, { 20.0f, 0.0f } }; + b2CreateSegmentShape( groundId, &shapeDef, &segment ); + } + + b2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.type = b2_dynamicBody; + bodyDef.sleepThreshold = 0.01f; + + b2ShapeDef shapeDef = b2DefaultShapeDef(); + b2Capsule capsule = { { -2.5f, 0.0f }, { 2.5f, 0.0f }, 0.15f }; + + b2BodyId baseId1 = groundId; + b2BodyId baseId2 = groundId; + b2Vec2 baseAnchor1 = { -2.5f, 0.2f }; + b2Vec2 baseAnchor2 = { 2.5f, 0.2f }; + float y = 0.5f; + + b2BodyId linkId1; + int N = 3; + + for ( int i = 0; i < N; ++i ) + { + bodyDef.position = { 0.0f, y }; + bodyDef.rotation = b2MakeRot( 0.15f ); + b2BodyId bodyId1 = b2CreateBody( m_worldId, &bodyDef ); + b2CreateCapsuleShape( bodyId1, &shapeDef, &capsule ); + + bodyDef.position = { 0.0f, y }; + bodyDef.rotation = b2MakeRot( -0.15f ); + + b2BodyId bodyId2 = b2CreateBody( m_worldId, &bodyDef ); + b2CreateCapsuleShape( bodyId2, &shapeDef, &capsule ); + + if ( i == 1 ) + { + linkId1 = bodyId2; + } + + b2RevoluteJointDef revoluteDef = b2DefaultRevoluteJointDef(); + + // left pin + revoluteDef.bodyIdA = baseId1; + revoluteDef.bodyIdB = bodyId1; + revoluteDef.localAnchorA = baseAnchor1; + revoluteDef.localAnchorB = { -2.5f, 0.0f }; + revoluteDef.enableMotor = false; + revoluteDef.maxMotorTorque = 1.0f; + revoluteDef.collideConnected = ( i == 0 ) ? true : false; + + b2CreateRevoluteJoint( m_worldId, &revoluteDef ); + + // right pin + if ( i == 0 ) + { + b2WheelJointDef wheelDef = b2DefaultWheelJointDef(); + wheelDef.bodyIdA = baseId2; + wheelDef.bodyIdB = bodyId2; + wheelDef.localAxisA = { 1.0f, 0.0f }; + wheelDef.localAnchorA = baseAnchor2; + wheelDef.localAnchorB = { 2.5f, 0.0f }; + wheelDef.enableSpring = false; + wheelDef.collideConnected = true; + + b2CreateWheelJoint( m_worldId, &wheelDef ); + } + else + { + revoluteDef.bodyIdA = baseId2; + revoluteDef.bodyIdB = bodyId2; + revoluteDef.localAnchorA = baseAnchor2; + revoluteDef.localAnchorB = { 2.5f, 0.0f }; + revoluteDef.enableMotor = false; + revoluteDef.maxMotorTorque = 1.0f; + revoluteDef.collideConnected = false; + + b2CreateRevoluteJoint( m_worldId, &revoluteDef ); + } + + // middle pin + revoluteDef.bodyIdA = bodyId1; + revoluteDef.bodyIdB = bodyId2; + revoluteDef.localAnchorA = { 0.0f, 0.0f }; + revoluteDef.localAnchorB = { 0.0f, 0.0f }; + revoluteDef.enableMotor = false; + revoluteDef.maxMotorTorque = 1.0f; + revoluteDef.collideConnected = false; + + b2CreateRevoluteJoint( m_worldId, &revoluteDef ); + + baseId1 = bodyId2; + baseId2 = bodyId1; + baseAnchor1 = { -2.5f, 0.0f }; + baseAnchor2 = { 2.5f, 0.0f }; + y += 1.0f; + } + + bodyDef.position = { 0.0f, y }; + bodyDef.rotation = b2Rot_identity; + b2BodyId platformId = b2CreateBody( m_worldId, &bodyDef ); + + b2Polygon box = b2MakeBox( 3.0f, 0.2f ); + b2CreatePolygonShape( platformId, &shapeDef, &box ); + + // left pin + b2RevoluteJointDef revoluteDef = b2DefaultRevoluteJointDef(); + revoluteDef.bodyIdA = platformId; + revoluteDef.bodyIdB = baseId1; + revoluteDef.localAnchorA = { -2.5f, -0.4f }; + revoluteDef.localAnchorB = baseAnchor1; + revoluteDef.enableMotor = false; + revoluteDef.maxMotorTorque = 1.0f; + revoluteDef.collideConnected = true; + b2CreateRevoluteJoint( m_worldId, &revoluteDef ); + + // right pin + b2WheelJointDef wheelDef = b2DefaultWheelJointDef(); + wheelDef.bodyIdA = platformId; + wheelDef.bodyIdB = baseId2; + wheelDef.localAxisA = { 1.0f, 0.0f }; + wheelDef.localAnchorA = { 2.5f, -0.4f }; + wheelDef.localAnchorB = baseAnchor2; + wheelDef.enableSpring = false; + wheelDef.collideConnected = true; + b2CreateWheelJoint( m_worldId, &wheelDef ); + + m_enableMotor = false; + m_motorSpeed = 0.25f; + m_motorForce = 2000.0f; + + b2DistanceJointDef distanceDef = b2DefaultDistanceJointDef(); + distanceDef.bodyIdA = groundId; + distanceDef.bodyIdB = linkId1; + distanceDef.localAnchorA = { -2.5f, 0.2f }; + distanceDef.localAnchorB = { 0.5f, 0.0f }; + distanceDef.enableSpring = true; + distanceDef.minLength = 0.2f; + distanceDef.maxLength = 5.5f; + distanceDef.enableLimit = true; + distanceDef.enableMotor = m_enableMotor; + distanceDef.motorSpeed = m_motorSpeed; + distanceDef.maxMotorForce = m_motorForce; + m_liftJointId = b2CreateDistanceJoint( m_worldId, &distanceDef ); + + Car car; + car.Spawn( m_worldId, { 0.0f, y + 2.0f }, 1.0f, 3.0f, 0.7f, 0.0f, NULL ); + } + + void UpdateUI() override + { + float height = 140.0f; + ImGui::SetNextWindowPos( ImVec2( 10.0f, g_camera.m_height - height - 50.0f ), ImGuiCond_Once ); + ImGui::SetNextWindowSize( ImVec2( 240.0f, height ) ); + + ImGui::Begin( "Scissor Lift", nullptr, ImGuiWindowFlags_NoResize ); + + if ( ImGui::Checkbox( "Motor", &m_enableMotor ) ) + { + b2DistanceJoint_EnableMotor( m_liftJointId, m_enableMotor ); + b2Joint_WakeBodies( m_liftJointId ); + } + + if ( ImGui::SliderFloat( "Max Force", &m_motorForce, 0.0f, 3000.0f, "%.0f" ) ) + { + b2DistanceJoint_SetMaxMotorForce( m_liftJointId, m_motorForce ); + b2Joint_WakeBodies( m_liftJointId ); + } + + if ( ImGui::SliderFloat( "Speed", &m_motorSpeed, -0.3f, 0.3f, "%.2f" ) ) + { + b2DistanceJoint_SetMotorSpeed( m_liftJointId, m_motorSpeed ); + b2Joint_WakeBodies( m_liftJointId ); + } + + ImGui::End(); + } + + void Step( Settings& settings ) override + { + Sample::Step( settings ); + } + + static Sample* Create( Settings& settings ) + { + return new ScissorLift( settings ); + } + + b2JointId m_liftJointId; + float m_motorForce; + float m_motorSpeed; + bool m_enableMotor; +}; + +static int sampleScissorLift = RegisterSample( "Joints", "Scissor Lift", ScissorLift::Create ); diff --git a/tests/cpp-tests/Source/Box2DTestBed/samples/sample_robustness.cpp b/tests/cpp-tests/Source/Box2DTestBed/samples/sample_robustness.cpp new file mode 100644 index 000000000000..5d997f2195b3 --- /dev/null +++ b/tests/cpp-tests/Source/Box2DTestBed/samples/sample_robustness.cpp @@ -0,0 +1,261 @@ +// SPDX-FileCopyrightText: 2022 Erin Catto +// SPDX-License-Identifier: MIT + +#include "draw.h" +#include "sample.h" +#include "settings.h" + +#include "box2d/box2d.h" + +#include +#include + +// Pyramid with heavy box on top +class HighMassRatio1 : public Sample +{ +public: + explicit HighMassRatio1( Settings& settings ) + : Sample( settings ) + { + if ( settings.restart == false ) + { + g_camera.m_center = { 3.0f, 14.0f }; + g_camera.m_zoom = 25.0f; + } + + float extent = 1.0f; + + { + b2BodyDef bodyDef = b2DefaultBodyDef(); + b2BodyId groundId = b2CreateBody( m_worldId, &bodyDef ); + b2ShapeDef shapeDef = b2DefaultShapeDef(); + b2Polygon box = b2MakeOffsetBox( 50.0f, 1.0f, { 0.0f, -1.0f }, b2Rot_identity ); + b2CreatePolygonShape( groundId, &shapeDef, &box ); + } + + { + b2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.type = b2_dynamicBody; + b2Polygon box = b2MakeBox( extent, extent ); + b2ShapeDef shapeDef = b2DefaultShapeDef(); + + for ( int j = 0; j < 3; ++j ) + { + int count = 10; + float offset = -20.0f * extent + 2.0f * ( count + 1.0f ) * extent * j; + float y = extent; + while ( count > 0 ) + { + for ( int i = 0; i < count; ++i ) + { + float coeff = i - 0.5f * count; + + float yy = count == 1 ? y + 2.0f : y; + bodyDef.position = { 2.0f * coeff * extent + offset, yy }; + b2BodyId bodyId = b2CreateBody( m_worldId, &bodyDef ); + + shapeDef.density = count == 1 ? ( j + 1.0f ) * 100.0f : 1.0f; + b2CreatePolygonShape( bodyId, &shapeDef, &box ); + } + + --count; + y += 2.0f * extent; + } + } + } + } + + static Sample* Create( Settings& settings ) + { + return new HighMassRatio1( settings ); + } +}; + +static int sampleIndex1 = RegisterSample( "Robustness", "HighMassRatio1", HighMassRatio1::Create ); + +// Big box on small boxes +class HighMassRatio2 : public Sample +{ +public: + explicit HighMassRatio2( Settings& settings ) + : Sample( settings ) + { + if ( settings.restart == false ) + { + g_camera.m_center = { 0.0f, 16.5f }; + g_camera.m_zoom = 25.0f; + } + + { + b2BodyDef bodyDef = b2DefaultBodyDef(); + b2BodyId groundId = b2CreateBody( m_worldId, &bodyDef ); + b2ShapeDef shapeDef = b2DefaultShapeDef(); + b2Polygon box = b2MakeOffsetBox( 50.0f, 1.0f, { 0.0f, -1.0f }, b2Rot_identity ); + b2CreatePolygonShape( groundId, &shapeDef, &box ); + } + + { + b2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.type = b2_dynamicBody; + b2ShapeDef shapeDef = b2DefaultShapeDef(); + + float extent = 1.0f; + b2Vec2 points[3] = { { -0.5f * extent, 0.0f }, { 0.5f * extent, 0.0f }, { 0.0f, 1.0f * extent } }; + b2Hull hull = b2ComputeHull( points, 3 ); + b2Polygon smallTriangle = b2MakePolygon( &hull, 0.0f ); + b2Polygon smallBox = b2MakeBox( 0.5f * extent, 0.5f * extent ); + b2Polygon bigBox = b2MakeBox( 10.0f * extent, 10.0f * extent ); + + { + bodyDef.position = { -9.0f * extent, 0.5f * extent }; + b2BodyId bodyId = b2CreateBody( m_worldId, &bodyDef ); + b2CreatePolygonShape( bodyId, &shapeDef, &smallBox ); + } + + { + bodyDef.position = { 9.0f * extent, 0.5f * extent }; + b2BodyId bodyId = b2CreateBody( m_worldId, &bodyDef ); + b2CreatePolygonShape( bodyId, &shapeDef, &smallBox ); + } + + { + bodyDef.position = { 0.0f, ( 10.0f + 16.0f ) * extent }; + b2BodyId bodyId = b2CreateBody( m_worldId, &bodyDef ); + b2CreatePolygonShape( bodyId, &shapeDef, &bigBox ); + } + } + } + + static Sample* Create( Settings& settings ) + { + return new HighMassRatio2( settings ); + } +}; + +static int sampleIndex2 = RegisterSample( "Robustness", "HighMassRatio2", HighMassRatio2::Create ); + +class OverlapRecovery : public Sample +{ +public: + explicit OverlapRecovery( Settings& settings ) + : Sample( settings ) + { + if ( settings.restart == false ) + { + g_camera.m_center = { 0.0f, 2.5f }; + g_camera.m_zoom = 25.0f * 0.15f; + } + + m_bodyIds = nullptr; + m_bodyCount = 0; + m_baseCount = 4; + m_overlap = 0.25f; + m_extent = 0.5f; + m_pushout = 3.0f; + m_hertz = 30.0f; + m_dampingRatio = 10.0f; + + b2BodyDef bodyDef = b2DefaultBodyDef(); + b2BodyId groundId = b2CreateBody( m_worldId, &bodyDef ); + + float groundWidth = 40.0f; + b2ShapeDef shapeDef = b2DefaultShapeDef(); + shapeDef.density = 1.0f; + + b2Segment segment = { { -groundWidth, 0.0f }, { groundWidth, 0.0f } }; + b2CreateSegmentShape( groundId, &shapeDef, &segment ); + + CreateScene(); + } + + ~OverlapRecovery() override + { + free( m_bodyIds ); + } + + void CreateScene() + { + for ( int32_t i = 0; i < m_bodyCount; ++i ) + { + b2DestroyBody( m_bodyIds[i] ); + } + + b2World_SetContactTuning( m_worldId, m_hertz, m_dampingRatio, m_pushout ); + + b2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.type = b2_dynamicBody; + + b2Polygon box = b2MakeBox( m_extent, m_extent ); + b2ShapeDef shapeDef = b2DefaultShapeDef(); + shapeDef.density = 1.0f; + + m_bodyCount = m_baseCount * ( m_baseCount + 1 ) / 2; + m_bodyIds = (b2BodyId*)realloc( m_bodyIds, m_bodyCount * sizeof( b2BodyId ) ); + + int32_t bodyIndex = 0; + float fraction = 1.0f - m_overlap; + float y = m_extent; + for ( int32_t i = 0; i < m_baseCount; ++i ) + { + float x = fraction * m_extent * ( i - m_baseCount ); + for ( int32_t j = i; j < m_baseCount; ++j ) + { + bodyDef.position = { x, y }; + b2BodyId bodyId = b2CreateBody( m_worldId, &bodyDef ); + + b2CreatePolygonShape( bodyId, &shapeDef, &box ); + + m_bodyIds[bodyIndex++] = bodyId; + + x += 2.0f * fraction * m_extent; + } + + y += 2.0f * fraction * m_extent; + } + + assert( bodyIndex == m_bodyCount ); + } + + void UpdateUI() override + { + float height = 210.0f; + ImGui::SetNextWindowPos( ImVec2( 10.0f, g_camera.m_height - height - 50.0f ), ImGuiCond_Once ); + ImGui::SetNextWindowSize( ImVec2( 220.0f, height ) ); + + ImGui::Begin( "Overlap Recovery", nullptr, ImGuiWindowFlags_NoResize ); + ImGui::PushItemWidth( 100.0f ); + + bool changed = false; + changed = changed || ImGui::SliderFloat( "Extent", &m_extent, 0.1f, 1.0f, "%.1f" ); + changed = changed || ImGui::SliderInt( "Base Count", &m_baseCount, 1, 10 ); + changed = changed || ImGui::SliderFloat( "Overlap", &m_overlap, 0.0f, 1.0f, "%.2f" ); + changed = changed || ImGui::SliderFloat( "Pushout", &m_pushout, 0.0f, 10.0f, "%.1f" ); + changed = changed || ImGui::SliderFloat( "Hertz", &m_hertz, 0.0f, 120.0f, "%.f" ); + changed = changed || ImGui::SliderFloat( "Damping Ratio", &m_dampingRatio, 0.0f, 20.0f, "%.1f" ); + changed = changed || ImGui::Button( "Reset Scene" ); + + if ( changed ) + { + CreateScene(); + } + + ImGui::PopItemWidth(); + ImGui::End(); + } + + static Sample* Create( Settings& settings ) + { + return new OverlapRecovery( settings ); + } + + b2BodyId* m_bodyIds; + int32_t m_bodyCount; + int32_t m_baseCount; + float m_overlap; + float m_extent; + float m_pushout; + float m_hertz; + float m_dampingRatio; +}; + +static int sampleIndex4 = RegisterSample( "Robustness", "Overlap Recovery", OverlapRecovery::Create ); diff --git a/tests/cpp-tests/Source/Box2DTestBed/samples/sample_shapes.cpp b/tests/cpp-tests/Source/Box2DTestBed/samples/sample_shapes.cpp new file mode 100644 index 000000000000..91df17c43f88 --- /dev/null +++ b/tests/cpp-tests/Source/Box2DTestBed/samples/sample_shapes.cpp @@ -0,0 +1,1304 @@ +// SPDX-FileCopyrightText: 2022 Erin Catto +// SPDX-License-Identifier: MIT + +#include "draw.h" +#include "sample.h" +#include "settings.h" + +#include "box2d/box2d.h" +#include "box2d/math_functions.h" + +#include +#include + +class ChainShape : public Sample +{ +public: + enum ShapeType + { + e_circleShape = 0, + e_capsuleShape, + e_boxShape + }; + + explicit ChainShape( Settings& settings ) + : Sample( settings ) + { + if ( settings.restart == false ) + { + g_camera.m_center = { 0.0f, 0.0f }; + g_camera.m_zoom = 25.0f * 1.75f; + } + + m_groundId = b2_nullBodyId; + m_bodyId = b2_nullBodyId; + m_chainId = b2_nullChainId; + m_shapeId = b2_nullShapeId; + m_shapeType = e_circleShape; + m_restitution = 0.0f; + m_friction = 0.2f; + CreateScene(); + Launch(); + } + + void CreateScene() + { + if ( B2_IS_NON_NULL( m_groundId ) ) + { + b2DestroyBody( m_groundId ); + } + + // https://betravis.github.io/shape-tools/path-to-polygon/ + // b2Vec2 points[] = {{-20.58325, 14.54175}, {-21.90625, 15.8645}, {-24.552, 17.1875}, + // {-27.198, 11.89575}, {-29.84375, 15.8645}, {-29.84375, 21.15625}, + // {-25.875, 23.802}, {-20.58325, 25.125}, {-25.875, 29.09375}, + // {-20.58325, 31.7395}, {-11.0089998, 23.2290001}, {-8.67700005, 21.15625}, + // {-6.03125, 21.15625}, {-7.35424995, 29.09375}, {-3.38549995, 29.09375}, + // {1.90625, 30.41675}, {5.875, 17.1875}, {11.16675, 25.125}, + // {9.84375, 29.09375}, {13.8125, 31.7395}, {21.75, 30.41675}, + // {28.3644981, 26.448}, {25.71875, 18.5105}, {24.3957481, 13.21875}, + // {17.78125, 11.89575}, {15.1355, 7.92700005}, {5.875, 9.25}, + // {1.90625, 11.89575}, {-3.25, 11.89575}, {-3.25, 9.9375}, + // {-4.70825005, 9.25}, {-8.67700005, 9.25}, {-11.323, 11.89575}, + // {-13.96875, 11.89575}, {-15.29175, 14.54175}, {-19.2605, 14.54175}}; + + b2Vec2 points[] = { + { -56.885498, 12.8985004 }, { -56.885498, 16.2057495 }, { 56.885498, 16.2057495 }, { 56.885498, -16.2057514 }, + { 51.5935059, -16.2057514 }, { 43.6559982, -10.9139996 }, { 35.7184982, -10.9139996 }, { 27.7809982, -10.9139996 }, + { 21.1664963, -14.2212505 }, { 11.9059982, -16.2057514 }, { 0, -16.2057514 }, { -10.5835037, -14.8827496 }, + { -17.1980019, -13.5597477 }, { -21.1665001, -12.2370014 }, { -25.1355019, -9.5909977 }, { -31.75, -3.63799858 }, + { -38.3644981, 6.2840004 }, { -42.3334999, 9.59125137 }, { -47.625, 11.5755005 }, { -56.885498, 12.8985004 }, + }; + + int count = sizeof( points ) / sizeof( points[0] ); + + // float scale = 0.25f; + // b2Vec2 lower = {FLT_MAX, FLT_MAX}; + // b2Vec2 upper = {-FLT_MAX, -FLT_MAX}; + // for (int i = 0; i < count; ++i) + //{ + // points[i].x = 2.0f * scale * points[i].x; + // points[i].y = -scale * points[i].y; + + // lower = b2Min(lower, points[i]); + // upper = b2Max(upper, points[i]); + //} + + // b2Vec2 center = b2MulSV(0.5f, b2Add(lower, upper)); + // for (int i = 0; i < count; ++i) + //{ + // points[i] = b2Sub(points[i], center); + // } + + // for (int i = 0; i < count / 2; ++i) + //{ + // b2Vec2 temp = points[i]; + // points[i] = points[count - 1 - i]; + // points[count - 1 - i] = temp; + // } + + // printf("{"); + // for (int i = 0; i < count; ++i) + //{ + // printf("{%.9g, %.9g},", points[i].x, points[i].y); + // } + // printf("};\n"); + + b2ChainDef chainDef = b2DefaultChainDef(); + chainDef.points = points; + chainDef.count = count; + chainDef.isLoop = true; + chainDef.friction = 0.2f; + + b2BodyDef bodyDef = b2DefaultBodyDef(); + m_groundId = b2CreateBody( m_worldId, &bodyDef ); + + m_chainId = b2CreateChain( m_groundId, &chainDef ); + } + + void Launch() + { + if ( B2_IS_NON_NULL( m_bodyId ) ) + { + b2DestroyBody( m_bodyId ); + } + + b2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.type = b2_dynamicBody; + bodyDef.position = { -55.0f, 13.5f }; + m_bodyId = b2CreateBody( m_worldId, &bodyDef ); + + b2ShapeDef shapeDef = b2DefaultShapeDef(); + shapeDef.density = 1.0f; + shapeDef.friction = m_friction; + shapeDef.restitution = m_restitution; + + if ( m_shapeType == e_circleShape ) + { + b2Circle circle = { { 0.0f, 0.0f }, 0.5f }; + m_shapeId = b2CreateCircleShape( m_bodyId, &shapeDef, &circle ); + } + else if ( m_shapeType == e_capsuleShape ) + { + b2Capsule capsule = { { -0.5f, 0.0f }, { 0.5f, 0.0 }, 0.25f }; + m_shapeId = b2CreateCapsuleShape( m_bodyId, &shapeDef, &capsule ); + } + else + { + float h = 0.5f; + b2Polygon box = b2MakeBox( h, h ); + m_shapeId = b2CreatePolygonShape( m_bodyId, &shapeDef, &box ); + } + } + + void UpdateUI() override + { + float height = 135.0f; + ImGui::SetNextWindowPos( ImVec2( 10.0f, g_camera.m_height - height - 50.0f ), ImGuiCond_Once ); + ImGui::SetNextWindowSize( ImVec2( 240.0f, height ) ); + + ImGui::Begin( "Chain Shape", nullptr, ImGuiWindowFlags_NoResize ); + + const char* shapeTypes[] = { "Circle", "Capsule", "Box" }; + int shapeType = int( m_shapeType ); + if ( ImGui::Combo( "Shape", &shapeType, shapeTypes, IM_ARRAYSIZE( shapeTypes ) ) ) + { + m_shapeType = ShapeType( shapeType ); + Launch(); + } + + if ( ImGui::SliderFloat( "Friction", &m_friction, 0.0f, 1.0f, "%.2f" ) ) + { + b2Shape_SetFriction( m_shapeId, m_friction ); + b2Chain_SetFriction( m_chainId, m_friction ); + } + + if ( ImGui::SliderFloat( "Restitution", &m_restitution, 0.0f, 2.0f, "%.1f" ) ) + { + b2Shape_SetRestitution( m_shapeId, m_restitution ); + } + + if ( ImGui::Button( "Launch" ) ) + { + Launch(); + } + + ImGui::End(); + } + + void Step( Settings& settings ) override + { + Sample::Step( settings ); + + g_draw.DrawSegment( b2Vec2_zero, { 0.5f, 0.0f }, b2_colorRed ); + g_draw.DrawSegment( b2Vec2_zero, { 0.0f, 0.5f }, b2_colorGreen ); + } + + static Sample* Create( Settings& settings ) + { + return new ChainShape( settings ); + } + + b2BodyId m_groundId; + b2BodyId m_bodyId; + b2ChainId m_chainId; + ShapeType m_shapeType; + b2ShapeId m_shapeId; + float m_restitution; + float m_friction; +}; + +static int sampleChainShape = RegisterSample( "Shapes", "Chain Shape", ChainShape::Create ); + +// This sample shows how careful creation of compound shapes leads to better simulation and avoids +// objects getting stuck. +// This also shows how to get the combined AABB for the body. +class CompoundShapes : public Sample +{ +public: + explicit CompoundShapes( Settings& settings ) + : Sample( settings ) + { + if ( settings.restart == false ) + { + g_camera.m_center = { 0.0f, 6.0f }; + g_camera.m_zoom = 25.0f * 0.5f; + } + + { + b2BodyDef bodyDef = b2DefaultBodyDef(); + b2BodyId groundId = b2CreateBody( m_worldId, &bodyDef ); + b2ShapeDef shapeDef = b2DefaultShapeDef(); + b2Segment segment = { { 50.0f, 0.0f }, { -50.0f, 0.0f } }; + b2CreateSegmentShape( groundId, &shapeDef, &segment ); + } + + // Table 1 + { + b2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.type = b2_dynamicBody; + bodyDef.position = { -15.0f, 1.0f }; + m_table1Id = b2CreateBody( m_worldId, &bodyDef ); + + b2ShapeDef shapeDef = b2DefaultShapeDef(); + b2Polygon top = b2MakeOffsetBox( 3.0f, 0.5f, { 0.0f, 3.5f }, b2Rot_identity ); + b2Polygon leftLeg = b2MakeOffsetBox( 0.5f, 1.5f, { -2.5f, 1.5f }, b2Rot_identity ); + b2Polygon rightLeg = b2MakeOffsetBox( 0.5f, 1.5f, { 2.5f, 1.5f }, b2Rot_identity ); + + b2CreatePolygonShape( m_table1Id, &shapeDef, &top ); + b2CreatePolygonShape( m_table1Id, &shapeDef, &leftLeg ); + b2CreatePolygonShape( m_table1Id, &shapeDef, &rightLeg ); + } + + // Table 2 + { + b2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.type = b2_dynamicBody; + bodyDef.position = { -5.0f, 1.0f }; + m_table2Id = b2CreateBody( m_worldId, &bodyDef ); + + b2ShapeDef shapeDef = b2DefaultShapeDef(); + b2Polygon top = b2MakeOffsetBox( 3.0f, 0.5f, { 0.0f, 3.5f }, b2Rot_identity ); + b2Polygon leftLeg = b2MakeOffsetBox( 0.5f, 2.0f, { -2.5f, 2.0f }, b2Rot_identity ); + b2Polygon rightLeg = b2MakeOffsetBox( 0.5f, 2.0f, { 2.5f, 2.0f }, b2Rot_identity ); + + b2CreatePolygonShape( m_table2Id, &shapeDef, &top ); + b2CreatePolygonShape( m_table2Id, &shapeDef, &leftLeg ); + b2CreatePolygonShape( m_table2Id, &shapeDef, &rightLeg ); + } + + // Spaceship 1 + { + b2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.type = b2_dynamicBody; + bodyDef.position = { 5.0f, 1.0f }; + m_ship1Id = b2CreateBody( m_worldId, &bodyDef ); + + b2ShapeDef shapeDef = b2DefaultShapeDef(); + b2Vec2 vertices[3]; + + vertices[0] = { -2.0f, 0.0f }; + vertices[1] = { 0.0f, 4.0f / 3.0f }; + vertices[2] = { 0.0f, 4.0f }; + b2Hull hull = b2ComputeHull( vertices, 3 ); + b2Polygon left = b2MakePolygon( &hull, 0.0f ); + + vertices[0] = { 2.0f, 0.0f }; + vertices[1] = { 0.0f, 4.0f / 3.0f }; + vertices[2] = { 0.0f, 4.0f }; + hull = b2ComputeHull( vertices, 3 ); + b2Polygon right = b2MakePolygon( &hull, 0.0f ); + + b2CreatePolygonShape( m_ship1Id, &shapeDef, &left ); + b2CreatePolygonShape( m_ship1Id, &shapeDef, &right ); + } + + // Spaceship 2 + { + b2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.type = b2_dynamicBody; + bodyDef.position = { 15.0f, 1.0f }; + m_ship2Id = b2CreateBody( m_worldId, &bodyDef ); + + b2ShapeDef shapeDef = b2DefaultShapeDef(); + b2Vec2 vertices[3]; + + vertices[0] = { -2.0f, 0.0f }; + vertices[1] = { 1.0f, 2.0f }; + vertices[2] = { 0.0f, 4.0f }; + b2Hull hull = b2ComputeHull( vertices, 3 ); + b2Polygon left = b2MakePolygon( &hull, 0.0f ); + + vertices[0] = { 2.0f, 0.0f }; + vertices[1] = { -1.0f, 2.0f }; + vertices[2] = { 0.0f, 4.0f }; + hull = b2ComputeHull( vertices, 3 ); + b2Polygon right = b2MakePolygon( &hull, 0.0f ); + + b2CreatePolygonShape( m_ship2Id, &shapeDef, &left ); + b2CreatePolygonShape( m_ship2Id, &shapeDef, &right ); + } + + m_drawBodyAABBs = false; + } + + void Spawn() + { + // Table 1 obstruction + { + b2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.type = b2_dynamicBody; + bodyDef.position = b2Body_GetPosition( m_table1Id ); + bodyDef.rotation = b2Body_GetRotation( m_table1Id ); + b2BodyId bodyId = b2CreateBody( m_worldId, &bodyDef ); + + b2ShapeDef shapeDef = b2DefaultShapeDef(); + b2Polygon box = b2MakeOffsetBox( 4.0f, 0.1f, { 0.0f, 3.0f }, b2Rot_identity ); + b2CreatePolygonShape( bodyId, &shapeDef, &box ); + } + + // Table 2 obstruction + { + b2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.type = b2_dynamicBody; + bodyDef.position = b2Body_GetPosition( m_table2Id ); + bodyDef.rotation = b2Body_GetRotation( m_table2Id ); + b2BodyId bodyId = b2CreateBody( m_worldId, &bodyDef ); + + b2ShapeDef shapeDef = b2DefaultShapeDef(); + b2Polygon box = b2MakeOffsetBox( 4.0f, 0.1f, { 0.0f, 3.0f }, b2Rot_identity ); + b2CreatePolygonShape( bodyId, &shapeDef, &box ); + } + + // Ship 1 obstruction + { + b2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.type = b2_dynamicBody; + bodyDef.position = b2Body_GetPosition( m_ship1Id ); + bodyDef.rotation = b2Body_GetRotation( m_ship1Id ); + // bodyDef.gravityScale = 0.0f; + b2BodyId bodyId = b2CreateBody( m_worldId, &bodyDef ); + + b2ShapeDef shapeDef = b2DefaultShapeDef(); + b2Circle circle = { { 0.0f, 2.0f }, 0.5f }; + b2CreateCircleShape( bodyId, &shapeDef, &circle ); + } + + // Ship 2 obstruction + { + b2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.type = b2_dynamicBody; + bodyDef.position = b2Body_GetPosition( m_ship2Id ); + bodyDef.rotation = b2Body_GetRotation( m_ship2Id ); + // bodyDef.gravityScale = 0.0f; + b2BodyId bodyId = b2CreateBody( m_worldId, &bodyDef ); + + b2ShapeDef shapeDef = b2DefaultShapeDef(); + b2Circle circle = { { 0.0f, 2.0f }, 0.5f }; + b2CreateCircleShape( bodyId, &shapeDef, &circle ); + } + } + + void UpdateUI() override + { + float height = 100.0f; + ImGui::SetNextWindowPos( ImVec2( 10.0f, g_camera.m_height - height - 50.0f ), ImGuiCond_Once ); + ImGui::SetNextWindowSize( ImVec2( 180.0f, height ) ); + + ImGui::Begin( "Compound Shapes", nullptr, ImGuiWindowFlags_NoResize ); + + if ( ImGui::Button( "Intrude" ) ) + { + Spawn(); + } + + ImGui::Checkbox( "Body AABBs", &m_drawBodyAABBs ); + + ImGui::End(); + } + + void Step( Settings& settings ) override + { + Sample::Step( settings ); + + if ( m_drawBodyAABBs ) + { + b2AABB aabb = b2Body_ComputeAABB( m_table1Id ); + g_draw.DrawAABB( aabb, b2_colorYellow ); + + aabb = b2Body_ComputeAABB( m_table2Id ); + g_draw.DrawAABB( aabb, b2_colorYellow ); + + aabb = b2Body_ComputeAABB( m_ship1Id ); + g_draw.DrawAABB( aabb, b2_colorYellow ); + + aabb = b2Body_ComputeAABB( m_ship2Id ); + g_draw.DrawAABB( aabb, b2_colorYellow ); + } + } + + static Sample* Create( Settings& settings ) + { + return new CompoundShapes( settings ); + } + + b2BodyId m_table1Id; + b2BodyId m_table2Id; + b2BodyId m_ship1Id; + b2BodyId m_ship2Id; + bool m_drawBodyAABBs; +}; + +static int sampleCompoundShape = RegisterSample( "Shapes", "Compound Shapes", CompoundShapes::Create ); + +class ShapeFilter : public Sample +{ +public: + enum CollisionBits + { + GROUND = 0x00000001, + TEAM1 = 0x00000002, + TEAM2 = 0x00000004, + TEAM3 = 0x00000008, + + ALL_BITS = ( ~0u ) + }; + + explicit ShapeFilter( Settings& settings ) + : Sample( settings ) + { + if ( settings.restart == false ) + { + g_camera.m_zoom = 25.0f * 0.5f; + g_camera.m_center = { 0.0f, 5.0f }; + } + + { + b2BodyDef bodyDef = b2DefaultBodyDef(); + b2BodyId groundId = b2CreateBody( m_worldId, &bodyDef ); + b2Segment segment = { { -20.0f, 0.0f }, { 20.0f, 0.0f } }; + + b2ShapeDef shapeDef = b2DefaultShapeDef(); + shapeDef.filter.categoryBits = GROUND; + shapeDef.filter.maskBits = ALL_BITS; + + b2CreateSegmentShape( groundId, &shapeDef, &segment ); + } + + { + b2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.type = b2_dynamicBody; + + bodyDef.position = { 0.0f, 2.0f }; + m_player1Id = b2CreateBody( m_worldId, &bodyDef ); + + bodyDef.position = { 0.0f, 5.0f }; + m_player2Id = b2CreateBody( m_worldId, &bodyDef ); + + bodyDef.position = { 0.0f, 8.0f }; + m_player3Id = b2CreateBody( m_worldId, &bodyDef ); + + b2Polygon box = b2MakeBox( 2.0f, 1.0f ); + + b2ShapeDef shapeDef = b2DefaultShapeDef(); + + shapeDef.filter.categoryBits = TEAM1; + shapeDef.filter.maskBits = GROUND | TEAM2 | TEAM3; + m_shape1Id = b2CreatePolygonShape( m_player1Id, &shapeDef, &box ); + + shapeDef.filter.categoryBits = TEAM2; + shapeDef.filter.maskBits = GROUND | TEAM1 | TEAM3; + m_shape2Id = b2CreatePolygonShape( m_player2Id, &shapeDef, &box ); + + shapeDef.filter.categoryBits = TEAM3; + shapeDef.filter.maskBits = GROUND | TEAM1 | TEAM2; + m_shape3Id = b2CreatePolygonShape( m_player3Id, &shapeDef, &box ); + } + } + + void UpdateUI() override + { + float height = 240.0f; + ImGui::SetNextWindowPos( ImVec2( 10.0f, g_camera.m_height - height - 50.0f ), ImGuiCond_Once ); + ImGui::SetNextWindowSize( ImVec2( 240.0f, height ) ); + + ImGui::Begin( "Shape Filter", nullptr, ImGuiWindowFlags_NoResize ); + + ImGui::Text( "Player 1 Collides With" ); + { + b2Filter filter1 = b2Shape_GetFilter( m_shape1Id ); + bool team2 = ( filter1.maskBits & TEAM2 ) == TEAM2; + if ( ImGui::Checkbox( "Team 2##1", &team2 ) ) + { + if ( team2 ) + { + filter1.maskBits |= TEAM2; + } + else + { + filter1.maskBits &= ~TEAM2; + } + + b2Shape_SetFilter( m_shape1Id, filter1 ); + } + + bool team3 = ( filter1.maskBits & TEAM3 ) == TEAM3; + if ( ImGui::Checkbox( "Team 3##1", &team3 ) ) + { + if ( team3 ) + { + filter1.maskBits |= TEAM3; + } + else + { + filter1.maskBits &= ~TEAM3; + } + + b2Shape_SetFilter( m_shape1Id, filter1 ); + } + } + + ImGui::Separator(); + + ImGui::Text( "Player 2 Collides With" ); + { + b2Filter filter2 = b2Shape_GetFilter( m_shape2Id ); + bool team1 = ( filter2.maskBits & TEAM1 ) == TEAM1; + if ( ImGui::Checkbox( "Team 1##2", &team1 ) ) + { + if ( team1 ) + { + filter2.maskBits |= TEAM1; + } + else + { + filter2.maskBits &= ~TEAM1; + } + + b2Shape_SetFilter( m_shape2Id, filter2 ); + } + + bool team3 = ( filter2.maskBits & TEAM3 ) == TEAM3; + if ( ImGui::Checkbox( "Team 3##2", &team3 ) ) + { + if ( team3 ) + { + filter2.maskBits |= TEAM3; + } + else + { + filter2.maskBits &= ~TEAM3; + } + + b2Shape_SetFilter( m_shape2Id, filter2 ); + } + } + + ImGui::Separator(); + + ImGui::Text( "Player 3 Collides With" ); + { + b2Filter filter3 = b2Shape_GetFilter( m_shape3Id ); + bool team1 = ( filter3.maskBits & TEAM1 ) == TEAM1; + if ( ImGui::Checkbox( "Team 1##3", &team1 ) ) + { + if ( team1 ) + { + filter3.maskBits |= TEAM1; + } + else + { + filter3.maskBits &= ~TEAM1; + } + + b2Shape_SetFilter( m_shape3Id, filter3 ); + } + + bool team2 = ( filter3.maskBits & TEAM2 ) == TEAM2; + if ( ImGui::Checkbox( "Team 2##3", &team2 ) ) + { + if ( team2 ) + { + filter3.maskBits |= TEAM2; + } + else + { + filter3.maskBits &= ~TEAM2; + } + + b2Shape_SetFilter( m_shape3Id, filter3 ); + } + } + + ImGui::End(); + } + + void Step( Settings& settings ) override + { + Sample::Step( settings ); + + b2Vec2 p1 = b2Body_GetPosition( m_player1Id ); + g_draw.DrawString( { p1.x - 0.5f, p1.y }, "player 1" ); + + b2Vec2 p2 = b2Body_GetPosition( m_player2Id ); + g_draw.DrawString( { p2.x - 0.5f, p2.y }, "player 2" ); + + b2Vec2 p3 = b2Body_GetPosition( m_player3Id ); + g_draw.DrawString( { p3.x - 0.5f, p3.y }, "player 3" ); + } + + static Sample* Create( Settings& settings ) + { + return new ShapeFilter( settings ); + } + + b2BodyId m_player1Id; + b2BodyId m_player2Id; + b2BodyId m_player3Id; + + b2ShapeId m_shape1Id; + b2ShapeId m_shape2Id; + b2ShapeId m_shape3Id; +}; + +static int sampleShapeFilter = RegisterSample( "Shapes", "Filter", ShapeFilter::Create ); + +// This shows how to use custom filtering +class CustomFilter : public Sample +{ +public: + enum + { + e_count = 10 + }; + + explicit CustomFilter( Settings& settings ) + : Sample( settings ) + { + if ( settings.restart == false ) + { + g_camera.m_center = { 0.0f, 5.0f }; + g_camera.m_zoom = 10.0f; + } + + // Register custom filter + b2World_SetCustomFilterCallback( m_worldId, CustomFilterStatic, this ); + + { + b2BodyDef bodyDef = b2DefaultBodyDef(); + b2BodyId groundId = b2CreateBody( m_worldId, &bodyDef ); + b2Segment segment = { { -40.0f, 0.0f }, { 40.0f, 0.0f } }; + + b2ShapeDef shapeDef = b2DefaultShapeDef(); + + b2CreateSegmentShape( groundId, &shapeDef, &segment ); + } + + b2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.type = b2_dynamicBody; + b2ShapeDef shapeDef = b2DefaultShapeDef(); + b2Polygon box = b2MakeSquare( 1.0f ); + float x = -e_count; + + for ( int i = 0; i < e_count; ++i ) + { + bodyDef.position = { x, 5.0f }; + m_bodyIds[i] = b2CreateBody( m_worldId, &bodyDef ); + + shapeDef.userData = reinterpret_cast( intptr_t( i + 1 ) ); + m_shapeIds[i] = b2CreatePolygonShape( m_bodyIds[i], &shapeDef, &box ); + x += 2.0f; + } + } + + void Step( Settings& settings ) override + { + g_draw.DrawString( 5, m_textLine, "Custom filter disables collision between odd and even shapes" ); + m_textLine += m_textIncrement; + + Sample::Step( settings ); + + for ( int i = 0; i < e_count; ++i ) + { + b2Vec2 p = b2Body_GetPosition( m_bodyIds[i] ); + g_draw.DrawString( { p.x, p.y }, "%d", i ); + } + } + + bool ShouldCollide( b2ShapeId shapeIdA, b2ShapeId shapeIdB ) + { + void* userDataA = b2Shape_GetUserData( shapeIdA ); + void* userDataB = b2Shape_GetUserData( shapeIdB ); + + if ( userDataA == NULL || userDataB == NULL ) + { + return true; + } + + int indexA = static_cast( reinterpret_cast( userDataA ) ); + int indexB = static_cast( reinterpret_cast( userDataB ) ); + + return ( ( indexA & 1 ) + ( indexB & 1 ) ) != 1; + } + + static bool CustomFilterStatic( b2ShapeId shapeIdA, b2ShapeId shapeIdB, void* context ) + { + CustomFilter* customFilter = static_cast( context ); + return customFilter->ShouldCollide( shapeIdA, shapeIdB ); + } + + static Sample* Create( Settings& settings ) + { + return new CustomFilter( settings ); + } + + b2BodyId m_bodyIds[e_count]; + b2ShapeId m_shapeIds[e_count]; +}; + +static int sampleCustomFilter = RegisterSample( "Shapes", "Custom Filter", CustomFilter::Create ); + +// Restitution is approximate since Box2D uses speculative collision +class Restitution : public Sample +{ +public: + enum + { + e_count = 40 + }; + + enum ShapeType + { + e_circleShape = 0, + e_boxShape + }; + + explicit Restitution( Settings& settings ) + : Sample( settings ) + { + if ( settings.restart == false ) + { + g_camera.m_center = { 4.0f, 17.0f }; + g_camera.m_zoom = 27.5f; + } + + { + b2BodyDef bodyDef = b2DefaultBodyDef(); + b2BodyId groundId = b2CreateBody( m_worldId, &bodyDef ); + + float h = 1.0f * e_count; + b2Segment segment = { { -h, 0.0f }, { h, 0.0f } }; + b2ShapeDef shapeDef = b2DefaultShapeDef(); + b2CreateSegmentShape( groundId, &shapeDef, &segment ); + } + + for ( int i = 0; i < e_count; ++i ) + { + m_bodyIds[i] = b2_nullBodyId; + } + + m_shapeType = e_circleShape; + + CreateBodies(); + } + + void CreateBodies() + { + for ( int i = 0; i < e_count; ++i ) + { + if ( B2_IS_NON_NULL( m_bodyIds[i] ) ) + { + b2DestroyBody( m_bodyIds[i] ); + m_bodyIds[i] = b2_nullBodyId; + } + } + + b2Circle circle = { 0 }; + circle.radius = 0.5f; + + b2Polygon box = b2MakeBox( 0.5f, 0.5f ); + + b2ShapeDef shapeDef = b2DefaultShapeDef(); + shapeDef.density = 1.0f; + shapeDef.restitution = 0.0f; + + b2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.type = b2_dynamicBody; + + float dr = 1.0f / ( e_count > 1 ? e_count - 1 : 1 ); + float x = -1.0f * ( e_count - 1 ); + float dx = 2.0f; + + for ( int i = 0; i < e_count; ++i ) + { + bodyDef.position = { x, 40.0f }; + b2BodyId bodyId = b2CreateBody( m_worldId, &bodyDef ); + + m_bodyIds[i] = bodyId; + + if ( m_shapeType == e_circleShape ) + { + b2CreateCircleShape( bodyId, &shapeDef, &circle ); + } + else + { + b2CreatePolygonShape( bodyId, &shapeDef, &box ); + } + + shapeDef.restitution += dr; + x += dx; + } + } + + void UpdateUI() override + { + float height = 100.0f; + ImGui::SetNextWindowPos( ImVec2( 10.0f, g_camera.m_height - height - 50.0f ), ImGuiCond_Once ); + ImGui::SetNextWindowSize( ImVec2( 240.0f, height ) ); + + ImGui::Begin( "Restitution", nullptr, ImGuiWindowFlags_NoResize ); + + bool changed = false; + const char* shapeTypes[] = { "Circle", "Box" }; + + int shapeType = int( m_shapeType ); + changed = changed || ImGui::Combo( "Shape", &shapeType, shapeTypes, IM_ARRAYSIZE( shapeTypes ) ); + m_shapeType = ShapeType( shapeType ); + + changed = changed || ImGui::Button( "Reset" ); + + if ( changed ) + { + CreateBodies(); + } + + ImGui::End(); + } + + static Sample* Create( Settings& settings ) + { + return new Restitution( settings ); + } + + b2BodyId m_bodyIds[e_count]; + ShapeType m_shapeType; +}; + +static int sampleIndex = RegisterSample( "Shapes", "Restitution", Restitution::Create ); + +class Friction : public Sample +{ +public: + explicit Friction( Settings& settings ) + : Sample( settings ) + { + if ( settings.restart == false ) + { + g_camera.m_center = { 0.0f, 14.0f }; + g_camera.m_zoom = 25.0f * 0.6f; + } + + { + b2BodyDef bodyDef = b2DefaultBodyDef(); + b2BodyId groundId = b2CreateBody( m_worldId, &bodyDef ); + + b2ShapeDef shapeDef = b2DefaultShapeDef(); + shapeDef.friction = 0.2f; + + b2Segment segment = { { -40.0f, 0.0f }, { 40.0f, 0.0f } }; + b2CreateSegmentShape( groundId, &shapeDef, &segment ); + + b2Polygon box = b2MakeOffsetBox( 13.0f, 0.25f, { -4.0f, 22.0f }, b2MakeRot(-0.25f) ); + b2CreatePolygonShape( groundId, &shapeDef, &box ); + + box = b2MakeOffsetBox( 0.25f, 1.0f, { 10.5f, 19.0f }, b2Rot_identity ); + b2CreatePolygonShape( groundId, &shapeDef, &box ); + + box = b2MakeOffsetBox( 13.0f, 0.25f, { 4.0f, 14.0f }, b2MakeRot(0.25f) ); + b2CreatePolygonShape( groundId, &shapeDef, &box ); + + box = b2MakeOffsetBox( 0.25f, 1.0f, { -10.5f, 11.0f }, b2Rot_identity ); + b2CreatePolygonShape( groundId, &shapeDef, &box ); + + box = b2MakeOffsetBox( 13.0f, 0.25f, { -4.0f, 6.0f }, b2MakeRot(-0.25f) ); + b2CreatePolygonShape( groundId, &shapeDef, &box ); + } + + { + b2Polygon box = b2MakeBox( 0.5f, 0.5f ); + + b2ShapeDef shapeDef = b2DefaultShapeDef(); + shapeDef.density = 25.0f; + + float friction[5] = { 0.75f, 0.5f, 0.35f, 0.1f, 0.0f }; + + for ( int i = 0; i < 5; ++i ) + { + b2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.type = b2_dynamicBody; + bodyDef.position = { -15.0f + 4.0f * i, 28.0f }; + b2BodyId bodyId = b2CreateBody( m_worldId, &bodyDef ); + + shapeDef.friction = friction[i]; + b2CreatePolygonShape( bodyId, &shapeDef, &box ); + } + } + } + + static Sample* Create( Settings& settings ) + { + return new Friction( settings ); + } +}; + +static int sampleIndex3 = RegisterSample( "Shapes", "Friction", Friction::Create ); + +// This sample shows how to modify the geometry on an existing shape. This is only supported on +// dynamic and kinematic shapes because static shapes don't look for new collisions. +class ModifyGeometry : public Sample +{ +public: + explicit ModifyGeometry( Settings& settings ) + : Sample( settings ) + { + if ( settings.restart == false ) + { + g_camera.m_zoom = 25.0f * 0.25f; + g_camera.m_center = { 0.0f, 5.0f }; + } + + { + b2BodyDef bodyDef = b2DefaultBodyDef(); + b2BodyId groundId = b2CreateBody( m_worldId, &bodyDef ); + b2ShapeDef shapeDef = b2DefaultShapeDef(); + b2Polygon box = b2MakeOffsetBox( 10.0f, 1.0f, { 0.0f, -1.0f }, b2Rot_identity ); + b2CreatePolygonShape( groundId, &shapeDef, &box ); + } + + { + b2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.type = b2_dynamicBody; + bodyDef.position = { 0.0f, 4.0f }; + b2BodyId bodyId = b2CreateBody( m_worldId, &bodyDef ); + b2ShapeDef shapeDef = b2DefaultShapeDef(); + b2Polygon box = b2MakeBox( 1.0f, 1.0f ); + b2CreatePolygonShape( bodyId, &shapeDef, &box ); + } + + { + m_shapeType = b2_circleShape; + m_scale = 1.0f; + m_circle = { { 0.0f, 0.0f }, 0.5f }; + b2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.type = b2_kinematicBody; + bodyDef.position = { 0.0f, 1.0f }; + b2BodyId bodyId = b2CreateBody( m_worldId, &bodyDef ); + b2ShapeDef shapeDef = b2DefaultShapeDef(); + m_shapeId = b2CreateCircleShape( bodyId, &shapeDef, &m_circle ); + } + } + + void UpdateShape() + { + switch ( m_shapeType ) + { + case b2_circleShape: + m_circle = { { 0.0f, 0.0f }, 0.5f * m_scale }; + b2Shape_SetCircle( m_shapeId, &m_circle ); + break; + + case b2_capsuleShape: + m_capsule = { { -0.5f * m_scale, 0.0f }, { 0.0f, 0.5f * m_scale }, 0.5f * m_scale }; + b2Shape_SetCapsule( m_shapeId, &m_capsule ); + break; + + case b2_segmentShape: + m_segment = { { -0.5f * m_scale, 0.0f }, { 0.75f * m_scale, 0.0f } }; + b2Shape_SetSegment( m_shapeId, &m_segment ); + break; + + case b2_polygonShape: + m_polygon = b2MakeBox( 0.5f * m_scale, 0.75f * m_scale ); + b2Shape_SetPolygon( m_shapeId, &m_polygon ); + break; + + default: + assert( false ); + break; + } + + b2BodyId bodyId = b2Shape_GetBody( m_shapeId ); + b2Body_ApplyMassFromShapes( bodyId ); + } + + void UpdateUI() override + { + float height = 230.0f; + ImGui::SetNextWindowPos( ImVec2( 10.0f, g_camera.m_height - height - 50.0f ), ImGuiCond_Once ); + ImGui::SetNextWindowSize( ImVec2( 200.0f, height ) ); + + ImGui::Begin( "Modify Geometry", nullptr, ImGuiWindowFlags_NoResize ); + + if ( ImGui::RadioButton( "Circle", m_shapeType == b2_circleShape ) ) + { + m_shapeType = b2_circleShape; + UpdateShape(); + } + + if ( ImGui::RadioButton( "Capsule", m_shapeType == b2_capsuleShape ) ) + { + m_shapeType = b2_capsuleShape; + UpdateShape(); + } + + if ( ImGui::RadioButton( "Segment", m_shapeType == b2_segmentShape ) ) + { + m_shapeType = b2_segmentShape; + UpdateShape(); + } + + if ( ImGui::RadioButton( "Polygon", m_shapeType == b2_polygonShape ) ) + { + m_shapeType = b2_polygonShape; + UpdateShape(); + } + + if ( ImGui::SliderFloat( "Scale", &m_scale, 0.1f, 10.0f, "%.2f" ) ) + { + UpdateShape(); + } + + b2BodyId bodyId = b2Shape_GetBody( m_shapeId ); + b2BodyType bodyType = b2Body_GetType( bodyId ); + + if ( ImGui::RadioButton( "Static", bodyType == b2_staticBody ) ) + { + b2Body_SetType( bodyId, b2_staticBody ); + } + + if ( ImGui::RadioButton( "Kinematic", bodyType == b2_kinematicBody ) ) + { + b2Body_SetType( bodyId, b2_kinematicBody ); + } + + if ( ImGui::RadioButton( "Dynamic", bodyType == b2_dynamicBody ) ) + { + b2Body_SetType( bodyId, b2_dynamicBody ); + } + + ImGui::End(); + } + + void Step( Settings& settings ) override + { + Sample::Step( settings ); + } + + static Sample* Create( Settings& settings ) + { + return new ModifyGeometry( settings ); + } + + b2ShapeId m_shapeId; + b2ShapeType m_shapeType; + float m_scale; + + union + { + b2Circle m_circle; + b2Capsule m_capsule; + b2Segment m_segment; + b2Polygon m_polygon; + }; +}; + +static int sampleModifyGeometry = RegisterSample( "Shapes", "Modify Geometry", ModifyGeometry::Create ); + +// Shows how to link to chain shapes together. This is a useful technique for building large game levels with smooth collision. +class ChainLink : public Sample +{ +public: + explicit ChainLink( Settings& settings ) + : Sample( settings ) + { + if ( settings.restart == false ) + { + g_camera.m_center = { 0.0f, 5.0f }; + g_camera.m_zoom = 25.0f * 0.5f; + } + + b2Vec2 points1[] = { { 40.0f, 1.0f }, { 0.0f, 0.0f }, { -40.0f, 0.0f }, + { -40.0f, -1.0f }, { 0.0f, -1.0f }, { 40.0f, -1.0f } }; + b2Vec2 points2[] = { { -40.0f, -1.0f }, { 0.0f, -1.0f }, { 40.0f, -1.0f }, + { 40.0f, 0.0f }, { 0.0f, 0.0f }, { -40.0f, 0.0f } }; + + int count1 = std::size( points1 ); + int count2 = std::size( points2 ); + + b2BodyDef bodyDef = b2DefaultBodyDef(); + b2BodyId groundId = b2CreateBody( m_worldId, &bodyDef ); + + { + b2ChainDef chainDef = b2DefaultChainDef(); + chainDef.points = points1; + chainDef.count = count1; + chainDef.isLoop = false; + b2CreateChain( groundId, &chainDef ); + } + + { + b2ChainDef chainDef = b2DefaultChainDef(); + chainDef.points = points2; + chainDef.count = count2; + chainDef.isLoop = false; + b2CreateChain( groundId, &chainDef ); + } + + bodyDef.type = b2_dynamicBody; + b2ShapeDef shapeDef = b2DefaultShapeDef(); + + { + bodyDef.position = { -5.0f, 2.0f }; + b2BodyId bodyId = b2CreateBody( m_worldId, &bodyDef ); + b2Circle circle = { { 0.0f, 0.0f }, 0.5f }; + b2CreateCircleShape( bodyId, &shapeDef, &circle ); + } + + { + bodyDef.position = { 0.0f, 2.0f }; + b2BodyId bodyId = b2CreateBody( m_worldId, &bodyDef ); + b2Capsule capsule = { { -0.5f, 0.0f }, { 0.5f, 0.0 }, 0.25f }; + b2CreateCapsuleShape( bodyId, &shapeDef, &capsule ); + } + + { + bodyDef.position = { 5.0f, 2.0f }; + b2BodyId bodyId = b2CreateBody( m_worldId, &bodyDef ); + float h = 0.5f; + b2Polygon box = b2MakeBox( h, h ); + b2CreatePolygonShape( bodyId, &shapeDef, &box ); + } + } + + void Step( Settings& settings ) override + { + Sample::Step( settings ); + + g_draw.DrawString( 5, m_textLine, "This shows how to link together two chain shapes" ); + m_textLine += m_textIncrement; + } + + static Sample* Create( Settings& settings ) + { + return new ChainLink( settings ); + } +}; + +static int sampleChainLink = RegisterSample( "Shapes", "Chain Link", ChainLink::Create ); + +class RoundedShapes : public Sample +{ +public: + explicit RoundedShapes( Settings& settings ) + : Sample( settings ) + { + if ( settings.restart == false ) + { + g_camera.m_zoom = 25.0f * 0.55f; + g_camera.m_center = { 2.0f, 8.0f }; + } + + { + b2BodyDef bodyDef = b2DefaultBodyDef(); + b2BodyId groundId = b2CreateBody( m_worldId, &bodyDef ); + + b2ShapeDef shapeDef = b2DefaultShapeDef(); + b2Polygon box = b2MakeOffsetBox( 20.0f, 1.0f, { 0.0f, -1.0f }, b2Rot_identity ); + b2CreatePolygonShape( groundId, &shapeDef, &box ); + + box = b2MakeOffsetBox( 1.0f, 5.0f, { 19.0f, 5.0f }, b2Rot_identity ); + b2CreatePolygonShape( groundId, &shapeDef, &box ); + + box = b2MakeOffsetBox( 1.0f, 5.0f, { -19.0f, 5.0f }, b2Rot_identity ); + b2CreatePolygonShape( groundId, &shapeDef, &box ); + } + + // b2Capsule capsule = {{-0.25f, 0.0f}, {0.25f, 0.0f}, 0.25f}; + // b2Circle circle = {{0.0f, 0.0f}, 0.35f}; + // b2Polygon square = b2MakeSquare(0.35f); + + // b2Vec2 points[3] = {{-0.1f, -0.5f}, {0.1f, -0.5f}, {0.0f, 0.5f}}; + // b2Hull wedgeHull = b2ComputeHull(points, 3); + // b2Polygon wedge = b2MakePolygon(&wedgeHull, 0.0f); + + b2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.type = b2_dynamicBody; + b2ShapeDef shapeDef = b2DefaultShapeDef(); + + float y = 2.0f; + int xcount = 10, ycount = 10; + + for ( int i = 0; i < ycount; ++i ) + { + float x = -5.0f; + for ( int j = 0; j < xcount; ++j ) + { + bodyDef.position = { x, y }; + b2BodyId bodyId = b2CreateBody( m_worldId, &bodyDef ); + + b2Polygon poly = RandomPolygon( 0.5f ); + poly.radius = RandomFloat( 0.05f, 0.25f ); + b2CreatePolygonShape( bodyId, &shapeDef, &poly ); + + x += 1.0f; + } + + y += 1.0f; + } + } + + static Sample* Create( Settings& settings ) + { + return new RoundedShapes( settings ); + } +}; + +static int sampleRoundedShapes = RegisterSample( "Shapes", "Rounded", RoundedShapes::Create ); + +class OffsetShapes : public Sample +{ +public: + explicit OffsetShapes( Settings& settings ) + : Sample( settings ) + { + if ( settings.restart == false ) + { + g_camera.m_zoom = 25.0f * 0.55f; + g_camera.m_center = { 2.0f, 8.0f }; + } + + { + b2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.position = { -1.0f, 1.0f }; + b2BodyId groundId = b2CreateBody( m_worldId, &bodyDef ); + + b2ShapeDef shapeDef = b2DefaultShapeDef(); + b2Polygon box = b2MakeOffsetBox( 1.0f, 1.0f, { 10.0f, -2.0f }, b2MakeRot(0.5f * b2_pi) ); + b2CreatePolygonShape( groundId, &shapeDef, &box ); + } + + { + b2Capsule capsule = { { -5.0f, 1.0f }, { -4.0f, 1.0f }, 0.25f }; + b2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.position = { 13.5f, -0.75f }; + bodyDef.type = b2_dynamicBody; + b2BodyId bodyId = b2CreateBody( m_worldId, &bodyDef ); + b2ShapeDef shapeDef = b2DefaultShapeDef(); + b2CreateCapsuleShape( bodyId, &shapeDef, &capsule ); + } + + { + b2Polygon box = b2MakeOffsetBox( 0.75f, 0.5f, { 9.0f, 2.0f }, b2MakeRot(0.5f * b2_pi) ); + b2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.position = { 0.0f, 0.0f }; + bodyDef.type = b2_dynamicBody; + b2BodyId bodyId = b2CreateBody( m_worldId, &bodyDef ); + b2ShapeDef shapeDef = b2DefaultShapeDef(); + b2CreatePolygonShape( bodyId, &shapeDef, &box ); + } + } + + void Step( Settings& settings ) override + { + Sample::Step( settings ); + + g_draw.DrawTransform( b2Transform_identity ); + } + + static Sample* Create( Settings& settings ) + { + return new OffsetShapes( settings ); + } +}; + +static int sampleOffsetShapes = RegisterSample( "Shapes", "Offset", OffsetShapes::Create ); diff --git a/tests/cpp-tests/Source/Box2DTestBed/samples/sample_stacking.cpp b/tests/cpp-tests/Source/Box2DTestBed/samples/sample_stacking.cpp new file mode 100644 index 000000000000..b0e39edda39a --- /dev/null +++ b/tests/cpp-tests/Source/Box2DTestBed/samples/sample_stacking.cpp @@ -0,0 +1,889 @@ +// SPDX-FileCopyrightText: 2022 Erin Catto +// SPDX-License-Identifier: MIT + +#include "draw.h" +#include "sample.h" +#include "settings.h" + +#include "box2d/box2d.h" +#include "box2d/math_functions.h" + +#include +#include + +class SingleBox : public Sample +{ +public: + explicit SingleBox( Settings& settings ) + : Sample( settings ) + { + if ( settings.restart == false ) + { + g_camera.m_center = { 0.0f, 2.5f }; + g_camera.m_zoom = 3.5f; + } + + float extent = 1.0f; + + b2BodyDef bodyDef = b2DefaultBodyDef(); + b2BodyId groundId = b2CreateBody( m_worldId, &bodyDef ); + + float groundWidth = 66.0f * extent; + b2ShapeDef shapeDef = b2DefaultShapeDef(); + shapeDef.friction = 0.5f; + + b2Segment segment = { { -0.5f * 2.0f * groundWidth, 0.0f }, { 0.5f * 2.0f * groundWidth, 0.0f } }; + b2CreateSegmentShape( groundId, &shapeDef, &segment ); + bodyDef.type = b2_dynamicBody; + + b2Polygon box = b2MakeBox( extent, extent ); + bodyDef.position = { 0.0f, 4.0f }; + b2BodyId bodyId = b2CreateBody( m_worldId, &bodyDef ); + b2CreatePolygonShape( bodyId, &shapeDef, &box ); + } + + void Step( Settings& settings ) override + { + Sample::Step( settings ); + + // g_draw.DrawCircle({0.0f, 2.0f}, 1.0f, b2_colorWhite); + } + + static Sample* Create( Settings& settings ) + { + return new SingleBox( settings ); + } +}; + +static int sampleSingleBox = RegisterSample( "Stacking", "Single Box", SingleBox::Create ); + +class TiltedStack : public Sample +{ +public: + enum + { + e_columns = 10, + e_rows = 10, + }; + + explicit TiltedStack( Settings& settings ) + : Sample( settings ) + { + if ( settings.restart == false ) + { + g_camera.m_center = { 7.5f, 7.5f }; + g_camera.m_zoom = 20.0f; + } + + { + b2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.position = { 0.0f, -1.0f }; + b2BodyId groundId = b2CreateBody( m_worldId, &bodyDef ); + + b2Polygon box = b2MakeBox( 1000.0f, 1.0f ); + b2ShapeDef shapeDef = b2DefaultShapeDef(); + b2CreatePolygonShape( groundId, &shapeDef, &box ); + } + + for ( int i = 0; i < e_rows * e_columns; ++i ) + { + m_bodies[i] = b2_nullBodyId; + } + + b2Polygon box = b2MakeRoundedBox( 0.45f, 0.45f, 0.05f ); + + b2ShapeDef shapeDef = b2DefaultShapeDef(); + shapeDef.density = 1.0f; + shapeDef.friction = 0.3f; + + float offset = 0.2f; + float dx = 5.0f; + float xroot = -0.5f * dx * ( e_columns - 1.0f ); + + for ( int j = 0; j < e_columns; ++j ) + { + float x = xroot + j * dx; + + for ( int i = 0; i < e_rows; ++i ) + { + b2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.type = b2_dynamicBody; + + int n = j * e_rows + i; + + bodyDef.position = { x + offset * i, 0.5f + 1.0f * i }; + b2BodyId bodyId = b2CreateBody( m_worldId, &bodyDef ); + + m_bodies[n] = bodyId; + + b2CreatePolygonShape( bodyId, &shapeDef, &box ); + } + } + } + + static Sample* Create( Settings& settings ) + { + return new TiltedStack( settings ); + } + + b2BodyId m_bodies[e_rows * e_columns]; +}; + +static int sampleTiltedStack = RegisterSample( "Stacking", "Tilted Stack", TiltedStack::Create ); + +class VerticalStack : public Sample +{ +public: + enum + { + e_maxColumns = 10, + e_maxRows = 12, + e_maxBullets = 8 + }; + + enum ShapeType + { + e_circleShape = 0, + e_boxShape + }; + + explicit VerticalStack( Settings& settings ) + : Sample( settings ) + { + if ( settings.restart == false ) + { + g_camera.m_center = { -7.0f, 9.0f }; + g_camera.m_zoom = 14.0f; + } + + { + b2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.position = { 0.0f, -1.0f }; + b2BodyId groundId = b2CreateBody( m_worldId, &bodyDef ); + + b2Polygon box = b2MakeBox( 100.0f, 1.0f ); + b2ShapeDef shapeDef = b2DefaultShapeDef(); + b2CreatePolygonShape( groundId, &shapeDef, &box ); + + b2Segment segment = { { 10.0f, 1.0f }, { 10.0f, 21.0f } }; + b2CreateSegmentShape( groundId, &shapeDef, &segment ); + } + + for ( int i = 0; i < e_maxRows * e_maxColumns; ++i ) + { + m_bodies[i] = b2_nullBodyId; + } + + for ( int i = 0; i < e_maxBullets; ++i ) + { + m_bullets[i] = b2_nullBodyId; + } + + m_shapeType = e_boxShape; + m_rowCount = e_maxRows; + m_columnCount = 5; + m_bulletCount = 1; + m_bulletType = e_circleShape; + + CreateStacks(); + } + + void CreateStacks() + { + for ( int i = 0; i < e_maxRows * e_maxColumns; ++i ) + { + if ( B2_IS_NON_NULL( m_bodies[i] ) ) + { + b2DestroyBody( m_bodies[i] ); + m_bodies[i] = b2_nullBodyId; + } + } + + b2Circle circle = { 0 }; + circle.radius = 0.5f; + + b2Polygon box = b2MakeBox( 0.5f, 0.5f ); + // b2Polygon box = b2MakeRoundedBox(0.45f, 0.45f, 0.05f); + + b2ShapeDef shapeDef = b2DefaultShapeDef(); + shapeDef.density = 1.0f; + shapeDef.friction = 0.3f; + + float offset; + + if ( m_shapeType == e_circleShape ) + { + offset = 0.0f; + } + else + { + offset = 0.01f; + } + + float dx = -3.0f; + float xroot = 8.0f; + + for ( int j = 0; j < m_columnCount; ++j ) + { + float x = xroot + j * dx; + + for ( int i = 0; i < m_rowCount; ++i ) + { + b2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.type = b2_dynamicBody; + + int n = j * m_rowCount + i; + + float shift = ( i % 2 == 0 ? -offset : offset ); + bodyDef.position = { x + shift, 0.5f + 1.0f * i }; + // bodyDef.position = {x + shift, 1.0f + 1.51f * i}; + b2BodyId bodyId = b2CreateBody( m_worldId, &bodyDef ); + + m_bodies[n] = bodyId; + + if ( m_shapeType == e_circleShape ) + { + b2CreateCircleShape( bodyId, &shapeDef, &circle ); + } + else + { + b2CreatePolygonShape( bodyId, &shapeDef, &box ); + } + } + } + } + + void DestroyBody() + { + for ( int j = 0; j < m_columnCount; ++j ) + { + for ( int i = 0; i < m_rowCount; ++i ) + { + int n = j * m_rowCount + i; + + if ( B2_IS_NON_NULL( m_bodies[n] ) ) + { + b2DestroyBody( m_bodies[n] ); + m_bodies[n] = b2_nullBodyId; + break; + } + } + } + } + + void DestroyBullets() + { + for ( int i = 0; i < e_maxBullets; ++i ) + { + b2BodyId bullet = m_bullets[i]; + + if ( B2_IS_NON_NULL( bullet ) ) + { + b2DestroyBody( bullet ); + m_bullets[i] = b2_nullBodyId; + } + } + } + + void FireBullets() + { + b2Circle circle = { { 0.0f, 0.0f }, 0.25f }; + b2Polygon box = b2MakeBox( 0.25f, 0.25f ); + + b2ShapeDef shapeDef = b2DefaultShapeDef(); + shapeDef.density = 4.0f; + + for ( int i = 0; i < m_bulletCount; ++i ) + { + b2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.type = b2_dynamicBody; + bodyDef.position = { -25.0f - i, 6.0f }; + float speed = RandomFloat( 200.0f, 300.0f ); + bodyDef.linearVelocity = { speed, 0.0f }; + bodyDef.isBullet = true; + + b2BodyId bullet = b2CreateBody( m_worldId, &bodyDef ); + + if ( m_bulletType == e_boxShape ) + { + b2CreatePolygonShape( bullet, &shapeDef, &box ); + } + else + { + b2CreateCircleShape( bullet, &shapeDef, &circle ); + } + assert( B2_IS_NULL( m_bullets[i] ) ); + m_bullets[i] = bullet; + } + } + + void UpdateUI() override + { + float height = 230.0f; + ImGui::SetNextWindowPos( ImVec2( 10.0f, g_camera.m_height - height - 50.0f ), ImGuiCond_Once ); + ImGui::SetNextWindowSize( ImVec2( 240.0f, height ) ); + + ImGui::Begin( "Vertical Stack", nullptr, ImGuiWindowFlags_NoResize ); + + ImGui::PushItemWidth( 120.0f ); + + bool changed = false; + const char* shapeTypes[] = { "Circle", "Box" }; + + int shapeType = int( m_shapeType ); + changed = changed || ImGui::Combo( "Shape", &shapeType, shapeTypes, IM_ARRAYSIZE( shapeTypes ) ); + m_shapeType = ShapeType( shapeType ); + + changed = changed || ImGui::SliderInt( "Rows", &m_rowCount, 1, e_maxRows ); + changed = changed || ImGui::SliderInt( "Columns", &m_columnCount, 1, e_maxColumns ); + + ImGui::SliderInt( "Bullets", &m_bulletCount, 1, e_maxBullets ); + + int bulletType = int( m_bulletType ); + ImGui::Combo( "Bullet Shape", &bulletType, shapeTypes, IM_ARRAYSIZE( shapeTypes ) ); + m_bulletType = ShapeType( bulletType ); + + ImGui::PopItemWidth(); + + if ( ImGui::Button( "Fire Bullets" ) || glfwGetKey( g_mainWindow, GLFW_KEY_B ) == GLFW_PRESS ) + { + DestroyBullets(); + FireBullets(); + } + + if ( ImGui::Button( "Destroy Body" ) ) + { + DestroyBody(); + } + + changed = changed || ImGui::Button( "Reset Stack" ); + + if ( changed ) + { + DestroyBullets(); + CreateStacks(); + } + + ImGui::End(); + } + + static Sample* Create( Settings& settings ) + { + return new VerticalStack( settings ); + } + + b2BodyId m_bullets[e_maxBullets]; + b2BodyId m_bodies[e_maxRows * e_maxColumns]; + int m_columnCount; + int m_rowCount; + int m_bulletCount; + ShapeType m_shapeType; + ShapeType m_bulletType; +}; + +static int sampleVerticalStack = RegisterSample( "Stacking", "Vertical Stack", VerticalStack::Create ); + +// This shows how to handle high gravity and small shapes using a small time step +class CircleStack : public Sample +{ +public: + explicit CircleStack( Settings& settings ) + : Sample( settings ) + { + if ( settings.restart == false ) + { + g_camera.m_center = { 0.0f, 5.0f }; + g_camera.m_zoom = 6.0f; + } + + { + b2BodyDef bodyDef = b2DefaultBodyDef(); + b2BodyId groundId = b2CreateBody( m_worldId, &bodyDef ); + + b2ShapeDef shapeDef = b2DefaultShapeDef(); + b2Segment segment = { { -10.0f, 0.0f }, { 10.0f, 0.0f } }; + b2CreateSegmentShape( groundId, &shapeDef, &segment ); + } + + b2World_SetGravity( m_worldId, { 0.0f, -20.0f } ); + b2World_SetContactTuning( m_worldId, 0.25f * 360.0f, 10.0f, 3.0f ); + + b2Circle circle = {}; + circle.radius = 0.5f; + + b2ShapeDef shapeDef = b2DefaultShapeDef(); + b2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.type = b2_dynamicBody; + + float y = 0.5f; + + for ( int i = 0; i < 8; ++i ) + { + bodyDef.position.y = y; + + b2BodyId bodyId = b2CreateBody( m_worldId, &bodyDef ); + b2CreateCircleShape( bodyId, &shapeDef, &circle ); + + y += 1.0f; + } + } + + static Sample* Create( Settings& settings ) + { + return new CircleStack( settings ); + } +}; + +static int sampleCircleStack = RegisterSample( "Stacking", "Circle Stack", CircleStack::Create ); + +class Cliff : public Sample +{ +public: + explicit Cliff( Settings& settings ) + : Sample( settings ) + { + if ( settings.restart == false ) + { + g_camera.m_zoom = 25.0f * 0.5f; + g_camera.m_center = { 0.0f, 5.0f }; + } + + { + b2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.position = { 0.0f, 0.0f }; + b2BodyId groundId = b2CreateBody( m_worldId, &bodyDef ); + + b2ShapeDef shapeDef = b2DefaultShapeDef(); + b2Polygon box = b2MakeOffsetBox( 100.0f, 1.0f, { 0.0f, -1.0f }, b2Rot_identity ); + b2CreatePolygonShape( groundId, &shapeDef, &box ); + + b2Segment segment = { { -14.0f, 4.0f }, { -8.0f, 4.0f } }; + b2CreateSegmentShape( groundId, &shapeDef, &segment ); + + box = b2MakeOffsetBox( 3.0f, 0.5f, { 0.0f, 4.0f }, b2Rot_identity ); + b2CreatePolygonShape( groundId, &shapeDef, &box ); + + b2Capsule capsule = { { 8.5f, 4.0f }, { 13.5f, 4.0f }, 0.5f }; + b2CreateCapsuleShape( groundId, &shapeDef, &capsule ); + } + + m_flip = false; + + for ( int i = 0; i < 9; ++i ) + { + m_bodyIds[i] = b2_nullBodyId; + } + + CreateBodies(); + } + + void CreateBodies() + { + for ( int i = 0; i < 9; ++i ) + { + if ( B2_IS_NON_NULL( m_bodyIds[i] ) ) + { + b2DestroyBody( m_bodyIds[i] ); + m_bodyIds[i] = b2_nullBodyId; + } + } + + float sign = m_flip ? -1.0f : 1.0f; + + b2Capsule capsule = { { -0.25f, 0.0f }, { 0.25f, 0.0f }, 0.25f }; + b2Circle circle = { { 0.0f, 0.0f }, 0.5f }; + b2Polygon square = b2MakeSquare( 0.5f ); + + b2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.type = b2_dynamicBody; + + { + b2ShapeDef shapeDef = b2DefaultShapeDef(); + shapeDef.friction = 0.01f; + bodyDef.linearVelocity = { 2.0f * sign, 0.0f }; + + float offset = m_flip ? -4.0f : 0.0f; + + bodyDef.position = { -9.0f + offset, 4.25f }; + m_bodyIds[0] = b2CreateBody( m_worldId, &bodyDef ); + b2CreateCapsuleShape( m_bodyIds[0], &shapeDef, &capsule ); + + bodyDef.position = { 2.0f + offset, 4.75f }; + m_bodyIds[1] = b2CreateBody( m_worldId, &bodyDef ); + b2CreateCapsuleShape( m_bodyIds[1], &shapeDef, &capsule ); + + bodyDef.position = { 13.0f + offset, 4.75f }; + m_bodyIds[2] = b2CreateBody( m_worldId, &bodyDef ); + b2CreateCapsuleShape( m_bodyIds[2], &shapeDef, &capsule ); + } + + { + b2ShapeDef shapeDef = b2DefaultShapeDef(); + shapeDef.friction = 0.01f; + bodyDef.linearVelocity = { 2.5f * sign, 0.0f }; + + bodyDef.position = { -11.0f, 4.5f }; + m_bodyIds[3] = b2CreateBody( m_worldId, &bodyDef ); + b2CreatePolygonShape( m_bodyIds[3], &shapeDef, &square ); + + bodyDef.position = { 0.0f, 5.0f }; + m_bodyIds[4] = b2CreateBody( m_worldId, &bodyDef ); + b2CreatePolygonShape( m_bodyIds[4], &shapeDef, &square ); + + bodyDef.position = { 11.0f, 5.0f }; + m_bodyIds[5] = b2CreateBody( m_worldId, &bodyDef ); + b2CreatePolygonShape( m_bodyIds[5], &shapeDef, &square ); + } + + { + b2ShapeDef shapeDef = b2DefaultShapeDef(); + shapeDef.friction = 0.2f; + bodyDef.linearVelocity = { 1.5f * sign, 0.0f }; + + float offset = m_flip ? 4.0f : 0.0f; + + bodyDef.position = { -13.0f + offset, 4.5f }; + m_bodyIds[6] = b2CreateBody( m_worldId, &bodyDef ); + b2CreateCircleShape( m_bodyIds[6], &shapeDef, &circle ); + + bodyDef.position = { -2.0f + offset, 5.0f }; + m_bodyIds[7] = b2CreateBody( m_worldId, &bodyDef ); + b2CreateCircleShape( m_bodyIds[7], &shapeDef, &circle ); + + bodyDef.position = { 9.0f + offset, 5.0f }; + m_bodyIds[8] = b2CreateBody( m_worldId, &bodyDef ); + b2CreateCircleShape( m_bodyIds[8], &shapeDef, &circle ); + } + } + + void UpdateUI() override + { + float height = 60.0f; + ImGui::SetNextWindowPos( ImVec2( 10.0f, g_camera.m_height - height - 50.0f ), ImGuiCond_Once ); + ImGui::SetNextWindowSize( ImVec2( 160.0f, height ) ); + + ImGui::Begin( "Cliff", nullptr, ImGuiWindowFlags_NoResize ); + + if ( ImGui::Button( "Flip" ) ) + { + m_flip = !m_flip; + CreateBodies(); + } + + ImGui::End(); + } + + static Sample* Create( Settings& settings ) + { + return new Cliff( settings ); + } + + b2BodyId m_bodyIds[9]; + bool m_flip; +}; + +static int sampleCliff = RegisterSample( "Stacking", "Cliff", Cliff::Create ); + +class Arch : public Sample +{ +public: + explicit Arch( Settings& settings ) + : Sample( settings ) + { + if ( settings.restart == false ) + { + g_camera.m_center = { 0.0f, 8.0f }; + g_camera.m_zoom = 25.0f * 0.35f; + } + + b2Vec2 ps1[9] = { { 16.0f, 0.0f }, + { 14.93803712795643f, 5.133601056842984f }, + { 13.79871746027416f, 10.24928069555078f }, + { 12.56252963284711f, 15.34107019122473f }, + { 11.20040987372525f, 20.39856541571217f }, + { 9.66521217819836f, 25.40369899225096f }, + { 7.87179930638133f, 30.3179337000085f }, + { 5.635199558196225f, 35.03820717801641f }, + { 2.405937953536585f, 39.09554102558315f } }; + + b2Vec2 ps2[9] = { { 24.0f, 0.0f }, + { 22.33619528222415f, 6.02299846205841f }, + { 20.54936888969905f, 12.00964361211476f }, + { 18.60854610798073f, 17.9470321677465f }, + { 16.46769273811807f, 23.81367936585418f }, + { 14.05325025774858f, 29.57079353071012f }, + { 11.23551045834022f, 35.13775818285372f }, + { 7.752568160730571f, 40.30450679009583f }, + { 3.016931552701656f, 44.28891593799322f } }; + + float scale = 0.25f; + for ( int i = 0; i < 9; ++i ) + { + ps1[i] = b2MulSV( scale, ps1[i] ); + ps2[i] = b2MulSV( scale, ps2[i] ); + } + + b2ShapeDef shapeDef = b2DefaultShapeDef(); + shapeDef.friction = 0.6f; + + { + b2BodyDef bodyDef = b2DefaultBodyDef(); + b2BodyId groundId = b2CreateBody( m_worldId, &bodyDef ); + b2Segment segment = { { -100.0f, 0.0f }, { 100.0f, 0.0f } }; + b2CreateSegmentShape( groundId, &shapeDef, &segment ); + } + + b2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.type = b2_dynamicBody; + + for ( int i = 0; i < 8; ++i ) + { + b2BodyId bodyId = b2CreateBody( m_worldId, &bodyDef ); + b2Vec2 ps[4] = { ps1[i], ps2[i], ps2[i + 1], ps1[i + 1] }; + b2Hull hull = b2ComputeHull( ps, 4 ); + b2Polygon polygon = b2MakePolygon( &hull, 0.0f ); + b2CreatePolygonShape( bodyId, &shapeDef, &polygon ); + } + + for ( int i = 0; i < 8; ++i ) + { + b2BodyId bodyId = b2CreateBody( m_worldId, &bodyDef ); + b2Vec2 ps[4] = { { -ps2[i].x, ps2[i].y }, + { -ps1[i].x, ps1[i].y }, + { -ps1[i + 1].x, ps1[i + 1].y }, + { -ps2[i + 1].x, ps2[i + 1].y } }; + b2Hull hull = b2ComputeHull( ps, 4 ); + b2Polygon polygon = b2MakePolygon( &hull, 0.0f ); + b2CreatePolygonShape( bodyId, &shapeDef, &polygon ); + } + + { + b2BodyId bodyId = b2CreateBody( m_worldId, &bodyDef ); + b2Vec2 ps[4] = { ps1[8], ps2[8], { -ps2[8].x, ps2[8].y }, { -ps1[8].x, ps1[8].y } }; + b2Hull hull = b2ComputeHull( ps, 4 ); + b2Polygon polygon = b2MakePolygon( &hull, 0.0f ); + b2CreatePolygonShape( bodyId, &shapeDef, &polygon ); + } + + for ( int i = 0; i < 4; ++i ) + { + b2Polygon box = b2MakeBox( 2.0f, 0.5f ); + bodyDef.position = { 0.0f, 0.5f + ps2[8].y + 1.0f * i }; + b2BodyId bodyId = b2CreateBody( m_worldId, &bodyDef ); + b2CreatePolygonShape( bodyId, &shapeDef, &box ); + } + } + + static Sample* Create( Settings& settings ) + { + return new Arch( settings ); + } +}; + +static int sampleArch = RegisterSample( "Stacking", "Arch", Arch::Create ); + +class DoubleDomino : public Sample +{ +public: + explicit DoubleDomino( Settings& settings ) + : Sample( settings ) + { + if ( settings.restart == false ) + { + g_camera.m_center = { 0.0f, 4.0f }; + g_camera.m_zoom = 25.0f * 0.25f; + } + + { + b2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.position = { 0.0f, -1.0f }; + b2BodyId groundId = b2CreateBody( m_worldId, &bodyDef ); + + b2Polygon box = b2MakeBox( 100.0f, 1.0f ); + b2ShapeDef shapeDef = b2DefaultShapeDef(); + b2CreatePolygonShape( groundId, &shapeDef, &box ); + } + + b2Polygon box = b2MakeBox( 0.125f, 0.5f ); + + b2ShapeDef shapeDef = b2DefaultShapeDef(); + shapeDef.friction = 0.6f; + b2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.type = b2_dynamicBody; + + int count = 15; + float x = -0.5f * count; + for ( int i = 0; i < count; ++i ) + { + bodyDef.position = { x, 0.5f }; + b2BodyId bodyId = b2CreateBody( m_worldId, &bodyDef ); + b2CreatePolygonShape( bodyId, &shapeDef, &box ); + if ( i == 0 ) + { + b2Body_ApplyLinearImpulse( bodyId, b2Vec2{ 0.2f, 0.0f }, b2Vec2{ x, 1.0f }, true ); + } + + x += 1.0f; + } + } + + static Sample* Create( Settings& settings ) + { + return new DoubleDomino( settings ); + } +}; + +static int sampleDoubleDomino = RegisterSample( "Stacking", "Double Domino", DoubleDomino::Create ); + +class Confined : public Sample +{ +public: + enum + { + e_gridCount = 25, + e_maxCount = e_gridCount * e_gridCount + }; + + explicit Confined( Settings& settings ) + : Sample( settings ) + { + if ( settings.restart == false ) + { + g_camera.m_center = { 0.0f, 10.0f }; + g_camera.m_zoom = 25.0f * 0.5f; + } + + { + b2BodyDef bodyDef = b2DefaultBodyDef(); + b2BodyId groundId = b2CreateBody( m_worldId, &bodyDef ); + + b2ShapeDef shapeDef = b2DefaultShapeDef(); + b2Capsule capsule; + capsule = { { -10.5f, 0.0f }, { 10.5f, 0.0f }, 0.5f }; + b2CreateCapsuleShape( groundId, &shapeDef, &capsule ); + capsule = { { -10.5f, 0.0f }, { -10.5f, 20.5f }, 0.5f }; + b2CreateCapsuleShape( groundId, &shapeDef, &capsule ); + capsule = { { 10.5f, 0.0f }, { 10.5f, 20.5f }, 0.5f }; + b2CreateCapsuleShape( groundId, &shapeDef, &capsule ); + capsule = { { -10.5f, 20.5f }, { 10.5f, 20.5f }, 0.5f }; + b2CreateCapsuleShape( groundId, &shapeDef, &capsule ); + } + + m_row = 0; + m_column = 0; + m_count = 0; + + b2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.type = b2_dynamicBody; + bodyDef.gravityScale = 0.0f; + + b2ShapeDef shapeDef = b2DefaultShapeDef(); + b2Circle circle = { { 0.0f, 0.0f }, 0.5f }; + + while ( m_count < e_maxCount ) + { + m_row = 0; + for ( int i = 0; i < e_gridCount; ++i ) + { + float x = -8.75f + m_column * 18.0f / e_gridCount; + float y = 1.5f + m_row * 18.0f / e_gridCount; + + bodyDef.position = { x, y }; + b2BodyId bodyId = b2CreateBody( m_worldId, &bodyDef ); + b2CreateCircleShape( bodyId, &shapeDef, &circle ); + + m_count += 1; + m_row += 1; + } + m_column += 1; + } + } + + static Sample* Create( Settings& settings ) + { + return new Confined( settings ); + } + + int m_row; + int m_column; + int m_count; +}; + +static int sampleConfined = RegisterSample( "Stacking", "Confined", Confined::Create ); + +// From PEEL +class CardHouse : public Sample +{ +public: + explicit CardHouse( Settings& settings ) + : Sample( settings ) + { + if ( settings.restart == false ) + { + g_camera.m_center = { 0.75f, 0.9f }; + g_camera.m_zoom = 25.0f * 0.05f; + } + + b2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.position = { 0.0f, -2.0f }; + b2BodyId groundId = b2CreateBody( m_worldId, &bodyDef ); + + b2ShapeDef shapeDef = b2DefaultShapeDef(); + shapeDef.friction = 0.7f; + + b2Polygon groundBox = b2MakeBox( 40.0f, 2.0f ); + b2CreatePolygonShape( groundId, &shapeDef, &groundBox ); + + float cardHeight = 0.2f; + float cardThickness = 0.001f; + + float angle0 = 25.0f * b2_pi / 180.0f; + float angle1 = -25.0f * b2_pi / 180.0f; + float angle2 = 0.5f * b2_pi; + + b2Polygon cardBox = b2MakeBox( cardThickness, cardHeight ); + bodyDef.type = b2_dynamicBody; + + int Nb = 5; + float z0 = 0.0f; + float y = cardHeight - 0.02f; + while ( Nb ) + { + float z = z0; + for ( int i = 0; i < Nb; i++ ) + { + if ( i != Nb - 1 ) + { + bodyDef.position = { z + 0.25f, y + cardHeight - 0.015f }; + bodyDef.rotation = b2MakeRot( angle2 ); + b2BodyId bodyId = b2CreateBody( m_worldId, &bodyDef ); + b2CreatePolygonShape( bodyId, &shapeDef, &cardBox ); + } + + bodyDef.position = { z, y }; + bodyDef.rotation = b2MakeRot( angle1 ); + b2BodyId bodyId = b2CreateBody( m_worldId, &bodyDef ); + b2CreatePolygonShape( bodyId, &shapeDef, &cardBox ); + + z += 0.175f; + + bodyDef.position = { z, y }; + bodyDef.rotation = b2MakeRot( angle0 ); + bodyId = b2CreateBody( m_worldId, &bodyDef ); + b2CreatePolygonShape( bodyId, &shapeDef, &cardBox ); + + z += 0.175f; + } + y += cardHeight * 2.0f - 0.03f; + z0 += 0.175f; + Nb--; + } + } + + static Sample* Create( Settings& settings ) + { + return new CardHouse( settings ); + } +}; + +static int sampleCardHouse = RegisterSample( "Stacking", "Card House", CardHouse::Create ); diff --git a/tests/cpp-tests/Source/Box2DTestBed/samples/sample_world.cpp b/tests/cpp-tests/Source/Box2DTestBed/samples/sample_world.cpp new file mode 100644 index 000000000000..1e5ad87be943 --- /dev/null +++ b/tests/cpp-tests/Source/Box2DTestBed/samples/sample_world.cpp @@ -0,0 +1,242 @@ +// SPDX-FileCopyrightText: 2022 Erin Catto +// SPDX-License-Identifier: MIT + +#include "car.h" +#include "donut.h" +#include "draw.h" +#include "human.h" +#include "sample.h" +#include "settings.h" + +#include "box2d/box2d.h" +#include "box2d/math_functions.h" + +#include +#include + +class LargeWorld : public Sample +{ +public: + explicit LargeWorld( Settings& settings ) + : Sample( settings ) + { + m_period = 40.0f; + float omega = 2.0 * b2_pi / m_period; + m_cycleCount = g_sampleDebug ? 10 : 600; + m_gridSize = 1.0f; + m_gridCount = (int)( m_cycleCount * m_period / m_gridSize ); + + float xStart = -0.5f * ( m_cycleCount * m_period ); + + m_viewPosition = { xStart, 15.0f }; + + if ( settings.restart == false ) + { + g_camera.m_center = m_viewPosition; + g_camera.m_zoom = 25.0f * 1.0f; + settings.drawJoints = false; + settings.useCameraBounds = true; + } + + { + b2BodyDef bodyDef = b2DefaultBodyDef(); + b2ShapeDef shapeDef = b2DefaultShapeDef(); + + // Setting this to false significantly reduces the cost of creating + // static bodies and shapes. + shapeDef.forceContactCreation = false; + + float height = 4.0f; + float xBody = xStart; + float xShape = xStart; + + b2BodyId groundId; + + for ( int i = 0; i < m_gridCount; ++i ) + { + // Create a new body regularly so that shapes are not too far from the body origin. + // Most algorithms in Box2D work in local coordinates, but contact points are computed + // relative to the body origin. + // This makes a noticeable improvement in stability far from the origin. + if ( i % 10 == 0 ) + { + bodyDef.position.x = xBody; + groundId = b2CreateBody( m_worldId, &bodyDef ); + xShape = 0.0f; + } + + float y = 0.0f; + + int ycount = (int)roundf( height * cosf( omega * xBody ) ) + 12; + + for ( int j = 0; j < ycount; ++j ) + { + b2Polygon square = b2MakeOffsetBox( 0.4f * m_gridSize, 0.4f * m_gridSize, { xShape, y }, b2Rot_identity ); + square.radius = 0.1f; + b2CreatePolygonShape( groundId, &shapeDef, &square ); + + y += m_gridSize; + } + + xBody += m_gridSize; + xShape += m_gridSize; + } + } + + int humanIndex = 0; + int donutIndex = 0; + for ( int cycleIndex = 0; cycleIndex < m_cycleCount; ++cycleIndex ) + { + float xbase = ( 0.5f + cycleIndex ) * m_period + xStart; + + int remainder = cycleIndex % 3; + if ( remainder == 0 ) + { + b2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.type = b2_dynamicBody; + bodyDef.position = { xbase - 3.0f, 10.0f }; + + b2ShapeDef shapeDef = b2DefaultShapeDef(); + b2Polygon box = b2MakeBox( 0.3f, 0.2f ); + + for ( int i = 0; i < 10; ++i ) + { + bodyDef.position.y = 10.0f; + for ( int j = 0; j < 5; ++j ) + { + b2BodyId bodyId = b2CreateBody( m_worldId, &bodyDef ); + b2CreatePolygonShape( bodyId, &shapeDef, &box ); + bodyDef.position.y += 0.5f; + } + bodyDef.position.x += 0.6f; + } + } + else if ( remainder == 1 ) + { + b2Vec2 position = { xbase - 2.0f, 10.0f }; + for ( int i = 0; i < 5; ++i ) + { + Human human; + human.Spawn( m_worldId, position, 1.5f, 0.05f, 0.0f, 0.0f, humanIndex + 1, NULL, false ); + humanIndex += 1; + position.x += 1.0f; + } + } + else + { + b2Vec2 position = { xbase - 4.0f, 12.0f }; + + for ( int i = 0; i < 5; ++i ) + { + Donut donut; + donut.Spawn( m_worldId, position, 0.75f, 0, NULL ); + donutIndex += 1; + position.x += 2.0f; + } + } + } + + m_car.Spawn( m_worldId, { xStart + 20.0f, 40.0f }, 10.0f, 2.0f, 0.7f, 2000.0f, nullptr ); + + m_cycleIndex = 0; + m_speed = 0.0f; + m_explosionPosition = { ( 0.5f + m_cycleIndex ) * m_period + xStart, 7.0f }; + m_explode = true; + m_followCar = false; + } + + void UpdateUI() override + { + float height = 160.0f; + ImGui::SetNextWindowPos( ImVec2( 10.0f, g_camera.m_height - height - 50.0f ), ImGuiCond_Once ); + ImGui::SetNextWindowSize( ImVec2( 240.0f, height ) ); + + ImGui::Begin( "Large World", nullptr, ImGuiWindowFlags_NoResize ); + + ImGui::SliderFloat( "speed", &m_speed, -400.0f, 400.0f, "%.0f" ); + if ( ImGui::Button( "stop" ) ) + { + m_speed = 0.0f; + } + + ImGui::Checkbox( "explode", &m_explode ); + ImGui::Checkbox( "follow car", &m_followCar ); + + ImGui::Text( "world size = %g kilometers", m_gridSize * m_gridCount / 1000.0f ); + ImGui::End(); + } + + void Step( Settings& settings ) override + { + float span = 0.5f * ( m_period * m_cycleCount ); + float timeStep = settings.hertz > 0.0f ? 1.0f / settings.hertz : 0.0f; + + if ( settings.pause ) + { + timeStep = 0.0f; + } + + m_viewPosition.x += timeStep * m_speed; + m_viewPosition.x = b2ClampFloat( m_viewPosition.x, -span, span ); + + if ( m_speed != 0.0f ) + { + g_camera.m_center = m_viewPosition; + } + + if ( m_followCar ) + { + g_camera.m_center.x = b2Body_GetPosition( m_car.m_chassisId ).x; + } + + float radius = 2.0f; + if ( ( m_stepCount & 0x1 ) == 0x1 && m_explode ) + { + m_explosionPosition.x = ( 0.5f + m_cycleIndex ) * m_period - span; + b2World_Explode( m_worldId, m_explosionPosition, radius, 1.0f ); + m_cycleIndex = ( m_cycleIndex + 1 ) % m_cycleCount; + } + + if ( m_explode ) + { + g_draw.DrawCircle( m_explosionPosition, radius, b2_colorAzure ); + } + + if ( glfwGetKey( g_mainWindow, GLFW_KEY_A ) == GLFW_PRESS ) + { + m_car.SetSpeed( 20.0f ); + } + + if ( glfwGetKey( g_mainWindow, GLFW_KEY_S ) == GLFW_PRESS ) + { + m_car.SetSpeed( 0.0f ); + } + + if ( glfwGetKey( g_mainWindow, GLFW_KEY_D ) == GLFW_PRESS ) + { + m_car.SetSpeed( -5.0f ); + } + + Sample::Step( settings ); + } + + static Sample* Create( Settings& settings ) + { + return new LargeWorld( settings ); + } + + Car m_car; + b2Vec2 m_viewPosition; + float m_period; + int m_cycleCount; + int m_cycleIndex; + float m_gridCount; + float m_gridSize; + float m_speed; + + b2Vec2 m_explosionPosition; + bool m_explode; + bool m_followCar; +}; + +static int sampleLargeWorld = RegisterSample( "World", "Large World", LargeWorld::Create ); diff --git a/tests/cpp-tests/Source/Box2DTestBed/samples/settings.cpp b/tests/cpp-tests/Source/Box2DTestBed/samples/settings.cpp new file mode 100644 index 000000000000..76edc758c168 --- /dev/null +++ b/tests/cpp-tests/Source/Box2DTestBed/samples/settings.cpp @@ -0,0 +1,136 @@ +// SPDX-FileCopyrightText: 2023 Erin Catto +// SPDX-License-Identifier: MIT + +#define _CRT_SECURE_NO_WARNINGS +#include "settings.h" + +// todo consider using https://github.com/skeeto/pdjson +#include +#include "jsmn/jsmn.h" +#include +#include +#include + +static const char* fileName = "settings.ini"; + +// Load a file. You must free the character array. +static bool ReadFile( char*& data, int& size, const char* filename ) +{ + FILE* file = fopen( filename, "rb" ); + if ( file == nullptr ) + { + return false; + } + + fseek( file, 0, SEEK_END ); + size = (int)ftell( file ); + fseek( file, 0, SEEK_SET ); + + if ( size == 0 ) + { + return false; + } + + data = (char*)malloc( size + 1 ); + size_t count = fread( data, size, 1, file ); + fclose( file ); + data[size] = 0; + + return true; +} + +void Settings::Save() +{ + FILE* file = fopen( fileName, "w" ); + fprintf( file, "{\n" ); + fprintf( file, " \"sampleIndex\": %d,\n", sampleIndex ); + fprintf( file, " \"drawShapes\": %s,\n", drawShapes ? "true" : "false" ); + fprintf( file, " \"drawJoints\": %s,\n", drawJoints ? "true" : "false" ); + fprintf( file, " \"drawAABBs\": %s,\n", drawAABBs ? "true" : "false" ); + fprintf( file, " \"drawContactPoints\": %s,\n", drawContactPoints ? "true" : "false" ); + fprintf( file, " \"drawContactNormals\": %s,\n", drawContactNormals ? "true" : "false" ); + fprintf( file, " \"drawContactImpulses\": %s,\n", drawContactImpulses ? "true" : "false" ); + fprintf( file, " \"drawFrictionImpulse\": %s,\n", drawFrictionImpulses ? "true" : "false" ); + fprintf( file, " \"drawMass\": %s,\n", drawMass ? "true" : "false" ); + fprintf( file, " \"drawCounters\": %s,\n", drawCounters ? "true" : "false" ); + fprintf( file, " \"drawProfile\": %s,\n", drawProfile ? "true" : "false" ); + fprintf( file, " \"enableWarmStarting\": %s,\n", enableWarmStarting ? "true" : "false" ); + fprintf( file, " \"enableContinuous\": %s,\n", enableContinuous ? "true" : "false" ); + fprintf( file, " \"enableSleep\": %s\n", enableSleep ? "true" : "false" ); + fprintf( file, "}\n" ); + fclose( file ); +} + +static int jsoneq( const char* json, jsmntok_t* tok, const char* s ) +{ + if ( tok->type == JSMN_STRING && (int)strlen( s ) == tok->end - tok->start && + strncmp( json + tok->start, s, tok->end - tok->start ) == 0 ) + { + return 0; + } + return -1; +} + +#define MAX_TOKENS 32 + +void Settings::Load() +{ + char* data = nullptr; + int size = 0; + bool found = ReadFile( data, size, fileName ); + if ( found == false ) + { + return; + } + + jsmn_parser parser; + jsmntok_t tokens[MAX_TOKENS]; + + jsmn_init( &parser ); + + // js - pointer to JSON string + // tokens - an array of tokens available + // 10 - number of tokens available + int tokenCount = jsmn_parse( &parser, data, size, tokens, MAX_TOKENS ); + char buffer[32]; + + for ( int i = 0; i < tokenCount; ++i ) + { + if ( jsoneq( data, &tokens[i], "sampleIndex" ) == 0 ) + { + int count = tokens[i + 1].end - tokens[i + 1].start; + assert( count < 32 ); + const char* s = data + tokens[i + 1].start; + strncpy( buffer, s, count ); + buffer[count] = 0; + char* dummy; + sampleIndex = (int)strtol( buffer, &dummy, 10 ); + } + else if ( jsoneq( data, &tokens[i], "drawShapes" ) == 0 ) + { + const char* s = data + tokens[i + 1].start; + if ( strncmp( s, "true", 4 ) == 0 ) + { + drawShapes = true; + } + else if ( strncmp( s, "false", 5 ) == 0 ) + { + drawShapes = false; + } + } + else if ( jsoneq( data, &tokens[i], "drawJoints" ) == 0 ) + { + const char* s = data + tokens[i + 1].start; + if ( strncmp( s, "true", 4 ) == 0 ) + { + drawJoints = true; + } + else if ( strncmp( s, "false", 5 ) == 0 ) + { + drawJoints = false; + } + } + } + + free( data ); +} diff --git a/tests/cpp-tests/Source/Box2DTestBed/samples/settings.h b/tests/cpp-tests/Source/Box2DTestBed/samples/settings.h new file mode 100644 index 000000000000..04a16a47b4af --- /dev/null +++ b/tests/cpp-tests/Source/Box2DTestBed/samples/settings.h @@ -0,0 +1,43 @@ +// SPDX-FileCopyrightText: 2023 Erin Catto +// SPDX-License-Identifier: MIT + +#pragma once + +// todo add camera and draw and remove globals +struct Settings +{ + void Save(); + void Load(); + + int sampleIndex = 0; + // int windowWidth = 3840; + // int windowHeight = 2160; + int windowWidth = 1920; + int windowHeight = 1080; + // int windowWidth = 1280; + // int windowHeight = 720; + // int windowWidth = 800; + // int windowHeight = 600; + float hertz = 60.0f; + int subStepCount = 4; + int workerCount = 1; + bool useCameraBounds = false; + bool drawShapes = true; + bool drawJoints = true; + bool drawJointExtras = false; + bool drawAABBs = false; + bool drawContactPoints = false; + bool drawContactNormals = false; + bool drawContactImpulses = false; + bool drawFrictionImpulses = false; + bool drawMass = false; + bool drawGraphColors = false; + bool drawCounters = false; + bool drawProfile = false; + bool enableWarmStarting = true; + bool enableContinuous = true; + bool enableSleep = true; + bool pause = false; + bool singleStep = false; + bool restart = false; +}; diff --git a/tests/cpp-tests/Source/Box2DTestBed/test.cpp b/tests/cpp-tests/Source/Box2DTestBed/test.cpp deleted file mode 100644 index d5c26853a097..000000000000 --- a/tests/cpp-tests/Source/Box2DTestBed/test.cpp +++ /dev/null @@ -1,514 +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 "tests/test.h" -#include "tests/settings.h" - -#include "extensions/axmol-ext.h" -#include "axmol.h" - -#include - -using namespace ax; -USING_NS_AX_EXT; - -#if defined(AX_PLATFORM_PC) -extern ax::Label* labelDebugDraw; -#endif - -void DestructionListener::SayGoodbye(b2Joint* joint) -{ - if (test->m_mouseJoint == joint) - { - test->m_mouseJoint = NULL; - } - else - { - test->JointDestroyed(joint); - } -} - -Test::Test() -{ - b2Vec2 gravity; - gravity.Set(0.0f, -10.0f); - m_world = new b2World(gravity); - m_bomb = NULL; - m_textLine = 30; - m_textIncrement = 13; - m_mouseJoint = NULL; - m_pointCount = 0; - - m_destructionListener.test = this; - m_world->SetDestructionListener(&m_destructionListener); - m_world->SetContactListener(this); - m_world->SetDebugDraw(&g_debugDraw); - - m_bombSpawning = false; - - m_stepCount = 0; - - b2BodyDef bodyDef; - m_groundBody = m_world->CreateBody(&bodyDef); - - memset(&m_maxProfile, 0, sizeof(b2Profile)); - memset(&m_totalProfile, 0, sizeof(b2Profile)); -} - -Test::~Test() -{ - // By deleting the world, we delete the bomb, mouse joint, etc. - delete m_world; - m_world = NULL; -} - -void Test::PreSolve(b2Contact* contact, const b2Manifold* oldManifold) -{ - const b2Manifold* manifold = contact->GetManifold(); - - if (manifold->pointCount == 0) - { - return; - } - - b2Fixture* fixtureA = contact->GetFixtureA(); - b2Fixture* fixtureB = contact->GetFixtureB(); - - b2PointState state1[b2_maxManifoldPoints], state2[b2_maxManifoldPoints]; - b2GetPointStates(state1, state2, oldManifold, manifold); - - b2WorldManifold worldManifold; - contact->GetWorldManifold(&worldManifold); - - for (int32 i = 0; i < manifold->pointCount && m_pointCount < k_maxContactPoints; ++i) - { - ContactPoint* cp = m_points + m_pointCount; - cp->fixtureA = fixtureA; - cp->fixtureB = fixtureB; - cp->position = worldManifold.points[i]; - cp->normal = worldManifold.normal; - cp->state = state2[i]; - cp->normalImpulse = manifold->points[i].normalImpulse; - cp->tangentImpulse = manifold->points[i].tangentImpulse; - cp->separation = worldManifold.separations[i]; - ++m_pointCount; - } -} - -void Test::DrawTitle(const char* string) -{ - DrawString(5, 5, string); - m_textLine = int32(26.0f); -} - -class QueryCallback : public b2QueryCallback -{ -public: - QueryCallback(const b2Vec2& point) - { - m_point = point; - m_fixture = NULL; - } - - bool ReportFixture(b2Fixture* fixture) override - { - b2Body* body = fixture->GetBody(); - if (body->GetType() == b2_dynamicBody) - { - bool inside = fixture->TestPoint(m_point); - if (inside) - { - m_fixture = fixture; - - // We are done, terminate the query. - return false; - } - } - - // Continue the query. - return true; - } - - b2Vec2 m_point; - b2Fixture* m_fixture; -}; - -bool Test::MouseDown(const b2Vec2& p) -{ - m_mouseWorld = p; - - if (m_mouseJoint != NULL) - { - return false; - } - - // Make a small box. - b2AABB aabb; - b2Vec2 d; - d.Set(0.001f, 0.001f); - aabb.lowerBound = p - d; - aabb.upperBound = p + d; - - // Query the world for overlapping shapes. - QueryCallback callback(p); - m_world->QueryAABB(&callback, aabb); - - if (callback.m_fixture) - { - float frequencyHz = 5.0f; - float dampingRatio = 0.7f; - - b2Body* body = callback.m_fixture->GetBody(); - b2MouseJointDef jd; - jd.bodyA = m_groundBody; - jd.bodyB = body; - jd.target = p; - jd.maxForce = 1000.0f * body->GetMass(); - b2LinearStiffness(jd.stiffness, jd.damping, frequencyHz, dampingRatio, jd.bodyA, jd.bodyB); - - m_mouseJoint = (b2MouseJoint*)m_world->CreateJoint(&jd); - body->SetAwake(true); - return true; - } - return false; -} - -void Test::SpawnBomb(const b2Vec2& worldPt) -{ - m_bombSpawnPoint = worldPt; - m_bombSpawning = true; -} - -void Test::CompleteBombSpawn(const b2Vec2& p) -{ - if (m_bombSpawning == false) - { - return; - } - - const float multiplier = 30.0f; - b2Vec2 vel = m_bombSpawnPoint - p; - vel *= multiplier; - LaunchBomb(m_bombSpawnPoint, vel); - m_bombSpawning = false; -} - -void Test::ShiftMouseDown(const b2Vec2& p) -{ - m_mouseWorld = p; - - if (m_mouseJoint != NULL) - { - return; - } - - SpawnBomb(p); -} - -void Test::MouseUp(const b2Vec2& p) -{ - if (m_mouseJoint) - { - m_world->DestroyJoint(m_mouseJoint); - m_mouseJoint = NULL; - } - - if (m_bombSpawning) - { - CompleteBombSpawn(p); - } -} - -void Test::MouseMove(const b2Vec2& p) -{ - m_mouseWorld = p; - - if (m_mouseJoint) - { - m_mouseJoint->SetTarget(p); - } -} - -void Test::LaunchBomb() -{ - b2Vec2 p(RandomFloat(-15.0f, 15.0f), 30.0f); - b2Vec2 v = -5.0f * p; - LaunchBomb(p, v); -} - -void Test::LaunchBomb(const b2Vec2& position, const b2Vec2& velocity) -{ - if (m_bomb) - { - m_world->DestroyBody(m_bomb); - m_bomb = NULL; - } - - b2BodyDef bd; - bd.type = b2_dynamicBody; - bd.position = position; - bd.bullet = true; - m_bomb = m_world->CreateBody(&bd); - m_bomb->SetLinearVelocity(velocity); - - b2CircleShape circle; - circle.m_radius = 0.3f; - - b2FixtureDef fd; - fd.shape = &circle; - fd.density = 20.0f; - fd.restitution = 0.0f; - - b2Vec2 minV = position - b2Vec2(0.3f, 0.3f); - b2Vec2 maxV = position + b2Vec2(0.3f, 0.3f); - - b2AABB aabb; - aabb.lowerBound = minV; - aabb.upperBound = maxV; - - m_bomb->CreateFixture(&fd); -} - -void Test::Step(Settings& settings) -{ - float timeStep = settings.m_hertz > 0.0f ? 1.0f / settings.m_hertz : float(0.0f); - - if (settings.m_pause) - { - if (settings.m_singleStep) - { - settings.m_singleStep = 0; - } - else - { - timeStep = 0.0f; - } - - DrawString(5, m_textLine, "****PAUSED****"); - } - - uint32 flags = 0; - flags += settings.m_drawShapes * b2Draw::e_shapeBit; - flags += settings.m_drawJoints * b2Draw::e_jointBit; - flags += settings.m_drawAABBs * b2Draw::e_aabbBit; - flags += settings.m_drawCOMs * b2Draw::e_centerOfMassBit; - g_debugDraw.SetFlags(flags); - - m_world->SetAllowSleeping(settings.m_enableSleep); - m_world->SetWarmStarting(settings.m_enableWarmStarting); - m_world->SetContinuousPhysics(settings.m_enableContinuous); - m_world->SetSubStepping(settings.m_enableSubStepping); - - m_pointCount = 0; - - m_world->Step(timeStep, settings.m_velocityIterations, settings.m_positionIterations); - - m_world->DebugDraw(); - - if (timeStep > 0.0f) - { - ++m_stepCount; - } - - if (settings.m_drawStats) - { - int32 bodyCount = m_world->GetBodyCount(); - int32 contactCount = m_world->GetContactCount(); - int32 jointCount = m_world->GetJointCount(); - DrawString(5, m_textLine, "bodies/contacts/joints = %d/%d/%d", bodyCount, contactCount, jointCount); - - int32 proxyCount = m_world->GetProxyCount(); - int32 height = m_world->GetTreeHeight(); - int32 balance = m_world->GetTreeBalance(); - float quality = m_world->GetTreeQuality(); - DrawString(5, m_textLine, "proxies/height/balance/quality = %d/%d/%d/%g", proxyCount, height, balance, quality); - } - - // Track maximum profile times - { - const b2Profile& p = m_world->GetProfile(); - m_maxProfile.step = b2Max(m_maxProfile.step, p.step); - m_maxProfile.collide = b2Max(m_maxProfile.collide, p.collide); - m_maxProfile.solve = b2Max(m_maxProfile.solve, p.solve); - m_maxProfile.solveInit = b2Max(m_maxProfile.solveInit, p.solveInit); - m_maxProfile.solveVelocity = b2Max(m_maxProfile.solveVelocity, p.solveVelocity); - m_maxProfile.solvePosition = b2Max(m_maxProfile.solvePosition, p.solvePosition); - m_maxProfile.solveTOI = b2Max(m_maxProfile.solveTOI, p.solveTOI); - m_maxProfile.broadphase = b2Max(m_maxProfile.broadphase, p.broadphase); - - m_totalProfile.step += p.step; - m_totalProfile.collide += p.collide; - m_totalProfile.solve += p.solve; - m_totalProfile.solveInit += p.solveInit; - m_totalProfile.solveVelocity += p.solveVelocity; - m_totalProfile.solvePosition += p.solvePosition; - m_totalProfile.solveTOI += p.solveTOI; - m_totalProfile.broadphase += p.broadphase; - } - - if (settings.m_drawProfile) - { - const b2Profile& p = m_world->GetProfile(); - - b2Profile aveProfile; - memset(&aveProfile, 0, sizeof(b2Profile)); - if (m_stepCount > 0) - { - float scale = 1.0f / m_stepCount; - aveProfile.step = scale * m_totalProfile.step; - aveProfile.collide = scale * m_totalProfile.collide; - aveProfile.solve = scale * m_totalProfile.solve; - aveProfile.solveInit = scale * m_totalProfile.solveInit; - aveProfile.solveVelocity = scale * m_totalProfile.solveVelocity; - aveProfile.solvePosition = scale * m_totalProfile.solvePosition; - aveProfile.solveTOI = scale * m_totalProfile.solveTOI; - aveProfile.broadphase = scale * m_totalProfile.broadphase; - } - - DrawString(5, m_textLine, "step [ave] (max) = %5.2f [%6.2f] (%6.2f)", p.step, aveProfile.step, - m_maxProfile.step); - DrawString(5, m_textLine, "collide [ave] (max) = %5.2f [%6.2f] (%6.2f)", p.collide, aveProfile.collide, - m_maxProfile.collide); - DrawString(5, m_textLine, "solve [ave] (max) = %5.2f [%6.2f] (%6.2f)", p.solve, aveProfile.solve, - m_maxProfile.solve); - DrawString(5, m_textLine, "solve init [ave] (max) = %5.2f [%6.2f] (%6.2f)", p.solveInit, aveProfile.solveInit, - m_maxProfile.solveInit); - DrawString(5, m_textLine, "solve velocity [ave] (max) = %5.2f [%6.2f] (%6.2f)", p.solveVelocity, - aveProfile.solveVelocity, m_maxProfile.solveVelocity); - DrawString(5, m_textLine, "solve position [ave] (max) = %5.2f [%6.2f] (%6.2f)", p.solvePosition, - aveProfile.solvePosition, m_maxProfile.solvePosition); - DrawString(5, m_textLine, "solveTOI [ave] (max) = %5.2f [%6.2f] (%6.2f)", p.solveTOI, aveProfile.solveTOI, - m_maxProfile.solveTOI); - DrawString(5, m_textLine, "broad-phase [ave] (max) = %5.2f [%6.2f] (%6.2f)", p.broadphase, - aveProfile.broadphase, m_maxProfile.broadphase); - } - - if (m_bombSpawning) - { - b2Color c; - c.Set(0.0f, 0.0f, 1.0f); - g_debugDraw.DrawPoint(m_bombSpawnPoint, 4.0f, c); - - c.Set(0.8f, 0.8f, 0.8f); - g_debugDraw.DrawSegment(m_mouseWorld, m_bombSpawnPoint, c); - } - - if (settings.m_drawContactPoints) - { - const float k_impulseScale = 0.1f; - const float k_axisScale = 0.3f; - - for (int32 i = 0; i < m_pointCount; ++i) - { - ContactPoint* point = m_points + i; - - if (point->state == b2_addState) - { - // Add - g_debugDraw.DrawPoint(point->position, 10.0f, b2Color(0.3f, 0.95f, 0.3f)); - } - else if (point->state == b2_persistState) - { - // Persist - g_debugDraw.DrawPoint(point->position, 5.0f, b2Color(0.3f, 0.3f, 0.95f)); - } - - if (settings.m_drawContactNormals == 1) - { - b2Vec2 p1 = point->position; - b2Vec2 p2 = p1 + k_axisScale * point->normal; - g_debugDraw.DrawSegment(p1, p2, b2Color(0.9f, 0.9f, 0.9f)); - } - else if (settings.m_drawContactImpulse == 1) - { - b2Vec2 p1 = point->position; - b2Vec2 p2 = p1 + k_impulseScale * point->normalImpulse * point->normal; - g_debugDraw.DrawSegment(p1, p2, b2Color(0.9f, 0.9f, 0.3f)); - } - - if (settings.m_drawFrictionImpulse == 1) - { - b2Vec2 tangent = b2Cross(point->normal, 1.0f); - b2Vec2 p1 = point->position; - b2Vec2 p2 = p1 + k_impulseScale * point->tangentImpulse * tangent; - g_debugDraw.DrawSegment(p1, p2, b2Color(0.9f, 0.9f, 0.3f)); - } - } - } -} - -void Test::ShiftOrigin(const b2Vec2& newOrigin) -{ - m_world->ShiftOrigin(newOrigin); -} - -void Test::initShader(void) -{ - // initShader is unsupported -} - -void Test::DrawString(int x, int y, const char* fmt, ...) -{ -#if defined(AX_PLATFORM_PC) - va_list args; - va_start(args, fmt); - auto str = StringUtils::vformat(fmt, args); - va_end(args); - debugString.append(str); - debugString.append("\n"); - labelDebugDraw->setString(debugString); - // labelDebugDraw->setPosition(x, y); -#endif -} - -void Test::DrawString(const b2Vec2& pw, const char* fmt, ...) -{ -#if defined(AX_PLATFORM_PC) - va_list args; - va_start(args, fmt); - auto str = StringUtils::vformat(fmt, args); - va_end(args); - debugString.append(str); - debugString.append("\n"); - labelDebugDraw->setString(debugString); - // labelDebugDraw->setPosition(pw.x, pw.y); -#endif -} - -void Test::DrawAABB(b2AABB* aabb, const b2Color& color) -{ - b2Vec2 p1 = aabb->lowerBound; - b2Vec2 p2 = b2Vec2(aabb->upperBound.x, aabb->lowerBound.y); - b2Vec2 p3 = aabb->upperBound; - b2Vec2 p4 = b2Vec2(aabb->lowerBound.x, aabb->upperBound.y); - - Vec2 verts[] = { - Vec2(p1.x * g_debugDraw.mRatio, p1.y * g_debugDraw.mRatio) + g_debugDraw.debugNodeOffset, - Vec2(p2.x * g_debugDraw.mRatio, p2.y * g_debugDraw.mRatio) + g_debugDraw.debugNodeOffset, - Vec2(p3.x * g_debugDraw.mRatio, p3.y * g_debugDraw.mRatio) + g_debugDraw.debugNodeOffset, - Vec2(p4.x * g_debugDraw.mRatio, p4.y * g_debugDraw.mRatio) + g_debugDraw.debugNodeOffset, - }; - debugDrawNode->drawPolygon(verts, sizeof(verts) / sizeof(verts[0]), - Color4F(color.r / 2, color.g / 2, color.b / 2, 0), 0.4f, - Color4F(color.r, color.g, color.b, color.a)); -} - -void Test::Flush() -{ - // Flush is unsupported -} diff --git a/tests/cpp-tests/Source/Box2DTestBed/tests/add_pair.cpp b/tests/cpp-tests/Source/Box2DTestBed/tests/add_pair.cpp deleted file mode 100644 index 4465d35f3e01..000000000000 --- a/tests/cpp-tests/Source/Box2DTestBed/tests/add_pair.cpp +++ /dev/null @@ -1,67 +0,0 @@ -// MIT License - -// Copyright (c) 2019 Erin Catto - -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: - -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. - -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -#include "test.h" - -class AddPair : public Test -{ -public: - AddPair() - { - m_world->SetGravity(b2Vec2(0.0f, 0.0f)); - { - b2CircleShape shape; - shape.m_p.SetZero(); - shape.m_radius = 0.1f; - - float minX = -6.0f; - float maxX = 0.0f; - float minY = 4.0f; - float maxY = 6.0f; - - for (int32 i = 0; i < 400; ++i) - { - b2BodyDef bd; - bd.type = b2_dynamicBody; - bd.position = b2Vec2(RandomFloat(minX, maxX), RandomFloat(minY, maxY)); - b2Body* body = m_world->CreateBody(&bd); - body->CreateFixture(&shape, 0.01f); - } - } - - { - b2PolygonShape shape; - shape.SetAsBox(1.5f, 1.5f); - b2BodyDef bd; - bd.type = b2_dynamicBody; - bd.position.Set(-40.0f, 5.0f); - bd.bullet = true; - b2Body* body = m_world->CreateBody(&bd); - body->CreateFixture(&shape, 1.0f); - body->SetLinearVelocity(b2Vec2(10.0f, 0.0f)); - } - } - - static Test* Create() { return new AddPair; } -}; - -static int testIndex = RegisterTest("Benchmark", "Add Pair", AddPair::Create); diff --git a/tests/cpp-tests/Source/Box2DTestBed/tests/apply_force.cpp b/tests/cpp-tests/Source/Box2DTestBed/tests/apply_force.cpp deleted file mode 100644 index 51239f60e2f3..000000000000 --- a/tests/cpp-tests/Source/Box2DTestBed/tests/apply_force.cpp +++ /dev/null @@ -1,199 +0,0 @@ -// MIT License - -// Copyright (c) 2019 Erin Catto - -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: - -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. - -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -#include "test.h" - -// This test shows how to apply forces and torques to a body. -// It also shows how to use the friction joint that can be useful -// for overhead games. -class ApplyForce : public Test -{ -public: - ApplyForce() - { - m_world->SetGravity(b2Vec2(0.0f, 0.0f)); - - const float k_restitution = 0.4f; - - b2Body* ground; - { - b2BodyDef bd; - bd.position.Set(0.0f, 20.0f); - ground = m_world->CreateBody(&bd); - - b2EdgeShape shape; - - b2FixtureDef sd; - sd.shape = &shape; - sd.density = 0.0f; - sd.restitution = k_restitution; - - // Left vertical - shape.SetTwoSided(b2Vec2(-20.0f, -20.0f), b2Vec2(-20.0f, 20.0f)); - ground->CreateFixture(&sd); - - // Right vertical - shape.SetTwoSided(b2Vec2(20.0f, -20.0f), b2Vec2(20.0f, 20.0f)); - ground->CreateFixture(&sd); - - // Top horizontal - shape.SetTwoSided(b2Vec2(-20.0f, 20.0f), b2Vec2(20.0f, 20.0f)); - ground->CreateFixture(&sd); - - // Bottom horizontal - shape.SetTwoSided(b2Vec2(-20.0f, -20.0f), b2Vec2(20.0f, -20.0f)); - ground->CreateFixture(&sd); - } - - { - b2Transform xf1; - xf1.q.Set(0.3524f * b2_pi); - xf1.p = xf1.q.GetXAxis(); - - b2Vec2 vertices[3]; - vertices[0] = b2Mul(xf1, b2Vec2(-1.0f, 0.0f)); - vertices[1] = b2Mul(xf1, b2Vec2(1.0f, 0.0f)); - vertices[2] = b2Mul(xf1, b2Vec2(0.0f, 0.5f)); - - b2PolygonShape poly1; - poly1.Set(vertices, 3); - - b2FixtureDef sd1; - sd1.shape = &poly1; - sd1.density = 2.0f; - - b2Transform xf2; - xf2.q.Set(-0.3524f * b2_pi); - xf2.p = -xf2.q.GetXAxis(); - - vertices[0] = b2Mul(xf2, b2Vec2(-1.0f, 0.0f)); - vertices[1] = b2Mul(xf2, b2Vec2(1.0f, 0.0f)); - vertices[2] = b2Mul(xf2, b2Vec2(0.0f, 0.5f)); - - b2PolygonShape poly2; - poly2.Set(vertices, 3); - - b2FixtureDef sd2; - sd2.shape = &poly2; - sd2.density = 2.0f; - - b2BodyDef bd; - bd.type = b2_dynamicBody; - - bd.position.Set(0.0f, 3.0); - bd.angle = b2_pi; - bd.allowSleep = false; - m_body = m_world->CreateBody(&bd); - m_body->CreateFixture(&sd1); - m_body->CreateFixture(&sd2); - - float gravity = 10.0f; - float I = m_body->GetInertia(); - float mass = m_body->GetMass(); - - // Compute an effective radius that can be used to - // set the max torque for a friction joint - // For a circle: I = 0.5 * m * r * r ==> r = sqrt(2 * I / m) - float radius = b2Sqrt(2.0f * I / mass); - - b2FrictionJointDef jd; - jd.bodyA = ground; - jd.bodyB = m_body; - jd.localAnchorA.SetZero(); - jd.localAnchorB = m_body->GetLocalCenter(); - jd.collideConnected = true; - jd.maxForce = 0.5f * mass * gravity; - jd.maxTorque = 0.2f * mass * radius * gravity; - - m_world->CreateJoint(&jd); - } - - { - b2PolygonShape shape; - shape.SetAsBox(0.5f, 0.5f); - - b2FixtureDef fd; - fd.shape = &shape; - fd.density = 1.0f; - fd.friction = 0.3f; - - for (int i = 0; i < 10; ++i) - { - b2BodyDef bd; - bd.type = b2_dynamicBody; - - bd.position.Set(0.0f, 7.0f + 1.54f * i); - b2Body* body = m_world->CreateBody(&bd); - - body->CreateFixture(&fd); - - float gravity = 10.0f; - float I = body->GetInertia(); - float mass = body->GetMass(); - - // For a circle: I = 0.5 * m * r * r ==> r = sqrt(2 * I / m) - float radius = b2Sqrt(2.0f * I / mass); - - b2FrictionJointDef jd; - jd.localAnchorA.SetZero(); - jd.localAnchorB.SetZero(); - jd.bodyA = ground; - jd.bodyB = body; - jd.collideConnected = true; - jd.maxForce = mass * gravity; - jd.maxTorque = 0.1f * mass * radius * gravity; - - m_world->CreateJoint(&jd); - } - } - } - - void Step(Settings& settings) override - { - DrawString(5, m_textLine, "Forward (W), Turn (A) and (D)"); - - // if (glfwGetKey(g_mainWindow, GLFW_KEY_W) == GLFW_PRESS) - //{ - // b2Vec2 f = m_body->GetWorldVector(b2Vec2(0.0f, -50.0f)); - // b2Vec2 p = m_body->GetWorldPoint(b2Vec2(0.0f, 3.0f)); - // m_body->ApplyForce(f, p, true); - // } - - // if (glfwGetKey(g_mainWindow, GLFW_KEY_A) == GLFW_PRESS) - //{ - // m_body->ApplyTorque(10.0f, true); - // } - - // if (glfwGetKey(g_mainWindow, GLFW_KEY_D) == GLFW_PRESS) - //{ - // m_body->ApplyTorque(-10.0f, true); - // } - - Test::Step(settings); - } - - static Test* Create() { return new ApplyForce; } - - b2Body* m_body; -}; - -static int testIndex = RegisterTest("Forces", "Apply Force", ApplyForce::Create); diff --git a/tests/cpp-tests/Source/Box2DTestBed/tests/body_types.cpp b/tests/cpp-tests/Source/Box2DTestBed/tests/body_types.cpp deleted file mode 100644 index 0de1079826f0..000000000000 --- a/tests/cpp-tests/Source/Box2DTestBed/tests/body_types.cpp +++ /dev/null @@ -1,158 +0,0 @@ -// MIT License - -// Copyright (c) 2019 Erin Catto - -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: - -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. - -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -#include "test.h" - -class BodyTypes : public Test -{ -public: - BodyTypes() - { - b2Body* ground = NULL; - { - b2BodyDef bd; - ground = m_world->CreateBody(&bd); - - b2EdgeShape shape; - shape.SetTwoSided(b2Vec2(-20.0f, 0.0f), b2Vec2(20.0f, 0.0f)); - - b2FixtureDef fd; - fd.shape = &shape; - - ground->CreateFixture(&fd); - } - - // Define attachment - { - b2BodyDef bd; - bd.type = b2_dynamicBody; - bd.position.Set(0.0f, 3.0f); - m_attachment = m_world->CreateBody(&bd); - - b2PolygonShape shape; - shape.SetAsBox(0.5f, 2.0f); - m_attachment->CreateFixture(&shape, 2.0f); - } - - // Define platform - { - b2BodyDef bd; - bd.type = b2_dynamicBody; - bd.position.Set(-4.0f, 5.0f); - m_platform = m_world->CreateBody(&bd); - - b2PolygonShape shape; - shape.SetAsBox(0.5f, 4.0f, b2Vec2(4.0f, 0.0f), 0.5f * b2_pi); - - b2FixtureDef fd; - fd.shape = &shape; - fd.friction = 0.6f; - fd.density = 2.0f; - m_platform->CreateFixture(&fd); - - b2RevoluteJointDef rjd; - rjd.Initialize(m_attachment, m_platform, b2Vec2(0.0f, 5.0f)); - rjd.maxMotorTorque = 50.0f; - rjd.enableMotor = true; - m_world->CreateJoint(&rjd); - - b2PrismaticJointDef pjd; - pjd.Initialize(ground, m_platform, b2Vec2(0.0f, 5.0f), b2Vec2(1.0f, 0.0f)); - - pjd.maxMotorForce = 1000.0f; - pjd.enableMotor = true; - pjd.lowerTranslation = -10.0f; - pjd.upperTranslation = 10.0f; - pjd.enableLimit = true; - - m_world->CreateJoint(&pjd); - - m_speed = 3.0f; - } - - // Create a payload - { - b2BodyDef bd; - bd.type = b2_dynamicBody; - bd.position.Set(0.0f, 8.0f); - b2Body* body = m_world->CreateBody(&bd); - - b2PolygonShape shape; - shape.SetAsBox(0.75f, 0.75f); - - b2FixtureDef fd; - fd.shape = &shape; - fd.friction = 0.6f; - fd.density = 2.0f; - - body->CreateFixture(&fd); - } - } - - void Keyboard(int key) override - { - switch (key) - { - case GLFW_KEY_D: - m_platform->SetType(b2_dynamicBody); - break; - - case GLFW_KEY_S: - m_platform->SetType(b2_staticBody); - break; - - case GLFW_KEY_K: - m_platform->SetType(b2_kinematicBody); - m_platform->SetLinearVelocity(b2Vec2(-m_speed, 0.0f)); - m_platform->SetAngularVelocity(0.0f); - break; - } - } - - void Step(Settings& settings) override - { - // Drive the kinematic body. - if (m_platform->GetType() == b2_kinematicBody) - { - b2Vec2 p = m_platform->GetTransform().p; - b2Vec2 v = m_platform->GetLinearVelocity(); - - if ((p.x < -10.0f && v.x < 0.0f) || (p.x > 10.0f && v.x > 0.0f)) - { - v.x = -v.x; - m_platform->SetLinearVelocity(v); - } - } - - Test::Step(settings); - - DrawString(5, m_textLine, "Keys: (d) dynamic, (s) static, (k) kinematic"); - } - - static Test* Create() { return new BodyTypes; } - - b2Body* m_attachment; - b2Body* m_platform; - float m_speed; -}; - -static int testIndex = RegisterTest("Examples", "Body Types", BodyTypes::Create); diff --git a/tests/cpp-tests/Source/Box2DTestBed/tests/box_stack.cpp b/tests/cpp-tests/Source/Box2DTestBed/tests/box_stack.cpp deleted file mode 100644 index 463d399b7f8e..000000000000 --- a/tests/cpp-tests/Source/Box2DTestBed/tests/box_stack.cpp +++ /dev/null @@ -1,170 +0,0 @@ -// MIT License - -// Copyright (c) 2019 Erin Catto - -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: - -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. - -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -#include "test.h" - -extern B2_API bool g_blockSolve; - -class BoxStack : public Test -{ -public: - enum - { - e_columnCount = 1, - e_rowCount = 15 - // e_columnCount = 1, - // e_rowCount = 1 - }; - - BoxStack() - { - { - b2BodyDef bd; - b2Body* ground = m_world->CreateBody(&bd); - - b2EdgeShape shape; - shape.SetTwoSided(b2Vec2(-40.0f, 0.0f), b2Vec2(40.0f, 0.0f)); - ground->CreateFixture(&shape, 0.0f); - - shape.SetTwoSided(b2Vec2(20.0f, 0.0f), b2Vec2(20.0f, 20.0f)); - ground->CreateFixture(&shape, 0.0f); - } - - float xs[5] = {0.0f, -10.0f, -5.0f, 5.0f, 10.0f}; - - for (int32 j = 0; j < e_columnCount; ++j) - { - b2PolygonShape shape; - shape.SetAsBox(0.5f, 0.5f); - - b2FixtureDef fd; - fd.shape = &shape; - fd.density = 1.0f; - fd.friction = 0.3f; - - for (int i = 0; i < e_rowCount; ++i) - { - b2BodyDef bd; - bd.type = b2_dynamicBody; - - int32 n = j * e_rowCount + i; - b2Assert(n < e_rowCount * e_columnCount); - m_indices[n] = n; - bd.userData.pointer = n; - - float x = 0.0f; - // float x = RandomFloat(-0.02f, 0.02f); - // float x = i % 2 == 0 ? -0.01f : 0.01f; - bd.position.Set(xs[j] + x, 0.55f + 1.1f * i); - b2Body* body = m_world->CreateBody(&bd); - - m_bodies[n] = body; - - body->CreateFixture(&fd); - } - } - - m_bullet = NULL; - } - - void Keyboard(int key) override - { - switch (key) - { - case GLFW_KEY_COMMA: - if (m_bullet != NULL) - { - m_world->DestroyBody(m_bullet); - m_bullet = NULL; - } - - { - b2CircleShape shape; - shape.m_radius = 0.25f; - - b2FixtureDef fd; - fd.shape = &shape; - fd.density = 20.0f; - fd.restitution = 0.05f; - - b2BodyDef bd; - bd.type = b2_dynamicBody; - bd.bullet = true; - bd.position.Set(-31.0f, 5.0f); - - m_bullet = m_world->CreateBody(&bd); - m_bullet->CreateFixture(&fd); - - m_bullet->SetLinearVelocity(b2Vec2(400.0f, 0.0f)); - } - break; - - case GLFW_KEY_B: - g_blockSolve = !g_blockSolve; - break; - } - } - - void Step(Settings& settings) override - { - Test::Step(settings); - DrawString(5, m_textLine, "Press: (,) to launch a bullet."); - - DrawString(5, m_textLine, "Blocksolve = %d", g_blockSolve); - if (m_stepCount == 300) - { - if (m_bullet != NULL) - { - m_world->DestroyBody(m_bullet); - m_bullet = NULL; - } - - { - b2CircleShape shape; - shape.m_radius = 0.25f; - - b2FixtureDef fd; - fd.shape = &shape; - fd.density = 20.0f; - fd.restitution = 0.05f; - - b2BodyDef bd; - bd.type = b2_dynamicBody; - bd.bullet = true; - bd.position.Set(-31.0f, 5.0f); - - m_bullet = m_world->CreateBody(&bd); - m_bullet->CreateFixture(&fd); - - m_bullet->SetLinearVelocity(b2Vec2(400.0f, 0.0f)); - } - } - } - - static Test* Create() { return new BoxStack; } - - b2Body* m_bullet; - b2Body* m_bodies[e_rowCount * e_columnCount]; - int32 m_indices[e_rowCount * e_columnCount]; -}; - -static int testIndex = RegisterTest("Stacking", "Boxes", BoxStack::Create); diff --git a/tests/cpp-tests/Source/Box2DTestBed/tests/breakable.cpp b/tests/cpp-tests/Source/Box2DTestBed/tests/breakable.cpp deleted file mode 100644 index 1128a56ef498..000000000000 --- a/tests/cpp-tests/Source/Box2DTestBed/tests/breakable.cpp +++ /dev/null @@ -1,154 +0,0 @@ -// MIT License - -// Copyright (c) 2019 Erin Catto - -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: - -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. - -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -#include "test.h" - -// This is used to test sensor shapes. -class Breakable : public Test -{ -public: - enum - { - e_count = 7 - }; - - Breakable() - { - // Ground body - { - b2BodyDef bd; - b2Body* ground = m_world->CreateBody(&bd); - - b2EdgeShape shape; - shape.SetTwoSided(b2Vec2(-40.0f, 0.0f), b2Vec2(40.0f, 0.0f)); - ground->CreateFixture(&shape, 0.0f); - } - - // Breakable dynamic body - { - b2BodyDef bd; - bd.type = b2_dynamicBody; - bd.position.Set(0.0f, 40.0f); - bd.angle = 0.25f * b2_pi; - m_body1 = m_world->CreateBody(&bd); - - m_shape1.SetAsBox(0.5f, 0.5f, b2Vec2(-0.5f, 0.0f), 0.0f); - m_piece1 = m_body1->CreateFixture(&m_shape1, 1.0f); - - m_shape2.SetAsBox(0.5f, 0.5f, b2Vec2(0.5f, 0.0f), 0.0f); - m_piece2 = m_body1->CreateFixture(&m_shape2, 1.0f); - } - - m_break = false; - m_broke = false; - } - - void PostSolve(b2Contact* contact, const b2ContactImpulse* impulse) override - { - if (m_broke) - { - // The body already broke. - return; - } - - // Should the body break? - int32 count = contact->GetManifold()->pointCount; - - float maxImpulse = 0.0f; - for (int32 i = 0; i < count; ++i) - { - maxImpulse = b2Max(maxImpulse, impulse->normalImpulses[i]); - } - - if (maxImpulse > 40.0f) - { - // Flag the body for breaking. - m_break = true; - } - } - - void Break() - { - // Create two bodies from one. - b2Body* body1 = m_piece1->GetBody(); - b2Vec2 center = body1->GetWorldCenter(); - - body1->DestroyFixture(m_piece2); - m_piece2 = NULL; - - b2BodyDef bd; - bd.type = b2_dynamicBody; - bd.position = body1->GetPosition(); - bd.angle = body1->GetAngle(); - - b2Body* body2 = m_world->CreateBody(&bd); - m_piece2 = body2->CreateFixture(&m_shape2, 1.0f); - - // Compute consistent velocities for new bodies based on - // cached velocity. - b2Vec2 center1 = body1->GetWorldCenter(); - b2Vec2 center2 = body2->GetWorldCenter(); - - b2Vec2 velocity1 = m_velocity + b2Cross(m_angularVelocity, center1 - center); - b2Vec2 velocity2 = m_velocity + b2Cross(m_angularVelocity, center2 - center); - - body1->SetAngularVelocity(m_angularVelocity); - body1->SetLinearVelocity(velocity1); - - body2->SetAngularVelocity(m_angularVelocity); - body2->SetLinearVelocity(velocity2); - } - - void Step(Settings& settings) override - { - if (m_break) - { - Break(); - m_broke = true; - m_break = false; - } - - // Cache velocities to improve movement on breakage. - if (m_broke == false) - { - m_velocity = m_body1->GetLinearVelocity(); - m_angularVelocity = m_body1->GetAngularVelocity(); - } - - Test::Step(settings); - } - - static Test* Create() { return new Breakable; } - - b2Body* m_body1; - b2Vec2 m_velocity; - float m_angularVelocity; - b2PolygonShape m_shape1; - b2PolygonShape m_shape2; - b2Fixture* m_piece1; - b2Fixture* m_piece2; - - bool m_broke; - bool m_break; -}; - -static int testIndex = RegisterTest("Examples", "Breakable", Breakable::Create); diff --git a/tests/cpp-tests/Source/Box2DTestBed/tests/bridge.cpp b/tests/cpp-tests/Source/Box2DTestBed/tests/bridge.cpp deleted file mode 100644 index 8447bd43dd30..000000000000 --- a/tests/cpp-tests/Source/Box2DTestBed/tests/bridge.cpp +++ /dev/null @@ -1,124 +0,0 @@ -// MIT License - -// Copyright (c) 2019 Erin Catto - -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: - -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. - -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -#include "test.h" - -class Bridge : public Test -{ -public: - enum - { - e_count = 30 - }; - - Bridge() - { - b2Body* ground = NULL; - { - b2BodyDef bd; - ground = m_world->CreateBody(&bd); - - b2EdgeShape shape; - shape.SetTwoSided(b2Vec2(-40.0f, 0.0f), b2Vec2(40.0f, 0.0f)); - ground->CreateFixture(&shape, 0.0f); - } - - { - b2PolygonShape shape; - shape.SetAsBox(0.5f, 0.125f); - - b2FixtureDef fd; - fd.shape = &shape; - fd.density = 20.0f; - fd.friction = 0.2f; - - b2RevoluteJointDef jd; - - b2Body* prevBody = ground; - for (int32 i = 0; i < e_count; ++i) - { - b2BodyDef bd; - bd.type = b2_dynamicBody; - bd.position.Set(-14.5f + 1.0f * i, 5.0f); - b2Body* body = m_world->CreateBody(&bd); - body->CreateFixture(&fd); - - b2Vec2 anchor(-15.0f + 1.0f * i, 5.0f); - jd.Initialize(prevBody, body, anchor); - m_world->CreateJoint(&jd); - - if (i == (e_count >> 1)) - { - m_middle = body; - } - prevBody = body; - } - - b2Vec2 anchor(-15.0f + 1.0f * static_cast(e_count), 5.0f); - jd.Initialize(prevBody, ground, anchor); - m_world->CreateJoint(&jd); - } - - for (int32 i = 0; i < 2; ++i) - { - b2Vec2 vertices[3]; - vertices[0].Set(-0.5f, 0.0f); - vertices[1].Set(0.5f, 0.0f); - vertices[2].Set(0.0f, 1.5f); - - b2PolygonShape shape; - shape.Set(vertices, 3); - - b2FixtureDef fd; - fd.shape = &shape; - fd.density = 1.0f; - - b2BodyDef bd; - bd.type = b2_dynamicBody; - bd.position.Set(-8.0f + 8.0f * i, 12.0f); - b2Body* body = m_world->CreateBody(&bd); - body->CreateFixture(&fd); - } - - for (int32 i = 0; i < 3; ++i) - { - b2CircleShape shape; - shape.m_radius = 0.5f; - - b2FixtureDef fd; - fd.shape = &shape; - fd.density = 1.0f; - - b2BodyDef bd; - bd.type = b2_dynamicBody; - bd.position.Set(-6.0f + 6.0f * i, 10.0f); - b2Body* body = m_world->CreateBody(&bd); - body->CreateFixture(&fd); - } - } - - static Test* Create() { return new Bridge; } - - b2Body* m_middle; -}; - -static int testIndex = RegisterTest("Joints", "Bridge", Bridge::Create); diff --git a/tests/cpp-tests/Source/Box2DTestBed/tests/bullet_test.cpp b/tests/cpp-tests/Source/Box2DTestBed/tests/bullet_test.cpp deleted file mode 100644 index 945887da6f9c..000000000000 --- a/tests/cpp-tests/Source/Box2DTestBed/tests/bullet_test.cpp +++ /dev/null @@ -1,132 +0,0 @@ -// MIT License - -// Copyright (c) 2019 Erin Catto - -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: - -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. - -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -#include "test.h" - -class BulletTest : public Test -{ -public: - BulletTest() - { - { - b2BodyDef bd; - bd.position.Set(0.0f, 0.0f); - b2Body* body = m_world->CreateBody(&bd); - - b2EdgeShape edge; - - edge.SetTwoSided(b2Vec2(-10.0f, 0.0f), b2Vec2(10.0f, 0.0f)); - body->CreateFixture(&edge, 0.0f); - - b2PolygonShape shape; - shape.SetAsBox(0.2f, 1.0f, b2Vec2(0.5f, 1.0f), 0.0f); - body->CreateFixture(&shape, 0.0f); - } - - { - b2BodyDef bd; - bd.type = b2_dynamicBody; - bd.position.Set(0.0f, 4.0f); - - b2PolygonShape box; - box.SetAsBox(2.0f, 0.1f); - - m_body = m_world->CreateBody(&bd); - m_body->CreateFixture(&box, 1.0f); - - box.SetAsBox(0.25f, 0.25f); - - // m_x = RandomFloat(-1.0f, 1.0f); - m_x = 0.20352793f; - bd.position.Set(m_x, 10.0f); - bd.bullet = true; - - m_bullet = m_world->CreateBody(&bd); - m_bullet->CreateFixture(&box, 100.0f); - - m_bullet->SetLinearVelocity(b2Vec2(0.0f, -50.0f)); - } - } - - void Launch() - { - m_body->SetTransform(b2Vec2(0.0f, 4.0f), 0.0f); - m_body->SetLinearVelocity(b2Vec2_zero); - m_body->SetAngularVelocity(0.0f); - - m_x = RandomFloat(-1.0f, 1.0f); - m_bullet->SetTransform(b2Vec2(m_x, 10.0f), 0.0f); - m_bullet->SetLinearVelocity(b2Vec2(0.0f, -50.0f)); - m_bullet->SetAngularVelocity(0.0f); - - extern B2_API int32 b2_gjkCalls, b2_gjkIters, b2_gjkMaxIters; - extern B2_API int32 b2_toiCalls, b2_toiIters, b2_toiMaxIters; - extern B2_API int32 b2_toiRootIters, b2_toiMaxRootIters; - - b2_gjkCalls = 0; - b2_gjkIters = 0; - b2_gjkMaxIters = 0; - - b2_toiCalls = 0; - b2_toiIters = 0; - b2_toiMaxIters = 0; - b2_toiRootIters = 0; - b2_toiMaxRootIters = 0; - } - - void Step(Settings& settings) override - { - Test::Step(settings); - - extern B2_API int32 b2_gjkCalls, b2_gjkIters, b2_gjkMaxIters; - extern B2_API int32 b2_toiCalls, b2_toiIters; - extern B2_API int32 b2_toiRootIters, b2_toiMaxRootIters; - - if (b2_gjkCalls > 0) - { - DrawString(5, m_textLine, "gjk calls = %d, ave gjk iters = %3.1f, max gjk iters = %d", b2_gjkCalls, - b2_gjkIters / float(b2_gjkCalls), b2_gjkMaxIters); - } - - if (b2_toiCalls > 0) - { - DrawString(5, m_textLine, "toi calls = %d, ave toi iters = %3.1f, max toi iters = %d", b2_toiCalls, - b2_toiIters / float(b2_toiCalls), b2_toiMaxRootIters); - - DrawString(5, m_textLine, "ave toi root iters = %3.1f, max toi root iters = %d", - b2_toiRootIters / float(b2_toiCalls), b2_toiMaxRootIters); - } - - if (m_stepCount % 60 == 0) - { - Launch(); - } - } - - static Test* Create() { return new BulletTest; } - - b2Body* m_body; - b2Body* m_bullet; - float m_x; -}; - -static int testIndex = RegisterTest("Continuous", "Bullet Test", BulletTest::Create); diff --git a/tests/cpp-tests/Source/Box2DTestBed/tests/cantilever.cpp b/tests/cpp-tests/Source/Box2DTestBed/tests/cantilever.cpp deleted file mode 100644 index 8a9ffce0b7cd..000000000000 --- a/tests/cpp-tests/Source/Box2DTestBed/tests/cantilever.cpp +++ /dev/null @@ -1,214 +0,0 @@ -// MIT License - -// Copyright (c) 2019 Erin Catto - -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: - -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. - -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -#include "test.h" - -// It is difficult to make a cantilever made of links completely rigid with weld joints. -// You will have to use a high number of iterations to make them stiff. -// So why not go ahead and use soft weld joints? They behave like a revolute -// joint with a rotational spring. -class Cantilever : public Test -{ -public: - enum - { - e_count = 8 - }; - - Cantilever() - { - b2Body* ground = NULL; - { - b2BodyDef bd; - ground = m_world->CreateBody(&bd); - - b2EdgeShape shape; - shape.SetTwoSided(b2Vec2(-40.0f, 0.0f), b2Vec2(40.0f, 0.0f)); - ground->CreateFixture(&shape, 0.0f); - } - - { - b2PolygonShape shape; - shape.SetAsBox(0.5f, 0.125f); - - b2FixtureDef fd; - fd.shape = &shape; - fd.density = 20.0f; - - b2WeldJointDef jd; - - b2Body* prevBody = ground; - for (int32 i = 0; i < e_count; ++i) - { - b2BodyDef bd; - bd.type = b2_dynamicBody; - bd.position.Set(-14.5f + 1.0f * i, 5.0f); - b2Body* body = m_world->CreateBody(&bd); - body->CreateFixture(&fd); - - b2Vec2 anchor(-15.0f + 1.0f * i, 5.0f); - jd.Initialize(prevBody, body, anchor); - m_world->CreateJoint(&jd); - - prevBody = body; - } - } - - { - b2PolygonShape shape; - shape.SetAsBox(1.0f, 0.125f); - - b2FixtureDef fd; - fd.shape = &shape; - fd.density = 20.0f; - - b2WeldJointDef jd; - float frequencyHz = 5.0f; - float dampingRatio = 0.7f; - - b2Body* prevBody = ground; - for (int32 i = 0; i < 3; ++i) - { - b2BodyDef bd; - bd.type = b2_dynamicBody; - bd.position.Set(-14.0f + 2.0f * i, 15.0f); - b2Body* body = m_world->CreateBody(&bd); - body->CreateFixture(&fd); - - b2Vec2 anchor(-15.0f + 2.0f * i, 15.0f); - jd.Initialize(prevBody, body, anchor); - b2AngularStiffness(jd.stiffness, jd.damping, frequencyHz, dampingRatio, jd.bodyA, jd.bodyB); - m_world->CreateJoint(&jd); - - prevBody = body; - } - } - - { - b2PolygonShape shape; - shape.SetAsBox(0.5f, 0.125f); - - b2FixtureDef fd; - fd.shape = &shape; - fd.density = 20.0f; - - b2WeldJointDef jd; - - b2Body* prevBody = ground; - for (int32 i = 0; i < e_count; ++i) - { - b2BodyDef bd; - bd.type = b2_dynamicBody; - bd.position.Set(-4.5f + 1.0f * i, 5.0f); - b2Body* body = m_world->CreateBody(&bd); - body->CreateFixture(&fd); - - if (i > 0) - { - b2Vec2 anchor(-5.0f + 1.0f * i, 5.0f); - jd.Initialize(prevBody, body, anchor); - m_world->CreateJoint(&jd); - } - - prevBody = body; - } - } - - { - b2PolygonShape shape; - shape.SetAsBox(0.5f, 0.125f); - - b2FixtureDef fd; - fd.shape = &shape; - fd.density = 20.0f; - - b2WeldJointDef jd; - float frequencyHz = 8.0f; - float dampingRatio = 0.7f; - - b2Body* prevBody = ground; - for (int32 i = 0; i < e_count; ++i) - { - b2BodyDef bd; - bd.type = b2_dynamicBody; - bd.position.Set(5.5f + 1.0f * i, 10.0f); - b2Body* body = m_world->CreateBody(&bd); - body->CreateFixture(&fd); - - if (i > 0) - { - b2Vec2 anchor(5.0f + 1.0f * i, 10.0f); - jd.Initialize(prevBody, body, anchor); - - b2AngularStiffness(jd.stiffness, jd.damping, frequencyHz, dampingRatio, prevBody, body); - - m_world->CreateJoint(&jd); - } - - prevBody = body; - } - } - - for (int32 i = 0; i < 2; ++i) - { - b2Vec2 vertices[3]; - vertices[0].Set(-0.5f, 0.0f); - vertices[1].Set(0.5f, 0.0f); - vertices[2].Set(0.0f, 1.5f); - - b2PolygonShape shape; - shape.Set(vertices, 3); - - b2FixtureDef fd; - fd.shape = &shape; - fd.density = 1.0f; - - b2BodyDef bd; - bd.type = b2_dynamicBody; - bd.position.Set(-8.0f + 8.0f * i, 12.0f); - b2Body* body = m_world->CreateBody(&bd); - body->CreateFixture(&fd); - } - - for (int32 i = 0; i < 2; ++i) - { - b2CircleShape shape; - shape.m_radius = 0.5f; - - b2FixtureDef fd; - fd.shape = &shape; - fd.density = 1.0f; - - b2BodyDef bd; - bd.type = b2_dynamicBody; - bd.position.Set(-6.0f + 6.0f * i, 10.0f); - b2Body* body = m_world->CreateBody(&bd); - body->CreateFixture(&fd); - } - } - - static Test* Create() { return new Cantilever; } - - b2Body* m_middle; -}; - -static int testIndex = RegisterTest("Joints", "Cantilever", Cantilever::Create); diff --git a/tests/cpp-tests/Source/Box2DTestBed/tests/car.cpp b/tests/cpp-tests/Source/Box2DTestBed/tests/car.cpp deleted file mode 100644 index 2051641bde4f..000000000000 --- a/tests/cpp-tests/Source/Box2DTestBed/tests/car.cpp +++ /dev/null @@ -1,286 +0,0 @@ -// MIT License - -// Copyright (c) 2019 Erin Catto -// Copyright(c) 2021 @aismann; Peter Eismann, Germany; dreifrankensoft - -// 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 "test.h" -#include "extensions/axmol-ext.h" - -USING_NS_AX_EXT; - -// This is a fun demo that shows off the wheel joint -class Car : public Test -{ -public: - Car() - { - m_speed = 50.0f; - - b2Body* ground = NULL; - { - b2BodyDef bd; - ground = m_world->CreateBody(&bd); - - b2EdgeShape shape; - - b2FixtureDef fd; - fd.shape = &shape; - fd.density = 0.0f; - fd.friction = 0.6f; - - shape.SetTwoSided(b2Vec2(-20.0f, 0.0f), b2Vec2(20.0f, 0.0f)); - ground->CreateFixture(&fd); - - float hs[10] = {0.25f, 1.0f, 4.0f, 0.0f, 0.0f, -1.0f, -2.0f, -2.0f, -1.25f, 0.0f}; - - float x = 20.0f, y1 = 0.0f, dx = 5.0f; - - for (int32 i = 0; i < 10; ++i) - { - float y2 = hs[i]; - shape.SetTwoSided(b2Vec2(x, y1), b2Vec2(x + dx, y2)); - ground->CreateFixture(&fd); - y1 = y2; - x += dx; - } - - for (int32 i = 0; i < 10; ++i) - { - float y2 = hs[i]; - shape.SetTwoSided(b2Vec2(x, y1), b2Vec2(x + dx, y2)); - ground->CreateFixture(&fd); - y1 = y2; - x += dx; - } - - shape.SetTwoSided(b2Vec2(x, 0.0f), b2Vec2(x + 40.0f, 0.0f)); - ground->CreateFixture(&fd); - - x += 80.0f; - shape.SetTwoSided(b2Vec2(x, 0.0f), b2Vec2(x + 40.0f, 0.0f)); - ground->CreateFixture(&fd); - - x += 40.0f; - shape.SetTwoSided(b2Vec2(x, 0.0f), b2Vec2(x + 10.0f, 5.0f)); - ground->CreateFixture(&fd); - - x += 20.0f; - shape.SetTwoSided(b2Vec2(x, 0.0f), b2Vec2(x + 40.0f, 0.0f)); - ground->CreateFixture(&fd); - - x += 40.0f; - shape.SetTwoSided(b2Vec2(x, 0.0f), b2Vec2(x, 20.0f)); - ground->CreateFixture(&fd); - } - - // Teeter - { - b2BodyDef bd; - bd.position.Set(140.0f, 1.0f); - bd.type = b2_dynamicBody; - b2Body* body = m_world->CreateBody(&bd); - - b2PolygonShape box; - box.SetAsBox(10.0f, 0.25f); - body->CreateFixture(&box, 1.0f); - - b2RevoluteJointDef jd; - jd.Initialize(ground, body, body->GetPosition()); - jd.lowerAngle = -8.0f * b2_pi / 180.0f; - jd.upperAngle = 8.0f * b2_pi / 180.0f; - jd.enableLimit = true; - m_world->CreateJoint(&jd); - - body->ApplyAngularImpulse(100.0f, true); - } - - // Bridge - { - int32 N = 20; - b2PolygonShape shape; - shape.SetAsBox(1.0f, 0.125f); - - b2FixtureDef fd; - fd.shape = &shape; - fd.density = 1.0f; - fd.friction = 0.6f; - - b2RevoluteJointDef jd; - - b2Body* prevBody = ground; - for (int32 i = 0; i < N; ++i) - { - b2BodyDef bd; - bd.type = b2_dynamicBody; - bd.position.Set(161.0f + 2.0f * i, -0.125f); - b2Body* body = m_world->CreateBody(&bd); - body->CreateFixture(&fd); - - b2Vec2 anchor(160.0f + 2.0f * i, -0.125f); - jd.Initialize(prevBody, body, anchor); - m_world->CreateJoint(&jd); - - prevBody = body; - } - - b2Vec2 anchor(160.0f + 2.0f * N, -0.125f); - jd.Initialize(prevBody, ground, anchor); - m_world->CreateJoint(&jd); - } - - // Boxes - { - b2PolygonShape box; - box.SetAsBox(0.5f, 0.5f); - - b2Body* body = NULL; - b2BodyDef bd; - bd.type = b2_dynamicBody; - - bd.position.Set(230.0f, 0.5f); - body = m_world->CreateBody(&bd); - body->CreateFixture(&box, 0.5f); - - bd.position.Set(230.0f, 1.5f); - body = m_world->CreateBody(&bd); - body->CreateFixture(&box, 0.5f); - - bd.position.Set(230.0f, 2.5f); - body = m_world->CreateBody(&bd); - body->CreateFixture(&box, 0.5f); - - bd.position.Set(230.0f, 3.5f); - body = m_world->CreateBody(&bd); - body->CreateFixture(&box, 0.5f); - - bd.position.Set(230.0f, 4.5f); - body = m_world->CreateBody(&bd); - body->CreateFixture(&box, 0.5f); - } - - // Car - { - b2PolygonShape chassis; - b2Vec2 vertices[8]; - vertices[0].Set(-1.5f, -0.5f); - vertices[1].Set(1.5f, -0.5f); - vertices[2].Set(1.5f, 0.0f); - vertices[3].Set(0.0f, 0.9f); - vertices[4].Set(-1.15f, 0.9f); - vertices[5].Set(-1.5f, 0.2f); - chassis.Set(vertices, 6); - - b2CircleShape circle; - circle.m_radius = 0.4f; - - b2BodyDef bd; - bd.type = b2_dynamicBody; - bd.position.Set(0.0f, 1.0f); - m_car = m_world->CreateBody(&bd); - m_car->CreateFixture(&chassis, 1.0f); - - b2FixtureDef fd; - fd.shape = &circle; - fd.density = 1.0f; - fd.friction = 0.9f; - - bd.position.Set(-1.0f, 0.35f); - m_wheel1 = m_world->CreateBody(&bd); - m_wheel1->CreateFixture(&fd); - - bd.position.Set(1.0f, 0.4f); - m_wheel2 = m_world->CreateBody(&bd); - m_wheel2->CreateFixture(&fd); - - b2WheelJointDef jd; - b2Vec2 axis(0.0f, 1.0f); - - float mass1 = m_wheel1->GetMass(); - float mass2 = m_wheel2->GetMass(); - - float hertz = 4.0f; - float dampingRatio = 0.7f; - float omega = 2.0f * b2_pi * hertz; - - jd.Initialize(m_car, m_wheel1, m_wheel1->GetPosition(), axis); - jd.motorSpeed = 0.0f; - jd.maxMotorTorque = 20.0f; - jd.enableMotor = true; - jd.stiffness = mass1 * omega * omega; - jd.damping = 2.0f * mass1 * dampingRatio * omega; - jd.lowerTranslation = -0.25f; - jd.upperTranslation = 0.25f; - jd.enableLimit = true; - m_spring1 = (b2WheelJoint*)m_world->CreateJoint(&jd); - - jd.Initialize(m_car, m_wheel2, m_wheel2->GetPosition(), axis); - jd.motorSpeed = 0.0f; - jd.maxMotorTorque = 10.0f; - jd.enableMotor = false; - jd.stiffness = mass2 * omega * omega; - jd.damping = 2.0f * mass2 * dampingRatio * omega; - jd.lowerTranslation = -0.25f; - jd.upperTranslation = 0.25f; - jd.enableLimit = true; - m_spring2 = (b2WheelJoint*)m_world->CreateJoint(&jd); - } - } - - void Keyboard(int key) override - { - switch (key) - { - case GLFW_KEY_A: - m_spring1->SetMotorSpeed(m_speed); - break; - - case GLFW_KEY_S: - m_spring1->SetMotorSpeed(0.0f); - break; - - case GLFW_KEY_D: - m_spring1->SetMotorSpeed(-m_speed); - break; - } - } - - void Step(Settings& settings) override - { - DrawString(5, m_textLine, "Keys: left = a, brake = s, right = d, hz down = q, hz up = e"); - - // g_camera.m_center.x = m_car->GetPosition().x; - g_debugDraw.debugNodeOffset.x += m_car->GetPosition().x; - - Test::Step(settings); - } - - static Test* Create() { return new Car; } - - b2Body* m_car; - b2Body* m_wheel1; - b2Body* m_wheel2; - - float m_speed; - b2WheelJoint* m_spring1; - b2WheelJoint* m_spring2; -}; - -static int testIndex = RegisterTest("Examples", "Car", Car::Create); diff --git a/tests/cpp-tests/Source/Box2DTestBed/tests/chain.cpp b/tests/cpp-tests/Source/Box2DTestBed/tests/chain.cpp deleted file mode 100644 index 3520b89330ca..000000000000 --- a/tests/cpp-tests/Source/Box2DTestBed/tests/chain.cpp +++ /dev/null @@ -1,89 +0,0 @@ -// MIT License - -// Copyright (c) 2019 Erin Catto - -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: - -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. - -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -#include "test.h" - -#define TEST_BAD_BODY 0 - -class Chain : public Test -{ -public: - Chain() - { - b2Body* ground = NULL; - { - b2BodyDef bd; - ground = m_world->CreateBody(&bd); - - b2EdgeShape shape; - shape.SetTwoSided(b2Vec2(-40.0f, 0.0f), b2Vec2(40.0f, 0.0f)); - ground->CreateFixture(&shape, 0.0f); - } - - { - b2PolygonShape shape; - shape.SetAsBox(0.6f, 0.125f); - - b2FixtureDef fd; - fd.shape = &shape; - fd.density = 20.0f; - fd.friction = 0.2f; - - b2RevoluteJointDef jd; - jd.collideConnected = false; - - const float y = 25.0f; - b2Body* prevBody = ground; - for (int32 i = 0; i < 30; ++i) - { - b2BodyDef bd; - bd.type = b2_dynamicBody; - bd.position.Set(0.5f + i, y); - b2Body* body = m_world->CreateBody(&bd); - -#if TEST_BAD_BODY == 1 - if (i == 10) - { - // Test zero density dynamic body - fd.density = 0.0f; - } - else - { - fd.density = 20.0f; - } -#endif - - body->CreateFixture(&fd); - - b2Vec2 anchor(float(i), y); - jd.Initialize(prevBody, body, anchor); - m_world->CreateJoint(&jd); - - prevBody = body; - } - } - } - - static Test* Create() { return new Chain; } -}; - -static int testIndex = RegisterTest("Joints", "Chain", Chain::Create); diff --git a/tests/cpp-tests/Source/Box2DTestBed/tests/chain_problem.cpp b/tests/cpp-tests/Source/Box2DTestBed/tests/chain_problem.cpp deleted file mode 100644 index a9a48a87301d..000000000000 --- a/tests/cpp-tests/Source/Box2DTestBed/tests/chain_problem.cpp +++ /dev/null @@ -1,89 +0,0 @@ -// MIT License - -// Copyright (c) 2019 Erin Catto - -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: - -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. - -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -#include "test.h" - -class ChainProblem : public Test -{ -public: - ChainProblem() - { - { - b2Vec2 g(0.0f, -10.0f); - m_world->SetGravity(g); - b2Body** bodies = (b2Body**)b2Alloc(2 * sizeof(b2Body*)); - b2Joint** joints = (b2Joint**)b2Alloc(0 * sizeof(b2Joint*)); - { - b2BodyDef bd; - bd.type = b2BodyType(0); - bodies[0] = m_world->CreateBody(&bd); - - { - b2FixtureDef fd; - - b2Vec2 v1(0.0f, 1.0f); - b2Vec2 v2(0.0f, 0.0f); - b2Vec2 v3(4.0f, 0.0f); - - b2EdgeShape shape; - shape.SetTwoSided(v1, v2); - bodies[0]->CreateFixture(&shape, 0.0f); - - shape.SetTwoSided(v2, v3); - bodies[0]->CreateFixture(&shape, 0.0f); - } - } - { - b2BodyDef bd; - bd.type = b2BodyType(2); - // bd.position.Set(6.033980250358582e-01f, 3.028350114822388e+00f); - bd.position.Set(1.0f, 3.0f); - bodies[1] = m_world->CreateBody(&bd); - - { - b2FixtureDef fd; - fd.friction = 0.2f; - fd.density = 10.0f; - b2PolygonShape shape; - b2Vec2 vs[8]; - vs[0].Set(0.5f, -3.0f); - vs[1].Set(0.5f, 3.0f); - vs[2].Set(-0.5f, 3.0f); - vs[3].Set(-0.5f, -3.0f); - shape.Set(vs, 4); - - fd.shape = &shape; - - bodies[1]->CreateFixture(&fd); - } - } - b2Free(joints); - b2Free(bodies); - joints = NULL; - bodies = NULL; - } - } - - static Test* Create() { return new ChainProblem; } -}; - -static int testIndex = RegisterTest("Bugs", "Chain Problem", ChainProblem::Create); diff --git a/tests/cpp-tests/Source/Box2DTestBed/tests/character_collision.cpp b/tests/cpp-tests/Source/Box2DTestBed/tests/character_collision.cpp deleted file mode 100644 index 42f9f81d6f98..000000000000 --- a/tests/cpp-tests/Source/Box2DTestBed/tests/character_collision.cpp +++ /dev/null @@ -1,252 +0,0 @@ -// MIT License - -// Copyright (c) 2019 Erin Catto - -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: - -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. - -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -#include "test.h" - -/// This is a test of typical character collision scenarios. This does not -/// show how you should implement a character in your application. -/// Instead this is used to test smooth collision on edge chains. -class CharacterCollision : public Test -{ -public: - CharacterCollision() - { - // Ground body - { - b2BodyDef bd; - b2Body* ground = m_world->CreateBody(&bd); - - b2EdgeShape shape; - shape.SetTwoSided(b2Vec2(-20.0f, 0.0f), b2Vec2(20.0f, 0.0f)); - ground->CreateFixture(&shape, 0.0f); - } - - // Collinear edges with no adjacency information. - // This shows the problematic case where a box shape can hit - // an internal vertex. - { - b2BodyDef bd; - b2Body* ground = m_world->CreateBody(&bd); - - b2EdgeShape shape; - shape.SetTwoSided(b2Vec2(-8.0f, 1.0f), b2Vec2(-6.0f, 1.0f)); - ground->CreateFixture(&shape, 0.0f); - shape.SetTwoSided(b2Vec2(-6.0f, 1.0f), b2Vec2(-4.0f, 1.0f)); - ground->CreateFixture(&shape, 0.0f); - shape.SetTwoSided(b2Vec2(-4.0f, 1.0f), b2Vec2(-2.0f, 1.0f)); - ground->CreateFixture(&shape, 0.0f); - } - - // Chain shape - { - b2BodyDef bd; - bd.angle = 0.25f * b2_pi; - b2Body* ground = m_world->CreateBody(&bd); - - b2Vec2 vs[4]; - vs[0].Set(5.0f, 7.0f); - vs[1].Set(6.0f, 8.0f); - vs[2].Set(7.0f, 8.0f); - vs[3].Set(8.0f, 7.0f); - b2ChainShape shape; - shape.CreateLoop(vs, 4); - ground->CreateFixture(&shape, 0.0f); - } - - // Square tiles. This shows that adjacency shapes may - // have non-smooth collision. There is no solution - // to this problem. - { - b2BodyDef bd; - b2Body* ground = m_world->CreateBody(&bd); - - b2PolygonShape shape; - shape.SetAsBox(1.0f, 1.0f, b2Vec2(4.0f, 3.0f), 0.0f); - ground->CreateFixture(&shape, 0.0f); - shape.SetAsBox(1.0f, 1.0f, b2Vec2(6.0f, 3.0f), 0.0f); - ground->CreateFixture(&shape, 0.0f); - shape.SetAsBox(1.0f, 1.0f, b2Vec2(8.0f, 3.0f), 0.0f); - ground->CreateFixture(&shape, 0.0f); - } - - // Square made from an edge loop. Collision should be smooth. - { - b2BodyDef bd; - b2Body* ground = m_world->CreateBody(&bd); - - b2Vec2 vs[4]; - vs[0].Set(-1.0f, 3.0f); - vs[1].Set(1.0f, 3.0f); - vs[2].Set(1.0f, 5.0f); - vs[3].Set(-1.0f, 5.0f); - b2ChainShape shape; - shape.CreateLoop(vs, 4); - ground->CreateFixture(&shape, 0.0f); - } - - // Edge loop. Collision should be smooth. - { - b2BodyDef bd; - bd.position.Set(-10.0f, 4.0f); - b2Body* ground = m_world->CreateBody(&bd); - - b2Vec2 vs[10]; - vs[0].Set(0.0f, 0.0f); - vs[1].Set(6.0f, 0.0f); - vs[2].Set(6.0f, 2.0f); - vs[3].Set(4.0f, 1.0f); - vs[4].Set(2.0f, 2.0f); - vs[5].Set(0.0f, 2.0f); - vs[6].Set(-2.0f, 2.0f); - vs[7].Set(-4.0f, 3.0f); - vs[8].Set(-6.0f, 2.0f); - vs[9].Set(-6.0f, 0.0f); - b2ChainShape shape; - shape.CreateLoop(vs, 10); - ground->CreateFixture(&shape, 0.0f); - } - - // Square character 1 - { - b2BodyDef bd; - bd.position.Set(-3.0f, 8.0f); - bd.type = b2_dynamicBody; - bd.fixedRotation = true; - bd.allowSleep = false; - - b2Body* body = m_world->CreateBody(&bd); - - b2PolygonShape shape; - shape.SetAsBox(0.5f, 0.5f); - - b2FixtureDef fd; - fd.shape = &shape; - fd.density = 20.0f; - body->CreateFixture(&fd); - } - - // Square character 2 - { - b2BodyDef bd; - bd.position.Set(-5.0f, 5.0f); - bd.type = b2_dynamicBody; - bd.fixedRotation = true; - bd.allowSleep = false; - - b2Body* body = m_world->CreateBody(&bd); - - b2PolygonShape shape; - shape.SetAsBox(0.25f, 0.25f); - - b2FixtureDef fd; - fd.shape = &shape; - fd.density = 20.0f; - body->CreateFixture(&fd); - } - - // Hexagon character - { - b2BodyDef bd; - bd.position.Set(-5.0f, 8.0f); - bd.type = b2_dynamicBody; - bd.fixedRotation = true; - bd.allowSleep = false; - - b2Body* body = m_world->CreateBody(&bd); - - float angle = 0.0f; - float delta = b2_pi / 3.0f; - b2Vec2 vertices[6]; - for (int32 i = 0; i < 6; ++i) - { - vertices[i].Set(0.5f * cosf(angle), 0.5f * sinf(angle)); - angle += delta; - } - - b2PolygonShape shape; - shape.Set(vertices, 6); - - b2FixtureDef fd; - fd.shape = &shape; - fd.density = 20.0f; - body->CreateFixture(&fd); - } - - // Circle character - { - b2BodyDef bd; - bd.position.Set(3.0f, 5.0f); - bd.type = b2_dynamicBody; - bd.fixedRotation = true; - bd.allowSleep = false; - - b2Body* body = m_world->CreateBody(&bd); - - b2CircleShape shape; - shape.m_radius = 0.5f; - - b2FixtureDef fd; - fd.shape = &shape; - fd.density = 20.0f; - body->CreateFixture(&fd); - } - - // Circle character - { - b2BodyDef bd; - bd.position.Set(-7.0f, 6.0f); - bd.type = b2_dynamicBody; - bd.allowSleep = false; - - m_character = m_world->CreateBody(&bd); - - b2CircleShape shape; - shape.m_radius = 0.25f; - - b2FixtureDef fd; - fd.shape = &shape; - fd.density = 20.0f; - fd.friction = 1.0f; - m_character->CreateFixture(&fd); - } - } - - void Step(Settings& settings) override - { - b2Vec2 v = m_character->GetLinearVelocity(); - v.x = -5.0f; - m_character->SetLinearVelocity(v); - - Test::Step(settings); - DrawString(5, m_textLine, "This tests various character collision shapes."); - - DrawString(5, m_textLine, "Limitation: square and hexagon can snag on aligned boxes."); - - DrawString(5, m_textLine, "Feature: edge chains have smooth collision inside and out."); - } - - static Test* Create() { return new CharacterCollision; } - - b2Body* m_character; -}; - -static int testIndex = RegisterTest("Examples", "Character Collision", CharacterCollision::Create); diff --git a/tests/cpp-tests/Source/Box2DTestBed/tests/circle_stack.cpp b/tests/cpp-tests/Source/Box2DTestBed/tests/circle_stack.cpp deleted file mode 100644 index d642a12d33e2..000000000000 --- a/tests/cpp-tests/Source/Box2DTestBed/tests/circle_stack.cpp +++ /dev/null @@ -1,85 +0,0 @@ -// MIT License - -// Copyright (c) 2019 Erin Catto - -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: - -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. - -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -#include "test.h" - -class CircleStack : public Test -{ -public: - enum - { - e_count = 10 - }; - - CircleStack() - { - { - b2BodyDef bd; - b2Body* ground = m_world->CreateBody(&bd); - - b2EdgeShape shape; - shape.SetTwoSided(b2Vec2(-40.0f, 0.0f), b2Vec2(40.0f, 0.0f)); - ground->CreateFixture(&shape, 0.0f); - } - - { - b2CircleShape shape; - shape.m_radius = 1.0f; - - for (int32 i = 0; i < e_count; ++i) - { - b2BodyDef bd; - bd.type = b2_dynamicBody; - bd.position.Set(0.0, 4.0f + 3.0f * i); - - m_bodies[i] = m_world->CreateBody(&bd); - - m_bodies[i]->CreateFixture(&shape, 1.0f); - - m_bodies[i]->SetLinearVelocity(b2Vec2(0.0f, -50.0f)); - } - } - } - - void Step(Settings& settings) override - { - Test::Step(settings); - - // for (int32 i = 0; i < e_count; ++i) - //{ - // printf("%g ", m_bodies[i]->GetWorldCenter().y); - // } - - // for (int32 i = 0; i < e_count; ++i) - //{ - // printf("%g ", m_bodies[i]->GetLinearVelocity().y); - // } - - // printf("\n"); - } - - static Test* Create() { return new CircleStack; } - - b2Body* m_bodies[e_count]; -}; - -static int testIndex = RegisterTest("Stacking", "Circles", CircleStack::Create); diff --git a/tests/cpp-tests/Source/Box2DTestBed/tests/collision_filtering.cpp b/tests/cpp-tests/Source/Box2DTestBed/tests/collision_filtering.cpp deleted file mode 100644 index 65acb3866862..000000000000 --- a/tests/cpp-tests/Source/Box2DTestBed/tests/collision_filtering.cpp +++ /dev/null @@ -1,176 +0,0 @@ -// MIT License - -// Copyright (c) 2019 Erin Catto - -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: - -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. - -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -#include "test.h" - -// This is a test of collision filtering. -// There is a triangle, a box, and a circle. -// There are 6 shapes. 3 large and 3 small. -// The 3 small ones always collide. -// The 3 large ones never collide. -// The boxes don't collide with triangles (except if both are small). -const int16 k_smallGroup = 1; -const int16 k_largeGroup = -1; - -const uint16 k_triangleCategory = 0x0002; -const uint16 k_boxCategory = 0x0004; -const uint16 k_circleCategory = 0x0008; - -const uint16 k_triangleMask = 0xFFFF; -const uint16 k_boxMask = 0xFFFF ^ k_triangleCategory; -const uint16 k_circleMask = 0xFFFF; - -class CollisionFiltering : public Test -{ -public: - CollisionFiltering() - { - // Ground body - { - b2EdgeShape shape; - shape.SetTwoSided(b2Vec2(-40.0f, 0.0f), b2Vec2(40.0f, 0.0f)); - - b2FixtureDef sd; - sd.shape = &shape; - sd.friction = 0.3f; - - b2BodyDef bd; - b2Body* ground = m_world->CreateBody(&bd); - ground->CreateFixture(&sd); - } - - // Small triangle - b2Vec2 vertices[3]; - vertices[0].Set(-1.0f, 0.0f); - vertices[1].Set(1.0f, 0.0f); - vertices[2].Set(0.0f, 2.0f); - b2PolygonShape polygon; - polygon.Set(vertices, 3); - - b2FixtureDef triangleShapeDef; - triangleShapeDef.shape = &polygon; - triangleShapeDef.density = 1.0f; - - triangleShapeDef.filter.groupIndex = k_smallGroup; - triangleShapeDef.filter.categoryBits = k_triangleCategory; - triangleShapeDef.filter.maskBits = k_triangleMask; - - b2BodyDef triangleBodyDef; - triangleBodyDef.type = b2_dynamicBody; - triangleBodyDef.position.Set(-5.0f, 2.0f); - - b2Body* body1 = m_world->CreateBody(&triangleBodyDef); - body1->CreateFixture(&triangleShapeDef); - - // Large triangle (recycle definitions) - vertices[0] *= 2.0f; - vertices[1] *= 2.0f; - vertices[2] *= 2.0f; - polygon.Set(vertices, 3); - triangleShapeDef.filter.groupIndex = k_largeGroup; - triangleBodyDef.position.Set(-5.0f, 6.0f); - triangleBodyDef.fixedRotation = true; // look at me! - - b2Body* body2 = m_world->CreateBody(&triangleBodyDef); - body2->CreateFixture(&triangleShapeDef); - - { - b2BodyDef bd; - bd.type = b2_dynamicBody; - bd.position.Set(-5.0f, 10.0f); - b2Body* body = m_world->CreateBody(&bd); - - b2PolygonShape p; - p.SetAsBox(0.5f, 1.0f); - body->CreateFixture(&p, 1.0f); - - b2PrismaticJointDef jd; - jd.bodyA = body2; - jd.bodyB = body; - jd.enableLimit = true; - jd.localAnchorA.Set(0.0f, 4.0f); - jd.localAnchorB.SetZero(); - jd.localAxisA.Set(0.0f, 1.0f); - jd.lowerTranslation = -1.0f; - jd.upperTranslation = 1.0f; - - m_world->CreateJoint(&jd); - } - - // Small box - polygon.SetAsBox(1.0f, 0.5f); - b2FixtureDef boxShapeDef; - boxShapeDef.shape = &polygon; - boxShapeDef.density = 1.0f; - boxShapeDef.restitution = 0.1f; - - boxShapeDef.filter.groupIndex = k_smallGroup; - boxShapeDef.filter.categoryBits = k_boxCategory; - boxShapeDef.filter.maskBits = k_boxMask; - - b2BodyDef boxBodyDef; - boxBodyDef.type = b2_dynamicBody; - boxBodyDef.position.Set(0.0f, 2.0f); - - b2Body* body3 = m_world->CreateBody(&boxBodyDef); - body3->CreateFixture(&boxShapeDef); - - // Large box (recycle definitions) - polygon.SetAsBox(2.0f, 1.0f); - boxShapeDef.filter.groupIndex = k_largeGroup; - boxBodyDef.position.Set(0.0f, 6.0f); - - b2Body* body4 = m_world->CreateBody(&boxBodyDef); - body4->CreateFixture(&boxShapeDef); - - // Small circle - b2CircleShape circle; - circle.m_radius = 1.0f; - - b2FixtureDef circleShapeDef; - circleShapeDef.shape = &circle; - circleShapeDef.density = 1.0f; - - circleShapeDef.filter.groupIndex = k_smallGroup; - circleShapeDef.filter.categoryBits = k_circleCategory; - circleShapeDef.filter.maskBits = k_circleMask; - - b2BodyDef circleBodyDef; - circleBodyDef.type = b2_dynamicBody; - circleBodyDef.position.Set(5.0f, 2.0f); - - b2Body* body5 = m_world->CreateBody(&circleBodyDef); - body5->CreateFixture(&circleShapeDef); - - // Large circle - circle.m_radius *= 2.0f; - circleShapeDef.filter.groupIndex = k_largeGroup; - circleBodyDef.position.Set(5.0f, 6.0f); - - b2Body* body6 = m_world->CreateBody(&circleBodyDef); - body6->CreateFixture(&circleShapeDef); - } - - static Test* Create() { return new CollisionFiltering; } -}; - -static int testIndex = RegisterTest("Examples", "Collision Filtering", CollisionFiltering::Create); diff --git a/tests/cpp-tests/Source/Box2DTestBed/tests/collision_processing.cpp b/tests/cpp-tests/Source/Box2DTestBed/tests/collision_processing.cpp deleted file mode 100644 index f31db91a3b7d..000000000000 --- a/tests/cpp-tests/Source/Box2DTestBed/tests/collision_processing.cpp +++ /dev/null @@ -1,189 +0,0 @@ -// MIT License - -// Copyright (c) 2019 Erin Catto - -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: - -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. - -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -#include "test.h" - -#include - -// This test shows collision processing and tests -// deferred body destruction. -class CollisionProcessing : public Test -{ -public: - CollisionProcessing() - { - // Ground body - { - b2EdgeShape shape; - shape.SetTwoSided(b2Vec2(-50.0f, 0.0f), b2Vec2(50.0f, 0.0f)); - - b2FixtureDef sd; - sd.shape = &shape; - ; - - b2BodyDef bd; - b2Body* ground = m_world->CreateBody(&bd); - ground->CreateFixture(&sd); - } - - float xLo = -5.0f, xHi = 5.0f; - float yLo = 2.0f, yHi = 35.0f; - - // Small triangle - b2Vec2 vertices[3]; - vertices[0].Set(-1.0f, 0.0f); - vertices[1].Set(1.0f, 0.0f); - vertices[2].Set(0.0f, 2.0f); - - b2PolygonShape polygon; - polygon.Set(vertices, 3); - - b2FixtureDef triangleShapeDef; - triangleShapeDef.shape = &polygon; - triangleShapeDef.density = 1.0f; - - b2BodyDef triangleBodyDef; - triangleBodyDef.type = b2_dynamicBody; - triangleBodyDef.position.Set(RandomFloat(xLo, xHi), RandomFloat(yLo, yHi)); - - b2Body* body1 = m_world->CreateBody(&triangleBodyDef); - body1->CreateFixture(&triangleShapeDef); - - // Large triangle (recycle definitions) - vertices[0] *= 2.0f; - vertices[1] *= 2.0f; - vertices[2] *= 2.0f; - polygon.Set(vertices, 3); - - triangleBodyDef.position.Set(RandomFloat(xLo, xHi), RandomFloat(yLo, yHi)); - - b2Body* body2 = m_world->CreateBody(&triangleBodyDef); - body2->CreateFixture(&triangleShapeDef); - - // Small box - polygon.SetAsBox(1.0f, 0.5f); - - b2FixtureDef boxShapeDef; - boxShapeDef.shape = &polygon; - boxShapeDef.density = 1.0f; - - b2BodyDef boxBodyDef; - boxBodyDef.type = b2_dynamicBody; - boxBodyDef.position.Set(RandomFloat(xLo, xHi), RandomFloat(yLo, yHi)); - - b2Body* body3 = m_world->CreateBody(&boxBodyDef); - body3->CreateFixture(&boxShapeDef); - - // Large box (recycle definitions) - polygon.SetAsBox(2.0f, 1.0f); - boxBodyDef.position.Set(RandomFloat(xLo, xHi), RandomFloat(yLo, yHi)); - - b2Body* body4 = m_world->CreateBody(&boxBodyDef); - body4->CreateFixture(&boxShapeDef); - - // Small circle - b2CircleShape circle; - circle.m_radius = 1.0f; - - b2FixtureDef circleShapeDef; - circleShapeDef.shape = &circle; - circleShapeDef.density = 1.0f; - - b2BodyDef circleBodyDef; - circleBodyDef.type = b2_dynamicBody; - circleBodyDef.position.Set(RandomFloat(xLo, xHi), RandomFloat(yLo, yHi)); - - b2Body* body5 = m_world->CreateBody(&circleBodyDef); - body5->CreateFixture(&circleShapeDef); - - // Large circle - circle.m_radius *= 2.0f; - circleBodyDef.position.Set(RandomFloat(xLo, xHi), RandomFloat(yLo, yHi)); - - b2Body* body6 = m_world->CreateBody(&circleBodyDef); - body6->CreateFixture(&circleShapeDef); - } - - void Step(Settings& settings) override - { - Test::Step(settings); - - // We are going to destroy some bodies according to contact - // points. We must buffer the bodies that should be destroyed - // because they may belong to multiple contact points. - const int32 k_maxNuke = 6; - b2Body* nuke[k_maxNuke]; - int32 nukeCount = 0; - - // Traverse the contact results. Destroy bodies that - // are touching heavier bodies. - for (int32 i = 0; i < m_pointCount; ++i) - { - ContactPoint* point = m_points + i; - - b2Body* body1 = point->fixtureA->GetBody(); - b2Body* body2 = point->fixtureB->GetBody(); - float mass1 = body1->GetMass(); - float mass2 = body2->GetMass(); - - if (mass1 > 0.0f && mass2 > 0.0f) - { - if (mass2 > mass1) - { - nuke[nukeCount++] = body1; - } - else - { - nuke[nukeCount++] = body2; - } - - if (nukeCount == k_maxNuke) - { - break; - } - } - } - - // Sort the nuke array to group duplicates. - std::sort(nuke, nuke + nukeCount); - - // Destroy the bodies, skipping duplicates. - int32 i = 0; - while (i < nukeCount) - { - b2Body* b = nuke[i++]; - while (i < nukeCount && nuke[i] == b) - { - ++i; - } - - if (b != m_bomb) - { - m_world->DestroyBody(b); - } - } - } - - static Test* Create() { return new CollisionProcessing; } -}; - -static int testIndex = RegisterTest("Examples", "Collision Processing", CollisionProcessing::Create); diff --git a/tests/cpp-tests/Source/Box2DTestBed/tests/compound_shapes.cpp b/tests/cpp-tests/Source/Box2DTestBed/tests/compound_shapes.cpp deleted file mode 100644 index bb3d7579fbcc..000000000000 --- a/tests/cpp-tests/Source/Box2DTestBed/tests/compound_shapes.cpp +++ /dev/null @@ -1,224 +0,0 @@ -// MIT License - -// Copyright (c) 2019 Erin Catto - -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: - -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. - -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -#include "test.h" -#include "ImGui/ImGuiPresenter.h" - -class CompoundShapes : public Test -{ -public: - CompoundShapes() - { - { - b2BodyDef bd; - bd.position.Set(0.0f, 0.0f); - b2Body* body = m_world->CreateBody(&bd); - - b2EdgeShape shape; - shape.SetTwoSided(b2Vec2(50.0f, 0.0f), b2Vec2(-50.0f, 0.0f)); - - body->CreateFixture(&shape, 0.0f); - } - - // Table 1 - { - b2BodyDef bd; - bd.type = b2_dynamicBody; - bd.position.Set(-15.0f, 1.0f); - m_table1 = m_world->CreateBody(&bd); - - b2PolygonShape top; - top.SetAsBox(3.0f, 0.5f, b2Vec2(0.0f, 3.5f), 0.0f); - - b2PolygonShape leftLeg; - leftLeg.SetAsBox(0.5f, 1.5f, b2Vec2(-2.5f, 1.5f), 0.0f); - - b2PolygonShape rightLeg; - rightLeg.SetAsBox(0.5f, 1.5f, b2Vec2(2.5f, 1.5f), 0.0f); - - m_table1->CreateFixture(&top, 2.0f); - m_table1->CreateFixture(&leftLeg, 2.0f); - m_table1->CreateFixture(&rightLeg, 2.0f); - } - - // Table 2 - { - b2BodyDef bd; - bd.type = b2_dynamicBody; - bd.position.Set(-5.0f, 1.0f); - m_table2 = m_world->CreateBody(&bd); - - b2PolygonShape top; - top.SetAsBox(3.0f, 0.5f, b2Vec2(0.0f, 3.5f), 0.0f); - - b2PolygonShape leftLeg; - leftLeg.SetAsBox(0.5f, 2.0f, b2Vec2(-2.5f, 2.0f), 0.0f); - - b2PolygonShape rightLeg; - rightLeg.SetAsBox(0.5f, 2.0f, b2Vec2(2.5f, 2.0f), 0.0f); - - m_table2->CreateFixture(&top, 2.0f); - m_table2->CreateFixture(&leftLeg, 2.0f); - m_table2->CreateFixture(&rightLeg, 2.0f); - } - - // Spaceship 1 - { - b2BodyDef bd; - bd.type = b2_dynamicBody; - bd.position.Set(5.0f, 1.0f); - m_ship1 = m_world->CreateBody(&bd); - - b2Vec2 vertices[3]; - - b2PolygonShape left; - vertices[0].Set(-2.0f, 0.0f); - vertices[1].Set(0.0f, 4.0f / 3.0f); - vertices[2].Set(0.0f, 4.0f); - left.Set(vertices, 3); - - b2PolygonShape right; - vertices[0].Set(2.0f, 0.0f); - vertices[1].Set(0.0f, 4.0f / 3.0f); - vertices[2].Set(0.0f, 4.0f); - right.Set(vertices, 3); - - m_ship1->CreateFixture(&left, 2.0f); - m_ship1->CreateFixture(&right, 2.0f); - } - - // Spaceship 2 - { - b2BodyDef bd; - bd.type = b2_dynamicBody; - bd.position.Set(15.0f, 1.0f); - m_ship2 = m_world->CreateBody(&bd); - - b2Vec2 vertices[3]; - - b2PolygonShape left; - vertices[0].Set(-2.0f, 0.0f); - vertices[1].Set(1.0f, 2.0f); - vertices[2].Set(0.0f, 4.0f); - left.Set(vertices, 3); - - b2PolygonShape right; - vertices[0].Set(2.0f, 0.0f); - vertices[1].Set(-1.0f, 2.0f); - vertices[2].Set(0.0f, 4.0f); - right.Set(vertices, 3); - - m_ship2->CreateFixture(&left, 2.0f); - m_ship2->CreateFixture(&right, 2.0f); - } - } - - void Spawn() - { - // Table 1 obstruction - { - b2BodyDef bd; - bd.type = b2_dynamicBody; - bd.position = m_table1->GetPosition(); - bd.angle = m_table1->GetAngle(); - - b2Body* body = m_world->CreateBody(&bd); - - b2PolygonShape box; - box.SetAsBox(4.0f, 0.1f, b2Vec2(0.0f, 3.0f), 0.0f); - - body->CreateFixture(&box, 2.0f); - } - - // Table 2 obstruction - { - b2BodyDef bd; - bd.type = b2_dynamicBody; - bd.position = m_table2->GetPosition(); - bd.angle = m_table2->GetAngle(); - - b2Body* body = m_world->CreateBody(&bd); - - b2PolygonShape box; - box.SetAsBox(4.0f, 0.1f, b2Vec2(0.0f, 3.0f), 0.0f); - - body->CreateFixture(&box, 2.0f); - } - - // Ship 1 obstruction - { - b2BodyDef bd; - bd.type = b2_dynamicBody; - bd.position = m_ship1->GetPosition(); - bd.angle = m_ship1->GetAngle(); - bd.gravityScale = 0.0f; - - b2Body* body = m_world->CreateBody(&bd); - - b2CircleShape circle; - circle.m_radius = 0.5f; - circle.m_p.Set(0.0f, 2.0f); - - body->CreateFixture(&circle, 2.0f); - } - - // Ship 2 obstruction - { - b2BodyDef bd; - bd.type = b2_dynamicBody; - bd.position = m_ship2->GetPosition(); - bd.angle = m_ship2->GetAngle(); - bd.gravityScale = 0.0f; - - b2Body* body = m_world->CreateBody(&bd); - - b2CircleShape circle; - circle.m_radius = 0.5f; - circle.m_p.Set(0.0f, 2.0f); - - body->CreateFixture(&circle, 2.0f); - } - } - - void UpdateUI() override - { - // ImGui::SetNextWindowPos(ImVec2(10.0f, 100.0f)); - ImGui::SetNextWindowSize(ImVec2(200.0f, 100.0f)); - ImGui::Begin("Controls", nullptr, ImGuiWindowFlags_NoResize); - - if (ImGui::Button("Spawn")) - { - Spawn(); - } - - ImGui::End(); - } - - static Test* Create() { return new CompoundShapes; } - - b2Body* m_table1; - b2Body* m_table2; - b2Body* m_ship1; - b2Body* m_ship2; -}; - -static int testIndex = RegisterTest("Examples", "Compound Shapes", CompoundShapes::Create); diff --git a/tests/cpp-tests/Source/Box2DTestBed/tests/confined.cpp b/tests/cpp-tests/Source/Box2DTestBed/tests/confined.cpp deleted file mode 100644 index 44e2f6201036..000000000000 --- a/tests/cpp-tests/Source/Box2DTestBed/tests/confined.cpp +++ /dev/null @@ -1,165 +0,0 @@ -// MIT License - -// Copyright (c) 2019 Erin Catto - -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: - -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. - -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -#include "test.h" - -class Confined : public Test -{ -public: - enum - { - e_columnCount = 0, - e_rowCount = 0 - }; - - Confined() - { - { - b2BodyDef bd; - b2Body* ground = m_world->CreateBody(&bd); - - b2EdgeShape shape; - - // Floor - shape.SetTwoSided(b2Vec2(-10.0f, 0.0f), b2Vec2(10.0f, 0.0f)); - ground->CreateFixture(&shape, 0.0f); - - // Left wall - shape.SetTwoSided(b2Vec2(-10.0f, 0.0f), b2Vec2(-10.0f, 20.0f)); - ground->CreateFixture(&shape, 0.0f); - - // Right wall - shape.SetTwoSided(b2Vec2(10.0f, 0.0f), b2Vec2(10.0f, 20.0f)); - ground->CreateFixture(&shape, 0.0f); - - // Roof - shape.SetTwoSided(b2Vec2(-10.0f, 20.0f), b2Vec2(10.0f, 20.0f)); - ground->CreateFixture(&shape, 0.0f); - } - - float radius = 0.5f; - b2CircleShape shape; - shape.m_p.SetZero(); - shape.m_radius = radius; - - b2FixtureDef fd; - fd.shape = &shape; - fd.density = 1.0f; - fd.friction = 0.1f; - - for (int32 j = 0; j < e_columnCount; ++j) - { - for (int i = 0; i < e_rowCount; ++i) - { - b2BodyDef bd; - bd.type = b2_dynamicBody; - bd.position.Set(-10.0f + (2.1f * j + 1.0f + 0.01f * i) * radius, (2.0f * i + 1.0f) * radius); - b2Body* body = m_world->CreateBody(&bd); - - body->CreateFixture(&fd); - } - } - - m_world->SetGravity(b2Vec2(0.0f, 0.0f)); - } - - void CreateCircle() - { - float radius = 2.0f; - b2CircleShape shape; - shape.m_p.SetZero(); - shape.m_radius = radius; - - b2FixtureDef fd; - fd.shape = &shape; - fd.density = 1.0f; - fd.friction = 0.0f; - - b2Vec2 p(RandomFloat(), 3.0f + RandomFloat()); - b2BodyDef bd; - bd.type = b2_dynamicBody; - bd.position = p; - // bd.allowSleep = false; - b2Body* body = m_world->CreateBody(&bd); - - body->CreateFixture(&fd); - } - - void Keyboard(int key) override - { - switch (key) - { - case GLFW_KEY_C: - CreateCircle(); - break; - } - } - - void Step(Settings& settings) override - { - bool sleeping = true; - for (b2Body* b = m_world->GetBodyList(); b; b = b->GetNext()) - { - if (b->GetType() != b2_dynamicBody) - { - continue; - } - - if (b->IsAwake()) - { - sleeping = false; - } - } - - if (m_stepCount == 180) - { - m_stepCount += 0; - } - - // if (sleeping) - //{ - // CreateCircle(); - // } - - Test::Step(settings); - - for (b2Body* b = m_world->GetBodyList(); b; b = b->GetNext()) - { - if (b->GetType() != b2_dynamicBody) - { - continue; - } - - b2Vec2 p = b->GetPosition(); - if (p.x <= -10.0f || 10.0f <= p.x || p.y <= 0.0f || 20.0f <= p.y) - { - p.x += 0.0f; - } - } - - DrawString(5, m_textLine, "Press 'c' to create a circle."); - } - - static Test* Create() { return new Confined; } -}; - -static int testIndex = RegisterTest("Solver", "Confined", Confined::Create); diff --git a/tests/cpp-tests/Source/Box2DTestBed/tests/continuous_test.cpp b/tests/cpp-tests/Source/Box2DTestBed/tests/continuous_test.cpp deleted file mode 100644 index 80187d9daaad..000000000000 --- a/tests/cpp-tests/Source/Box2DTestBed/tests/continuous_test.cpp +++ /dev/null @@ -1,162 +0,0 @@ -// MIT License - -// Copyright (c) 2019 Erin Catto - -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: - -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. - -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -#include "test.h" - -class ContinuousTest : public Test -{ -public: - ContinuousTest() - { - { - b2BodyDef bd; - bd.position.Set(0.0f, 0.0f); - b2Body* body = m_world->CreateBody(&bd); - - b2EdgeShape edge; - - edge.SetTwoSided(b2Vec2(-10.0f, 0.0f), b2Vec2(10.0f, 0.0f)); - body->CreateFixture(&edge, 0.0f); - - b2PolygonShape shape; - shape.SetAsBox(0.2f, 1.0f, b2Vec2(0.5f, 1.0f), 0.0f); - body->CreateFixture(&shape, 0.0f); - } - -#if 1 - { - b2BodyDef bd; - bd.type = b2_dynamicBody; - bd.position.Set(0.0f, 20.0f); - // bd.angle = 0.1f; - - b2PolygonShape shape; - shape.SetAsBox(2.0f, 0.1f); - - m_body = m_world->CreateBody(&bd); - m_body->CreateFixture(&shape, 1.0f); - - m_angularVelocity = RandomFloat(-50.0f, 50.0f); - // m_angularVelocity = 46.661274f; - m_body->SetLinearVelocity(b2Vec2(0.0f, -100.0f)); - m_body->SetAngularVelocity(m_angularVelocity); - } -#else - { - b2BodyDef bd; - bd.type = b2_dynamicBody; - bd.position.Set(0.0f, 2.0f); - b2Body* body = m_world->CreateBody(&bd); - - b2CircleShape shape; - shape.m_p.SetZero(); - shape.m_radius = 0.5f; - body->CreateFixture(&shape, 1.0f); - - bd.bullet = true; - bd.position.Set(0.0f, 10.0f); - body = m_world->CreateBody(&bd); - body->CreateFixture(&shape, 1.0f); - body->SetLinearVelocity(b2Vec2(0.0f, -100.0f)); - } -#endif - - extern B2_API int32 b2_gjkCalls, b2_gjkIters, b2_gjkMaxIters; - extern B2_API int32 b2_toiCalls, b2_toiIters; - extern B2_API int32 b2_toiRootIters, b2_toiMaxRootIters; - extern B2_API float b2_toiTime, b2_toiMaxTime; - - b2_gjkCalls = 0; - b2_gjkIters = 0; - b2_gjkMaxIters = 0; - b2_toiCalls = 0; - b2_toiIters = 0; - b2_toiRootIters = 0; - b2_toiMaxRootIters = 0; - b2_toiTime = 0.0f; - b2_toiMaxTime = 0.0f; - } - - void Launch() - { - extern B2_API int32 b2_gjkCalls, b2_gjkIters, b2_gjkMaxIters; - extern B2_API int32 b2_toiCalls, b2_toiIters; - extern B2_API int32 b2_toiRootIters, b2_toiMaxRootIters; - extern B2_API float b2_toiTime, b2_toiMaxTime; - - b2_gjkCalls = 0; - b2_gjkIters = 0; - b2_gjkMaxIters = 0; - b2_toiCalls = 0; - b2_toiIters = 0; - b2_toiRootIters = 0; - b2_toiMaxRootIters = 0; - b2_toiTime = 0.0f; - b2_toiMaxTime = 0.0f; - - m_body->SetTransform(b2Vec2(0.0f, 20.0f), 0.0f); - m_angularVelocity = RandomFloat(-50.0f, 50.0f); - m_body->SetLinearVelocity(b2Vec2(0.0f, -100.0f)); - m_body->SetAngularVelocity(m_angularVelocity); - } - - void Step(Settings& settings) override - { - Test::Step(settings); - - extern B2_API int32 b2_gjkCalls, b2_gjkIters, b2_gjkMaxIters; - - if (b2_gjkCalls > 0) - { - DrawString(5, m_textLine, "gjk calls = %d, ave gjk iters = %3.1f, max gjk iters = %d", b2_gjkCalls, - b2_gjkIters / float(b2_gjkCalls), b2_gjkMaxIters); - } - - extern B2_API int32 b2_toiCalls, b2_toiIters; - extern B2_API int32 b2_toiRootIters, b2_toiMaxRootIters; - extern B2_API float b2_toiTime, b2_toiMaxTime; - - if (b2_toiCalls > 0) - { - DrawString(5, m_textLine, "toi calls = %d, ave [max] toi iters = %3.1f [%d]", b2_toiCalls, - b2_toiIters / float(b2_toiCalls), b2_toiMaxRootIters); - - DrawString(5, m_textLine, "ave [max] toi root iters = %3.1f [%d]", b2_toiRootIters / float(b2_toiCalls), - b2_toiMaxRootIters); - - DrawString(5, m_textLine, "ave [max] toi time = %.1f [%.1f] (microseconds)", - 1000.0f * b2_toiTime / float(b2_toiCalls), 1000.0f * b2_toiMaxTime); - } - - if (m_stepCount % 60 == 0) - { - Launch(); - } - } - - static Test* Create() { return new ContinuousTest; } - - b2Body* m_body; - float m_angularVelocity; -}; - -static int testIndex = RegisterTest("Continuous", "Continuous Test", ContinuousTest::Create); diff --git a/tests/cpp-tests/Source/Box2DTestBed/tests/convex_hull.cpp b/tests/cpp-tests/Source/Box2DTestBed/tests/convex_hull.cpp deleted file mode 100644 index 44f4da8906e5..000000000000 --- a/tests/cpp-tests/Source/Box2DTestBed/tests/convex_hull.cpp +++ /dev/null @@ -1,108 +0,0 @@ -// MIT License - -// Copyright (c) 2019 Erin Catto - -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: - -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. - -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -#include "test.h" - -class ConvexHull : public Test -{ -public: - enum - { - e_count = b2_maxPolygonVertices - }; - - ConvexHull() - { - Generate(); - m_auto = false; - } - - void Generate() - { - b2Vec2 lowerBound(-8.0f, -8.0f); - b2Vec2 upperBound(8.0f, 8.0f); - - for (int32 i = 0; i < e_count; ++i) - { - float x = 10.0f * RandomFloat(); - float y = 10.0f * RandomFloat(); - - // Clamp onto a square to help create collinearities. - // This will stress the convex hull algorithm. - b2Vec2 v(x, y); - v = b2Clamp(v, lowerBound, upperBound); - m_points[i] = v; - } - - m_count = e_count; - } - - void Keyboard(int key) override - { - switch (key) - { - case GLFW_KEY_A: - m_auto = !m_auto; - break; - - case GLFW_KEY_G: - Generate(); - break; - } - } - - void Step(Settings& settings) override - { - Test::Step(settings); - - b2PolygonShape shape; - shape.Set(m_points, m_count); - - DrawString(5, m_textLine, "Press g to generate a new random convex hull"); - - g_debugDraw.DrawPolygon(shape.m_vertices, shape.m_count, b2Color(0.9f, 0.9f, 0.9f)); - - for (int32 i = 0; i < m_count; ++i) - { - g_debugDraw.DrawPoint(m_points[i], 3.0f, b2Color(0.3f, 0.9f, 0.3f)); - DrawString(m_points[i] + b2Vec2(0.05f, 0.05f), "%d", i); - } - - if (shape.Validate() == false) - { - m_textLine += 0; - } - - if (m_auto) - { - Generate(); - } - } - - static Test* Create() { return new ConvexHull; } - - b2Vec2 m_points[b2_maxPolygonVertices]; - int32 m_count; - bool m_auto; -}; - -static int testIndex = RegisterTest("Geometry", "Convex Hull", ConvexHull::Create); diff --git a/tests/cpp-tests/Source/Box2DTestBed/tests/conveyor_belt.cpp b/tests/cpp-tests/Source/Box2DTestBed/tests/conveyor_belt.cpp deleted file mode 100644 index 967ff5826369..000000000000 --- a/tests/cpp-tests/Source/Box2DTestBed/tests/conveyor_belt.cpp +++ /dev/null @@ -1,94 +0,0 @@ -// MIT License - -// Copyright (c) 2019 Erin Catto - -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: - -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. - -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -#include "test.h" - -class ConveyorBelt : public Test -{ -public: - ConveyorBelt() - { - // Ground - { - b2BodyDef bd; - b2Body* ground = m_world->CreateBody(&bd); - - b2EdgeShape shape; - shape.SetTwoSided(b2Vec2(-20.0f, 0.0f), b2Vec2(20.0f, 0.0f)); - ground->CreateFixture(&shape, 0.0f); - } - - // Platform - { - b2BodyDef bd; - bd.position.Set(-5.0f, 5.0f); - b2Body* body = m_world->CreateBody(&bd); - - b2PolygonShape shape; - shape.SetAsBox(10.0f, 0.5f); - - b2FixtureDef fd; - fd.shape = &shape; - fd.friction = 0.8f; - m_platform = body->CreateFixture(&fd); - } - - // Boxes - for (int32 i = 0; i < 5; ++i) - { - b2BodyDef bd; - bd.type = b2_dynamicBody; - bd.position.Set(-10.0f + 2.0f * i, 7.0f); - b2Body* body = m_world->CreateBody(&bd); - - b2PolygonShape shape; - shape.SetAsBox(0.5f, 0.5f); - body->CreateFixture(&shape, 20.0f); - } - } - - void PreSolve(b2Contact* contact, const b2Manifold* oldManifold) override - { - Test::PreSolve(contact, oldManifold); - - b2Fixture* fixtureA = contact->GetFixtureA(); - b2Fixture* fixtureB = contact->GetFixtureB(); - - if (fixtureA == m_platform) - { - contact->SetTangentSpeed(5.0f); - } - - if (fixtureB == m_platform) - { - contact->SetTangentSpeed(-5.0f); - } - } - - void Step(Settings& settings) override { Test::Step(settings); } - - static Test* Create() { return new ConveyorBelt; } - - b2Fixture* m_platform; -}; - -static int testIndex = RegisterTest("Examples", "Conveyor Belt", ConveyorBelt::Create); diff --git a/tests/cpp-tests/Source/Box2DTestBed/tests/distance_joint.cpp b/tests/cpp-tests/Source/Box2DTestBed/tests/distance_joint.cpp deleted file mode 100644 index 96eded3cf53c..000000000000 --- a/tests/cpp-tests/Source/Box2DTestBed/tests/distance_joint.cpp +++ /dev/null @@ -1,120 +0,0 @@ -// MIT License - -// Copyright (c) 2019 Erin Catto - -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: - -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. - -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -#include "test.h" -#include "ImGui/ImGuiPresenter.h" - -// This tests distance joints, body destruction, and joint destruction. -class DistanceJoint : public Test -{ -public: - DistanceJoint() - { - b2Body* ground = NULL; - { - b2BodyDef bd; - ground = m_world->CreateBody(&bd); - - b2EdgeShape shape; - shape.SetTwoSided(b2Vec2(-40.0f, 0.0f), b2Vec2(40.0f, 0.0f)); - ground->CreateFixture(&shape, 0.0f); - } - - { - b2BodyDef bd; - bd.type = b2_dynamicBody; - bd.angularDamping = 0.1f; - - bd.position.Set(0.0f, 5.0f); - b2Body* body = m_world->CreateBody(&bd); - - b2PolygonShape shape; - shape.SetAsBox(0.5f, 0.5f); - body->CreateFixture(&shape, 5.0f); - - m_hertz = 1.0f; - m_dampingRatio = 0.7f; - - b2DistanceJointDef jd; - jd.Initialize(ground, body, b2Vec2(0.0f, 15.0f), bd.position); - jd.collideConnected = true; - m_length = jd.length; - m_minLength = m_length; - m_maxLength = m_length; - b2LinearStiffness(jd.stiffness, jd.damping, m_hertz, m_dampingRatio, jd.bodyA, jd.bodyB); - m_joint = (b2DistanceJoint*)m_world->CreateJoint(&jd); - } - } - - void UpdateUI() override - { - // ImGui::SetNextWindowPos(ImVec2(10.0f, 100.0f)); - ImGui::SetNextWindowSize(ImVec2(260.0f, 150.0f)); - ImGui::Begin("Joint Controls", nullptr, ImGuiWindowFlags_NoResize); - - if (ImGui::SliderFloat("Length", &m_length, 0.0f, 20.0f, "%.0f")) - { - m_length = m_joint->SetLength(m_length); - } - - if (ImGui::SliderFloat("Min Length", &m_minLength, 0.0f, 20.0f, "%.0f")) - { - m_minLength = m_joint->SetMinLength(m_minLength); - } - - if (ImGui::SliderFloat("Max Length", &m_maxLength, 0.0f, 20.0f, "%.0f")) - { - m_maxLength = m_joint->SetMaxLength(m_maxLength); - } - - if (ImGui::SliderFloat("Hertz", &m_hertz, 0.0f, 10.0f, "%.1f")) - { - float stiffness; - float damping; - b2LinearStiffness(stiffness, damping, m_hertz, m_dampingRatio, m_joint->GetBodyA(), m_joint->GetBodyB()); - m_joint->SetStiffness(stiffness); - m_joint->SetDamping(damping); - } - - if (ImGui::SliderFloat("Damping Ratio", &m_dampingRatio, 0.0f, 2.0f, "%.1f")) - { - float stiffness; - float damping; - b2LinearStiffness(stiffness, damping, m_hertz, m_dampingRatio, m_joint->GetBodyA(), m_joint->GetBodyB()); - m_joint->SetStiffness(stiffness); - m_joint->SetDamping(damping); - } - - ImGui::End(); - } - - static Test* Create() { return new DistanceJoint; } - - b2DistanceJoint* m_joint; - float m_length; - float m_minLength; - float m_maxLength; - float m_hertz; - float m_dampingRatio; -}; - -static int testIndex = RegisterTest("Joints", "Distance Joint", DistanceJoint::Create); diff --git a/tests/cpp-tests/Source/Box2DTestBed/tests/distance_test.cpp b/tests/cpp-tests/Source/Box2DTestBed/tests/distance_test.cpp deleted file mode 100644 index 6372330426b0..000000000000 --- a/tests/cpp-tests/Source/Box2DTestBed/tests/distance_test.cpp +++ /dev/null @@ -1,134 +0,0 @@ -// MIT License - -// Copyright (c) 2019 Erin Catto - -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: - -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. - -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -#include "test.h" -#include "box2d/b2_distance.h" - -class DistanceTest : public Test -{ -public: - DistanceTest() - { - { - m_transformA.SetIdentity(); - m_transformA.p.Set(0.0f, -0.2f); - m_polygonA.SetAsBox(10.0f, 0.2f); - } - - { - m_positionB.Set(12.017401f, 0.13678508f); - m_angleB = -0.0109265f; - m_transformB.Set(m_positionB, m_angleB); - - m_polygonB.SetAsBox(2.0f, 0.1f); - } - } - - static Test* Create() { return new DistanceTest; } - - void Step(Settings& settings) override - { - Test::Step(settings); - - b2DistanceInput input; - input.proxyA.Set(&m_polygonA, 0); - input.proxyB.Set(&m_polygonB, 0); - input.transformA = m_transformA; - input.transformB = m_transformB; - input.useRadii = true; - b2SimplexCache cache; - cache.count = 0; - b2DistanceOutput output; - b2Distance(&output, &cache, &input); - - DrawString(5, m_textLine, "distance = %g", output.distance); - - DrawString(5, m_textLine, "iterations = %d", output.iterations); - - { - b2Color color(0.9f, 0.9f, 0.9f); - b2Vec2 v[b2_maxPolygonVertices]; - for (int32 i = 0; i < m_polygonA.m_count; ++i) - { - v[i] = b2Mul(m_transformA, m_polygonA.m_vertices[i]); - } - g_debugDraw.DrawPolygon(v, m_polygonA.m_count, color); - - for (int32 i = 0; i < m_polygonB.m_count; ++i) - { - v[i] = b2Mul(m_transformB, m_polygonB.m_vertices[i]); - } - g_debugDraw.DrawPolygon(v, m_polygonB.m_count, color); - } - - b2Vec2 x1 = output.pointA; - b2Vec2 x2 = output.pointB; - - b2Color c1(1.0f, 0.0f, 0.0f); - g_debugDraw.DrawPoint(x1, 4.0f, c1); - - b2Color c2(1.0f, 1.0f, 0.0f); - g_debugDraw.DrawPoint(x2, 4.0f, c2); - } - - void Keyboard(int key) override - { - switch (key) - { - case GLFW_KEY_A: - m_positionB.x -= 0.1f; - break; - - case GLFW_KEY_D: - m_positionB.x += 0.1f; - break; - - case GLFW_KEY_S: - m_positionB.y -= 0.1f; - break; - - case GLFW_KEY_W: - m_positionB.y += 0.1f; - break; - - case GLFW_KEY_Q: - m_angleB += 0.1f * b2_pi; - break; - - case GLFW_KEY_E: - m_angleB -= 0.1f * b2_pi; - break; - } - - m_transformB.Set(m_positionB, m_angleB); - } - - b2Vec2 m_positionB; - float m_angleB; - - b2Transform m_transformA; - b2Transform m_transformB; - b2PolygonShape m_polygonA; - b2PolygonShape m_polygonB; -}; - -static int testIndex = RegisterTest("Geometry", "Distance Test", DistanceTest::Create); diff --git a/tests/cpp-tests/Source/Box2DTestBed/tests/dominos.cpp b/tests/cpp-tests/Source/Box2DTestBed/tests/dominos.cpp deleted file mode 100644 index f64a141bd407..000000000000 --- a/tests/cpp-tests/Source/Box2DTestBed/tests/dominos.cpp +++ /dev/null @@ -1,216 +0,0 @@ -// MIT License - -// Copyright (c) 2019 Erin Catto - -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: - -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. - -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -#include "test.h" - -class Dominos : public Test -{ -public: - Dominos() - { - b2Body* b1; - { - b2EdgeShape shape; - shape.SetTwoSided(b2Vec2(-40.0f, 0.0f), b2Vec2(40.0f, 0.0f)); - - b2BodyDef bd; - b1 = m_world->CreateBody(&bd); - b1->CreateFixture(&shape, 0.0f); - } - - { - b2PolygonShape shape; - shape.SetAsBox(6.0f, 0.25f); - - b2BodyDef bd; - bd.position.Set(-1.5f, 10.0f); - b2Body* ground = m_world->CreateBody(&bd); - ground->CreateFixture(&shape, 0.0f); - } - - { - b2PolygonShape shape; - shape.SetAsBox(0.1f, 1.0f); - - b2FixtureDef fd; - fd.shape = &shape; - fd.density = 20.0f; - fd.friction = 0.1f; - - for (int i = 0; i < 10; ++i) - { - b2BodyDef bd; - bd.type = b2_dynamicBody; - bd.position.Set(-6.0f + 1.0f * i, 11.25f); - b2Body* body = m_world->CreateBody(&bd); - body->CreateFixture(&fd); - } - } - - { - b2PolygonShape shape; - shape.SetAsBox(7.0f, 0.25f, b2Vec2_zero, 0.3f); - - b2BodyDef bd; - bd.position.Set(1.0f, 6.0f); - b2Body* ground = m_world->CreateBody(&bd); - ground->CreateFixture(&shape, 0.0f); - } - - b2Body* b2; - { - b2PolygonShape shape; - shape.SetAsBox(0.25f, 1.5f); - - b2BodyDef bd; - bd.position.Set(-7.0f, 4.0f); - b2 = m_world->CreateBody(&bd); - b2->CreateFixture(&shape, 0.0f); - } - - b2Body* b3; - { - b2PolygonShape shape; - shape.SetAsBox(6.0f, 0.125f); - - b2BodyDef bd; - bd.type = b2_dynamicBody; - bd.position.Set(-0.9f, 1.0f); - bd.angle = -0.15f; - - b3 = m_world->CreateBody(&bd); - b3->CreateFixture(&shape, 10.0f); - } - - b2RevoluteJointDef jd; - b2Vec2 anchor; - - anchor.Set(-2.0f, 1.0f); - jd.Initialize(b1, b3, anchor); - jd.collideConnected = true; - m_world->CreateJoint(&jd); - - b2Body* b4; - { - b2PolygonShape shape; - shape.SetAsBox(0.25f, 0.25f); - - b2BodyDef bd; - bd.type = b2_dynamicBody; - bd.position.Set(-10.0f, 15.0f); - b4 = m_world->CreateBody(&bd); - b4->CreateFixture(&shape, 10.0f); - } - - anchor.Set(-7.0f, 15.0f); - jd.Initialize(b2, b4, anchor); - m_world->CreateJoint(&jd); - - b2Body* b5; - { - b2BodyDef bd; - bd.type = b2_dynamicBody; - bd.position.Set(6.5f, 3.0f); - b5 = m_world->CreateBody(&bd); - - b2PolygonShape shape; - b2FixtureDef fd; - - fd.shape = &shape; - fd.density = 10.0f; - fd.friction = 0.1f; - - shape.SetAsBox(1.0f, 0.1f, b2Vec2(0.0f, -0.9f), 0.0f); - b5->CreateFixture(&fd); - - shape.SetAsBox(0.1f, 1.0f, b2Vec2(-0.9f, 0.0f), 0.0f); - b5->CreateFixture(&fd); - - shape.SetAsBox(0.1f, 1.0f, b2Vec2(0.9f, 0.0f), 0.0f); - b5->CreateFixture(&fd); - } - - anchor.Set(6.0f, 2.0f); - jd.Initialize(b1, b5, anchor); - m_world->CreateJoint(&jd); - - b2Body* b6; - { - b2PolygonShape shape; - shape.SetAsBox(1.0f, 0.1f); - - b2BodyDef bd; - bd.type = b2_dynamicBody; - bd.position.Set(6.5f, 4.1f); - b6 = m_world->CreateBody(&bd); - b6->CreateFixture(&shape, 30.0f); - } - - anchor.Set(7.5f, 4.0f); - jd.Initialize(b5, b6, anchor); - m_world->CreateJoint(&jd); - - b2Body* b7; - { - b2PolygonShape shape; - shape.SetAsBox(0.1f, 1.0f); - - b2BodyDef bd; - bd.type = b2_dynamicBody; - bd.position.Set(7.4f, 1.0f); - - b7 = m_world->CreateBody(&bd); - b7->CreateFixture(&shape, 10.0f); - } - - b2DistanceJointDef djd; - djd.bodyA = b3; - djd.bodyB = b7; - djd.localAnchorA.Set(6.0f, 0.0f); - djd.localAnchorB.Set(0.0f, -1.0f); - b2Vec2 d = djd.bodyB->GetWorldPoint(djd.localAnchorB) - djd.bodyA->GetWorldPoint(djd.localAnchorA); - djd.length = d.Length(); - - b2LinearStiffness(djd.stiffness, djd.damping, 1.0f, 1.0f, djd.bodyA, djd.bodyB); - m_world->CreateJoint(&djd); - - { - float radius = 0.2f; - - b2CircleShape shape; - shape.m_radius = radius; - - for (int32 i = 0; i < 4; ++i) - { - b2BodyDef bd; - bd.type = b2_dynamicBody; - bd.position.Set(5.9f + 2.0f * radius * i, 2.4f); - b2Body* body = m_world->CreateBody(&bd); - body->CreateFixture(&shape, 10.0f); - } - } - } - - static Test* Create() { return new Dominos; } -}; - -static int testIndex = RegisterTest("Examples", "Dominos", Dominos::Create); diff --git a/tests/cpp-tests/Source/Box2DTestBed/tests/dump_loader.cpp b/tests/cpp-tests/Source/Box2DTestBed/tests/dump_loader.cpp deleted file mode 100644 index b874a2a174ef..000000000000 --- a/tests/cpp-tests/Source/Box2DTestBed/tests/dump_loader.cpp +++ /dev/null @@ -1,82 +0,0 @@ -// MIT License - -// Copyright (c) 2019 Erin Catto - -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: - -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. - -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -#include "test.h" - -// This test holds worlds dumped using b2World::Dump. -class DumpLoader : public Test -{ -public: - DumpLoader() - { - b2ChainShape chainShape; - b2Vec2 vertices[] = {b2Vec2(-5, 0), b2Vec2(5, 0), b2Vec2(5, 5), b2Vec2(4, 1), b2Vec2(-4, 1), b2Vec2(-5, 5)}; - chainShape.CreateLoop(vertices, 6); - - b2FixtureDef groundFixtureDef; - groundFixtureDef.density = 0; - groundFixtureDef.shape = &chainShape; - - b2BodyDef groundBodyDef; - groundBodyDef.type = b2_staticBody; - - b2Body* groundBody = m_world->CreateBody(&groundBodyDef); - b2Fixture* groundBodyFixture = groundBody->CreateFixture(&groundFixtureDef); - - b2CircleShape ballShape; - ballShape.m_radius = 1; - - b2FixtureDef ballFixtureDef; - ballFixtureDef.restitution = 0.75f; - ballFixtureDef.density = 1; - ballFixtureDef.shape = &ballShape; - - b2BodyDef ballBodyDef; - ballBodyDef.type = b2BodyType::b2_dynamicBody; - ballBodyDef.position = b2Vec2(0, 10); - // ballBodyDef.angularDamping = 0.2f; - - m_ball = m_world->CreateBody(&ballBodyDef); - b2Fixture* ballFixture = m_ball->CreateFixture(&ballFixtureDef); - m_ball->ApplyForceToCenter(b2Vec2(-1000, -400), true); - } - - void Step(Settings& settings) override - { - b2Vec2 v = m_ball->GetLinearVelocity(); - float omega = m_ball->GetAngularVelocity(); - - b2MassData massData = m_ball->GetMassData(); - - float ke = 0.5f * massData.mass * b2Dot(v, v) + 0.5f * massData.I * omega * omega; - - DrawString(5, m_textLine, "kinetic energy = %.6f", ke); - - Test::Step(settings); - } - - static Test* Create() { return new DumpLoader; } - - b2Body* m_ball; -}; - -static int testIndex = RegisterTest("Bugs", "Dump Loader", DumpLoader::Create); diff --git a/tests/cpp-tests/Source/Box2DTestBed/tests/dynamic_tree.cpp b/tests/cpp-tests/Source/Box2DTestBed/tests/dynamic_tree.cpp deleted file mode 100644 index bd2de4d80a0f..000000000000 --- a/tests/cpp-tests/Source/Box2DTestBed/tests/dynamic_tree.cpp +++ /dev/null @@ -1,357 +0,0 @@ -// MIT License - -// Copyright (c) 2019 Erin Catto - -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: - -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. - -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -#include "test.h" - -class DynamicTree : public Test -{ -public: - enum - { - e_actorCount = 128 - }; - - DynamicTree() - { - m_worldExtent = 15.0f; - m_proxyExtent = 0.5f; - - srand(888); - - for (int32 i = 0; i < e_actorCount; ++i) - { - Actor* actor = m_actors + i; - GetRandomAABB(&actor->aabb); - actor->proxyId = m_tree.CreateProxy(actor->aabb, actor); - } - - m_stepCount = 0; - - float h = m_worldExtent; - m_queryAABB.lowerBound.Set(-3.0f, -4.0f + h); - m_queryAABB.upperBound.Set(5.0f, 6.0f + h); - - m_rayCastInput.p1.Set(-5.0, 5.0f + h); - m_rayCastInput.p2.Set(7.0f, -4.0f + h); - // m_rayCastInput.p1.Set(0.0f, 2.0f + h); - // m_rayCastInput.p2.Set(0.0f, -2.0f + h); - m_rayCastInput.maxFraction = 1.0f; - - m_automated = false; - } - - static Test* Create() { return new DynamicTree; } - - void Step(Settings& settings) override - { - B2_NOT_USED(settings); - - m_rayActor = NULL; - for (int32 i = 0; i < e_actorCount; ++i) - { - m_actors[i].fraction = 1.0f; - m_actors[i].overlap = false; - } - - if (m_automated == true) - { - int32 actionCount = b2Max(1, e_actorCount >> 2); - - for (int32 i = 0; i < actionCount; ++i) - { - Action(); - } - } - - Query(); - RayCast(); - - for (int32 i = 0; i < e_actorCount; ++i) - { - Actor* actor = m_actors + i; - if (actor->proxyId == b2_nullNode) - continue; - - b2Color c(0.9f, 0.9f, 0.9f); - if (actor == m_rayActor && actor->overlap) - { - c.Set(0.9f, 0.6f, 0.6f); - } - else if (actor == m_rayActor) - { - c.Set(0.6f, 0.9f, 0.6f); - } - else if (actor->overlap) - { - c.Set(0.6f, 0.6f, 0.9f); - } - - DrawAABB(&actor->aabb, c); - } - - b2Color c(0.7f, 0.7f, 0.7f); - DrawAABB(&m_queryAABB, c); - - g_debugDraw.DrawSegment(m_rayCastInput.p1, m_rayCastInput.p2, c); - - b2Color c1(0.2f, 0.9f, 0.2f); - b2Color c2(0.9f, 0.2f, 0.2f); - g_debugDraw.DrawPoint(m_rayCastInput.p1, 6.0f, c1); - g_debugDraw.DrawPoint(m_rayCastInput.p2, 6.0f, c2); - - if (m_rayActor) - { - b2Color cr(0.2f, 0.2f, 0.9f); - b2Vec2 p = m_rayCastInput.p1 + m_rayActor->fraction * (m_rayCastInput.p2 - m_rayCastInput.p1); - g_debugDraw.DrawPoint(p, 6.0f, cr); - } - - { - int32 height = m_tree.GetHeight(); - DrawString(5, m_textLine, "dynamic tree height = %d", height); - } - - ++m_stepCount; - } - - void Keyboard(int key) override - { - switch (key) - { - case GLFW_KEY_A: - m_automated = !m_automated; - break; - - case GLFW_KEY_C: - CreateProxy(); - break; - - case GLFW_KEY_D: - DestroyProxy(); - break; - - case GLFW_KEY_M: - MoveProxy(); - break; - } - } - - bool QueryCallback(int32 proxyId) - { - Actor* actor = (Actor*)m_tree.GetUserData(proxyId); - actor->overlap = b2TestOverlap(m_queryAABB, actor->aabb); - return true; - } - - float RayCastCallback(const b2RayCastInput& input, int32 proxyId) - { - Actor* actor = (Actor*)m_tree.GetUserData(proxyId); - - b2RayCastOutput output; - bool hit = actor->aabb.RayCast(&output, input); - - if (hit) - { - m_rayCastOutput = output; - m_rayActor = actor; - m_rayActor->fraction = output.fraction; - return output.fraction; - } - - return input.maxFraction; - } - -private: - struct Actor - { - b2AABB aabb; - float fraction; - bool overlap; - int32 proxyId; - }; - - void GetRandomAABB(b2AABB* aabb) - { - b2Vec2 w; - w.Set(2.0f * m_proxyExtent, 2.0f * m_proxyExtent); - // aabb->lowerBound.x = -m_proxyExtent; - // aabb->lowerBound.y = -m_proxyExtent + m_worldExtent; - aabb->lowerBound.x = RandomFloat(-m_worldExtent, m_worldExtent); - aabb->lowerBound.y = RandomFloat(0.0f, 2.0f * m_worldExtent); - aabb->upperBound = aabb->lowerBound + w; - } - - void MoveAABB(b2AABB* aabb) - { - b2Vec2 d; - d.x = RandomFloat(-0.5f, 0.5f); - d.y = RandomFloat(-0.5f, 0.5f); - // d.x = 2.0f; - // d.y = 0.0f; - aabb->lowerBound += d; - aabb->upperBound += d; - - b2Vec2 c0 = 0.5f * (aabb->lowerBound + aabb->upperBound); - b2Vec2 min; - min.Set(-m_worldExtent, 0.0f); - b2Vec2 max; - max.Set(m_worldExtent, 2.0f * m_worldExtent); - b2Vec2 c = b2Clamp(c0, min, max); - - aabb->lowerBound += c - c0; - aabb->upperBound += c - c0; - } - - void CreateProxy() - { - for (int32 i = 0; i < e_actorCount; ++i) - { - int32 j = rand() % e_actorCount; - Actor* actor = m_actors + j; - if (actor->proxyId == b2_nullNode) - { - GetRandomAABB(&actor->aabb); - actor->proxyId = m_tree.CreateProxy(actor->aabb, actor); - return; - } - } - } - - void DestroyProxy() - { - for (int32 i = 0; i < e_actorCount; ++i) - { - int32 j = rand() % e_actorCount; - Actor* actor = m_actors + j; - if (actor->proxyId != b2_nullNode) - { - m_tree.DestroyProxy(actor->proxyId); - actor->proxyId = b2_nullNode; - return; - } - } - } - - void MoveProxy() - { - for (int32 i = 0; i < e_actorCount; ++i) - { - int32 j = rand() % e_actorCount; - Actor* actor = m_actors + j; - if (actor->proxyId == b2_nullNode) - { - continue; - } - - b2AABB aabb0 = actor->aabb; - MoveAABB(&actor->aabb); - b2Vec2 displacement = actor->aabb.GetCenter() - aabb0.GetCenter(); - m_tree.MoveProxy(actor->proxyId, actor->aabb, displacement); - return; - } - } - - void Action() - { - int32 choice = rand() % 20; - - switch (choice) - { - case 0: - CreateProxy(); - break; - - case 1: - DestroyProxy(); - break; - - default: - MoveProxy(); - } - } - - void Query() - { - m_tree.Query(this, m_queryAABB); - - for (int32 i = 0; i < e_actorCount; ++i) - { - if (m_actors[i].proxyId == b2_nullNode) - { - continue; - } - - bool overlap = b2TestOverlap(m_queryAABB, m_actors[i].aabb); - B2_NOT_USED(overlap); - b2Assert(overlap == m_actors[i].overlap); - } - } - - void RayCast() - { - m_rayActor = NULL; - - b2RayCastInput input = m_rayCastInput; - - // Ray cast against the dynamic tree. - m_tree.RayCast(this, input); - - // Brute force ray cast. - Actor* bruteActor = NULL; - b2RayCastOutput bruteOutput; - for (int32 i = 0; i < e_actorCount; ++i) - { - if (m_actors[i].proxyId == b2_nullNode) - { - continue; - } - - b2RayCastOutput output; - bool hit = m_actors[i].aabb.RayCast(&output, input); - if (hit) - { - bruteActor = m_actors + i; - bruteOutput = output; - input.maxFraction = output.fraction; - } - } - - if (bruteActor != NULL) - { - b2Assert(bruteOutput.fraction == m_rayCastOutput.fraction); - } - } - - float m_worldExtent; - float m_proxyExtent; - - b2DynamicTree m_tree; - b2AABB m_queryAABB; - b2RayCastInput m_rayCastInput; - b2RayCastOutput m_rayCastOutput; - Actor* m_rayActor; - Actor m_actors[e_actorCount]; - int32 m_stepCount; - bool m_automated; -}; - -static int testIndex = RegisterTest("Collision", "Dynamic Tree", DynamicTree::Create); diff --git a/tests/cpp-tests/Source/Box2DTestBed/tests/edge_shapes.cpp b/tests/cpp-tests/Source/Box2DTestBed/tests/edge_shapes.cpp deleted file mode 100644 index f7416dc4373d..000000000000 --- a/tests/cpp-tests/Source/Box2DTestBed/tests/edge_shapes.cpp +++ /dev/null @@ -1,244 +0,0 @@ -// MIT License - -// Copyright (c) 2019 Erin Catto - -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: - -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. - -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -#include "settings.h" -#include "test.h" - -class EdgeShapesCallback : public b2RayCastCallback -{ -public: - EdgeShapesCallback() { m_fixture = NULL; } - - float ReportFixture(b2Fixture* fixture, const b2Vec2& point, const b2Vec2& normal, float fraction) override - { - m_fixture = fixture; - m_point = point; - m_normal = normal; - - return fraction; - } - - b2Fixture* m_fixture; - b2Vec2 m_point; - b2Vec2 m_normal; -}; - -class EdgeShapes : public Test -{ -public: - enum - { - e_maxBodies = 256 - }; - - EdgeShapes() - { - // Ground body - { - b2BodyDef bd; - b2Body* ground = m_world->CreateBody(&bd); - - float x1 = -20.0f; - float y1 = 2.0f * cosf(x1 / 10.0f * b2_pi); - for (int32 i = 0; i < 80; ++i) - { - float x2 = x1 + 0.5f; - float y2 = 2.0f * cosf(x2 / 10.0f * b2_pi); - - b2EdgeShape shape; - shape.SetTwoSided(b2Vec2(x1, y1), b2Vec2(x2, y2)); - ground->CreateFixture(&shape, 0.0f); - - x1 = x2; - y1 = y2; - } - } - - { - b2Vec2 vertices[3]; - vertices[0].Set(-0.5f, 0.0f); - vertices[1].Set(0.5f, 0.0f); - vertices[2].Set(0.0f, 1.5f); - m_polygons[0].Set(vertices, 3); - } - - { - b2Vec2 vertices[3]; - vertices[0].Set(-0.1f, 0.0f); - vertices[1].Set(0.1f, 0.0f); - vertices[2].Set(0.0f, 1.5f); - m_polygons[1].Set(vertices, 3); - } - - { - float w = 1.0f; - float b = w / (2.0f + b2Sqrt(2.0f)); - float s = b2Sqrt(2.0f) * b; - - b2Vec2 vertices[8]; - vertices[0].Set(0.5f * s, 0.0f); - vertices[1].Set(0.5f * w, b); - vertices[2].Set(0.5f * w, b + s); - vertices[3].Set(0.5f * s, w); - vertices[4].Set(-0.5f * s, w); - vertices[5].Set(-0.5f * w, b + s); - vertices[6].Set(-0.5f * w, b); - vertices[7].Set(-0.5f * s, 0.0f); - - m_polygons[2].Set(vertices, 8); - } - - { - m_polygons[3].SetAsBox(0.5f, 0.5f); - } - - { - m_circle.m_radius = 0.5f; - } - - m_bodyIndex = 0; - memset(m_bodies, 0, sizeof(m_bodies)); - - m_angle = 0.0f; - } - - void Create(int32 index) - { - if (m_bodies[m_bodyIndex] != NULL) - { - m_world->DestroyBody(m_bodies[m_bodyIndex]); - m_bodies[m_bodyIndex] = NULL; - } - - b2BodyDef bd; - - float x = RandomFloat(-10.0f, 10.0f); - float y = RandomFloat(10.0f, 20.0f); - bd.position.Set(x, y); - bd.angle = RandomFloat(-b2_pi, b2_pi); - bd.type = b2_dynamicBody; - - if (index == 4) - { - bd.angularDamping = 0.02f; - } - - m_bodies[m_bodyIndex] = m_world->CreateBody(&bd); - - if (index < 4) - { - b2FixtureDef fd; - fd.shape = m_polygons + index; - fd.friction = 0.3f; - fd.density = 20.0f; - m_bodies[m_bodyIndex]->CreateFixture(&fd); - } - else - { - b2FixtureDef fd; - fd.shape = &m_circle; - fd.friction = 0.3f; - fd.density = 20.0f; - m_bodies[m_bodyIndex]->CreateFixture(&fd); - } - - m_bodyIndex = (m_bodyIndex + 1) % e_maxBodies; - } - - void DestroyBody() - { - for (int32 i = 0; i < e_maxBodies; ++i) - { - if (m_bodies[i] != NULL) - { - m_world->DestroyBody(m_bodies[i]); - m_bodies[i] = NULL; - return; - } - } - } - - void Keyboard(int key) override - { - switch (key) - { - case GLFW_KEY_1: - case GLFW_KEY_2: - case GLFW_KEY_3: - case GLFW_KEY_4: - case GLFW_KEY_5: - Create(key - GLFW_KEY_1); - break; - - case GLFW_KEY_D: - DestroyBody(); - break; - } - } - - void Step(Settings& settings) override - { - bool advanceRay = settings.m_pause == 0 || settings.m_singleStep; - - Test::Step(settings); - DrawString(5, m_textLine, "Press 1-5 to drop stuff"); - - float L = 25.0f; - b2Vec2 point1(0.0f, 10.0f); - b2Vec2 d(L * cosf(m_angle), -L * b2Abs(sinf(m_angle))); - b2Vec2 point2 = point1 + d; - - EdgeShapesCallback callback; - - m_world->RayCast(&callback, point1, point2); - - if (callback.m_fixture) - { - g_debugDraw.DrawPoint(callback.m_point, 5.0f, b2Color(0.4f, 0.9f, 0.4f)); - - g_debugDraw.DrawSegment(point1, callback.m_point, b2Color(0.8f, 0.8f, 0.8f)); - - b2Vec2 head = callback.m_point + 0.5f * callback.m_normal; - g_debugDraw.DrawSegment(callback.m_point, head, b2Color(0.9f, 0.9f, 0.4f)); - } - else - { - g_debugDraw.DrawSegment(point1, point2, b2Color(0.8f, 0.8f, 0.8f)); - } - - if (advanceRay) - { - m_angle += 0.25f * b2_pi / 180.0f; - } - } - - static Test* Create() { return new EdgeShapes; } - - int32 m_bodyIndex; - b2Body* m_bodies[e_maxBodies]; - b2PolygonShape m_polygons[4]; - b2CircleShape m_circle; - - float m_angle; -}; - -static int testIndex = RegisterTest("Geometry", "Edge Shapes", EdgeShapes::Create); diff --git a/tests/cpp-tests/Source/Box2DTestBed/tests/edge_test.cpp b/tests/cpp-tests/Source/Box2DTestBed/tests/edge_test.cpp deleted file mode 100644 index 6cd086c3972c..000000000000 --- a/tests/cpp-tests/Source/Box2DTestBed/tests/edge_test.cpp +++ /dev/null @@ -1,267 +0,0 @@ -// MIT License - -// Copyright (c) 2019 Erin Catto - -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: - -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. - -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -#include "test.h" -#include "ImGui/ImGuiPresenter.h" - -class EdgeTest : public Test -{ -public: - EdgeTest() - { - b2Vec2 vertices[10] = {{10.0f, -4.0f}, {10.0f, 0.0f}, {6.0f, 0.0f}, {4.0f, 2.0f}, {2.0f, 0.0f}, - {-2.0f, 0.0f}, {-6.0f, 0.0f}, {-8.0f, -3.0f}, {-10.0f, 0.0f}, {-10.0f, -4.0f}}; - - m_offset1.Set(0.0f, 8.0f); - m_offset2.Set(0.0f, 16.0f); - - { - b2Vec2 v1 = vertices[0] + m_offset1; - b2Vec2 v2 = vertices[1] + m_offset1; - b2Vec2 v3 = vertices[2] + m_offset1; - b2Vec2 v4 = vertices[3] + m_offset1; - b2Vec2 v5 = vertices[4] + m_offset1; - b2Vec2 v6 = vertices[5] + m_offset1; - b2Vec2 v7 = vertices[6] + m_offset1; - b2Vec2 v8 = vertices[7] + m_offset1; - b2Vec2 v9 = vertices[8] + m_offset1; - b2Vec2 v10 = vertices[9] + m_offset1; - - b2BodyDef bd; - b2Body* ground = m_world->CreateBody(&bd); - - b2EdgeShape shape; - - shape.SetOneSided(v10, v1, v2, v3); - ground->CreateFixture(&shape, 0.0f); - - shape.SetOneSided(v1, v2, v3, v4); - ground->CreateFixture(&shape, 0.0f); - - shape.SetOneSided(v2, v3, v4, v5); - ground->CreateFixture(&shape, 0.0f); - - shape.SetOneSided(v3, v4, v5, v6); - ground->CreateFixture(&shape, 0.0f); - - shape.SetOneSided(v4, v5, v6, v7); - ground->CreateFixture(&shape, 0.0f); - - shape.SetOneSided(v5, v6, v7, v8); - ground->CreateFixture(&shape, 0.0f); - - shape.SetOneSided(v6, v7, v8, v9); - ground->CreateFixture(&shape, 0.0f); - - shape.SetOneSided(v7, v8, v9, v10); - ground->CreateFixture(&shape, 0.0f); - - shape.SetOneSided(v8, v9, v10, v1); - ground->CreateFixture(&shape, 0.0f); - - shape.SetOneSided(v9, v10, v1, v2); - ground->CreateFixture(&shape, 0.0f); - } - - { - b2Vec2 v1 = vertices[0] + m_offset2; - b2Vec2 v2 = vertices[1] + m_offset2; - b2Vec2 v3 = vertices[2] + m_offset2; - b2Vec2 v4 = vertices[3] + m_offset2; - b2Vec2 v5 = vertices[4] + m_offset2; - b2Vec2 v6 = vertices[5] + m_offset2; - b2Vec2 v7 = vertices[6] + m_offset2; - b2Vec2 v8 = vertices[7] + m_offset2; - b2Vec2 v9 = vertices[8] + m_offset2; - b2Vec2 v10 = vertices[9] + m_offset2; - - b2BodyDef bd; - b2Body* ground = m_world->CreateBody(&bd); - - b2EdgeShape shape; - - shape.SetTwoSided(v1, v2); - ground->CreateFixture(&shape, 0.0f); - - shape.SetTwoSided(v2, v3); - ground->CreateFixture(&shape, 0.0f); - - shape.SetTwoSided(v3, v4); - ground->CreateFixture(&shape, 0.0f); - - shape.SetTwoSided(v4, v5); - ground->CreateFixture(&shape, 0.0f); - - shape.SetTwoSided(v5, v6); - ground->CreateFixture(&shape, 0.0f); - - shape.SetTwoSided(v6, v7); - ground->CreateFixture(&shape, 0.0f); - - shape.SetTwoSided(v7, v8); - ground->CreateFixture(&shape, 0.0f); - - shape.SetTwoSided(v8, v9); - ground->CreateFixture(&shape, 0.0f); - - shape.SetTwoSided(v9, v10); - ground->CreateFixture(&shape, 0.0f); - - shape.SetTwoSided(v10, v1); - ground->CreateFixture(&shape, 0.0f); - } - - m_body1 = nullptr; - m_body2 = nullptr; - CreateBoxes(); - m_boxes = true; - } - - void CreateBoxes() - { - if (m_body1) - { - m_world->DestroyBody(m_body1); - m_body1 = nullptr; - } - - if (m_body2) - { - m_world->DestroyBody(m_body2); - m_body2 = nullptr; - } - - { - b2BodyDef bd; - bd.type = b2_dynamicBody; - bd.position = b2Vec2(8.0f, 2.6f) + m_offset1; - bd.allowSleep = false; - m_body1 = m_world->CreateBody(&bd); - - b2PolygonShape shape; - shape.SetAsBox(0.5f, 1.0f); - - m_body1->CreateFixture(&shape, 1.0f); - } - - { - b2BodyDef bd; - bd.type = b2_dynamicBody; - bd.position = b2Vec2(8.0f, 2.6f) + m_offset2; - bd.allowSleep = false; - m_body2 = m_world->CreateBody(&bd); - - b2PolygonShape shape; - shape.SetAsBox(0.5f, 1.0f); - - m_body2->CreateFixture(&shape, 1.0f); - } - } - - void CreateCircles() - { - if (m_body1) - { - m_world->DestroyBody(m_body1); - m_body1 = nullptr; - } - - if (m_body2) - { - m_world->DestroyBody(m_body2); - m_body2 = nullptr; - } - - { - b2BodyDef bd; - bd.type = b2_dynamicBody; - bd.position = b2Vec2(-0.5f, 0.6f) + m_offset1; - bd.allowSleep = false; - m_body1 = m_world->CreateBody(&bd); - - b2CircleShape shape; - shape.m_radius = 0.5f; - - m_body1->CreateFixture(&shape, 1.0f); - } - - { - b2BodyDef bd; - bd.type = b2_dynamicBody; - bd.position = b2Vec2(-0.5f, 0.6f) + m_offset2; - bd.allowSleep = false; - m_body2 = m_world->CreateBody(&bd); - - b2CircleShape shape; - shape.m_radius = 0.5f; - - m_body2->CreateFixture(&shape, 1.0f); - } - } - - void UpdateUI() override - { - // ImGui::SetNextWindowPos(ImVec2(10.0f, 100.0f)); - ImGui::SetNextWindowSize(ImVec2(200.0f, 100.0f)); - ImGui::Begin("Custom Controls", nullptr, ImGuiWindowFlags_NoResize); - - if (ImGui::RadioButton("Boxes", m_boxes == true)) - { - CreateBoxes(); - m_boxes = true; - } - - if (ImGui::RadioButton("Circles", m_boxes == false)) - { - CreateCircles(); - m_boxes = false; - } - - ImGui::End(); - } - - void Step(Settings& settings) override - { - // if (glfwGetKey(g_mainWindow, GLFW_KEY_A) == GLFW_PRESS) - //{ - // m_body1->ApplyForceToCenter(b2Vec2(-10.0f, 0.0f), true); - // m_body2->ApplyForceToCenter(b2Vec2(-10.0f, 0.0f), true); - // } - - // if (glfwGetKey(g_mainWindow, GLFW_KEY_D) == GLFW_PRESS) - //{ - // m_body1->ApplyForceToCenter(b2Vec2(10.0f, 0.0f), true); - // m_body2->ApplyForceToCenter(b2Vec2(10.0f, 0.0f), true); - // } - - Test::Step(settings); - } - - static Test* Create() { return new EdgeTest; } - - b2Vec2 m_offset1, m_offset2; - b2Body* m_body1; - b2Body* m_body2; - bool m_boxes; -}; - -static int testIndex = RegisterTest("Geometry", "Edge Test", EdgeTest::Create); diff --git a/tests/cpp-tests/Source/Box2DTestBed/tests/friction.cpp b/tests/cpp-tests/Source/Box2DTestBed/tests/friction.cpp deleted file mode 100644 index b935e0459c64..000000000000 --- a/tests/cpp-tests/Source/Box2DTestBed/tests/friction.cpp +++ /dev/null @@ -1,123 +0,0 @@ -// MIT License - -// Copyright (c) 2019 Erin Catto - -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: - -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. - -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -#include "test.h" - -class Friction : public Test -{ -public: - Friction() - { - { - b2BodyDef bd; - b2Body* ground = m_world->CreateBody(&bd); - - b2EdgeShape shape; - shape.SetTwoSided(b2Vec2(-40.0f, 0.0f), b2Vec2(40.0f, 0.0f)); - ground->CreateFixture(&shape, 0.0f); - } - - { - b2PolygonShape shape; - shape.SetAsBox(13.0f, 0.25f); - - b2BodyDef bd; - bd.position.Set(-4.0f, 22.0f); - bd.angle = -0.25f; - - b2Body* ground = m_world->CreateBody(&bd); - ground->CreateFixture(&shape, 0.0f); - } - - { - b2PolygonShape shape; - shape.SetAsBox(0.25f, 1.0f); - - b2BodyDef bd; - bd.position.Set(10.5f, 19.0f); - - b2Body* ground = m_world->CreateBody(&bd); - ground->CreateFixture(&shape, 0.0f); - } - - { - b2PolygonShape shape; - shape.SetAsBox(13.0f, 0.25f); - - b2BodyDef bd; - bd.position.Set(4.0f, 14.0f); - bd.angle = 0.25f; - - b2Body* ground = m_world->CreateBody(&bd); - ground->CreateFixture(&shape, 0.0f); - } - - { - b2PolygonShape shape; - shape.SetAsBox(0.25f, 1.0f); - - b2BodyDef bd; - bd.position.Set(-10.5f, 11.0f); - - b2Body* ground = m_world->CreateBody(&bd); - ground->CreateFixture(&shape, 0.0f); - } - - { - b2PolygonShape shape; - shape.SetAsBox(13.0f, 0.25f); - - b2BodyDef bd; - bd.position.Set(-4.0f, 6.0f); - bd.angle = -0.25f; - - b2Body* ground = m_world->CreateBody(&bd); - ground->CreateFixture(&shape, 0.0f); - } - - { - b2PolygonShape shape; - shape.SetAsBox(0.5f, 0.5f); - - b2FixtureDef fd; - fd.shape = &shape; - fd.density = 25.0f; - - float friction[5] = {0.75f, 0.5f, 0.35f, 0.1f, 0.0f}; - - for (int i = 0; i < 5; ++i) - { - b2BodyDef bd; - bd.type = b2_dynamicBody; - bd.position.Set(-15.0f + 4.0f * i, 28.0f); - b2Body* body = m_world->CreateBody(&bd); - - fd.friction = friction[i]; - body->CreateFixture(&fd); - } - } - } - - static Test* Create() { return new Friction; } -}; - -static int testIndex = RegisterTest("Forces", "Friction", Friction::Create); diff --git a/tests/cpp-tests/Source/Box2DTestBed/tests/gear_joint.cpp b/tests/cpp-tests/Source/Box2DTestBed/tests/gear_joint.cpp deleted file mode 100644 index d7e1b15771e2..000000000000 --- a/tests/cpp-tests/Source/Box2DTestBed/tests/gear_joint.cpp +++ /dev/null @@ -1,175 +0,0 @@ -// MIT License - -// Copyright (c) 2019 Erin Catto - -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: - -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. - -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -#include "test.h" - -class GearJoint : public Test -{ -public: - GearJoint() - { - b2Body* ground = NULL; - { - b2BodyDef bd; - ground = m_world->CreateBody(&bd); - - b2EdgeShape shape; - shape.SetTwoSided(b2Vec2(50.0f, 0.0f), b2Vec2(-50.0f, 0.0f)); - ground->CreateFixture(&shape, 0.0f); - } - - { - b2CircleShape circle1; - circle1.m_radius = 1.0f; - - b2PolygonShape box; - box.SetAsBox(0.5f, 5.0f); - - b2CircleShape circle2; - circle2.m_radius = 2.0f; - - b2BodyDef bd1; - bd1.type = b2_staticBody; - bd1.position.Set(10.0f, 9.0f); - b2Body* body1 = m_world->CreateBody(&bd1); - body1->CreateFixture(&circle1, 5.0f); - - b2BodyDef bd2; - bd2.type = b2_dynamicBody; - bd2.position.Set(10.0f, 8.0f); - b2Body* body2 = m_world->CreateBody(&bd2); - body2->CreateFixture(&box, 5.0f); - - b2BodyDef bd3; - bd3.type = b2_dynamicBody; - bd3.position.Set(10.0f, 6.0f); - b2Body* body3 = m_world->CreateBody(&bd3); - body3->CreateFixture(&circle2, 5.0f); - - b2RevoluteJointDef jd1; - jd1.Initialize(body1, body2, bd1.position); - b2Joint* joint1 = m_world->CreateJoint(&jd1); - - b2RevoluteJointDef jd2; - jd2.Initialize(body2, body3, bd3.position); - b2Joint* joint2 = m_world->CreateJoint(&jd2); - - b2GearJointDef jd4; - jd4.bodyA = body1; - jd4.bodyB = body3; - jd4.joint1 = joint1; - jd4.joint2 = joint2; - jd4.ratio = circle2.m_radius / circle1.m_radius; - m_world->CreateJoint(&jd4); - } - - { - b2CircleShape circle1; - circle1.m_radius = 1.0f; - - b2CircleShape circle2; - circle2.m_radius = 2.0f; - - b2PolygonShape box; - box.SetAsBox(0.5f, 5.0f); - - b2BodyDef bd1; - bd1.type = b2_dynamicBody; - bd1.position.Set(-3.0f, 12.0f); - b2Body* body1 = m_world->CreateBody(&bd1); - body1->CreateFixture(&circle1, 5.0f); - - b2RevoluteJointDef jd1; - jd1.bodyA = ground; - jd1.bodyB = body1; - jd1.localAnchorA = ground->GetLocalPoint(bd1.position); - jd1.localAnchorB = body1->GetLocalPoint(bd1.position); - jd1.referenceAngle = body1->GetAngle() - ground->GetAngle(); - m_joint1 = (b2RevoluteJoint*)m_world->CreateJoint(&jd1); - - b2BodyDef bd2; - bd2.type = b2_dynamicBody; - bd2.position.Set(0.0f, 12.0f); - b2Body* body2 = m_world->CreateBody(&bd2); - body2->CreateFixture(&circle2, 5.0f); - - b2RevoluteJointDef jd2; - jd2.Initialize(ground, body2, bd2.position); - m_joint2 = (b2RevoluteJoint*)m_world->CreateJoint(&jd2); - - b2BodyDef bd3; - bd3.type = b2_dynamicBody; - bd3.position.Set(2.5f, 12.0f); - b2Body* body3 = m_world->CreateBody(&bd3); - body3->CreateFixture(&box, 5.0f); - - b2PrismaticJointDef jd3; - jd3.Initialize(ground, body3, bd3.position, b2Vec2(0.0f, 1.0f)); - jd3.lowerTranslation = -5.0f; - jd3.upperTranslation = 5.0f; - jd3.enableLimit = true; - - m_joint3 = (b2PrismaticJoint*)m_world->CreateJoint(&jd3); - - b2GearJointDef jd4; - jd4.bodyA = body1; - jd4.bodyB = body2; - jd4.joint1 = m_joint1; - jd4.joint2 = m_joint2; - jd4.ratio = circle2.m_radius / circle1.m_radius; - m_joint4 = (b2GearJoint*)m_world->CreateJoint(&jd4); - - b2GearJointDef jd5; - jd5.bodyA = body2; - jd5.bodyB = body3; - jd5.joint1 = m_joint2; - jd5.joint2 = m_joint3; - jd5.ratio = -1.0f / circle2.m_radius; - m_joint5 = (b2GearJoint*)m_world->CreateJoint(&jd5); - } - } - - void Step(Settings& settings) override - { - Test::Step(settings); - - float ratio, value; - - ratio = m_joint4->GetRatio(); - value = m_joint1->GetJointAngle() + ratio * m_joint2->GetJointAngle(); - DrawString(5, m_textLine, "theta1 + %4.2f * theta2 = %4.2f", (float)ratio, (float)value); - - ratio = m_joint5->GetRatio(); - value = m_joint2->GetJointAngle() + ratio * m_joint3->GetJointTranslation(); - DrawString(5, m_textLine, "theta2 + %4.2f * delta = %4.2f", (float)ratio, (float)value); - } - - static Test* Create() { return new GearJoint; } - - b2RevoluteJoint* m_joint1; - b2RevoluteJoint* m_joint2; - b2PrismaticJoint* m_joint3; - b2GearJoint* m_joint4; - b2GearJoint* m_joint5; -}; - -static int testIndex = RegisterTest("Joints", "Gear", GearJoint::Create); diff --git a/tests/cpp-tests/Source/Box2DTestBed/tests/heavy1.cpp b/tests/cpp-tests/Source/Box2DTestBed/tests/heavy1.cpp deleted file mode 100644 index 17ba6d183f23..000000000000 --- a/tests/cpp-tests/Source/Box2DTestBed/tests/heavy1.cpp +++ /dev/null @@ -1,57 +0,0 @@ -// MIT License - -// Copyright (c) 2019 Erin Catto - -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: - -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. - -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -#include "test.h" - -class Heavy1 : public Test -{ -public: - Heavy1() - { - { - b2BodyDef bd; - b2Body* ground = m_world->CreateBody(&bd); - - b2EdgeShape shape; - shape.SetTwoSided(b2Vec2(-40.0f, 0.0f), b2Vec2(40.0f, 0.0f)); - ground->CreateFixture(&shape, 0.0f); - } - - b2BodyDef bd; - bd.type = b2_dynamicBody; - bd.position.Set(0.0f, 0.5f); - b2Body* body = m_world->CreateBody(&bd); - - b2CircleShape shape; - shape.m_radius = 0.5f; - body->CreateFixture(&shape, 10.0f); - - bd.position.Set(0.0f, 6.0f); - body = m_world->CreateBody(&bd); - shape.m_radius = 5.0f; - body->CreateFixture(&shape, 10.0f); - } - - static Test* Create() { return new Heavy1; } -}; - -static int testIndex = RegisterTest("Solver", "Heavy 1", Heavy1::Create); diff --git a/tests/cpp-tests/Source/Box2DTestBed/tests/heavy2.cpp b/tests/cpp-tests/Source/Box2DTestBed/tests/heavy2.cpp deleted file mode 100644 index c34a36995eb8..000000000000 --- a/tests/cpp-tests/Source/Box2DTestBed/tests/heavy2.cpp +++ /dev/null @@ -1,90 +0,0 @@ -// MIT License - -// Copyright (c) 2019 Erin Catto - -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: - -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. - -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -#include "test.h" - -class Heavy2 : public Test -{ -public: - Heavy2() - { - { - b2BodyDef bd; - b2Body* ground = m_world->CreateBody(&bd); - - b2EdgeShape shape; - shape.SetTwoSided(b2Vec2(-40.0f, 0.0f), b2Vec2(40.0f, 0.0f)); - ground->CreateFixture(&shape, 0.0f); - } - - b2BodyDef bd; - bd.type = b2_dynamicBody; - bd.position.Set(0.0f, 2.5f); - b2Body* body = m_world->CreateBody(&bd); - - b2CircleShape shape; - shape.m_radius = 0.5f; - body->CreateFixture(&shape, 10.0f); - - bd.position.Set(0.0f, 3.5f); - body = m_world->CreateBody(&bd); - body->CreateFixture(&shape, 10.0f); - - m_heavy = NULL; - } - - void ToggleHeavy() - { - if (m_heavy) - { - m_world->DestroyBody(m_heavy); - m_heavy = NULL; - } - else - { - b2BodyDef bd; - bd.type = b2_dynamicBody; - bd.position.Set(0.0f, 9.0f); - m_heavy = m_world->CreateBody(&bd); - - b2CircleShape shape; - shape.m_radius = 5.0f; - m_heavy->CreateFixture(&shape, 10.0f); - } - } - - void Keyboard(int key) override - { - switch (key) - { - case GLFW_KEY_H: - ToggleHeavy(); - break; - } - } - - static Test* Create() { return new Heavy2; } - - b2Body* m_heavy; -}; - -static int testIndex = RegisterTest("Solver", "Heavy 2", Heavy2::Create); diff --git a/tests/cpp-tests/Source/Box2DTestBed/tests/mobile_balanced.cpp b/tests/cpp-tests/Source/Box2DTestBed/tests/mobile_balanced.cpp deleted file mode 100644 index 8b68edd0b7c7..000000000000 --- a/tests/cpp-tests/Source/Box2DTestBed/tests/mobile_balanced.cpp +++ /dev/null @@ -1,104 +0,0 @@ -// MIT License - -// Copyright (c) 2019 Erin Catto - -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: - -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. - -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -#include "test.h" - -class MobileBalanced : public Test -{ -public: - enum - { - e_depth = 4 - }; - - MobileBalanced() - { - b2Body* ground; - - // Create ground body. - { - b2BodyDef bodyDef; - bodyDef.position.Set(0.0f, 20.0f); - ground = m_world->CreateBody(&bodyDef); - } - - float a = 0.5f; - b2Vec2 h(0.0f, a); - - b2Body* root = AddNode(ground, b2Vec2_zero, 0, 3.0f, a); - - b2RevoluteJointDef jointDef; - jointDef.bodyA = ground; - jointDef.bodyB = root; - jointDef.localAnchorA.SetZero(); - jointDef.localAnchorB = h; - m_world->CreateJoint(&jointDef); - } - - b2Body* AddNode(b2Body* parent, const b2Vec2& localAnchor, int32 depth, float offset, float a) - { - float density = 20.0f; - b2Vec2 h(0.0f, a); - - b2Vec2 p = parent->GetPosition() + localAnchor - h; - - b2BodyDef bodyDef; - bodyDef.type = b2_dynamicBody; - bodyDef.position = p; - b2Body* body = m_world->CreateBody(&bodyDef); - - b2PolygonShape shape; - shape.SetAsBox(0.25f * a, a); - body->CreateFixture(&shape, density); - - if (depth == e_depth) - { - return body; - } - - shape.SetAsBox(offset, 0.25f * a, b2Vec2(0, -a), 0.0f); - body->CreateFixture(&shape, density); - - b2Vec2 a1 = b2Vec2(offset, -a); - b2Vec2 a2 = b2Vec2(-offset, -a); - b2Body* body1 = AddNode(body, a1, depth + 1, 0.5f * offset, a); - b2Body* body2 = AddNode(body, a2, depth + 1, 0.5f * offset, a); - - b2RevoluteJointDef jointDef; - jointDef.bodyA = body; - jointDef.localAnchorB = h; - - jointDef.localAnchorA = a1; - jointDef.bodyB = body1; - m_world->CreateJoint(&jointDef); - - jointDef.localAnchorA = a2; - jointDef.bodyB = body2; - m_world->CreateJoint(&jointDef); - - return body; - } - - static Test* Create() { return new MobileBalanced; } -}; - -static int testIndex = RegisterTest("Solver", "Mobile Balanced", MobileBalanced::Create); diff --git a/tests/cpp-tests/Source/Box2DTestBed/tests/mobile_unbalanced.cpp b/tests/cpp-tests/Source/Box2DTestBed/tests/mobile_unbalanced.cpp deleted file mode 100644 index 6d95738e5443..000000000000 --- a/tests/cpp-tests/Source/Box2DTestBed/tests/mobile_unbalanced.cpp +++ /dev/null @@ -1,101 +0,0 @@ -// MIT License - -// Copyright (c) 2019 Erin Catto - -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: - -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. - -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -#include "test.h" - -class MobileUnbalanced : public Test -{ -public: - enum - { - e_depth = 4 - }; - - MobileUnbalanced() - { - b2Body* ground; - - // Create ground body. - { - b2BodyDef bodyDef; - bodyDef.position.Set(0.0f, 20.0f); - ground = m_world->CreateBody(&bodyDef); - } - - float a = 0.5f; - b2Vec2 h(0.0f, a); - - b2Body* root = AddNode(ground, b2Vec2_zero, 0, 3.0f, a); - - b2RevoluteJointDef jointDef; - jointDef.bodyA = ground; - jointDef.bodyB = root; - jointDef.localAnchorA.SetZero(); - jointDef.localAnchorB = h; - m_world->CreateJoint(&jointDef); - } - - b2Body* AddNode(b2Body* parent, const b2Vec2& localAnchor, int32 depth, float offset, float a) - { - float density = 20.0f; - b2Vec2 h(0.0f, a); - - b2Vec2 p = parent->GetPosition() + localAnchor - h; - - b2BodyDef bodyDef; - bodyDef.type = b2_dynamicBody; - bodyDef.position = p; - b2Body* body = m_world->CreateBody(&bodyDef); - - b2PolygonShape shape; - shape.SetAsBox(0.25f * a, a); - body->CreateFixture(&shape, density); - - if (depth == e_depth) - { - return body; - } - - b2Vec2 a1 = b2Vec2(offset, -a); - b2Vec2 a2 = b2Vec2(-offset, -a); - b2Body* body1 = AddNode(body, a1, depth + 1, 0.5f * offset, a); - b2Body* body2 = AddNode(body, a2, depth + 1, 0.5f * offset, a); - - b2RevoluteJointDef jointDef; - jointDef.bodyA = body; - jointDef.localAnchorB = h; - - jointDef.localAnchorA = a1; - jointDef.bodyB = body1; - m_world->CreateJoint(&jointDef); - - jointDef.localAnchorA = a2; - jointDef.bodyB = body2; - m_world->CreateJoint(&jointDef); - - return body; - } - - static Test* Create() { return new MobileUnbalanced; } -}; - -static int testIndex = RegisterTest("Solver", "Mobile Unbalanced", MobileUnbalanced::Create); diff --git a/tests/cpp-tests/Source/Box2DTestBed/tests/motor_joint.cpp b/tests/cpp-tests/Source/Box2DTestBed/tests/motor_joint.cpp deleted file mode 100644 index 1b4873ad0eda..000000000000 --- a/tests/cpp-tests/Source/Box2DTestBed/tests/motor_joint.cpp +++ /dev/null @@ -1,115 +0,0 @@ -// MIT License - -// Copyright (c) 2019 Erin Catto - -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: - -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. - -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -#include "settings.h" -#include "test.h" - -/// This test shows how to use a motor joint. A motor joint -/// can be used to animate a dynamic body. With finite motor forces -/// the body can be blocked by collision with other bodies. -class MotorJoint : public Test -{ -public: - MotorJoint() - { - b2Body* ground = NULL; - { - b2BodyDef bd; - ground = m_world->CreateBody(&bd); - - b2EdgeShape shape; - shape.SetTwoSided(b2Vec2(-20.0f, 0.0f), b2Vec2(20.0f, 0.0f)); - - b2FixtureDef fd; - fd.shape = &shape; - - ground->CreateFixture(&fd); - } - - // Define motorized body - { - b2BodyDef bd; - bd.type = b2_dynamicBody; - bd.position.Set(0.0f, 8.0f); - b2Body* body = m_world->CreateBody(&bd); - - b2PolygonShape shape; - shape.SetAsBox(2.0f, 0.5f); - - b2FixtureDef fd; - fd.shape = &shape; - fd.friction = 0.6f; - fd.density = 2.0f; - body->CreateFixture(&fd); - - b2MotorJointDef mjd; - mjd.Initialize(ground, body); - mjd.maxForce = 1000.0f; - mjd.maxTorque = 1000.0f; - m_joint = (b2MotorJoint*)m_world->CreateJoint(&mjd); - } - - m_go = false; - m_time = 0.0f; - } - - void Keyboard(int key) override - { - switch (key) - { - case GLFW_KEY_S: - m_go = !m_go; - break; - } - } - - void Step(Settings& settings) override - { - if (m_go && settings.m_hertz > 0.0f) - { - m_time += 1.0f / settings.m_hertz; - } - - b2Vec2 linearOffset; - linearOffset.x = 6.0f * sinf(2.0f * m_time); - linearOffset.y = 8.0f + 4.0f * sinf(1.0f * m_time); - - float angularOffset = 4.0f * m_time; - - m_joint->SetLinearOffset(linearOffset); - m_joint->SetAngularOffset(angularOffset); - - g_debugDraw.DrawPoint(linearOffset, 4.0f, b2Color(0.9f, 0.9f, 0.9f)); - - Test::Step(settings); - DrawString(5, m_textLine, "Keys: (s) pause"); - m_textLine += 15; - } - - static Test* Create() { return new MotorJoint; } - - b2MotorJoint* m_joint; - float m_time; - bool m_go; -}; - -static int testIndex = RegisterTest("Joints", "Motor Joint", MotorJoint::Create); diff --git a/tests/cpp-tests/Source/Box2DTestBed/tests/pinball.cpp b/tests/cpp-tests/Source/Box2DTestBed/tests/pinball.cpp deleted file mode 100644 index 1c117de53c0e..000000000000 --- a/tests/cpp-tests/Source/Box2DTestBed/tests/pinball.cpp +++ /dev/null @@ -1,165 +0,0 @@ -// MIT License - -// Copyright (c) 2019 Erin Catto - -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: - -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. - -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -#include "test.h" - -/// This tests bullet collision and provides an example of a gameplay scenario. -/// This also uses a loop shape. -class Pinball : public Test -{ -public: - Pinball() - { - // Ground body - b2Body* ground = NULL; - { - b2BodyDef bd; - ground = m_world->CreateBody(&bd); - - b2Vec2 vs[5]; - vs[0].Set(-8.0f, 6.0f); - vs[1].Set(-8.0f, 20.0f); - vs[2].Set(8.0f, 20.0f); - vs[3].Set(8.0f, 6.0f); - vs[4].Set(0.0f, -2.0f); - - b2ChainShape loop; - loop.CreateLoop(vs, 5); - b2FixtureDef fd; - fd.shape = &loop; - fd.density = 0.0f; - ground->CreateFixture(&fd); - } - - // Flippers - { - b2Vec2 p1(-2.0f, 0.0f), p2(2.0f, 0.0f); - - b2BodyDef bd; - bd.type = b2_dynamicBody; - - bd.position = p1; - b2Body* leftFlipper = m_world->CreateBody(&bd); - - bd.position = p2; - b2Body* rightFlipper = m_world->CreateBody(&bd); - - b2PolygonShape box; - box.SetAsBox(1.75f, 0.1f); - - b2FixtureDef fd; - fd.shape = &box; - fd.density = 1.0f; - - leftFlipper->CreateFixture(&fd); - rightFlipper->CreateFixture(&fd); - - b2RevoluteJointDef jd; - jd.bodyA = ground; - jd.localAnchorB.SetZero(); - jd.enableMotor = true; - jd.maxMotorTorque = 1000.0f; - jd.enableLimit = true; - - jd.motorSpeed = 0.0f; - jd.localAnchorA = p1; - jd.bodyB = leftFlipper; - jd.lowerAngle = -30.0f * b2_pi / 180.0f; - jd.upperAngle = 5.0f * b2_pi / 180.0f; - m_leftJoint = (b2RevoluteJoint*)m_world->CreateJoint(&jd); - - jd.motorSpeed = 0.0f; - jd.localAnchorA = p2; - jd.bodyB = rightFlipper; - jd.lowerAngle = -5.0f * b2_pi / 180.0f; - jd.upperAngle = 30.0f * b2_pi / 180.0f; - m_rightJoint = (b2RevoluteJoint*)m_world->CreateJoint(&jd); - } - - // Circle character - { - b2BodyDef bd; - bd.position.Set(1.0f, 15.0f); - bd.type = b2_dynamicBody; - bd.bullet = true; - - m_ball = m_world->CreateBody(&bd); - - b2CircleShape shape; - shape.m_radius = 0.2f; - - b2FixtureDef fd; - fd.shape = &shape; - fd.density = 1.0f; - m_ball->CreateFixture(&fd); - } - - m_button = false; - } - - void Step(Settings& settings) override - { - if (m_button) - { - m_leftJoint->SetMotorSpeed(20.0f); - m_rightJoint->SetMotorSpeed(-20.0f); - } - else - { - m_leftJoint->SetMotorSpeed(-10.0f); - m_rightJoint->SetMotorSpeed(10.0f); - } - - Test::Step(settings); - - DrawString(5, m_textLine, "Press 'a' to control the flippers"); - } - - void Keyboard(int key) override - { - switch (key) - { - case GLFW_KEY_A: - m_button = true; - break; - } - } - - void KeyboardUp(int key) override - { - switch (key) - { - case GLFW_KEY_A: - m_button = false; - break; - } - } - - static Test* Create() { return new Pinball; } - - b2RevoluteJoint* m_leftJoint; - b2RevoluteJoint* m_rightJoint; - b2Body* m_ball; - bool m_button; -}; - -static int testIndex = RegisterTest("Examples", "Pinball", Pinball::Create); diff --git a/tests/cpp-tests/Source/Box2DTestBed/tests/platformer.cpp b/tests/cpp-tests/Source/Box2DTestBed/tests/platformer.cpp deleted file mode 100644 index c308f4922bac..000000000000 --- a/tests/cpp-tests/Source/Box2DTestBed/tests/platformer.cpp +++ /dev/null @@ -1,128 +0,0 @@ -// MIT License - -// Copyright (c) 2019 Erin Catto - -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: - -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. - -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -#include "test.h" - -class Platformer : public Test -{ -public: - enum State - { - e_unknown, - e_above, - e_below - }; - - Platformer() - { - // Ground - { - b2BodyDef bd; - b2Body* ground = m_world->CreateBody(&bd); - - b2EdgeShape shape; - shape.SetTwoSided(b2Vec2(-20.0f, 0.0f), b2Vec2(20.0f, 0.0f)); - ground->CreateFixture(&shape, 0.0f); - } - - // Platform - { - b2BodyDef bd; - bd.position.Set(0.0f, 10.0f); - b2Body* body = m_world->CreateBody(&bd); - - b2PolygonShape shape; - shape.SetAsBox(3.0f, 0.5f); - m_platform = body->CreateFixture(&shape, 0.0f); - - m_bottom = 10.0f - 0.5f; - m_top = 10.0f + 0.5f; - } - - // Actor - { - b2BodyDef bd; - bd.type = b2_dynamicBody; - bd.position.Set(0.0f, 12.0f); - b2Body* body = m_world->CreateBody(&bd); - - m_radius = 0.5f; - b2CircleShape shape; - shape.m_radius = m_radius; - m_character = body->CreateFixture(&shape, 20.0f); - - body->SetLinearVelocity(b2Vec2(0.0f, -50.0f)); - - m_state = e_unknown; - } - } - - void PreSolve(b2Contact* contact, const b2Manifold* oldManifold) override - { - Test::PreSolve(contact, oldManifold); - - b2Fixture* fixtureA = contact->GetFixtureA(); - b2Fixture* fixtureB = contact->GetFixtureB(); - - if (fixtureA != m_platform && fixtureA != m_character) - { - return; - } - - if (fixtureB != m_platform && fixtureB != m_character) - { - return; - } - -#if 1 - b2Vec2 position = m_character->GetBody()->GetPosition(); - - if (position.y < m_top + m_radius - 3.0f * b2_linearSlop) - { - contact->SetEnabled(false); - } -#else - b2Vec2 v = m_character->GetBody()->GetLinearVelocity(); - if (v.y > 0.0f) - { - contact->SetEnabled(false); - } -#endif - } - - void Step(Settings& settings) override - { - Test::Step(settings); - - b2Vec2 v = m_character->GetBody()->GetLinearVelocity(); - DrawString(5, m_textLine, "Character Linear Velocity: %f", v.y); - } - - static Test* Create() { return new Platformer; } - - float m_radius, m_top, m_bottom; - State m_state; - b2Fixture* m_platform; - b2Fixture* m_character; -}; - -static int testIndex = RegisterTest("Examples", "Platformer", Platformer::Create); diff --git a/tests/cpp-tests/Source/Box2DTestBed/tests/polygon_collision.cpp b/tests/cpp-tests/Source/Box2DTestBed/tests/polygon_collision.cpp deleted file mode 100644 index dbc924694c2d..000000000000 --- a/tests/cpp-tests/Source/Box2DTestBed/tests/polygon_collision.cpp +++ /dev/null @@ -1,123 +0,0 @@ -// MIT License - -// Copyright (c) 2019 Erin Catto - -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: - -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. - -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -#include "test.h" - -class PolygonCollision : public Test -{ -public: - PolygonCollision() - { - { - m_polygonA.SetAsBox(0.2f, 0.4f); - m_transformA.Set(b2Vec2(0.0f, 0.0f), 0.0f); - } - - { - m_polygonB.SetAsBox(0.5f, 0.5f); - m_positionB.Set(19.345284f, 1.5632932f); - m_angleB = 1.9160721f; - m_transformB.Set(m_positionB, m_angleB); - } - } - - static Test* Create() { return new PolygonCollision; } - - void Step(Settings& settings) override - { - B2_NOT_USED(settings); - - b2Manifold manifold; - b2CollidePolygons(&manifold, &m_polygonA, m_transformA, &m_polygonB, m_transformB); - - b2WorldManifold worldManifold; - worldManifold.Initialize(&manifold, m_transformA, m_polygonA.m_radius, m_transformB, m_polygonB.m_radius); - - DrawString(5, m_textLine, "point count = %d", manifold.pointCount); - - { - b2Color color(0.9f, 0.9f, 0.9f); - b2Vec2 v[b2_maxPolygonVertices]; - for (int32 i = 0; i < m_polygonA.m_count; ++i) - { - v[i] = b2Mul(m_transformA, m_polygonA.m_vertices[i]); - } - g_debugDraw.DrawPolygon(v, m_polygonA.m_count, color); - - for (int32 i = 0; i < m_polygonB.m_count; ++i) - { - v[i] = b2Mul(m_transformB, m_polygonB.m_vertices[i]); - } - g_debugDraw.DrawPolygon(v, m_polygonB.m_count, color); - } - - for (int32 i = 0; i < manifold.pointCount; ++i) - { - g_debugDraw.DrawPoint(worldManifold.points[i], 4.0f, b2Color(0.9f, 0.3f, 0.3f)); - } - - Test::Step(settings); - } - - void Keyboard(int key) override - { - switch (key) - { - case GLFW_KEY_A: - m_positionB.x -= 0.1f; - break; - - case GLFW_KEY_D: - m_positionB.x += 0.1f; - break; - - case GLFW_KEY_S: - m_positionB.y -= 0.1f; - break; - - case GLFW_KEY_W: - m_positionB.y += 0.1f; - break; - - case GLFW_KEY_Q: - m_angleB += 0.1f * b2_pi; - break; - - case GLFW_KEY_E: - m_angleB -= 0.1f * b2_pi; - break; - } - - m_transformB.Set(m_positionB, m_angleB); - } - - b2PolygonShape m_polygonA; - b2PolygonShape m_polygonB; - - b2Transform m_transformA; - b2Transform m_transformB; - - b2Vec2 m_positionB; - float m_angleB; -}; - -static int testIndex = RegisterTest("Geometry", "Polygon Collision", PolygonCollision::Create); diff --git a/tests/cpp-tests/Source/Box2DTestBed/tests/polygon_shapes.cpp b/tests/cpp-tests/Source/Box2DTestBed/tests/polygon_shapes.cpp deleted file mode 100644 index 369f777ee85c..000000000000 --- a/tests/cpp-tests/Source/Box2DTestBed/tests/polygon_shapes.cpp +++ /dev/null @@ -1,257 +0,0 @@ -// MIT License - -// Copyright (c) 2019 Erin Catto - -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: - -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. - -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -#include "test.h" - -/// This tests stacking. It also shows how to use b2World::Query -/// and b2TestOverlap. - -/// This callback is called by b2World::QueryAABB. We find all the fixtures -/// that overlap an AABB. Of those, we use b2TestOverlap to determine which fixtures -/// overlap a circle. Up to 4 overlapped fixtures will be highlighted with a yellow border. -class PolygonShapesCallback : public b2QueryCallback -{ -public: - enum - { - e_maxCount = 4 - }; - - PolygonShapesCallback() { m_count = 0; } - - /// Called for each fixture found in the query AABB. - /// @return false to terminate the query. - bool ReportFixture(b2Fixture* fixture) override - { - if (m_count == e_maxCount) - { - return false; - } - - b2Body* body = fixture->GetBody(); - b2Shape* shape = fixture->GetShape(); - - bool overlap = b2TestOverlap(shape, 0, &m_circle, 0, body->GetTransform(), m_transform); - - if (overlap) - { - b2Color color(0.95f, 0.95f, 0.6f); - b2Vec2 center = body->GetWorldCenter(); - g_debugDraw->DrawPoint(center, 5.0f, color); - ++m_count; - } - - return true; - } - - b2CircleShape m_circle; - b2Transform m_transform; - b2Draw* g_debugDraw; - int32 m_count; -}; - -class PolygonShapes : public Test -{ -public: - enum - { - e_maxBodies = 256 - }; - - PolygonShapes() - { - // Ground body - { - b2BodyDef bd; - b2Body* ground = m_world->CreateBody(&bd); - - b2EdgeShape shape; - shape.SetTwoSided(b2Vec2(-40.0f, 0.0f), b2Vec2(40.0f, 0.0f)); - ground->CreateFixture(&shape, 0.0f); - } - - { - b2Vec2 vertices[3]; - vertices[0].Set(-0.5f, 0.0f); - vertices[1].Set(0.5f, 0.0f); - vertices[2].Set(0.0f, 1.5f); - m_polygons[0].Set(vertices, 3); - } - - { - b2Vec2 vertices[3]; - vertices[0].Set(-0.1f, 0.0f); - vertices[1].Set(0.1f, 0.0f); - vertices[2].Set(0.0f, 1.5f); - m_polygons[1].Set(vertices, 3); - } - - { - float w = 1.0f; - float b = w / (2.0f + b2Sqrt(2.0f)); - float s = b2Sqrt(2.0f) * b; - - b2Vec2 vertices[8]; - vertices[0].Set(0.5f * s, 0.0f); - vertices[1].Set(0.5f * w, b); - vertices[2].Set(0.5f * w, b + s); - vertices[3].Set(0.5f * s, w); - vertices[4].Set(-0.5f * s, w); - vertices[5].Set(-0.5f * w, b + s); - vertices[6].Set(-0.5f * w, b); - vertices[7].Set(-0.5f * s, 0.0f); - - m_polygons[2].Set(vertices, 8); - } - - { - m_polygons[3].SetAsBox(0.5f, 0.5f); - } - - { - m_circle.m_radius = 0.5f; - } - - m_bodyIndex = 0; - memset(m_bodies, 0, sizeof(m_bodies)); - } - - void Create(int32 index) - { - if (m_bodies[m_bodyIndex] != NULL) - { - m_world->DestroyBody(m_bodies[m_bodyIndex]); - m_bodies[m_bodyIndex] = NULL; - } - - b2BodyDef bd; - bd.type = b2_dynamicBody; - - float x = RandomFloat(-2.0f, 2.0f); - bd.position.Set(x, 10.0f); - bd.angle = RandomFloat(-b2_pi, b2_pi); - - if (index == 4) - { - bd.angularDamping = 0.02f; - } - - m_bodies[m_bodyIndex] = m_world->CreateBody(&bd); - - if (index < 4) - { - b2FixtureDef fd; - fd.shape = m_polygons + index; - fd.density = 1.0f; - fd.friction = 0.3f; - m_bodies[m_bodyIndex]->CreateFixture(&fd); - } - else - { - b2FixtureDef fd; - fd.shape = &m_circle; - fd.density = 1.0f; - fd.friction = 0.3f; - - m_bodies[m_bodyIndex]->CreateFixture(&fd); - } - - m_bodyIndex = (m_bodyIndex + 1) % e_maxBodies; - } - - void DestroyBody() - { - for (int32 i = 0; i < e_maxBodies; ++i) - { - if (m_bodies[i] != NULL) - { - m_world->DestroyBody(m_bodies[i]); - m_bodies[i] = NULL; - return; - } - } - } - - void Keyboard(int key) override - { - switch (key) - { - case GLFW_KEY_1: - case GLFW_KEY_2: - case GLFW_KEY_3: - case GLFW_KEY_4: - case GLFW_KEY_5: - Create(key - GLFW_KEY_1); - break; - - case GLFW_KEY_A: - for (int32 i = 0; i < e_maxBodies; i += 2) - { - if (m_bodies[i]) - { - bool enabled = m_bodies[i]->IsEnabled(); - m_bodies[i]->SetEnabled(!enabled); - } - } - break; - - case GLFW_KEY_D: - DestroyBody(); - break; - } - } - - void Step(Settings& settings) override - { - Test::Step(settings); - - PolygonShapesCallback callback; - callback.m_circle.m_radius = 2.0f; - callback.m_circle.m_p.Set(0.0f, 1.1f); - callback.m_transform.SetIdentity(); - callback.g_debugDraw = &g_debugDraw; - - b2AABB aabb; - callback.m_circle.ComputeAABB(&aabb, callback.m_transform, 0); - - m_world->QueryAABB(&callback, aabb); - - b2Color color(0.4f, 0.7f, 0.8f); - g_debugDraw.DrawCircle(callback.m_circle.m_p, callback.m_circle.m_radius, color); - - DrawString(5, m_textLine, "Press 1-5 to drop stuff, maximum of %d overlaps detected", - PolygonShapesCallback::e_maxCount); - - DrawString(5, m_textLine, "Press 'a' to enable/disable some bodies"); - - DrawString(5, m_textLine, "Press 'd' to destroy a body"); - } - - static Test* Create() { return new PolygonShapes; } - - int32 m_bodyIndex; - b2Body* m_bodies[e_maxBodies]; - b2PolygonShape m_polygons[4]; - b2CircleShape m_circle; -}; - -static int testIndex = RegisterTest("Geometry", "Polygon Shapes", PolygonShapes::Create); diff --git a/tests/cpp-tests/Source/Box2DTestBed/tests/prismatic_joint.cpp b/tests/cpp-tests/Source/Box2DTestBed/tests/prismatic_joint.cpp deleted file mode 100644 index c9d05bed0c04..000000000000 --- a/tests/cpp-tests/Source/Box2DTestBed/tests/prismatic_joint.cpp +++ /dev/null @@ -1,114 +0,0 @@ -// MIT License - -// Copyright (c) 2019 Erin Catto - -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: - -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. - -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -#include "settings.h" -#include "test.h" -#include "ImGui/ImGuiPresenter.h" - -// Test the prismatic joint with limits and motor options. -class PrismaticJoint : public Test -{ -public: - PrismaticJoint() - { - b2Body* ground = NULL; - { - b2BodyDef bd; - ground = m_world->CreateBody(&bd); - - b2EdgeShape shape; - shape.SetTwoSided(b2Vec2(-40.0f, 0.0f), b2Vec2(40.0f, 0.0f)); - ground->CreateFixture(&shape, 0.0f); - } - - m_enableLimit = true; - m_enableMotor = false; - m_motorSpeed = 10.0f; - - { - b2PolygonShape shape; - shape.SetAsBox(1.0f, 1.0f); - - b2BodyDef bd; - bd.type = b2_dynamicBody; - bd.position.Set(0.0f, 10.0f); - bd.angle = 0.5f * b2_pi; - bd.allowSleep = false; - b2Body* body = m_world->CreateBody(&bd); - body->CreateFixture(&shape, 5.0f); - - b2PrismaticJointDef pjd; - - // Horizontal - pjd.Initialize(ground, body, bd.position, b2Vec2(1.0f, 0.0f)); - - pjd.motorSpeed = m_motorSpeed; - pjd.maxMotorForce = 10000.0f; - pjd.enableMotor = m_enableMotor; - pjd.lowerTranslation = -10.0f; - pjd.upperTranslation = 10.0f; - pjd.enableLimit = m_enableLimit; - - m_joint = (b2PrismaticJoint*)m_world->CreateJoint(&pjd); - } - } - - void UpdateUI() override - { - // ImGui::SetNextWindowPos(ImVec2(10.0f, 100.0f)); - ImGui::SetNextWindowSize(ImVec2(200.0f, 100.0f)); - ImGui::Begin("Joint Controls", nullptr, ImGuiWindowFlags_NoResize); - - if (ImGui::Checkbox("Limit", &m_enableLimit)) - { - m_joint->EnableLimit(m_enableLimit); - } - - if (ImGui::Checkbox("Motor", &m_enableMotor)) - { - m_joint->EnableMotor(m_enableMotor); - } - - if (ImGui::SliderFloat("Speed", &m_motorSpeed, -100.0f, 100.0f, "%.0f")) - { - m_joint->SetMotorSpeed(m_motorSpeed); - } - - ImGui::End(); - } - - void Step(Settings& settings) override - { - Test::Step(settings); - float force = m_joint->GetMotorForce(settings.m_hertz); - DrawString(5, m_textLine, "Motor Force = %4.0f", force); - } - - static Test* Create() { return new PrismaticJoint; } - - b2PrismaticJoint* m_joint; - float m_motorSpeed; - bool m_enableMotor; - bool m_enableLimit; -}; - -static int testIndex = RegisterTest("Joints", "Prismatic", PrismaticJoint::Create); diff --git a/tests/cpp-tests/Source/Box2DTestBed/tests/pulley_joint.cpp b/tests/cpp-tests/Source/Box2DTestBed/tests/pulley_joint.cpp deleted file mode 100644 index b724673d148a..000000000000 --- a/tests/cpp-tests/Source/Box2DTestBed/tests/pulley_joint.cpp +++ /dev/null @@ -1,92 +0,0 @@ -// MIT License - -// Copyright (c) 2019 Erin Catto - -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: - -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. - -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -#include "test.h" - -class PulleyJoint : public Test -{ -public: - PulleyJoint() - { - float y = 16.0f; - float L = 12.0f; - float a = 1.0f; - float b = 2.0f; - - b2Body* ground = NULL; - { - b2BodyDef bd; - ground = m_world->CreateBody(&bd); - - b2CircleShape circle; - circle.m_radius = 2.0f; - - circle.m_p.Set(-10.0f, y + b + L); - ground->CreateFixture(&circle, 0.0f); - - circle.m_p.Set(10.0f, y + b + L); - ground->CreateFixture(&circle, 0.0f); - } - - { - - b2PolygonShape shape; - shape.SetAsBox(a, b); - - b2BodyDef bd; - bd.type = b2_dynamicBody; - - // bd.fixedRotation = true; - bd.position.Set(-10.0f, y); - b2Body* body1 = m_world->CreateBody(&bd); - body1->CreateFixture(&shape, 5.0f); - - bd.position.Set(10.0f, y); - b2Body* body2 = m_world->CreateBody(&bd); - body2->CreateFixture(&shape, 5.0f); - - b2PulleyJointDef pulleyDef; - b2Vec2 anchor1(-10.0f, y + b); - b2Vec2 anchor2(10.0f, y + b); - b2Vec2 groundAnchor1(-10.0f, y + b + L); - b2Vec2 groundAnchor2(10.0f, y + b + L); - pulleyDef.Initialize(body1, body2, groundAnchor1, groundAnchor2, anchor1, anchor2, 1.5f); - - m_joint1 = (b2PulleyJoint*)m_world->CreateJoint(&pulleyDef); - } - } - - void Step(Settings& settings) override - { - Test::Step(settings); - - float ratio = m_joint1->GetRatio(); - float L = m_joint1->GetCurrentLengthA() + ratio * m_joint1->GetCurrentLengthB(); - DrawString(5, m_textLine, "L1 + %4.2f * L2 = %4.2f", (float)ratio, (float)L); - } - - static Test* Create() { return new PulleyJoint; } - - b2PulleyJoint* m_joint1; -}; - -static int testIndex = RegisterTest("Joints", "Pulley", PulleyJoint::Create); diff --git a/tests/cpp-tests/Source/Box2DTestBed/tests/pyramid.cpp b/tests/cpp-tests/Source/Box2DTestBed/tests/pyramid.cpp deleted file mode 100644 index 5f527451601a..000000000000 --- a/tests/cpp-tests/Source/Box2DTestBed/tests/pyramid.cpp +++ /dev/null @@ -1,89 +0,0 @@ -// MIT License - -// Copyright (c) 2019 Erin Catto - -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: - -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. - -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -#include "test.h" - -class Pyramid : public Test -{ -public: - enum - { - e_count = 20 - }; - - Pyramid() - { - { - b2BodyDef bd; - b2Body* ground = m_world->CreateBody(&bd); - - b2EdgeShape shape; - shape.SetTwoSided(b2Vec2(-40.0f, 0.0f), b2Vec2(40.0f, 0.0f)); - ground->CreateFixture(&shape, 0.0f); - } - - { - float a = 0.5f; - b2PolygonShape shape; - shape.SetAsBox(a, a); - - b2Vec2 x(-7.0f, 0.75f); - b2Vec2 y; - b2Vec2 deltaX(0.5625f, 1.25f); - b2Vec2 deltaY(1.125f, 0.0f); - - for (int32 i = 0; i < e_count; ++i) - { - y = x; - - for (int32 j = i; j < e_count; ++j) - { - b2BodyDef bd; - bd.type = b2_dynamicBody; - bd.position = y; - b2Body* body = m_world->CreateBody(&bd); - body->CreateFixture(&shape, 5.0f); - - y += deltaY; - } - - x += deltaX; - } - } - } - - void Step(Settings& settings) override - { - Test::Step(settings); - - // b2DynamicTree* tree = &m_world->m_contactManager.m_broadPhase.m_tree; - - // if (m_stepCount == 400) - //{ - // tree->RebuildBottomUp(); - // } - } - - static Test* Create() { return new Pyramid; } -}; - -static int testIndex = RegisterTest("Stacking", "Pyramid", Pyramid::Create); diff --git a/tests/cpp-tests/Source/Box2DTestBed/tests/ray_cast.cpp b/tests/cpp-tests/Source/Box2DTestBed/tests/ray_cast.cpp deleted file mode 100644 index 81697a8542af..000000000000 --- a/tests/cpp-tests/Source/Box2DTestBed/tests/ray_cast.cpp +++ /dev/null @@ -1,463 +0,0 @@ -// MIT License - -// Copyright (c) 2019 Erin Catto - -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: - -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. - -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -#include "settings.h" -#include "test.h" -#include "ImGui/ImGuiPresenter.h" - -enum -{ - e_maxBodies = 256 -}; - -// This test demonstrates how to use the world ray-cast feature. -// NOTE: we are intentionally filtering one of the polygons, therefore -// the ray will always miss one type of polygon. - -// This callback finds the closest hit. Polygon 0 is filtered. -class RayCastClosestCallback : public b2RayCastCallback -{ -public: - RayCastClosestCallback() { m_hit = false; } - - float ReportFixture(b2Fixture* fixture, const b2Vec2& point, const b2Vec2& normal, float fraction) override - { - uintptr_t index = fixture->GetUserData().pointer; - if (index == 1) - { - // By returning -1, we instruct the calling code to ignore this fixture and - // continue the ray-cast to the next fixture. - return -1.0f; - } - - m_hit = true; - m_point = point; - m_normal = normal; - - // By returning the current fraction, we instruct the calling code to clip the ray and - // continue the ray-cast to the next fixture. WARNING: do not assume that fixtures - // are reported in order. However, by clipping, we can always get the closest fixture. - return fraction; - } - - bool m_hit; - b2Vec2 m_point; - b2Vec2 m_normal; -}; - -// This callback finds any hit. Polygon 0 is filtered. For this type of query we are usually -// just checking for obstruction, so the actual fixture and hit point are irrelevant. -class RayCastAnyCallback : public b2RayCastCallback -{ -public: - RayCastAnyCallback() { m_hit = false; } - - float ReportFixture(b2Fixture* fixture, const b2Vec2& point, const b2Vec2& normal, float) override - { - uintptr_t index = fixture->GetUserData().pointer; - if (index == 1) - { - // By returning -1, we instruct the calling code to ignore this fixture and - // continue the ray-cast to the next fixture. - return -1.0f; - } - - m_hit = true; - m_point = point; - m_normal = normal; - - // At this point we have a hit, so we know the ray is obstructed. - // By returning 0, we instruct the calling code to terminate the ray-cast. - return 0.0f; - } - - bool m_hit; - b2Vec2 m_point; - b2Vec2 m_normal; -}; - -// This ray cast collects multiple hits along the ray. Polygon 0 is filtered. -// The fixtures are not necessary reported in order, so we might not capture -// the closest fixture. -class RayCastMultipleCallback : public b2RayCastCallback -{ -public: - enum - { - e_maxCount = 3 - }; - - RayCastMultipleCallback() { m_count = 0; } - - float ReportFixture(b2Fixture* fixture, const b2Vec2& point, const b2Vec2& normal, float) override - { - uintptr_t index = fixture->GetUserData().pointer; - if (index == 1) - { - // By returning -1, we instruct the calling code to ignore this fixture and - // continue the ray-cast to the next fixture. - return -1.0f; - } - - b2Assert(m_count < e_maxCount); - - m_points[m_count] = point; - m_normals[m_count] = normal; - ++m_count; - - if (m_count == e_maxCount) - { - // At this point the buffer is full. - // By returning 0, we instruct the calling code to terminate the ray-cast. - return 0.0f; - } - - // By returning 1, we instruct the caller to continue without clipping the ray. - return 1.0f; - } - - b2Vec2 m_points[e_maxCount]; - b2Vec2 m_normals[e_maxCount]; - int32 m_count; -}; - -class RayCast : public Test -{ -public: - enum Mode - { - e_any = 0, - e_closest = 1, - e_multiple = 2 - }; - - RayCast() - { - // Ground body - { - b2BodyDef bd; - b2Body* ground = m_world->CreateBody(&bd); - - b2EdgeShape shape; - shape.SetTwoSided(b2Vec2(-40.0f, 0.0f), b2Vec2(40.0f, 0.0f)); - ground->CreateFixture(&shape, 0.0f); - } - - { - b2Vec2 vertices[3]; - vertices[0].Set(-0.5f, 0.0f); - vertices[1].Set(0.5f, 0.0f); - vertices[2].Set(0.0f, 1.5f); - m_polygons[0].Set(vertices, 3); - } - - { - b2Vec2 vertices[3]; - vertices[0].Set(-0.1f, 0.0f); - vertices[1].Set(0.1f, 0.0f); - vertices[2].Set(0.0f, 1.5f); - m_polygons[1].Set(vertices, 3); - } - - { - float w = 1.0f; - float b = w / (2.0f + b2Sqrt(2.0f)); - float s = b2Sqrt(2.0f) * b; - - b2Vec2 vertices[8]; - vertices[0].Set(0.5f * s, 0.0f); - vertices[1].Set(0.5f * w, b); - vertices[2].Set(0.5f * w, b + s); - vertices[3].Set(0.5f * s, w); - vertices[4].Set(-0.5f * s, w); - vertices[5].Set(-0.5f * w, b + s); - vertices[6].Set(-0.5f * w, b); - vertices[7].Set(-0.5f * s, 0.0f); - - m_polygons[2].Set(vertices, 8); - } - - { - m_polygons[3].SetAsBox(0.5f, 0.5f); - } - - { - m_circle.m_radius = 0.5f; - } - - { - m_edge.SetTwoSided(b2Vec2(-1.0f, 0.0f), b2Vec2(1.0f, 0.0f)); - } - - m_bodyIndex = 0; - memset(m_bodies, 0, sizeof(m_bodies)); - - m_degrees = 0.0f; - - m_mode = e_closest; - } - - void Create(int32 index) - { - if (m_bodies[m_bodyIndex] != NULL) - { - m_world->DestroyBody(m_bodies[m_bodyIndex]); - m_bodies[m_bodyIndex] = NULL; - } - - b2BodyDef bd; - - float x = RandomFloat(-10.0f, 10.0f); - float y = RandomFloat(0.0f, 20.0f); - bd.position.Set(x, y); - bd.angle = RandomFloat(-b2_pi, b2_pi); - - if (index == 4) - { - bd.angularDamping = 0.02f; - } - - m_bodies[m_bodyIndex] = m_world->CreateBody(&bd); - - if (index < 4) - { - b2FixtureDef fd; - fd.shape = m_polygons + index; - fd.friction = 0.3f; - fd.userData.pointer = index + 1; - m_bodies[m_bodyIndex]->CreateFixture(&fd); - } - else if (index < 5) - { - b2FixtureDef fd; - fd.shape = &m_circle; - fd.friction = 0.3f; - fd.userData.pointer = index + 1; - m_bodies[m_bodyIndex]->CreateFixture(&fd); - } - else - { - b2FixtureDef fd; - fd.shape = &m_edge; - fd.friction = 0.3f; - fd.userData.pointer = index + 1; - - m_bodies[m_bodyIndex]->CreateFixture(&fd); - } - - m_bodyIndex = (m_bodyIndex + 1) % e_maxBodies; - } - - void DestroyBody() - { - for (int32 i = 0; i < e_maxBodies; ++i) - { - if (m_bodies[i] != NULL) - { - m_world->DestroyBody(m_bodies[i]); - m_bodies[i] = NULL; - return; - } - } - } - - void UpdateUI() override - { - // ImGui::SetNextWindowPos(ImVec2(g_debugDrawTestBed.debugNodeOffset.x, g_debugDrawTestBed.debugNodeOffset.y)); - ImGui::SetNextWindowSize(ImVec2(210.0f, 285.0f)); - ImGui::Begin("Ray-cast Controls", nullptr, ImGuiWindowFlags_NoResize); - - if (ImGui::Button("Shape 1")) - { - Create(0); - } - - if (ImGui::Button("Shape 2")) - { - Create(1); - } - - if (ImGui::Button("Shape 3")) - { - Create(2); - } - - if (ImGui::Button("Shape 4")) - { - Create(3); - } - - if (ImGui::Button("Shape 5")) - { - Create(4); - } - - if (ImGui::Button("Shape 6")) - { - Create(5); - } - - if (ImGui::Button("Destroy Shape")) - { - DestroyBody(); - } - - ImGui::RadioButton("Any", &m_mode, e_any); - ImGui::RadioButton("Closest", &m_mode, e_closest); - ImGui::RadioButton("Multiple", &m_mode, e_multiple); - - ImGui::SliderFloat("Angle", &m_degrees, 0.0f, 360.0f, "%.0f"); - - ImGui::End(); - } - - void Step(Settings& settings) override - { - Test::Step(settings); - - DrawString(5, m_textLine, "Shape 1 is intentionally ignored by the ray"); - - switch (m_mode) - { - case e_closest: - DrawString(5, m_textLine, "Ray-cast mode: closest - find closest fixture along the ray"); - break; - - case e_any: - DrawString(5, m_textLine, "Ray-cast mode: any - check for obstruction"); - break; - - case e_multiple: - DrawString(5, m_textLine, "Ray-cast mode: multiple - gather multiple fixtures"); - break; - } - - float angle = b2_pi * m_degrees / 180.0f; - float L = 11.0f; - b2Vec2 point1(0.0f, 10.0f); - b2Vec2 d(L * cosf(angle), L * sinf(angle)); - b2Vec2 point2 = point1 + d; - - if (m_mode == e_closest) - { - RayCastClosestCallback callback; - m_world->RayCast(&callback, point1, point2); - - if (callback.m_hit) - { - g_debugDraw.DrawPoint(callback.m_point, 5.0f, b2Color(0.4f, 0.9f, 0.4f)); - g_debugDraw.DrawSegment(point1, callback.m_point, b2Color(0.8f, 0.8f, 0.8f)); - b2Vec2 head = callback.m_point + 0.5f * callback.m_normal; - g_debugDraw.DrawSegment(callback.m_point, head, b2Color(0.9f, 0.9f, 0.4f)); - } - else - { - g_debugDraw.DrawSegment(point1, point2, b2Color(0.8f, 0.8f, 0.8f)); - } - } - else if (m_mode == e_any) - { - RayCastAnyCallback callback; - m_world->RayCast(&callback, point1, point2); - - if (callback.m_hit) - { - g_debugDraw.DrawPoint(callback.m_point, 5.0f, b2Color(0.4f, 0.9f, 0.4f)); - g_debugDraw.DrawSegment(point1, callback.m_point, b2Color(0.8f, 0.8f, 0.8f)); - b2Vec2 head = callback.m_point + 0.5f * callback.m_normal; - g_debugDraw.DrawSegment(callback.m_point, head, b2Color(0.9f, 0.9f, 0.4f)); - } - else - { - g_debugDraw.DrawSegment(point1, point2, b2Color(0.8f, 0.8f, 0.8f)); - } - } - else if (m_mode == e_multiple) - { - RayCastMultipleCallback callback; - m_world->RayCast(&callback, point1, point2); - g_debugDraw.DrawSegment(point1, point2, b2Color(0.8f, 0.8f, 0.8f)); - - for (int32 i = 0; i < callback.m_count; ++i) - { - b2Vec2 p = callback.m_points[i]; - b2Vec2 n = callback.m_normals[i]; - g_debugDraw.DrawPoint(p, 5.0f, b2Color(0.4f, 0.9f, 0.4f)); - g_debugDraw.DrawSegment(point1, p, b2Color(0.8f, 0.8f, 0.8f)); - b2Vec2 head = p + 0.5f * n; - g_debugDraw.DrawSegment(p, head, b2Color(0.9f, 0.9f, 0.4f)); - } - } - -#if 0 - // This case was failing. - { - b2Vec2 vertices[4]; - //vertices[0].Set(-22.875f, -3.0f); - //vertices[1].Set(22.875f, -3.0f); - //vertices[2].Set(22.875f, 3.0f); - //vertices[3].Set(-22.875f, 3.0f); - - b2PolygonShape shape; - //shape.Set(vertices, 4); - shape.SetAsBox(22.875f, 3.0f); - - b2RayCastInput input; - input.p1.Set(10.2725f,1.71372f); - input.p2.Set(10.2353f,2.21807f); - //input.maxFraction = 0.567623f; - input.maxFraction = 0.56762173f; - - b2Transform xf; - xf.SetIdentity(); - xf.position.Set(23.0f, 5.0f); - - b2RayCastOutput output; - bool hit; - hit = shape.RayCast(&output, input, xf); - hit = false; - - b2Color color(1.0f, 1.0f, 1.0f); - b2Vec2 vs[4]; - for (int32 i = 0; i < 4; ++i) - { - vs[i] = b2Mul(xf, shape.m_vertices[i]); - } - - g_debugDraw.DrawPolygon(vs, 4, color); - g_debugDraw.DrawSegment(input.p1, input.p2, color); - } -#endif - } - - static Test* Create() { return new RayCast; } - - int32 m_bodyIndex; - b2Body* m_bodies[e_maxBodies]; - b2PolygonShape m_polygons[4]; - b2CircleShape m_circle; - b2EdgeShape m_edge; - float m_degrees; - int32 m_mode; -}; - -static int testIndex = RegisterTest("Collision", "Ray Cast", RayCast::Create); diff --git a/tests/cpp-tests/Source/Box2DTestBed/tests/restitution.cpp b/tests/cpp-tests/Source/Box2DTestBed/tests/restitution.cpp deleted file mode 100644 index 10dd861e57f5..000000000000 --- a/tests/cpp-tests/Source/Box2DTestBed/tests/restitution.cpp +++ /dev/null @@ -1,75 +0,0 @@ -// MIT License - -// Copyright (c) 2019 Erin Catto - -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: - -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. - -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -#include "test.h" - -// Note: even with a restitution of 1.0, there is some energy change -// due to position correction. -class Restitution : public Test -{ -public: - Restitution() - { - const float threshold = 10.0f; - - { - b2BodyDef bd; - b2Body* ground = m_world->CreateBody(&bd); - - b2EdgeShape shape; - shape.SetTwoSided(b2Vec2(-40.0f, 0.0f), b2Vec2(40.0f, 0.0f)); - - b2FixtureDef fd; - fd.shape = &shape; - fd.restitutionThreshold = threshold; - ground->CreateFixture(&fd); - } - - { - b2CircleShape shape; - shape.m_radius = 1.0f; - - b2FixtureDef fd; - fd.shape = &shape; - fd.density = 1.0f; - - float restitution[7] = {0.0f, 0.1f, 0.3f, 0.5f, 0.75f, 0.9f, 1.0f}; - - for (int32 i = 0; i < 7; ++i) - { - b2BodyDef bd; - bd.type = b2_dynamicBody; - bd.position.Set(-10.0f + 3.0f * i, 20.0f); - - b2Body* body = m_world->CreateBody(&bd); - - fd.restitution = restitution[i]; - fd.restitutionThreshold = threshold; - body->CreateFixture(&fd); - } - } - } - - static Test* Create() { return new Restitution; } -}; - -static int testIndex = RegisterTest("Forces", "Restitution", Restitution::Create); diff --git a/tests/cpp-tests/Source/Box2DTestBed/tests/revolute_joint.cpp b/tests/cpp-tests/Source/Box2DTestBed/tests/revolute_joint.cpp deleted file mode 100644 index 5c29859d833e..000000000000 --- a/tests/cpp-tests/Source/Box2DTestBed/tests/revolute_joint.cpp +++ /dev/null @@ -1,157 +0,0 @@ -// MIT License - -// Copyright (c) 2019 Erin Catto - -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: - -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. - -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -#include "settings.h" -#include "test.h" -#include "ImGui/ImGuiPresenter.h" - -class RevoluteJoint : public Test -{ -public: - RevoluteJoint() - { - b2Body* ground = NULL; - { - b2BodyDef bd; - ground = m_world->CreateBody(&bd); - - b2EdgeShape shape; - shape.SetTwoSided(b2Vec2(-40.0f, 0.0f), b2Vec2(40.0f, 0.0f)); - - b2FixtureDef fd; - fd.shape = &shape; - // fd.filter.categoryBits = 2; - - ground->CreateFixture(&fd); - } - - m_enableLimit = true; - m_enableMotor = false; - m_motorSpeed = 1.0f; - - { - b2PolygonShape shape; - shape.SetAsBox(0.25f, 3.0f, b2Vec2(0.0f, 3.0f), 0.0f); - - b2BodyDef bd; - bd.type = b2_dynamicBody; - bd.position.Set(-10.0f, 20.0f); - b2Body* body = m_world->CreateBody(&bd); - body->CreateFixture(&shape, 5.0f); - - b2RevoluteJointDef jd; - jd.Initialize(ground, body, b2Vec2(-10.0f, 20.5f)); - jd.motorSpeed = m_motorSpeed; - jd.maxMotorTorque = 10000.0f; - jd.enableMotor = m_enableMotor; - jd.lowerAngle = -0.25f * b2_pi; - jd.upperAngle = 0.5f * b2_pi; - jd.enableLimit = m_enableLimit; - - m_joint1 = (b2RevoluteJoint*)m_world->CreateJoint(&jd); - } - - { - b2CircleShape circle_shape; - circle_shape.m_radius = 2.0f; - - b2BodyDef circle_bd; - circle_bd.type = b2_dynamicBody; - circle_bd.position.Set(5.0f, 30.0f); - - b2FixtureDef fd; - fd.density = 5.0f; - fd.filter.maskBits = 1; - fd.shape = &circle_shape; - - m_ball = m_world->CreateBody(&circle_bd); - m_ball->CreateFixture(&fd); - - b2PolygonShape polygon_shape; - polygon_shape.SetAsBox(10.0f, 0.5f, b2Vec2(-10.0f, 0.0f), 0.0f); - - b2BodyDef polygon_bd; - polygon_bd.position.Set(20.0f, 10.0f); - polygon_bd.type = b2_dynamicBody; - polygon_bd.bullet = true; - b2Body* polygon_body = m_world->CreateBody(&polygon_bd); - polygon_body->CreateFixture(&polygon_shape, 2.0f); - - b2RevoluteJointDef jd; - jd.Initialize(ground, polygon_body, b2Vec2(19.0f, 10.0f)); - jd.lowerAngle = -0.25f * b2_pi; - jd.upperAngle = 0.0f * b2_pi; - jd.enableLimit = true; - jd.enableMotor = true; - jd.motorSpeed = 0.0f; - jd.maxMotorTorque = 10000.0f; - - m_joint2 = (b2RevoluteJoint*)m_world->CreateJoint(&jd); - } - } - - void UpdateUI() override - { - // ImGui::SetNextWindowPos(ImVec2(10.0f, 100.0f)); - ImGui::SetNextWindowSize(ImVec2(200.0f, 100.0f)); - ImGui::Begin("Joint Controls", nullptr, ImGuiWindowFlags_NoResize); - - if (ImGui::Checkbox("Limit", &m_enableLimit)) - { - m_joint1->EnableLimit(m_enableLimit); - } - - if (ImGui::Checkbox("Motor", &m_enableMotor)) - { - m_joint1->EnableMotor(m_enableMotor); - } - - if (ImGui::SliderFloat("Speed", &m_motorSpeed, -20.0f, 20.0f, "%.0f")) - { - m_joint1->SetMotorSpeed(m_motorSpeed); - } - - ImGui::End(); - } - - void Step(Settings& settings) override - { - Test::Step(settings); - - float torque1 = m_joint1->GetMotorTorque(settings.m_hertz); - DrawString(5, m_textLine, "Motor Torque 1= %4.0f", torque1); - - float torque2 = m_joint2->GetMotorTorque(settings.m_hertz); - DrawString(5, m_textLine, "Motor Torque 2= %4.0f", torque2); - } - - static Test* Create() { return new RevoluteJoint; } - - b2Body* m_ball; - b2RevoluteJoint* m_joint1; - b2RevoluteJoint* m_joint2; - float m_motorSpeed; - bool m_enableMotor; - bool m_enableLimit; -}; - -static int testIndex = RegisterTest("Joints", "Revolute", RevoluteJoint::Create); diff --git a/tests/cpp-tests/Source/Box2DTestBed/tests/rope.cpp b/tests/cpp-tests/Source/Box2DTestBed/tests/rope.cpp deleted file mode 100644 index a36a60105ec7..000000000000 --- a/tests/cpp-tests/Source/Box2DTestBed/tests/rope.cpp +++ /dev/null @@ -1,282 +0,0 @@ -// MIT License - -// Copyright (c) 2019 Erin Catto - -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: - -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. - -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -#include "settings.h" -#include "test.h" -#include "box2d/b2_rope.h" -#include "ImGui/ImGuiPresenter.h" - -/// -class Rope : public Test -{ -public: - Rope() - { - const int32 N = 20; - const float L = 0.5f; - b2Vec2 vertices[N]; - float masses[N]; - - for (int32 i = 0; i < N; ++i) - { - vertices[i].Set(0.0f, L * (N - i)); - masses[i] = 1.0f; - } - masses[0] = 0.0f; - masses[1] = 0.0f; - - m_tuning1.bendHertz = 30.0f; - m_tuning1.bendDamping = 4.0f; - m_tuning1.bendStiffness = 1.0f; - m_tuning1.bendingModel = b2_pbdTriangleBendingModel; - m_tuning1.isometric = true; - - m_tuning1.stretchHertz = 30.0f; - m_tuning1.stretchDamping = 4.0f; - m_tuning1.stretchStiffness = 1.0f; - m_tuning1.stretchingModel = b2_pbdStretchingModel; - - m_tuning2.bendHertz = 30.0f; - m_tuning2.bendDamping = 0.7f; - m_tuning2.bendStiffness = 1.0f; - m_tuning2.bendingModel = b2_pbdHeightBendingModel; - m_tuning2.isometric = true; - - m_tuning2.stretchHertz = 30.0f; - m_tuning2.stretchDamping = 1.0f; - m_tuning2.stretchStiffness = 1.0f; - m_tuning2.stretchingModel = b2_pbdStretchingModel; - - m_position1.Set(-5.0f, 15.0f); - m_position2.Set(5.0f, 15.0f); - - b2RopeDef def; - def.vertices = vertices; - def.count = N; - def.gravity.Set(0.0f, -10.0f); - def.masses = masses; - - def.position = m_position1; - def.tuning = m_tuning1; - m_rope1.Create(def); - - def.position = m_position2; - def.tuning = m_tuning2; - m_rope2.Create(def); - - m_iterations1 = 8; - m_iterations2 = 8; - - m_speed = 10.0f; - } - - void UpdateUI() override - { - // ImGui::SetNextWindowPos(ImVec2(10.0f, 100.0f)); - ImGui::SetNextWindowSize(ImVec2(200.0f, 700.0f)); - ImGui::Begin("Tuning", nullptr, ImGuiWindowFlags_NoResize); - - ImGui::Separator(); - - ImGui::PushItemWidth(ImGui::GetWindowWidth() * 0.5f); - - const ImGuiComboFlags comboFlags = 0; - const char* bendModels[] = {"Spring", "PBD Ang", "XPBD Ang", "PBD Dist", "PBD Height", "PBD Triangle"}; - const char* stretchModels[] = {"PBD", "XPBD"}; - - ImGui::Text("Rope 1"); - static int bendModel1 = m_tuning1.bendingModel; - if (ImGui::BeginCombo("Bend Model##1", bendModels[bendModel1], comboFlags)) - { - for (int i = 0; i < IM_ARRAYSIZE(bendModels); ++i) - { - bool isSelected = (bendModel1 == i); - if (ImGui::Selectable(bendModels[i], isSelected)) - { - bendModel1 = i; - m_tuning1.bendingModel = b2BendingModel(i); - } - - if (isSelected) - { - ImGui::SetItemDefaultFocus(); - } - } - ImGui::EndCombo(); - } - - ImGui::SliderFloat("Damping##B1", &m_tuning1.bendDamping, 0.0f, 4.0f, "%.1f"); - ImGui::SliderFloat("Hertz##B1", &m_tuning1.bendHertz, 0.0f, 60.0f, "%.0f"); - ImGui::SliderFloat("Stiffness##B1", &m_tuning1.bendStiffness, 0.0f, 1.0f, "%.1f"); - - ImGui::Checkbox("Isometric##1", &m_tuning1.isometric); - ImGui::Checkbox("Fixed Mass##1", &m_tuning1.fixedEffectiveMass); - ImGui::Checkbox("Warm Start##1", &m_tuning1.warmStart); - - static int stretchModel1 = m_tuning1.stretchingModel; - if (ImGui::BeginCombo("Stretch Model##1", stretchModels[stretchModel1], comboFlags)) - { - for (int i = 0; i < IM_ARRAYSIZE(stretchModels); ++i) - { - bool isSelected = (stretchModel1 == i); - if (ImGui::Selectable(stretchModels[i], isSelected)) - { - stretchModel1 = i; - m_tuning1.stretchingModel = b2StretchingModel(i); - } - - if (isSelected) - { - ImGui::SetItemDefaultFocus(); - } - } - ImGui::EndCombo(); - } - - ImGui::SliderFloat("Damping##S1", &m_tuning1.stretchDamping, 0.0f, 4.0f, "%.1f"); - ImGui::SliderFloat("Hertz##S1", &m_tuning1.stretchHertz, 0.0f, 60.0f, "%.0f"); - ImGui::SliderFloat("Stiffness##S1", &m_tuning1.stretchStiffness, 0.0f, 1.0f, "%.1f"); - - ImGui::SliderInt("Iterations##1", &m_iterations1, 1, 100, "%d"); - - ImGui::Separator(); - - ImGui::Text("Rope 2"); - static int bendModel2 = m_tuning2.bendingModel; - if (ImGui::BeginCombo("Bend Model##2", bendModels[bendModel2], comboFlags)) - { - for (int i = 0; i < IM_ARRAYSIZE(bendModels); ++i) - { - bool isSelected = (bendModel2 == i); - if (ImGui::Selectable(bendModels[i], isSelected)) - { - bendModel2 = i; - m_tuning2.bendingModel = b2BendingModel(i); - } - - if (isSelected) - { - ImGui::SetItemDefaultFocus(); - } - } - ImGui::EndCombo(); - } - - ImGui::SliderFloat("Damping##B2", &m_tuning2.bendDamping, 0.0f, 4.0f, "%.1f"); - ImGui::SliderFloat("Hertz##B2", &m_tuning2.bendHertz, 0.0f, 60.0f, "%.0f"); - ImGui::SliderFloat("Stiffness##B2", &m_tuning2.bendStiffness, 0.0f, 1.0f, "%.1f"); - - ImGui::Checkbox("Isometric##2", &m_tuning2.isometric); - ImGui::Checkbox("Fixed Mass##2", &m_tuning2.fixedEffectiveMass); - ImGui::Checkbox("Warm Start##2", &m_tuning2.warmStart); - - static int stretchModel2 = m_tuning2.stretchingModel; - if (ImGui::BeginCombo("Stretch Model##2", stretchModels[stretchModel2], comboFlags)) - { - for (int i = 0; i < IM_ARRAYSIZE(stretchModels); ++i) - { - bool isSelected = (stretchModel2 == i); - if (ImGui::Selectable(stretchModels[i], isSelected)) - { - stretchModel2 = i; - m_tuning2.stretchingModel = b2StretchingModel(i); - } - - if (isSelected) - { - ImGui::SetItemDefaultFocus(); - } - } - ImGui::EndCombo(); - } - - ImGui::SliderFloat("Damping##S2", &m_tuning2.stretchDamping, 0.0f, 4.0f, "%.1f"); - ImGui::SliderFloat("Hertz##S2", &m_tuning2.stretchHertz, 0.0f, 60.0f, "%.0f"); - ImGui::SliderFloat("Stiffness##S2", &m_tuning2.stretchStiffness, 0.0f, 1.0f, "%.1f"); - - ImGui::SliderInt("Iterations##2", &m_iterations2, 1, 100, "%d"); - - ImGui::Separator(); - - ImGui::SliderFloat("Speed", &m_speed, 10.0f, 100.0f, "%.0f"); - - if (ImGui::Button("Reset")) - { - m_position1.Set(-5.0f, 15.0f); - m_position2.Set(5.0f, 15.0f); - m_rope1.Reset(m_position1); - m_rope2.Reset(m_position2); - } - - ImGui::PopItemWidth(); - - ImGui::End(); - } - - void Step(Settings& settings) override - { - float dt = settings.m_hertz > 0.0f ? 1.0f / settings.m_hertz : 0.0f; - - if (settings.m_pause == 1 && settings.m_singleStep == 0) - { - dt = 0.0f; - } - - // if (glfwGetKey(g_mainWindow, GLFW_KEY_COMMA) == GLFW_PRESS) - //{ - // m_position1.x -= m_speed * dt; - // m_position2.x -= m_speed * dt; - // } - - // if (glfwGetKey(g_mainWindow, GLFW_KEY_PERIOD) == GLFW_PRESS) - //{ - // m_position1.x += m_speed * dt; - // m_position2.x += m_speed * dt; - // } - - m_rope1.SetTuning(m_tuning1); - m_rope2.SetTuning(m_tuning2); - m_rope1.Step(dt, m_iterations1, m_position1); - m_rope2.Step(dt, m_iterations2, m_position2); - - Test::Step(settings); - - m_rope1.Draw(&g_debugDraw); - m_rope2.Draw(&g_debugDraw); - - DrawString(5, m_textLine, "Press comma and period to move left and right"); - } - - static Test* Create() { return new Rope; } - - b2Rope m_rope1; - b2Rope m_rope2; - b2RopeTuning m_tuning1; - b2RopeTuning m_tuning2; - int32 m_iterations1; - int32 m_iterations2; - b2Vec2 m_position1; - b2Vec2 m_position2; - float m_speed; -}; - -static int testIndex = RegisterTest("Rope", "Bending", Rope::Create); diff --git a/tests/cpp-tests/Source/Box2DTestBed/tests/sensor.cpp b/tests/cpp-tests/Source/Box2DTestBed/tests/sensor.cpp deleted file mode 100644 index 9c3985412566..000000000000 --- a/tests/cpp-tests/Source/Box2DTestBed/tests/sensor.cpp +++ /dev/null @@ -1,191 +0,0 @@ -// MIT License - -// Copyright (c) 2019 Erin Catto - -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: - -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. - -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -#include "test.h" -#include "ImGui/ImGuiPresenter.h" - -// This shows how to use sensor shapes. Sensors don't have collision, but report overlap events. -class Sensors : public Test -{ -public: - enum - { - e_count = 7 - }; - - Sensors() - { - { - b2BodyDef bd; - b2Body* ground = m_world->CreateBody(&bd); - - { - b2EdgeShape shape; - shape.SetTwoSided(b2Vec2(-40.0f, 0.0f), b2Vec2(40.0f, 0.0f)); - ground->CreateFixture(&shape, 0.0f); - } - -#if 0 - { - b2FixtureDef sd; - sd.SetAsBox(10.0f, 2.0f, b2Vec2(0.0f, 20.0f), 0.0f); - sd.isSensor = true; - m_sensor = ground->CreateFixture(&sd); - } -#else - { - b2CircleShape shape; - shape.m_radius = 5.0f; - shape.m_p.Set(0.0f, 10.0f); - - b2FixtureDef fd; - fd.shape = &shape; - fd.isSensor = true; - m_sensor = ground->CreateFixture(&fd); - } -#endif - } - - { - b2CircleShape shape; - shape.m_radius = 1.0f; - - for (int32 i = 0; i < e_count; ++i) - { - b2BodyDef bd; - bd.type = b2_dynamicBody; - bd.position.Set(-10.0f + 3.0f * i, 20.0f); - bd.userData.pointer = i; - - m_touching[i] = false; - m_bodies[i] = m_world->CreateBody(&bd); - - m_bodies[i]->CreateFixture(&shape, 1.0f); - } - } - - m_force = 100.0f; - } - - // Implement contact listener. - void BeginContact(b2Contact* contact) override - { - b2Fixture* fixtureA = contact->GetFixtureA(); - b2Fixture* fixtureB = contact->GetFixtureB(); - - if (fixtureA == m_sensor) - { - uintptr_t index = fixtureB->GetBody()->GetUserData().pointer; - if (index < e_count) - { - m_touching[index] = true; - } - } - - if (fixtureB == m_sensor) - { - uintptr_t index = fixtureA->GetBody()->GetUserData().pointer; - if (index < e_count) - { - m_touching[index] = true; - } - } - } - - // Implement contact listener. - void EndContact(b2Contact* contact) override - { - b2Fixture* fixtureA = contact->GetFixtureA(); - b2Fixture* fixtureB = contact->GetFixtureB(); - - if (fixtureA == m_sensor) - { - uintptr_t index = fixtureB->GetBody()->GetUserData().pointer; - if (index < e_count) - { - m_touching[index] = false; - } - } - - if (fixtureB == m_sensor) - { - uintptr_t index = fixtureA->GetBody()->GetUserData().pointer; - if (index < e_count) - { - m_touching[index] = false; - } - } - } - - void UpdateUI() override - { - // ImGui::SetNextWindowPos(ImVec2(10.0f, 100.0f)); - ImGui::SetNextWindowSize(ImVec2(200.0f, 60.0f)); - ImGui::Begin("Sensor Controls", nullptr, ImGuiWindowFlags_NoResize); - - ImGui::SliderFloat("Force", &m_force, 0.0f, 2000.0f, "%.0f"); - - ImGui::End(); - } - - void Step(Settings& settings) override - { - Test::Step(settings); - - // Traverse the contact results. Apply a force on shapes - // that overlap the sensor. - for (int32 i = 0; i < e_count; ++i) - { - if (m_touching[i] == false) - { - continue; - } - - b2Body* body = m_bodies[i]; - b2Body* ground = m_sensor->GetBody(); - - b2CircleShape* circle = (b2CircleShape*)m_sensor->GetShape(); - b2Vec2 center = ground->GetWorldPoint(circle->m_p); - - b2Vec2 position = body->GetPosition(); - - b2Vec2 d = center - position; - if (d.LengthSquared() < FLT_EPSILON * FLT_EPSILON) - { - continue; - } - - d.Normalize(); - b2Vec2 F = m_force * d; - body->ApplyForce(F, position, false); - } - } - - static Test* Create() { return new Sensors; } - - b2Fixture* m_sensor; - b2Body* m_bodies[e_count]; - float m_force; - bool m_touching[e_count]; -}; - -static int testIndex = RegisterTest("Collision", "Sensors", Sensors::Create); diff --git a/tests/cpp-tests/Source/Box2DTestBed/tests/settings.h b/tests/cpp-tests/Source/Box2DTestBed/tests/settings.h deleted file mode 100644 index 7ba71423e898..000000000000 --- a/tests/cpp-tests/Source/Box2DTestBed/tests/settings.h +++ /dev/null @@ -1,83 +0,0 @@ -// MIT License - -// Copyright (c) 2019 Erin Catto - -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: - -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. - -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -#ifndef SETTINGS_H -#define SETTINGS_H - -struct Settings -{ - Settings() { Reset(); } - - void Reset() - { - m_testIndex = 0; - m_windowWidth = 1600; - m_windowHeight = 900; - m_hertz = 60.0f; - m_velocityIterations = 8; - m_positionIterations = 3; - m_drawShapes = true; - m_drawJoints = true; - m_drawAABBs = false; - m_drawContactPoints = false; - m_drawContactNormals = false; - m_drawContactImpulse = false; - m_drawFrictionImpulse = false; - m_drawCOMs = false; - m_drawStats = false; - m_drawProfile = false; - m_enableWarmStarting = true; - m_enableContinuous = true; - m_enableSubStepping = false; - m_enableSleep = true; - m_pause = false; - m_singleStep = false; - } - - void Save(); - void Load(); - - int m_testIndex; - int m_windowWidth; - int m_windowHeight; - float m_hertz; - int m_velocityIterations; - int m_positionIterations; - bool m_drawShapes; - bool m_drawJoints; - bool m_drawAABBs; - bool m_drawContactPoints; - bool m_drawContactNormals; - bool m_drawContactImpulse; - bool m_drawFrictionImpulse; - bool m_drawCOMs; - bool m_drawStats; - bool m_drawProfile; - bool m_enableWarmStarting; - bool m_enableContinuous; - bool m_enableSubStepping; - bool m_enableSleep; - bool m_pause; - bool m_singleStep; -}; - -#endif \ No newline at end of file diff --git a/tests/cpp-tests/Source/Box2DTestBed/tests/shape_cast.cpp b/tests/cpp-tests/Source/Box2DTestBed/tests/shape_cast.cpp deleted file mode 100644 index 60771b589681..000000000000 --- a/tests/cpp-tests/Source/Box2DTestBed/tests/shape_cast.cpp +++ /dev/null @@ -1,189 +0,0 @@ -// MIT License - -// Copyright (c) 2019 Erin Catto - -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: - -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. - -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -#include "test.h" -#include "box2d/b2_distance.h" - -class ShapeCast : public Test -{ -public: - enum - { - e_vertexCount = 8 - }; - - ShapeCast() - { -#if 1 - m_vAs[0].Set(-0.5f, 1.0f); - m_vAs[1].Set(0.5f, 1.0f); - m_vAs[2].Set(0.0f, 0.0f); - m_countA = 3; - m_radiusA = b2_polygonRadius; - - m_vBs[0].Set(-0.5f, -0.5f); - m_vBs[1].Set(0.5f, -0.5f); - m_vBs[2].Set(0.5f, 0.5f); - m_vBs[3].Set(-0.5f, 0.5f); - m_countB = 4; - m_radiusB = b2_polygonRadius; - - m_transformA.p.Set(0.0f, 0.25f); - m_transformA.q.SetIdentity(); - m_transformB.p.Set(-4.0f, 0.0f); - m_transformB.q.SetIdentity(); - m_translationB.Set(8.0f, 0.0f); -#elif 0 - m_vAs[0].Set(0.0f, 0.0f); - m_countA = 1; - m_radiusA = 0.5f; - - m_vBs[0].Set(0.0f, 0.0f); - m_countB = 1; - m_radiusB = 0.5f; - - m_transformA.p.Set(0.0f, 0.25f); - m_transformA.q.SetIdentity(); - m_transformB.p.Set(-4.0f, 0.0f); - m_transformB.q.SetIdentity(); - m_translationB.Set(8.0f, 0.0f); -#else - m_vAs[0].Set(0.0f, 0.0f); - m_vAs[1].Set(2.0f, 0.0f); - m_countA = 2; - m_radiusA = b2_polygonRadius; - - m_vBs[0].Set(0.0f, 0.0f); - m_countB = 1; - m_radiusB = 0.25f; - - // Initial overlap - m_transformA.p.Set(0.0f, 0.0f); - m_transformA.q.SetIdentity(); - m_transformB.p.Set(-0.244360745f, 0.05999358f); - m_transformB.q.SetIdentity(); - m_translationB.Set(0.0f, 0.0399999991f); -#endif - } - - static Test* Create() { return new ShapeCast; } - - void Step(Settings& settings) override - { - Test::Step(settings); - - b2ShapeCastInput input; - input.proxyA.Set(m_vAs, m_countA, m_radiusA); - input.proxyB.Set(m_vBs, m_countB, m_radiusB); - input.transformA = m_transformA; - input.transformB = m_transformB; - input.translationB = m_translationB; - - b2ShapeCastOutput output; - bool hit = b2ShapeCast(&output, &input); - - b2Transform transformB2; - transformB2.q = m_transformB.q; - transformB2.p = m_transformB.p + output.lambda * input.translationB; - - b2DistanceInput distanceInput; - distanceInput.proxyA.Set(m_vAs, m_countA, m_radiusA); - distanceInput.proxyB.Set(m_vBs, m_countB, m_radiusB); - distanceInput.transformA = m_transformA; - distanceInput.transformB = transformB2; - distanceInput.useRadii = false; - b2SimplexCache simplexCache; - simplexCache.count = 0; - b2DistanceOutput distanceOutput; - - b2Distance(&distanceOutput, &simplexCache, &distanceInput); - - DrawString(5, m_textLine, "hit = %s, iters = %d, lambda = %g, distance = %g", hit ? "true" : "false", - output.iterations, output.lambda, distanceOutput.distance); - - b2Vec2 vertices[b2_maxPolygonVertices]; - - for (int32 i = 0; i < m_countA; ++i) - { - vertices[i] = b2Mul(m_transformA, m_vAs[i]); - } - - if (m_countA == 1) - { - g_debugDraw.DrawCircle(vertices[0], m_radiusA, b2Color(0.9f, 0.9f, 0.9f)); - } - else - { - g_debugDraw.DrawPolygon(vertices, m_countA, b2Color(0.9f, 0.9f, 0.9f)); - } - - for (int32 i = 0; i < m_countB; ++i) - { - vertices[i] = b2Mul(m_transformB, m_vBs[i]); - } - - if (m_countB == 1) - { - g_debugDraw.DrawCircle(vertices[0], m_radiusB, b2Color(0.5f, 0.9f, 0.5f)); - } - else - { - g_debugDraw.DrawPolygon(vertices, m_countB, b2Color(0.5f, 0.9f, 0.5f)); - } - - for (int32 i = 0; i < m_countB; ++i) - { - vertices[i] = b2Mul(transformB2, m_vBs[i]); - } - - if (m_countB == 1) - { - g_debugDraw.DrawCircle(vertices[0], m_radiusB, b2Color(0.5f, 0.7f, 0.9f)); - } - else - { - g_debugDraw.DrawPolygon(vertices, m_countB, b2Color(0.5f, 0.7f, 0.9f)); - } - - if (hit) - { - b2Vec2 p1 = output.point; - g_debugDraw.DrawPoint(p1, 10.0f, b2Color(0.9f, 0.3f, 0.3f)); - b2Vec2 p2 = p1 + output.normal; - g_debugDraw.DrawSegment(p1, p2, b2Color(0.9f, 0.3f, 0.3f)); - } - } - - b2Vec2 m_vAs[b2_maxPolygonVertices]; - int32 m_countA; - float m_radiusA; - - b2Vec2 m_vBs[b2_maxPolygonVertices]; - int32 m_countB; - float m_radiusB; - - b2Transform m_transformA; - b2Transform m_transformB; - b2Vec2 m_translationB; -}; - -static int testIndex = RegisterTest("Collision", "Shape Cast", ShapeCast::Create); diff --git a/tests/cpp-tests/Source/Box2DTestBed/tests/shape_editing.cpp b/tests/cpp-tests/Source/Box2DTestBed/tests/shape_editing.cpp deleted file mode 100644 index c7b753b52400..000000000000 --- a/tests/cpp-tests/Source/Box2DTestBed/tests/shape_editing.cpp +++ /dev/null @@ -1,103 +0,0 @@ -// MIT License - -// Copyright (c) 2019 Erin Catto - -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: - -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. - -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -#include "test.h" - -class ShapeEditing : public Test -{ -public: - ShapeEditing() - { - { - b2BodyDef bd; - b2Body* ground = m_world->CreateBody(&bd); - - b2EdgeShape shape; - shape.SetTwoSided(b2Vec2(-40.0f, 0.0f), b2Vec2(40.0f, 0.0f)); - ground->CreateFixture(&shape, 0.0f); - } - - b2BodyDef bd; - bd.type = b2_dynamicBody; - bd.position.Set(0.0f, 10.0f); - m_body = m_world->CreateBody(&bd); - - b2PolygonShape shape; - shape.SetAsBox(4.0f, 4.0f, b2Vec2(0.0f, 0.0f), 0.0f); - m_fixture1 = m_body->CreateFixture(&shape, 10.0f); - - m_fixture2 = NULL; - - m_sensor = false; - } - - void Keyboard(int key) override - { - switch (key) - { - case GLFW_KEY_C: - if (m_fixture2 == NULL) - { - b2CircleShape shape; - shape.m_radius = 3.0f; - shape.m_p.Set(0.5f, -4.0f); - m_fixture2 = m_body->CreateFixture(&shape, 10.0f); - m_body->SetAwake(true); - } - break; - - case GLFW_KEY_D: - if (m_fixture2 != NULL) - { - m_body->DestroyFixture(m_fixture2); - m_fixture2 = NULL; - m_body->SetAwake(true); - } - break; - - case GLFW_KEY_S: - if (m_fixture2 != NULL) - { - m_sensor = !m_sensor; - m_fixture2->SetSensor(m_sensor); - } - break; - } - } - - void Step(Settings& settings) override - { - Test::Step(settings); - DrawString(5, m_textLine, "Press: (c) create a shape, (d) destroy a shape."); - - DrawString(5, m_textLine, "sensor = %d", m_sensor); - } - - static Test* Create() { return new ShapeEditing; } - - b2Body* m_body; - b2Fixture* m_fixture1; - b2Fixture* m_fixture2; - bool m_sensor; -}; - -static int testIndex = RegisterTest("Examples", "Shape Editing", ShapeEditing::Create); diff --git a/tests/cpp-tests/Source/Box2DTestBed/tests/skier.cpp b/tests/cpp-tests/Source/Box2DTestBed/tests/skier.cpp deleted file mode 100644 index 05529640a25c..000000000000 --- a/tests/cpp-tests/Source/Box2DTestBed/tests/skier.cpp +++ /dev/null @@ -1,146 +0,0 @@ -/* -Test case for collision/jerking issue. -*/ - -#include "test.h" - -#include -#include - -class Skier : public Test -{ -public: - Skier() - { - b2Body* ground = NULL; - { - b2BodyDef bd; - ground = m_world->CreateBody(&bd); - - float const PlatformWidth = 8.0f; - - /* - First angle is from the horizontal and should be negative for a downward slope. - Second angle is relative to the preceding slope, and should be positive, creating a kind of - loose 'Z'-shape from the 3 edges. - If A1 = -10, then A2 <= ~1.5 will result in the collision glitch. - If A1 = -30, then A2 <= ~10.0 will result in the glitch. - */ - float const Angle1Degrees = -30.0f; - float const Angle2Degrees = 10.0f; - - /* - The larger the value of SlopeLength, the less likely the glitch will show up. - */ - float const SlopeLength = 2.0f; - - float const SurfaceFriction = 0.2f; - - // Convert to radians - float const Slope1Incline = -Angle1Degrees * b2_pi / 180.0f; - float const Slope2Incline = Slope1Incline - Angle2Degrees * b2_pi / 180.0f; - // - - m_platform_width = PlatformWidth; - - // Horizontal platform - b2Vec2 v1(-PlatformWidth, 0.0f); - b2Vec2 v2(0.0f, 0.0f); - b2Vec2 v3(SlopeLength * cosf(Slope1Incline), -SlopeLength * sinf(Slope1Incline)); - b2Vec2 v4(v3.x + SlopeLength * cosf(Slope2Incline), v3.y - SlopeLength * sinf(Slope2Incline)); - b2Vec2 v5(v4.x, v4.y - 1.0f); - - b2Vec2 vertices[5] = {v5, v4, v3, v2, v1}; - - b2ChainShape shape; - shape.CreateLoop(vertices, 5); - b2FixtureDef fd; - fd.shape = &shape; - fd.density = 0.0f; - fd.friction = SurfaceFriction; - - ground->CreateFixture(&fd); - } - - { - float const BodyWidth = 1.0f; - float const BodyHeight = 2.5f; - float const SkiLength = 3.0f; - - /* - Larger values for this seem to alleviate the issue to some extent. - */ - float const SkiThickness = 0.3f; - - float const SkiFriction = 0.0f; - float const SkiRestitution = 0.15f; - - b2BodyDef bd; - bd.type = b2_dynamicBody; - - float initial_y = BodyHeight / 2 + SkiThickness; - bd.position.Set(-m_platform_width / 2, initial_y); - - b2Body* skier = m_world->CreateBody(&bd); - - b2PolygonShape ski; - b2Vec2 verts[4]; - verts[0].Set(-SkiLength / 2 - SkiThickness, -BodyHeight / 2); - verts[1].Set(-SkiLength / 2, -BodyHeight / 2 - SkiThickness); - verts[2].Set(SkiLength / 2, -BodyHeight / 2 - SkiThickness); - verts[3].Set(SkiLength / 2 + SkiThickness, -BodyHeight / 2); - ski.Set(verts, 4); - - b2FixtureDef fd; - fd.density = 1.0f; - - fd.friction = SkiFriction; - fd.restitution = SkiRestitution; - - fd.shape = &ski; - skier->CreateFixture(&fd); - - skier->SetLinearVelocity(b2Vec2(0.5f, 0.0f)); - - m_skier = skier; - } - - // g_camera.m_center = b2Vec2(m_platform_width / 2.0f, 0.0f); - // g_camera.m_zoom = 0.4f; - m_fixed_camera = true; - } - - void Keyboard(int key) override - { - switch (key) - { - case GLFW_KEY_C: - m_fixed_camera = !m_fixed_camera; - if (m_fixed_camera) - { - // g_camera.m_center = b2Vec2(m_platform_width / 2.0f, 0.0f); - } - break; - } - } - - void Step(Settings& settings) override - { - DrawString(5, m_textLine, "Keys: c = Camera fixed/tracking"); - - if (!m_fixed_camera) - { - // g_camera.m_center = m_skier->GetPosition(); - } - - Test::Step(settings); - } - - static Test* Create() { return new Skier; } - - b2Body* m_skier; - float m_platform_width; - bool m_fixed_camera; -}; - -static int testIndex = RegisterTest("Bugs", "Skier", Skier::Create); diff --git a/tests/cpp-tests/Source/Box2DTestBed/tests/slider_crank_1.cpp b/tests/cpp-tests/Source/Box2DTestBed/tests/slider_crank_1.cpp deleted file mode 100644 index e09e8ade1992..000000000000 --- a/tests/cpp-tests/Source/Box2DTestBed/tests/slider_crank_1.cpp +++ /dev/null @@ -1,103 +0,0 @@ -// MIT License - -// Copyright (c) 2019 Erin Catto - -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: - -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. - -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -#include "test.h" - -// A basic slider crank created for GDC tutorial: Understanding Constraints -class SliderCrank1 : public Test -{ -public: - SliderCrank1() - { - b2Body* ground = NULL; - { - b2BodyDef bd; - bd.position.Set(0.0f, 17.0f); - ground = m_world->CreateBody(&bd); - } - - { - b2Body* prevBody = ground; - - // Define crank. - { - b2PolygonShape shape; - shape.SetAsBox(4.0f, 1.0f); - - b2BodyDef bd; - bd.type = b2_dynamicBody; - bd.position.Set(-8.0f, 20.0f); - b2Body* body = m_world->CreateBody(&bd); - body->CreateFixture(&shape, 2.0f); - - b2RevoluteJointDef rjd; - rjd.Initialize(prevBody, body, b2Vec2(-12.0f, 20.0f)); - m_world->CreateJoint(&rjd); - - prevBody = body; - } - - // Define connecting rod - { - b2PolygonShape shape; - shape.SetAsBox(8.0f, 1.0f); - - b2BodyDef bd; - bd.type = b2_dynamicBody; - bd.position.Set(4.0f, 20.0f); - b2Body* body = m_world->CreateBody(&bd); - body->CreateFixture(&shape, 2.0f); - - b2RevoluteJointDef rjd; - rjd.Initialize(prevBody, body, b2Vec2(-4.0f, 20.0f)); - m_world->CreateJoint(&rjd); - - prevBody = body; - } - - // Define piston - { - b2PolygonShape shape; - shape.SetAsBox(3.0f, 3.0f); - - b2BodyDef bd; - bd.type = b2_dynamicBody; - bd.fixedRotation = true; - bd.position.Set(12.0f, 20.0f); - b2Body* body = m_world->CreateBody(&bd); - body->CreateFixture(&shape, 2.0f); - - b2RevoluteJointDef rjd; - rjd.Initialize(prevBody, body, b2Vec2(12.0f, 20.0f)); - m_world->CreateJoint(&rjd); - - b2PrismaticJointDef pjd; - pjd.Initialize(ground, body, b2Vec2(12.0f, 17.0f), b2Vec2(1.0f, 0.0f)); - m_world->CreateJoint(&pjd); - } - } - } - - static Test* Create() { return new SliderCrank1; } -}; - -static int testIndex = RegisterTest("Examples", "Slider Crank 1", SliderCrank1::Create); diff --git a/tests/cpp-tests/Source/Box2DTestBed/tests/slider_crank_2.cpp b/tests/cpp-tests/Source/Box2DTestBed/tests/slider_crank_2.cpp deleted file mode 100644 index f55a05859cf6..000000000000 --- a/tests/cpp-tests/Source/Box2DTestBed/tests/slider_crank_2.cpp +++ /dev/null @@ -1,156 +0,0 @@ -// MIT License - -// Copyright (c) 2019 Erin Catto - -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: - -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. - -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -#include "settings.h" -#include "test.h" - -// A motor driven slider crank with joint friction. - -class SliderCrank2 : public Test -{ -public: - SliderCrank2() - { - b2Body* ground = NULL; - { - b2BodyDef bd; - ground = m_world->CreateBody(&bd); - - b2EdgeShape shape; - shape.SetTwoSided(b2Vec2(-40.0f, 0.0f), b2Vec2(40.0f, 0.0f)); - ground->CreateFixture(&shape, 0.0f); - } - - { - b2Body* prevBody = ground; - - // Define crank. - { - b2PolygonShape shape; - shape.SetAsBox(0.5f, 2.0f); - - b2BodyDef bd; - bd.type = b2_dynamicBody; - bd.position.Set(0.0f, 7.0f); - b2Body* body = m_world->CreateBody(&bd); - body->CreateFixture(&shape, 2.0f); - - b2RevoluteJointDef rjd; - rjd.Initialize(prevBody, body, b2Vec2(0.0f, 5.0f)); - rjd.motorSpeed = 1.0f * b2_pi; - rjd.maxMotorTorque = 10000.0f; - rjd.enableMotor = true; - m_joint1 = (b2RevoluteJoint*)m_world->CreateJoint(&rjd); - - prevBody = body; - } - - // Define follower. - { - b2PolygonShape shape; - shape.SetAsBox(0.5f, 4.0f); - - b2BodyDef bd; - bd.type = b2_dynamicBody; - bd.position.Set(0.0f, 13.0f); - b2Body* body = m_world->CreateBody(&bd); - body->CreateFixture(&shape, 2.0f); - - b2RevoluteJointDef rjd; - rjd.Initialize(prevBody, body, b2Vec2(0.0f, 9.0f)); - rjd.enableMotor = false; - m_world->CreateJoint(&rjd); - - prevBody = body; - } - - // Define piston - { - b2PolygonShape shape; - shape.SetAsBox(1.5f, 1.5f); - - b2BodyDef bd; - bd.type = b2_dynamicBody; - bd.fixedRotation = true; - bd.position.Set(0.0f, 17.0f); - b2Body* body = m_world->CreateBody(&bd); - body->CreateFixture(&shape, 2.0f); - - b2RevoluteJointDef rjd; - rjd.Initialize(prevBody, body, b2Vec2(0.0f, 17.0f)); - m_world->CreateJoint(&rjd); - - b2PrismaticJointDef pjd; - pjd.Initialize(ground, body, b2Vec2(0.0f, 17.0f), b2Vec2(0.0f, 1.0f)); - - pjd.maxMotorForce = 1000.0f; - pjd.enableMotor = true; - - m_joint2 = (b2PrismaticJoint*)m_world->CreateJoint(&pjd); - } - - // Create a payload - { - b2PolygonShape shape; - shape.SetAsBox(1.5f, 1.5f); - - b2BodyDef bd; - bd.type = b2_dynamicBody; - bd.position.Set(0.0f, 23.0f); - b2Body* body = m_world->CreateBody(&bd); - body->CreateFixture(&shape, 2.0f); - } - } - } - - void Keyboard(int key) override - { - switch (key) - { - case GLFW_KEY_F: - m_joint2->EnableMotor(!m_joint2->IsMotorEnabled()); - m_joint2->GetBodyB()->SetAwake(true); - break; - - case GLFW_KEY_M: - m_joint1->EnableMotor(!m_joint1->IsMotorEnabled()); - m_joint1->GetBodyB()->SetAwake(true); - break; - } - } - - void Step(Settings& settings) override - { - Test::Step(settings); - DrawString(5, m_textLine, "Keys: (f) toggle friction, (m) toggle motor"); - - float torque = m_joint1->GetMotorTorque(settings.m_hertz); - DrawString(5, m_textLine, "Motor Torque = %5.0f", (float)torque); - } - - static Test* Create() { return new SliderCrank2; } - - b2RevoluteJoint* m_joint1; - b2PrismaticJoint* m_joint2; -}; - -static int testIndex = RegisterTest("Examples", "Slider Crank 2", SliderCrank2::Create); diff --git a/tests/cpp-tests/Source/Box2DTestBed/tests/test.h b/tests/cpp-tests/Source/Box2DTestBed/tests/test.h deleted file mode 100644 index 680a6226c4cd..000000000000 --- a/tests/cpp-tests/Source/Box2DTestBed/tests/test.h +++ /dev/null @@ -1,150 +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 TEST_H -#define TEST_H - -#include "box2d/box2d.h" -#include "extensions/axmol-ext.h" - -#include - -class Test; -struct Settings; - -#define RAND_LIMIT 32767 -#define DRAW_STRING_NEW_LINE 25 - -/// Random number in range [-1,1] -inline float RandomFloat() -{ - float r = (float)(std::rand() & (RAND_LIMIT)); - r /= RAND_LIMIT; - r = 2.0f * r - 1.0f; - return r; -} - -/// Random floating point number in range [lo, hi] -inline float RandomFloat(float lo, float hi) -{ - float r = (float)(std::rand() & (RAND_LIMIT)); - r /= RAND_LIMIT; - r = (hi - lo) * r + lo; - return r; -} - -// This is called when a joint in the world is implicitly destroyed -// because an attached body is destroyed. This gives us a chance to -// nullify the mouse joint. -class DestructionListener : public b2DestructionListener -{ -public: - void SayGoodbye(b2Fixture* fixture) { B2_NOT_USED(fixture); } - void SayGoodbye(b2Joint* joint); - - Test* test; -}; - -const int32 k_maxContactPoints = 2048; - -struct ContactPoint -{ - b2Fixture* fixtureA; - b2Fixture* fixtureB; - b2Vec2 normal; - b2Vec2 position; - b2PointState state; - float normalImpulse; - float tangentImpulse; - float separation; -}; -class Test : public b2ContactListener -{ -public: - Test(); - virtual ~Test(); - - void DrawTitle(const char* string); - virtual void Step(Settings& settings); - virtual void UpdateUI() {} - virtual void Keyboard(int key) { B2_NOT_USED(key); } - virtual void KeyboardUp(int key) { B2_NOT_USED(key); } - void ShiftMouseDown(const b2Vec2& p); - virtual bool MouseDown(const b2Vec2& p); - virtual void MouseUp(const b2Vec2& p); - virtual void MouseMove(const b2Vec2& p); - void LaunchBomb(); - void LaunchBomb(const b2Vec2& position, const b2Vec2& velocity); - - void SpawnBomb(const b2Vec2& worldPt); - void CompleteBombSpawn(const b2Vec2& p); - - // Let derived tests know that a joint was destroyed. - virtual void JointDestroyed(b2Joint* joint) { B2_NOT_USED(joint); } - - // Callbacks for derived classes. - virtual void BeginContact(b2Contact* contact) override { B2_NOT_USED(contact); } - virtual void EndContact(b2Contact* contact) override { B2_NOT_USED(contact); } - virtual void PreSolve(b2Contact* contact, const b2Manifold* oldManifold) override; - virtual void PostSolve(b2Contact* contact, const b2ContactImpulse* impulse) override - { - B2_NOT_USED(contact); - B2_NOT_USED(impulse); - } - - void ShiftOrigin(const b2Vec2& newOrigin); - - void initShader(void); - void DrawString(int x, int y, const char* fmt, ...); - void DrawString(const b2Vec2& p, const char* fmt, ...); - void DrawAABB(b2AABB* aabb, const b2Color& color); - void Flush(); - - ax::extension::PhysicsDebugNodeBox2D g_debugDraw; - ax::DrawNode* debugDrawNode; - std::string debugString = ""; - - b2World* m_world; - -protected: - friend class DestructionListener; - friend class BoundaryListener; - friend class ContactListener; - - b2Body* m_groundBody; - b2AABB m_worldAABB; - ContactPoint m_points[k_maxContactPoints]; - int32 m_pointCount; - DestructionListener m_destructionListener; - int32 m_textLine; - - b2Body* m_bomb; - b2MouseJoint* m_mouseJoint; - b2Vec2 m_bombSpawnPoint; - bool m_bombSpawning; - b2Vec2 m_mouseWorld; - int32 m_stepCount; - int32 m_textIncrement; - b2Profile m_maxProfile; - b2Profile m_totalProfile; -}; - -typedef Test* TestCreateFcn(); -int RegisterTest(const char* category, const char* name, TestCreateFcn* fcn); - -#endif diff --git a/tests/cpp-tests/Source/Box2DTestBed/tests/theo_jansen.cpp b/tests/cpp-tests/Source/Box2DTestBed/tests/theo_jansen.cpp deleted file mode 100644 index cab814037c1c..000000000000 --- a/tests/cpp-tests/Source/Box2DTestBed/tests/theo_jansen.cpp +++ /dev/null @@ -1,261 +0,0 @@ -// MIT License - -// Copyright (c) 2019 Erin Catto - -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: - -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. - -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -// Inspired by a contribution from roman_m -// Dimensions scooped from APE (http://www.cove.org/ape/index.htm) - -#include "test.h" - -class TheoJansen : public Test -{ -public: - void CreateLeg(float s, const b2Vec2& wheelAnchor) - { - b2Vec2 p1(5.4f * s, -6.1f); - b2Vec2 p2(7.2f * s, -1.2f); - b2Vec2 p3(4.3f * s, -1.9f); - b2Vec2 p4(3.1f * s, 0.8f); - b2Vec2 p5(6.0f * s, 1.5f); - b2Vec2 p6(2.5f * s, 3.7f); - - b2FixtureDef fd1, fd2; - fd1.filter.groupIndex = -1; - fd2.filter.groupIndex = -1; - fd1.density = 1.0f; - fd2.density = 1.0f; - - b2PolygonShape poly1, poly2; - - if (s > 0.0f) - { - b2Vec2 vertices[3]; - - vertices[0] = p1; - vertices[1] = p2; - vertices[2] = p3; - poly1.Set(vertices, 3); - - vertices[0] = b2Vec2_zero; - vertices[1] = p5 - p4; - vertices[2] = p6 - p4; - poly2.Set(vertices, 3); - } - else - { - b2Vec2 vertices[3]; - - vertices[0] = p1; - vertices[1] = p3; - vertices[2] = p2; - poly1.Set(vertices, 3); - - vertices[0] = b2Vec2_zero; - vertices[1] = p6 - p4; - vertices[2] = p5 - p4; - poly2.Set(vertices, 3); - } - - fd1.shape = &poly1; - fd2.shape = &poly2; - - b2BodyDef bd1, bd2; - bd1.type = b2_dynamicBody; - bd2.type = b2_dynamicBody; - bd1.position = m_offset; - bd2.position = p4 + m_offset; - - bd1.angularDamping = 10.0f; - bd2.angularDamping = 10.0f; - - b2Body* body1 = m_world->CreateBody(&bd1); - b2Body* body2 = m_world->CreateBody(&bd2); - - body1->CreateFixture(&fd1); - body2->CreateFixture(&fd2); - - { - b2DistanceJointDef jd; - - // Using a soft distance constraint can reduce some jitter. - // It also makes the structure seem a bit more fluid by - // acting like a suspension system. - float dampingRatio = 0.5f; - float frequencyHz = 10.0f; - - jd.Initialize(body1, body2, p2 + m_offset, p5 + m_offset); - b2LinearStiffness(jd.stiffness, jd.damping, frequencyHz, dampingRatio, jd.bodyA, jd.bodyB); - m_world->CreateJoint(&jd); - - jd.Initialize(body1, body2, p3 + m_offset, p4 + m_offset); - b2LinearStiffness(jd.stiffness, jd.damping, frequencyHz, dampingRatio, jd.bodyA, jd.bodyB); - m_world->CreateJoint(&jd); - - jd.Initialize(body1, m_wheel, p3 + m_offset, wheelAnchor + m_offset); - b2LinearStiffness(jd.stiffness, jd.damping, frequencyHz, dampingRatio, jd.bodyA, jd.bodyB); - m_world->CreateJoint(&jd); - - jd.Initialize(body2, m_wheel, p6 + m_offset, wheelAnchor + m_offset); - b2LinearStiffness(jd.stiffness, jd.damping, frequencyHz, dampingRatio, jd.bodyA, jd.bodyB); - m_world->CreateJoint(&jd); - } - - { - b2RevoluteJointDef jd; - jd.Initialize(body2, m_chassis, p4 + m_offset); - m_world->CreateJoint(&jd); - } - } - - TheoJansen() - { - m_offset.Set(0.0f, 8.0f); - m_motorSpeed = 2.0f; - m_motorOn = true; - b2Vec2 pivot(0.0f, 0.8f); - - // Ground - { - b2BodyDef bd; - b2Body* ground = m_world->CreateBody(&bd); - - b2EdgeShape shape; - shape.SetTwoSided(b2Vec2(-50.0f, 0.0f), b2Vec2(50.0f, 0.0f)); - ground->CreateFixture(&shape, 0.0f); - - shape.SetTwoSided(b2Vec2(-50.0f, 0.0f), b2Vec2(-50.0f, 10.0f)); - ground->CreateFixture(&shape, 0.0f); - - shape.SetTwoSided(b2Vec2(50.0f, 0.0f), b2Vec2(50.0f, 10.0f)); - ground->CreateFixture(&shape, 0.0f); - } - - // Balls - for (int32 i = 0; i < 40; ++i) - { - b2CircleShape shape; - shape.m_radius = 0.25f; - - b2BodyDef bd; - bd.type = b2_dynamicBody; - bd.position.Set(-40.0f + 2.0f * i, 0.5f); - - b2Body* body = m_world->CreateBody(&bd); - body->CreateFixture(&shape, 1.0f); - } - - // Chassis - { - b2PolygonShape shape; - shape.SetAsBox(2.5f, 1.0f); - - b2FixtureDef sd; - sd.density = 1.0f; - sd.shape = &shape; - sd.filter.groupIndex = -1; - b2BodyDef bd; - bd.type = b2_dynamicBody; - bd.position = pivot + m_offset; - m_chassis = m_world->CreateBody(&bd); - m_chassis->CreateFixture(&sd); - } - - { - b2CircleShape shape; - shape.m_radius = 1.6f; - - b2FixtureDef sd; - sd.density = 1.0f; - sd.shape = &shape; - sd.filter.groupIndex = -1; - b2BodyDef bd; - bd.type = b2_dynamicBody; - bd.position = pivot + m_offset; - m_wheel = m_world->CreateBody(&bd); - m_wheel->CreateFixture(&sd); - } - - { - b2RevoluteJointDef jd; - jd.Initialize(m_wheel, m_chassis, pivot + m_offset); - jd.collideConnected = false; - jd.motorSpeed = m_motorSpeed; - jd.maxMotorTorque = 400.0f; - jd.enableMotor = m_motorOn; - m_motorJoint = (b2RevoluteJoint*)m_world->CreateJoint(&jd); - } - - b2Vec2 wheelAnchor; - - wheelAnchor = pivot + b2Vec2(0.0f, -0.8f); - - CreateLeg(-1.0f, wheelAnchor); - CreateLeg(1.0f, wheelAnchor); - - m_wheel->SetTransform(m_wheel->GetPosition(), 120.0f * b2_pi / 180.0f); - CreateLeg(-1.0f, wheelAnchor); - CreateLeg(1.0f, wheelAnchor); - - m_wheel->SetTransform(m_wheel->GetPosition(), -120.0f * b2_pi / 180.0f); - CreateLeg(-1.0f, wheelAnchor); - CreateLeg(1.0f, wheelAnchor); - } - - void Step(Settings& settings) override - { - DrawString(5, m_textLine, "Keys: left = a, brake = s, right = d, toggle motor = m"); - - Test::Step(settings); - } - - void Keyboard(int key) override - { - switch (key) - { - case GLFW_KEY_A: - m_motorJoint->SetMotorSpeed(-m_motorSpeed); - break; - - case GLFW_KEY_S: - m_motorJoint->SetMotorSpeed(0.0f); - break; - - case GLFW_KEY_D: - m_motorJoint->SetMotorSpeed(m_motorSpeed); - break; - - case GLFW_KEY_M: - m_motorJoint->EnableMotor(!m_motorJoint->IsMotorEnabled()); - break; - } - } - - static Test* Create() { return new TheoJansen; } - - b2Vec2 m_offset; - b2Body* m_chassis; - b2Body* m_wheel; - b2RevoluteJoint* m_motorJoint; - bool m_motorOn; - float m_motorSpeed; -}; - -static int testIndex = RegisterTest("Examples", "Theo Jansen", TheoJansen::Create); diff --git a/tests/cpp-tests/Source/Box2DTestBed/tests/tiles.cpp b/tests/cpp-tests/Source/Box2DTestBed/tests/tiles.cpp deleted file mode 100644 index e5b9b5dd5f40..000000000000 --- a/tests/cpp-tests/Source/Box2DTestBed/tests/tiles.cpp +++ /dev/null @@ -1,153 +0,0 @@ -// MIT License - -// Copyright (c) 2019 Erin Catto - -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: - -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. - -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -#include "test.h" - -/// This stress tests the dynamic tree broad-phase. This also shows that tile -/// based collision is _not_ smooth due to Box2D not knowing about adjacency. -class Tiles : public Test -{ -public: - enum - { - e_count = 20 - }; - - Tiles() - { - m_fixtureCount = 0; - b2Timer timer; - - { - float a = 0.5f; - b2BodyDef bd; - bd.position.y = -a; - b2Body* ground = m_world->CreateBody(&bd); - -#if 1 - int32 N = 200; - int32 M = 10; - b2Vec2 position; - position.y = 0.0f; - for (int32 j = 0; j < M; ++j) - { - position.x = -N * a; - for (int32 i = 0; i < N; ++i) - { - b2PolygonShape shape; - shape.SetAsBox(a, a, position, 0.0f); - ground->CreateFixture(&shape, 0.0f); - ++m_fixtureCount; - position.x += 2.0f * a; - } - position.y -= 2.0f * a; - } -#else - int32 N = 200; - int32 M = 10; - b2Vec2 position; - position.x = -N * a; - for (int32 i = 0; i < N; ++i) - { - position.y = 0.0f; - for (int32 j = 0; j < M; ++j) - { - b2PolygonShape shape; - shape.SetAsBox(a, a, position, 0.0f); - ground->CreateFixture(&shape, 0.0f); - position.y -= 2.0f * a; - } - position.x += 2.0f * a; - } -#endif - } - - { - float a = 0.5f; - b2PolygonShape shape; - shape.SetAsBox(a, a); - - b2Vec2 x(-7.0f, 0.75f); - b2Vec2 y; - b2Vec2 deltaX(0.5625f, 1.25f); - b2Vec2 deltaY(1.125f, 0.0f); - - for (int32 i = 0; i < e_count; ++i) - { - y = x; - - for (int32 j = i; j < e_count; ++j) - { - b2BodyDef bd; - bd.type = b2_dynamicBody; - bd.position = y; - - // if (i == 0 && j == 0) - //{ - // bd.allowSleep = false; - // } - // else - //{ - // bd.allowSleep = true; - // } - - b2Body* body = m_world->CreateBody(&bd); - body->CreateFixture(&shape, 5.0f); - ++m_fixtureCount; - y += deltaY; - } - - x += deltaX; - } - } - - m_createTime = timer.GetMilliseconds(); - } - - void Step(Settings& settings) override - { - const b2ContactManager& cm = m_world->GetContactManager(); - int32 height = cm.m_broadPhase.GetTreeHeight(); - int32 leafCount = cm.m_broadPhase.GetProxyCount(); - int32 minimumNodeCount = 2 * leafCount - 1; - float minimumHeight = ceilf(logf(float(minimumNodeCount)) / logf(2.0f)); - DrawString(5, m_textLine, "dynamic tree height = %d, min = %d", height, int32(minimumHeight)); - - Test::Step(settings); - - DrawString(5, m_textLine, "create time = %6.2f ms, fixture count = %d", m_createTime, m_fixtureCount); - - // b2DynamicTree* tree = &m_world->m_contactManager.m_broadPhase.m_tree; - - // if (m_stepCount == 400) - //{ - // tree->RebuildBottomUp(); - // } - } - - static Test* Create() { return new Tiles; } - - int32 m_fixtureCount; - float m_createTime; -}; - -static int testIndex = RegisterTest("Benchmark", "Tiles", Tiles::Create); diff --git a/tests/cpp-tests/Source/Box2DTestBed/tests/time_of_impact.cpp b/tests/cpp-tests/Source/Box2DTestBed/tests/time_of_impact.cpp deleted file mode 100644 index f33b53476ddd..000000000000 --- a/tests/cpp-tests/Source/Box2DTestBed/tests/time_of_impact.cpp +++ /dev/null @@ -1,126 +0,0 @@ -// MIT License - -// Copyright (c) 2019 Erin Catto - -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: - -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. - -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -#include "test.h" -#include "box2d/b2_time_of_impact.h" - -class TimeOfImpact : public Test -{ -public: - TimeOfImpact() - { - m_shapeA.SetAsBox(25.0f, 5.0f); - m_shapeB.SetAsBox(2.5f, 2.5f); - } - - static Test* Create() { return new TimeOfImpact; } - - void Step(Settings& settings) override - { - Test::Step(settings); - - b2Sweep sweepA; - sweepA.c0.Set(24.0f, -60.0f); - sweepA.a0 = 2.95f; - sweepA.c = sweepA.c0; - sweepA.a = sweepA.a0; - sweepA.localCenter.SetZero(); - - b2Sweep sweepB; - sweepB.c0.Set(53.474274f, -50.252514f); - sweepB.a0 = 513.36676f; // - 162.0f * b2_pi; - sweepB.c.Set(54.595478f, -51.083473f); - sweepB.a = 513.62781f; // - 162.0f * b2_pi; - sweepB.localCenter.SetZero(); - - // sweepB.a0 -= 300.0f * b2_pi; - // sweepB.a -= 300.0f * b2_pi; - - b2TOIInput input; - input.proxyA.Set(&m_shapeA, 0); - input.proxyB.Set(&m_shapeB, 0); - input.sweepA = sweepA; - input.sweepB = sweepB; - input.tMax = 1.0f; - - b2TOIOutput output; - - b2TimeOfImpact(&output, &input); - - DrawString(5, m_textLine, "toi = %g", output.t); - - extern B2_API int32 b2_toiMaxIters, b2_toiMaxRootIters; - DrawString(5, m_textLine, "max toi iters = %d, max root iters = %d", b2_toiMaxIters, b2_toiMaxRootIters); - - b2Vec2 vertices[b2_maxPolygonVertices]; - - b2Transform transformA; - sweepA.GetTransform(&transformA, 0.0f); - for (int32 i = 0; i < m_shapeA.m_count; ++i) - { - vertices[i] = b2Mul(transformA, m_shapeA.m_vertices[i]); - } - g_debugDraw.DrawPolygon(vertices, m_shapeA.m_count, b2Color(0.9f, 0.9f, 0.9f)); - - b2Transform transformB; - sweepB.GetTransform(&transformB, 0.0f); - - // b2Vec2 localPoint(2.0f, -0.1f); - - for (int32 i = 0; i < m_shapeB.m_count; ++i) - { - vertices[i] = b2Mul(transformB, m_shapeB.m_vertices[i]); - } - g_debugDraw.DrawPolygon(vertices, m_shapeB.m_count, b2Color(0.5f, 0.9f, 0.5f)); - - sweepB.GetTransform(&transformB, output.t); - for (int32 i = 0; i < m_shapeB.m_count; ++i) - { - vertices[i] = b2Mul(transformB, m_shapeB.m_vertices[i]); - } - g_debugDraw.DrawPolygon(vertices, m_shapeB.m_count, b2Color(0.5f, 0.7f, 0.9f)); - - sweepB.GetTransform(&transformB, 1.0f); - for (int32 i = 0; i < m_shapeB.m_count; ++i) - { - vertices[i] = b2Mul(transformB, m_shapeB.m_vertices[i]); - } - g_debugDraw.DrawPolygon(vertices, m_shapeB.m_count, b2Color(0.9f, 0.5f, 0.5f)); - -#if 0 - for (float t = 0.0f; t < 1.0f; t += 0.1f) - { - sweepB.GetTransform(&transformB, t); - for (int32 i = 0; i < m_shapeB.m_count; ++i) - { - vertices[i] = b2Mul(transformB, m_shapeB.m_vertices[i]); - } - g_debugDraw.DrawPolygon(vertices, m_shapeB.m_count, b2Color(0.9f, 0.5f, 0.5f)); - } -#endif - } - - b2PolygonShape m_shapeA; - b2PolygonShape m_shapeB; -}; - -static int testIndex = RegisterTest("Collision", "Time of Impact", TimeOfImpact::Create); diff --git a/tests/cpp-tests/Source/Box2DTestBed/tests/tumbler.cpp b/tests/cpp-tests/Source/Box2DTestBed/tests/tumbler.cpp deleted file mode 100644 index e19ffae13f29..000000000000 --- a/tests/cpp-tests/Source/Box2DTestBed/tests/tumbler.cpp +++ /dev/null @@ -1,98 +0,0 @@ -// MIT License - -// Copyright (c) 2019 Erin Catto - -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: - -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. - -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -#include "test.h" - -class Tumbler : public Test -{ -public: - enum - { - e_count = 800 - }; - - Tumbler() - { - b2Body* ground = NULL; - { - b2BodyDef bd; - ground = m_world->CreateBody(&bd); - } - - { - b2BodyDef bd; - bd.type = b2_dynamicBody; - bd.allowSleep = false; - bd.position.Set(0.0f, 10.0f); - b2Body* body = m_world->CreateBody(&bd); - - b2PolygonShape shape; - shape.SetAsBox(0.5f, 10.0f, b2Vec2(10.0f, 0.0f), 0.0); - body->CreateFixture(&shape, 5.0f); - shape.SetAsBox(0.5f, 10.0f, b2Vec2(-10.0f, 0.0f), 0.0); - body->CreateFixture(&shape, 5.0f); - shape.SetAsBox(10.0f, 0.5f, b2Vec2(0.0f, 10.0f), 0.0); - body->CreateFixture(&shape, 5.0f); - shape.SetAsBox(10.0f, 0.5f, b2Vec2(0.0f, -10.0f), 0.0); - body->CreateFixture(&shape, 5.0f); - - b2RevoluteJointDef jd; - jd.bodyA = ground; - jd.bodyB = body; - jd.localAnchorA.Set(0.0f, 10.0f); - jd.localAnchorB.Set(0.0f, 0.0f); - jd.referenceAngle = 0.0f; - jd.motorSpeed = 0.05f * b2_pi; - jd.maxMotorTorque = 1e8f; - jd.enableMotor = true; - m_joint = (b2RevoluteJoint*)m_world->CreateJoint(&jd); - } - - m_count = 0; - } - - void Step(Settings& settings) override - { - Test::Step(settings); - - if (m_count < e_count) - { - b2BodyDef bd; - bd.type = b2_dynamicBody; - bd.position.Set(0.0f, 10.0f); - b2Body* body = m_world->CreateBody(&bd); - - b2PolygonShape shape; - shape.SetAsBox(0.125f, 0.125f); - body->CreateFixture(&shape, 1.0f); - - ++m_count; - } - } - - static Test* Create() { return new Tumbler; } - - b2RevoluteJoint* m_joint; - int32 m_count; -}; - -static int testIndex = RegisterTest("Benchmark", "Tumbler", Tumbler::Create); diff --git a/tests/cpp-tests/Source/Box2DTestBed/tests/web.cpp b/tests/cpp-tests/Source/Box2DTestBed/tests/web.cpp deleted file mode 100644 index 658bc9f87bc0..000000000000 --- a/tests/cpp-tests/Source/Box2DTestBed/tests/web.cpp +++ /dev/null @@ -1,215 +0,0 @@ -// MIT License - -// Copyright (c) 2019 Erin Catto - -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: - -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. - -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -#include "test.h" - -// Test distance joints, body destruction, and joint destruction. -class Web : public Test -{ -public: - Web() - { - b2Body* ground = NULL; - { - b2BodyDef bd; - ground = m_world->CreateBody(&bd); - - b2EdgeShape shape; - shape.SetTwoSided(b2Vec2(-40.0f, 0.0f), b2Vec2(40.0f, 0.0f)); - ground->CreateFixture(&shape, 0.0f); - } - - { - b2PolygonShape shape; - shape.SetAsBox(0.5f, 0.5f); - - b2BodyDef bd; - bd.type = b2_dynamicBody; - - bd.position.Set(-5.0f, 5.0f); - m_bodies[0] = m_world->CreateBody(&bd); - m_bodies[0]->CreateFixture(&shape, 5.0f); - - bd.position.Set(5.0f, 5.0f); - m_bodies[1] = m_world->CreateBody(&bd); - m_bodies[1]->CreateFixture(&shape, 5.0f); - - bd.position.Set(5.0f, 15.0f); - m_bodies[2] = m_world->CreateBody(&bd); - m_bodies[2]->CreateFixture(&shape, 5.0f); - - bd.position.Set(-5.0f, 15.0f); - m_bodies[3] = m_world->CreateBody(&bd); - m_bodies[3]->CreateFixture(&shape, 5.0f); - - b2DistanceJointDef jd; - b2Vec2 p1, p2, d; - - float frequencyHz = 2.0f; - float dampingRatio = 0.0f; - - jd.bodyA = ground; - jd.bodyB = m_bodies[0]; - jd.localAnchorA.Set(-10.0f, 0.0f); - jd.localAnchorB.Set(-0.5f, -0.5f); - p1 = jd.bodyA->GetWorldPoint(jd.localAnchorA); - p2 = jd.bodyB->GetWorldPoint(jd.localAnchorB); - d = p2 - p1; - jd.length = d.Length(); - b2LinearStiffness(jd.stiffness, jd.damping, frequencyHz, dampingRatio, jd.bodyA, jd.bodyB); - m_joints[0] = m_world->CreateJoint(&jd); - - jd.bodyA = ground; - jd.bodyB = m_bodies[1]; - jd.localAnchorA.Set(10.0f, 0.0f); - jd.localAnchorB.Set(0.5f, -0.5f); - p1 = jd.bodyA->GetWorldPoint(jd.localAnchorA); - p2 = jd.bodyB->GetWorldPoint(jd.localAnchorB); - d = p2 - p1; - jd.length = d.Length(); - b2LinearStiffness(jd.stiffness, jd.damping, frequencyHz, dampingRatio, jd.bodyA, jd.bodyB); - m_joints[1] = m_world->CreateJoint(&jd); - - jd.bodyA = ground; - jd.bodyB = m_bodies[2]; - jd.localAnchorA.Set(10.0f, 20.0f); - jd.localAnchorB.Set(0.5f, 0.5f); - p1 = jd.bodyA->GetWorldPoint(jd.localAnchorA); - p2 = jd.bodyB->GetWorldPoint(jd.localAnchorB); - d = p2 - p1; - jd.length = d.Length(); - b2LinearStiffness(jd.stiffness, jd.damping, frequencyHz, dampingRatio, jd.bodyA, jd.bodyB); - m_joints[2] = m_world->CreateJoint(&jd); - - jd.bodyA = ground; - jd.bodyB = m_bodies[3]; - jd.localAnchorA.Set(-10.0f, 20.0f); - jd.localAnchorB.Set(-0.5f, 0.5f); - p1 = jd.bodyA->GetWorldPoint(jd.localAnchorA); - p2 = jd.bodyB->GetWorldPoint(jd.localAnchorB); - d = p2 - p1; - jd.length = d.Length(); - b2LinearStiffness(jd.stiffness, jd.damping, frequencyHz, dampingRatio, jd.bodyA, jd.bodyB); - m_joints[3] = m_world->CreateJoint(&jd); - - jd.bodyA = m_bodies[0]; - jd.bodyB = m_bodies[1]; - jd.localAnchorA.Set(0.5f, 0.0f); - jd.localAnchorB.Set(-0.5f, 0.0f); - ; - p1 = jd.bodyA->GetWorldPoint(jd.localAnchorA); - p2 = jd.bodyB->GetWorldPoint(jd.localAnchorB); - d = p2 - p1; - jd.length = d.Length(); - b2LinearStiffness(jd.stiffness, jd.damping, frequencyHz, dampingRatio, jd.bodyA, jd.bodyB); - m_joints[4] = m_world->CreateJoint(&jd); - - jd.bodyA = m_bodies[1]; - jd.bodyB = m_bodies[2]; - jd.localAnchorA.Set(0.0f, 0.5f); - jd.localAnchorB.Set(0.0f, -0.5f); - p1 = jd.bodyA->GetWorldPoint(jd.localAnchorA); - p2 = jd.bodyB->GetWorldPoint(jd.localAnchorB); - d = p2 - p1; - jd.length = d.Length(); - b2LinearStiffness(jd.stiffness, jd.damping, frequencyHz, dampingRatio, jd.bodyA, jd.bodyB); - m_joints[5] = m_world->CreateJoint(&jd); - - jd.bodyA = m_bodies[2]; - jd.bodyB = m_bodies[3]; - jd.localAnchorA.Set(-0.5f, 0.0f); - jd.localAnchorB.Set(0.5f, 0.0f); - p1 = jd.bodyA->GetWorldPoint(jd.localAnchorA); - p2 = jd.bodyB->GetWorldPoint(jd.localAnchorB); - d = p2 - p1; - jd.length = d.Length(); - b2LinearStiffness(jd.stiffness, jd.damping, frequencyHz, dampingRatio, jd.bodyA, jd.bodyB); - m_joints[6] = m_world->CreateJoint(&jd); - - jd.bodyA = m_bodies[3]; - jd.bodyB = m_bodies[0]; - jd.localAnchorA.Set(0.0f, -0.5f); - jd.localAnchorB.Set(0.0f, 0.5f); - p1 = jd.bodyA->GetWorldPoint(jd.localAnchorA); - p2 = jd.bodyB->GetWorldPoint(jd.localAnchorB); - d = p2 - p1; - jd.length = d.Length(); - b2LinearStiffness(jd.stiffness, jd.damping, frequencyHz, dampingRatio, jd.bodyA, jd.bodyB); - m_joints[7] = m_world->CreateJoint(&jd); - } - } - - void Keyboard(int key) override - { - switch (key) - { - case GLFW_KEY_B: - for (int32 i = 0; i < 4; ++i) - { - if (m_bodies[i]) - { - m_world->DestroyBody(m_bodies[i]); - m_bodies[i] = NULL; - break; - } - } - break; - - case GLFW_KEY_J: - for (int32 i = 0; i < 8; ++i) - { - if (m_joints[i]) - { - m_world->DestroyJoint(m_joints[i]); - m_joints[i] = NULL; - break; - } - } - break; - } - } - - void Step(Settings& settings) override - { - Test::Step(settings); - DrawString(5, m_textLine, "Press: (b) to delete a body, (j) to delete a joint"); - } - - void JointDestroyed(b2Joint* joint) override - { - for (int32 i = 0; i < 8; ++i) - { - if (m_joints[i] == joint) - { - m_joints[i] = NULL; - break; - } - } - } - - static Test* Create() { return new Web; } - - b2Body* m_bodies[4]; - b2Joint* m_joints[8]; -}; - -static int testIndex = RegisterTest("Examples", "Web", Web::Create); diff --git a/tests/cpp-tests/Source/Box2DTestBed/tests/wheel_joint.cpp b/tests/cpp-tests/Source/Box2DTestBed/tests/wheel_joint.cpp deleted file mode 100644 index 08f62427232f..000000000000 --- a/tests/cpp-tests/Source/Box2DTestBed/tests/wheel_joint.cpp +++ /dev/null @@ -1,121 +0,0 @@ -// MIT License - -// Copyright (c) 2019 Erin Catto - -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: - -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. - -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -#include "settings.h" -#include "test.h" -#include "ImGui/ImGuiPresenter.h" - -// Test the wheel joint with motor, spring, and limit options. -class WheelJoint : public Test -{ -public: - WheelJoint() - { - b2Body* ground = NULL; - { - b2BodyDef bd; - ground = m_world->CreateBody(&bd); - - b2EdgeShape shape; - shape.SetTwoSided(b2Vec2(-40.0f, 0.0f), b2Vec2(40.0f, 0.0f)); - ground->CreateFixture(&shape, 0.0f); - } - - m_enableLimit = true; - m_enableMotor = false; - m_motorSpeed = 10.0f; - - { - b2CircleShape shape; - shape.m_radius = 2.0f; - - b2BodyDef bd; - bd.type = b2_dynamicBody; - bd.position.Set(0.0f, 10.0f); - bd.allowSleep = false; - b2Body* body = m_world->CreateBody(&bd); - body->CreateFixture(&shape, 5.0f); - - b2WheelJointDef jd; - - // Horizontal - jd.Initialize(ground, body, bd.position, b2Vec2(0.0f, 1.0f)); - - jd.motorSpeed = m_motorSpeed; - jd.maxMotorTorque = 10000.0f; - jd.enableMotor = m_enableMotor; - jd.lowerTranslation = -3.0f; - jd.upperTranslation = 3.0f; - jd.enableLimit = m_enableLimit; - - float hertz = 1.0f; - float dampingRatio = 0.7f; - b2LinearStiffness(jd.stiffness, jd.damping, hertz, dampingRatio, ground, body); - - m_joint = (b2WheelJoint*)m_world->CreateJoint(&jd); - } - } - - void Step(Settings& settings) override - { - Test::Step(settings); - - float torque = m_joint->GetMotorTorque(settings.m_hertz); - DrawString(5, m_textLine, "Motor Torque = %4.0f", torque); - - b2Vec2 F = m_joint->GetReactionForce(settings.m_hertz); - DrawString(5, m_textLine, "Reaction Force = (%4.1f, %4.1f)", F.x, F.y); - } - - void UpdateUI() override - { - // ImGui::SetNextWindowPos(ImVec2(10.0f, 100.0f)); - ImGui::SetNextWindowSize(ImVec2(200.0f, 100.0f)); - ImGui::Begin("Joint Controls", nullptr, ImGuiWindowFlags_NoResize); - - if (ImGui::Checkbox("Limit", &m_enableLimit)) - { - m_joint->EnableLimit(m_enableLimit); - } - - if (ImGui::Checkbox("Motor", &m_enableMotor)) - { - m_joint->EnableMotor(m_enableMotor); - } - - if (ImGui::SliderFloat("Speed", &m_motorSpeed, -100.0f, 100.0f, "%.0f")) - { - m_joint->SetMotorSpeed(m_motorSpeed); - } - - ImGui::End(); - } - - static Test* Create() { return new WheelJoint; } - - b2WheelJoint* m_joint; - float m_motorSpeed; - bool m_enableMotor; - bool m_enableLimit; -}; - -static int testIndex = RegisterTest("Joints", "Wheel", WheelJoint::Create); diff --git a/tests/cpp-tests/Source/Box2DTestBed/tests/wrecking_ball.cpp b/tests/cpp-tests/Source/Box2DTestBed/tests/wrecking_ball.cpp deleted file mode 100644 index 69a7745e4c89..000000000000 --- a/tests/cpp-tests/Source/Box2DTestBed/tests/wrecking_ball.cpp +++ /dev/null @@ -1,161 +0,0 @@ -// MIT License - -// Copyright (c) 2019 Erin Catto - -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: - -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. - -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -#include "test.h" -#include "ImGui/ImGuiPresenter.h" - -/// This test shows how a distance joint can be used to stabilize a chain of -/// bodies with a heavy payload. Notice that the distance joint just prevents -/// excessive stretching and has no other effect. -/// By disabling the distance joint you can see that the Box2D solver has trouble -/// supporting heavy bodies with light bodies. Try playing around with the -/// densities, time step, and iterations to see how they affect stability. -/// This test also shows how to use contact filtering. Filtering is configured -/// so that the payload does not collide with the chain. -class WreckingBall : public Test -{ -public: - WreckingBall() - { - b2Body* ground = NULL; - { - b2BodyDef bd; - ground = m_world->CreateBody(&bd); - - b2EdgeShape shape; - shape.SetTwoSided(b2Vec2(-40.0f, 0.0f), b2Vec2(40.0f, 0.0f)); - ground->CreateFixture(&shape, 0.0f); - } - - { - b2PolygonShape shape; - shape.SetAsBox(0.5f, 0.125f); - - b2FixtureDef fd; - fd.shape = &shape; - fd.density = 20.0f; - fd.friction = 0.2f; - fd.filter.categoryBits = 0x0001; - fd.filter.maskBits = 0xFFFF & ~0x0002; - - b2RevoluteJointDef jd; - jd.collideConnected = false; - - const int32 N = 10; - const float y = 15.0f; - m_distanceJointDef.localAnchorA.Set(0.0f, y); - - b2Body* prevBody = ground; - for (int32 i = 0; i < N; ++i) - { - b2BodyDef bd; - bd.type = b2_dynamicBody; - bd.position.Set(0.5f + 1.0f * i, y); - if (i == N - 1) - { - bd.position.Set(1.0f * i, y); - bd.angularDamping = 0.4f; - } - - b2Body* body = m_world->CreateBody(&bd); - - if (i == N - 1) - { - b2CircleShape circleShape; - circleShape.m_radius = 1.5f; - b2FixtureDef sfd; - sfd.shape = &circleShape; - sfd.density = 100.0f; - sfd.filter.categoryBits = 0x0002; - body->CreateFixture(&sfd); - } - else - { - body->CreateFixture(&fd); - } - - b2Vec2 anchor(float(i), y); - jd.Initialize(prevBody, body, anchor); - m_world->CreateJoint(&jd); - - prevBody = body; - } - - m_distanceJointDef.localAnchorB.SetZero(); - - float extraLength = 0.01f; - m_distanceJointDef.minLength = 0.0f; - m_distanceJointDef.maxLength = N - 1.0f + extraLength; - m_distanceJointDef.bodyB = prevBody; - } - - { - m_distanceJointDef.bodyA = ground; - m_distanceJoint = m_world->CreateJoint(&m_distanceJointDef); - m_stabilize = true; - } - } - - void UpdateUI() override - { - // ImGui::SetNextWindowPos(ImVec2(Test::g_debugDraw.debugNodeOffset.x, 100.0f)); - ImGui::SetNextWindowSize(ImVec2(200.0f, 100.0f)); - ImGui::Begin("Wrecking Ball Controls", nullptr, ImGuiWindowFlags_NoResize); - - if (ImGui::Checkbox("Stabilize", &m_stabilize)) - { - if (m_stabilize == true && m_distanceJoint == nullptr) - { - m_distanceJoint = m_world->CreateJoint(&m_distanceJointDef); - } - else if (m_stabilize == false && m_distanceJoint != nullptr) - { - m_world->DestroyJoint(m_distanceJoint); - m_distanceJoint = nullptr; - } - } - - ImGui::End(); - } - - void Step(Settings& settings) override - { - Test::Step(settings); - - if (m_distanceJoint) - { - DrawString(5, m_textLine, "Distance Joint ON"); - } - else - { - DrawString(5, m_textLine, "Distance Joint OFF"); - } - } - - static Test* Create() { return new WreckingBall; } - - b2DistanceJointDef m_distanceJointDef; - b2Joint* m_distanceJoint; - bool m_stabilize; -}; - -static int testIndex = RegisterTest("Examples", "Wrecking Ball", WreckingBall::Create); diff --git a/tests/cpp-tests/Source/ChipmunkTest/ChipmunkTest.cpp b/tests/cpp-tests/Source/ChipmunkTest/ChipmunkTest.cpp deleted file mode 100644 index 3c2ea6873a40..000000000000 --- a/tests/cpp-tests/Source/ChipmunkTest/ChipmunkTest.cpp +++ /dev/null @@ -1,279 +0,0 @@ -/**************************************************************************** - Copyright (c) 2017-2018 Xiamen Yaji Software Co., Ltd. - Copyright (c) 2021 @aismann; Peter Eismann, Germany; dreifrankensoft - - https://axmol.dev/ - - 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. - ****************************************************************************/ - -// -// Accelerometer + Chipmunk physics + multi touches example -// a cocos2d example -// https://axmol.dev/ -// - -#include "chipmunk/chipmunk.h" - -#include "ChipmunkTest.h" - -using namespace ax; -USING_NS_AX_EXT; - -enum -{ - kTagParentNode = 1, -}; - -enum -{ - Z_PHYSICS_DEBUG = 100, -}; - -// callback to remove Shapes from the Space - -ChipmunkTest::ChipmunkTest() -{ - // enable events - - auto touchListener = EventListenerTouchAllAtOnce::create(); - touchListener->onTouchesEnded = AX_CALLBACK_2(ChipmunkTest::onTouchesEnded, this); - _eventDispatcher->addEventListenerWithSceneGraphPriority(touchListener, this); - - Device::setAccelerometerEnabled(true); - auto accListener = EventListenerAcceleration::create(AX_CALLBACK_2(ChipmunkTest::onAcceleration, this)); - _eventDispatcher->addEventListenerWithSceneGraphPriority(accListener, this); - - // title - auto label = Label::createWithTTF("Multi touch the screen", "fonts/Marker Felt.ttf", 36.0f); - label->setPosition(VisibleRect::center().x, VisibleRect::top().y - 30); - this->addChild(label, -1); - - // reset button - createResetButton(); - - // init physics - - physicsDebugNodeOffset = Vec2(0, 0); - initPhysics(); - -#if 1 - // Use batch node. Faster - auto parent = SpriteBatchNode::create("Images/grossini_dance_atlas.png", 100); - _spriteTexture = parent->getTexture(); -#else - // doesn't use batch node. Slower - _spriteTexture = Director::getInstance()->getTextureCache()->addImage("Images/grossini_dance_atlas.png"); - auto parent = Node::create(); -#endif - addChild(parent, 0, kTagParentNode); - - addNewSpriteAtPosition(ax::Vec2(200.0f, 200.0f)); - - // menu for debug layer - MenuItemFont::setFontSize(18); - auto item = MenuItemFont::create("Toggle debug", AX_CALLBACK_1(ChipmunkTest::toggleDebugCallback, this)); - - auto menu = Menu::create(item, nullptr); - this->addChild(menu); - menu->setPosition(VisibleRect::right().x - 100, VisibleRect::top().y - 60); - - scheduleUpdate(); -} - -void ChipmunkTest::toggleDebugCallback(Object* sender) -{ - _debugLayer->setVisible(!_debugLayer->isVisible()); -} - -ChipmunkTest::~ChipmunkTest() -{ - // manually Free rogue shapes - for (int i = 0; i < 4; i++) - { - cpShapeFree(_walls[i]); - } - -#if AX_TARGET_PLATFORM == AX_PLATFORM_WIN32 - cpSpaceFree(_space); -#else - cpHastySpaceFree(_space); -#endif - - Device::setAccelerometerEnabled(false); -} - -void ChipmunkTest::initPhysics() -{ - - // init chipmunk - // cpInitChipmunk(); - -#if AX_TARGET_PLATFORM == AX_PLATFORM_WIN32 - _space = cpSpaceNew(); -#else - _space = cpHastySpaceNew(); - cpHastySpaceSetThreads(_space, 0); -#endif - - cpSpaceSetGravity(_space, cpv(0.0f, -100.0f)); - - // - // rogue shapes - // We have to free them manually - // - // bottom - _walls[0] = - cpSegmentShapeNew(cpSpaceGetStaticBody(_space), cpv(VisibleRect::leftBottom().x, VisibleRect::leftBottom().y), - cpv(VisibleRect::rightBottom().x, VisibleRect::rightBottom().y), 0.0f); - - // top - _walls[1] = cpSegmentShapeNew(cpSpaceGetStaticBody(_space), cpv(VisibleRect::leftTop().x, VisibleRect::leftTop().y), - cpv(VisibleRect::rightTop().x, VisibleRect::rightTop().y), 0.0f); - - // left - _walls[2] = - cpSegmentShapeNew(cpSpaceGetStaticBody(_space), cpv(VisibleRect::leftBottom().x, VisibleRect::leftBottom().y), - cpv(VisibleRect::leftTop().x, VisibleRect::leftTop().y), 0.0f); - - // right - _walls[3] = - cpSegmentShapeNew(cpSpaceGetStaticBody(_space), cpv(VisibleRect::rightBottom().x, VisibleRect::rightBottom().y), - cpv(VisibleRect::rightTop().x, VisibleRect::rightTop().y), 0.0f); - - for (int i = 0; i < 4; i++) - { - - cpShapeSetElasticity(_walls[i], 1.0f); - cpShapeSetFriction(_walls[i], 1.0f); - cpSpaceAddShape(_space, _walls[i]); - } - - // Physics debug layer - _debugLayer = PhysicsDebugNodeChipmunk2D::create(_space); - this->addChild(_debugLayer, Z_PHYSICS_DEBUG); -} - -void ChipmunkTest::update(float delta) -{ - // Should use a fixed size step based on the animation interval. - int steps = 2; - float dt = Director::getInstance()->getAnimationInterval() / (float)steps; - - for (int i = 0; i < steps; i++) - { - -#if AX_TARGET_PLATFORM == AX_PLATFORM_WIN32 - cpSpaceStep(_space, dt); -#else - cpHastySpaceStep(_space, dt); -#endif - } -} - -void ChipmunkTest::createResetButton() -{ - auto reset = MenuItemImage::create("Images/r1.png", "Images/r2.png", AX_CALLBACK_1(ChipmunkTest::reset, this)); - - auto menu = Menu::create(reset, nullptr); - - menu->setPosition(VisibleRect::center().x, VisibleRect::bottom().y + 30); - this->addChild(menu, -1); -} - -void ChipmunkTest::reset(Object* sender) -{ - getTestSuite()->restartCurrTest(); -} - -void ChipmunkTest::addNewSpriteAtPosition(ax::Vec2 pos) -{ - int posx, posy; - - auto parent = getChildByTag(kTagParentNode); - - posx = AXRANDOM_0_1() * 200.0f; - posy = AXRANDOM_0_1() * 200.0f; - - posx = (posx % 4) * 85; - posy = (posy % 3) * 121; - - int num = 4; - cpVect verts[] = { - cpv(-24, -54), - cpv(-24, 54), - cpv(24, 54), - cpv(24, -54), - }; - - cpBody* body = cpBodyNew(1.0f, cpMomentForPoly(1.0f, num, verts, cpvzero, 0.0f)); - - cpBodySetPosition(body, cpv(pos.x, pos.y)); - cpSpaceAddBody(_space, body); - - cpShape* shape = cpPolyShapeNew(body, num, verts, cpTransformIdentity, 0.0f); - cpShapeSetElasticity(shape, 0.5f); - cpShapeSetFriction(shape, 0.5f); - cpSpaceAddShape(_space, shape); - - auto sprite = PhysicsSpriteChipmunk2D::createWithTexture(_spriteTexture, ax::Rect(posx, posy, 85, 121)); - parent->addChild(sprite); - - sprite->setCPBody(body); - sprite->setPosition(pos); -} - -void ChipmunkTest::onEnter() -{ - TestCase::onEnter(); -} - -void ChipmunkTest::onTouchesEnded(const std::vector& touches, Event* event) -{ - // Add a new body/atlas sprite at the touched location - - for (auto& touch : touches) - { - auto location = touch->getLocation(); - - addNewSpriteAtPosition(location); - } -} - -void ChipmunkTest::onAcceleration(Acceleration* acc, Event* event) -{ - static float prevX = 0, prevY = 0; - -#define kFilterFactor 0.05f - - float accelX = (float)acc->x * kFilterFactor + (1 - kFilterFactor) * prevX; - float accelY = (float)acc->y * kFilterFactor + (1 - kFilterFactor) * prevY; - - prevX = accelX; - prevY = accelY; - - auto v = ax::Vec2(accelX, accelY); - v = v * 200; - cpSpaceSetGravity(_space, cpv(v.x, v.y)); -} - -ChipmunkTests::ChipmunkTests() -{ - ADD_TEST_CASE(ChipmunkTest); -} diff --git a/tests/cpp-tests/Source/ChipmunkTest/ChipmunkTest.h b/tests/cpp-tests/Source/ChipmunkTest/ChipmunkTest.h deleted file mode 100644 index 4da9edf3e65c..000000000000 --- a/tests/cpp-tests/Source/ChipmunkTest/ChipmunkTest.h +++ /dev/null @@ -1,66 +0,0 @@ -/**************************************************************************** - Copyright (c) 2017-2018 Xiamen Yaji Software Co., Ltd. - Copyright (c) 2021 @aismann; Peter Eismann, Germany; dreifrankensoft - - https://axmol.dev/ - - 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. - ****************************************************************************/ - -// -// cocos2d -// -#ifndef __CHIPMUNKTEST_H__ -#define __CHIPMUNKTEST_H__ - -#include "axmol.h" -#include "chipmunk/chipmunk.h" -#include "../BaseTest.h" -#include "extensions/axmol-ext.h" - -class ChipmunkTest : public TestCase -{ -public: - CREATE_FUNC(ChipmunkTest); - - ChipmunkTest(); - ~ChipmunkTest(); - void onEnter() override; - void initPhysics(); - void createResetButton(); - void reset(ax::Object* sender); - - void addNewSpriteAtPosition(ax::Vec2 p); - void update(float dt) override; - void toggleDebugCallback(ax::Object* sender); - void onTouchesEnded(const std::vector& touches, ax::Event* event); - virtual void onAcceleration(ax::Acceleration* acc, ax::Event* event); - -private: - ax::Texture2D* _spriteTexture; // weak ref - ax::extension::PhysicsDebugNodeChipmunk2D* _debugLayer; // weak ref - - cpSpace* _space; // strong ref - cpShape* _walls[4]; - ax::DrawNode* drawNode; -}; - -DEFINE_TEST_SUITE(ChipmunkTests); - -#endif /* __CHIPMUNKACCELTOUCHTEST_H__ */ diff --git a/tests/cpp-tests/Source/DrawNodeTest/DrawNodeTest.cpp b/tests/cpp-tests/Source/DrawNodeTest/DrawNodeTest.cpp index 124df8b6b148..6f8bafad0d45 100644 --- a/tests/cpp-tests/Source/DrawNodeTest/DrawNodeTest.cpp +++ b/tests/cpp-tests/Source/DrawNodeTest/DrawNodeTest.cpp @@ -3591,9 +3591,7 @@ void CandyMixEeffect::update(float dt) static float WID = 400; static float HIG = 600; - static b2Timer timer; - - float t = timer.GetMilliseconds() / 1000.0f; + float t = utils::getTimeInMilliseconds() / 1000.0f; float ta = sin(t * cos(t) * 0.02f) + t; float tb = (1.0f + sin(t) * 1.0f) * 0.02f + 0.01f; float xa = WID * 0.5f; diff --git a/tests/cpp-tests/Source/PhysicsTest/PhysicsTest.cpp b/tests/cpp-tests/Source/PhysicsTest/PhysicsTest.cpp index 4403c95be7d9..d63ba2bedaa3 100644 --- a/tests/cpp-tests/Source/PhysicsTest/PhysicsTest.cpp +++ b/tests/cpp-tests/Source/PhysicsTest/PhysicsTest.cpp @@ -25,7 +25,7 @@ #include "PhysicsTest.h" -#if defined(AX_ENABLE_PHYSICS) +#if defined(AX_ENABLE_PHYSICS) && 0 # include # include "ui/CocosGUI.h" diff --git a/tests/cpp-tests/Source/PhysicsTest/PhysicsTest.h b/tests/cpp-tests/Source/PhysicsTest/PhysicsTest.h index 8575d6a9a958..0f74e70e4fb9 100644 --- a/tests/cpp-tests/Source/PhysicsTest/PhysicsTest.h +++ b/tests/cpp-tests/Source/PhysicsTest/PhysicsTest.h @@ -29,7 +29,7 @@ #include "../BaseTest.h" -#if defined(AX_ENABLE_PHYSICS) +#if defined(AX_ENABLE_PHYSICS) && 0 DEFINE_TEST_SUITE(PhysicsTests); diff --git a/tests/cpp-tests/Source/controller.cpp b/tests/cpp-tests/Source/controller.cpp index 3c944fe04cb0..cfbee33eca75 100644 --- a/tests/cpp-tests/Source/controller.cpp +++ b/tests/cpp-tests/Source/controller.cpp @@ -60,10 +60,6 @@ class RootTests : public TestList addTest("Box2D - Basic", []() { return new Box2DTests(); }); #if defined(AX_PLATFORM_PC) || defined(__EMSCRIPTEN__) addTest("Box2D - TestBed", []() { return new Box2DTestBedTests(); }); -#endif - addTest("Chipmunk2D - Basic", []() { return new ChipmunkTests(); }); -#if defined(AX_PLATFORM_PC) || defined(__EMSCRIPTEN__) - addTest("Chipmunk2D - TestBed", []() { return new ChipmunkTestBedTests(); }); #endif addTest("Bugs", []() { return new BugsTests(); }); addTest("Click and Move", []() { return new ClickAndMoveTest(); }); @@ -98,8 +94,8 @@ class RootTests : public TestList addTest("Node: Parallax", []() { return new ParallaxTests(); }); addTest("Node: Particles", []() { return new ParticleTests(); }); addTest("Node: Particle3D (PU)", []() { return new Particle3DTests(); }); -#if defined(AX_ENABLE_PHYSICS) - addTest("Node: Physics", []() { return new PhysicsTests(); }); +#if defined(AX_ENABLE_PHYSICS) && 0 + addTest("Node: Physics", []() { return new PhysicsTests(); }); #endif addTest("Node: Physics3D", []() { return new Physics3DTests(); }); addTest("Node: RenderTexture", []() { return new RenderTextureTests(); }); diff --git a/tests/cpp-tests/Source/shaders/circle.fs b/tests/cpp-tests/Source/shaders/circle.fs new file mode 100644 index 000000000000..a1e7e6091efb --- /dev/null +++ b/tests/cpp-tests/Source/shaders/circle.fs @@ -0,0 +1,23 @@ +// SPDX-FileCopyrightText: 2024 Erin Catto +// SPDX-License-Identifier: MIT + +#version 420 + +layout(location = 0) in vec2 v_position; +layout(location = 1) in vec4 v_color; +layout(location = 2) in float v_thickness; + +layout(location = SV_Target0) out vec4 fragColor; + +void main() +{ + // radius in unit quad + float radius = 1.0; + + // distance to circle + vec2 w = v_position; + float dw = length(w); + float d = abs(dw - radius); + + fragColor = vec4(v_color.rgb, smoothstep(v_thickness, 0.0, d)); +} diff --git a/tests/cpp-tests/Source/shaders/circle.vs b/tests/cpp-tests/Source/shaders/circle.vs new file mode 100644 index 000000000000..361c07cdcddf --- /dev/null +++ b/tests/cpp-tests/Source/shaders/circle.vs @@ -0,0 +1,31 @@ +// SPDX-FileCopyrightText: 2024 Erin Catto +// SPDX-License-Identifier: MIT + +#version 420 + +layout(location = 0) in vec2 a_localPosition; +layout(location = 1) in vec2 a_instancePosition; +layout(location = 2) in float a_instanceRadius; +layout(location = 3) in vec4 a_instanceColor; + +layout(location = 0) out vec2 v_position; +layout(location = 1) out vec4 v_color; +layout(location = 2) out float v_thickness; + +layout(std140) uniform vs_ub { + float u_pixelScale; + mat4 u_MVPMatrix; +}; + +void main() +{ + v_position = a_localPosition; + v_color = a_instanceColor; + float radius = a_instanceRadius; + + // resolution.y = pixelScale * radius + v_thickness = 3.0f / (u_pixelScale * radius); + + vec2 p = vec2(radius * a_localPosition.x, radius * a_localPosition.y) + a_instancePosition; + gl_Position = u_MVPMatrix * vec4(p, 0.0f, 1.0f); +} diff --git a/tests/cpp-tests/Source/shaders/solid_capsule.fs b/tests/cpp-tests/Source/shaders/solid_capsule.fs new file mode 100644 index 000000000000..8536db9f44c5 --- /dev/null +++ b/tests/cpp-tests/Source/shaders/solid_capsule.fs @@ -0,0 +1,61 @@ +// SPDX-FileCopyrightText: 2024 Erin Catto +// SPDX-License-Identifier: MIT + +#version 420 + +layout(location=0) in vec2 v_position; +layout(location=1) in vec4 v_color; +layout(location=2) in float v_length; +layout(location=3) in float v_thickness; + +layout(location = SV_Target0) out vec4 fragColor; + +// Thanks to baz and kolyan3040 for help on this shader +// todo this can be optimized a bit, keeping some terms for clarity + +// https://en.wikipedia.org/wiki/Alpha_compositing +vec4 blend_colors(vec4 front,vec4 back) +{ + vec3 cSrc = front.rgb; + float alphaSrc = front.a; + vec3 cDst = back.rgb; + float alphaDst = back.a; + + vec3 cOut = cSrc * alphaSrc + cDst * alphaDst * (1.0 - alphaSrc); + float alphaOut = alphaSrc + alphaDst * (1.0 - alphaSrc); + + // remove alpha from rgb + cOut = cOut / alphaOut; + + return vec4(cOut, alphaOut); +} + +void main() +{ + // radius in unit quad + float radius = 0.5 * (2.0 - v_length); + + vec4 borderColor = v_color; + vec4 fillColor = 0.6f * borderColor; + + vec2 v1 = vec2(-0.5 * v_length, 0); + vec2 v2 = vec2(0.5 * v_length, 0); + + // distance to line segment + vec2 e = v2 - v1; + vec2 w = v_position - v1; + float we = dot(w, e); + vec2 b = w - e * clamp(we / dot(e, e), 0.0, 1.0); + float dw = length(b); + + // SDF union of capsule and line segment + float d = min(dw, abs(dw - radius)); + + // roll the fill alpha down at the border + vec4 back = vec4(fillColor.rgb, fillColor.a * smoothstep(radius + v_thickness, radius, dw)); + + // roll the border alpha down from 1 to 0 across the border thickness + vec4 front = vec4(borderColor.rgb, smoothstep(v_thickness, 0.0f, d)); + + fragColor = blend_colors(front, back); +} diff --git a/tests/cpp-tests/Source/shaders/solid_capsule.vs b/tests/cpp-tests/Source/shaders/solid_capsule.vs new file mode 100644 index 000000000000..94b7a6e228f4 --- /dev/null +++ b/tests/cpp-tests/Source/shaders/solid_capsule.vs @@ -0,0 +1,46 @@ +// SPDX-FileCopyrightText: 2024 Erin Catto +// SPDX-License-Identifier: MIT + +#version 420 + +layout(location=0) in vec2 a_localPosition; +layout(location=1) in vec4 a_instanceTransform; +layout(location=2) in float a_instanceRadius; +layout(location=3) in float a_instanceLength; +layout(location=4) in vec4 a_instanceColor; + +layout(location=0) out vec2 v_position; +layout(location=1) out vec4 v_color; +layout(location=2) out float v_length; +layout(location=3) out float v_thickness; + +layout(std140) uniform vs_ub { + float u_pixelScale; + mat4 u_MVPMatrix; +}; + +void main() +{ + v_position = a_localPosition; + v_color = a_instanceColor; + + float radius = a_instanceRadius; + float length = a_instanceLength; + + // scale quad large enough to hold capsule + float scale = radius + 0.5 * length; + + // quad range of [-1, 1] implies normalize radius and length + v_length = length / scale; + + // resolution.y = pixelScale * scale + v_thickness = 3.0f / (u_pixelScale * scale); + + float x = a_instanceTransform.x; + float y = a_instanceTransform.y; + float c = a_instanceTransform.z; + float s = a_instanceTransform.w; + vec2 p = vec2(scale * a_localPosition.x, scale * a_localPosition.y); + p = vec2((c * p.x - s * p.y) + x, (s * p.x + c * p.y) + y); + gl_Position = u_MVPMatrix * vec4(p, 0.0, 1.0); +} diff --git a/tests/cpp-tests/Source/shaders/solid_circle.fs b/tests/cpp-tests/Source/shaders/solid_circle.fs new file mode 100644 index 000000000000..e9f5d47e3a10 --- /dev/null +++ b/tests/cpp-tests/Source/shaders/solid_circle.fs @@ -0,0 +1,56 @@ +// SPDX-FileCopyrightText: 2024 Erin Catto +// SPDX-License-Identifier: MIT + +#version 420 + +layout(location = 0) in vec2 v_position; +layout(location = 1) in vec4 v_color; +layout(location = 2) in float v_thickness; + +layout(location = SV_Target0) out vec4 fragColor; + +// https://en.wikipedia.org/wiki/Alpha_compositing +vec4 blend_colors(vec4 front, vec4 back) +{ + vec3 cSrc = front.rgb; + float alphaSrc = front.a; + vec3 cDst = back.rgb; + float alphaDst = back.a; + + vec3 cOut = cSrc * alphaSrc + cDst * alphaDst * (1.0 - alphaSrc); + float alphaOut = alphaSrc + alphaDst * (1.0 - alphaSrc); + cOut = cOut / alphaOut; + + return vec4(cOut, alphaOut); +} + +void main() +{ + // radius in unit quad + float radius = 1.0; + + // distance to axis line segment + vec2 e = vec2(radius, 0); + vec2 w = v_position; + float we = dot(w, e); + vec2 b = w - e * clamp(we / dot(e, e), 0.0, 1.0); + float da = length(b); + + // distance to circle + float dw = length(w); + float dc = abs(dw - radius); + + // union of circle and axis + float d = min(da, dc); + + vec4 borderColor = v_color; + vec4 fillColor = 0.6f * borderColor; + + // roll the fill alpha down at the border + vec4 back = vec4(fillColor.rgb, fillColor.a * smoothstep(radius + v_thickness, radius, dw)); + + // roll the border alpha down from 1 to 0 across the border thickness + vec4 front = vec4(borderColor.rgb, smoothstep(v_thickness, 0.0f, d)); + + fragColor = blend_colors(front, back); +} diff --git a/tests/cpp-tests/Source/shaders/solid_circle.vs b/tests/cpp-tests/Source/shaders/solid_circle.vs new file mode 100644 index 000000000000..c4061d56b48f --- /dev/null +++ b/tests/cpp-tests/Source/shaders/solid_circle.vs @@ -0,0 +1,36 @@ +// SPDX-FileCopyrightText: 2024 Erin Catto +// SPDX-License-Identifier: MIT + +#version 420 + +layout(location = 0) in vec2 a_localPosition; +layout(location = 1) in vec4 a_instanceTransform; +layout(location = 2) in float a_instanceRadius; +layout(location = 3) in vec4 a_instanceColor; + +layout(location = 0) out vec2 v_position; +layout(location = 1) out vec4 v_color; +layout(location = 2) out float v_thickness; + +layout(std140) uniform vs_ub { + float u_pixelScale; + mat4 u_MVPMatrix; +}; + +void main() +{ + v_position = a_localPosition; + v_color = a_instanceColor; + float radius = a_instanceRadius; + + // resolution.y = pixelScale * radius + v_thickness = 3.0f / (u_pixelScale * radius); + + float x = a_instanceTransform.x; + float y = a_instanceTransform.y; + float c = a_instanceTransform.z; + float s = a_instanceTransform.w; + vec2 p = vec2(radius * a_localPosition.x, radius * a_localPosition.y); + p = vec2((c * p.x - s * p.y) + x, (s * p.x + c * p.y) + y); + gl_Position = u_MVPMatrix * vec4(p, 0.0f, 1.0f); +} diff --git a/tests/cpp-tests/Source/tests.h b/tests/cpp-tests/Source/tests.h index 38542c183b4d..ca4b0ed17208 100644 --- a/tests/cpp-tests/Source/tests.h +++ b/tests/cpp-tests/Source/tests.h @@ -29,11 +29,6 @@ #include "Box2DTest/Box2dTest.h" #include "Box2DTestBed/Box2DTestBed.h" -#include "ChipmunkTest/ChipmunkTest.h" -#if defined(AX_PLATFORM_PC) || defined(__EMSCRIPTEN__) -# include "ChipmunkTestBed/ChipmunkTestBed.h" -#endif - #if (AX_TARGET_PLATFORM != AX_PLATFORM_MARMALADE) # include "ClippingNodeTest/ClippingNodeTest.h" #endif