Merge pull request #58946 from akien-mga/remove-unused-bullet-code

Remove unused Bullet module and thirdparty code

COPYRIGHT.txt

@@ -130,11 +130,6 @@ Comment: Basis Universal
 Copyright: 2019, Binomial LLC.
 License: Apache-2.0
 
-Files: ./thirdparty/bullet/
-Comment: Bullet Continuous Collision Detection and Physics Library
-Copyright: 2003-2013, Erwin Coumans
-License: Zlib
-
 Files: ./thirdparty/certs/ca-certificates.crt
 Comment: CA certificates
 Copyright: Mozilla Contributors

SConstruct

@@ -198,7 +198,6 @@ opts.Add("system_certs_path", "Use this path as SSL certificates default for edi
 opts.Add(BoolVariable("use_precise_math_checks", "Math checks use very precise epsilon (debug option)", False))
 
 # Thirdparty libraries
-opts.Add(BoolVariable("builtin_bullet", "Use the built-in Bullet library", True))
 opts.Add(BoolVariable("builtin_certs", "Use the built-in SSL certificates bundles", True))
 opts.Add(BoolVariable("builtin_embree", "Use the built-in Embree library", True))
 opts.Add(BoolVariable("builtin_enet", "Use the built-in ENet library", True))

core/config/project_settings.cpp

@@ -1236,9 +1236,6 @@ ProjectSettings::ProjectSettings() {
 	custom_prop_info["rendering/driver/threads/thread_model"] = PropertyInfo(Variant::INT, "rendering/driver/threads/thread_model", PROPERTY_HINT_ENUM, "Single-Unsafe,Single-Safe,Multi-Threaded");
 	GLOBAL_DEF("physics/2d/run_on_separate_thread", false);
 	GLOBAL_DEF("physics/3d/run_on_separate_thread", false);
-	// Required to make the project setting appear even if the physics engine is GodotPhysics,
-	// while also making it appear in the ProjectSettings class documentation.
-	GLOBAL_DEF("physics/3d/smooth_trimesh_collision", false);
 
 	GLOBAL_DEF("debug/settings/profiler/max_functions", 16384);
 	custom_prop_info["debug/settings/profiler/max_functions"] = PropertyInfo(Variant::INT, "debug/settings/profiler/max_functions", PROPERTY_HINT_RANGE, "128,65535,1");

doc/classes/ProjectSettings.xml

@@ -1493,7 +1493,7 @@
 		</member>
 		<member name="physics/3d/physics_engine" type="String" setter="" getter="" default="&quot;DEFAULT&quot;">
 			Sets which physics engine to use for 3D physics.
-			"DEFAULT" is currently the [url=https://bulletphysics.org]Bullet[/url] physics engine. The "GodotPhysics3D" engine is still supported as an alternative.
+			"DEFAULT" and "GodotPhysics3D" are the same, as there is currently no alternative 3D physics server implemented.
 		</member>
 		<member name="physics/3d/run_on_separate_thread" type="bool" setter="" getter="" default="false">
 			If [code]true[/code], the 3D physics server runs on a separate thread, making better use of multi-core CPUs. If [code]false[/code], the 3D physics server runs on the main thread. Running the physics server on a separate thread can increase performance, but restricts API access to only physics process.
@@ -1504,10 +1504,6 @@
 		<member name="physics/3d/sleep_threshold_linear" type="float" setter="" getter="" default="0.1">
 			Threshold linear velocity under which a 3D physics body will be considered inactive. See [constant PhysicsServer3D.SPACE_PARAM_BODY_LINEAR_VELOCITY_SLEEP_THRESHOLD].
 		</member>
-		<member name="physics/3d/smooth_trimesh_collision" type="bool" setter="" getter="" default="false">
-			If [code]true[/code], smooths out collision with trimesh shapes ([ConcavePolygonShape3D]) by telling the Bullet physics engine to generate internal edge information for every trimesh shape created.
-			[b]Note:[/b] Only effective if [member physics/3d/physics_engine] is set to [code]Bullet[/code], [i]not[/i] [code]DEFAULT[/code] or [code]GodotPhysics[/code].
-		</member>
 		<member name="physics/3d/solver/contact_max_allowed_penetration" type="float" setter="" getter="" default="0.01">
 			Maximum distance a shape can penetrate another shape before it is considered a collision. See [constant PhysicsServer3D.SPACE_PARAM_CONTACT_MAX_ALLOWED_PENETRATION].
 		</member>

doc/classes/RigidDynamicBody3D.xml

@@ -227,7 +227,6 @@
 				[code]body[/code] the [Node], if it exists in the tree, of the other [PhysicsBody3D] or [GridMap].
 				[code]body_shape_index[/code] the index of the [Shape3D] of the other [PhysicsBody3D] or [GridMap] used by the [PhysicsServer3D]. Get the [CollisionShape3D] node with [code]body.shape_owner_get_owner(body.shape_find_owner(body_shape_index))[/code].
 				[code]local_shape_index[/code] the index of the [Shape3D] of this RigidDynamicBody3D used by the [PhysicsServer3D]. Get the [CollisionShape3D] node with [code]self.shape_owner_get_owner(self.shape_find_owner(local_shape_index))[/code].
-				[b]Note:[/b] Bullet physics cannot identify the shape index when using a [ConcavePolygonShape3D]. Don't use multiple [CollisionShape3D]s when using a [ConcavePolygonShape3D] with Bullet physics if you need shape indices.
 			</description>
 		</signal>
 		<signal name="body_shape_exited">
@@ -241,7 +240,6 @@
 				[code]body[/code] the [Node], if it exists in the tree, of the other [PhysicsBody3D] or [GridMap].
 				[code]body_shape_index[/code] the index of the [Shape3D] of the other [PhysicsBody3D] or [GridMap] used by the [PhysicsServer3D]. Get the [CollisionShape3D] node with [code]body.shape_owner_get_owner(body.shape_find_owner(body_shape_index))[/code].
 				[code]local_shape_index[/code] the index of the [Shape3D] of this RigidDynamicBody3D used by the [PhysicsServer3D]. Get the [CollisionShape3D] node with [code]self.shape_owner_get_owner(self.shape_find_owner(local_shape_index))[/code].
-				[b]Note:[/b] Bullet physics cannot identify the shape index when using a [ConcavePolygonShape3D]. Don't use multiple [CollisionShape3D]s when using a [ConcavePolygonShape3D] with Bullet physics if you need shape indices.
 			</description>
 		</signal>
 		<signal name="sleeping_state_changed">

modules/bullet/SCsub

@@ -1,224 +0,0 @@
-#!/usr/bin/env python
-
-Import("env")
-Import("env_modules")
-
-env_bullet = env_modules.Clone()
-
-# Thirdparty source files
-
-thirdparty_obj = []
-
-if env["builtin_bullet"]:
-    # Build only "Bullet2" API (not "Bullet3" folders).
-    # Sync file list with relevant upstream CMakeLists.txt for each folder.
-    if env["float"] == "64":
-        env.Append(CPPDEFINES=["BT_USE_DOUBLE_PRECISION=1"])
-    thirdparty_dir = "#thirdparty/bullet/"
-
-    bullet2_src = [
-        # BulletCollision
-        "BulletCollision/BroadphaseCollision/btAxisSweep3.cpp",
-        "BulletCollision/BroadphaseCollision/btBroadphaseProxy.cpp",
-        "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.cpp",
-        "BulletCollision/BroadphaseCollision/btDbvt.cpp",
-        "BulletCollision/BroadphaseCollision/btDbvtBroadphase.cpp",
-        "BulletCollision/BroadphaseCollision/btDispatcher.cpp",
-        "BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp",
-        "BulletCollision/BroadphaseCollision/btQuantizedBvh.cpp",
-        "BulletCollision/BroadphaseCollision/btSimpleBroadphase.cpp",
-        "BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.cpp",
-        "BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.cpp",
-        "BulletCollision/CollisionDispatch/btBox2dBox2dCollisionAlgorithm.cpp",
-        "BulletCollision/CollisionDispatch/btBoxBoxDetector.cpp",
-        "BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp",
-        "BulletCollision/CollisionDispatch/btCollisionDispatcherMt.cpp",
-        "BulletCollision/CollisionDispatch/btCollisionObject.cpp",
-        "BulletCollision/CollisionDispatch/btCollisionWorld.cpp",
-        "BulletCollision/CollisionDispatch/btCollisionWorldImporter.cpp",
-        "BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp",
-        "BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.cpp",
-        "BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp",
-        "BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp",
-        "BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.cpp",
-        "BulletCollision/CollisionDispatch/btConvex2dConvex2dAlgorithm.cpp",
-        "BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.cpp",
-        "BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.cpp",
-        "BulletCollision/CollisionDispatch/btGhostObject.cpp",
-        "BulletCollision/CollisionDispatch/btHashedSimplePairCache.cpp",
-        "BulletCollision/CollisionDispatch/btInternalEdgeUtility.cpp",
-        "BulletCollision/CollisionDispatch/btManifoldResult.cpp",
-        "BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp",
-        "BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.cpp",
-        "BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.cpp",
-        "BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.cpp",
-        "BulletCollision/CollisionDispatch/btUnionFind.cpp",
-        "BulletCollision/CollisionDispatch/SphereTriangleDetector.cpp",
-        "BulletCollision/CollisionShapes/btBoxShape.cpp",
-        "BulletCollision/CollisionShapes/btBox2dShape.cpp",
-        "BulletCollision/CollisionShapes/btBvhTriangleMeshShape.cpp",
-        "BulletCollision/CollisionShapes/btCapsuleShape.cpp",
-        "BulletCollision/CollisionShapes/btCollisionShape.cpp",
-        "BulletCollision/CollisionShapes/btCompoundShape.cpp",
-        "BulletCollision/CollisionShapes/btConcaveShape.cpp",
-        "BulletCollision/CollisionShapes/btConeShape.cpp",
-        "BulletCollision/CollisionShapes/btConvexHullShape.cpp",
-        "BulletCollision/CollisionShapes/btConvexInternalShape.cpp",
-        "BulletCollision/CollisionShapes/btConvexPointCloudShape.cpp",
-        "BulletCollision/CollisionShapes/btConvexPolyhedron.cpp",
-        "BulletCollision/CollisionShapes/btConvexShape.cpp",
-        "BulletCollision/CollisionShapes/btConvex2dShape.cpp",
-        "BulletCollision/CollisionShapes/btConvexTriangleMeshShape.cpp",
-        "BulletCollision/CollisionShapes/btCylinderShape.cpp",
-        "BulletCollision/CollisionShapes/btEmptyShape.cpp",
-        "BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp",
-        "BulletCollision/CollisionShapes/btMiniSDF.cpp",
-        "BulletCollision/CollisionShapes/btMinkowskiSumShape.cpp",
-        "BulletCollision/CollisionShapes/btMultimaterialTriangleMeshShape.cpp",
-        "BulletCollision/CollisionShapes/btMultiSphereShape.cpp",
-        "BulletCollision/CollisionShapes/btOptimizedBvh.cpp",
-        "BulletCollision/CollisionShapes/btPolyhedralConvexShape.cpp",
-        "BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.cpp",
-        "BulletCollision/CollisionShapes/btSdfCollisionShape.cpp",
-        "BulletCollision/CollisionShapes/btShapeHull.cpp",
-        "BulletCollision/CollisionShapes/btSphereShape.cpp",
-        "BulletCollision/CollisionShapes/btStaticPlaneShape.cpp",
-        "BulletCollision/CollisionShapes/btStridingMeshInterface.cpp",
-        "BulletCollision/CollisionShapes/btTetrahedronShape.cpp",
-        "BulletCollision/CollisionShapes/btTriangleBuffer.cpp",
-        "BulletCollision/CollisionShapes/btTriangleCallback.cpp",
-        "BulletCollision/CollisionShapes/btTriangleIndexVertexArray.cpp",
-        "BulletCollision/CollisionShapes/btTriangleIndexVertexMaterialArray.cpp",
-        "BulletCollision/CollisionShapes/btTriangleMesh.cpp",
-        "BulletCollision/CollisionShapes/btTriangleMeshShape.cpp",
-        "BulletCollision/CollisionShapes/btUniformScalingShape.cpp",
-        "BulletCollision/Gimpact/btContactProcessing.cpp",
-        "BulletCollision/Gimpact/btGenericPoolAllocator.cpp",
-        "BulletCollision/Gimpact/btGImpactBvh.cpp",
-        "BulletCollision/Gimpact/btGImpactCollisionAlgorithm.cpp",
-        "BulletCollision/Gimpact/btGImpactQuantizedBvh.cpp",
-        "BulletCollision/Gimpact/btGImpactShape.cpp",
-        "BulletCollision/Gimpact/btTriangleShapeEx.cpp",
-        "BulletCollision/Gimpact/gim_box_set.cpp",
-        "BulletCollision/Gimpact/gim_contact.cpp",
-        "BulletCollision/Gimpact/gim_memory.cpp",
-        "BulletCollision/Gimpact/gim_tri_collision.cpp",
-        "BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.cpp",
-        "BulletCollision/NarrowPhaseCollision/btConvexCast.cpp",
-        "BulletCollision/NarrowPhaseCollision/btGjkConvexCast.cpp",
-        "BulletCollision/NarrowPhaseCollision/btGjkEpa2.cpp",
-        "BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.cpp",
-        "BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp",
-        "BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.cpp",
-        "BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp",
-        "BulletCollision/NarrowPhaseCollision/btRaycastCallback.cpp",
-        "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.cpp",
-        "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.cpp",
-        "BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.cpp",
-        # BulletDynamics
-        "BulletDynamics/Character/btKinematicCharacterController.cpp",
-        "BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp",
-        "BulletDynamics/ConstraintSolver/btContactConstraint.cpp",
-        "BulletDynamics/ConstraintSolver/btFixedConstraint.cpp",
-        "BulletDynamics/ConstraintSolver/btGearConstraint.cpp",
-        "BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp",
-        "BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.cpp",
-        "BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.cpp",
-        "BulletDynamics/ConstraintSolver/btHinge2Constraint.cpp",
-        "BulletDynamics/ConstraintSolver/btHingeConstraint.cpp",
-        "BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp",
-        "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp",
-        "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolverMt.cpp",
-        "BulletDynamics/ConstraintSolver/btBatchedConstraints.cpp",
-        "BulletDynamics/ConstraintSolver/btNNCGConstraintSolver.cpp",
-        "BulletDynamics/ConstraintSolver/btSliderConstraint.cpp",
-        "BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.cpp",
-        "BulletDynamics/ConstraintSolver/btTypedConstraint.cpp",
-        "BulletDynamics/ConstraintSolver/btUniversalConstraint.cpp",
-        "BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp",
-        "BulletDynamics/Dynamics/btDiscreteDynamicsWorldMt.cpp",
-        "BulletDynamics/Dynamics/btSimulationIslandManagerMt.cpp",
-        "BulletDynamics/Dynamics/btRigidBody.cpp",
-        "BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp",
-        # "BulletDynamics/Dynamics/Bullet-C-API.cpp",
-        "BulletDynamics/Vehicle/btRaycastVehicle.cpp",
-        "BulletDynamics/Vehicle/btWheelInfo.cpp",
-        "BulletDynamics/Featherstone/btMultiBody.cpp",
-        "BulletDynamics/Featherstone/btMultiBodyConstraint.cpp",
-        "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp",
-        "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp",
-        "BulletDynamics/Featherstone/btMultiBodyFixedConstraint.cpp",
-        "BulletDynamics/Featherstone/btMultiBodyGearConstraint.cpp",
-        "BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp",
-        "BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp",
-        "BulletDynamics/Featherstone/btMultiBodyMLCPConstraintSolver.cpp",
-        "BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp",
-        "BulletDynamics/Featherstone/btMultiBodySliderConstraint.cpp",
-        "BulletDynamics/Featherstone/btMultiBodySphericalJointMotor.cpp",
-        "BulletDynamics/MLCPSolvers/btDantzigLCP.cpp",
-        "BulletDynamics/MLCPSolvers/btMLCPSolver.cpp",
-        "BulletDynamics/MLCPSolvers/btLemkeAlgorithm.cpp",
-        # BulletInverseDynamics
-        "BulletInverseDynamics/IDMath.cpp",
-        "BulletInverseDynamics/MultiBodyTree.cpp",
-        "BulletInverseDynamics/details/MultiBodyTreeInitCache.cpp",
-        "BulletInverseDynamics/details/MultiBodyTreeImpl.cpp",
-        # BulletSoftBody
-        "BulletSoftBody/btSoftBody.cpp",
-        "BulletSoftBody/btSoftBodyConcaveCollisionAlgorithm.cpp",
-        "BulletSoftBody/btSoftBodyHelpers.cpp",
-        "BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.cpp",
-        "BulletSoftBody/btSoftRigidCollisionAlgorithm.cpp",
-        "BulletSoftBody/btSoftRigidDynamicsWorld.cpp",
-        "BulletSoftBody/btSoftMultiBodyDynamicsWorld.cpp",
-        "BulletSoftBody/btSoftSoftCollisionAlgorithm.cpp",
-        "BulletSoftBody/btDefaultSoftBodySolver.cpp",
-        "BulletSoftBody/btDeformableBackwardEulerObjective.cpp",
-        "BulletSoftBody/btDeformableBodySolver.cpp",
-        "BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp",
-        "BulletSoftBody/btDeformableContactProjection.cpp",
-        "BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp",
-        "BulletSoftBody/btDeformableContactConstraint.cpp",
-        "BulletSoftBody/poly34.cpp",
-        # clew
-        "clew/clew.c",
-        # LinearMath
-        "LinearMath/btAlignedAllocator.cpp",
-        "LinearMath/btConvexHull.cpp",
-        "LinearMath/btConvexHullComputer.cpp",
-        "LinearMath/btGeometryUtil.cpp",
-        "LinearMath/btPolarDecomposition.cpp",
-        "LinearMath/btQuickprof.cpp",
-        "LinearMath/btReducedVector.cpp",
-        "LinearMath/btSerializer.cpp",
-        "LinearMath/btSerializer64.cpp",
-        "LinearMath/btThreads.cpp",
-        "LinearMath/btVector3.cpp",
-        "LinearMath/TaskScheduler/btTaskScheduler.cpp",
-        "LinearMath/TaskScheduler/btThreadSupportPosix.cpp",
-        "LinearMath/TaskScheduler/btThreadSupportWin32.cpp",
-    ]
-
-    thirdparty_sources = [thirdparty_dir + file for file in bullet2_src]
-
-    env_bullet.Prepend(CPPPATH=[thirdparty_dir])
-    if env["target"] == "debug" or env["target"] == "release_debug":
-        env_bullet.Append(CPPDEFINES=["DEBUG"])
-
-    env_bullet.Append(CPPDEFINES=["BT_USE_OLD_DAMPING_METHOD", "BT_THREADSAFE"])
-
-    env_thirdparty = env_bullet.Clone()
-    env_thirdparty.disable_warnings()
-    env_thirdparty.add_source_files(thirdparty_obj, thirdparty_sources)
-    env.modules_sources += thirdparty_obj
-
-
-# Godot source files
-
-module_obj = []
-
-env_bullet.add_source_files(module_obj, "*.cpp")
-env.modules_sources += module_obj
-
-# Needed to force rebuilding the module files when the thirdparty library is updated.
-env.Depends(module_obj, thirdparty_obj)

modules/bullet/area_bullet.cpp

@@ -1,324 +0,0 @@
-/*************************************************************************/
-/*  area_bullet.cpp                                                      */
-/*************************************************************************/
-/*                       This file is part of:                           */
-/*                           GODOT ENGINE                                */
-/*                      https://godotengine.org                          */
-/*************************************************************************/
-/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur.                 */
-/* Copyright (c) 2014-2022 Godot Engine contributors (cf. AUTHORS.md).   */
-/*                                                                       */
-/* Permission is hereby granted, free of charge, to any person obtaining */
-/* a copy of this software and associated documentation files (the       */
-/* "Software"), to deal in the Software without restriction, including   */
-/* without limitation the rights to use, copy, modify, merge, publish,   */
-/* distribute, sublicense, and/or sell copies of the Software, and to    */
-/* permit persons to whom the Software is furnished to do so, subject to */
-/* the following conditions:                                             */
-/*                                                                       */
-/* The above copyright notice and this permission notice shall be        */
-/* included in all copies or substantial portions of the Software.       */
-/*                                                                       */
-/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
-/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
-/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
-/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
-/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION 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 "area_bullet.h"
-
-#include "bullet_physics_server.h"
-#include "bullet_types_converter.h"
-#include "bullet_utilities.h"
-#include "collision_object_bullet.h"
-#include "space_bullet.h"
-
-#include <BulletCollision/CollisionDispatch/btGhostObject.h>
-#include <btBulletCollisionCommon.h>
-
-AreaBullet::AreaBullet() :
-		RigidCollisionObjectBullet(CollisionObjectBullet::TYPE_AREA) {
-	btGhost = bulletnew(btGhostObject);
-	reload_shapes();
-	setupBulletCollisionObject(btGhost);
-	/// Collision objects with a callback still have collision response with dynamic rigid bodies.
-	/// In order to use collision objects as trigger, you have to disable the collision response.
-	set_collision_enabled(false);
-
-	for (int i = 0; i < 5; ++i) {
-		call_event_res_ptr[i] = &call_event_res[i];
-	}
-}
-
-AreaBullet::~AreaBullet() {
-	// signal are handled by godot, so just clear without notify
-	for (int i = 0; i < overlapping_shapes.size(); i++) {
-		overlapping_shapes[i].other_object->on_exit_area(this);
-	}
-}
-
-void AreaBullet::dispatch_callbacks() {
-	if (!isScratched) {
-		return;
-	}
-	isScratched = false;
-
-	// Reverse order so items can be removed.
-	for (int i = overlapping_shapes.size() - 1; i >= 0; i--) {
-		OverlappingShapeData &overlapping_shape = overlapping_shapes.write[i];
-
-		switch (overlapping_shape.state) {
-			case OVERLAP_STATE_ENTER:
-				overlapping_shape.state = OVERLAP_STATE_INSIDE;
-				call_event(overlapping_shape, PhysicsServer3D::AREA_BODY_ADDED);
-				if (_overlapping_shape_count(overlapping_shape.other_object) == 1) {
-					// This object's first shape being added.
-					overlapping_shape.other_object->on_enter_area(this);
-				}
-				break;
-			case OVERLAP_STATE_EXIT:
-				call_event(overlapping_shape, PhysicsServer3D::AREA_BODY_REMOVED);
-				if (_overlapping_shape_count(overlapping_shape.other_object) == 1) {
-					// This object's last shape being removed.
-					overlapping_shape.other_object->on_exit_area(this);
-				}
-				overlapping_shapes.remove_at(i); // Remove after callback
-				break;
-			case OVERLAP_STATE_INSIDE: {
-				if (overlapping_shape.other_object->getType() == TYPE_RIGID_BODY) {
-					RigidBodyBullet *body = static_cast<RigidBodyBullet *>(overlapping_shape.other_object);
-					body->scratch_space_override_modificator();
-				}
-				break;
-			}
-			case OVERLAP_STATE_DIRTY:
-				break;
-		}
-	}
-}
-
-void AreaBullet::call_event(const OverlappingShapeData &p_overlapping_shape, PhysicsServer3D::AreaBodyStatus p_status) {
-	InOutEventCallback &event = eventsCallbacks[static_cast<int>(p_overlapping_shape.other_object->getType())];
-
-	if (!event.event_callback.is_valid()) {
-		event.event_callback = Callable();
-		return;
-	}
-
-	call_event_res[0] = p_status;
-	call_event_res[1] = p_overlapping_shape.other_object->get_self(); // RID
-	call_event_res[2] = p_overlapping_shape.other_object->get_instance_id(); // Object ID
-	call_event_res[3] = p_overlapping_shape.other_shape_id; // Other object's shape ID
-	call_event_res[4] = p_overlapping_shape.our_shape_id; // This area's shape ID
-
-	Callable::CallError outResp;
-	Variant ret;
-	event.event_callback.call((const Variant **)call_event_res, 5, ret, outResp);
-}
-
-int AreaBullet::_overlapping_shape_count(CollisionObjectBullet *p_other_object) {
-	int count = 0;
-	for (int i = 0; i < overlapping_shapes.size(); i++) {
-		if (overlapping_shapes[i].other_object == p_other_object) {
-			count++;
-		}
-	}
-	return count;
-}
-
-int AreaBullet::_find_overlapping_shape(CollisionObjectBullet *p_other_object, uint32_t p_other_shape_id, uint32_t p_our_shape_id) {
-	for (int i = 0; i < overlapping_shapes.size(); i++) {
-		const OverlappingShapeData &overlapping_shape = overlapping_shapes[i];
-		if (overlapping_shape.other_object == p_other_object && overlapping_shape.other_shape_id == p_other_shape_id && overlapping_shape.our_shape_id == p_our_shape_id) {
-			return i;
-		}
-	}
-	return -1;
-}
-
-void AreaBullet::mark_all_overlaps_dirty() {
-	OverlappingShapeData *overlapping_shapes_w = overlapping_shapes.ptrw();
-	for (int i = 0; i < overlapping_shapes.size(); i++) {
-		// Don't overwrite OVERLAP_STATE_ENTER state.
-		if (overlapping_shapes_w[i].state != OVERLAP_STATE_ENTER) {
-			overlapping_shapes_w[i].state = OVERLAP_STATE_DIRTY;
-		}
-	}
-}
-
-void AreaBullet::mark_object_overlaps_inside(CollisionObjectBullet *p_other_object) {
-	OverlappingShapeData *overlapping_shapes_w = overlapping_shapes.ptrw();
-	for (int i = 0; i < overlapping_shapes.size(); i++) {
-		if (overlapping_shapes_w[i].other_object == p_other_object && overlapping_shapes_w[i].state == OVERLAP_STATE_DIRTY) {
-			overlapping_shapes_w[i].state = OVERLAP_STATE_INSIDE;
-		}
-	}
-}
-
-void AreaBullet::set_overlap(CollisionObjectBullet *p_other_object, uint32_t p_other_shape_id, uint32_t p_our_shape_id) {
-	int i = _find_overlapping_shape(p_other_object, p_other_shape_id, p_our_shape_id);
-	if (i == -1) { // Not found, create new one.
-		OverlappingShapeData overlapping_shape(p_other_object, OVERLAP_STATE_ENTER, p_other_shape_id, p_our_shape_id);
-		overlapping_shapes.push_back(overlapping_shape);
-		p_other_object->notify_new_overlap(this);
-		isScratched = true;
-	} else {
-		overlapping_shapes.ptrw()[i].state = OVERLAP_STATE_INSIDE;
-	}
-}
-
-void AreaBullet::mark_all_dirty_overlaps_as_exit() {
-	OverlappingShapeData *overlapping_shapes_w = overlapping_shapes.ptrw();
-	for (int i = 0; i < overlapping_shapes.size(); i++) {
-		if (overlapping_shapes[i].state == OVERLAP_STATE_DIRTY) {
-			overlapping_shapes_w[i].state = OVERLAP_STATE_EXIT;
-			isScratched = true;
-		}
-	}
-}
-
-void AreaBullet::remove_object_overlaps(CollisionObjectBullet *p_object) {
-	// Reverse order so items can be removed.
-	for (int i = overlapping_shapes.size() - 1; i >= 0; i--) {
-		if (overlapping_shapes[i].other_object == p_object) {
-			overlapping_shapes.remove_at(i);
-		}
-	}
-}
-
-void AreaBullet::clear_overlaps() {
-	for (int i = 0; i < overlapping_shapes.size(); i++) {
-		call_event(overlapping_shapes[i], PhysicsServer3D::AREA_BODY_REMOVED);
-		overlapping_shapes[i].other_object->on_exit_area(this);
-	}
-	overlapping_shapes.clear();
-}
-
-void AreaBullet::set_monitorable(bool p_monitorable) {
-	monitorable = p_monitorable;
-	updated = true;
-}
-
-bool AreaBullet::is_monitoring() const {
-	return get_godot_object_flags() & GOF_IS_MONITORING_AREA;
-}
-
-void AreaBullet::main_shape_changed() {
-	CRASH_COND(!get_main_shape());
-	btGhost->setCollisionShape(get_main_shape());
-	updated = true;
-}
-
-void AreaBullet::reload_body() {
-	if (space) {
-		space->remove_area(this);
-		space->add_area(this);
-	}
-}
-
-void AreaBullet::set_space(SpaceBullet *p_space) {
-	// Clear the old space if there is one
-	if (space) {
-		clear_overlaps();
-		isScratched = false;
-
-		// Remove this object form the physics world
-		space->remove_area(this);
-	}
-
-	space = p_space;
-
-	if (space) {
-		space->add_area(this);
-	}
-}
-
-void AreaBullet::on_collision_filters_change() {
-	if (space) {
-		space->reload_collision_filters(this);
-	}
-	updated = true;
-}
-
-void AreaBullet::set_param(PhysicsServer3D::AreaParameter p_param, const Variant &p_value) {
-	switch (p_param) {
-		case PhysicsServer3D::AREA_PARAM_GRAVITY:
-			set_spOv_gravityMag(p_value);
-			break;
-		case PhysicsServer3D::AREA_PARAM_GRAVITY_VECTOR:
-			set_spOv_gravityVec(p_value);
-			break;
-		case PhysicsServer3D::AREA_PARAM_LINEAR_DAMP:
-			set_spOv_linearDump(p_value);
-			break;
-		case PhysicsServer3D::AREA_PARAM_ANGULAR_DAMP:
-			set_spOv_angularDump(p_value);
-			break;
-		case PhysicsServer3D::AREA_PARAM_PRIORITY:
-			set_spOv_priority(p_value);
-			break;
-		case PhysicsServer3D::AREA_PARAM_GRAVITY_IS_POINT:
-			set_spOv_gravityPoint(p_value);
-			break;
-		case PhysicsServer3D::AREA_PARAM_GRAVITY_DISTANCE_SCALE:
-			set_spOv_gravityPointDistanceScale(p_value);
-			break;
-		case PhysicsServer3D::AREA_PARAM_GRAVITY_POINT_ATTENUATION:
-			set_spOv_gravityPointAttenuation(p_value);
-			break;
-		default:
-			WARN_PRINT("Area doesn't support this parameter in the Bullet backend: " + itos(p_param));
-	}
-	isScratched = true;
-}
-
-Variant AreaBullet::get_param(PhysicsServer3D::AreaParameter p_param) const {
-	switch (p_param) {
-		case PhysicsServer3D::AREA_PARAM_GRAVITY:
-			return spOv_gravityMag;
-		case PhysicsServer3D::AREA_PARAM_GRAVITY_VECTOR:
-			return spOv_gravityVec;
-		case PhysicsServer3D::AREA_PARAM_LINEAR_DAMP:
-			return spOv_linearDump;
-		case PhysicsServer3D::AREA_PARAM_ANGULAR_DAMP:
-			return spOv_angularDump;
-		case PhysicsServer3D::AREA_PARAM_PRIORITY:
-			return spOv_priority;
-		case PhysicsServer3D::AREA_PARAM_GRAVITY_IS_POINT:
-			return spOv_gravityPoint;
-		case PhysicsServer3D::AREA_PARAM_GRAVITY_DISTANCE_SCALE:
-			return spOv_gravityPointDistanceScale;
-		case PhysicsServer3D::AREA_PARAM_GRAVITY_POINT_ATTENUATION:
-			return spOv_gravityPointAttenuation;
-		default:
-			WARN_PRINT("Area doesn't support this parameter in the Bullet backend: " + itos(p_param));
-			return Variant();
-	}
-}
-
-void AreaBullet::set_event_callback(Type p_callbackObjectType, const Callable &p_callback) {
-	InOutEventCallback &ev = eventsCallbacks[static_cast<int>(p_callbackObjectType)];
-	ev.event_callback = p_callback;
-
-	/// Set if monitoring
-	if (!eventsCallbacks[0].event_callback.is_null() || !eventsCallbacks[1].event_callback.is_null()) {
-		set_godot_object_flags(get_godot_object_flags() | GOF_IS_MONITORING_AREA);
-	} else {
-		set_godot_object_flags(get_godot_object_flags() & (~GOF_IS_MONITORING_AREA));
-		clear_overlaps();
-	}
-}
-
-bool AreaBullet::has_event_callback(Type p_callbackObjectType) {
-	return !eventsCallbacks[static_cast<int>(p_callbackObjectType)].event_callback.is_null();
-}
-
-void AreaBullet::on_enter_area(AreaBullet *p_area) {
-}
-
-void AreaBullet::on_exit_area(AreaBullet *p_area) {
-	CollisionObjectBullet::on_exit_area(p_area);
-}

modules/bullet/area_bullet.h

@@ -1,162 +0,0 @@
-/*************************************************************************/
-/*  area_bullet.h                                                        */
-/*************************************************************************/
-/*                       This file is part of:                           */
-/*                           GODOT ENGINE                                */
-/*                      https://godotengine.org                          */
-/*************************************************************************/
-/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur.                 */
-/* Copyright (c) 2014-2022 Godot Engine contributors (cf. AUTHORS.md).   */
-/*                                                                       */
-/* Permission is hereby granted, free of charge, to any person obtaining */
-/* a copy of this software and associated documentation files (the       */
-/* "Software"), to deal in the Software without restriction, including   */
-/* without limitation the rights to use, copy, modify, merge, publish,   */
-/* distribute, sublicense, and/or sell copies of the Software, and to    */
-/* permit persons to whom the Software is furnished to do so, subject to */
-/* the following conditions:                                             */
-/*                                                                       */
-/* The above copyright notice and this permission notice shall be        */
-/* included in all copies or substantial portions of the Software.       */
-/*                                                                       */
-/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
-/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
-/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
-/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
-/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION 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 AREA_BULLET_H
-#define AREA_BULLET_H
-
-#include "collision_object_bullet.h"
-#include "core/templates/vector.h"
-#include "servers/physics_server_3d.h"
-#include "space_bullet.h"
-
-class btGhostObject;
-
-class AreaBullet : public RigidCollisionObjectBullet {
-public:
-	struct InOutEventCallback {
-		Callable event_callback;
-
-		InOutEventCallback() {}
-	};
-
-	enum OverlapState {
-		OVERLAP_STATE_DIRTY = 0, // Mark processed overlaps
-		OVERLAP_STATE_INSIDE, // Mark old overlap
-		OVERLAP_STATE_ENTER, // Mark just enter overlap
-		OVERLAP_STATE_EXIT // Mark ended overlaps
-	};
-
-	struct OverlappingShapeData {
-		CollisionObjectBullet *other_object = nullptr;
-		OverlapState state = OVERLAP_STATE_DIRTY;
-		uint32_t other_shape_id = 0;
-		uint32_t our_shape_id = 0;
-
-		OverlappingShapeData() {}
-
-		OverlappingShapeData(CollisionObjectBullet *p_other_object, OverlapState p_state, uint32_t p_other_shape_id, uint32_t p_our_shape_id) :
-				other_object(p_other_object),
-				state(p_state),
-				other_shape_id(p_other_shape_id),
-				our_shape_id(p_our_shape_id) {}
-	};
-
-private:
-	// These are used by function callEvent. Instead to create this each call I create if one time.
-	Variant call_event_res[5];
-	Variant *call_event_res_ptr[5] = {};
-
-	btGhostObject *btGhost = nullptr;
-	Vector<OverlappingShapeData> overlapping_shapes;
-	int _overlapping_shape_count(CollisionObjectBullet *p_other_object);
-	int _find_overlapping_shape(CollisionObjectBullet *p_other_object, uint32_t p_other_shape_id, uint32_t p_our_shape_id);
-	bool monitorable = true;
-
-	PhysicsServer3D::AreaSpaceOverrideMode spOv_mode = PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED;
-	bool spOv_gravityPoint = false;
-	real_t spOv_gravityPointDistanceScale = 0.0;
-	real_t spOv_gravityPointAttenuation = 1.0;
-	Vector3 spOv_gravityVec = Vector3(0, -1, 0);
-	real_t spOv_gravityMag = 10.0;
-	real_t spOv_linearDump = 0.1;
-	real_t spOv_angularDump = 0.1;
-	int spOv_priority = 0;
-
-	bool isScratched = false;
-
-	InOutEventCallback eventsCallbacks[2];
-
-public:
-	AreaBullet();
-	~AreaBullet();
-
-	_FORCE_INLINE_ btGhostObject *get_bt_ghost() const { return btGhost; }
-
-	void set_monitorable(bool p_monitorable);
-	_FORCE_INLINE_ bool is_monitorable() const { return monitorable; }
-
-	bool is_monitoring() const;
-
-	_FORCE_INLINE_ void set_spOv_mode(PhysicsServer3D::AreaSpaceOverrideMode p_mode) { spOv_mode = p_mode; }
-	_FORCE_INLINE_ PhysicsServer3D::AreaSpaceOverrideMode get_spOv_mode() { return spOv_mode; }
-
-	_FORCE_INLINE_ void set_spOv_gravityPoint(bool p_isGP) { spOv_gravityPoint = p_isGP; }
-	_FORCE_INLINE_ bool is_spOv_gravityPoint() { return spOv_gravityPoint; }
-
-	_FORCE_INLINE_ void set_spOv_gravityPointDistanceScale(real_t p_GPDS) { spOv_gravityPointDistanceScale = p_GPDS; }
-	_FORCE_INLINE_ real_t get_spOv_gravityPointDistanceScale() { return spOv_gravityPointDistanceScale; }
-
-	_FORCE_INLINE_ void set_spOv_gravityPointAttenuation(real_t p_GPA) { spOv_gravityPointAttenuation = p_GPA; }
-	_FORCE_INLINE_ real_t get_spOv_gravityPointAttenuation() { return spOv_gravityPointAttenuation; }
-
-	_FORCE_INLINE_ void set_spOv_gravityVec(Vector3 p_vec) { spOv_gravityVec = p_vec; }
-	_FORCE_INLINE_ const Vector3 &get_spOv_gravityVec() const { return spOv_gravityVec; }
-
-	_FORCE_INLINE_ void set_spOv_gravityMag(real_t p_gravityMag) { spOv_gravityMag = p_gravityMag; }
-	_FORCE_INLINE_ real_t get_spOv_gravityMag() { return spOv_gravityMag; }
-
-	_FORCE_INLINE_ void set_spOv_linearDump(real_t p_linearDump) { spOv_linearDump = p_linearDump; }
-	_FORCE_INLINE_ real_t get_spOv_linearDamp() { return spOv_linearDump; }
-
-	_FORCE_INLINE_ void set_spOv_angularDump(real_t p_angularDump) { spOv_angularDump = p_angularDump; }
-	_FORCE_INLINE_ real_t get_spOv_angularDamp() { return spOv_angularDump; }
-
-	_FORCE_INLINE_ void set_spOv_priority(int p_priority) { spOv_priority = p_priority; }
-	_FORCE_INLINE_ int get_spOv_priority() { return spOv_priority; }
-
-	virtual void main_shape_changed();
-	virtual void reload_body();
-	virtual void set_space(SpaceBullet *p_space);
-
-	virtual void dispatch_callbacks();
-	void call_event(const OverlappingShapeData &p_overlapping_shape, PhysicsServer3D::AreaBodyStatus p_status);
-
-	virtual void on_collision_filters_change();
-	virtual void on_collision_checker_start() {}
-	virtual void on_collision_checker_end() { updated = false; }
-
-	void mark_all_overlaps_dirty();
-	void mark_object_overlaps_inside(CollisionObjectBullet *p_other_object);
-	void set_overlap(CollisionObjectBullet *p_other_object, uint32_t p_other_shape_id, uint32_t p_our_shape_id);
-	void mark_all_dirty_overlaps_as_exit();
-	void remove_object_overlaps(CollisionObjectBullet *p_object);
-	void clear_overlaps();
-
-	void set_param(PhysicsServer3D::AreaParameter p_param, const Variant &p_value);
-	Variant get_param(PhysicsServer3D::AreaParameter p_param) const;
-
-	void set_event_callback(Type p_callbackObjectType, const Callable &p_callback);
-	bool has_event_callback(Type p_callbackObjectType);
-
-	virtual void on_enter_area(AreaBullet *p_area);
-	virtual void on_exit_area(AreaBullet *p_area);
-};
-
-#endif // AREA_BULLET_H

modules/bullet/btRayShape.cpp

@@ -1,101 +0,0 @@
-/*************************************************************************/
-/*  btRayShape.cpp                                                       */
-/*************************************************************************/
-/*                       This file is part of:                           */
-/*                           GODOT ENGINE                                */
-/*                      https://godotengine.org                          */
-/*************************************************************************/
-/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur.                 */
-/* Copyright (c) 2014-2022 Godot Engine contributors (cf. AUTHORS.md).   */
-/*                                                                       */
-/* Permission is hereby granted, free of charge, to any person obtaining */
-/* a copy of this software and associated documentation files (the       */
-/* "Software"), to deal in the Software without restriction, including   */
-/* without limitation the rights to use, copy, modify, merge, publish,   */
-/* distribute, sublicense, and/or sell copies of the Software, and to    */
-/* permit persons to whom the Software is furnished to do so, subject to */
-/* the following conditions:                                             */
-/*                                                                       */
-/* The above copyright notice and this permission notice shall be        */
-/* included in all copies or substantial portions of the Software.       */
-/*                                                                       */
-/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
-/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
-/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
-/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
-/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION 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 "btRayShape.h"
-
-#include "core/math/math_funcs.h"
-
-#include <LinearMath/btAabbUtil2.h>
-
-btRayShape::btRayShape(btScalar length) :
-		btConvexInternalShape() {
-	m_shapeType = CUSTOM_CONVEX_SHAPE_TYPE;
-	setLength(length);
-}
-
-btRayShape::~btRayShape() {
-}
-
-void btRayShape::setLength(btScalar p_length) {
-	m_length = p_length;
-	reload_cache();
-}
-
-void btRayShape::setMargin(btScalar margin) {
-	btConvexInternalShape::setMargin(margin);
-	reload_cache();
-}
-
-void btRayShape::setSlipsOnSlope(bool p_slipsOnSlope) {
-	slipsOnSlope = p_slipsOnSlope;
-}
-
-btVector3 btRayShape::localGetSupportingVertex(const btVector3 &vec) const {
-	return localGetSupportingVertexWithoutMargin(vec) + (m_shapeAxis * m_collisionMargin);
-}
-
-btVector3 btRayShape::localGetSupportingVertexWithoutMargin(const btVector3 &vec) const {
-	if (vec.z() > 0) {
-		return m_shapeAxis * m_cacheScaledLength;
-	} else {
-		return btVector3(0, 0, 0);
-	}
-}
-
-void btRayShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3 *vectors, btVector3 *supportVerticesOut, int numVectors) const {
-	for (int i = 0; i < numVectors; ++i) {
-		supportVerticesOut[i] = localGetSupportingVertexWithoutMargin(vectors[i]);
-	}
-}
-
-void btRayShape::getAabb(const btTransform &t, btVector3 &aabbMin, btVector3 &aabbMax) const {
-	btVector3 localAabbMin(0, 0, 0);
-	btVector3 localAabbMax(m_shapeAxis * m_cacheScaledLength);
-	btTransformAabb(localAabbMin, localAabbMax, m_collisionMargin, t, aabbMin, aabbMax);
-}
-
-void btRayShape::calculateLocalInertia(btScalar mass, btVector3 &inertia) const {
-	inertia.setZero();
-}
-
-int btRayShape::getNumPreferredPenetrationDirections() const {
-	return 0;
-}
-
-void btRayShape::getPreferredPenetrationDirection(int index, btVector3 &penetrationVector) const {
-	penetrationVector.setZero();
-}
-
-void btRayShape::reload_cache() {
-	m_cacheScaledLength = m_length * m_localScaling[2];
-
-	m_cacheSupportPoint.setIdentity();
-	m_cacheSupportPoint.setOrigin(m_shapeAxis * m_cacheScaledLength);
-}

modules/bullet/btRayShape.h

@@ -1,91 +0,0 @@
-/*************************************************************************/
-/*  btRayShape.h                                                         */
-/*************************************************************************/
-/*                       This file is part of:                           */
-/*                           GODOT ENGINE                                */
-/*                      https://godotengine.org                          */
-/*************************************************************************/
-/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur.                 */
-/* Copyright (c) 2014-2022 Godot Engine contributors (cf. AUTHORS.md).   */
-/*                                                                       */
-/* Permission is hereby granted, free of charge, to any person obtaining */
-/* a copy of this software and associated documentation files (the       */
-/* "Software"), to deal in the Software without restriction, including   */
-/* without limitation the rights to use, copy, modify, merge, publish,   */
-/* distribute, sublicense, and/or sell copies of the Software, and to    */
-/* permit persons to whom the Software is furnished to do so, subject to */
-/* the following conditions:                                             */
-/*                                                                       */
-/* The above copyright notice and this permission notice shall be        */
-/* included in all copies or substantial portions of the Software.       */
-/*                                                                       */
-/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
-/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
-/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
-/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
-/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
-/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
-/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
-/*************************************************************************/
-
-/// IMPORTANT The class name and filename was created by following Bullet writing rules for an easy (eventually) porting to bullet
-/// This shape is a custom shape that is not present to Bullet physics engine
-#ifndef BTRAYSHAPE_H
-#define BTRAYSHAPE_H
-
-#include <BulletCollision/CollisionShapes/btConvexInternalShape.h>
-
-/// Ray shape around z axis
-ATTRIBUTE_ALIGNED16(class)
-btRayShape : public btConvexInternalShape {
-	btScalar m_length = 0;
-	bool slipsOnSlope = false;
-	/// The default axis is the z
-	btVector3 m_shapeAxis = btVector3(0, 0, 1);
-
-	btTransform m_cacheSupportPoint;
-	btScalar m_cacheScaledLength;
-
-public:
-	BT_DECLARE_ALIGNED_ALLOCATOR();
-
-	btRayShape(btScalar length);
-	virtual ~btRayShape();
-
-	void setLength(btScalar p_length);
-	btScalar getLength() const { return m_length; }
-
-	virtual void setMargin(btScalar margin);
-
-	void setSlipsOnSlope(bool p_slipsOnSlope);
-	bool getSlipsOnSlope() const { return slipsOnSlope; }
-
-	const btTransform &getSupportPoint() const { return m_cacheSupportPoint; }
-	const btScalar &getScaledLength() const { return m_cacheScaledLength; }
-
-	virtual btVector3 localGetSupportingVertex(const btVector3 &vec) const;
-#ifndef __SPU__
-	virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3 &vec) const;
-#endif //#ifndef __SPU__
-
-	virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3 *vectors, btVector3 *supportVerticesOut, int numVectors) const;
-
-	///getAabb returns the axis aligned bounding box in the coordinate frame of the given transform t.
-	virtual void getAabb(const btTransform &t, btVector3 &aabbMin, btVector3 &aabbMax) const;
-
-#ifndef __SPU__
-	virtual void calculateLocalInertia(btScalar mass, btVector3 & inertia) const;
-
-	virtual const char *getName() const {
-		return "RayZ";
-	}
-#endif //__SPU__
-
-	virtual int getNumPreferredPenetrationDirections() const;
-	virtual void getPreferredPenetrationDirection(int index, btVector3 &penetrationVector) const;
-
-private:
-	void reload_cache();
-};
-
-#endif // BTRAYSHAPE_H

modules/bullet/bullet_physics_server.cpp

@@ -1,1535 +0,0 @@
-/*************************************************************************/
-/*  bullet_physics_server.cpp                                            */
-/*************************************************************************/
-/*                       This file is part of:                           */
-/*                           GODOT ENGINE                                */
-/*                      https://godotengine.org                          */
-/*************************************************************************/
-/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur.                 */
-/* Copyright (c) 2014-2022 Godot Engine contributors (cf. AUTHORS.md).   */
-/*                                                                       */
-/* Permission is hereby granted, free of charge, to any person obtaining */
-/* a copy of this software and associated documentation files (the       */
-/* "Software"), to deal in the Software without restriction, including   */
-/* without limitation the rights to use, copy, modify, merge, publish,   */
-/* distribute, sublicense, and/or sell copies of the Software, and to    */
-/* permit persons to whom the Software is furnished to do so, subject to */
-/* the following conditions:                                             */
-/*                                                                       */
-/* The above copyright notice and this permission notice shall be        */
-/* included in all copies or substantial portions of the Software.       */
-/*                                                                       */
-/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
-/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
-/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
-/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
-/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION 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 "bullet_physics_server.h"
-
-#include "bullet_utilities.h"
-#include "cone_twist_joint_bullet.h"
-#include "core/error/error_macros.h"
-#include "core/object/class_db.h"
-#include "core/string/ustring.h"
-#include "generic_6dof_joint_bullet.h"
-#include "hinge_joint_bullet.h"
-#include "pin_joint_bullet.h"
-#include "shape_bullet.h"
-#include "slider_joint_bullet.h"
-
-#include <LinearMath/btVector3.h>
-
-#include <assert.h>
-
-#define CreateThenReturnRID(owner, ridData) \
-	RID rid = owner.make_rid(ridData);      \
-	ridData->set_self(rid);                 \
-	ridData->_set_physics_server(this);     \
-	return rid;
-
-// <--------------- Joint creation asserts
-/// Assert the body is assigned to a space
-#define JointAssertSpace(body, bIndex, ret)                                                          \
-	if (!body->get_space()) {                                                                        \
-		ERR_PRINT("Before create a joint the Body" + String(bIndex) + " must be added to a space!"); \
-		return ret;                                                                                  \
-	}
-
-/// Assert the two bodies of joint are in the same space
-#define JointAssertSameSpace(bodyA, bodyB, ret)                                                   \
-	if (bodyA->get_space() != bodyB->get_space()) {                                               \
-		ERR_PRINT("In order to create a joint the Body_A and Body_B must be in the same space!"); \
-		return RID();                                                                             \
-	}
-
-#define AddJointToSpace(body, joint) \
-	body->get_space()->add_constraint(joint, joint->is_disabled_collisions_between_bodies());
-// <--------------- Joint creation asserts
-
-void BulletPhysicsServer3D::_bind_methods() {
-	//ClassDB::bind_method(D_METHOD("DoTest"), &BulletPhysicsServer3D::DoTest);
-}
-
-BulletPhysicsServer3D::BulletPhysicsServer3D() :
-		PhysicsServer3D() {}
-
-BulletPhysicsServer3D::~BulletPhysicsServer3D() {}
-
-RID BulletPhysicsServer3D::shape_create(ShapeType p_shape) {
-	ShapeBullet *shape = nullptr;
-
-	switch (p_shape) {
-		case SHAPE_WORLD_BOUNDARY: {
-			shape = bulletnew(WorldBoundaryShapeBullet);
-		} break;
-		case SHAPE_SPHERE: {
-			shape = bulletnew(SphereShapeBullet);
-		} break;
-		case SHAPE_BOX: {
-			shape = bulletnew(BoxShapeBullet);
-		} break;
-		case SHAPE_CAPSULE: {
-			shape = bulletnew(CapsuleShapeBullet);
-		} break;
-		case SHAPE_CYLINDER: {
-			shape = bulletnew(CylinderShapeBullet);
-		} break;
-		case SHAPE_CONVEX_POLYGON: {
-			shape = bulletnew(ConvexPolygonShapeBullet);
-		} break;
-		case SHAPE_CONCAVE_POLYGON: {
-			shape = bulletnew(ConcavePolygonShapeBullet);
-		} break;
-		case SHAPE_HEIGHTMAP: {
-			shape = bulletnew(HeightMapShapeBullet);
-		} break;
-		case SHAPE_RAY: {
-			shape = bulletnew(RayShapeBullet);
-		} break;
-		case SHAPE_CUSTOM:
-		default:
-			ERR_FAIL_V(RID());
-			break;
-	}
-
-	CreateThenReturnRID(shape_owner, shape)
-}
-
-void BulletPhysicsServer3D::shape_set_data(RID p_shape, const Variant &p_data) {
-	ShapeBullet *shape = shape_owner.get_or_null(p_shape);
-	ERR_FAIL_COND(!shape);
-	shape->set_data(p_data);
-}
-
-void BulletPhysicsServer3D::shape_set_custom_solver_bias(RID p_shape, real_t p_bias) {
-	//WARN_PRINT("Bias not supported by Bullet physics engine");
-}
-
-PhysicsServer3D::ShapeType BulletPhysicsServer3D::shape_get_type(RID p_shape) const {
-	ShapeBullet *shape = shape_owner.get_or_null(p_shape);
-	ERR_FAIL_COND_V(!shape, PhysicsServer3D::SHAPE_CUSTOM);
-	return shape->get_type();
-}
-
-Variant BulletPhysicsServer3D::shape_get_data(RID p_shape) const {
-	ShapeBullet *shape = shape_owner.get_or_null(p_shape);
-	ERR_FAIL_COND_V(!shape, Variant());
-	return shape->get_data();
-}
-
-void BulletPhysicsServer3D::shape_set_margin(RID p_shape, real_t p_margin) {
-	ShapeBullet *shape = shape_owner.get_or_null(p_shape);
-	ERR_FAIL_COND(!shape);
-	shape->set_margin(p_margin);
-}
-
-real_t BulletPhysicsServer3D::shape_get_margin(RID p_shape) const {
-	ShapeBullet *shape = shape_owner.get_or_null(p_shape);
-	ERR_FAIL_COND_V(!shape, 0.0);
-	return shape->get_margin();
-}
-
-real_t BulletPhysicsServer3D::shape_get_custom_solver_bias(RID p_shape) const {
-	//WARN_PRINT("Bias not supported by Bullet physics engine");
-	return 0.;
-}
-
-RID BulletPhysicsServer3D::space_create() {
-	SpaceBullet *space = bulletnew(SpaceBullet);
-	CreateThenReturnRID(space_owner, space);
-}
-
-void BulletPhysicsServer3D::space_set_active(RID p_space, bool p_active) {
-	SpaceBullet *space = space_owner.get_or_null(p_space);
-	ERR_FAIL_COND(!space);
-
-	if (space_is_active(p_space) == p_active) {
-		return;
-	}
-
-	if (p_active) {
-		++active_spaces_count;
-		active_spaces.push_back(space);
-	} else {
-		--active_spaces_count;
-		active_spaces.erase(space);
-	}
-}
-
-bool BulletPhysicsServer3D::space_is_active(RID p_space) const {
-	SpaceBullet *space = space_owner.get_or_null(p_space);
-	ERR_FAIL_COND_V(!space, false);
-
-	return -1 != active_spaces.find(space);
-}
-
-void BulletPhysicsServer3D::space_set_param(RID p_space, SpaceParameter p_param, real_t p_value) {
-	SpaceBullet *space = space_owner.get_or_null(p_space);
-	ERR_FAIL_COND(!space);
-	space->set_param(p_param, p_value);
-}
-
-real_t BulletPhysicsServer3D::space_get_param(RID p_space, SpaceParameter p_param) const {
-	SpaceBullet *space = space_owner.get_or_null(p_space);
-	ERR_FAIL_COND_V(!space, 0);
-	return space->get_param(p_param);
-}
-
-PhysicsDirectSpaceState3D *BulletPhysicsServer3D::space_get_direct_state(RID p_space) {
-	SpaceBullet *space = space_owner.get_or_null(p_space);
-	ERR_FAIL_COND_V(!space, nullptr);
-
-	return space->get_direct_state();
-}
-
-void BulletPhysicsServer3D::space_set_debug_contacts(RID p_space, int p_max_contacts) {
-	SpaceBullet *space = space_owner.get_or_null(p_space);
-	ERR_FAIL_COND(!space);
-
-	space->set_debug_contacts(p_max_contacts);
-}
-
-Vector<Vector3> BulletPhysicsServer3D::space_get_contacts(RID p_space) const {
-	SpaceBullet *space = space_owner.get_or_null(p_space);
-	ERR_FAIL_COND_V(!space, Vector<Vector3>());
-
-	return space->get_debug_contacts();
-}
-
-int BulletPhysicsServer3D::space_get_contact_count(RID p_space) const {
-	SpaceBullet *space = space_owner.get_or_null(p_space);
-	ERR_FAIL_COND_V(!space, 0);
-
-	return space->get_debug_contact_count();
-}
-
-RID BulletPhysicsServer3D::area_create() {
-	AreaBullet *area = bulletnew(AreaBullet);
-	area->set_collision_layer(1);
-	area->set_collision_mask(1);
-	CreateThenReturnRID(area_owner, area)
-}
-
-void BulletPhysicsServer3D::area_set_space(RID p_area, RID p_space) {
-	AreaBullet *area = area_owner.get_or_null(p_area);
-	ERR_FAIL_COND(!area);
-	SpaceBullet *space = nullptr;
-	if (p_space.is_valid()) {
-		space = space_owner.get_or_null(p_space);
-		ERR_FAIL_COND(!space);
-	}
-	area->set_space(space);
-}
-
-RID BulletPhysicsServer3D::area_get_space(RID p_area) const {
-	AreaBullet *area = area_owner.get_or_null(p_area);
-	return area->get_space()->get_self();
-}
-
-void BulletPhysicsServer3D::area_set_space_override_mode(RID p_area, AreaSpaceOverrideMode p_mode) {
-	AreaBullet *area = area_owner.get_or_null(p_area);
-	ERR_FAIL_COND(!area);
-
-	area->set_spOv_mode(p_mode);
-}
-
-PhysicsServer3D::AreaSpaceOverrideMode BulletPhysicsServer3D::area_get_space_override_mode(RID p_area) const {
-	AreaBullet *area = area_owner.get_or_null(p_area);
-	ERR_FAIL_COND_V(!area, PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED);
-
-	return area->get_spOv_mode();
-}
-
-void BulletPhysicsServer3D::area_add_shape(RID p_area, RID p_shape, const Transform3D &p_transform, bool p_disabled) {
-	AreaBullet *area = area_owner.get_or_null(p_area);
-	ERR_FAIL_COND(!area);
-
-	ShapeBullet *shape = shape_owner.get_or_null(p_shape);
-	ERR_FAIL_COND(!shape);
-
-	area->add_shape(shape, p_transform, p_disabled);
-}
-
-void BulletPhysicsServer3D::area_set_shape(RID p_area, int p_shape_idx, RID p_shape) {
-	AreaBullet *area = area_owner.get_or_null(p_area);
-	ERR_FAIL_COND(!area);
-
-	ShapeBullet *shape = shape_owner.get_or_null(p_shape);
-	ERR_FAIL_COND(!shape);
-
-	area->set_shape(p_shape_idx, shape);
-}
-
-void BulletPhysicsServer3D::area_set_shape_transform(RID p_area, int p_shape_idx, const Transform3D &p_transform) {
-	AreaBullet *area = area_owner.get_or_null(p_area);
-	ERR_FAIL_COND(!area);
-
-	area->set_shape_transform(p_shape_idx, p_transform);
-}
-
-int BulletPhysicsServer3D::area_get_shape_count(RID p_area) const {
-	AreaBullet *area = area_owner.get_or_null(p_area);
-	ERR_FAIL_COND_V(!area, 0);
-
-	return area->get_shape_count();
-}
-
-RID BulletPhysicsServer3D::area_get_shape(RID p_area, int p_shape_idx) const {
-	AreaBullet *area = area_owner.get_or_null(p_area);
-	ERR_FAIL_COND_V(!area, RID());
-
-	return area->get_shape(p_shape_idx)->get_self();
-}
-
-Transform3D BulletPhysicsServer3D::area_get_shape_transform(RID p_area, int p_shape_idx) const {
-	AreaBullet *area = area_owner.get_or_null(p_area);
-	ERR_FAIL_COND_V(!area, Transform3D());
-
-	return area->get_shape_transform(p_shape_idx);
-}
-
-void BulletPhysicsServer3D::area_remove_shape(RID p_area, int p_shape_idx) {
-	AreaBullet *area = area_owner.get_or_null(p_area);
-	ERR_FAIL_COND(!area);
-	return area->remove_shape_full(p_shape_idx);
-}
-
-void BulletPhysicsServer3D::area_clear_shapes(RID p_area) {
-	AreaBullet *area = area_owner.get_or_null(p_area);
-	ERR_FAIL_COND(!area);
-
-	for (int i = area->get_shape_count(); 0 < i; --i) {
-		area->remove_shape_full(0);
-	}
-}
-
-void BulletPhysicsServer3D::area_set_shape_disabled(RID p_area, int p_shape_idx, bool p_disabled) {
-	AreaBullet *area = area_owner.get_or_null(p_area);
-	ERR_FAIL_COND(!area);
-
-	area->set_shape_disabled(p_shape_idx, p_disabled);
-}
-
-void BulletPhysicsServer3D::area_attach_object_instance_id(RID p_area, ObjectID p_id) {
-	if (space_owner.owns(p_area)) {
-		return;
-	}
-	AreaBullet *area = area_owner.get_or_null(p_area);
-	ERR_FAIL_COND(!area);
-	area->set_instance_id(p_id);
-}
-
-ObjectID BulletPhysicsServer3D::area_get_object_instance_id(RID p_area) const {
-	if (space_owner.owns(p_area)) {
-		return ObjectID();
-	}
-	AreaBullet *area = area_owner.get_or_null(p_area);
-	ERR_FAIL_COND_V(!area, ObjectID());
-	return area->get_instance_id();
-}
-
-void BulletPhysicsServer3D::area_set_param(RID p_area, AreaParameter p_param, const Variant &p_value) {
-	if (space_owner.owns(p_area)) {
-		SpaceBullet *space = space_owner.get_or_null(p_area);
-		if (space) {
-			space->set_param(p_param, p_value);
-		}
-	} else {
-		AreaBullet *area = area_owner.get_or_null(p_area);
-		ERR_FAIL_COND(!area);
-
-		area->set_param(p_param, p_value);
-	}
-}
-
-Variant BulletPhysicsServer3D::area_get_param(RID p_area, AreaParameter p_param) const {
-	if (space_owner.owns(p_area)) {
-		SpaceBullet *space = space_owner.get_or_null(p_area);
-		return space->get_param(p_param);
-	} else {
-		AreaBullet *area = area_owner.get_or_null(p_area);
-		ERR_FAIL_COND_V(!area, Variant());
-
-		return area->get_param(p_param);
-	}
-}
-
-void BulletPhysicsServer3D::area_set_transform(RID p_area, const Transform3D &p_transform) {
-	AreaBullet *area = area_owner.get_or_null(p_area);
-	ERR_FAIL_COND(!area);
-	area->set_transform(p_transform);
-}
-
-Transform3D BulletPhysicsServer3D::area_get_transform(RID p_area) const {
-	AreaBullet *area = area_owner.get_or_null(p_area);
-	ERR_FAIL_COND_V(!area, Transform3D());
-	return area->get_transform();
-}
-
-void BulletPhysicsServer3D::area_set_collision_mask(RID p_area, uint32_t p_mask) {
-	AreaBullet *area = area_owner.get_or_null(p_area);
-	ERR_FAIL_COND(!area);
-	area->set_collision_mask(p_mask);
-}
-
-void BulletPhysicsServer3D::area_set_collision_layer(RID p_area, uint32_t p_layer) {
-	AreaBullet *area = area_owner.get_or_null(p_area);
-	ERR_FAIL_COND(!area);
-	area->set_collision_layer(p_layer);
-}
-
-void BulletPhysicsServer3D::area_set_monitorable(RID p_area, bool p_monitorable) {
-	AreaBullet *area = area_owner.get_or_null(p_area);
-	ERR_FAIL_COND(!area);
-
-	area->set_monitorable(p_monitorable);
-}
-
-void BulletPhysicsServer3D::area_set_monitor_callback(RID p_area, const Callable &p_callback) {
-	AreaBullet *area = area_owner.get_or_null(p_area);
-	ERR_FAIL_COND(!area);
-
-	area->set_event_callback(CollisionObjectBullet::TYPE_RIGID_BODY, p_callback.is_valid() ? p_callback : Callable());
-}
-
-void BulletPhysicsServer3D::area_set_area_monitor_callback(RID p_area, const Callable &p_callback) {
-	AreaBullet *area = area_owner.get_or_null(p_area);
-	ERR_FAIL_COND(!area);
-
-	area->set_event_callback(CollisionObjectBullet::TYPE_AREA, p_callback.is_valid() ? p_callback : Callable());
-}
-
-void BulletPhysicsServer3D::area_set_ray_pickable(RID p_area, bool p_enable) {
-	AreaBullet *area = area_owner.get_or_null(p_area);
-	ERR_FAIL_COND(!area);
-	area->set_ray_pickable(p_enable);
-}
-
-RID BulletPhysicsServer3D::body_create(BodyMode p_mode, bool p_init_sleeping) {
-	RigidBodyBullet *body = bulletnew(RigidBodyBullet);
-	body->set_mode(p_mode);
-	body->set_collision_layer(1);
-	body->set_collision_mask(1);
-	if (p_init_sleeping) {
-		body->set_state(BODY_STATE_SLEEPING, p_init_sleeping);
-	}
-	CreateThenReturnRID(rigid_body_owner, body);
-}
-
-void BulletPhysicsServer3D::body_set_space(RID p_body, RID p_space) {
-	RigidBodyBullet *body = rigid_body_owner.get_or_null(p_body);
-	ERR_FAIL_COND(!body);
-	SpaceBullet *space = nullptr;
-
-	if (p_space.is_valid()) {
-		space = space_owner.get_or_null(p_space);
-		ERR_FAIL_COND(!space);
-	}
-
-	if (body->get_space() == space) {
-		return; //pointless
-	}
-
-	body->set_space(space);
-}
-
-RID BulletPhysicsServer3D::body_get_space(RID p_body) const {
-	RigidBodyBullet *body = rigid_body_owner.get_or_null(p_body);
-	ERR_FAIL_COND_V(!body, RID());
-
-	SpaceBullet *space = body->get_space();
-	if (!space) {
-		return RID();
-	}
-	return space->get_self();
-}
-
-void BulletPhysicsServer3D::body_set_mode(RID p_body, PhysicsServer3D::BodyMode p_mode) {
-	RigidBodyBullet *body = rigid_body_owner.get_or_null(p_body);
-	ERR_FAIL_COND(!body);
-	body->set_mode(p_mode);
-}
-
-PhysicsServer3D::BodyMode BulletPhysicsServer3D::body_get_mode(RID p_body) const {
-	RigidBodyBullet *body = rigid_body_owner.get_or_null(p_body);
-	ERR_FAIL_COND_V(!body, BODY_MODE_STATIC);
-	return body->get_mode();
-}
-
-void BulletPhysicsServer3D::body_add_shape(RID p_body, RID p_shape, const Transform3D &p_transform, bool p_disabled) {
-	RigidBodyBullet *body = rigid_body_owner.get_or_null(p_body);
-	ERR_FAIL_COND(!body);
-
-	ShapeBullet *shape = shape_owner.get_or_null(p_shape);
-	ERR_FAIL_COND(!shape);
-
-	body->add_shape(shape, p_transform, p_disabled);
-}
-
-void BulletPhysicsServer3D::body_set_shape(RID p_body, int p_shape_idx, RID p_shape) {
-	RigidBodyBullet *body = rigid_body_owner.get_or_null(p_body);
-	ERR_FAIL_COND(!body);
-
-	ShapeBullet *shape = shape_owner.get_or_null(p_shape);
-	ERR_FAIL_COND(!shape);
-
-	body->set_shape(p_shape_idx, shape);
-}
-
-void BulletPhysicsServer3D::body_set_shape_transform(RID p_body, int p_shape_idx, const Transform3D &p_transform) {
-	RigidBodyBullet *body = rigid_body_owner.get_or_null(p_body);
-	ERR_FAIL_COND(!body);
-
-	body->set_shape_transform(p_shape_idx, p_transform);
-}
-
-int BulletPhysicsServer3D::body_get_shape_count(RID p_body) const {
-	RigidBodyBullet *body = rigid_body_owner.get_or_null(p_body);
-	ERR_FAIL_COND_V(!body, 0);
-	return body->get_shape_count();
-}
-
-RID BulletPhysicsServer3D::body_get_shape(RID p_body, int p_shape_idx) const {
-	RigidBodyBullet *body = rigid_body_owner.get_or_null(p_body);
-	ERR_FAIL_COND_V(!body, RID());
-
-	ShapeBullet *shape = body->get_shape(p_shape_idx);
-	ERR_FAIL_COND_V(!shape, RID());
-
-	return shape->get_self();
-}
-
-Transform3D BulletPhysicsServer3D::body_get_shape_transform(RID p_body, int p_shape_idx) const {
-	RigidBodyBullet *body = rigid_body_owner.get_or_null(p_body);
-	ERR_FAIL_COND_V(!body, Transform3D());
-	return body->get_shape_transform(p_shape_idx);
-}
-
-void BulletPhysicsServer3D::body_set_shape_disabled(RID p_body, int p_shape_idx, bool p_disabled) {
-	RigidBodyBullet *body = rigid_body_owner.get_or_null(p_body);
-	ERR_FAIL_COND(!body);
-
-	body->set_shape_disabled(p_shape_idx, p_disabled);
-}
-
-void BulletPhysicsServer3D::body_remove_shape(RID p_body, int p_shape_idx) {
-	RigidBodyBullet *body = rigid_body_owner.get_or_null(p_body);
-	ERR_FAIL_COND(!body);
-
-	body->remove_shape_full(p_shape_idx);
-}
-
-void BulletPhysicsServer3D::body_clear_shapes(RID p_body) {
-	RigidBodyBullet *body = rigid_body_owner.get_or_null(p_body);
-	ERR_FAIL_COND(!body);
-
-	body->remove_all_shapes();
-}
-
-void BulletPhysicsServer3D::body_attach_object_instance_id(RID p_body, ObjectID p_id) {
-	CollisionObjectBullet *body = get_collision_object(p_body);
-	ERR_FAIL_COND(!body);
-
-	body->set_instance_id(p_id);
-}
-
-ObjectID BulletPhysicsServer3D::body_get_object_instance_id(RID p_body) const {
-	CollisionObjectBullet *body = get_collision_object(p_body);
-	ERR_FAIL_COND_V(!body, ObjectID());
-
-	return body->get_instance_id();
-}
-
-void BulletPhysicsServer3D::body_set_enable_continuous_collision_detection(RID p_body, bool p_enable) {
-	RigidBodyBullet *body = rigid_body_owner.get_or_null(p_body);
-	ERR_FAIL_COND(!body);
-
-	body->set_continuous_collision_detection(p_enable);
-}
-
-bool BulletPhysicsServer3D::body_is_continuous_collision_detection_enabled(RID p_body) const {
-	RigidBodyBullet *body = rigid_body_owner.get_or_null(p_body);
-	ERR_FAIL_COND_V(!body, false);
-
-	return body->is_continuous_collision_detection_enabled();
-}
-
-void BulletPhysicsServer3D::body_set_collision_layer(RID p_body, uint32_t p_layer) {
-	RigidBodyBullet *body = rigid_body_owner.get_or_null(p_body);
-	ERR_FAIL_COND(!body);
-
-	body->set_collision_layer(p_layer);
-}
-
-uint32_t BulletPhysicsServer3D::body_get_collision_layer(RID p_body) const {
-	const RigidBodyBullet *body = rigid_body_owner.get_or_null(p_body);
-	ERR_FAIL_COND_V(!body, 0);
-
-	return body->get_collision_layer();
-}
-
-void BulletPhysicsServer3D::body_set_collision_mask(RID p_body, uint32_t p_mask) {
-	RigidBodyBullet *body = rigid_body_owner.get_or_null(p_body);
-	ERR_FAIL_COND(!body);
-
-	body->set_collision_mask(p_mask);
-}
-
-uint32_t BulletPhysicsServer3D::body_get_collision_mask(RID p_body) const {
-	RigidBodyBullet *body = rigid_body_owner.get_or_null(p_body);
-	ERR_FAIL_COND_V(!body, 0);
-
-	return body->get_collision_mask();
-}
-
-void BulletPhysicsServer3D::body_set_user_flags(RID p_body, uint32_t p_flags) {
-	// This function is not currently supported
-}
-
-uint32_t BulletPhysicsServer3D::body_get_user_flags(RID p_body) const {
-	// This function is not currently supported
-	return 0;
-}
-
-void BulletPhysicsServer3D::body_set_param(RID p_body, BodyParameter p_param, real_t p_value) {
-	RigidBodyBullet *body = rigid_body_owner.get_or_null(p_body);
-	ERR_FAIL_COND(!body);
-
-	body->set_param(p_param, p_value);
-}
-
-real_t BulletPhysicsServer3D::body_get_param(RID p_body, BodyParameter p_param) const {
-	RigidBodyBullet *body = rigid_body_owner.get_or_null(p_body);
-	ERR_FAIL_COND_V(!body, 0);
-
-	return body->get_param(p_param);
-}
-
-void BulletPhysicsServer3D::body_set_kinematic_safe_margin(RID p_body, real_t p_margin) {
-	RigidBodyBullet *body = rigid_body_owner.get_or_null(p_body);
-	ERR_FAIL_COND(!body);
-
-	if (body->get_kinematic_utilities()) {
-		body->get_kinematic_utilities()->setSafeMargin(p_margin);
-	}
-}
-
-real_t BulletPhysicsServer3D::body_get_kinematic_safe_margin(RID p_body) const {
-	RigidBodyBullet *body = rigid_body_owner.get_or_null(p_body);
-	ERR_FAIL_COND_V(!body, 0);
-
-	if (body->get_kinematic_utilities()) {
-		return body->get_kinematic_utilities()->safe_margin;
-	}
-
-	return 0;
-}
-
-void BulletPhysicsServer3D::body_set_state(RID p_body, BodyState p_state, const Variant &p_variant) {
-	RigidBodyBullet *body = rigid_body_owner.get_or_null(p_body);
-	ERR_FAIL_COND(!body);
-
-	body->set_state(p_state, p_variant);
-}
-
-Variant BulletPhysicsServer3D::body_get_state(RID p_body, BodyState p_state) const {
-	RigidBodyBullet *body = rigid_body_owner.get_or_null(p_body);
-	ERR_FAIL_COND_V(!body, Variant());
-
-	return body->get_state(p_state);
-}
-
-void BulletPhysicsServer3D::body_set_applied_force(RID p_body, const Vector3 &p_force) {
-	RigidBodyBullet *body = rigid_body_owner.get_or_null(p_body);
-	ERR_FAIL_COND(!body);
-
-	body->set_applied_force(p_force);
-}
-
-Vector3 BulletPhysicsServer3D::body_get_applied_force(RID p_body) const {
-	RigidBodyBullet *body = rigid_body_owner.get_or_null(p_body);
-	ERR_FAIL_COND_V(!body, Vector3());
-	return body->get_applied_force();
-}
-
-void BulletPhysicsServer3D::body_set_applied_torque(RID p_body, const Vector3 &p_torque) {
-	RigidBodyBullet *body = rigid_body_owner.get_or_null(p_body);
-	ERR_FAIL_COND(!body);
-
-	body->set_applied_torque(p_torque);
-}
-
-Vector3 BulletPhysicsServer3D::body_get_applied_torque(RID p_body) const {
-	RigidBodyBullet *body = rigid_body_owner.get_or_null(p_body);
-	ERR_FAIL_COND_V(!body, Vector3());
-
-	return body->get_applied_torque();
-}
-
-void BulletPhysicsServer3D::body_add_central_force(RID p_body, const Vector3 &p_force) {
-	RigidBodyBullet *body = rigid_body_owner.get_or_null(p_body);
-	ERR_FAIL_COND(!body);
-
-	body->apply_central_force(p_force);
-}
-
-void BulletPhysicsServer3D::body_add_force(RID p_body, const Vector3 &p_force, const Vector3 &p_position) {
-	RigidBodyBullet *body = rigid_body_owner.get_or_null(p_body);
-	ERR_FAIL_COND(!body);
-
-	body->apply_force(p_force, p_position);
-}
-
-void BulletPhysicsServer3D::body_add_torque(RID p_body, const Vector3 &p_torque) {
-	RigidBodyBullet *body = rigid_body_owner.get_or_null(p_body);
-	ERR_FAIL_COND(!body);
-
-	body->apply_torque(p_torque);
-}
-
-void BulletPhysicsServer3D::body_apply_central_impulse(RID p_body, const Vector3 &p_impulse) {
-	RigidBodyBullet *body = rigid_body_owner.get_or_null(p_body);
-	ERR_FAIL_COND(!body);
-
-	body->apply_central_impulse(p_impulse);
-}
-
-void BulletPhysicsServer3D::body_apply_impulse(RID p_body, const Vector3 &p_impulse, const Vector3 &p_position) {
-	RigidBodyBullet *body = rigid_body_owner.get_or_null(p_body);
-	ERR_FAIL_COND(!body);
-
-	body->apply_impulse(p_impulse, p_position);
-}
-
-void BulletPhysicsServer3D::body_apply_torque_impulse(RID p_body, const Vector3 &p_impulse) {
-	RigidBodyBullet *body = rigid_body_owner.get_or_null(p_body);
-	ERR_FAIL_COND(!body);
-
-	body->apply_torque_impulse(p_impulse);
-}
-
-void BulletPhysicsServer3D::body_set_axis_velocity(RID p_body, const Vector3 &p_axis_velocity) {
-	RigidBodyBullet *body = rigid_body_owner.get_or_null(p_body);
-	ERR_FAIL_COND(!body);
-
-	Vector3 v = body->get_linear_velocity();
-	Vector3 axis = p_axis_velocity.normalized();
-	v -= axis * axis.dot(v);
-	v += p_axis_velocity;
-	body->set_linear_velocity(v);
-}
-
-void BulletPhysicsServer3D::body_set_axis_lock(RID p_body, BodyAxis p_axis, bool p_lock) {
-	RigidBodyBullet *body = rigid_body_owner.get_or_null(p_body);
-	ERR_FAIL_COND(!body);
-	body->set_axis_lock(p_axis, p_lock);
-}
-
-bool BulletPhysicsServer3D::body_is_axis_locked(RID p_body, BodyAxis p_axis) const {
-	const RigidBodyBullet *body = rigid_body_owner.get_or_null(p_body);
-	ERR_FAIL_COND_V(!body, 0);
-	return body->is_axis_locked(p_axis);
-}
-
-void BulletPhysicsServer3D::body_add_collision_exception(RID p_body, RID p_body_b) {
-	RigidBodyBullet *body = rigid_body_owner.get_or_null(p_body);
-	ERR_FAIL_COND(!body);
-
-	RigidBodyBullet *other_body = rigid_body_owner.get_or_null(p_body_b);
-	ERR_FAIL_COND(!other_body);
-
-	body->add_collision_exception(other_body);
-}
-
-void BulletPhysicsServer3D::body_remove_collision_exception(RID p_body, RID p_body_b) {
-	RigidBodyBullet *body = rigid_body_owner.get_or_null(p_body);
-	ERR_FAIL_COND(!body);
-
-	RigidBodyBullet *other_body = rigid_body_owner.get_or_null(p_body_b);
-	ERR_FAIL_COND(!other_body);
-
-	body->remove_collision_exception(other_body);
-}
-
-void BulletPhysicsServer3D::body_get_collision_exceptions(RID p_body, List<RID> *p_exceptions) {
-	RigidBodyBullet *body = rigid_body_owner.get_or_null(p_body);
-	ERR_FAIL_COND(!body);
-	for (int i = 0; i < body->get_exceptions().size(); i++) {
-		p_exceptions->push_back(body->get_exceptions()[i]);
-	}
-}
-
-void BulletPhysicsServer3D::body_set_max_contacts_reported(RID p_body, int p_contacts) {
-	RigidBodyBullet *body = rigid_body_owner.get_or_null(p_body);
-	ERR_FAIL_COND(!body);
-
-	body->set_max_collisions_detection(p_contacts);
-}
-
-int BulletPhysicsServer3D::body_get_max_contacts_reported(RID p_body) const {
-	RigidBodyBullet *body = rigid_body_owner.get_or_null(p_body);
-	ERR_FAIL_COND_V(!body, 0);
-
-	return body->get_max_collisions_detection();
-}
-
-void BulletPhysicsServer3D::body_set_contacts_reported_depth_threshold(RID p_body, real_t p_threshold) {
-	// Not supported by bullet and even Godot
-}
-
-real_t BulletPhysicsServer3D::body_get_contacts_reported_depth_threshold(RID p_body) const {
-	// Not supported by bullet and even Godot
-	return 0.;
-}
-
-void BulletPhysicsServer3D::body_set_omit_force_integration(RID p_body, bool p_omit) {
-	RigidBodyBullet *body = rigid_body_owner.get_or_null(p_body);
-	ERR_FAIL_COND(!body);
-
-	body->set_omit_forces_integration(p_omit);
-}
-
-bool BulletPhysicsServer3D::body_is_omitting_force_integration(RID p_body) const {
-	RigidBodyBullet *body = rigid_body_owner.get_or_null(p_body);
-	ERR_FAIL_COND_V(!body, false);
-
-	return body->get_omit_forces_integration();
-}
-
-void BulletPhysicsServer3D::body_set_force_integration_callback(RID p_body, const Callable &p_callable, const Variant &p_udata) {
-	RigidBodyBullet *body = rigid_body_owner.get_or_null(p_body);
-	ERR_FAIL_COND(!body);
-	body->set_force_integration_callback(p_callable, p_udata);
-}
-
-void BulletPhysicsServer3D::body_set_ray_pickable(RID p_body, bool p_enable) {
-	RigidBodyBullet *body = rigid_body_owner.get_or_null(p_body);
-	ERR_FAIL_COND(!body);
-	body->set_ray_pickable(p_enable);
-}
-
-PhysicsDirectBodyState3D *BulletPhysicsServer3D::body_get_direct_state(RID p_body) {
-	if (!rigid_body_owner.owns(p_body)) {
-		return nullptr;
-	}
-
-	RigidBodyBullet *body = rigid_body_owner.get_or_null(p_body);
-	ERR_FAIL_COND_V(!body, nullptr);
-
-	if (!body->get_space()) {
-		return nullptr;
-	}
-
-	return BulletPhysicsDirectBodyState3D::get_singleton(body);
-}
-
-bool BulletPhysicsServer3D::body_test_motion(RID p_body, const Transform3D &p_from, const Vector3 &p_motion, bool p_infinite_inertia, MotionResult *r_result, bool p_exclude_raycast_shapes, const Set<RID> &p_exclude) {
-	RigidBodyBullet *body = rigid_body_owner.get_or_null(p_body);
-	ERR_FAIL_COND_V(!body, false);
-	ERR_FAIL_COND_V(!body->get_space(), false);
-
-	return body->get_space()->test_body_motion(body, p_from, p_motion, p_infinite_inertia, r_result, p_exclude_raycast_shapes, p_exclude);
-}
-
-int BulletPhysicsServer3D::body_test_ray_separation(RID p_body, const Transform3D &p_transform, bool p_infinite_inertia, Vector3 &r_recover_motion, SeparationResult *r_results, int p_result_max, real_t p_margin) {
-	RigidBodyBullet *body = rigid_body_owner.get_or_null(p_body);
-	ERR_FAIL_COND_V(!body, 0);
-	ERR_FAIL_COND_V(!body->get_space(), 0);
-
-	return body->get_space()->test_ray_separation(body, p_transform, p_infinite_inertia, r_recover_motion, r_results, p_result_max, p_margin);
-}
-
-RID BulletPhysicsServer3D::soft_body_create(bool p_init_sleeping) {
-	SoftBodyBullet *body = bulletnew(SoftBodyBullet);
-	body->set_collision_layer(1);
-	body->set_collision_mask(1);
-	if (p_init_sleeping) {
-		body->set_activation_state(false);
-	}
-	CreateThenReturnRID(soft_body_owner, body);
-}
-
-void BulletPhysicsServer3D::soft_body_update_rendering_server(RID p_body, RenderingServerHandler *p_rendering_server_handler) {
-	SoftBodyBullet *body = soft_body_owner.get_or_null(p_body);
-	ERR_FAIL_COND(!body);
-
-	body->update_rendering_server(p_rendering_server_handler);
-}
-
-void BulletPhysicsServer3D::soft_body_set_space(RID p_body, RID p_space) {
-	SoftBodyBullet *body = soft_body_owner.get_or_null(p_body);
-	ERR_FAIL_COND(!body);
-	SpaceBullet *space = nullptr;
-
-	if (p_space.is_valid()) {
-		space = space_owner.get_or_null(p_space);
-		ERR_FAIL_COND(!space);
-	}
-
-	if (body->get_space() == space) {
-		return; //pointless
-	}
-
-	body->set_space(space);
-}
-
-RID BulletPhysicsServer3D::soft_body_get_space(RID p_body) const {
-	SoftBodyBullet *body = soft_body_owner.get_or_null(p_body);
-	ERR_FAIL_COND_V(!body, RID());
-
-	SpaceBullet *space = body->get_space();
-	if (!space) {
-		return RID();
-	}
-	return space->get_self();
-}
-
-void BulletPhysicsServer3D::soft_body_set_mesh(RID p_body, RID p_mesh) {
-	SoftBodyBullet *body = soft_body_owner.get_or_null(p_body);
-	ERR_FAIL_COND(!body);
-
-	body->set_soft_mesh(p_mesh);
-}
-
-AABB BulletPhysicsServer::soft_body_get_bounds(RID p_body) const {
-	SoftBodyBullet *body = soft_body_owner.get(p_body);
-	ERR_FAIL_COND_V(!body, AABB());
-
-	return body->get_bounds();
-}
-
-void BulletPhysicsServer3D::soft_body_set_collision_layer(RID p_body, uint32_t p_layer) {
-	SoftBodyBullet *body = soft_body_owner.get_or_null(p_body);
-	ERR_FAIL_COND(!body);
-
-	body->set_collision_layer(p_layer);
-}
-
-uint32_t BulletPhysicsServer3D::soft_body_get_collision_layer(RID p_body) const {
-	const SoftBodyBullet *body = soft_body_owner.get_or_null(p_body);
-	ERR_FAIL_COND_V(!body, 0);
-
-	return body->get_collision_layer();
-}
-
-void BulletPhysicsServer3D::soft_body_set_collision_mask(RID p_body, uint32_t p_mask) {
-	SoftBodyBullet *body = soft_body_owner.get_or_null(p_body);
-	ERR_FAIL_COND(!body);
-
-	body->set_collision_mask(p_mask);
-}
-
-uint32_t BulletPhysicsServer3D::soft_body_get_collision_mask(RID p_body) const {
-	const SoftBodyBullet *body = soft_body_owner.get_or_null(p_body);
-	ERR_FAIL_COND_V(!body, 0);
-
-	return body->get_collision_mask();
-}
-
-void BulletPhysicsServer3D::soft_body_add_collision_exception(RID p_body, RID p_body_b) {
-	SoftBodyBullet *body = soft_body_owner.get_or_null(p_body);
-	ERR_FAIL_COND(!body);
-
-	CollisionObjectBullet *other_body = rigid_body_owner.get_or_null(p_body_b);
-	if (!other_body) {
-		other_body = soft_body_owner.get_or_null(p_body_b);
-	}
-	ERR_FAIL_COND(!other_body);
-
-	body->add_collision_exception(other_body);
-}
-
-void BulletPhysicsServer3D::soft_body_remove_collision_exception(RID p_body, RID p_body_b) {
-	SoftBodyBullet *body = soft_body_owner.get_or_null(p_body);
-	ERR_FAIL_COND(!body);
-
-	CollisionObjectBullet *other_body = rigid_body_owner.get_or_null(p_body_b);
-	if (!other_body) {
-		other_body = soft_body_owner.get_or_null(p_body_b);
-	}
-	ERR_FAIL_COND(!other_body);
-
-	body->remove_collision_exception(other_body);
-}
-
-void BulletPhysicsServer3D::soft_body_get_collision_exceptions(RID p_body, List<RID> *p_exceptions) {
-	SoftBodyBullet *body = soft_body_owner.get_or_null(p_body);
-	ERR_FAIL_COND(!body);
-	for (int i = 0; i < body->get_exceptions().size(); i++) {
-		p_exceptions->push_back(body->get_exceptions()[i]);
-	}
-}
-
-void BulletPhysicsServer3D::soft_body_set_state(RID p_body, BodyState p_state, const Variant &p_variant) {
-	// FIXME: Must be implemented.
-	WARN_PRINT("soft_body_state is not implemented yet in Bullet backend.");
-}
-
-Variant BulletPhysicsServer3D::soft_body_get_state(RID p_body, BodyState p_state) const {
-	// FIXME: Must be implemented.
-	WARN_PRINT("soft_body_state is not implemented yet in Bullet backend.");
-	return Variant();
-}
-
-void BulletPhysicsServer3D::soft_body_set_transform(RID p_body, const Transform3D &p_transform) {
-	SoftBodyBullet *body = soft_body_owner.get_or_null(p_body);
-	ERR_FAIL_COND(!body);
-
-	body->set_soft_transform(p_transform);
-}
-
-void BulletPhysicsServer3D::soft_body_set_ray_pickable(RID p_body, bool p_enable) {
-	SoftBodyBullet *body = soft_body_owner.get_or_null(p_body);
-	ERR_FAIL_COND(!body);
-	body->set_ray_pickable(p_enable);
-}
-
-void BulletPhysicsServer3D::soft_body_set_simulation_precision(RID p_body, int p_simulation_precision) {
-	SoftBodyBullet *body = soft_body_owner.get_or_null(p_body);
-	ERR_FAIL_COND(!body);
-	body->set_simulation_precision(p_simulation_precision);
-}
-
-int BulletPhysicsServer3D::soft_body_get_simulation_precision(RID p_body) const {
-	SoftBodyBullet *body = soft_body_owner.get_or_null(p_body);
-	ERR_FAIL_COND_V(!body, 0.f);
-	return body->get_simulation_precision();
-}
-
-void BulletPhysicsServer3D::soft_body_set_total_mass(RID p_body, real_t p_total_mass) {
-	SoftBodyBullet *body = soft_body_owner.get_or_null(p_body);
-	ERR_FAIL_COND(!body);
-	body->set_total_mass(p_total_mass);
-}
-
-real_t BulletPhysicsServer3D::soft_body_get_total_mass(RID p_body) const {
-	SoftBodyBullet *body = soft_body_owner.get_or_null(p_body);
-	ERR_FAIL_COND_V(!body, 0.f);
-	return body->get_total_mass();
-}
-
-void BulletPhysicsServer3D::soft_body_set_linear_stiffness(RID p_body, real_t p_stiffness) const {
-	SoftBodyBullet *body = soft_body_owner.get_or_null(p_body);
-	ERR_FAIL_COND(!body);
-	body->set_linear_stiffness(p_stiffness);
-}
-
-real_t BulletPhysicsServer3D::soft_body_get_linear_stiffness(RID p_body) {
-	SoftBodyBullet *body = soft_body_owner.get_or_null(p_body);
-	ERR_FAIL_COND_V(!body, 0.f);
-	return body->get_linear_stiffness();
-}
-
-void BulletPhysicsServer3D::soft_body_set_pressure_coefficient(RID p_body, real_t p_pressure_coefficient) {
-	SoftBodyBullet *body = soft_body_owner.get_or_null(p_body);
-	ERR_FAIL_COND(!body);
-	body->set_pressure_coefficient(p_pressure_coefficient);
-}
-
-real_t BulletPhysicsServer3D::soft_body_get_pressure_coefficient(RID p_body) const {
-	SoftBodyBullet *body = soft_body_owner.get_or_null(p_body);
-	ERR_FAIL_COND_V(!body, 0.f);
-	return body->get_pressure_coefficient();
-}
-
-void BulletPhysicsServer3D::soft_body_set_damping_coefficient(RID p_body, real_t p_damping_coefficient) {
-	SoftBodyBullet *body = soft_body_owner.get_or_null(p_body);
-	ERR_FAIL_COND(!body);
-	body->set_damping_coefficient(p_damping_coefficient);
-}
-
-real_t BulletPhysicsServer3D::soft_body_get_damping_coefficient(RID p_body) const {
-	SoftBodyBullet *body = soft_body_owner.get_or_null(p_body);
-	ERR_FAIL_COND_V(!body, 0.f);
-	return body->get_damping_coefficient();
-}
-
-void BulletPhysicsServer3D::soft_body_set_drag_coefficient(RID p_body, real_t p_drag_coefficient) {
-	SoftBodyBullet *body = soft_body_owner.get_or_null(p_body);
-	ERR_FAIL_COND(!body);
-	body->set_drag_coefficient(p_drag_coefficient);
-}
-
-real_t BulletPhysicsServer3D::soft_body_get_drag_coefficient(RID p_body) const {
-	SoftBodyBullet *body = soft_body_owner.get_or_null(p_body);
-	ERR_FAIL_COND_V(!body, 0.f);
-	return body->get_drag_coefficient();
-}
-
-void BulletPhysicsServer3D::soft_body_move_point(RID p_body, int p_point_index, const Vector3 &p_global_position) {
-	SoftBodyBullet *body = soft_body_owner.get_or_null(p_body);
-	ERR_FAIL_COND(!body);
-	body->set_node_position(p_point_index, p_global_position);
-}
-
-Vector3 BulletPhysicsServer3D::soft_body_get_point_global_position(RID p_body, int p_point_index) const {
-	SoftBodyBullet *body = soft_body_owner.get_or_null(p_body);
-	ERR_FAIL_COND_V(!body, Vector3(0., 0., 0.));
-	Vector3 pos;
-	body->get_node_position(p_point_index, pos);
-	return pos;
-}
-
-void BulletPhysicsServer3D::soft_body_remove_all_pinned_points(RID p_body) {
-	SoftBodyBullet *body = soft_body_owner.get_or_null(p_body);
-	ERR_FAIL_COND(!body);
-	body->reset_all_node_mass();
-}
-
-void BulletPhysicsServer3D::soft_body_pin_point(RID p_body, int p_point_index, bool p_pin) {
-	SoftBodyBullet *body = soft_body_owner.get_or_null(p_body);
-	ERR_FAIL_COND(!body);
-	body->set_node_mass(p_point_index, p_pin ? 0 : 1);
-}
-
-bool BulletPhysicsServer3D::soft_body_is_point_pinned(RID p_body, int p_point_index) const {
-	SoftBodyBullet *body = soft_body_owner.get_or_null(p_body);
-	ERR_FAIL_COND_V(!body, 0.f);
-	return body->get_node_mass(p_point_index);
-}
-
-PhysicsServer3D::JointType BulletPhysicsServer3D::joint_get_type(RID p_joint) const {
-	JointBullet *joint = joint_owner.get_or_null(p_joint);
-	ERR_FAIL_COND_V(!joint, JOINT_PIN);
-	return joint->get_type();
-}
-
-void BulletPhysicsServer3D::joint_set_solver_priority(RID p_joint, int p_priority) {
-	// Joint priority not supported by bullet
-}
-
-int BulletPhysicsServer3D::joint_get_solver_priority(RID p_joint) const {
-	// Joint priority not supported by bullet
-	return 0;
-}
-
-void BulletPhysicsServer3D::joint_disable_collisions_between_bodies(RID p_joint, const bool p_disable) {
-	JointBullet *joint = joint_owner.get_or_null(p_joint);
-	ERR_FAIL_COND(!joint);
-
-	joint->disable_collisions_between_bodies(p_disable);
-}
-
-bool BulletPhysicsServer3D::joint_is_disabled_collisions_between_bodies(RID p_joint) const {
-	JointBullet *joint(joint_owner.get_or_null(p_joint));
-	ERR_FAIL_COND_V(!joint, false);
-
-	return joint->is_disabled_collisions_between_bodies();
-}
-
-RID BulletPhysicsServer3D::joint_create_pin(RID p_body_A, const Vector3 &p_local_A, RID p_body_B, const Vector3 &p_local_B) {
-	RigidBodyBullet *body_A = rigid_body_owner.get_or_null(p_body_A);
-	ERR_FAIL_COND_V(!body_A, RID());
-
-	JointAssertSpace(body_A, "A", RID());
-
-	RigidBodyBullet *body_B = nullptr;
-	if (p_body_B.is_valid()) {
-		body_B = rigid_body_owner.get_or_null(p_body_B);
-		JointAssertSpace(body_B, "B", RID());
-		JointAssertSameSpace(body_A, body_B, RID());
-	}
-
-	ERR_FAIL_COND_V(body_A == body_B, RID());
-
-	JointBullet *joint = bulletnew(PinJointBullet(body_A, p_local_A, body_B, p_local_B));
-	AddJointToSpace(body_A, joint);
-
-	CreateThenReturnRID(joint_owner, joint);
-}
-
-void BulletPhysicsServer3D::pin_joint_set_param(RID p_joint, PinJointParam p_param, real_t p_value) {
-	JointBullet *joint = joint_owner.get_or_null(p_joint);
-	ERR_FAIL_COND(!joint);
-	ERR_FAIL_COND(joint->get_type() != JOINT_PIN);
-	PinJointBullet *pin_joint = static_cast<PinJointBullet *>(joint);
-	pin_joint->set_param(p_param, p_value);
-}
-
-real_t BulletPhysicsServer3D::pin_joint_get_param(RID p_joint, PinJointParam p_param) const {
-	JointBullet *joint = joint_owner.get_or_null(p_joint);
-	ERR_FAIL_COND_V(!joint, 0);
-	ERR_FAIL_COND_V(joint->get_type() != JOINT_PIN, 0);
-	PinJointBullet *pin_joint = static_cast<PinJointBullet *>(joint);
-	return pin_joint->get_param(p_param);
-}
-
-void BulletPhysicsServer3D::pin_joint_set_local_a(RID p_joint, const Vector3 &p_A) {
-	JointBullet *joint = joint_owner.get_or_null(p_joint);
-	ERR_FAIL_COND(!joint);
-	ERR_FAIL_COND(joint->get_type() != JOINT_PIN);
-	PinJointBullet *pin_joint = static_cast<PinJointBullet *>(joint);
-	pin_joint->setPivotInA(p_A);
-}
-
-Vector3 BulletPhysicsServer3D::pin_joint_get_local_a(RID p_joint) const {
-	JointBullet *joint = joint_owner.get_or_null(p_joint);
-	ERR_FAIL_COND_V(!joint, Vector3());
-	ERR_FAIL_COND_V(joint->get_type() != JOINT_PIN, Vector3());
-	PinJointBullet *pin_joint = static_cast<PinJointBullet *>(joint);
-	return pin_joint->getPivotInA();
-}
-
-void BulletPhysicsServer3D::pin_joint_set_local_b(RID p_joint, const Vector3 &p_B) {
-	JointBullet *joint = joint_owner.get_or_null(p_joint);
-	ERR_FAIL_COND(!joint);
-	ERR_FAIL_COND(joint->get_type() != JOINT_PIN);
-	PinJointBullet *pin_joint = static_cast<PinJointBullet *>(joint);
-	pin_joint->setPivotInB(p_B);
-}
-
-Vector3 BulletPhysicsServer3D::pin_joint_get_local_b(RID p_joint) const {
-	JointBullet *joint = joint_owner.get_or_null(p_joint);
-	ERR_FAIL_COND_V(!joint, Vector3());
-	ERR_FAIL_COND_V(joint->get_type() != JOINT_PIN, Vector3());
-	PinJointBullet *pin_joint = static_cast<PinJointBullet *>(joint);
-	return pin_joint->getPivotInB();
-}
-
-RID BulletPhysicsServer3D::joint_create_hinge(RID p_body_A, const Transform3D &p_hinge_A, RID p_body_B, const Transform3D &p_hinge_B) {
-	RigidBodyBullet *body_A = rigid_body_owner.get_or_null(p_body_A);
-	ERR_FAIL_COND_V(!body_A, RID());
-	JointAssertSpace(body_A, "A", RID());
-
-	RigidBodyBullet *body_B = nullptr;
-	if (p_body_B.is_valid()) {
-		body_B = rigid_body_owner.get_or_null(p_body_B);
-		JointAssertSpace(body_B, "B", RID());
-		JointAssertSameSpace(body_A, body_B, RID());
-	}
-
-	ERR_FAIL_COND_V(body_A == body_B, RID());
-
-	JointBullet *joint = bulletnew(HingeJointBullet(body_A, body_B, p_hinge_A, p_hinge_B));
-	AddJointToSpace(body_A, joint);
-
-	CreateThenReturnRID(joint_owner, joint);
-}
-
-RID BulletPhysicsServer3D::joint_create_hinge_simple(RID p_body_A, const Vector3 &p_pivot_A, const Vector3 &p_axis_A, RID p_body_B, const Vector3 &p_pivot_B, const Vector3 &p_axis_B) {
-	RigidBodyBullet *body_A = rigid_body_owner.get_or_null(p_body_A);
-	ERR_FAIL_COND_V(!body_A, RID());
-	JointAssertSpace(body_A, "A", RID());
-
-	RigidBodyBullet *body_B = nullptr;
-	if (p_body_B.is_valid()) {
-		body_B = rigid_body_owner.get_or_null(p_body_B);
-		JointAssertSpace(body_B, "B", RID());
-		JointAssertSameSpace(body_A, body_B, RID());
-	}
-
-	ERR_FAIL_COND_V(body_A == body_B, RID());
-
-	JointBullet *joint = bulletnew(HingeJointBullet(body_A, body_B, p_pivot_A, p_pivot_B, p_axis_A, p_axis_B));
-	AddJointToSpace(body_A, joint);
-
-	CreateThenReturnRID(joint_owner, joint);
-}
-
-void BulletPhysicsServer3D::hinge_joint_set_param(RID p_joint, HingeJointParam p_param, real_t p_value) {
-	JointBullet *joint = joint_owner.get_or_null(p_joint);
-	ERR_FAIL_COND(!joint);
-	ERR_FAIL_COND(joint->get_type() != JOINT_HINGE);
-	HingeJointBullet *hinge_joint = static_cast<HingeJointBullet *>(joint);
-	hinge_joint->set_param(p_param, p_value);
-}
-
-real_t BulletPhysicsServer3D::hinge_joint_get_param(RID p_joint, HingeJointParam p_param) const {
-	JointBullet *joint = joint_owner.get_or_null(p_joint);
-	ERR_FAIL_COND_V(!joint, 0);
-	ERR_FAIL_COND_V(joint->get_type() != JOINT_HINGE, 0);
-	HingeJointBullet *hinge_joint = static_cast<HingeJointBullet *>(joint);
-	return hinge_joint->get_param(p_param);
-}
-
-void BulletPhysicsServer3D::hinge_joint_set_flag(RID p_joint, HingeJointFlag p_flag, bool p_value) {
-	JointBullet *joint = joint_owner.get_or_null(p_joint);
-	ERR_FAIL_COND(!joint);
-	ERR_FAIL_COND(joint->get_type() != JOINT_HINGE);
-	HingeJointBullet *hinge_joint = static_cast<HingeJointBullet *>(joint);
-	hinge_joint->set_flag(p_flag, p_value);
-}
-
-bool BulletPhysicsServer3D::hinge_joint_get_flag(RID p_joint, HingeJointFlag p_flag) const {
-	JointBullet *joint = joint_owner.get_or_null(p_joint);
-	ERR_FAIL_COND_V(!joint, false);
-	ERR_FAIL_COND_V(joint->get_type() != JOINT_HINGE, false);
-	HingeJointBullet *hinge_joint = static_cast<HingeJointBullet *>(joint);
-	return hinge_joint->get_flag(p_flag);
-}
-
-RID BulletPhysicsServer3D::joint_create_slider(RID p_body_A, const Transform3D &p_local_frame_A, RID p_body_B, const Transform3D &p_local_frame_B) {
-	RigidBodyBullet *body_A = rigid_body_owner.get_or_null(p_body_A);
-	ERR_FAIL_COND_V(!body_A, RID());
-	JointAssertSpace(body_A, "A", RID());
-
-	RigidBodyBullet *body_B = nullptr;
-	if (p_body_B.is_valid()) {
-		body_B = rigid_body_owner.get_or_null(p_body_B);
-		JointAssertSpace(body_B, "B", RID());
-		JointAssertSameSpace(body_A, body_B, RID());
-	}
-
-	ERR_FAIL_COND_V(body_A == body_B, RID());
-
-	JointBullet *joint = bulletnew(SliderJointBullet(body_A, body_B, p_local_frame_A, p_local_frame_B));
-	AddJointToSpace(body_A, joint);
-
-	CreateThenReturnRID(joint_owner, joint);
-}
-
-void BulletPhysicsServer3D::slider_joint_set_param(RID p_joint, SliderJointParam p_param, real_t p_value) {
-	JointBullet *joint = joint_owner.get_or_null(p_joint);
-	ERR_FAIL_COND(!joint);
-	ERR_FAIL_COND(joint->get_type() != JOINT_SLIDER);
-	SliderJointBullet *slider_joint = static_cast<SliderJointBullet *>(joint);
-	slider_joint->set_param(p_param, p_value);
-}
-
-real_t BulletPhysicsServer3D::slider_joint_get_param(RID p_joint, SliderJointParam p_param) const {
-	JointBullet *joint = joint_owner.get_or_null(p_joint);
-	ERR_FAIL_COND_V(!joint, 0);
-	ERR_FAIL_COND_V(joint->get_type() != JOINT_SLIDER, 0);
-	SliderJointBullet *slider_joint = static_cast<SliderJointBullet *>(joint);
-	return slider_joint->get_param(p_param);
-}
-
-RID BulletPhysicsServer3D::joint_create_cone_twist(RID p_body_A, const Transform3D &p_local_frame_A, RID p_body_B, const Transform3D &p_local_frame_B) {
-	RigidBodyBullet *body_A = rigid_body_owner.get_or_null(p_body_A);
-	ERR_FAIL_COND_V(!body_A, RID());
-	JointAssertSpace(body_A, "A", RID());
-
-	RigidBodyBullet *body_B = nullptr;
-	if (p_body_B.is_valid()) {
-		body_B = rigid_body_owner.get_or_null(p_body_B);
-		JointAssertSpace(body_B, "B", RID());
-		JointAssertSameSpace(body_A, body_B, RID());
-	}
-
-	JointBullet *joint = bulletnew(ConeTwistJointBullet(body_A, body_B, p_local_frame_A, p_local_frame_B));
-	AddJointToSpace(body_A, joint);
-
-	CreateThenReturnRID(joint_owner, joint);
-}
-
-void BulletPhysicsServer3D::cone_twist_joint_set_param(RID p_joint, ConeTwistJointParam p_param, real_t p_value) {
-	JointBullet *joint = joint_owner.get_or_null(p_joint);
-	ERR_FAIL_COND(!joint);
-	ERR_FAIL_COND(joint->get_type() != JOINT_CONE_TWIST);
-	ConeTwistJointBullet *coneTwist_joint = static_cast<ConeTwistJointBullet *>(joint);
-	coneTwist_joint->set_param(p_param, p_value);
-}
-
-real_t BulletPhysicsServer3D::cone_twist_joint_get_param(RID p_joint, ConeTwistJointParam p_param) const {
-	JointBullet *joint = joint_owner.get_or_null(p_joint);
-	ERR_FAIL_COND_V(!joint, 0.);
-	ERR_FAIL_COND_V(joint->get_type() != JOINT_CONE_TWIST, 0.);
-	ConeTwistJointBullet *coneTwist_joint = static_cast<ConeTwistJointBullet *>(joint);
-	return coneTwist_joint->get_param(p_param);
-}
-
-RID BulletPhysicsServer3D::joint_create_generic_6dof(RID p_body_A, const Transform3D &p_local_frame_A, RID p_body_B, const Transform3D &p_local_frame_B) {
-	RigidBodyBullet *body_A = rigid_body_owner.get_or_null(p_body_A);
-	ERR_FAIL_COND_V(!body_A, RID());
-	JointAssertSpace(body_A, "A", RID());
-
-	RigidBodyBullet *body_B = nullptr;
-	if (p_body_B.is_valid()) {
-		body_B = rigid_body_owner.get_or_null(p_body_B);
-		JointAssertSpace(body_B, "B", RID());
-		JointAssertSameSpace(body_A, body_B, RID());
-	}
-
-	ERR_FAIL_COND_V(body_A == body_B, RID());
-
-	JointBullet *joint = bulletnew(Generic6DOFJointBullet(body_A, body_B, p_local_frame_A, p_local_frame_B));
-	AddJointToSpace(body_A, joint);
-
-	CreateThenReturnRID(joint_owner, joint);
-}
-
-void BulletPhysicsServer3D::generic_6dof_joint_set_param(RID p_joint, Vector3::Axis p_axis, G6DOFJointAxisParam p_param, real_t p_value) {
-	JointBullet *joint = joint_owner.get_or_null(p_joint);
-	ERR_FAIL_COND(!joint);
-	ERR_FAIL_COND(joint->get_type() != JOINT_6DOF);
-	Generic6DOFJointBullet *generic_6dof_joint = static_cast<Generic6DOFJointBullet *>(joint);
-	generic_6dof_joint->set_param(p_axis, p_param, p_value);
-}
-
-real_t BulletPhysicsServer3D::generic_6dof_joint_get_param(RID p_joint, Vector3::Axis p_axis, G6DOFJointAxisParam p_param) {
-	JointBullet *joint = joint_owner.get_or_null(p_joint);
-	ERR_FAIL_COND_V(!joint, 0);
-	ERR_FAIL_COND_V(joint->get_type() != JOINT_6DOF, 0);
-	Generic6DOFJointBullet *generic_6dof_joint = static_cast<Generic6DOFJointBullet *>(joint);
-	return generic_6dof_joint->get_param(p_axis, p_param);
-}
-
-void BulletPhysicsServer3D::generic_6dof_joint_set_flag(RID p_joint, Vector3::Axis p_axis, G6DOFJointAxisFlag p_flag, bool p_enable) {
-	JointBullet *joint = joint_owner.get_or_null(p_joint);
-	ERR_FAIL_COND(!joint);
-	ERR_FAIL_COND(joint->get_type() != JOINT_6DOF);
-	Generic6DOFJointBullet *generic_6dof_joint = static_cast<Generic6DOFJointBullet *>(joint);
-	generic_6dof_joint->set_flag(p_axis, p_flag, p_enable);
-}
-
-bool BulletPhysicsServer3D::generic_6dof_joint_get_flag(RID p_joint, Vector3::Axis p_axis, G6DOFJointAxisFlag p_flag) {
-	JointBullet *joint = joint_owner.get_or_null(p_joint);
-	ERR_FAIL_COND_V(!joint, false);
-	ERR_FAIL_COND_V(joint->get_type() != JOINT_6DOF, false);
-	Generic6DOFJointBullet *generic_6dof_joint = static_cast<Generic6DOFJointBullet *>(joint);
-	return generic_6dof_joint->get_flag(p_axis, p_flag);
-}
-
-void BulletPhysicsServer3D::free(RID p_rid) {
-	if (shape_owner.owns(p_rid)) {
-		ShapeBullet *shape = shape_owner.get_or_null(p_rid);
-
-		// Notify the shape is configured
-		for (const KeyValue<ShapeOwnerBullet *, int> &element : shape->get_owners()) {
-			static_cast<ShapeOwnerBullet *>(element.key)->remove_shape_full(shape);
-		}
-
-		shape_owner.free(p_rid);
-		bulletdelete(shape);
-	} else if (rigid_body_owner.owns(p_rid)) {
-		RigidBodyBullet *body = rigid_body_owner.get_or_null(p_rid);
-
-		body->set_space(nullptr);
-
-		body->remove_all_shapes(true, true);
-
-		rigid_body_owner.free(p_rid);
-		bulletdelete(body);
-
-	} else if (soft_body_owner.owns(p_rid)) {
-		SoftBodyBullet *body = soft_body_owner.get_or_null(p_rid);
-
-		body->set_space(nullptr);
-
-		soft_body_owner.free(p_rid);
-		bulletdelete(body);
-
-	} else if (area_owner.owns(p_rid)) {
-		AreaBullet *area = area_owner.get_or_null(p_rid);
-
-		area->set_space(nullptr);
-
-		area->remove_all_shapes(true, true);
-
-		area_owner.free(p_rid);
-		bulletdelete(area);
-
-	} else if (joint_owner.owns(p_rid)) {
-		JointBullet *joint = joint_owner.get_or_null(p_rid);
-		joint->destroy_internal_constraint();
-		joint_owner.free(p_rid);
-		bulletdelete(joint);
-
-	} else if (space_owner.owns(p_rid)) {
-		SpaceBullet *space = space_owner.get_or_null(p_rid);
-
-		space->remove_all_collision_objects();
-
-		space_set_active(p_rid, false);
-		space_owner.free(p_rid);
-		bulletdelete(space);
-	} else {
-		ERR_FAIL_MSG("Invalid ID.");
-	}
-}
-
-void BulletPhysicsServer3D::init() {
-	BulletPhysicsDirectBodyState3D::initSingleton();
-}
-
-void BulletPhysicsServer3D::step(real_t p_deltaTime) {
-	if (!active) {
-		return;
-	}
-
-	BulletPhysicsDirectBodyState3D::singleton_setDeltaTime(p_deltaTime);
-
-	for (int i = 0; i < active_spaces_count; ++i) {
-		active_spaces[i]->step(p_deltaTime);
-	}
-}
-
-void BulletPhysicsServer3D::flush_queries() {
-	if (!active) {
-		return;
-	}
-
-	for (int i = 0; i < active_spaces_count; ++i) {
-		active_spaces[i]->flush_queries();
-	}
-}
-
-void BulletPhysicsServer3D::finish() {
-	BulletPhysicsDirectBodyState3D::destroySingleton();
-}
-
-int BulletPhysicsServer3D::get_process_info(ProcessInfo p_info) {
-	return 0;
-}
-
-SpaceBullet *BulletPhysicsServer3D::get_space(RID p_rid) const {
-	ERR_FAIL_COND_V_MSG(space_owner.owns(p_rid) == false, nullptr, "The RID is not valid.");
-	return space_owner.get_or_null(p_rid);
-}
-
-ShapeBullet *BulletPhysicsServer3D::get_shape(RID p_rid) const {
-	ERR_FAIL_COND_V_MSG(shape_owner.owns(p_rid) == false, nullptr, "The RID is not valid.");
-	return shape_owner.get_or_null(p_rid);
-}
-
-CollisionObjectBullet *BulletPhysicsServer3D::get_collision_object(RID p_object) const {
-	if (rigid_body_owner.owns(p_object)) {
-		return rigid_body_owner.get_or_null(p_object);
-	}
-	if (area_owner.owns(p_object)) {
-		return area_owner.get_or_null(p_object);
-	}
-	if (soft_body_owner.owns(p_object)) {
-		return soft_body_owner.get_or_null(p_object);
-	}
-	ERR_FAIL_V_MSG(nullptr, "The RID is no valid.");
-}
-
-RigidCollisionObjectBullet *BulletPhysicsServer3D::get_rigid_collision_object(RID p_object) const {
-	if (rigid_body_owner.owns(p_object)) {
-		return rigid_body_owner.get_or_null(p_object);
-	}
-	if (area_owner.owns(p_object)) {
-		return area_owner.get_or_null(p_object);
-	}
-	ERR_FAIL_V_MSG(nullptr, "The RID is no valid.");
-}
-
-JointBullet *BulletPhysicsServer3D::get_joint(RID p_rid) const {
-	ERR_FAIL_COND_V_MSG(joint_owner.owns(p_rid) == false, nullptr, "The RID is not valid.");
-	return joint_owner.get_or_null(p_rid);
-}

modules/bullet/bullet_physics_server.h

@@ -1,394 +0,0 @@
-/*************************************************************************/
-/*  bullet_physics_server.h                                              */
-/*************************************************************************/
-/*                       This file is part of:                           */
-/*                           GODOT ENGINE                                */
-/*                      https://godotengine.org                          */
-/*************************************************************************/
-/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur.                 */
-/* Copyright (c) 2014-2022 Godot Engine contributors (cf. AUTHORS.md).   */
-/*                                                                       */
-/* Permission is hereby granted, free of charge, to any person obtaining */
-/* a copy of this software and associated documentation files (the       */
-/* "Software"), to deal in the Software without restriction, including   */
-/* without limitation the rights to use, copy, modify, merge, publish,   */
-/* distribute, sublicense, and/or sell copies of the Software, and to    */
-/* permit persons to whom the Software is furnished to do so, subject to */
-/* the following conditions:                                             */
-/*                                                                       */
-/* The above copyright notice and this permission notice shall be        */
-/* included in all copies or substantial portions of the Software.       */
-/*                                                                       */
-/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
-/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
-/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
-/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
-/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION 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 BULLET_PHYSICS_SERVER_H
-#define BULLET_PHYSICS_SERVER_H
-
-#include "area_bullet.h"
-#include "core/templates/rid.h"
-#include "core/templates/rid_owner.h"
-#include "joint_bullet.h"
-#include "rigid_body_bullet.h"
-#include "servers/physics_server_3d.h"
-#include "shape_bullet.h"
-#include "soft_body_bullet.h"
-#include "space_bullet.h"
-
-class BulletPhysicsServer3D : public PhysicsServer3D {
-	GDCLASS(BulletPhysicsServer3D, PhysicsServer3D);
-
-	friend class BulletPhysicsDirectSpaceState;
-
-	bool active = true;
-	char active_spaces_count = 0;
-	Vector<SpaceBullet *> active_spaces;
-
-	mutable RID_PtrOwner<SpaceBullet> space_owner;
-	mutable RID_PtrOwner<ShapeBullet> shape_owner;
-	mutable RID_PtrOwner<AreaBullet> area_owner;
-	mutable RID_PtrOwner<RigidBodyBullet> rigid_body_owner;
-	mutable RID_PtrOwner<SoftBodyBullet> soft_body_owner;
-	mutable RID_PtrOwner<JointBullet> joint_owner;
-
-protected:
-	static void _bind_methods();
-
-public:
-	BulletPhysicsServer3D();
-	~BulletPhysicsServer3D();
-
-	_FORCE_INLINE_ RID_PtrOwner<SpaceBullet> *get_space_owner() {
-		return &space_owner;
-	}
-	_FORCE_INLINE_ RID_PtrOwner<ShapeBullet> *get_shape_owner() {
-		return &shape_owner;
-	}
-	_FORCE_INLINE_ RID_PtrOwner<AreaBullet> *get_area_owner() {
-		return &area_owner;
-	}
-	_FORCE_INLINE_ RID_PtrOwner<RigidBodyBullet> *get_rigid_body_owner() {
-		return &rigid_body_owner;
-	}
-	_FORCE_INLINE_ RID_PtrOwner<SoftBodyBullet> *get_soft_body_owner() {
-		return &soft_body_owner;
-	}
-	_FORCE_INLINE_ RID_PtrOwner<JointBullet> *get_joint_owner() {
-		return &joint_owner;
-	}
-
-	/* SHAPE API */
-	virtual RID shape_create(ShapeType p_shape) override;
-	virtual void shape_set_data(RID p_shape, const Variant &p_data) override;
-	virtual ShapeType shape_get_type(RID p_shape) const override;
-	virtual Variant shape_get_data(RID p_shape) const override;
-
-	virtual void shape_set_margin(RID p_shape, real_t p_margin) override;
-	virtual real_t shape_get_margin(RID p_shape) const override;
-
-	/// Not supported
-	virtual void shape_set_custom_solver_bias(RID p_shape, real_t p_bias) override;
-	/// Not supported
-	virtual real_t shape_get_custom_solver_bias(RID p_shape) const override;
-
-	/* SPACE API */
-
-	virtual RID space_create() override;
-	virtual void space_set_active(RID p_space, bool p_active) override;
-	virtual bool space_is_active(RID p_space) const override;
-
-	/// Not supported
-	virtual void space_set_param(RID p_space, SpaceParameter p_param, real_t p_value) override;
-	/// Not supported
-	virtual real_t space_get_param(RID p_space, SpaceParameter p_param) const override;
-
-	virtual PhysicsDirectSpaceState3D *space_get_direct_state(RID p_space) override;
-
-	virtual void space_set_debug_contacts(RID p_space, int p_max_contacts) override;
-	virtual Vector<Vector3> space_get_contacts(RID p_space) const override;
-	virtual int space_get_contact_count(RID p_space) const override;
-
-	/* AREA API */
-
-	/// Bullet Physics Engine not support "Area", this must be handled by the game developer in another way.
-	/// Since godot Physics use the concept of area even to define the main world, the API area_set_param is used to set initial physics world information.
-	/// The API area_set_param is a bit hacky, and allow Godot to set some parameters on Bullet's world, a different use print a warning to console.
-	/// All other APIs returns a warning message if used
-
-	virtual RID area_create() override;
-
-	virtual void area_set_space(RID p_area, RID p_space) override;
-
-	virtual RID area_get_space(RID p_area) const override;
-
-	virtual void area_set_space_override_mode(RID p_area, AreaSpaceOverrideMode p_mode) override;
-	virtual AreaSpaceOverrideMode area_get_space_override_mode(RID p_area) const override;
-
-	virtual void area_add_shape(RID p_area, RID p_shape, const Transform3D &p_transform = Transform3D(), bool p_disabled = false) override;
-	virtual void area_set_shape(RID p_area, int p_shape_idx, RID p_shape) override;
-	virtual void area_set_shape_transform(RID p_area, int p_shape_idx, const Transform3D &p_transform) override;
-	virtual int area_get_shape_count(RID p_area) const override;
-	virtual RID area_get_shape(RID p_area, int p_shape_idx) const override;
-	virtual Transform3D area_get_shape_transform(RID p_area, int p_shape_idx) const override;
-	virtual void area_remove_shape(RID p_area, int p_shape_idx) override;
-	virtual void area_clear_shapes(RID p_area) override;
-	virtual void area_set_shape_disabled(RID p_area, int p_shape_idx, bool p_disabled) override;
-	virtual void area_attach_object_instance_id(RID p_area, ObjectID p_id) override;
-	virtual ObjectID area_get_object_instance_id(RID p_area) const override;
-
-	/// If you pass as p_area the SpaceBullet you can set some parameters as specified below
-	/// AREA_PARAM_GRAVITY
-	/// AREA_PARAM_GRAVITY_VECTOR
-	/// Otherwise you can set area parameters
-	virtual void area_set_param(RID p_area, AreaParameter p_param, const Variant &p_value) override;
-	virtual Variant area_get_param(RID p_area, AreaParameter p_param) const override;
-
-	virtual void area_set_transform(RID p_area, const Transform3D &p_transform) override;
-	virtual Transform3D area_get_transform(RID p_area) const override;
-
-	virtual void area_set_collision_mask(RID p_area, uint32_t p_mask) override;
-	virtual void area_set_collision_layer(RID p_area, uint32_t p_layer) override;
-
-	virtual void area_set_monitorable(RID p_area, bool p_monitorable) override;
-	virtual void area_set_monitor_callback(RID p_area, const Callable &p_callback) override;
-	virtual void area_set_area_monitor_callback(RID p_area, const Callable &p_callback) override;
-	virtual void area_set_ray_pickable(RID p_area, bool p_enable) override;
-
-	/* RIGID BODY API */
-
-	virtual RID body_create(BodyMode p_mode = BODY_MODE_DYNAMIC, bool p_init_sleeping = false) override;
-
-	virtual void body_set_space(RID p_body, RID p_space) override;
-	virtual RID body_get_space(RID p_body) const override;
-
-	virtual void body_set_mode(RID p_body, BodyMode p_mode) override;
-	virtual BodyMode body_get_mode(RID p_body) const override;
-
-	virtual void body_add_shape(RID p_body, RID p_shape, const Transform3D &p_transform = Transform3D(), bool p_disabled = false) override;
-	// Not supported, Please remove and add new shape
-	virtual void body_set_shape(RID p_body, int p_shape_idx, RID p_shape) override;
-	virtual void body_set_shape_transform(RID p_body, int p_shape_idx, const Transform3D &p_transform) override;
-
-	virtual int body_get_shape_count(RID p_body) const override;
-	virtual RID body_get_shape(RID p_body, int p_shape_idx) const override;
-	virtual Transform3D body_get_shape_transform(RID p_body, int p_shape_idx) const override;
-
-	virtual void body_set_shape_disabled(RID p_body, int p_shape_idx, bool p_disabled) override;
-
-	virtual void body_remove_shape(RID p_body, int p_shape_idx) override;
-	virtual void body_clear_shapes(RID p_body) override;
-
-	// Used for Rigid and Soft Bodies
-	virtual void body_attach_object_instance_id(RID p_body, ObjectID p_id) override;
-	virtual ObjectID body_get_object_instance_id(RID p_body) const override;
-
-	virtual void body_set_enable_continuous_collision_detection(RID p_body, bool p_enable) override;
-	virtual bool body_is_continuous_collision_detection_enabled(RID p_body) const override;
-
-	virtual void body_set_collision_layer(RID p_body, uint32_t p_layer) override;
-	virtual uint32_t body_get_collision_layer(RID p_body) const override;
-
-	virtual void body_set_collision_mask(RID p_body, uint32_t p_mask) override;
-	virtual uint32_t body_get_collision_mask(RID p_body) const override;
-
-	/// This is not supported by physics server
-	virtual void body_set_user_flags(RID p_body, uint32_t p_flags) override;
-	/// This is not supported by physics server
-	virtual uint32_t body_get_user_flags(RID p_body) const override;
-
-	virtual void body_set_param(RID p_body, BodyParameter p_param, real_t p_value) override;
-	virtual real_t body_get_param(RID p_body, BodyParameter p_param) const override;
-
-	virtual void body_set_kinematic_safe_margin(RID p_body, real_t p_margin) override;
-	virtual real_t body_get_kinematic_safe_margin(RID p_body) const override;
-
-	virtual void body_set_state(RID p_body, BodyState p_state, const Variant &p_variant) override;
-	virtual Variant body_get_state(RID p_body, BodyState p_state) const override;
-
-	virtual void body_set_applied_force(RID p_body, const Vector3 &p_force) override;
-	virtual Vector3 body_get_applied_force(RID p_body) const override;
-
-	virtual void body_set_applied_torque(RID p_body, const Vector3 &p_torque) override;
-	virtual Vector3 body_get_applied_torque(RID p_body) const override;
-
-	virtual void body_add_central_force(RID p_body, const Vector3 &p_force) override;
-	virtual void body_add_force(RID p_body, const Vector3 &p_force, const Vector3 &p_position = Vector3()) override;
-	virtual void body_add_torque(RID p_body, const Vector3 &p_torque) override;
-
-	virtual void body_apply_central_impulse(RID p_body, const Vector3 &p_impulse) override;
-	virtual void body_apply_impulse(RID p_body, const Vector3 &p_impulse, const Vector3 &p_position = Vector3()) override;
-	virtual void body_apply_torque_impulse(RID p_body, const Vector3 &p_impulse) override;
-	virtual void body_set_axis_velocity(RID p_body, const Vector3 &p_axis_velocity) override;
-
-	virtual void body_set_axis_lock(RID p_body, BodyAxis p_axis, bool p_lock) override;
-	virtual bool body_is_axis_locked(RID p_body, BodyAxis p_axis) const override;
-
-	virtual void body_add_collision_exception(RID p_body, RID p_body_b) override;
-	virtual void body_remove_collision_exception(RID p_body, RID p_body_b) override;
-	virtual void body_get_collision_exceptions(RID p_body, List<RID> *p_exceptions) override;
-
-	virtual void body_set_max_contacts_reported(RID p_body, int p_contacts) override;
-	virtual int body_get_max_contacts_reported(RID p_body) const override;
-
-	virtual void body_set_contacts_reported_depth_threshold(RID p_body, real_t p_threshold) override;
-	virtual real_t body_get_contacts_reported_depth_threshold(RID p_body) const override;
-
-	virtual void body_set_omit_force_integration(RID p_body, bool p_omit) override;
-	virtual bool body_is_omitting_force_integration(RID p_body) const override;
-
-	virtual void body_set_force_integration_callback(RID p_body, const Callable &p_callable, const Variant &p_udata = Variant()) override;
-
-	virtual void body_set_ray_pickable(RID p_body, bool p_enable) override;
-
-	// this function only works on physics process, errors and returns null otherwise
-	virtual PhysicsDirectBodyState3D *body_get_direct_state(RID p_body) override;
-
-	virtual bool body_test_motion(RID p_body, const Transform3D &p_from, const Vector3 &p_motion, bool p_infinite_inertia, MotionResult *r_result = nullptr, bool p_exclude_raycast_shapes = true, const Set<RID> &p_exclude = Set<RID>()) override;
-	virtual int body_test_ray_separation(RID p_body, const Transform3D &p_transform, bool p_infinite_inertia, Vector3 &r_recover_motion, SeparationResult *r_results, int p_result_max, real_t p_margin = 0.001) override;
-
-	/* SOFT BODY API */
-
-	virtual RID soft_body_create(bool p_init_sleeping = false) override;
-
-	virtual void soft_body_update_rendering_server(RID p_body, RenderingServerHandler *p_rendering_server_handler) override;
-
-	virtual void soft_body_set_space(RID p_body, RID p_space) override;
-	virtual RID soft_body_get_space(RID p_body) const override;
-
-	virtual void soft_body_set_mesh(RID p_body, RID p_mesh) override;
-
-	virtual AABB soft_body_get_bounds(RID p_body) const override;
-
-	virtual void soft_body_set_collision_layer(RID p_body, uint32_t p_layer) override;
-	virtual uint32_t soft_body_get_collision_layer(RID p_body) const override;
-
-	virtual void soft_body_set_collision_mask(RID p_body, uint32_t p_mask) override;
-	virtual uint32_t soft_body_get_collision_mask(RID p_body) const override;
-
-	virtual void soft_body_add_collision_exception(RID p_body, RID p_body_b) override;
-	virtual void soft_body_remove_collision_exception(RID p_body, RID p_body_b) override;
-	virtual void soft_body_get_collision_exceptions(RID p_body, List<RID> *p_exceptions) override;
-
-	virtual void soft_body_set_state(RID p_body, BodyState p_state, const Variant &p_variant) override;
-	virtual Variant soft_body_get_state(RID p_body, BodyState p_state) const override;
-
-	/// Special function. This function has bad performance
-	virtual void soft_body_set_transform(RID p_body, const Transform3D &p_transform) override;
-
-	virtual void soft_body_set_ray_pickable(RID p_body, bool p_enable) override;
-
-	virtual void soft_body_set_simulation_precision(RID p_body, int p_simulation_precision) override;
-	virtual int soft_body_get_simulation_precision(RID p_body) const override;
-
-	virtual void soft_body_set_total_mass(RID p_body, real_t p_total_mass) override;
-	virtual real_t soft_body_get_total_mass(RID p_body) const override;
-
-	virtual void soft_body_set_linear_stiffness(RID p_body, real_t p_stiffness) override;
-	virtual real_t soft_body_get_linear_stiffness(RID p_body) const override;
-
-	virtual void soft_body_set_pressure_coefficient(RID p_body, real_t p_pressure_coefficient) override;
-	virtual real_t soft_body_get_pressure_coefficient(RID p_body) const override;
-
-	virtual void soft_body_set_damping_coefficient(RID p_body, real_t p_damping_coefficient) override;
-	virtual real_t soft_body_get_damping_coefficient(RID p_body) const override;
-
-	virtual void soft_body_set_drag_coefficient(RID p_body, real_t p_drag_coefficient) override;
-	virtual real_t soft_body_get_drag_coefficient(RID p_body) const override;
-
-	virtual void soft_body_move_point(RID p_body, int p_point_index, const Vector3 &p_global_position) override;
-	virtual Vector3 soft_body_get_point_global_position(RID p_body, int p_point_index) const override;
-
-	virtual void soft_body_remove_all_pinned_points(RID p_body) override;
-	virtual void soft_body_pin_point(RID p_body, int p_point_index, bool p_pin) override;
-	virtual bool soft_body_is_point_pinned(RID p_body, int p_point_index) const override;
-
-	/* JOINT API */
-
-	virtual JointType joint_get_type(RID p_joint) const override;
-
-	virtual void joint_set_solver_priority(RID p_joint, int p_priority) override;
-	virtual int joint_get_solver_priority(RID p_joint) const override;
-
-	virtual void joint_disable_collisions_between_bodies(RID p_joint, const bool p_disable) override;
-	virtual bool joint_is_disabled_collisions_between_bodies(RID p_joint) const override;
-
-	virtual RID joint_create_pin(RID p_body_A, const Vector3 &p_local_A, RID p_body_B, const Vector3 &p_local_B) override;
-
-	virtual void pin_joint_set_param(RID p_joint, PinJointParam p_param, real_t p_value) override;
-	virtual real_t pin_joint_get_param(RID p_joint, PinJointParam p_param) const override;
-
-	virtual void pin_joint_set_local_a(RID p_joint, const Vector3 &p_A) override;
-	virtual Vector3 pin_joint_get_local_a(RID p_joint) const override;
-
-	virtual void pin_joint_set_local_b(RID p_joint, const Vector3 &p_B) override;
-	virtual Vector3 pin_joint_get_local_b(RID p_joint) const override;
-
-	virtual RID joint_create_hinge(RID p_body_A, const Transform3D &p_hinge_A, RID p_body_B, const Transform3D &p_hinge_B) override;
-	virtual RID joint_create_hinge_simple(RID p_body_A, const Vector3 &p_pivot_A, const Vector3 &p_axis_A, RID p_body_B, const Vector3 &p_pivot_B, const Vector3 &p_axis_B) override;
-
-	virtual void hinge_joint_set_param(RID p_joint, HingeJointParam p_param, real_t p_value) override;
-	virtual real_t hinge_joint_get_param(RID p_joint, HingeJointParam p_param) const override;
-
-	virtual void hinge_joint_set_flag(RID p_joint, HingeJointFlag p_flag, bool p_value) override;
-	virtual bool hinge_joint_get_flag(RID p_joint, HingeJointFlag p_flag) const override;
-
-	/// Reference frame is A
-	virtual RID joint_create_slider(RID p_body_A, const Transform3D &p_local_frame_A, RID p_body_B, const Transform3D &p_local_frame_B) override;
-
-	virtual void slider_joint_set_param(RID p_joint, SliderJointParam p_param, real_t p_value) override;
-	virtual real_t slider_joint_get_param(RID p_joint, SliderJointParam p_param) const override;
-
-	/// Reference frame is A
-	virtual RID joint_create_cone_twist(RID p_body_A, const Transform3D &p_local_frame_A, RID p_body_B, const Transform3D &p_local_frame_B) override;
-
-	virtual void cone_twist_joint_set_param(RID p_joint, ConeTwistJointParam p_param, real_t p_value) override;
-	virtual real_t cone_twist_joint_get_param(RID p_joint, ConeTwistJointParam p_param) const override;
-
-	/// Reference frame is A
-	virtual RID joint_create_generic_6dof(RID p_body_A, const Transform3D &p_local_frame_A, RID p_body_B, const Transform3D &p_local_frame_B) override;
-
-	virtual void generic_6dof_joint_set_param(RID p_joint, Vector3::Axis p_axis, G6DOFJointAxisParam p_param, real_t p_value) override;
-	virtual real_t generic_6dof_joint_get_param(RID p_joint, Vector3::Axis p_axis, G6DOFJointAxisParam p_param) override;
-
-	virtual void generic_6dof_joint_set_flag(RID p_joint, Vector3::Axis p_axis, G6DOFJointAxisFlag p_flag, bool p_enable) override;
-	virtual bool generic_6dof_joint_get_flag(RID p_joint, Vector3::Axis p_axis, G6DOFJointAxisFlag p_flag) override;
-
-	/* MISC */
-
-	virtual void free(RID p_rid) override;
-
-	virtual void set_active(bool p_active) override {
-		active = p_active;
-	}
-
-	static bool singleton_isActive() {
-		return static_cast<BulletPhysicsServer3D *>(get_singleton())->active;
-	}
-
-	bool isActive() {
-		return active;
-	}
-
-	virtual void init() override;
-	virtual void step(real_t p_deltaTime) override;
-	virtual void flush_queries() override;
-	virtual void finish() override;
-
-	virtual bool is_flushing_queries() const override { return false; }
-
-	virtual int get_process_info(ProcessInfo p_info) override;
-
-	SpaceBullet *get_space(RID p_rid) const;
-	ShapeBullet *get_shape(RID p_rid) const;
-	CollisionObjectBullet *get_collision_object(RID p_object) const;
-	RigidCollisionObjectBullet *get_rigid_collision_object(RID p_object) const;
-	JointBullet *get_joint(RID p_rid) const;
-};
-
-#endif // BULLET_PHYSICS_SERVER_H

modules/bullet/bullet_types_converter.cpp

@@ -1,151 +0,0 @@
-/*************************************************************************/
-/*  bullet_types_converter.cpp                                           */
-/*************************************************************************/
-/*                       This file is part of:                           */
-/*                           GODOT ENGINE                                */
-/*                      https://godotengine.org                          */
-/*************************************************************************/
-/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur.                 */
-/* Copyright (c) 2014-2022 Godot Engine contributors (cf. AUTHORS.md).   */
-/*                                                                       */
-/* Permission is hereby granted, free of charge, to any person obtaining */
-/* a copy of this software and associated documentation files (the       */
-/* "Software"), to deal in the Software without restriction, including   */
-/* without limitation the rights to use, copy, modify, merge, publish,   */
-/* distribute, sublicense, and/or sell copies of the Software, and to    */
-/* permit persons to whom the Software is furnished to do so, subject to */
-/* the following conditions:                                             */
-/*                                                                       */
-/* The above copyright notice and this permission notice shall be        */
-/* included in all copies or substantial portions of the Software.       */
-/*                                                                       */
-/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
-/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
-/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
-/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
-/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION 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 "bullet_types_converter.h"
-
-// ++ BULLET to GODOT ++++++++++
-void B_TO_G(btVector3 const &inVal, Vector3 &outVal) {
-	outVal[0] = inVal[0];
-	outVal[1] = inVal[1];
-	outVal[2] = inVal[2];
-}
-
-void INVERT_B_TO_G(btVector3 const &inVal, Vector3 &outVal) {
-	outVal[0] = inVal[0] != 0. ? 1. / inVal[0] : 0.;
-	outVal[1] = inVal[1] != 0. ? 1. / inVal[1] : 0.;
-	outVal[2] = inVal[2] != 0. ? 1. / inVal[2] : 0.;
-}
-
-void B_TO_G(btMatrix3x3 const &inVal, Basis &outVal) {
-	B_TO_G(inVal[0], outVal[0]);
-	B_TO_G(inVal[1], outVal[1]);
-	B_TO_G(inVal[2], outVal[2]);
-}
-
-void INVERT_B_TO_G(btMatrix3x3 const &inVal, Basis &outVal) {
-	INVERT_B_TO_G(inVal[0], outVal[0]);
-	INVERT_B_TO_G(inVal[1], outVal[1]);
-	INVERT_B_TO_G(inVal[2], outVal[2]);
-}
-
-void B_TO_G(btTransform const &inVal, Transform3D &outVal) {
-	B_TO_G(inVal.getBasis(), outVal.basis);
-	B_TO_G(inVal.getOrigin(), outVal.origin);
-}
-
-// ++ GODOT to BULLET ++++++++++
-void G_TO_B(Vector3 const &inVal, btVector3 &outVal) {
-	outVal[0] = inVal[0];
-	outVal[1] = inVal[1];
-	outVal[2] = inVal[2];
-}
-
-void INVERT_G_TO_B(Vector3 const &inVal, btVector3 &outVal) {
-	outVal[0] = inVal[0] != 0. ? 1. / inVal[0] : 0.;
-	outVal[1] = inVal[1] != 0. ? 1. / inVal[1] : 0.;
-	outVal[2] = inVal[2] != 0. ? 1. / inVal[2] : 0.;
-}
-
-void G_TO_B(Basis const &inVal, btMatrix3x3 &outVal) {
-	G_TO_B(inVal[0], outVal[0]);
-	G_TO_B(inVal[1], outVal[1]);
-	G_TO_B(inVal[2], outVal[2]);
-}
-
-void INVERT_G_TO_B(Basis const &inVal, btMatrix3x3 &outVal) {
-	INVERT_G_TO_B(inVal[0], outVal[0]);
-	INVERT_G_TO_B(inVal[1], outVal[1]);
-	INVERT_G_TO_B(inVal[2], outVal[2]);
-}
-
-void G_TO_B(Transform3D const &inVal, btTransform &outVal) {
-	G_TO_B(inVal.basis, outVal.getBasis());
-	G_TO_B(inVal.origin, outVal.getOrigin());
-}
-
-void UNSCALE_BT_BASIS(btTransform &scaledBasis) {
-	btMatrix3x3 &basis(scaledBasis.getBasis());
-	btVector3 column0 = basis.getColumn(0);
-	btVector3 column1 = basis.getColumn(1);
-	btVector3 column2 = basis.getColumn(2);
-
-	// Check for zero scaling.
-	if (column0.fuzzyZero()) {
-		if (column1.fuzzyZero()) {
-			if (column2.fuzzyZero()) {
-				// All dimensions are fuzzy zero. Create a default basis.
-				column0 = btVector3(1, 0, 0);
-				column1 = btVector3(0, 1, 0);
-				column2 = btVector3(0, 0, 1);
-			} else { // Column 2 scale not fuzzy zero.
-				// Create two vectors orthogonal to row 2.
-				// Ensure that a default basis is created if row 2 = <0, 0, 1>
-				column1 = btVector3(0, column2[2], -column2[1]);
-				column0 = column1.cross(column2);
-			}
-		} else { // Column 1 scale not fuzzy zero.
-			if (column2.fuzzyZero()) {
-				// Create two vectors orthogonal to column 1.
-				// Ensure that a default basis is created if column 1 = <0, 1, 0>
-				column0 = btVector3(column1[1], -column1[0], 0);
-				column2 = column0.cross(column1);
-			} else { // Column 1 and column 2 scales not fuzzy zero.
-				// Create column 0 orthogonal to column 1 and column 2.
-				column0 = column1.cross(column2);
-			}
-		}
-	} else { // Column 0 scale not fuzzy zero.
-		if (column1.fuzzyZero()) {
-			if (column2.fuzzyZero()) {
-				// Create two vectors orthogonal to column 0.
-				// Ensure that a default basis is created if column 0 = <1, 0, 0>
-				column2 = btVector3(-column0[2], 0, column0[0]);
-				column1 = column2.cross(column0);
-			} else { // Column 0 and column 2 scales not fuzzy zero.
-				// Create column 1 orthogonal to column 0 and column 2.
-				column1 = column2.cross(column0);
-			}
-		} else { // Column 0 and column 1 scales not fuzzy zero.
-			if (column2.fuzzyZero()) {
-				// Create column 2 orthogonal to column 0 and column 1.
-				column2 = column0.cross(column1);
-			}
-		}
-	}
-
-	// Normalize
-	column0.normalize();
-	column1.normalize();
-	column2.normalize();
-
-	basis.setValue(column0[0], column1[0], column2[0],
-			column0[1], column1[1], column2[1],
-			column0[2], column1[2], column2[2]);
-}

modules/bullet/bullet_types_converter.h

@@ -1,59 +0,0 @@
-/*************************************************************************/
-/*  bullet_types_converter.h                                             */
-/*************************************************************************/
-/*                       This file is part of:                           */
-/*                           GODOT ENGINE                                */
-/*                      https://godotengine.org                          */
-/*************************************************************************/
-/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur.                 */
-/* Copyright (c) 2014-2022 Godot Engine contributors (cf. AUTHORS.md).   */
-/*                                                                       */
-/* Permission is hereby granted, free of charge, to any person obtaining */
-/* a copy of this software and associated documentation files (the       */
-/* "Software"), to deal in the Software without restriction, including   */
-/* without limitation the rights to use, copy, modify, merge, publish,   */
-/* distribute, sublicense, and/or sell copies of the Software, and to    */
-/* permit persons to whom the Software is furnished to do so, subject to */
-/* the following conditions:                                             */
-/*                                                                       */
-/* The above copyright notice and this permission notice shall be        */
-/* included in all copies or substantial portions of the Software.       */
-/*                                                                       */
-/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
-/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
-/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
-/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
-/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION 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 BULLET_TYPES_CONVERTER_H
-#define BULLET_TYPES_CONVERTER_H
-
-#include "core/math/basis.h"
-#include "core/math/transform_3d.h"
-#include "core/math/vector3.h"
-#include "core/typedefs.h"
-
-#include <LinearMath/btMatrix3x3.h>
-#include <LinearMath/btTransform.h>
-#include <LinearMath/btVector3.h>
-
-// Bullet to Godot
-extern void B_TO_G(btVector3 const &inVal, Vector3 &outVal);
-extern void INVERT_B_TO_G(btVector3 const &inVal, Vector3 &outVal);
-extern void B_TO_G(btMatrix3x3 const &inVal, Basis &outVal);
-extern void INVERT_B_TO_G(btMatrix3x3 const &inVal, Basis &outVal);
-extern void B_TO_G(btTransform const &inVal, Transform3D &outVal);
-
-// Godot TO Bullet
-extern void G_TO_B(Vector3 const &inVal, btVector3 &outVal);
-extern void INVERT_G_TO_B(Vector3 const &inVal, btVector3 &outVal);
-extern void G_TO_B(Basis const &inVal, btMatrix3x3 &outVal);
-extern void INVERT_G_TO_B(Basis const &inVal, btMatrix3x3 &outVal);
-extern void G_TO_B(Transform3D const &inVal, btTransform &outVal);
-
-extern void UNSCALE_BT_BASIS(btTransform &scaledBasis);
-
-#endif // BULLET_TYPES_CONVERTER_H

modules/bullet/bullet_utilities.h

@@ -1,43 +0,0 @@
-/*************************************************************************/
-/*  bullet_utilities.h                                                   */
-/*************************************************************************/
-/*                       This file is part of:                           */
-/*                           GODOT ENGINE                                */
-/*                      https://godotengine.org                          */
-/*************************************************************************/
-/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur.                 */
-/* Copyright (c) 2014-2022 Godot Engine contributors (cf. AUTHORS.md).   */
-/*                                                                       */
-/* Permission is hereby granted, free of charge, to any person obtaining */
-/* a copy of this software and associated documentation files (the       */
-/* "Software"), to deal in the Software without restriction, including   */
-/* without limitation the rights to use, copy, modify, merge, publish,   */
-/* distribute, sublicense, and/or sell copies of the Software, and to    */
-/* permit persons to whom the Software is furnished to do so, subject to */
-/* the following conditions:                                             */
-/*                                                                       */
-/* The above copyright notice and this permission notice shall be        */
-/* included in all copies or substantial portions of the Software.       */
-/*                                                                       */
-/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
-/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
-/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
-/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
-/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION 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 BULLET_UTILITIES_H
-#define BULLET_UTILITIES_H
-
-#define bulletnew(cl) \
-	new cl
-
-#define bulletdelete(cl) \
-	{                    \
-		delete cl;       \
-		cl = nullptr;    \
-	}
-
-#endif // BULLET_UTILITIES_H

modules/bullet/collision_object_bullet.cpp

@@ -1,396 +0,0 @@
-/*************************************************************************/
-/*  collision_object_bullet.cpp                                          */
-/*************************************************************************/
-/*                       This file is part of:                           */
-/*                           GODOT ENGINE                                */
-/*                      https://godotengine.org                          */
-/*************************************************************************/
-/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur.                 */
-/* Copyright (c) 2014-2022 Godot Engine contributors (cf. AUTHORS.md).   */
-/*                                                                       */
-/* Permission is hereby granted, free of charge, to any person obtaining */
-/* a copy of this software and associated documentation files (the       */
-/* "Software"), to deal in the Software without restriction, including   */
-/* without limitation the rights to use, copy, modify, merge, publish,   */
-/* distribute, sublicense, and/or sell copies of the Software, and to    */
-/* permit persons to whom the Software is furnished to do so, subject to */
-/* the following conditions:                                             */
-/*                                                                       */
-/* The above copyright notice and this permission notice shall be        */
-/* included in all copies or substantial portions of the Software.       */
-/*                                                                       */
-/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
-/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
-/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
-/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
-/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION 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 "collision_object_bullet.h"
-
-#include "area_bullet.h"
-#include "bullet_physics_server.h"
-#include "bullet_types_converter.h"
-#include "bullet_utilities.h"
-#include "shape_bullet.h"
-#include "space_bullet.h"
-
-#include <btBulletCollisionCommon.h>
-
-// We enable dynamic AABB tree so that we can actually perform a broadphase on bodies with compound collision shapes.
-// This is crucial for the performance of kinematic bodies and for bodies with transforming shapes.
-#define enableDynamicAabbTree true
-
-CollisionObjectBullet::ShapeWrapper::~ShapeWrapper() {}
-
-void CollisionObjectBullet::ShapeWrapper::set_transform(const Transform3D &p_transform) {
-	G_TO_B(p_transform.get_basis().get_scale_abs(), scale);
-	G_TO_B(p_transform, transform);
-	UNSCALE_BT_BASIS(transform);
-}
-
-void CollisionObjectBullet::ShapeWrapper::set_transform(const btTransform &p_transform) {
-	transform = p_transform;
-}
-
-btTransform CollisionObjectBullet::ShapeWrapper::get_adjusted_transform() const {
-	if (shape->get_type() == PhysicsServer3D::SHAPE_HEIGHTMAP) {
-		const HeightMapShapeBullet *hm_shape = (const HeightMapShapeBullet *)shape; // should be safe to cast now
-		btTransform adjusted_transform;
-
-		// Bullet centers our heightmap:
-		// https://github.com/bulletphysics/bullet3/blob/master/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h#L33
-		// This is really counter intuitive so we're adjusting for it
-
-		adjusted_transform.setIdentity();
-		adjusted_transform.setOrigin(btVector3(0.0, hm_shape->min_height + ((hm_shape->max_height - hm_shape->min_height) * 0.5), 0.0));
-		adjusted_transform *= transform;
-
-		return adjusted_transform;
-	} else {
-		return transform;
-	}
-}
-
-void CollisionObjectBullet::ShapeWrapper::claim_bt_shape(const btVector3 &body_scale) {
-	if (!bt_shape) {
-		if (active) {
-			bt_shape = shape->create_bt_shape(scale * body_scale);
-		} else {
-			bt_shape = ShapeBullet::create_shape_empty();
-		}
-	}
-}
-
-CollisionObjectBullet::CollisionObjectBullet(Type p_type) :
-		RIDBullet(),
-		type(p_type) {}
-
-CollisionObjectBullet::~CollisionObjectBullet() {
-	for (int i = 0; i < areasOverlapped.size(); i++) {
-		areasOverlapped[i]->remove_object_overlaps(this);
-	}
-	destroyBulletCollisionObject();
-}
-
-bool equal(real_t first, real_t second) {
-	return Math::abs(first - second) <= 0.001f;
-}
-
-void CollisionObjectBullet::set_body_scale(const Vector3 &p_new_scale) {
-	if (!equal(p_new_scale[0], body_scale[0]) || !equal(p_new_scale[1], body_scale[1]) || !equal(p_new_scale[2], body_scale[2])) {
-		body_scale = p_new_scale;
-		body_scale_changed();
-	}
-}
-
-btVector3 CollisionObjectBullet::get_bt_body_scale() const {
-	btVector3 s;
-	G_TO_B(body_scale, s);
-	return s;
-}
-
-void CollisionObjectBullet::body_scale_changed() {
-	force_shape_reset = true;
-}
-
-void CollisionObjectBullet::destroyBulletCollisionObject() {
-	bulletdelete(bt_collision_object);
-}
-
-void CollisionObjectBullet::setupBulletCollisionObject(btCollisionObject *p_collisionObject) {
-	bt_collision_object = p_collisionObject;
-	bt_collision_object->setUserPointer(this);
-	bt_collision_object->setUserIndex(type);
-	// Force the enabling of collision and avoid problems
-	set_collision_enabled(collisionsEnabled);
-	p_collisionObject->setCollisionFlags(p_collisionObject->getCollisionFlags() | btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK);
-}
-
-void CollisionObjectBullet::add_collision_exception(const CollisionObjectBullet *p_ignoreCollisionObject) {
-	exceptions.insert(p_ignoreCollisionObject->get_self());
-	if (!bt_collision_object) {
-		return;
-	}
-	bt_collision_object->setIgnoreCollisionCheck(p_ignoreCollisionObject->bt_collision_object, true);
-	if (space) {
-		space->get_broadphase()->getOverlappingPairCache()->cleanProxyFromPairs(bt_collision_object->getBroadphaseHandle(), space->get_dispatcher());
-	}
-}
-
-void CollisionObjectBullet::remove_collision_exception(const CollisionObjectBullet *p_ignoreCollisionObject) {
-	exceptions.erase(p_ignoreCollisionObject->get_self());
-	if (!bt_collision_object) {
-		return;
-	}
-	bt_collision_object->setIgnoreCollisionCheck(p_ignoreCollisionObject->bt_collision_object, false);
-	if (space) {
-		space->get_broadphase()->getOverlappingPairCache()->cleanProxyFromPairs(bt_collision_object->getBroadphaseHandle(), space->get_dispatcher());
-	}
-}
-
-bool CollisionObjectBullet::has_collision_exception(const CollisionObjectBullet *p_otherCollisionObject) const {
-	return exceptions.has(p_otherCollisionObject->get_self());
-}
-
-void CollisionObjectBullet::set_collision_enabled(bool p_enabled) {
-	collisionsEnabled = p_enabled;
-	if (!bt_collision_object) {
-		return;
-	}
-	if (collisionsEnabled) {
-		bt_collision_object->setCollisionFlags(bt_collision_object->getCollisionFlags() & (~btCollisionObject::CF_NO_CONTACT_RESPONSE));
-	} else {
-		bt_collision_object->setCollisionFlags(bt_collision_object->getCollisionFlags() | btCollisionObject::CF_NO_CONTACT_RESPONSE);
-	}
-}
-
-bool CollisionObjectBullet::is_collisions_response_enabled() {
-	return collisionsEnabled;
-}
-
-void CollisionObjectBullet::notify_new_overlap(AreaBullet *p_area) {
-	if (areasOverlapped.find(p_area) == -1) {
-		areasOverlapped.push_back(p_area);
-	}
-}
-
-void CollisionObjectBullet::on_exit_area(AreaBullet *p_area) {
-	areasOverlapped.erase(p_area);
-}
-
-void CollisionObjectBullet::set_godot_object_flags(int flags) {
-	bt_collision_object->setUserIndex2(flags);
-	updated = true;
-}
-
-int CollisionObjectBullet::get_godot_object_flags() const {
-	return bt_collision_object->getUserIndex2();
-}
-
-void CollisionObjectBullet::set_transform(const Transform3D &p_global_transform) {
-	set_body_scale(p_global_transform.basis.get_scale_abs());
-
-	btTransform bt_transform;
-	G_TO_B(p_global_transform, bt_transform);
-	UNSCALE_BT_BASIS(bt_transform);
-
-	set_transform__bullet(bt_transform);
-}
-
-Transform3D CollisionObjectBullet::get_transform() const {
-	Transform3D t;
-	B_TO_G(get_transform__bullet(), t);
-	t.basis.scale(body_scale);
-	return t;
-}
-
-void CollisionObjectBullet::set_transform__bullet(const btTransform &p_global_transform) {
-	bt_collision_object->setWorldTransform(p_global_transform);
-	notify_transform_changed();
-}
-
-const btTransform &CollisionObjectBullet::get_transform__bullet() const {
-	return bt_collision_object->getWorldTransform();
-}
-
-void CollisionObjectBullet::notify_transform_changed() {
-	updated = true;
-}
-
-RigidCollisionObjectBullet::~RigidCollisionObjectBullet() {
-	remove_all_shapes(true, true);
-	if (mainShape && mainShape->isCompound()) {
-		bulletdelete(mainShape);
-	}
-}
-
-void RigidCollisionObjectBullet::add_shape(ShapeBullet *p_shape, const Transform3D &p_transform, bool p_disabled) {
-	shapes.push_back(ShapeWrapper(p_shape, p_transform, !p_disabled));
-	p_shape->add_owner(this);
-	reload_shapes();
-}
-
-void RigidCollisionObjectBullet::set_shape(int p_index, ShapeBullet *p_shape) {
-	ShapeWrapper &shp = shapes.write[p_index];
-	shp.shape->remove_owner(this);
-	p_shape->add_owner(this);
-	shp.shape = p_shape;
-	reload_shapes();
-}
-
-int RigidCollisionObjectBullet::get_shape_count() const {
-	return shapes.size();
-}
-
-ShapeBullet *RigidCollisionObjectBullet::get_shape(int p_index) const {
-	return shapes[p_index].shape;
-}
-
-btCollisionShape *RigidCollisionObjectBullet::get_bt_shape(int p_index) const {
-	return shapes[p_index].bt_shape;
-}
-
-int RigidCollisionObjectBullet::find_shape(ShapeBullet *p_shape) const {
-	const int size = shapes.size();
-	for (int i = 0; i < size; ++i) {
-		if (shapes[i].shape == p_shape) {
-			return i;
-		}
-	}
-	return -1;
-}
-
-void RigidCollisionObjectBullet::remove_shape_full(ShapeBullet *p_shape) {
-	// Remove the shape, all the times it appears
-	// Reverse order required for delete.
-	for (int i = shapes.size() - 1; 0 <= i; --i) {
-		if (p_shape == shapes[i].shape) {
-			internal_shape_destroy(i);
-			shapes.remove_at(i);
-		}
-	}
-	reload_shapes();
-}
-
-void RigidCollisionObjectBullet::remove_shape_full(int p_index) {
-	ERR_FAIL_INDEX(p_index, get_shape_count());
-	internal_shape_destroy(p_index);
-	shapes.remove_at(p_index);
-	reload_shapes();
-}
-
-void RigidCollisionObjectBullet::remove_all_shapes(bool p_permanentlyFromThisBody, bool p_force_not_reload) {
-	// Reverse order required for delete.
-	for (int i = shapes.size() - 1; 0 <= i; --i) {
-		internal_shape_destroy(i, p_permanentlyFromThisBody);
-	}
-	shapes.clear();
-	if (!p_force_not_reload) {
-		reload_shapes();
-	}
-}
-
-void RigidCollisionObjectBullet::set_shape_transform(int p_index, const Transform3D &p_transform) {
-	ERR_FAIL_INDEX(p_index, get_shape_count());
-
-	shapes.write[p_index].set_transform(p_transform);
-	shape_changed(p_index);
-}
-
-const btTransform &RigidCollisionObjectBullet::get_bt_shape_transform(int p_index) const {
-	return shapes[p_index].transform;
-}
-
-Transform3D RigidCollisionObjectBullet::get_shape_transform(int p_index) const {
-	Transform3D trs;
-	B_TO_G(shapes[p_index].transform, trs);
-	return trs;
-}
-
-void RigidCollisionObjectBullet::set_shape_disabled(int p_index, bool p_disabled) {
-	if (shapes[p_index].active != p_disabled) {
-		return;
-	}
-	shapes.write[p_index].active = !p_disabled;
-	shape_changed(p_index);
-}
-
-bool RigidCollisionObjectBullet::is_shape_disabled(int p_index) {
-	return !shapes[p_index].active;
-}
-
-void RigidCollisionObjectBullet::shape_changed(int p_shape_index) {
-	ShapeWrapper &shp = shapes.write[p_shape_index];
-	if (shp.bt_shape == mainShape) {
-		mainShape = nullptr;
-	}
-	bulletdelete(shp.bt_shape);
-	reload_shapes();
-}
-
-void RigidCollisionObjectBullet::reload_shapes() {
-	if (mainShape && mainShape->isCompound()) {
-		// Destroy compound
-		bulletdelete(mainShape);
-	}
-
-	mainShape = nullptr;
-
-	ShapeWrapper *shpWrapper;
-	const int shape_count = shapes.size();
-
-	// Reset shape if required
-	if (force_shape_reset) {
-		for (int i(0); i < shape_count; ++i) {
-			shpWrapper = &shapes.write[i];
-			bulletdelete(shpWrapper->bt_shape);
-		}
-		force_shape_reset = false;
-	}
-
-	const btVector3 body_scale(get_bt_body_scale());
-
-	// Try to optimize by not using compound
-	if (1 == shape_count) {
-		shpWrapper = &shapes.write[0];
-		btTransform transform = shpWrapper->get_adjusted_transform();
-		if (transform.getOrigin().isZero() && transform.getBasis() == transform.getBasis().getIdentity()) {
-			shpWrapper->claim_bt_shape(body_scale);
-			mainShape = shpWrapper->bt_shape;
-			main_shape_changed();
-			return;
-		}
-	}
-
-	// Optimization not possible use a compound shape
-	btCompoundShape *compoundShape = bulletnew(btCompoundShape(enableDynamicAabbTree, shape_count));
-
-	for (int i(0); i < shape_count; ++i) {
-		shpWrapper = &shapes.write[i];
-		shpWrapper->claim_bt_shape(body_scale);
-		btTransform scaled_shape_transform(shpWrapper->get_adjusted_transform());
-		scaled_shape_transform.getOrigin() *= body_scale;
-		compoundShape->addChildShape(scaled_shape_transform, shpWrapper->bt_shape);
-	}
-
-	compoundShape->recalculateLocalAabb();
-	mainShape = compoundShape;
-	main_shape_changed();
-}
-
-void RigidCollisionObjectBullet::body_scale_changed() {
-	CollisionObjectBullet::body_scale_changed();
-	reload_shapes();
-}
-
-void RigidCollisionObjectBullet::internal_shape_destroy(int p_index, bool p_permanentlyFromThisBody) {
-	ShapeWrapper &shp = shapes.write[p_index];
-	shp.shape->remove_owner(this, p_permanentlyFromThisBody);
-	if (shp.bt_shape == mainShape) {
-		mainShape = nullptr;
-	}
-	bulletdelete(shp.bt_shape);
-}

modules/bullet/collision_object_bullet.h

@@ -1,255 +0,0 @@
-/*************************************************************************/
-/*  collision_object_bullet.h                                            */
-/*************************************************************************/
-/*                       This file is part of:                           */
-/*                           GODOT ENGINE                                */
-/*                      https://godotengine.org                          */
-/*************************************************************************/
-/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur.                 */
-/* Copyright (c) 2014-2022 Godot Engine contributors (cf. AUTHORS.md).   */
-/*                                                                       */
-/* Permission is hereby granted, free of charge, to any person obtaining */
-/* a copy of this software and associated documentation files (the       */
-/* "Software"), to deal in the Software without restriction, including   */
-/* without limitation the rights to use, copy, modify, merge, publish,   */
-/* distribute, sublicense, and/or sell copies of the Software, and to    */
-/* permit persons to whom the Software is furnished to do so, subject to */
-/* the following conditions:                                             */
-/*                                                                       */
-/* The above copyright notice and this permission notice shall be        */
-/* included in all copies or substantial portions of the Software.       */
-/*                                                                       */
-/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
-/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
-/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
-/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
-/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION 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 COLLISION_OBJECT_BULLET_H
-#define COLLISION_OBJECT_BULLET_H
-
-#include "core/math/transform_3d.h"
-#include "core/math/vector3.h"
-#include "core/object/class_db.h"
-#include "core/templates/vset.h"
-#include "shape_owner_bullet.h"
-
-#include <LinearMath/btTransform.h>
-
-class AreaBullet;
-class ShapeBullet;
-class btCollisionObject;
-class btCompoundShape;
-class btCollisionShape;
-class SpaceBullet;
-
-class CollisionObjectBullet : public RIDBullet {
-public:
-	enum GodotObjectFlags {
-		GOF_IS_MONITORING_AREA = 1 << 0
-		// FLAG2 = 1 << 1,
-		// FLAG3 = 1 << 2,
-		// FLAG4 = 1 << 3,
-		// FLAG5 = 1 << 4,
-		// FLAG6 = 1 << 5
-		// etc..
-	};
-	enum Type {
-		TYPE_AREA = 0,
-		TYPE_RIGID_BODY,
-		TYPE_SOFT_BODY,
-		TYPE_KINEMATIC_GHOST_BODY
-	};
-
-	struct ShapeWrapper {
-		ShapeBullet *shape = nullptr;
-		btCollisionShape *bt_shape = nullptr;
-		btTransform transform;
-		btVector3 scale;
-		bool active = true;
-
-		ShapeWrapper() {}
-
-		ShapeWrapper(ShapeBullet *p_shape, const btTransform &p_transform, bool p_active) :
-				shape(p_shape),
-				active(p_active) {
-			set_transform(p_transform);
-		}
-
-		ShapeWrapper(ShapeBullet *p_shape, const Transform3D &p_transform, bool p_active) :
-				shape(p_shape),
-				active(p_active) {
-			set_transform(p_transform);
-		}
-		~ShapeWrapper();
-
-		ShapeWrapper(const ShapeWrapper &otherShape) {
-			operator=(otherShape);
-		}
-
-		void operator=(const ShapeWrapper &otherShape) {
-			shape = otherShape.shape;
-			bt_shape = otherShape.bt_shape;
-			transform = otherShape.transform;
-			scale = otherShape.scale;
-			active = otherShape.active;
-		}
-
-		void set_transform(const Transform3D &p_transform);
-		void set_transform(const btTransform &p_transform);
-		btTransform get_adjusted_transform() const;
-
-		void claim_bt_shape(const btVector3 &body_scale);
-	};
-
-protected:
-	Type type = TYPE_AREA;
-	ObjectID instance_id;
-	uint32_t collisionLayer = 0;
-	uint32_t collisionMask = 0;
-	bool collisionsEnabled = true;
-	bool m_isStatic = false;
-	bool ray_pickable = false;
-	btCollisionObject *bt_collision_object = nullptr;
-	Vector3 body_scale = Vector3(1, 1, 1);
-	bool force_shape_reset = false;
-	SpaceBullet *space = nullptr;
-
-	VSet<RID> exceptions;
-
-	/// This array is used to know all areas where this Object is overlapped in
-	/// New area is added when overlap with new area (AreaBullet::addOverlap), then is removed when it exit (CollisionObjectBullet::onExitArea)
-	/// This array is used mainly to know which area hold the pointer of this object
-	Vector<AreaBullet *> areasOverlapped;
-	bool updated = false;
-
-public:
-	CollisionObjectBullet(Type p_type);
-	virtual ~CollisionObjectBullet();
-
-	Type getType() { return type; }
-
-protected:
-	void destroyBulletCollisionObject();
-	void setupBulletCollisionObject(btCollisionObject *p_collisionObject);
-
-public:
-	_FORCE_INLINE_ btCollisionObject *get_bt_collision_object() { return bt_collision_object; }
-
-	_FORCE_INLINE_ void set_instance_id(const ObjectID &p_instance_id) { instance_id = p_instance_id; }
-	_FORCE_INLINE_ ObjectID get_instance_id() const { return instance_id; }
-
-	_FORCE_INLINE_ bool is_static() const { return m_isStatic; }
-
-	_FORCE_INLINE_ void set_ray_pickable(bool p_enable) { ray_pickable = p_enable; }
-	_FORCE_INLINE_ bool is_ray_pickable() const { return ray_pickable; }
-
-	void set_body_scale(const Vector3 &p_new_scale);
-	const Vector3 &get_body_scale() const { return body_scale; }
-	btVector3 get_bt_body_scale() const;
-	virtual void body_scale_changed();
-
-	void add_collision_exception(const CollisionObjectBullet *p_ignoreCollisionObject);
-	void remove_collision_exception(const CollisionObjectBullet *p_ignoreCollisionObject);
-	bool has_collision_exception(const CollisionObjectBullet *p_otherCollisionObject) const;
-	_FORCE_INLINE_ const VSet<RID> &get_exceptions() const { return exceptions; }
-
-	_FORCE_INLINE_ void set_collision_layer(uint32_t p_layer) {
-		if (collisionLayer != p_layer) {
-			collisionLayer = p_layer;
-			on_collision_filters_change();
-		}
-	}
-	_FORCE_INLINE_ uint32_t get_collision_layer() const { return collisionLayer; }
-
-	_FORCE_INLINE_ void set_collision_mask(uint32_t p_mask) {
-		if (collisionMask != p_mask) {
-			collisionMask = p_mask;
-			on_collision_filters_change();
-		}
-	}
-	_FORCE_INLINE_ uint32_t get_collision_mask() const { return collisionMask; }
-
-	virtual void on_collision_filters_change() = 0;
-
-	_FORCE_INLINE_ bool test_collision_mask(CollisionObjectBullet *p_other) const {
-		return collisionLayer & p_other->collisionMask || p_other->collisionLayer & collisionMask;
-	}
-
-	virtual void reload_body() = 0;
-	virtual void set_space(SpaceBullet *p_space) = 0;
-	_FORCE_INLINE_ SpaceBullet *get_space() const { return space; }
-
-	virtual void on_collision_checker_start() = 0;
-	virtual void on_collision_checker_end() = 0;
-
-	virtual void dispatch_callbacks() = 0;
-
-	void set_collision_enabled(bool p_enabled);
-	bool is_collisions_response_enabled();
-
-	void notify_new_overlap(AreaBullet *p_area);
-	virtual void on_enter_area(AreaBullet *p_area) = 0;
-	virtual void on_exit_area(AreaBullet *p_area);
-
-	void set_godot_object_flags(int flags);
-	int get_godot_object_flags() const;
-
-	void set_transform(const Transform3D &p_global_transform);
-	Transform3D get_transform() const;
-	virtual void set_transform__bullet(const btTransform &p_global_transform);
-	virtual const btTransform &get_transform__bullet() const;
-	virtual void notify_transform_changed();
-
-	bool is_updated() const { return updated; }
-};
-
-class RigidCollisionObjectBullet : public CollisionObjectBullet, public ShapeOwnerBullet {
-protected:
-	btCollisionShape *mainShape = nullptr;
-	Vector<ShapeWrapper> shapes;
-
-public:
-	RigidCollisionObjectBullet(Type p_type) :
-			CollisionObjectBullet(p_type) {}
-	~RigidCollisionObjectBullet();
-
-	_FORCE_INLINE_ const Vector<ShapeWrapper> &get_shapes_wrappers() const { return shapes; }
-
-	_FORCE_INLINE_ btCollisionShape *get_main_shape() const { return mainShape; }
-
-	void add_shape(ShapeBullet *p_shape, const Transform3D &p_transform = Transform3D(), bool p_disabled = false);
-	void set_shape(int p_index, ShapeBullet *p_shape);
-
-	int get_shape_count() const;
-	ShapeBullet *get_shape(int p_index) const;
-	btCollisionShape *get_bt_shape(int p_index) const;
-
-	int find_shape(ShapeBullet *p_shape) const;
-
-	virtual void remove_shape_full(ShapeBullet *p_shape);
-	void remove_shape_full(int p_index);
-	void remove_all_shapes(bool p_permanentlyFromThisBody = false, bool p_force_not_reload = false);
-
-	void set_shape_transform(int p_index, const Transform3D &p_transform);
-
-	const btTransform &get_bt_shape_transform(int p_index) const;
-	Transform3D get_shape_transform(int p_index) const;
-
-	void set_shape_disabled(int p_index, bool p_disabled);
-	bool is_shape_disabled(int p_index);
-
-	virtual void shape_changed(int p_shape_index);
-	virtual void reload_shapes();
-
-	virtual void main_shape_changed() = 0;
-	virtual void body_scale_changed();
-
-private:
-	void internal_shape_destroy(int p_index, bool p_permanentlyFromThisBody = false);
-};
-
-#endif // COLLISION_OBJECT_BULLET_H

modules/bullet/cone_twist_joint_bullet.cpp

@@ -1,103 +0,0 @@
-/*************************************************************************/
-/*  cone_twist_joint_bullet.cpp                                          */
-/*************************************************************************/
-/*                       This file is part of:                           */
-/*                           GODOT ENGINE                                */
-/*                      https://godotengine.org                          */
-/*************************************************************************/
-/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur.                 */
-/* Copyright (c) 2014-2022 Godot Engine contributors (cf. AUTHORS.md).   */
-/*                                                                       */
-/* Permission is hereby granted, free of charge, to any person obtaining */
-/* a copy of this software and associated documentation files (the       */
-/* "Software"), to deal in the Software without restriction, including   */
-/* without limitation the rights to use, copy, modify, merge, publish,   */
-/* distribute, sublicense, and/or sell copies of the Software, and to    */
-/* permit persons to whom the Software is furnished to do so, subject to */
-/* the following conditions:                                             */
-/*                                                                       */
-/* The above copyright notice and this permission notice shall be        */
-/* included in all copies or substantial portions of the Software.       */
-/*                                                                       */
-/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
-/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
-/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
-/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
-/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION 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 "cone_twist_joint_bullet.h"
-
-#include "bullet_types_converter.h"
-#include "bullet_utilities.h"
-#include "rigid_body_bullet.h"
-
-#include <BulletDynamics/ConstraintSolver/btConeTwistConstraint.h>
-
-ConeTwistJointBullet::ConeTwistJointBullet(RigidBodyBullet *rbA, RigidBodyBullet *rbB, const Transform3D &rbAFrame, const Transform3D &rbBFrame) :
-		JointBullet() {
-	Transform3D scaled_AFrame(rbAFrame.scaled(rbA->get_body_scale()));
-	scaled_AFrame.basis.rotref_posscale_decomposition(scaled_AFrame.basis);
-
-	btTransform btFrameA;
-	G_TO_B(scaled_AFrame, btFrameA);
-
-	if (rbB) {
-		Transform3D scaled_BFrame(rbBFrame.scaled(rbB->get_body_scale()));
-		scaled_BFrame.basis.rotref_posscale_decomposition(scaled_BFrame.basis);
-
-		btTransform btFrameB;
-		G_TO_B(scaled_BFrame, btFrameB);
-
-		coneConstraint = bulletnew(btConeTwistConstraint(*rbA->get_bt_rigid_body(), *rbB->get_bt_rigid_body(), btFrameA, btFrameB));
-	} else {
-		coneConstraint = bulletnew(btConeTwistConstraint(*rbA->get_bt_rigid_body(), btFrameA));
-	}
-	setup(coneConstraint);
-}
-
-void ConeTwistJointBullet::set_param(PhysicsServer3D::ConeTwistJointParam p_param, real_t p_value) {
-	switch (p_param) {
-		case PhysicsServer3D::CONE_TWIST_JOINT_SWING_SPAN:
-			coneConstraint->setLimit(5, p_value);
-			coneConstraint->setLimit(4, p_value);
-			break;
-		case PhysicsServer3D::CONE_TWIST_JOINT_TWIST_SPAN:
-			coneConstraint->setLimit(3, p_value);
-			break;
-		case PhysicsServer3D::CONE_TWIST_JOINT_BIAS:
-			coneConstraint->setLimit(coneConstraint->getSwingSpan1(), coneConstraint->getSwingSpan2(), coneConstraint->getTwistSpan(), coneConstraint->getLimitSoftness(), p_value, coneConstraint->getRelaxationFactor());
-			break;
-		case PhysicsServer3D::CONE_TWIST_JOINT_SOFTNESS:
-			coneConstraint->setLimit(coneConstraint->getSwingSpan1(), coneConstraint->getSwingSpan2(), coneConstraint->getTwistSpan(), p_value, coneConstraint->getBiasFactor(), coneConstraint->getRelaxationFactor());
-			break;
-		case PhysicsServer3D::CONE_TWIST_JOINT_RELAXATION:
-			coneConstraint->setLimit(coneConstraint->getSwingSpan1(), coneConstraint->getSwingSpan2(), coneConstraint->getTwistSpan(), coneConstraint->getLimitSoftness(), coneConstraint->getBiasFactor(), p_value);
-			break;
-		case PhysicsServer3D::CONE_TWIST_MAX:
-			// Internal size value, nothing to do.
-			break;
-	}
-}
-
-real_t ConeTwistJointBullet::get_param(PhysicsServer3D::ConeTwistJointParam p_param) const {
-	switch (p_param) {
-		case PhysicsServer3D::CONE_TWIST_JOINT_SWING_SPAN:
-			return coneConstraint->getSwingSpan1();
-		case PhysicsServer3D::CONE_TWIST_JOINT_TWIST_SPAN:
-			return coneConstraint->getTwistSpan();
-		case PhysicsServer3D::CONE_TWIST_JOINT_BIAS:
-			return coneConstraint->getBiasFactor();
-		case PhysicsServer3D::CONE_TWIST_JOINT_SOFTNESS:
-			return coneConstraint->getLimitSoftness();
-		case PhysicsServer3D::CONE_TWIST_JOINT_RELAXATION:
-			return coneConstraint->getRelaxationFactor();
-		case PhysicsServer3D::CONE_TWIST_MAX:
-			// Internal size value, nothing to do.
-			return 0;
-	}
-	// Compiler doesn't seem to notice that all code paths are fulfilled...
-	return 0;
-}

modules/bullet/cone_twist_joint_bullet.h

@@ -1,50 +0,0 @@
-/*************************************************************************/
-/*  cone_twist_joint_bullet.h                                            */
-/*************************************************************************/
-/*                       This file is part of:                           */
-/*                           GODOT ENGINE                                */
-/*                      https://godotengine.org                          */
-/*************************************************************************/
-/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur.                 */
-/* Copyright (c) 2014-2022 Godot Engine contributors (cf. AUTHORS.md).   */
-/*                                                                       */
-/* Permission is hereby granted, free of charge, to any person obtaining */
-/* a copy of this software and associated documentation files (the       */
-/* "Software"), to deal in the Software without restriction, including   */
-/* without limitation the rights to use, copy, modify, merge, publish,   */
-/* distribute, sublicense, and/or sell copies of the Software, and to    */
-/* permit persons to whom the Software is furnished to do so, subject to */
-/* the following conditions:                                             */
-/*                                                                       */
-/* The above copyright notice and this permission notice shall be        */
-/* included in all copies or substantial portions of the Software.       */
-/*                                                                       */
-/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
-/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
-/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
-/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
-/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION 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 CONE_TWIST_JOINT_BULLET_H
-#define CONE_TWIST_JOINT_BULLET_H
-
-#include "joint_bullet.h"
-
-class RigidBodyBullet;
-
-class ConeTwistJointBullet : public JointBullet {
-	class btConeTwistConstraint *coneConstraint;
-
-public:
-	ConeTwistJointBullet(RigidBodyBullet *rbA, RigidBodyBullet *rbB, const Transform3D &rbAFrame, const Transform3D &rbBFrame);
-
-	virtual PhysicsServer3D::JointType get_type() const { return PhysicsServer3D::JOINT_CONE_TWIST; }
-
-	void set_param(PhysicsServer3D::ConeTwistJointParam p_param, real_t p_value);
-	real_t get_param(PhysicsServer3D::ConeTwistJointParam p_param) const;
-};
-
-#endif // CONE_TWIST_JOINT_BULLET_H

modules/bullet/config.py

@@ -1,8 +0,0 @@
-def can_build(env, platform):
-    # API Changed and bullet is disabled at the moment
-    return False
-    # Later change to return not env["disable_3d"]
-
-
-def configure(env):
-    pass

modules/bullet/constraint_bullet.cpp

@@ -1,58 +0,0 @@
-/*************************************************************************/
-/*  constraint_bullet.cpp                                                */
-/*************************************************************************/
-/*                       This file is part of:                           */
-/*                           GODOT ENGINE                                */
-/*                      https://godotengine.org                          */
-/*************************************************************************/
-/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur.                 */
-/* Copyright (c) 2014-2022 Godot Engine contributors (cf. AUTHORS.md).   */
-/*                                                                       */
-/* Permission is hereby granted, free of charge, to any person obtaining */
-/* a copy of this software and associated documentation files (the       */
-/* "Software"), to deal in the Software without restriction, including   */
-/* without limitation the rights to use, copy, modify, merge, publish,   */
-/* distribute, sublicense, and/or sell copies of the Software, and to    */
-/* permit persons to whom the Software is furnished to do so, subject to */
-/* the following conditions:                                             */
-/*                                                                       */
-/* The above copyright notice and this permission notice shall be        */
-/* included in all copies or substantial portions of the Software.       */
-/*                                                                       */
-/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
-/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
-/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
-/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
-/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION 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 "constraint_bullet.h"
-
-#include "collision_object_bullet.h"
-#include "space_bullet.h"
-
-ConstraintBullet::ConstraintBullet() {}
-
-void ConstraintBullet::setup(btTypedConstraint *p_constraint) {
-	constraint = p_constraint;
-	constraint->setUserConstraintPtr(this);
-}
-
-void ConstraintBullet::set_space(SpaceBullet *p_space) {
-	space = p_space;
-}
-
-void ConstraintBullet::destroy_internal_constraint() {
-	space->remove_constraint(this);
-}
-
-void ConstraintBullet::disable_collisions_between_bodies(const bool p_disabled) {
-	disabled_collisions_between_bodies = p_disabled;
-
-	if (space) {
-		space->remove_constraint(this);
-		space->add_constraint(this, disabled_collisions_between_bodies);
-	}
-}

modules/bullet/constraint_bullet.h

@@ -1,68 +0,0 @@
-/*************************************************************************/
-/*  constraint_bullet.h                                                  */
-/*************************************************************************/
-/*                       This file is part of:                           */
-/*                           GODOT ENGINE                                */
-/*                      https://godotengine.org                          */
-/*************************************************************************/
-/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur.                 */
-/* Copyright (c) 2014-2022 Godot Engine contributors (cf. AUTHORS.md).   */
-/*                                                                       */
-/* Permission is hereby granted, free of charge, to any person obtaining */
-/* a copy of this software and associated documentation files (the       */
-/* "Software"), to deal in the Software without restriction, including   */
-/* without limitation the rights to use, copy, modify, merge, publish,   */
-/* distribute, sublicense, and/or sell copies of the Software, and to    */
-/* permit persons to whom the Software is furnished to do so, subject to */
-/* the following conditions:                                             */
-/*                                                                       */
-/* The above copyright notice and this permission notice shall be        */
-/* included in all copies or substantial portions of the Software.       */
-/*                                                                       */
-/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
-/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
-/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
-/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
-/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION 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 CONSTRAINT_BULLET_H
-#define CONSTRAINT_BULLET_H
-
-#include "bullet_utilities.h"
-#include "rid_bullet.h"
-
-#include <BulletDynamics/ConstraintSolver/btTypedConstraint.h>
-
-class RigidBodyBullet;
-class SpaceBullet;
-class btTypedConstraint;
-
-class ConstraintBullet : public RIDBullet {
-protected:
-	SpaceBullet *space = nullptr;
-	btTypedConstraint *constraint = nullptr;
-	bool disabled_collisions_between_bodies = true;
-
-public:
-	ConstraintBullet();
-
-	virtual void setup(btTypedConstraint *p_constraint);
-	virtual void set_space(SpaceBullet *p_space);
-	virtual void destroy_internal_constraint();
-
-	void disable_collisions_between_bodies(const bool p_disabled);
-	_FORCE_INLINE_ bool is_disabled_collisions_between_bodies() const { return disabled_collisions_between_bodies; }
-
-public:
-	virtual ~ConstraintBullet() {
-		bulletdelete(constraint);
-		constraint = nullptr;
-	}
-
-	_FORCE_INLINE_ btTypedConstraint *get_bt_constraint() { return constraint; }
-};
-
-#endif // CONSTRAINT_BULLET_H

modules/bullet/generic_6dof_joint_bullet.cpp

@@ -1,271 +0,0 @@
-/*************************************************************************/
-/*  generic_6dof_joint_bullet.cpp                                        */
-/*************************************************************************/
-/*                       This file is part of:                           */
-/*                           GODOT ENGINE                                */
-/*                      https://godotengine.org                          */
-/*************************************************************************/
-/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur.                 */
-/* Copyright (c) 2014-2022 Godot Engine contributors (cf. AUTHORS.md).   */
-/*                                                                       */
-/* Permission is hereby granted, free of charge, to any person obtaining */
-/* a copy of this software and associated documentation files (the       */
-/* "Software"), to deal in the Software without restriction, including   */
-/* without limitation the rights to use, copy, modify, merge, publish,   */
-/* distribute, sublicense, and/or sell copies of the Software, and to    */
-/* permit persons to whom the Software is furnished to do so, subject to */
-/* the following conditions:                                             */
-/*                                                                       */
-/* The above copyright notice and this permission notice shall be        */
-/* included in all copies or substantial portions of the Software.       */
-/*                                                                       */
-/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
-/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
-/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
-/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
-/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION 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 "generic_6dof_joint_bullet.h"
-
-#include "bullet_types_converter.h"
-#include "bullet_utilities.h"
-#include "rigid_body_bullet.h"
-
-#include <BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h>
-
-Generic6DOFJointBullet::Generic6DOFJointBullet(RigidBodyBullet *rbA, RigidBodyBullet *rbB, const Transform3D &frameInA, const Transform3D &frameInB) :
-		JointBullet() {
-	for (int i = 0; i < 3; i++) {
-		for (int j = 0; j < PhysicsServer3D::G6DOF_JOINT_FLAG_MAX; j++) {
-			flags[i][j] = false;
-		}
-	}
-
-	Transform3D scaled_AFrame(frameInA.scaled(rbA->get_body_scale()));
-
-	scaled_AFrame.basis.rotref_posscale_decomposition(scaled_AFrame.basis);
-
-	btTransform btFrameA;
-	G_TO_B(scaled_AFrame, btFrameA);
-
-	if (rbB) {
-		Transform3D scaled_BFrame(frameInB.scaled(rbB->get_body_scale()));
-
-		scaled_BFrame.basis.rotref_posscale_decomposition(scaled_BFrame.basis);
-
-		btTransform btFrameB;
-		G_TO_B(scaled_BFrame, btFrameB);
-
-		sixDOFConstraint = bulletnew(btGeneric6DofSpring2Constraint(*rbA->get_bt_rigid_body(), *rbB->get_bt_rigid_body(), btFrameA, btFrameB));
-	} else {
-		sixDOFConstraint = bulletnew(btGeneric6DofSpring2Constraint(*rbA->get_bt_rigid_body(), btFrameA));
-	}
-
-	setup(sixDOFConstraint);
-}
-
-Transform3D Generic6DOFJointBullet::getFrameOffsetA() const {
-	btTransform btTrs = sixDOFConstraint->getFrameOffsetA();
-	Transform3D gTrs;
-	B_TO_G(btTrs, gTrs);
-	return gTrs;
-}
-
-Transform3D Generic6DOFJointBullet::getFrameOffsetB() const {
-	btTransform btTrs = sixDOFConstraint->getFrameOffsetB();
-	Transform3D gTrs;
-	B_TO_G(btTrs, gTrs);
-	return gTrs;
-}
-
-Transform3D Generic6DOFJointBullet::getFrameOffsetA() {
-	btTransform btTrs = sixDOFConstraint->getFrameOffsetA();
-	Transform3D gTrs;
-	B_TO_G(btTrs, gTrs);
-	return gTrs;
-}
-
-Transform3D Generic6DOFJointBullet::getFrameOffsetB() {
-	btTransform btTrs = sixDOFConstraint->getFrameOffsetB();
-	Transform3D gTrs;
-	B_TO_G(btTrs, gTrs);
-	return gTrs;
-}
-
-void Generic6DOFJointBullet::set_linear_lower_limit(const Vector3 &linearLower) {
-	btVector3 btVec;
-	G_TO_B(linearLower, btVec);
-	sixDOFConstraint->setLinearLowerLimit(btVec);
-}
-
-void Generic6DOFJointBullet::set_linear_upper_limit(const Vector3 &linearUpper) {
-	btVector3 btVec;
-	G_TO_B(linearUpper, btVec);
-	sixDOFConstraint->setLinearUpperLimit(btVec);
-}
-
-void Generic6DOFJointBullet::set_angular_lower_limit(const Vector3 &angularLower) {
-	btVector3 btVec;
-	G_TO_B(angularLower, btVec);
-	sixDOFConstraint->setAngularLowerLimit(btVec);
-}
-
-void Generic6DOFJointBullet::set_angular_upper_limit(const Vector3 &angularUpper) {
-	btVector3 btVec;
-	G_TO_B(angularUpper, btVec);
-	sixDOFConstraint->setAngularUpperLimit(btVec);
-}
-
-void Generic6DOFJointBullet::set_param(Vector3::Axis p_axis, PhysicsServer3D::G6DOFJointAxisParam p_param, real_t p_value) {
-	ERR_FAIL_INDEX(p_axis, 3);
-	switch (p_param) {
-		case PhysicsServer3D::G6DOF_JOINT_LINEAR_LOWER_LIMIT:
-			limits_lower[0][p_axis] = p_value;
-			set_flag(p_axis, PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_LINEAR_LIMIT, flags[p_axis][PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_LINEAR_LIMIT]); // Reload bullet parameter
-			break;
-		case PhysicsServer3D::G6DOF_JOINT_LINEAR_UPPER_LIMIT:
-			limits_upper[0][p_axis] = p_value;
-			set_flag(p_axis, PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_LINEAR_LIMIT, flags[p_axis][PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_LINEAR_LIMIT]); // Reload bullet parameter
-			break;
-		case PhysicsServer3D::G6DOF_JOINT_LINEAR_MOTOR_TARGET_VELOCITY:
-			sixDOFConstraint->getTranslationalLimitMotor()->m_targetVelocity.m_floats[p_axis] = p_value;
-			break;
-		case PhysicsServer3D::G6DOF_JOINT_LINEAR_MOTOR_FORCE_LIMIT:
-			sixDOFConstraint->getTranslationalLimitMotor()->m_maxMotorForce.m_floats[p_axis] = p_value;
-			break;
-		case PhysicsServer3D::G6DOF_JOINT_LINEAR_SPRING_DAMPING:
-			sixDOFConstraint->getTranslationalLimitMotor()->m_springDamping.m_floats[p_axis] = p_value;
-			break;
-		case PhysicsServer3D::G6DOF_JOINT_LINEAR_SPRING_STIFFNESS:
-			sixDOFConstraint->getTranslationalLimitMotor()->m_springStiffness.m_floats[p_axis] = p_value;
-			break;
-		case PhysicsServer3D::G6DOF_JOINT_LINEAR_SPRING_EQUILIBRIUM_POINT:
-			sixDOFConstraint->getTranslationalLimitMotor()->m_equilibriumPoint.m_floats[p_axis] = p_value;
-			break;
-		case PhysicsServer3D::G6DOF_JOINT_ANGULAR_LOWER_LIMIT:
-			limits_lower[1][p_axis] = p_value;
-			set_flag(p_axis, PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_ANGULAR_LIMIT, flags[p_axis][PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_ANGULAR_LIMIT]); // Reload bullet parameter
-			break;
-		case PhysicsServer3D::G6DOF_JOINT_ANGULAR_UPPER_LIMIT:
-			limits_upper[1][p_axis] = p_value;
-			set_flag(p_axis, PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_ANGULAR_LIMIT, flags[p_axis][PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_ANGULAR_LIMIT]); // Reload bullet parameter
-			break;
-		case PhysicsServer3D::G6DOF_JOINT_ANGULAR_RESTITUTION:
-			sixDOFConstraint->getRotationalLimitMotor(p_axis)->m_bounce = p_value;
-			break;
-		case PhysicsServer3D::G6DOF_JOINT_ANGULAR_ERP:
-			sixDOFConstraint->getRotationalLimitMotor(p_axis)->m_stopERP = p_value;
-			break;
-		case PhysicsServer3D::G6DOF_JOINT_ANGULAR_MOTOR_TARGET_VELOCITY:
-			sixDOFConstraint->getRotationalLimitMotor(p_axis)->m_targetVelocity = p_value;
-			break;
-		case PhysicsServer3D::G6DOF_JOINT_ANGULAR_MOTOR_FORCE_LIMIT:
-			sixDOFConstraint->getRotationalLimitMotor(p_axis)->m_maxMotorForce = p_value;
-			break;
-		case PhysicsServer3D::G6DOF_JOINT_ANGULAR_SPRING_STIFFNESS:
-			sixDOFConstraint->getRotationalLimitMotor(p_axis)->m_springStiffness = p_value;
-			break;
-		case PhysicsServer3D::G6DOF_JOINT_ANGULAR_SPRING_DAMPING:
-			sixDOFConstraint->getRotationalLimitMotor(p_axis)->m_springDamping = p_value;
-			break;
-		case PhysicsServer3D::G6DOF_JOINT_ANGULAR_SPRING_EQUILIBRIUM_POINT:
-			sixDOFConstraint->getRotationalLimitMotor(p_axis)->m_equilibriumPoint = p_value;
-			break;
-		case PhysicsServer3D::G6DOF_JOINT_MAX:
-			// Internal size value, nothing to do.
-			break;
-		default:
-			WARN_DEPRECATED_MSG("The parameter " + itos(p_param) + " is deprecated.");
-			break;
-	}
-}
-
-real_t Generic6DOFJointBullet::get_param(Vector3::Axis p_axis, PhysicsServer3D::G6DOFJointAxisParam p_param) const {
-	ERR_FAIL_INDEX_V(p_axis, 3, 0.);
-	switch (p_param) {
-		case PhysicsServer3D::G6DOF_JOINT_LINEAR_LOWER_LIMIT:
-			return limits_lower[0][p_axis];
-		case PhysicsServer3D::G6DOF_JOINT_LINEAR_UPPER_LIMIT:
-			return limits_upper[0][p_axis];
-		case PhysicsServer3D::G6DOF_JOINT_LINEAR_MOTOR_TARGET_VELOCITY:
-			return sixDOFConstraint->getTranslationalLimitMotor()->m_targetVelocity.m_floats[p_axis];
-		case PhysicsServer3D::G6DOF_JOINT_LINEAR_MOTOR_FORCE_LIMIT:
-			return sixDOFConstraint->getTranslationalLimitMotor()->m_maxMotorForce.m_floats[p_axis];
-		case PhysicsServer3D::G6DOF_JOINT_LINEAR_SPRING_DAMPING:
-			return sixDOFConstraint->getTranslationalLimitMotor()->m_springDamping.m_floats[p_axis];
-		case PhysicsServer3D::G6DOF_JOINT_LINEAR_SPRING_STIFFNESS:
-			return sixDOFConstraint->getTranslationalLimitMotor()->m_springStiffness.m_floats[p_axis];
-		case PhysicsServer3D::G6DOF_JOINT_LINEAR_SPRING_EQUILIBRIUM_POINT:
-			return sixDOFConstraint->getTranslationalLimitMotor()->m_equilibriumPoint.m_floats[p_axis];
-		case PhysicsServer3D::G6DOF_JOINT_ANGULAR_LOWER_LIMIT:
-			return limits_lower[1][p_axis];
-		case PhysicsServer3D::G6DOF_JOINT_ANGULAR_UPPER_LIMIT:
-			return limits_upper[1][p_axis];
-		case PhysicsServer3D::G6DOF_JOINT_ANGULAR_RESTITUTION:
-			return sixDOFConstraint->getRotationalLimitMotor(p_axis)->m_bounce;
-		case PhysicsServer3D::G6DOF_JOINT_ANGULAR_ERP:
-			return sixDOFConstraint->getRotationalLimitMotor(p_axis)->m_stopERP;
-		case PhysicsServer3D::G6DOF_JOINT_ANGULAR_MOTOR_TARGET_VELOCITY:
-			return sixDOFConstraint->getRotationalLimitMotor(p_axis)->m_targetVelocity;
-		case PhysicsServer3D::G6DOF_JOINT_ANGULAR_MOTOR_FORCE_LIMIT:
-			return sixDOFConstraint->getRotationalLimitMotor(p_axis)->m_maxMotorForce;
-		case PhysicsServer3D::G6DOF_JOINT_ANGULAR_SPRING_STIFFNESS:
-			return sixDOFConstraint->getRotationalLimitMotor(p_axis)->m_springStiffness;
-		case PhysicsServer3D::G6DOF_JOINT_ANGULAR_SPRING_DAMPING:
-			return sixDOFConstraint->getRotationalLimitMotor(p_axis)->m_springDamping;
-		case PhysicsServer3D::G6DOF_JOINT_ANGULAR_SPRING_EQUILIBRIUM_POINT:
-			return sixDOFConstraint->getRotationalLimitMotor(p_axis)->m_equilibriumPoint;
-		case PhysicsServer3D::G6DOF_JOINT_MAX:
-			// Internal size value, nothing to do.
-			return 0;
-		default:
-			WARN_DEPRECATED_MSG("The parameter " + itos(p_param) + " is deprecated.");
-			return 0;
-	}
-}
-
-void Generic6DOFJointBullet::set_flag(Vector3::Axis p_axis, PhysicsServer3D::G6DOFJointAxisFlag p_flag, bool p_value) {
-	ERR_FAIL_INDEX(p_axis, 3);
-
-	flags[p_axis][p_flag] = p_value;
-
-	switch (p_flag) {
-		case PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_LINEAR_LIMIT:
-			if (flags[p_axis][p_flag]) {
-				sixDOFConstraint->setLimit(p_axis, limits_lower[0][p_axis], limits_upper[0][p_axis]);
-			} else {
-				sixDOFConstraint->setLimit(p_axis, 0, -1); // Free
-			}
-			break;
-		case PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_ANGULAR_LIMIT:
-			if (flags[p_axis][p_flag]) {
-				sixDOFConstraint->setLimit(p_axis + 3, limits_lower[1][p_axis], limits_upper[1][p_axis]);
-			} else {
-				sixDOFConstraint->setLimit(p_axis + 3, 0, -1); // Free
-			}
-			break;
-		case PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_ANGULAR_SPRING:
-			sixDOFConstraint->getRotationalLimitMotor(p_axis)->m_enableSpring = p_value;
-			break;
-		case PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_LINEAR_SPRING:
-			sixDOFConstraint->getTranslationalLimitMotor()->m_enableSpring[p_axis] = p_value;
-			break;
-		case PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_MOTOR:
-			sixDOFConstraint->getRotationalLimitMotor(p_axis)->m_enableMotor = flags[p_axis][p_flag];
-			break;
-		case PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_LINEAR_MOTOR:
-			sixDOFConstraint->getTranslationalLimitMotor()->m_enableMotor[p_axis] = flags[p_axis][p_flag];
-			break;
-		case PhysicsServer3D::G6DOF_JOINT_FLAG_MAX:
-			// Internal size value, nothing to do.
-			break;
-	}
-}
-
-bool Generic6DOFJointBullet::get_flag(Vector3::Axis p_axis, PhysicsServer3D::G6DOFJointAxisFlag p_flag) const {
-	ERR_FAIL_INDEX_V(p_axis, 3, false);
-	return flags[p_axis][p_flag];
-}

modules/bullet/generic_6dof_joint_bullet.h

@@ -1,69 +0,0 @@
-/*************************************************************************/
-/*  generic_6dof_joint_bullet.h                                          */
-/*************************************************************************/
-/*                       This file is part of:                           */
-/*                           GODOT ENGINE                                */
-/*                      https://godotengine.org                          */
-/*************************************************************************/
-/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur.                 */
-/* Copyright (c) 2014-2022 Godot Engine contributors (cf. AUTHORS.md).   */
-/*                                                                       */
-/* Permission is hereby granted, free of charge, to any person obtaining */
-/* a copy of this software and associated documentation files (the       */
-/* "Software"), to deal in the Software without restriction, including   */
-/* without limitation the rights to use, copy, modify, merge, publish,   */
-/* distribute, sublicense, and/or sell copies of the Software, and to    */
-/* permit persons to whom the Software is furnished to do so, subject to */
-/* the following conditions:                                             */
-/*                                                                       */
-/* The above copyright notice and this permission notice shall be        */
-/* included in all copies or substantial portions of the Software.       */
-/*                                                                       */
-/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
-/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
-/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
-/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
-/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION 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 GENERIC_6DOF_JOINT_BULLET_H
-#define GENERIC_6DOF_JOINT_BULLET_H
-
-#include "joint_bullet.h"
-
-class RigidBodyBullet;
-
-class Generic6DOFJointBullet : public JointBullet {
-	class btGeneric6DofSpring2Constraint *sixDOFConstraint;
-
-	// First is linear second is angular
-	Vector3 limits_lower[2];
-	Vector3 limits_upper[2];
-	bool flags[3][PhysicsServer3D::G6DOF_JOINT_FLAG_MAX];
-
-public:
-	Generic6DOFJointBullet(RigidBodyBullet *rbA, RigidBodyBullet *rbB, const Transform3D &frameInA, const Transform3D &frameInB);
-
-	virtual PhysicsServer3D::JointType get_type() const { return PhysicsServer3D::JOINT_6DOF; }
-
-	Transform3D getFrameOffsetA() const;
-	Transform3D getFrameOffsetB() const;
-	Transform3D getFrameOffsetA();
-	Transform3D getFrameOffsetB();
-
-	void set_linear_lower_limit(const Vector3 &linearLower);
-	void set_linear_upper_limit(const Vector3 &linearUpper);
-
-	void set_angular_lower_limit(const Vector3 &angularLower);
-	void set_angular_upper_limit(const Vector3 &angularUpper);
-
-	void set_param(Vector3::Axis p_axis, PhysicsServer3D::G6DOFJointAxisParam p_param, real_t p_value);
-	real_t get_param(Vector3::Axis p_axis, PhysicsServer3D::G6DOFJointAxisParam p_param) const;
-
-	void set_flag(Vector3::Axis p_axis, PhysicsServer3D::G6DOFJointAxisFlag p_flag, bool p_value);
-	bool get_flag(Vector3::Axis p_axis, PhysicsServer3D::G6DOFJointAxisFlag p_flag) const;
-};
-
-#endif // GENERIC_6DOF_JOINT_BULLET_H

modules/bullet/godot_collision_configuration.cpp

@@ -1,126 +0,0 @@
-/*************************************************************************/
-/*  godot_collision_configuration.cpp                                    */
-/*************************************************************************/
-/*                       This file is part of:                           */
-/*                           GODOT ENGINE                                */
-/*                      https://godotengine.org                          */
-/*************************************************************************/
-/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur.                 */
-/* Copyright (c) 2014-2022 Godot Engine contributors (cf. AUTHORS.md).   */
-/*                                                                       */
-/* Permission is hereby granted, free of charge, to any person obtaining */
-/* a copy of this software and associated documentation files (the       */
-/* "Software"), to deal in the Software without restriction, including   */
-/* without limitation the rights to use, copy, modify, merge, publish,   */
-/* distribute, sublicense, and/or sell copies of the Software, and to    */
-/* permit persons to whom the Software is furnished to do so, subject to */
-/* the following conditions:                                             */
-/*                                                                       */
-/* The above copyright notice and this permission notice shall be        */
-/* included in all copies or substantial portions of the Software.       */
-/*                                                                       */
-/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
-/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
-/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
-/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
-/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION 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 "godot_collision_configuration.h"
-
-#include "godot_ray_world_algorithm.h"
-
-#include <BulletCollision/BroadphaseCollision/btBroadphaseProxy.h>
-#include <BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h>
-
-GodotCollisionConfiguration::GodotCollisionConfiguration(const btDiscreteDynamicsWorld *world, const btDefaultCollisionConstructionInfo &constructionInfo) :
-		btDefaultCollisionConfiguration(constructionInfo) {
-	void *mem = nullptr;
-
-	mem = btAlignedAlloc(sizeof(GodotRayWorldAlgorithm::CreateFunc), 16);
-	m_rayWorldCF = new (mem) GodotRayWorldAlgorithm::CreateFunc(world);
-
-	mem = btAlignedAlloc(sizeof(GodotRayWorldAlgorithm::SwappedCreateFunc), 16);
-	m_swappedRayWorldCF = new (mem) GodotRayWorldAlgorithm::SwappedCreateFunc(world);
-}
-
-GodotCollisionConfiguration::~GodotCollisionConfiguration() {
-	m_rayWorldCF->~btCollisionAlgorithmCreateFunc();
-	btAlignedFree(m_rayWorldCF);
-
-	m_swappedRayWorldCF->~btCollisionAlgorithmCreateFunc();
-	btAlignedFree(m_swappedRayWorldCF);
-}
-
-btCollisionAlgorithmCreateFunc *GodotCollisionConfiguration::getCollisionAlgorithmCreateFunc(int proxyType0, int proxyType1) {
-	if (CUSTOM_CONVEX_SHAPE_TYPE == proxyType0 && CUSTOM_CONVEX_SHAPE_TYPE == proxyType1) {
-		// This collision is not supported
-		return m_emptyCreateFunc;
-	} else if (CUSTOM_CONVEX_SHAPE_TYPE == proxyType0) {
-		return m_rayWorldCF;
-	} else if (CUSTOM_CONVEX_SHAPE_TYPE == proxyType1) {
-		return m_swappedRayWorldCF;
-	} else {
-		return btDefaultCollisionConfiguration::getCollisionAlgorithmCreateFunc(proxyType0, proxyType1);
-	}
-}
-
-btCollisionAlgorithmCreateFunc *GodotCollisionConfiguration::getClosestPointsAlgorithmCreateFunc(int proxyType0, int proxyType1) {
-	if (CUSTOM_CONVEX_SHAPE_TYPE == proxyType0 && CUSTOM_CONVEX_SHAPE_TYPE == proxyType1) {
-		// This collision is not supported
-		return m_emptyCreateFunc;
-	} else if (CUSTOM_CONVEX_SHAPE_TYPE == proxyType0) {
-		return m_rayWorldCF;
-	} else if (CUSTOM_CONVEX_SHAPE_TYPE == proxyType1) {
-		return m_swappedRayWorldCF;
-	} else {
-		return btDefaultCollisionConfiguration::getClosestPointsAlgorithmCreateFunc(proxyType0, proxyType1);
-	}
-}
-
-GodotSoftCollisionConfiguration::GodotSoftCollisionConfiguration(const btDiscreteDynamicsWorld *world, const btDefaultCollisionConstructionInfo &constructionInfo) :
-		btSoftBodyRigidBodyCollisionConfiguration(constructionInfo) {
-	void *mem = nullptr;
-
-	mem = btAlignedAlloc(sizeof(GodotRayWorldAlgorithm::CreateFunc), 16);
-	m_rayWorldCF = new (mem) GodotRayWorldAlgorithm::CreateFunc(world);
-
-	mem = btAlignedAlloc(sizeof(GodotRayWorldAlgorithm::SwappedCreateFunc), 16);
-	m_swappedRayWorldCF = new (mem) GodotRayWorldAlgorithm::SwappedCreateFunc(world);
-}
-
-GodotSoftCollisionConfiguration::~GodotSoftCollisionConfiguration() {
-	m_rayWorldCF->~btCollisionAlgorithmCreateFunc();
-	btAlignedFree(m_rayWorldCF);
-
-	m_swappedRayWorldCF->~btCollisionAlgorithmCreateFunc();
-	btAlignedFree(m_swappedRayWorldCF);
-}
-
-btCollisionAlgorithmCreateFunc *GodotSoftCollisionConfiguration::getCollisionAlgorithmCreateFunc(int proxyType0, int proxyType1) {
-	if (CUSTOM_CONVEX_SHAPE_TYPE == proxyType0 && CUSTOM_CONVEX_SHAPE_TYPE == proxyType1) {
-		// This collision is not supported
-		return m_emptyCreateFunc;
-	} else if (CUSTOM_CONVEX_SHAPE_TYPE == proxyType0) {
-		return m_rayWorldCF;
-	} else if (CUSTOM_CONVEX_SHAPE_TYPE == proxyType1) {
-		return m_swappedRayWorldCF;
-	} else {
-		return btSoftBodyRigidBodyCollisionConfiguration::getCollisionAlgorithmCreateFunc(proxyType0, proxyType1);
-	}
-}
-
-btCollisionAlgorithmCreateFunc *GodotSoftCollisionConfiguration::getClosestPointsAlgorithmCreateFunc(int proxyType0, int proxyType1) {
-	if (CUSTOM_CONVEX_SHAPE_TYPE == proxyType0 && CUSTOM_CONVEX_SHAPE_TYPE == proxyType1) {
-		// This collision is not supported
-		return m_emptyCreateFunc;
-	} else if (CUSTOM_CONVEX_SHAPE_TYPE == proxyType0) {
-		return m_rayWorldCF;
-	} else if (CUSTOM_CONVEX_SHAPE_TYPE == proxyType1) {
-		return m_swappedRayWorldCF;
-	} else {
-		return btSoftBodyRigidBodyCollisionConfiguration::getClosestPointsAlgorithmCreateFunc(proxyType0, proxyType1);
-	}
-}

modules/bullet/godot_collision_configuration.h

@@ -1,63 +0,0 @@
-/*************************************************************************/
-/*  godot_collision_configuration.h                                      */
-/*************************************************************************/
-/*                       This file is part of:                           */
-/*                           GODOT ENGINE                                */
-/*                      https://godotengine.org                          */
-/*************************************************************************/
-/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur.                 */
-/* Copyright (c) 2014-2022 Godot Engine contributors (cf. AUTHORS.md).   */
-/*                                                                       */
-/* Permission is hereby granted, free of charge, to any person obtaining */
-/* a copy of this software and associated documentation files (the       */
-/* "Software"), to deal in the Software without restriction, including   */
-/* without limitation the rights to use, copy, modify, merge, publish,   */
-/* distribute, sublicense, and/or sell copies of the Software, and to    */
-/* permit persons to whom the Software is furnished to do so, subject to */
-/* the following conditions:                                             */
-/*                                                                       */
-/* The above copyright notice and this permission notice shall be        */
-/* included in all copies or substantial portions of the Software.       */
-/*                                                                       */
-/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
-/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
-/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
-/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
-/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION 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 GODOT_COLLISION_CONFIGURATION_H
-#define GODOT_COLLISION_CONFIGURATION_H
-
-#include <BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h>
-#include <BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h>
-
-class btDiscreteDynamicsWorld;
-
-class GodotCollisionConfiguration : public btDefaultCollisionConfiguration {
-	btCollisionAlgorithmCreateFunc *m_rayWorldCF;
-	btCollisionAlgorithmCreateFunc *m_swappedRayWorldCF;
-
-public:
-	GodotCollisionConfiguration(const btDiscreteDynamicsWorld *world, const btDefaultCollisionConstructionInfo &constructionInfo = btDefaultCollisionConstructionInfo());
-	virtual ~GodotCollisionConfiguration();
-
-	virtual btCollisionAlgorithmCreateFunc *getCollisionAlgorithmCreateFunc(int proxyType0, int proxyType1);
-	virtual btCollisionAlgorithmCreateFunc *getClosestPointsAlgorithmCreateFunc(int proxyType0, int proxyType1);
-};
-
-class GodotSoftCollisionConfiguration : public btSoftBodyRigidBodyCollisionConfiguration {
-	btCollisionAlgorithmCreateFunc *m_rayWorldCF;
-	btCollisionAlgorithmCreateFunc *m_swappedRayWorldCF;
-
-public:
-	GodotSoftCollisionConfiguration(const btDiscreteDynamicsWorld *world, const btDefaultCollisionConstructionInfo &constructionInfo = btDefaultCollisionConstructionInfo());
-	virtual ~GodotSoftCollisionConfiguration();
-
-	virtual btCollisionAlgorithmCreateFunc *getCollisionAlgorithmCreateFunc(int proxyType0, int proxyType1);
-	virtual btCollisionAlgorithmCreateFunc *getClosestPointsAlgorithmCreateFunc(int proxyType0, int proxyType1);
-};
-
-#endif // GODOT_COLLISION_CONFIGURATION_H

modules/bullet/godot_collision_dispatcher.cpp

@@ -1,54 +0,0 @@
-/*************************************************************************/
-/*  godot_collision_dispatcher.cpp                                       */
-/*************************************************************************/
-/*                       This file is part of:                           */
-/*                           GODOT ENGINE                                */
-/*                      https://godotengine.org                          */
-/*************************************************************************/
-/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur.                 */
-/* Copyright (c) 2014-2022 Godot Engine contributors (cf. AUTHORS.md).   */
-/*                                                                       */
-/* Permission is hereby granted, free of charge, to any person obtaining */
-/* a copy of this software and associated documentation files (the       */
-/* "Software"), to deal in the Software without restriction, including   */
-/* without limitation the rights to use, copy, modify, merge, publish,   */
-/* distribute, sublicense, and/or sell copies of the Software, and to    */
-/* permit persons to whom the Software is furnished to do so, subject to */
-/* the following conditions:                                             */
-/*                                                                       */
-/* The above copyright notice and this permission notice shall be        */
-/* included in all copies or substantial portions of the Software.       */
-/*                                                                       */
-/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
-/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
-/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
-/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
-/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION 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 "godot_collision_dispatcher.h"
-
-#include "collision_object_bullet.h"
-
-const int GodotCollisionDispatcher::CASTED_TYPE_AREA = static_cast<int>(CollisionObjectBullet::TYPE_AREA);
-
-GodotCollisionDispatcher::GodotCollisionDispatcher(btCollisionConfiguration *collisionConfiguration) :
-		btCollisionDispatcher(collisionConfiguration) {}
-
-bool GodotCollisionDispatcher::needsCollision(const btCollisionObject *body0, const btCollisionObject *body1) {
-	if (body0->getUserIndex() == CASTED_TYPE_AREA || body1->getUserIndex() == CASTED_TYPE_AREA) {
-		// Avoid area narrow phase
-		return false;
-	}
-	return btCollisionDispatcher::needsCollision(body0, body1);
-}
-
-bool GodotCollisionDispatcher::needsResponse(const btCollisionObject *body0, const btCollisionObject *body1) {
-	if (body0->getUserIndex() == CASTED_TYPE_AREA || body1->getUserIndex() == CASTED_TYPE_AREA) {
-		// Avoid area narrow phase
-		return false;
-	}
-	return btCollisionDispatcher::needsResponse(body0, body1);
-}

modules/bullet/godot_collision_dispatcher.h

@@ -1,47 +0,0 @@
-/*************************************************************************/
-/*  godot_collision_dispatcher.h                                         */
-/*************************************************************************/
-/*                       This file is part of:                           */
-/*                           GODOT ENGINE                                */
-/*                      https://godotengine.org                          */
-/*************************************************************************/
-/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur.                 */
-/* Copyright (c) 2014-2022 Godot Engine contributors (cf. AUTHORS.md).   */
-/*                                                                       */
-/* Permission is hereby granted, free of charge, to any person obtaining */
-/* a copy of this software and associated documentation files (the       */
-/* "Software"), to deal in the Software without restriction, including   */
-/* without limitation the rights to use, copy, modify, merge, publish,   */
-/* distribute, sublicense, and/or sell copies of the Software, and to    */
-/* permit persons to whom the Software is furnished to do so, subject to */
-/* the following conditions:                                             */
-/*                                                                       */
-/* The above copyright notice and this permission notice shall be        */
-/* included in all copies or substantial portions of the Software.       */
-/*                                                                       */
-/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
-/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
-/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
-/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
-/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION 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 GODOT_COLLISION_DISPATCHER_H
-#define GODOT_COLLISION_DISPATCHER_H
-
-#include <btBulletDynamicsCommon.h>
-
-/// This class is required to implement custom collision behaviour in the narrowphase
-class GodotCollisionDispatcher : public btCollisionDispatcher {
-private:
-	static const int CASTED_TYPE_AREA;
-
-public:
-	GodotCollisionDispatcher(btCollisionConfiguration *collisionConfiguration);
-	virtual bool needsCollision(const btCollisionObject *body0, const btCollisionObject *body1);
-	virtual bool needsResponse(const btCollisionObject *body0, const btCollisionObject *body1);
-};
-
-#endif // GODOT_COLLISION_DISPATCHER_H

modules/bullet/godot_motion_state.h

@@ -1,96 +0,0 @@
-/*************************************************************************/
-/*  godot_motion_state.h                                                 */
-/*************************************************************************/
-/*                       This file is part of:                           */
-/*                           GODOT ENGINE                                */
-/*                      https://godotengine.org                          */
-/*************************************************************************/
-/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur.                 */
-/* Copyright (c) 2014-2022 Godot Engine contributors (cf. AUTHORS.md).   */
-/*                                                                       */
-/* Permission is hereby granted, free of charge, to any person obtaining */
-/* a copy of this software and associated documentation files (the       */
-/* "Software"), to deal in the Software without restriction, including   */
-/* without limitation the rights to use, copy, modify, merge, publish,   */
-/* distribute, sublicense, and/or sell copies of the Software, and to    */
-/* permit persons to whom the Software is furnished to do so, subject to */
-/* the following conditions:                                             */
-/*                                                                       */
-/* The above copyright notice and this permission notice shall be        */
-/* included in all copies or substantial portions of the Software.       */
-/*                                                                       */
-/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
-/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
-/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
-/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
-/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION 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 GODOT_MOTION_STATE_H
-#define GODOT_MOTION_STATE_H
-
-#include "rigid_body_bullet.h"
-
-#include <LinearMath/btMotionState.h>
-
-class RigidBodyBullet;
-
-// This class is responsible to move kinematic actor
-// and sincronize rendering engine with Bullet
-/// DOC:
-/// http://www.bulletphysics.org/mediawiki-1.5.8/index.php/MotionStates#What.27s_a_MotionState.3F
-class GodotMotionState : public btMotionState {
-	/// This data is used to store the new world position for kinematic body
-	btTransform bodyKinematicWorldTransf;
-	/// This data is used to store last world position
-	btTransform bodyCurrentWorldTransform;
-
-	RigidBodyBullet *owner = nullptr;
-
-public:
-	GodotMotionState(RigidBodyBullet *p_owner) :
-			bodyKinematicWorldTransf(btMatrix3x3(1., 0., 0., 0., 1., 0., 0., 0., 1.), btVector3(0., 0., 0.)),
-			bodyCurrentWorldTransform(btMatrix3x3(1., 0., 0., 0., 1., 0., 0., 0., 1.), btVector3(0., 0., 0.)),
-			owner(p_owner) {}
-
-	/// IMPORTANT DON'T USE THIS FUNCTION TO KNOW THE CURRENT BODY TRANSFORM
-	/// This class is used internally by Bullet
-	/// Use GodotMotionState::getCurrentWorldTransform to know current position
-	///
-	/// This function is used by Bullet to get the position of object in the world
-	/// if the body is kinematic Bullet will move the object to this location
-	/// if the body is static Bullet doesn't move at all
-	virtual void getWorldTransform(btTransform &worldTrans) const {
-		worldTrans = bodyKinematicWorldTransf;
-	}
-
-	/// IMPORTANT: to move the body use: moveBody
-	/// IMPORTANT: DON'T CALL THIS FUNCTION, IT IS CALLED BY BULLET TO UPDATE RENDERING ENGINE
-	///
-	/// This function is called each time by Bullet and set the current position of body
-	/// inside the physics world.
-	/// Don't allow Godot rendering scene takes world transform from this object because
-	/// the correct transform is set by Bullet only after the last step when there are sub steps
-	/// This function must update Godot transform rendering scene for this object.
-	virtual void setWorldTransform(const btTransform &worldTrans) {
-		bodyCurrentWorldTransform = worldTrans;
-
-		owner->notify_transform_changed();
-	}
-
-public:
-	/// Use this function to move kinematic body
-	/// -- or set initial transform before body creation.
-	void moveBody(const btTransform &newWorldTransform) {
-		bodyKinematicWorldTransf = newWorldTransform;
-	}
-
-	/// It returns the current body transform from last Bullet update
-	const btTransform &getCurrentWorldTransform() const {
-		return bodyCurrentWorldTransform;
-	}
-};
-
-#endif // GODOT_MOTION_STATE_H

modules/bullet/godot_ray_world_algorithm.cpp

@@ -1,111 +0,0 @@
-/*************************************************************************/
-/*  godot_ray_world_algorithm.cpp                                        */
-/*************************************************************************/
-/*                       This file is part of:                           */
-/*                           GODOT ENGINE                                */
-/*                      https://godotengine.org                          */
-/*************************************************************************/
-/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur.                 */
-/* Copyright (c) 2014-2022 Godot Engine contributors (cf. AUTHORS.md).   */
-/*                                                                       */
-/* Permission is hereby granted, free of charge, to any person obtaining */
-/* a copy of this software and associated documentation files (the       */
-/* "Software"), to deal in the Software without restriction, including   */
-/* without limitation the rights to use, copy, modify, merge, publish,   */
-/* distribute, sublicense, and/or sell copies of the Software, and to    */
-/* permit persons to whom the Software is furnished to do so, subject to */
-/* the following conditions:                                             */
-/*                                                                       */
-/* The above copyright notice and this permission notice shall be        */
-/* included in all copies or substantial portions of the Software.       */
-/*                                                                       */
-/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
-/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
-/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
-/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
-/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION 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 "godot_ray_world_algorithm.h"
-
-#include "btRayShape.h"
-#include "collision_object_bullet.h"
-
-#include <BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h>
-
-// Epsilon to account for floating point inaccuracies
-#define RAY_PENETRATION_DEPTH_EPSILON 0.01
-
-GodotRayWorldAlgorithm::CreateFunc::CreateFunc(const btDiscreteDynamicsWorld *world) :
-		m_world(world) {}
-
-GodotRayWorldAlgorithm::SwappedCreateFunc::SwappedCreateFunc(const btDiscreteDynamicsWorld *world) :
-		m_world(world) {}
-
-GodotRayWorldAlgorithm::GodotRayWorldAlgorithm(const btDiscreteDynamicsWorld *world, btPersistentManifold *mf, const btCollisionAlgorithmConstructionInfo &ci, const btCollisionObjectWrapper *body0Wrap, const btCollisionObjectWrapper *body1Wrap, bool isSwapped) :
-		btActivatingCollisionAlgorithm(ci, body0Wrap, body1Wrap),
-		m_world(world),
-		m_manifoldPtr(mf),
-		m_isSwapped(isSwapped) {}
-
-GodotRayWorldAlgorithm::~GodotRayWorldAlgorithm() {
-	if (m_ownManifold && m_manifoldPtr) {
-		m_dispatcher->releaseManifold(m_manifoldPtr);
-	}
-}
-
-void GodotRayWorldAlgorithm::processCollision(const btCollisionObjectWrapper *body0Wrap, const btCollisionObjectWrapper *body1Wrap, const btDispatcherInfo &dispatchInfo, btManifoldResult *resultOut) {
-	if (!m_manifoldPtr) {
-		if (m_isSwapped) {
-			m_manifoldPtr = m_dispatcher->getNewManifold(body1Wrap->getCollisionObject(), body0Wrap->getCollisionObject());
-		} else {
-			m_manifoldPtr = m_dispatcher->getNewManifold(body0Wrap->getCollisionObject(), body1Wrap->getCollisionObject());
-		}
-		m_ownManifold = true;
-	}
-	m_manifoldPtr->clearManifold();
-	resultOut->setPersistentManifold(m_manifoldPtr);
-
-	const btRayShape *ray_shape;
-	btTransform ray_transform;
-
-	const btCollisionObjectWrapper *other_co_wrapper;
-
-	if (m_isSwapped) {
-		ray_shape = static_cast<const btRayShape *>(body1Wrap->getCollisionShape());
-		ray_transform = body1Wrap->getWorldTransform();
-
-		other_co_wrapper = body0Wrap;
-	} else {
-		ray_shape = static_cast<const btRayShape *>(body0Wrap->getCollisionShape());
-		ray_transform = body0Wrap->getWorldTransform();
-
-		other_co_wrapper = body1Wrap;
-	}
-
-	btTransform to(ray_transform * ray_shape->getSupportPoint());
-
-	btCollisionWorld::ClosestRayResultCallback btResult(ray_transform.getOrigin(), to.getOrigin());
-
-	m_world->rayTestSingleInternal(ray_transform, to, other_co_wrapper, btResult);
-
-	if (btResult.hasHit()) {
-		btScalar depth(ray_shape->getScaledLength() * (btResult.m_closestHitFraction - 1));
-
-		if (depth > -RAY_PENETRATION_DEPTH_EPSILON) {
-			depth = 0.0;
-		}
-
-		if (ray_shape->getSlipsOnSlope()) {
-			resultOut->addContactPoint(btResult.m_hitNormalWorld, btResult.m_hitPointWorld, depth);
-		} else {
-			resultOut->addContactPoint((ray_transform.getOrigin() - to.getOrigin()).normalize(), btResult.m_hitPointWorld, depth);
-		}
-	}
-}
-
-btScalar GodotRayWorldAlgorithm::calculateTimeOfImpact(btCollisionObject *body0, btCollisionObject *body1, const btDispatcherInfo &dispatchInfo, btManifoldResult *resultOut) {
-	return 1;
-}

modules/bullet/godot_ray_world_algorithm.h

@@ -1,80 +0,0 @@
-/*************************************************************************/
-/*  godot_ray_world_algorithm.h                                          */
-/*************************************************************************/
-/*                       This file is part of:                           */
-/*                           GODOT ENGINE                                */
-/*                      https://godotengine.org                          */
-/*************************************************************************/
-/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur.                 */
-/* Copyright (c) 2014-2022 Godot Engine contributors (cf. AUTHORS.md).   */
-/*                                                                       */
-/* Permission is hereby granted, free of charge, to any person obtaining */
-/* a copy of this software and associated documentation files (the       */
-/* "Software"), to deal in the Software without restriction, including   */
-/* without limitation the rights to use, copy, modify, merge, publish,   */
-/* distribute, sublicense, and/or sell copies of the Software, and to    */
-/* permit persons to whom the Software is furnished to do so, subject to */
-/* the following conditions:                                             */
-/*                                                                       */
-/* The above copyright notice and this permission notice shall be        */
-/* included in all copies or substantial portions of the Software.       */
-/*                                                                       */
-/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
-/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
-/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
-/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
-/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION 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 GODOT_RAY_WORLD_ALGORITHM_H
-#define GODOT_RAY_WORLD_ALGORITHM_H
-
-#include <BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.h>
-#include <BulletCollision/CollisionDispatch/btCollisionCreateFunc.h>
-#include <BulletCollision/CollisionDispatch/btCollisionDispatcher.h>
-
-class btDiscreteDynamicsWorld;
-
-class GodotRayWorldAlgorithm : public btActivatingCollisionAlgorithm {
-	const btDiscreteDynamicsWorld *m_world;
-	btPersistentManifold *m_manifoldPtr;
-	bool m_ownManifold = false;
-	bool m_isSwapped = false;
-
-public:
-	GodotRayWorldAlgorithm(const btDiscreteDynamicsWorld *world, btPersistentManifold *mf, const btCollisionAlgorithmConstructionInfo &ci, const btCollisionObjectWrapper *body0Wrap, const btCollisionObjectWrapper *body1Wrap, bool isSwapped);
-	virtual ~GodotRayWorldAlgorithm();
-
-	virtual void processCollision(const btCollisionObjectWrapper *body0Wrap, const btCollisionObjectWrapper *body1Wrap, const btDispatcherInfo &dispatchInfo, btManifoldResult *resultOut);
-	virtual btScalar calculateTimeOfImpact(btCollisionObject *body0, btCollisionObject *body1, const btDispatcherInfo &dispatchInfo, btManifoldResult *resultOut);
-
-	virtual void getAllContactManifolds(btManifoldArray &manifoldArray) {
-		///should we use m_ownManifold to avoid adding duplicates?
-		if (m_manifoldPtr && m_ownManifold) {
-			manifoldArray.push_back(m_manifoldPtr);
-		}
-	}
-	struct CreateFunc : public btCollisionAlgorithmCreateFunc {
-		const btDiscreteDynamicsWorld *m_world;
-		CreateFunc(const btDiscreteDynamicsWorld *world);
-
-		virtual btCollisionAlgorithm *CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo &ci, const btCollisionObjectWrapper *body0Wrap, const btCollisionObjectWrapper *body1Wrap) {
-			void *mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(GodotRayWorldAlgorithm));
-			return new (mem) GodotRayWorldAlgorithm(m_world, ci.m_manifold, ci, body0Wrap, body1Wrap, false);
-		}
-	};
-
-	struct SwappedCreateFunc : public btCollisionAlgorithmCreateFunc {
-		const btDiscreteDynamicsWorld *m_world;
-		SwappedCreateFunc(const btDiscreteDynamicsWorld *world);
-
-		virtual btCollisionAlgorithm *CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo &ci, const btCollisionObjectWrapper *body0Wrap, const btCollisionObjectWrapper *body1Wrap) {
-			void *mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(GodotRayWorldAlgorithm));
-			return new (mem) GodotRayWorldAlgorithm(m_world, ci.m_manifold, ci, body0Wrap, body1Wrap, true);
-		}
-	};
-};
-
-#endif // GODOT_RAY_WORLD_ALGORITHM_H

modules/bullet/godot_result_callbacks.cpp

@@ -1,377 +0,0 @@
-/*************************************************************************/
-/*  godot_result_callbacks.cpp                                           */
-/*************************************************************************/
-/*                       This file is part of:                           */
-/*                           GODOT ENGINE                                */
-/*                      https://godotengine.org                          */
-/*************************************************************************/
-/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur.                 */
-/* Copyright (c) 2014-2022 Godot Engine contributors (cf. AUTHORS.md).   */
-/*                                                                       */
-/* Permission is hereby granted, free of charge, to any person obtaining */
-/* a copy of this software and associated documentation files (the       */
-/* "Software"), to deal in the Software without restriction, including   */
-/* without limitation the rights to use, copy, modify, merge, publish,   */
-/* distribute, sublicense, and/or sell copies of the Software, and to    */
-/* permit persons to whom the Software is furnished to do so, subject to */
-/* the following conditions:                                             */
-/*                                                                       */
-/* The above copyright notice and this permission notice shall be        */
-/* included in all copies or substantial portions of the Software.       */
-/*                                                                       */
-/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
-/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
-/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
-/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
-/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION 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 "godot_result_callbacks.h"
-
-#include "area_bullet.h"
-#include "bullet_types_converter.h"
-#include "collision_object_bullet.h"
-#include "rigid_body_bullet.h"
-
-#include <BulletCollision/CollisionDispatch/btInternalEdgeUtility.h>
-
-bool godotContactAddedCallback(btManifoldPoint &cp, const btCollisionObjectWrapper *colObj0Wrap, int partId0, int index0, const btCollisionObjectWrapper *colObj1Wrap, int partId1, int index1) {
-	if (!colObj1Wrap->getCollisionObject()->getCollisionShape()->isCompound()) {
-		btAdjustInternalEdgeContacts(cp, colObj1Wrap, colObj0Wrap, partId1, index1);
-	}
-	return true;
-}
-
-bool GodotFilterCallback::needBroadphaseCollision(btBroadphaseProxy *proxy0, btBroadphaseProxy *proxy1) const {
-	return (proxy0->m_collisionFilterGroup & proxy1->m_collisionFilterMask) || (proxy1->m_collisionFilterGroup & proxy0->m_collisionFilterMask);
-}
-
-bool GodotClosestRayResultCallback::needsCollision(btBroadphaseProxy *proxy0) const {
-	if (proxy0->m_collisionFilterGroup & m_collisionFilterMask) {
-		btCollisionObject *btObj = static_cast<btCollisionObject *>(proxy0->m_clientObject);
-		CollisionObjectBullet *gObj = static_cast<CollisionObjectBullet *>(btObj->getUserPointer());
-
-		if (CollisionObjectBullet::TYPE_AREA == gObj->getType()) {
-			if (!collide_with_areas) {
-				return false;
-			}
-		} else {
-			if (!collide_with_bodies) {
-				return false;
-			}
-		}
-
-		if (m_pickRay && !gObj->is_ray_pickable()) {
-			return false;
-		}
-
-		if (m_exclude->has(gObj->get_self())) {
-			return false;
-		}
-
-		return true;
-	} else {
-		return false;
-	}
-}
-
-bool GodotAllConvexResultCallback::needsCollision(btBroadphaseProxy *proxy0) const {
-	if (count >= m_resultMax) {
-		return false;
-	}
-
-	if (proxy0->m_collisionFilterGroup & m_collisionFilterMask) {
-		btCollisionObject *btObj = static_cast<btCollisionObject *>(proxy0->m_clientObject);
-		CollisionObjectBullet *gObj = static_cast<CollisionObjectBullet *>(btObj->getUserPointer());
-		if (m_exclude->has(gObj->get_self())) {
-			return false;
-		}
-
-		return true;
-	} else {
-		return false;
-	}
-}
-
-btScalar GodotAllConvexResultCallback::addSingleResult(btCollisionWorld::LocalConvexResult &convexResult, bool normalInWorldSpace) {
-	if (count >= m_resultMax) {
-		return 1; // not used by bullet
-	}
-
-	CollisionObjectBullet *gObj = static_cast<CollisionObjectBullet *>(convexResult.m_hitCollisionObject->getUserPointer());
-
-	PhysicsDirectSpaceState3D::ShapeResult &result = m_results[count];
-
-	// Triangle index is an odd name but contains the compound shape ID.
-	// A shape part of -1 indicates the index is a shape index and not a triangle index.
-	if (convexResult.m_localShapeInfo && convexResult.m_localShapeInfo->m_shapePart == -1) {
-		result.shape = convexResult.m_localShapeInfo->m_triangleIndex;
-	} else {
-		result.shape = 0;
-	}
-
-	result.rid = gObj->get_self();
-	result.collider_id = gObj->get_instance_id();
-	result.collider = result.collider_id.is_null() ? nullptr : ObjectDB::get_instance(result.collider_id);
-
-	++count;
-	return 1; // not used by bullet
-}
-
-bool GodotKinClosestConvexResultCallback::needsCollision(btBroadphaseProxy *proxy0) const {
-	if (proxy0->m_collisionFilterGroup & m_collisionFilterMask) {
-		btCollisionObject *btObj = static_cast<btCollisionObject *>(proxy0->m_clientObject);
-		CollisionObjectBullet *gObj = static_cast<CollisionObjectBullet *>(btObj->getUserPointer());
-		if (gObj == m_self_object) {
-			return false;
-		} else {
-			// A kinematic body can't be stopped by a rigid body since the mass of kinematic body is infinite
-			if (m_infinite_inertia && !btObj->isStaticOrKinematicObject()) {
-				return false;
-			}
-
-			if (gObj->getType() == CollisionObjectBullet::TYPE_AREA) {
-				return false;
-			}
-
-			if (m_self_object->has_collision_exception(gObj) || gObj->has_collision_exception(m_self_object)) {
-				return false;
-			}
-
-			if (m_exclude->has(gObj->get_self())) {
-				return false;
-			}
-		}
-		return true;
-	} else {
-		return false;
-	}
-}
-
-bool GodotClosestConvexResultCallback::needsCollision(btBroadphaseProxy *proxy0) const {
-	if (proxy0->m_collisionFilterGroup & m_collisionFilterMask) {
-		btCollisionObject *btObj = static_cast<btCollisionObject *>(proxy0->m_clientObject);
-		CollisionObjectBullet *gObj = static_cast<CollisionObjectBullet *>(btObj->getUserPointer());
-
-		if (CollisionObjectBullet::TYPE_AREA == gObj->getType()) {
-			if (!collide_with_areas) {
-				return false;
-			}
-		} else {
-			if (!collide_with_bodies) {
-				return false;
-			}
-		}
-
-		if (m_exclude->has(gObj->get_self())) {
-			return false;
-		}
-		return true;
-	} else {
-		return false;
-	}
-}
-
-btScalar GodotClosestConvexResultCallback::addSingleResult(btCollisionWorld::LocalConvexResult &convexResult, bool normalInWorldSpace) {
-	// Triangle index is an odd name but contains the compound shape ID.
-	// A shape part of -1 indicates the index is a shape index and not a triangle index.
-	if (convexResult.m_localShapeInfo && convexResult.m_localShapeInfo->m_shapePart == -1) {
-		m_shapeId = convexResult.m_localShapeInfo->m_triangleIndex;
-	} else {
-		m_shapeId = 0;
-	}
-
-	return btCollisionWorld::ClosestConvexResultCallback::addSingleResult(convexResult, normalInWorldSpace);
-}
-
-bool GodotAllContactResultCallback::needsCollision(btBroadphaseProxy *proxy0) const {
-	if (m_count >= m_resultMax) {
-		return false;
-	}
-
-	if (proxy0->m_collisionFilterGroup & m_collisionFilterMask) {
-		btCollisionObject *btObj = static_cast<btCollisionObject *>(proxy0->m_clientObject);
-		CollisionObjectBullet *gObj = static_cast<CollisionObjectBullet *>(btObj->getUserPointer());
-
-		if (CollisionObjectBullet::TYPE_AREA == gObj->getType()) {
-			if (!collide_with_areas) {
-				return false;
-			}
-		} else {
-			if (!collide_with_bodies) {
-				return false;
-			}
-		}
-
-		if (m_exclude->has(gObj->get_self())) {
-			return false;
-		}
-		return true;
-	} else {
-		return false;
-	}
-}
-
-btScalar GodotAllContactResultCallback::addSingleResult(btManifoldPoint &cp, const btCollisionObjectWrapper *colObj0Wrap, int partId0, int index0, const btCollisionObjectWrapper *colObj1Wrap, int partId1, int index1) {
-	if (m_count >= m_resultMax) {
-		return cp.getDistance();
-	}
-
-	if (cp.getDistance() <= 0) {
-		PhysicsDirectSpaceState3D::ShapeResult &result = m_results[m_count];
-		// Penetrated
-
-		CollisionObjectBullet *colObj;
-		if (m_self_object == colObj0Wrap->getCollisionObject()) {
-			colObj = static_cast<CollisionObjectBullet *>(colObj1Wrap->getCollisionObject()->getUserPointer());
-			// Checking for compound shape because the index might be uninitialized otherwise.
-			// A partId of -1 indicates the index is a shape index and not a triangle index.
-			if (colObj1Wrap->getCollisionObject()->getCollisionShape()->isCompound() && cp.m_partId1 == -1) {
-				result.shape = cp.m_index1;
-			} else {
-				result.shape = 0;
-			}
-		} else {
-			colObj = static_cast<CollisionObjectBullet *>(colObj0Wrap->getCollisionObject()->getUserPointer());
-			// Checking for compound shape because the index might be uninitialized otherwise.
-			// A partId of -1 indicates the index is a shape index and not a triangle index.
-			if (colObj0Wrap->getCollisionObject()->getCollisionShape()->isCompound() && cp.m_partId0 == -1) {
-				result.shape = cp.m_index0;
-			} else {
-				result.shape = 0;
-			}
-		}
-
-		result.collider_id = colObj->get_instance_id();
-		result.collider = result.collider_id.is_null() ? nullptr : ObjectDB::get_instance(result.collider_id);
-		result.rid = colObj->get_self();
-		++m_count;
-	}
-
-	return cp.getDistance();
-}
-
-bool GodotContactPairContactResultCallback::needsCollision(btBroadphaseProxy *proxy0) const {
-	if (m_count >= m_resultMax) {
-		return false;
-	}
-
-	if (proxy0->m_collisionFilterGroup & m_collisionFilterMask) {
-		btCollisionObject *btObj = static_cast<btCollisionObject *>(proxy0->m_clientObject);
-		CollisionObjectBullet *gObj = static_cast<CollisionObjectBullet *>(btObj->getUserPointer());
-
-		if (CollisionObjectBullet::TYPE_AREA == gObj->getType()) {
-			if (!collide_with_areas) {
-				return false;
-			}
-		} else {
-			if (!collide_with_bodies) {
-				return false;
-			}
-		}
-
-		if (m_exclude->has(gObj->get_self())) {
-			return false;
-		}
-		return true;
-	} else {
-		return false;
-	}
-}
-
-btScalar GodotContactPairContactResultCallback::addSingleResult(btManifoldPoint &cp, const btCollisionObjectWrapper *colObj0Wrap, int partId0, int index0, const btCollisionObjectWrapper *colObj1Wrap, int partId1, int index1) {
-	if (m_count >= m_resultMax) {
-		return 1; // not used by bullet
-	}
-
-	if (m_self_object == colObj0Wrap->getCollisionObject()) {
-		B_TO_G(cp.m_localPointA, m_results[m_count * 2 + 0]); // Local contact
-		B_TO_G(cp.m_localPointB, m_results[m_count * 2 + 1]);
-	} else {
-		B_TO_G(cp.m_localPointB, m_results[m_count * 2 + 0]); // Local contact
-		B_TO_G(cp.m_localPointA, m_results[m_count * 2 + 1]);
-	}
-
-	++m_count;
-
-	return 1; // Not used by bullet
-}
-
-bool GodotRestInfoContactResultCallback::needsCollision(btBroadphaseProxy *proxy0) const {
-	if (proxy0->m_collisionFilterGroup & m_collisionFilterMask) {
-		btCollisionObject *btObj = static_cast<btCollisionObject *>(proxy0->m_clientObject);
-		CollisionObjectBullet *gObj = static_cast<CollisionObjectBullet *>(btObj->getUserPointer());
-
-		if (CollisionObjectBullet::TYPE_AREA == gObj->getType()) {
-			if (!collide_with_areas) {
-				return false;
-			}
-		} else {
-			if (!collide_with_bodies) {
-				return false;
-			}
-		}
-
-		if (m_exclude->has(gObj->get_self())) {
-			return false;
-		}
-		return true;
-	} else {
-		return false;
-	}
-}
-
-btScalar GodotRestInfoContactResultCallback::addSingleResult(btManifoldPoint &cp, const btCollisionObjectWrapper *colObj0Wrap, int partId0, int index0, const btCollisionObjectWrapper *colObj1Wrap, int partId1, int index1) {
-	if (cp.getDistance() <= m_min_distance) {
-		m_min_distance = cp.getDistance();
-
-		CollisionObjectBullet *colObj;
-		if (m_self_object == colObj0Wrap->getCollisionObject()) {
-			colObj = static_cast<CollisionObjectBullet *>(colObj1Wrap->getCollisionObject()->getUserPointer());
-			// Checking for compound shape because the index might be uninitialized otherwise.
-			// A partId of -1 indicates the index is a shape index and not a triangle index.
-			if (colObj1Wrap->getCollisionObject()->getCollisionShape()->isCompound() && cp.m_partId1 == -1) {
-				m_result->shape = cp.m_index1;
-			} else {
-				m_result->shape = 0;
-			}
-			B_TO_G(cp.getPositionWorldOnB(), m_result->point);
-			B_TO_G(cp.m_normalWorldOnB, m_result->normal);
-			m_rest_info_bt_point = cp.getPositionWorldOnB();
-			m_rest_info_collision_object = colObj1Wrap->getCollisionObject();
-		} else {
-			colObj = static_cast<CollisionObjectBullet *>(colObj0Wrap->getCollisionObject()->getUserPointer());
-			// Checking for compound shape because the index might be uninitialized otherwise.
-			// A partId of -1 indicates the index is a shape index and not a triangle index.
-			if (colObj0Wrap->getCollisionObject()->getCollisionShape()->isCompound() && cp.m_partId0 == -1) {
-				m_result->shape = cp.m_index0;
-			} else {
-				m_result->shape = 0;
-			}
-			B_TO_G(cp.m_normalWorldOnB * -1, m_result->normal);
-			m_rest_info_bt_point = cp.getPositionWorldOnA();
-			m_rest_info_collision_object = colObj0Wrap->getCollisionObject();
-		}
-
-		m_result->collider_id = colObj->get_instance_id();
-		m_result->rid = colObj->get_self();
-
-		m_collided = true;
-	}
-
-	return 1; // Not used by bullet
-}
-
-void GodotDeepPenetrationContactResultCallback::addContactPoint(const btVector3 &normalOnBInWorld, const btVector3 &pointInWorldOnB, btScalar depth) {
-	if (m_penetration_distance > depth) { // Has penetration?
-
-		const bool isSwapped = m_manifoldPtr->getBody0() != m_body0Wrap->getCollisionObject();
-		m_penetration_distance = depth;
-		m_other_compound_shape_index = isSwapped ? m_index0 : m_index1;
-		m_pointWorld = isSwapped ? (pointInWorldOnB + (normalOnBInWorld * depth)) : pointInWorldOnB;
-
-		m_pointNormalWorld = isSwapped ? normalOnBInWorld * -1 : normalOnBInWorld;
-	}
-}

modules/bullet/godot_result_callbacks.h

@@ -1,225 +0,0 @@
-/*************************************************************************/
-/*  godot_result_callbacks.h                                             */
-/*************************************************************************/
-/*                       This file is part of:                           */
-/*                           GODOT ENGINE                                */
-/*                      https://godotengine.org                          */
-/*************************************************************************/
-/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur.                 */
-/* Copyright (c) 2014-2022 Godot Engine contributors (cf. AUTHORS.md).   */
-/*                                                                       */
-/* Permission is hereby granted, free of charge, to any person obtaining */
-/* a copy of this software and associated documentation files (the       */
-/* "Software"), to deal in the Software without restriction, including   */
-/* without limitation the rights to use, copy, modify, merge, publish,   */
-/* distribute, sublicense, and/or sell copies of the Software, and to    */
-/* permit persons to whom the Software is furnished to do so, subject to */
-/* the following conditions:                                             */
-/*                                                                       */
-/* The above copyright notice and this permission notice shall be        */
-/* included in all copies or substantial portions of the Software.       */
-/*                                                                       */
-/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
-/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
-/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
-/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
-/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION 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 GODOT_RESULT_CALLBACKS_H
-#define GODOT_RESULT_CALLBACKS_H
-
-#include "servers/physics_server_3d.h"
-
-#include <BulletCollision/BroadphaseCollision/btBroadphaseProxy.h>
-#include <btBulletDynamicsCommon.h>
-
-class RigidBodyBullet;
-
-/// This callback is injected inside bullet server and allow me to smooth contacts against trimesh
-bool godotContactAddedCallback(btManifoldPoint &cp, const btCollisionObjectWrapper *colObj0Wrap, int partId0, int index0, const btCollisionObjectWrapper *colObj1Wrap, int partId1, int index1);
-
-/// This class is required to implement custom collision behaviour in the broadphase
-struct GodotFilterCallback : public btOverlapFilterCallback {
-	// return true when pairs need collision
-	virtual bool needBroadphaseCollision(btBroadphaseProxy *proxy0, btBroadphaseProxy *proxy1) const;
-};
-
-/// It performs an additional check allow exclusions.
-struct GodotClosestRayResultCallback : public btCollisionWorld::ClosestRayResultCallback {
-	const Set<RID> *m_exclude;
-	bool m_pickRay = false;
-	int m_shapeId = 0;
-
-	bool collide_with_bodies = false;
-	bool collide_with_areas = false;
-
-public:
-	GodotClosestRayResultCallback(const btVector3 &rayFromWorld, const btVector3 &rayToWorld, const Set<RID> *p_exclude, bool p_collide_with_bodies, bool p_collide_with_areas) :
-			btCollisionWorld::ClosestRayResultCallback(rayFromWorld, rayToWorld),
-			m_exclude(p_exclude),
-			collide_with_bodies(p_collide_with_bodies),
-			collide_with_areas(p_collide_with_areas) {}
-
-	virtual bool needsCollision(btBroadphaseProxy *proxy0) const;
-
-	virtual btScalar addSingleResult(btCollisionWorld::LocalRayResult &rayResult, bool normalInWorldSpace) {
-		// Triangle index is an odd name but contains the compound shape ID.
-		// A shape part of -1 indicates the index is a shape index and not a triangle index.
-		if (rayResult.m_localShapeInfo && rayResult.m_localShapeInfo->m_shapePart == -1) {
-			m_shapeId = rayResult.m_localShapeInfo->m_triangleIndex;
-		} else {
-			m_shapeId = 0;
-		}
-		return btCollisionWorld::ClosestRayResultCallback::addSingleResult(rayResult, normalInWorldSpace);
-	}
-};
-
-// store all colliding object
-struct GodotAllConvexResultCallback : public btCollisionWorld::ConvexResultCallback {
-public:
-	PhysicsDirectSpaceState3D::ShapeResult *m_results = nullptr;
-	int m_resultMax = 0;
-	const Set<RID> *m_exclude;
-	int count = 0;
-
-	GodotAllConvexResultCallback(PhysicsDirectSpaceState3D::ShapeResult *p_results, int p_resultMax, const Set<RID> *p_exclude) :
-			m_results(p_results),
-			m_resultMax(p_resultMax),
-			m_exclude(p_exclude) {}
-
-	virtual bool needsCollision(btBroadphaseProxy *proxy0) const;
-
-	virtual btScalar addSingleResult(btCollisionWorld::LocalConvexResult &convexResult, bool normalInWorldSpace);
-};
-
-struct GodotKinClosestConvexResultCallback : public btCollisionWorld::ClosestConvexResultCallback {
-public:
-	const RigidBodyBullet *m_self_object;
-	const Set<RID> *m_exclude;
-	const bool m_infinite_inertia;
-
-	GodotKinClosestConvexResultCallback(const btVector3 &convexFromWorld, const btVector3 &convexToWorld, const RigidBodyBullet *p_self_object, bool p_infinite_inertia, const Set<RID> *p_exclude) :
-			btCollisionWorld::ClosestConvexResultCallback(convexFromWorld, convexToWorld),
-			m_self_object(p_self_object),
-			m_exclude(p_exclude),
-			m_infinite_inertia(p_infinite_inertia) {}
-
-	virtual bool needsCollision(btBroadphaseProxy *proxy0) const;
-};
-
-struct GodotClosestConvexResultCallback : public btCollisionWorld::ClosestConvexResultCallback {
-public:
-	const Set<RID> *m_exclude;
-	int m_shapeId = 0;
-
-	bool collide_with_bodies = false;
-	bool collide_with_areas = false;
-
-	GodotClosestConvexResultCallback(const btVector3 &convexFromWorld, const btVector3 &convexToWorld, const Set<RID> *p_exclude, bool p_collide_with_bodies, bool p_collide_with_areas) :
-			btCollisionWorld::ClosestConvexResultCallback(convexFromWorld, convexToWorld),
-			m_exclude(p_exclude),
-			collide_with_bodies(p_collide_with_bodies),
-			collide_with_areas(p_collide_with_areas) {}
-
-	virtual bool needsCollision(btBroadphaseProxy *proxy0) const;
-
-	virtual btScalar addSingleResult(btCollisionWorld::LocalConvexResult &convexResult, bool normalInWorldSpace);
-};
-
-struct GodotAllContactResultCallback : public btCollisionWorld::ContactResultCallback {
-public:
-	const btCollisionObject *m_self_object;
-	PhysicsDirectSpaceState3D::ShapeResult *m_results = nullptr;
-	int m_resultMax = 0;
-	const Set<RID> *m_exclude;
-	int m_count = 0;
-
-	bool collide_with_bodies = false;
-	bool collide_with_areas = false;
-
-	GodotAllContactResultCallback(btCollisionObject *p_self_object, PhysicsDirectSpaceState3D::ShapeResult *p_results, int p_resultMax, const Set<RID> *p_exclude, bool p_collide_with_bodies, bool p_collide_with_areas) :
-			m_self_object(p_self_object),
-			m_results(p_results),
-			m_resultMax(p_resultMax),
-			m_exclude(p_exclude),
-			collide_with_bodies(p_collide_with_bodies),
-			collide_with_areas(p_collide_with_areas) {}
-
-	virtual bool needsCollision(btBroadphaseProxy *proxy0) const;
-
-	virtual btScalar addSingleResult(btManifoldPoint &cp, const btCollisionObjectWrapper *colObj0Wrap, int partId0, int index0, const btCollisionObjectWrapper *colObj1Wrap, int partId1, int index1);
-};
-
-/// Returns the list of contacts pairs in this order: Local contact, other body contact
-struct GodotContactPairContactResultCallback : public btCollisionWorld::ContactResultCallback {
-public:
-	const btCollisionObject *m_self_object;
-	Vector3 *m_results = nullptr;
-	int m_resultMax = 0;
-	const Set<RID> *m_exclude;
-	int m_count = 0;
-
-	bool collide_with_bodies = false;
-	bool collide_with_areas = false;
-
-	GodotContactPairContactResultCallback(btCollisionObject *p_self_object, Vector3 *p_results, int p_resultMax, const Set<RID> *p_exclude, bool p_collide_with_bodies, bool p_collide_with_areas) :
-			m_self_object(p_self_object),
-			m_results(p_results),
-			m_resultMax(p_resultMax),
-			m_exclude(p_exclude),
-			collide_with_bodies(p_collide_with_bodies),
-			collide_with_areas(p_collide_with_areas) {}
-
-	virtual bool needsCollision(btBroadphaseProxy *proxy0) const;
-
-	virtual btScalar addSingleResult(btManifoldPoint &cp, const btCollisionObjectWrapper *colObj0Wrap, int partId0, int index0, const btCollisionObjectWrapper *colObj1Wrap, int partId1, int index1);
-};
-
-struct GodotRestInfoContactResultCallback : public btCollisionWorld::ContactResultCallback {
-public:
-	const btCollisionObject *m_self_object;
-	PhysicsDirectSpaceState3D::ShapeRestInfo *m_result = nullptr;
-	const Set<RID> *m_exclude;
-	bool m_collided = false;
-	real_t m_min_distance = 0.0;
-	const btCollisionObject *m_rest_info_collision_object = nullptr;
-	btVector3 m_rest_info_bt_point;
-	bool collide_with_bodies = false;
-	bool collide_with_areas = false;
-
-	GodotRestInfoContactResultCallback(btCollisionObject *p_self_object, PhysicsDirectSpaceState3D::ShapeRestInfo *p_result, const Set<RID> *p_exclude, bool p_collide_with_bodies, bool p_collide_with_areas) :
-			m_self_object(p_self_object),
-			m_result(p_result),
-			m_exclude(p_exclude),
-			collide_with_bodies(p_collide_with_bodies),
-			collide_with_areas(p_collide_with_areas) {}
-
-	virtual bool needsCollision(btBroadphaseProxy *proxy0) const;
-
-	virtual btScalar addSingleResult(btManifoldPoint &cp, const btCollisionObjectWrapper *colObj0Wrap, int partId0, int index0, const btCollisionObjectWrapper *colObj1Wrap, int partId1, int index1);
-};
-
-struct GodotDeepPenetrationContactResultCallback : public btManifoldResult {
-	btVector3 m_pointNormalWorld;
-	btVector3 m_pointWorld;
-	btScalar m_penetration_distance = 0;
-	int m_other_compound_shape_index = 0;
-
-	GodotDeepPenetrationContactResultCallback(const btCollisionObjectWrapper *body0Wrap, const btCollisionObjectWrapper *body1Wrap) :
-			btManifoldResult(body0Wrap, body1Wrap) {}
-
-	void reset() {
-		m_penetration_distance = 0;
-	}
-
-	bool hasHit() {
-		return m_penetration_distance < 0;
-	}
-
-	virtual void addContactPoint(const btVector3 &normalOnBInWorld, const btVector3 &pointInWorldOnB, btScalar depth);
-};
-
-#endif // GODOT_RESULT_CALLBACKS_H

modules/bullet/hinge_joint_bullet.cpp

@@ -1,170 +0,0 @@
-/*************************************************************************/
-/*  hinge_joint_bullet.cpp                                               */
-/*************************************************************************/
-/*                       This file is part of:                           */
-/*                           GODOT ENGINE                                */
-/*                      https://godotengine.org                          */
-/*************************************************************************/
-/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur.                 */
-/* Copyright (c) 2014-2022 Godot Engine contributors (cf. AUTHORS.md).   */
-/*                                                                       */
-/* Permission is hereby granted, free of charge, to any person obtaining */
-/* a copy of this software and associated documentation files (the       */
-/* "Software"), to deal in the Software without restriction, including   */
-/* without limitation the rights to use, copy, modify, merge, publish,   */
-/* distribute, sublicense, and/or sell copies of the Software, and to    */
-/* permit persons to whom the Software is furnished to do so, subject to */
-/* the following conditions:                                             */
-/*                                                                       */
-/* The above copyright notice and this permission notice shall be        */
-/* included in all copies or substantial portions of the Software.       */
-/*                                                                       */
-/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
-/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
-/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
-/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
-/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION 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 "hinge_joint_bullet.h"
-
-#include "bullet_types_converter.h"
-#include "bullet_utilities.h"
-#include "rigid_body_bullet.h"
-
-#include <BulletDynamics/ConstraintSolver/btHingeConstraint.h>
-
-HingeJointBullet::HingeJointBullet(RigidBodyBullet *rbA, RigidBodyBullet *rbB, const Transform3D &frameA, const Transform3D &frameB) :
-		JointBullet() {
-	Transform3D scaled_AFrame(frameA.scaled(rbA->get_body_scale()));
-	scaled_AFrame.basis.rotref_posscale_decomposition(scaled_AFrame.basis);
-
-	btTransform btFrameA;
-	G_TO_B(scaled_AFrame, btFrameA);
-
-	if (rbB) {
-		Transform3D scaled_BFrame(frameB.scaled(rbB->get_body_scale()));
-		scaled_BFrame.basis.rotref_posscale_decomposition(scaled_BFrame.basis);
-
-		btTransform btFrameB;
-		G_TO_B(scaled_BFrame, btFrameB);
-
-		hingeConstraint = bulletnew(btHingeConstraint(*rbA->get_bt_rigid_body(), *rbB->get_bt_rigid_body(), btFrameA, btFrameB));
-	} else {
-		hingeConstraint = bulletnew(btHingeConstraint(*rbA->get_bt_rigid_body(), btFrameA));
-	}
-
-	setup(hingeConstraint);
-}
-
-HingeJointBullet::HingeJointBullet(RigidBodyBullet *rbA, RigidBodyBullet *rbB, const Vector3 &pivotInA, const Vector3 &pivotInB, const Vector3 &axisInA, const Vector3 &axisInB) :
-		JointBullet() {
-	btVector3 btPivotA;
-	btVector3 btAxisA;
-	G_TO_B(pivotInA * rbA->get_body_scale(), btPivotA);
-	G_TO_B(axisInA * rbA->get_body_scale(), btAxisA);
-
-	if (rbB) {
-		btVector3 btPivotB;
-		btVector3 btAxisB;
-		G_TO_B(pivotInB * rbB->get_body_scale(), btPivotB);
-		G_TO_B(axisInB * rbB->get_body_scale(), btAxisB);
-
-		hingeConstraint = bulletnew(btHingeConstraint(*rbA->get_bt_rigid_body(), *rbB->get_bt_rigid_body(), btPivotA, btPivotB, btAxisA, btAxisB));
-	} else {
-		hingeConstraint = bulletnew(btHingeConstraint(*rbA->get_bt_rigid_body(), btPivotA, btAxisA));
-	}
-
-	setup(hingeConstraint);
-}
-
-real_t HingeJointBullet::get_hinge_angle() {
-	return hingeConstraint->getHingeAngle();
-}
-
-void HingeJointBullet::set_param(PhysicsServer3D::HingeJointParam p_param, real_t p_value) {
-	switch (p_param) {
-		case PhysicsServer3D::HINGE_JOINT_BIAS:
-			WARN_DEPRECATED_MSG("The HingeJoint3D parameter \"bias\" is deprecated.");
-			break;
-		case PhysicsServer3D::HINGE_JOINT_LIMIT_UPPER:
-			hingeConstraint->setLimit(hingeConstraint->getLowerLimit(), p_value, hingeConstraint->getLimitSoftness(), hingeConstraint->getLimitBiasFactor(), hingeConstraint->getLimitRelaxationFactor());
-			break;
-		case PhysicsServer3D::HINGE_JOINT_LIMIT_LOWER:
-			hingeConstraint->setLimit(p_value, hingeConstraint->getUpperLimit(), hingeConstraint->getLimitSoftness(), hingeConstraint->getLimitBiasFactor(), hingeConstraint->getLimitRelaxationFactor());
-			break;
-		case PhysicsServer3D::HINGE_JOINT_LIMIT_BIAS:
-			hingeConstraint->setLimit(hingeConstraint->getLowerLimit(), hingeConstraint->getUpperLimit(), hingeConstraint->getLimitSoftness(), p_value, hingeConstraint->getLimitRelaxationFactor());
-			break;
-		case PhysicsServer3D::HINGE_JOINT_LIMIT_SOFTNESS:
-			hingeConstraint->setLimit(hingeConstraint->getLowerLimit(), hingeConstraint->getUpperLimit(), p_value, hingeConstraint->getLimitBiasFactor(), hingeConstraint->getLimitRelaxationFactor());
-			break;
-		case PhysicsServer3D::HINGE_JOINT_LIMIT_RELAXATION:
-			hingeConstraint->setLimit(hingeConstraint->getLowerLimit(), hingeConstraint->getUpperLimit(), hingeConstraint->getLimitSoftness(), hingeConstraint->getLimitBiasFactor(), p_value);
-			break;
-		case PhysicsServer3D::HINGE_JOINT_MOTOR_TARGET_VELOCITY:
-			hingeConstraint->setMotorTargetVelocity(p_value);
-			break;
-		case PhysicsServer3D::HINGE_JOINT_MOTOR_MAX_IMPULSE:
-			hingeConstraint->setMaxMotorImpulse(p_value);
-			break;
-		case PhysicsServer3D::HINGE_JOINT_MAX:
-			// Internal size value, nothing to do.
-			break;
-	}
-}
-
-real_t HingeJointBullet::get_param(PhysicsServer3D::HingeJointParam p_param) const {
-	switch (p_param) {
-		case PhysicsServer3D::HINGE_JOINT_BIAS:
-			WARN_DEPRECATED_MSG("The HingeJoint3D parameter \"bias\" is deprecated.");
-			return 0;
-		case PhysicsServer3D::HINGE_JOINT_LIMIT_UPPER:
-			return hingeConstraint->getUpperLimit();
-		case PhysicsServer3D::HINGE_JOINT_LIMIT_LOWER:
-			return hingeConstraint->getLowerLimit();
-		case PhysicsServer3D::HINGE_JOINT_LIMIT_BIAS:
-			return hingeConstraint->getLimitBiasFactor();
-		case PhysicsServer3D::HINGE_JOINT_LIMIT_SOFTNESS:
-			return hingeConstraint->getLimitSoftness();
-		case PhysicsServer3D::HINGE_JOINT_LIMIT_RELAXATION:
-			return hingeConstraint->getLimitRelaxationFactor();
-		case PhysicsServer3D::HINGE_JOINT_MOTOR_TARGET_VELOCITY:
-			return hingeConstraint->getMotorTargetVelocity();
-		case PhysicsServer3D::HINGE_JOINT_MOTOR_MAX_IMPULSE:
-			return hingeConstraint->getMaxMotorImpulse();
-		case PhysicsServer3D::HINGE_JOINT_MAX:
-			// Internal size value, nothing to do.
-			return 0;
-	}
-	// Compiler doesn't seem to notice that all code paths are fulfilled...
-	return 0;
-}
-
-void HingeJointBullet::set_flag(PhysicsServer3D::HingeJointFlag p_flag, bool p_value) {
-	switch (p_flag) {
-		case PhysicsServer3D::HINGE_JOINT_FLAG_USE_LIMIT:
-			if (!p_value) {
-				hingeConstraint->setLimit(-Math_PI, Math_PI);
-			}
-			break;
-		case PhysicsServer3D::HINGE_JOINT_FLAG_ENABLE_MOTOR:
-			hingeConstraint->enableMotor(p_value);
-			break;
-		case PhysicsServer3D::HINGE_JOINT_FLAG_MAX:
-			break; // Can't happen, but silences warning
-	}
-}
-
-bool HingeJointBullet::get_flag(PhysicsServer3D::HingeJointFlag p_flag) const {
-	switch (p_flag) {
-		case PhysicsServer3D::HINGE_JOINT_FLAG_USE_LIMIT:
-			return true;
-		case PhysicsServer3D::HINGE_JOINT_FLAG_ENABLE_MOTOR:
-			return hingeConstraint->getEnableAngularMotor();
-		default:
-			return false;
-	}
-}

modules/bullet/hinge_joint_bullet.h

@@ -1,54 +0,0 @@
-/*************************************************************************/
-/*  hinge_joint_bullet.h                                                 */
-/*************************************************************************/
-/*                       This file is part of:                           */
-/*                           GODOT ENGINE                                */
-/*                      https://godotengine.org                          */
-/*************************************************************************/
-/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur.                 */
-/* Copyright (c) 2014-2022 Godot Engine contributors (cf. AUTHORS.md).   */
-/*                                                                       */
-/* Permission is hereby granted, free of charge, to any person obtaining */
-/* a copy of this software and associated documentation files (the       */
-/* "Software"), to deal in the Software without restriction, including   */
-/* without limitation the rights to use, copy, modify, merge, publish,   */
-/* distribute, sublicense, and/or sell copies of the Software, and to    */
-/* permit persons to whom the Software is furnished to do so, subject to */
-/* the following conditions:                                             */
-/*                                                                       */
-/* The above copyright notice and this permission notice shall be        */
-/* included in all copies or substantial portions of the Software.       */
-/*                                                                       */
-/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
-/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
-/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
-/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
-/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION 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 HINGE_JOINT_BULLET_H
-#define HINGE_JOINT_BULLET_H
-
-#include "joint_bullet.h"
-
-class HingeJointBullet : public JointBullet {
-	class btHingeConstraint *hingeConstraint;
-
-public:
-	HingeJointBullet(RigidBodyBullet *rbA, RigidBodyBullet *rbB, const Transform3D &frameA, const Transform3D &frameB);
-	HingeJointBullet(RigidBodyBullet *rbA, RigidBodyBullet *rbB, const Vector3 &pivotInA, const Vector3 &pivotInB, const Vector3 &axisInA, const Vector3 &axisInB);
-
-	virtual PhysicsServer3D::JointType get_type() const { return PhysicsServer3D::JOINT_HINGE; }
-
-	real_t get_hinge_angle();
-
-	void set_param(PhysicsServer3D::HingeJointParam p_param, real_t p_value);
-	real_t get_param(PhysicsServer3D::HingeJointParam p_param) const;
-
-	void set_flag(PhysicsServer3D::HingeJointFlag p_flag, bool p_value);
-	bool get_flag(PhysicsServer3D::HingeJointFlag p_flag) const;
-};
-
-#endif // HINGE_JOINT_BULLET_H

modules/bullet/joint_bullet.h

@@ -1,48 +0,0 @@
-/*************************************************************************/
-/*  joint_bullet.h                                                       */
-/*************************************************************************/
-/*                       This file is part of:                           */
-/*                           GODOT ENGINE                                */
-/*                      https://godotengine.org                          */
-/*************************************************************************/
-/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur.                 */
-/* Copyright (c) 2014-2022 Godot Engine contributors (cf. AUTHORS.md).   */
-/*                                                                       */
-/* Permission is hereby granted, free of charge, to any person obtaining */
-/* a copy of this software and associated documentation files (the       */
-/* "Software"), to deal in the Software without restriction, including   */
-/* without limitation the rights to use, copy, modify, merge, publish,   */
-/* distribute, sublicense, and/or sell copies of the Software, and to    */
-/* permit persons to whom the Software is furnished to do so, subject to */
-/* the following conditions:                                             */
-/*                                                                       */
-/* The above copyright notice and this permission notice shall be        */
-/* included in all copies or substantial portions of the Software.       */
-/*                                                                       */
-/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
-/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
-/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
-/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
-/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION 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 JOINT_BULLET_H
-#define JOINT_BULLET_H
-
-#include "constraint_bullet.h"
-#include "servers/physics_server_3d.h"
-
-class RigidBodyBullet;
-class btTypedConstraint;
-
-class JointBullet : public ConstraintBullet {
-public:
-	JointBullet() {}
-	virtual ~JointBullet() {}
-
-	virtual PhysicsServer3D::JointType get_type() const = 0;
-};
-
-#endif // JOINT_BULLET_H

modules/bullet/pin_joint_bullet.cpp

@@ -1,111 +0,0 @@
-/*************************************************************************/
-/*  pin_joint_bullet.cpp                                                 */
-/*************************************************************************/
-/*                       This file is part of:                           */
-/*                           GODOT ENGINE                                */
-/*                      https://godotengine.org                          */
-/*************************************************************************/
-/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur.                 */
-/* Copyright (c) 2014-2022 Godot Engine contributors (cf. AUTHORS.md).   */
-/*                                                                       */
-/* Permission is hereby granted, free of charge, to any person obtaining */
-/* a copy of this software and associated documentation files (the       */
-/* "Software"), to deal in the Software without restriction, including   */
-/* without limitation the rights to use, copy, modify, merge, publish,   */
-/* distribute, sublicense, and/or sell copies of the Software, and to    */
-/* permit persons to whom the Software is furnished to do so, subject to */
-/* the following conditions:                                             */
-/*                                                                       */
-/* The above copyright notice and this permission notice shall be        */
-/* included in all copies or substantial portions of the Software.       */
-/*                                                                       */
-/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
-/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
-/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
-/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
-/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION 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 "pin_joint_bullet.h"
-
-#include "bullet_types_converter.h"
-#include "rigid_body_bullet.h"
-
-#include <BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h>
-
-PinJointBullet::PinJointBullet(RigidBodyBullet *p_body_a, const Vector3 &p_pos_a, RigidBodyBullet *p_body_b, const Vector3 &p_pos_b) :
-		JointBullet() {
-	if (p_body_b) {
-		btVector3 btPivotA;
-		btVector3 btPivotB;
-		G_TO_B(p_pos_a * p_body_a->get_body_scale(), btPivotA);
-		G_TO_B(p_pos_b * p_body_b->get_body_scale(), btPivotB);
-		p2pConstraint = bulletnew(btPoint2PointConstraint(*p_body_a->get_bt_rigid_body(),
-				*p_body_b->get_bt_rigid_body(),
-				btPivotA,
-				btPivotB));
-	} else {
-		btVector3 btPivotA;
-		G_TO_B(p_pos_a, btPivotA);
-		p2pConstraint = bulletnew(btPoint2PointConstraint(*p_body_a->get_bt_rigid_body(), btPivotA));
-	}
-
-	setup(p2pConstraint);
-}
-
-PinJointBullet::~PinJointBullet() {}
-
-void PinJointBullet::set_param(PhysicsServer3D::PinJointParam p_param, real_t p_value) {
-	switch (p_param) {
-		case PhysicsServer3D::PIN_JOINT_BIAS:
-			p2pConstraint->m_setting.m_tau = p_value;
-			break;
-		case PhysicsServer3D::PIN_JOINT_DAMPING:
-			p2pConstraint->m_setting.m_damping = p_value;
-			break;
-		case PhysicsServer3D::PIN_JOINT_IMPULSE_CLAMP:
-			p2pConstraint->m_setting.m_impulseClamp = p_value;
-			break;
-	}
-}
-
-real_t PinJointBullet::get_param(PhysicsServer3D::PinJointParam p_param) const {
-	switch (p_param) {
-		case PhysicsServer3D::PIN_JOINT_BIAS:
-			return p2pConstraint->m_setting.m_tau;
-		case PhysicsServer3D::PIN_JOINT_DAMPING:
-			return p2pConstraint->m_setting.m_damping;
-		case PhysicsServer3D::PIN_JOINT_IMPULSE_CLAMP:
-			return p2pConstraint->m_setting.m_impulseClamp;
-	}
-	// Compiler doesn't seem to notice that all code paths are fulfilled...
-	return 0;
-}
-
-void PinJointBullet::setPivotInA(const Vector3 &p_pos) {
-	btVector3 btVec;
-	G_TO_B(p_pos, btVec);
-	p2pConstraint->setPivotA(btVec);
-}
-
-void PinJointBullet::setPivotInB(const Vector3 &p_pos) {
-	btVector3 btVec;
-	G_TO_B(p_pos, btVec);
-	p2pConstraint->setPivotB(btVec);
-}
-
-Vector3 PinJointBullet::getPivotInA() {
-	btVector3 vec = p2pConstraint->getPivotInA();
-	Vector3 gVec;
-	B_TO_G(vec, gVec);
-	return gVec;
-}
-
-Vector3 PinJointBullet::getPivotInB() {
-	btVector3 vec = p2pConstraint->getPivotInB();
-	Vector3 gVec;
-	B_TO_G(vec, gVec);
-	return gVec;
-}

modules/bullet/pin_joint_bullet.h

@@ -1,57 +0,0 @@
-/*************************************************************************/
-/*  pin_joint_bullet.h                                                   */
-/*************************************************************************/
-/*                       This file is part of:                           */
-/*                           GODOT ENGINE                                */
-/*                      https://godotengine.org                          */
-/*************************************************************************/
-/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur.                 */
-/* Copyright (c) 2014-2022 Godot Engine contributors (cf. AUTHORS.md).   */
-/*                                                                       */
-/* Permission is hereby granted, free of charge, to any person obtaining */
-/* a copy of this software and associated documentation files (the       */
-/* "Software"), to deal in the Software without restriction, including   */
-/* without limitation the rights to use, copy, modify, merge, publish,   */
-/* distribute, sublicense, and/or sell copies of the Software, and to    */
-/* permit persons to whom the Software is furnished to do so, subject to */
-/* the following conditions:                                             */
-/*                                                                       */
-/* The above copyright notice and this permission notice shall be        */
-/* included in all copies or substantial portions of the Software.       */
-/*                                                                       */
-/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
-/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
-/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
-/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
-/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION 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 PIN_JOINT_BULLET_H
-#define PIN_JOINT_BULLET_H
-
-#include "joint_bullet.h"
-
-class RigidBodyBullet;
-
-class PinJointBullet : public JointBullet {
-	class btPoint2PointConstraint *p2pConstraint;
-
-public:
-	PinJointBullet(RigidBodyBullet *p_body_a, const Vector3 &p_pos_a, RigidBodyBullet *p_body_b, const Vector3 &p_pos_b);
-	~PinJointBullet();
-
-	virtual PhysicsServer3D::JointType get_type() const { return PhysicsServer3D::JOINT_PIN; }
-
-	void set_param(PhysicsServer3D::PinJointParam p_param, real_t p_value);
-	real_t get_param(PhysicsServer3D::PinJointParam p_param) const;
-
-	void setPivotInA(const Vector3 &p_pos);
-	void setPivotInB(const Vector3 &p_pos);
-
-	Vector3 getPivotInA();
-	Vector3 getPivotInB();
-};
-
-#endif // PIN_JOINT_BULLET_H

modules/bullet/register_types.cpp

@@ -1,54 +0,0 @@
-/*************************************************************************/
-/*  register_types.cpp                                                   */
-/*************************************************************************/
-/*                       This file is part of:                           */
-/*                           GODOT ENGINE                                */
-/*                      https://godotengine.org                          */
-/*************************************************************************/
-/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur.                 */
-/* Copyright (c) 2014-2022 Godot Engine contributors (cf. AUTHORS.md).   */
-/*                                                                       */
-/* Permission is hereby granted, free of charge, to any person obtaining */
-/* a copy of this software and associated documentation files (the       */
-/* "Software"), to deal in the Software without restriction, including   */
-/* without limitation the rights to use, copy, modify, merge, publish,   */
-/* distribute, sublicense, and/or sell copies of the Software, and to    */
-/* permit persons to whom the Software is furnished to do so, subject to */
-/* the following conditions:                                             */
-/*                                                                       */
-/* The above copyright notice and this permission notice shall be        */
-/* included in all copies or substantial portions of the Software.       */
-/*                                                                       */
-/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
-/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
-/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
-/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
-/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION 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 "register_types.h"
-
-#include "bullet_physics_server.h"
-#include "core/config/project_settings.h"
-#include "core/object/class_db.h"
-
-#ifndef _3D_DISABLED
-PhysicsServer3D *_createBulletPhysicsCallback() {
-	return memnew(BulletPhysicsServer3D);
-}
-#endif
-
-void register_bullet_types() {
-#ifndef _3D_DISABLED
-	PhysicsServer3DManager::register_server("Bullet", &_createBulletPhysicsCallback);
-	PhysicsServer3DManager::set_default_server("Bullet", 1);
-
-	GLOBAL_DEF("physics/3d/active_soft_world", true);
-	ProjectSettings::get_singleton()->set_custom_property_info("physics/3d/active_soft_world", PropertyInfo(Variant::BOOL, "physics/3d/active_soft_world"));
-#endif
-}
-
-void unregister_bullet_types() {
-}

modules/bullet/register_types.h

@@ -1,37 +0,0 @@
-/*************************************************************************/
-/*  register_types.h                                                     */
-/*************************************************************************/
-/*                       This file is part of:                           */
-/*                           GODOT ENGINE                                */
-/*                      https://godotengine.org                          */
-/*************************************************************************/
-/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur.                 */
-/* Copyright (c) 2014-2022 Godot Engine contributors (cf. AUTHORS.md).   */
-/*                                                                       */
-/* Permission is hereby granted, free of charge, to any person obtaining */
-/* a copy of this software and associated documentation files (the       */
-/* "Software"), to deal in the Software without restriction, including   */
-/* without limitation the rights to use, copy, modify, merge, publish,   */
-/* distribute, sublicense, and/or sell copies of the Software, and to    */
-/* permit persons to whom the Software is furnished to do so, subject to */
-/* the following conditions:                                             */
-/*                                                                       */
-/* The above copyright notice and this permission notice shall be        */
-/* included in all copies or substantial portions of the Software.       */
-/*                                                                       */
-/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
-/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
-/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
-/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
-/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION 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 REGISTER_BULLET_TYPES_H
-#define REGISTER_BULLET_TYPES_H
-
-void register_bullet_types();
-void unregister_bullet_types();
-
-#endif // REGISTER_BULLET_TYPES_H

modules/bullet/rid_bullet.h

@@ -1,50 +0,0 @@
-/*************************************************************************/
-/*  rid_bullet.h                                                         */
-/*************************************************************************/
-/*                       This file is part of:                           */
-/*                           GODOT ENGINE                                */
-/*                      https://godotengine.org                          */
-/*************************************************************************/
-/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur.                 */
-/* Copyright (c) 2014-2022 Godot Engine contributors (cf. AUTHORS.md).   */
-/*                                                                       */
-/* Permission is hereby granted, free of charge, to any person obtaining */
-/* a copy of this software and associated documentation files (the       */
-/* "Software"), to deal in the Software without restriction, including   */
-/* without limitation the rights to use, copy, modify, merge, publish,   */
-/* distribute, sublicense, and/or sell copies of the Software, and to    */
-/* permit persons to whom the Software is furnished to do so, subject to */
-/* the following conditions:                                             */
-/*                                                                       */
-/* The above copyright notice and this permission notice shall be        */
-/* included in all copies or substantial portions of the Software.       */
-/*                                                                       */
-/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
-/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
-/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
-/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
-/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION 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 RID_BULLET_H
-#define RID_BULLET_H
-
-#include "core/templates/rid.h"
-
-class BulletPhysicsServer3D;
-
-class RIDBullet {
-	RID self;
-	BulletPhysicsServer3D *physicsServer = nullptr;
-
-public:
-	_FORCE_INLINE_ void set_self(const RID &p_self) { self = p_self; }
-	_FORCE_INLINE_ RID get_self() const { return self; }
-
-	_FORCE_INLINE_ void _set_physics_server(BulletPhysicsServer3D *p_physicsServer) { physicsServer = p_physicsServer; }
-	_FORCE_INLINE_ BulletPhysicsServer3D *get_physics_server() const { return physicsServer; }
-};
-
-#endif // RID_BULLET_H

modules/bullet/rigid_body_bullet.cpp

@@ -1,1050 +0,0 @@
-/*************************************************************************/
-/*  rigid_body_bullet.cpp                                                */
-/*************************************************************************/
-/*                       This file is part of:                           */
-/*                           GODOT ENGINE                                */
-/*                      https://godotengine.org                          */
-/*************************************************************************/
-/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur.                 */
-/* Copyright (c) 2014-2022 Godot Engine contributors (cf. AUTHORS.md).   */
-/*                                                                       */
-/* Permission is hereby granted, free of charge, to any person obtaining */
-/* a copy of this software and associated documentation files (the       */
-/* "Software"), to deal in the Software without restriction, including   */
-/* without limitation the rights to use, copy, modify, merge, publish,   */
-/* distribute, sublicense, and/or sell copies of the Software, and to    */
-/* permit persons to whom the Software is furnished to do so, subject to */
-/* the following conditions:                                             */
-/*                                                                       */
-/* The above copyright notice and this permission notice shall be        */
-/* included in all copies or substantial portions of the Software.       */
-/*                                                                       */
-/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
-/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
-/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
-/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
-/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION 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 "rigid_body_bullet.h"
-
-#include "btRayShape.h"
-#include "bullet_physics_server.h"
-#include "bullet_types_converter.h"
-#include "bullet_utilities.h"
-#include "godot_motion_state.h"
-#include "joint_bullet.h"
-
-#include <BulletCollision/CollisionDispatch/btGhostObject.h>
-#include <BulletCollision/CollisionShapes/btConvexPointCloudShape.h>
-#include <BulletDynamics/Dynamics/btRigidBody.h>
-#include <btBulletCollisionCommon.h>
-
-BulletPhysicsDirectBodyState3D *BulletPhysicsDirectBodyState3D::singleton = nullptr;
-
-Vector3 BulletPhysicsDirectBodyState3D::get_total_gravity() const {
-	Vector3 gVec;
-	B_TO_G(body->btBody->getGravity(), gVec);
-	return gVec;
-}
-
-real_t BulletPhysicsDirectBodyState3D::get_total_angular_damp() const {
-	return body->btBody->getAngularDamping();
-}
-
-real_t BulletPhysicsDirectBodyState3D::get_total_linear_damp() const {
-	return body->btBody->getLinearDamping();
-}
-
-Vector3 BulletPhysicsDirectBodyState3D::get_center_of_mass() const {
-	Vector3 gVec;
-	B_TO_G(body->btBody->getCenterOfMassPosition(), gVec);
-	return gVec;
-}
-
-Basis BulletPhysicsDirectBodyState3D::get_principal_inertia_axes() const {
-	return Basis();
-}
-
-real_t BulletPhysicsDirectBodyState3D::get_inverse_mass() const {
-	return body->btBody->getInvMass();
-}
-
-Vector3 BulletPhysicsDirectBodyState3D::get_inverse_inertia() const {
-	Vector3 gVec;
-	B_TO_G(body->btBody->getInvInertiaDiagLocal(), gVec);
-	return gVec;
-}
-
-Basis BulletPhysicsDirectBodyState3D::get_inverse_inertia_tensor() const {
-	Basis gInertia;
-	B_TO_G(body->btBody->getInvInertiaTensorWorld(), gInertia);
-	return gInertia;
-}
-
-void BulletPhysicsDirectBodyState3D::set_linear_velocity(const Vector3 &p_velocity) {
-	body->set_linear_velocity(p_velocity);
-}
-
-Vector3 BulletPhysicsDirectBodyState3D::get_linear_velocity() const {
-	return body->get_linear_velocity();
-}
-
-void BulletPhysicsDirectBodyState3D::set_angular_velocity(const Vector3 &p_velocity) {
-	body->set_angular_velocity(p_velocity);
-}
-
-Vector3 BulletPhysicsDirectBodyState3D::get_angular_velocity() const {
-	return body->get_angular_velocity();
-}
-
-void BulletPhysicsDirectBodyState3D::set_transform(const Transform3D &p_transform) {
-	body->set_transform(p_transform);
-}
-
-Transform3D BulletPhysicsDirectBodyState3D::get_transform() const {
-	return body->get_transform();
-}
-
-Vector3 BulletPhysicsDirectBodyState3D::get_velocity_at_local_position(const Vector3 &p_position) const {
-	btVector3 local_position;
-	G_TO_B(p_position, local_position);
-
-	Vector3 velocity;
-	B_TO_G(body->btBody->getVelocityInLocalPoint(local_position), velocity);
-
-	return velocity;
-}
-
-void BulletPhysicsDirectBodyState3D::add_central_force(const Vector3 &p_force) {
-	body->apply_central_force(p_force);
-}
-
-void BulletPhysicsDirectBodyState3D::add_force(const Vector3 &p_force, const Vector3 &p_position) {
-	body->apply_force(p_force, p_position);
-}
-
-void BulletPhysicsDirectBodyState3D::add_torque(const Vector3 &p_torque) {
-	body->apply_torque(p_torque);
-}
-
-void BulletPhysicsDirectBodyState3D::apply_central_impulse(const Vector3 &p_impulse) {
-	body->apply_central_impulse(p_impulse);
-}
-
-void BulletPhysicsDirectBodyState3D::apply_impulse(const Vector3 &p_impulse, const Vector3 &p_position) {
-	body->apply_impulse(p_impulse, p_position);
-}
-
-void BulletPhysicsDirectBodyState3D::apply_torque_impulse(const Vector3 &p_impulse) {
-	body->apply_torque_impulse(p_impulse);
-}
-
-void BulletPhysicsDirectBodyState3D::set_sleep_state(bool p_sleep) {
-	body->set_activation_state(!p_sleep);
-}
-
-bool BulletPhysicsDirectBodyState3D::is_sleeping() const {
-	return !body->is_active();
-}
-
-int BulletPhysicsDirectBodyState3D::get_contact_count() const {
-	return body->collisionsCount;
-}
-
-Vector3 BulletPhysicsDirectBodyState3D::get_contact_local_position(int p_contact_idx) const {
-	return body->collisions[p_contact_idx].hitLocalLocation;
-}
-
-Vector3 BulletPhysicsDirectBodyState3D::get_contact_local_normal(int p_contact_idx) const {
-	return body->collisions[p_contact_idx].hitNormal;
-}
-
-real_t BulletPhysicsDirectBodyState3D::get_contact_impulse(int p_contact_idx) const {
-	return body->collisions[p_contact_idx].appliedImpulse;
-}
-
-int BulletPhysicsDirectBodyState3D::get_contact_local_shape(int p_contact_idx) const {
-	return body->collisions[p_contact_idx].local_shape;
-}
-
-RID BulletPhysicsDirectBodyState3D::get_contact_collider(int p_contact_idx) const {
-	return body->collisions[p_contact_idx].otherObject->get_self();
-}
-
-Vector3 BulletPhysicsDirectBodyState3D::get_contact_collider_position(int p_contact_idx) const {
-	return body->collisions[p_contact_idx].hitWorldLocation;
-}
-
-ObjectID BulletPhysicsDirectBodyState3D::get_contact_collider_id(int p_contact_idx) const {
-	return body->collisions[p_contact_idx].otherObject->get_instance_id();
-}
-
-int BulletPhysicsDirectBodyState3D::get_contact_collider_shape(int p_contact_idx) const {
-	return body->collisions[p_contact_idx].other_object_shape;
-}
-
-Vector3 BulletPhysicsDirectBodyState3D::get_contact_collider_velocity_at_position(int p_contact_idx) const {
-	RigidBodyBullet::CollisionData &colDat = body->collisions.write[p_contact_idx];
-
-	btVector3 hitLocation;
-	G_TO_B(colDat.hitLocalLocation, hitLocation);
-
-	Vector3 velocityAtPoint;
-	B_TO_G(colDat.otherObject->get_bt_rigid_body()->getVelocityInLocalPoint(hitLocation), velocityAtPoint);
-
-	return velocityAtPoint;
-}
-
-PhysicsDirectSpaceState3D *BulletPhysicsDirectBodyState3D::get_space_state() {
-	return body->get_space()->get_direct_state();
-}
-
-RigidBodyBullet::KinematicUtilities::KinematicUtilities(RigidBodyBullet *p_owner) :
-		owner(p_owner),
-		safe_margin(0.001) {
-}
-
-RigidBodyBullet::KinematicUtilities::~KinematicUtilities() {
-	just_delete_shapes(shapes.size()); // don't need to resize
-}
-
-void RigidBodyBullet::KinematicUtilities::setSafeMargin(btScalar p_margin) {
-	safe_margin = p_margin;
-	copyAllOwnerShapes();
-}
-
-void RigidBodyBullet::KinematicUtilities::copyAllOwnerShapes() {
-	const Vector<CollisionObjectBullet::ShapeWrapper> &shapes_wrappers(owner->get_shapes_wrappers());
-	const int shapes_count = shapes_wrappers.size();
-
-	just_delete_shapes(shapes_count);
-
-	const CollisionObjectBullet::ShapeWrapper *shape_wrapper;
-
-	btVector3 owner_scale(owner->get_bt_body_scale());
-
-	for (int i = shapes_count - 1; 0 <= i; --i) {
-		shape_wrapper = &shapes_wrappers[i];
-		if (!shape_wrapper->active) {
-			continue;
-		}
-
-		shapes.write[i].transform = shape_wrapper->transform;
-		shapes.write[i].transform.getOrigin() *= owner_scale;
-		switch (shape_wrapper->shape->get_type()) {
-			case PhysicsServer3D::SHAPE_SPHERE:
-			case PhysicsServer3D::SHAPE_BOX:
-			case PhysicsServer3D::SHAPE_CAPSULE:
-			case PhysicsServer3D::SHAPE_CYLINDER:
-			case PhysicsServer3D::SHAPE_CONVEX_POLYGON:
-			case PhysicsServer3D::SHAPE_RAY: {
-				shapes.write[i].shape = static_cast<btConvexShape *>(shape_wrapper->shape->create_bt_shape(owner_scale * shape_wrapper->scale, safe_margin));
-			} break;
-			default:
-				WARN_PRINT("This shape is not supported for kinematic collision.");
-				shapes.write[i].shape = nullptr;
-		}
-	}
-}
-
-void RigidBodyBullet::KinematicUtilities::just_delete_shapes(int new_size) {
-	for (int i = shapes.size() - 1; 0 <= i; --i) {
-		if (shapes[i].shape) {
-			bulletdelete(shapes.write[i].shape);
-		}
-	}
-	shapes.resize(new_size);
-}
-
-RigidBodyBullet::RigidBodyBullet() :
-		RigidCollisionObjectBullet(CollisionObjectBullet::TYPE_RIGID_BODY) {
-	godotMotionState = bulletnew(GodotMotionState(this));
-
-	// Initial properties
-	const btVector3 localInertia(0, 0, 0);
-	btRigidBody::btRigidBodyConstructionInfo cInfo(mass, godotMotionState, nullptr, localInertia);
-
-	btBody = bulletnew(btRigidBody(cInfo));
-	btBody->setFriction(1.0);
-	reload_shapes();
-	setupBulletCollisionObject(btBody);
-
-	set_mode(PhysicsServer3D::BODY_MODE_DYNAMIC);
-	reload_axis_lock();
-
-	areasWhereIam.resize(maxAreasWhereIam);
-	for (int i = areasWhereIam.size() - 1; 0 <= i; --i) {
-		areasWhereIam.write[i] = nullptr;
-	}
-	btBody->setSleepingThresholds(0.2, 0.2);
-
-	prev_collision_traces = &collision_traces_1;
-	curr_collision_traces = &collision_traces_2;
-}
-
-RigidBodyBullet::~RigidBodyBullet() {
-	bulletdelete(godotMotionState);
-
-	if (force_integration_callback) {
-		memdelete(force_integration_callback);
-	}
-
-	destroy_kinematic_utilities();
-}
-
-void RigidBodyBullet::init_kinematic_utilities() {
-	kinematic_utilities = memnew(KinematicUtilities(this));
-	reload_kinematic_shapes();
-}
-
-void RigidBodyBullet::destroy_kinematic_utilities() {
-	if (kinematic_utilities) {
-		memdelete(kinematic_utilities);
-		kinematic_utilities = nullptr;
-	}
-}
-
-void RigidBodyBullet::main_shape_changed() {
-	CRASH_COND(!get_main_shape());
-	btBody->setCollisionShape(get_main_shape());
-	set_continuous_collision_detection(is_continuous_collision_detection_enabled()); // Reset
-}
-
-void RigidBodyBullet::reload_body() {
-	if (space) {
-		space->remove_rigid_body(this);
-		if (get_main_shape()) {
-			space->add_rigid_body(this);
-		}
-	}
-}
-
-void RigidBodyBullet::set_space(SpaceBullet *p_space) {
-	// Clear the old space if there is one
-	if (space) {
-		can_integrate_forces = false;
-		isScratchedSpaceOverrideModificator = false;
-		// Remove any constraints
-		space->remove_rigid_body_constraints(this);
-		// Remove this object form the physics world
-		space->remove_rigid_body(this);
-	}
-
-	space = p_space;
-
-	if (space) {
-		space->add_rigid_body(this);
-	}
-}
-
-void RigidBodyBullet::dispatch_callbacks() {
-	/// The check isFirstTransformChanged is necessary in order to call integrated forces only when the first transform is sent
-	if ((btBody->isKinematicObject() || btBody->isActive() || previousActiveState != btBody->isActive()) && force_integration_callback && can_integrate_forces) {
-		if (omit_forces_integration) {
-			btBody->clearForces();
-		}
-
-		BulletPhysicsDirectBodyState3D *bodyDirect = BulletPhysicsDirectBodyState3D::get_singleton(this);
-
-		Variant variantBodyDirect = bodyDirect;
-
-		Object *obj = force_integration_callback->callable.get_object();
-		if (!obj) {
-			// Remove integration callback
-			set_force_integration_callback(Callable());
-		} else {
-			const Variant *vp[2] = { &variantBodyDirect, &force_integration_callback->udata };
-
-			Callable::CallError responseCallError;
-			int argc = (force_integration_callback->udata.get_type() == Variant::NIL) ? 1 : 2;
-			Variant rv;
-			force_integration_callback->callable.call(vp, argc, rv, responseCallError);
-		}
-	}
-
-	if (isScratchedSpaceOverrideModificator || 0 < countGravityPointSpaces) {
-		isScratchedSpaceOverrideModificator = false;
-		reload_space_override_modificator();
-	}
-
-	/// Lock axis
-	btBody->setLinearVelocity(btBody->getLinearVelocity() * btBody->getLinearFactor());
-	btBody->setAngularVelocity(btBody->getAngularVelocity() * btBody->getAngularFactor());
-
-	previousActiveState = btBody->isActive();
-}
-
-void RigidBodyBullet::set_force_integration_callback(const Callable &p_callable, const Variant &p_udata) {
-	if (force_integration_callback) {
-		memdelete(force_integration_callback);
-		force_integration_callback = nullptr;
-	}
-
-	if (p_callable.get_object()) {
-		force_integration_callback = memnew(ForceIntegrationCallback);
-		force_integration_callback->callable = p_callable;
-		force_integration_callback->udata = p_udata;
-	}
-}
-
-void RigidBodyBullet::scratch_space_override_modificator() {
-	isScratchedSpaceOverrideModificator = true;
-}
-
-void RigidBodyBullet::on_collision_filters_change() {
-	if (space) {
-		space->reload_collision_filters(this);
-	}
-
-	set_activation_state(true);
-}
-
-void RigidBodyBullet::on_collision_checker_start() {
-	prev_collision_count = collisionsCount;
-	collisionsCount = 0;
-
-	// Swap array
-	Vector<RigidBodyBullet *> *s = prev_collision_traces;
-	prev_collision_traces = curr_collision_traces;
-	curr_collision_traces = s;
-}
-
-void RigidBodyBullet::on_collision_checker_end() {
-	// Always true if active and not a static or kinematic body
-	updated = btBody->isActive() && !btBody->isStaticOrKinematicObject();
-}
-
-bool RigidBodyBullet::add_collision_object(RigidBodyBullet *p_otherObject, const Vector3 &p_hitWorldLocation, const Vector3 &p_hitLocalLocation, const Vector3 &p_hitNormal, const real_t &p_appliedImpulse, int p_other_shape_index, int p_local_shape_index) {
-	if (collisionsCount >= maxCollisionsDetection) {
-		return false;
-	}
-
-	CollisionData &cd = collisions.write[collisionsCount];
-	cd.hitLocalLocation = p_hitLocalLocation;
-	cd.otherObject = p_otherObject;
-	cd.hitWorldLocation = p_hitWorldLocation;
-	cd.hitNormal = p_hitNormal;
-	cd.appliedImpulse = p_appliedImpulse;
-	cd.other_object_shape = p_other_shape_index;
-	cd.local_shape = p_local_shape_index;
-
-	curr_collision_traces->write[collisionsCount] = p_otherObject;
-
-	++collisionsCount;
-	return true;
-}
-
-bool RigidBodyBullet::was_colliding(RigidBodyBullet *p_other_object) {
-	for (int i = prev_collision_count - 1; 0 <= i; --i) {
-		if ((*prev_collision_traces)[i] == p_other_object) {
-			return true;
-		}
-	}
-	return false;
-}
-
-void RigidBodyBullet::set_activation_state(bool p_active) {
-	if (p_active) {
-		btBody->activate();
-	} else {
-		btBody->setActivationState(WANTS_DEACTIVATION);
-	}
-}
-
-bool RigidBodyBullet::is_active() const {
-	return btBody->isActive();
-}
-
-void RigidBodyBullet::set_omit_forces_integration(bool p_omit) {
-	omit_forces_integration = p_omit;
-}
-
-void RigidBodyBullet::set_param(PhysicsServer3D::BodyParameter p_param, real_t p_value) {
-	switch (p_param) {
-		case PhysicsServer3D::BODY_PARAM_BOUNCE:
-			btBody->setRestitution(p_value);
-			break;
-		case PhysicsServer3D::BODY_PARAM_FRICTION:
-			btBody->setFriction(p_value);
-			break;
-		case PhysicsServer3D::BODY_PARAM_MASS: {
-			ERR_FAIL_COND(p_value < 0);
-			mass = p_value;
-			_internal_set_mass(p_value);
-			break;
-		}
-		case PhysicsServer3D::BODY_PARAM_LINEAR_DAMP:
-			linearDamp = p_value;
-			// Mark for updating total linear damping.
-			scratch_space_override_modificator();
-			break;
-		case PhysicsServer3D::BODY_PARAM_ANGULAR_DAMP:
-			angularDamp = p_value;
-			// Mark for updating total angular damping.
-			scratch_space_override_modificator();
-			break;
-		case PhysicsServer3D::BODY_PARAM_GRAVITY_SCALE:
-			gravity_scale = p_value;
-			// The Bullet gravity will be is set by reload_space_override_modificator.
-			// Mark for updating total gravity scale.
-			scratch_space_override_modificator();
-			break;
-		default:
-			WARN_PRINT("Parameter " + itos(p_param) + " not supported by bullet. Value: " + itos(p_value));
-	}
-}
-
-real_t RigidBodyBullet::get_param(PhysicsServer3D::BodyParameter p_param) const {
-	switch (p_param) {
-		case PhysicsServer3D::BODY_PARAM_BOUNCE:
-			return btBody->getRestitution();
-		case PhysicsServer3D::BODY_PARAM_FRICTION:
-			return btBody->getFriction();
-		case PhysicsServer3D::BODY_PARAM_MASS: {
-			const btScalar invMass = btBody->getInvMass();
-			return 0 == invMass ? 0 : 1 / invMass;
-		}
-		case PhysicsServer3D::BODY_PARAM_LINEAR_DAMP:
-			return linearDamp;
-		case PhysicsServer3D::BODY_PARAM_ANGULAR_DAMP:
-			return angularDamp;
-		case PhysicsServer3D::BODY_PARAM_GRAVITY_SCALE:
-			return gravity_scale;
-		default:
-			WARN_PRINT("Parameter " + itos(p_param) + " not supported by bullet");
-			return 0;
-	}
-}
-
-void RigidBodyBullet::set_mode(PhysicsServer3D::BodyMode p_mode) {
-	// This is necessary to block force_integration until next move
-	can_integrate_forces = false;
-	destroy_kinematic_utilities();
-	// The mode change is relevant to its mass
-	mode = p_mode;
-	switch (p_mode) {
-		case PhysicsServer3D::BODY_MODE_KINEMATIC:
-			reload_axis_lock();
-			_internal_set_mass(0);
-			init_kinematic_utilities();
-			break;
-		case PhysicsServer3D::BODY_MODE_STATIC:
-			reload_axis_lock();
-			_internal_set_mass(0);
-			break;
-		case PhysicsServer3D::BODY_MODE_DYNAMIC:
-			reload_axis_lock();
-			_internal_set_mass(0 == mass ? 1 : mass);
-			scratch_space_override_modificator();
-			break;
-		case PhysicsServer3D::MODE_DYNAMIC_LINEAR:
-			reload_axis_lock();
-			_internal_set_mass(0 == mass ? 1 : mass);
-			scratch_space_override_modificator();
-			break;
-	}
-
-	btBody->setAngularVelocity(btVector3(0, 0, 0));
-	btBody->setLinearVelocity(btVector3(0, 0, 0));
-}
-
-PhysicsServer3D::BodyMode RigidBodyBullet::get_mode() const {
-	return mode;
-}
-
-void RigidBodyBullet::set_state(PhysicsServer3D::BodyState p_state, const Variant &p_variant) {
-	switch (p_state) {
-		case PhysicsServer3D::BODY_STATE_TRANSFORM:
-			set_transform(p_variant);
-			break;
-		case PhysicsServer3D::BODY_STATE_LINEAR_VELOCITY:
-			set_linear_velocity(p_variant);
-			break;
-		case PhysicsServer3D::BODY_STATE_ANGULAR_VELOCITY:
-			set_angular_velocity(p_variant);
-			break;
-		case PhysicsServer3D::BODY_STATE_SLEEPING:
-			set_activation_state(!bool(p_variant));
-			break;
-		case PhysicsServer3D::BODY_STATE_CAN_SLEEP:
-			can_sleep = bool(p_variant);
-			if (!can_sleep) {
-				// Can't sleep
-				btBody->forceActivationState(DISABLE_DEACTIVATION);
-			} else {
-				btBody->forceActivationState(ACTIVE_TAG);
-			}
-			break;
-	}
-}
-
-Variant RigidBodyBullet::get_state(PhysicsServer3D::BodyState p_state) const {
-	switch (p_state) {
-		case PhysicsServer3D::BODY_STATE_TRANSFORM:
-			return get_transform();
-		case PhysicsServer3D::BODY_STATE_LINEAR_VELOCITY:
-			return get_linear_velocity();
-		case PhysicsServer3D::BODY_STATE_ANGULAR_VELOCITY:
-			return get_angular_velocity();
-		case PhysicsServer3D::BODY_STATE_SLEEPING:
-			return !is_active();
-		case PhysicsServer3D::BODY_STATE_CAN_SLEEP:
-			return can_sleep;
-		default:
-			WARN_PRINT("This state " + itos(p_state) + " is not supported by Bullet");
-			return Variant();
-	}
-}
-
-void RigidBodyBullet::apply_central_impulse(const Vector3 &p_impulse) {
-	btVector3 btImpulse;
-	G_TO_B(p_impulse, btImpulse);
-	if (Vector3() != p_impulse) {
-		btBody->activate();
-	}
-	btBody->applyCentralImpulse(btImpulse);
-}
-
-void RigidBodyBullet::apply_impulse(const Vector3 &p_impulse, const Vector3 &p_position) {
-	btVector3 btImpulse;
-	btVector3 btPosition;
-	G_TO_B(p_impulse, btImpulse);
-	G_TO_B(p_position, btPosition);
-	if (Vector3() != p_impulse) {
-		btBody->activate();
-	}
-	btBody->applyImpulse(btImpulse, btPosition);
-}
-
-void RigidBodyBullet::apply_torque_impulse(const Vector3 &p_impulse) {
-	btVector3 btImp;
-	G_TO_B(p_impulse, btImp);
-	if (Vector3() != p_impulse) {
-		btBody->activate();
-	}
-	btBody->applyTorqueImpulse(btImp);
-}
-
-void RigidBodyBullet::apply_force(const Vector3 &p_force, const Vector3 &p_position) {
-	btVector3 btForce;
-	btVector3 btPosition;
-	G_TO_B(p_force, btForce);
-	G_TO_B(p_position, btPosition);
-	if (Vector3() != p_force) {
-		btBody->activate();
-	}
-	btBody->applyForce(btForce, btPosition);
-}
-
-void RigidBodyBullet::apply_central_force(const Vector3 &p_force) {
-	btVector3 btForce;
-	G_TO_B(p_force, btForce);
-	if (Vector3() != p_force) {
-		btBody->activate();
-	}
-	btBody->applyCentralForce(btForce);
-}
-
-void RigidBodyBullet::apply_torque(const Vector3 &p_torque) {
-	btVector3 btTorq;
-	G_TO_B(p_torque, btTorq);
-	if (Vector3() != p_torque) {
-		btBody->activate();
-	}
-	btBody->applyTorque(btTorq);
-}
-
-void RigidBodyBullet::set_applied_force(const Vector3 &p_force) {
-	btVector3 btVec = btBody->getTotalTorque();
-
-	if (Vector3() != p_force) {
-		btBody->activate();
-	}
-
-	btBody->clearForces();
-	btBody->applyTorque(btVec);
-
-	G_TO_B(p_force, btVec);
-	btBody->applyCentralForce(btVec);
-}
-
-Vector3 RigidBodyBullet::get_applied_force() const {
-	Vector3 gTotForc;
-	B_TO_G(btBody->getTotalForce(), gTotForc);
-	return gTotForc;
-}
-
-void RigidBodyBullet::set_applied_torque(const Vector3 &p_torque) {
-	btVector3 btVec = btBody->getTotalForce();
-
-	if (Vector3() != p_torque) {
-		btBody->activate();
-	}
-
-	btBody->clearForces();
-	btBody->applyCentralForce(btVec);
-
-	G_TO_B(p_torque, btVec);
-	btBody->applyTorque(btVec);
-}
-
-Vector3 RigidBodyBullet::get_applied_torque() const {
-	Vector3 gTotTorq;
-	B_TO_G(btBody->getTotalTorque(), gTotTorq);
-	return gTotTorq;
-}
-
-void RigidBodyBullet::set_axis_lock(PhysicsServer3D::BodyAxis p_axis, bool lock) {
-	if (lock) {
-		locked_axis |= p_axis;
-	} else {
-		locked_axis &= ~p_axis;
-	}
-
-	reload_axis_lock();
-}
-
-bool RigidBodyBullet::is_axis_locked(PhysicsServer3D::BodyAxis p_axis) const {
-	return locked_axis & p_axis;
-}
-
-void RigidBodyBullet::reload_axis_lock() {
-	btBody->setLinearFactor(btVector3(btScalar(!is_axis_locked(PhysicsServer3D::BODY_AXIS_LINEAR_X)), btScalar(!is_axis_locked(PhysicsServer3D::BODY_AXIS_LINEAR_Y)), btScalar(!is_axis_locked(PhysicsServer3D::BODY_AXIS_LINEAR_Z))));
-	if (PhysicsServer3D::MODE_DYNAMIC_LINEAR == mode) {
-		/// When character angular is always locked
-		btBody->setAngularFactor(btVector3(0., 0., 0.));
-	} else {
-		btBody->setAngularFactor(btVector3(btScalar(!is_axis_locked(PhysicsServer3D::BODY_AXIS_ANGULAR_X)), btScalar(!is_axis_locked(PhysicsServer3D::BODY_AXIS_ANGULAR_Y)), btScalar(!is_axis_locked(PhysicsServer3D::BODY_AXIS_ANGULAR_Z))));
-	}
-}
-
-void RigidBodyBullet::set_continuous_collision_detection(bool p_enable) {
-	if (p_enable) {
-		// This threshold enable CCD if the object moves more than
-		// 1 meter in one simulation frame
-		btBody->setCcdMotionThreshold(1e-7);
-
-		/// Calculate using the rule write below the CCD swept sphere radius
-		///     CCD works on an embedded sphere of radius, make sure this radius
-		///     is embedded inside the convex objects, preferably smaller:
-		///     for an object of dimensions 1 meter, try 0.2
-		btScalar radius(1.0);
-		if (btBody->getCollisionShape()) {
-			btVector3 center;
-			btBody->getCollisionShape()->getBoundingSphere(center, radius);
-		}
-		btBody->setCcdSweptSphereRadius(radius * 0.2);
-	} else {
-		btBody->setCcdMotionThreshold(0.);
-		btBody->setCcdSweptSphereRadius(0.);
-	}
-}
-
-bool RigidBodyBullet::is_continuous_collision_detection_enabled() const {
-	return 0. < btBody->getCcdMotionThreshold();
-}
-
-void RigidBodyBullet::set_linear_velocity(const Vector3 &p_velocity) {
-	btVector3 btVec;
-	G_TO_B(p_velocity, btVec);
-	if (Vector3() != p_velocity) {
-		btBody->activate();
-	}
-	btBody->setLinearVelocity(btVec);
-}
-
-Vector3 RigidBodyBullet::get_linear_velocity() const {
-	Vector3 gVec;
-	B_TO_G(btBody->getLinearVelocity(), gVec);
-	return gVec;
-}
-
-void RigidBodyBullet::set_angular_velocity(const Vector3 &p_velocity) {
-	btVector3 btVec;
-	G_TO_B(p_velocity, btVec);
-	if (Vector3() != p_velocity) {
-		btBody->activate();
-	}
-	btBody->setAngularVelocity(btVec);
-}
-
-Vector3 RigidBodyBullet::get_angular_velocity() const {
-	Vector3 gVec;
-	B_TO_G(btBody->getAngularVelocity(), gVec);
-	return gVec;
-}
-
-void RigidBodyBullet::set_transform__bullet(const btTransform &p_global_transform) {
-	if (mode == PhysicsServer3D::BODY_MODE_KINEMATIC) {
-		if (space && space->get_delta_time() != 0) {
-			btBody->setLinearVelocity((p_global_transform.getOrigin() - btBody->getWorldTransform().getOrigin()) / space->get_delta_time());
-		}
-		// The kinematic use MotionState class
-		godotMotionState->moveBody(p_global_transform);
-	} else {
-		// Is necessary to avoid wrong location on the rendering side on the next frame
-		godotMotionState->setWorldTransform(p_global_transform);
-	}
-	CollisionObjectBullet::set_transform__bullet(p_global_transform);
-}
-
-const btTransform &RigidBodyBullet::get_transform__bullet() const {
-	if (is_static()) {
-		return RigidCollisionObjectBullet::get_transform__bullet();
-	} else {
-		return godotMotionState->getCurrentWorldTransform();
-	}
-}
-
-void RigidBodyBullet::reload_shapes() {
-	RigidCollisionObjectBullet::reload_shapes();
-
-	const btScalar invMass = btBody->getInvMass();
-	const btScalar mass = invMass == 0 ? 0 : 1 / invMass;
-
-	if (mainShape) {
-		// inertia initialised zero here because some of bullet's collision
-		// shapes incorrectly do not set the vector in calculateLocalIntertia.
-		// Arbitrary zero is preferable to undefined behaviour.
-		btVector3 inertia(0, 0, 0);
-		if (EMPTY_SHAPE_PROXYTYPE != mainShape->getShapeType()) { // Necessary to avoid assertion of the empty shape
-			mainShape->calculateLocalInertia(mass, inertia);
-		}
-		btBody->setMassProps(mass, inertia);
-	}
-	btBody->updateInertiaTensor();
-
-	reload_kinematic_shapes();
-	set_continuous_collision_detection(is_continuous_collision_detection_enabled());
-	reload_body();
-}
-
-void RigidBodyBullet::on_enter_area(AreaBullet *p_area) {
-	/// Add this area to the array in an ordered way
-	++areaWhereIamCount;
-	if (areaWhereIamCount >= maxAreasWhereIam) {
-		--areaWhereIamCount;
-		return;
-	}
-	for (int i = 0; i < areaWhereIamCount; ++i) {
-		if (nullptr == areasWhereIam[i]) {
-			// This area has the highest priority
-			areasWhereIam.write[i] = p_area;
-			break;
-		} else {
-			if (areasWhereIam[i]->get_spOv_priority() > p_area->get_spOv_priority()) {
-				// The position was found, just shift all elements
-				for (int j = areaWhereIamCount; j > i; j--) {
-					areasWhereIam.write[j] = areasWhereIam[j - 1];
-				}
-				areasWhereIam.write[i] = p_area;
-				break;
-			}
-		}
-	}
-	if (PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED != p_area->get_spOv_mode()) {
-		scratch_space_override_modificator();
-	}
-
-	if (p_area->is_spOv_gravityPoint()) {
-		++countGravityPointSpaces;
-		ERR_FAIL_COND(countGravityPointSpaces <= 0);
-	}
-}
-
-void RigidBodyBullet::on_exit_area(AreaBullet *p_area) {
-	RigidCollisionObjectBullet::on_exit_area(p_area);
-	/// Remove this area and keep the order
-	/// N.B. Since I don't want resize the array I can't use the "erase" function
-	bool wasTheAreaFound = false;
-	for (int i = 0; i < areaWhereIamCount; ++i) {
-		if (p_area == areasWhereIam[i]) {
-			// The area was found, just shift down all elements
-			for (int j = i; j < areaWhereIamCount; ++j) {
-				areasWhereIam.write[j] = areasWhereIam[j + 1];
-			}
-			wasTheAreaFound = true;
-			break;
-		}
-	}
-	if (wasTheAreaFound) {
-		if (p_area->is_spOv_gravityPoint()) {
-			--countGravityPointSpaces;
-			ERR_FAIL_COND(countGravityPointSpaces < 0);
-		}
-
-		--areaWhereIamCount;
-		areasWhereIam.write[areaWhereIamCount] = nullptr; // Even if this is not required, I clear the last element to be safe
-		if (PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED != p_area->get_spOv_mode()) {
-			scratch_space_override_modificator();
-		}
-	}
-}
-
-void RigidBodyBullet::reload_space_override_modificator() {
-	if (mode == PhysicsServer3D::BODY_MODE_STATIC) {
-		return;
-	}
-
-	Vector3 newGravity(0.0, 0.0, 0.0);
-	real_t newLinearDamp = MAX(0.0, linearDamp);
-	real_t newAngularDamp = MAX(0.0, angularDamp);
-
-	AreaBullet *currentArea;
-	// Variable used to calculate new gravity for gravity point areas, it is pointed by currentGravity pointer
-	Vector3 support_gravity(0, 0, 0);
-
-	bool stopped = false;
-	for (int i = areaWhereIamCount - 1; (0 <= i) && !stopped; --i) {
-		currentArea = areasWhereIam[i];
-
-		if (!currentArea || PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED == currentArea->get_spOv_mode()) {
-			continue;
-		}
-
-		/// Here is calculated the gravity
-		if (currentArea->is_spOv_gravityPoint()) {
-			/// It calculates the direction of new gravity
-			support_gravity = currentArea->get_transform().xform(currentArea->get_spOv_gravityVec()) - get_transform().get_origin();
-			real_t distanceMag = support_gravity.length();
-			// Normalized in this way to avoid the double call of function "length()"
-			if (distanceMag == 0) {
-				support_gravity.x = 0;
-				support_gravity.y = 0;
-				support_gravity.z = 0;
-			} else {
-				support_gravity.x /= distanceMag;
-				support_gravity.y /= distanceMag;
-				support_gravity.z /= distanceMag;
-			}
-
-			/// Here is calculated the final gravity
-			if (currentArea->get_spOv_gravityPointDistanceScale() > 0) {
-				// Scaled gravity by distance
-				support_gravity *= currentArea->get_spOv_gravityMag() / Math::pow(distanceMag * currentArea->get_spOv_gravityPointDistanceScale() + 1, 2);
-			} else {
-				// Unscaled gravity
-				support_gravity *= currentArea->get_spOv_gravityMag();
-			}
-		} else {
-			support_gravity = currentArea->get_spOv_gravityVec() * currentArea->get_spOv_gravityMag();
-		}
-
-		switch (currentArea->get_spOv_mode()) {
-			case PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED:
-				/// This area does not affect gravity/damp. These are generally areas
-				/// that exist only to detect collisions, and objects entering or exiting them.
-				break;
-			case PhysicsServer3D::AREA_SPACE_OVERRIDE_COMBINE:
-				/// This area adds its gravity/damp values to whatever has been
-				/// calculated so far. This way, many overlapping areas can combine
-				/// their physics to make interesting
-				newGravity += support_gravity;
-				newLinearDamp += currentArea->get_spOv_linearDamp();
-				newAngularDamp += currentArea->get_spOv_angularDamp();
-				break;
-			case PhysicsServer3D::AREA_SPACE_OVERRIDE_COMBINE_REPLACE:
-				/// This area adds its gravity/damp values to whatever has been calculated
-				/// so far. Then stops taking into account the rest of the areas, even the
-				/// default one.
-				newGravity += support_gravity;
-				newLinearDamp += currentArea->get_spOv_linearDamp();
-				newAngularDamp += currentArea->get_spOv_angularDamp();
-				stopped = true;
-				break;
-			case PhysicsServer3D::AREA_SPACE_OVERRIDE_REPLACE:
-				/// This area replaces any gravity/damp, even the default one, and
-				/// stops taking into account the rest of the areas.
-				newGravity = support_gravity;
-				newLinearDamp = currentArea->get_spOv_linearDamp();
-				newAngularDamp = currentArea->get_spOv_angularDamp();
-				stopped = true;
-				break;
-			case PhysicsServer3D::AREA_SPACE_OVERRIDE_REPLACE_COMBINE:
-				/// This area replaces any gravity/damp calculated so far, but keeps
-				/// calculating the rest of the areas, down to the default one.
-				newGravity = support_gravity;
-				newLinearDamp = currentArea->get_spOv_linearDamp();
-				newAngularDamp = currentArea->get_spOv_angularDamp();
-				break;
-		}
-	}
-
-	// Add default gravity and damping from space.
-	if (!stopped) {
-		newGravity += space->get_gravity_direction() * space->get_gravity_magnitude();
-		newLinearDamp += space->get_linear_damp();
-		newAngularDamp += space->get_angular_damp();
-	}
-
-	btVector3 newBtGravity;
-	G_TO_B(newGravity * gravity_scale, newBtGravity);
-
-	btBody->setGravity(newBtGravity);
-	btBody->setDamping(newLinearDamp, newAngularDamp);
-}
-
-void RigidBodyBullet::reload_kinematic_shapes() {
-	if (!kinematic_utilities) {
-		return;
-	}
-	kinematic_utilities->copyAllOwnerShapes();
-}
-
-void RigidBodyBullet::notify_transform_changed() {
-	RigidCollisionObjectBullet::notify_transform_changed();
-	can_integrate_forces = true;
-}
-
-void RigidBodyBullet::_internal_set_mass(real_t p_mass) {
-	btVector3 localInertia(0, 0, 0);
-
-	int clearedCurrentFlags = btBody->getCollisionFlags();
-	clearedCurrentFlags &= ~(btCollisionObject::CF_KINEMATIC_OBJECT | btCollisionObject::CF_STATIC_OBJECT | btCollisionObject::CF_CHARACTER_OBJECT);
-
-	// Rigidbody is dynamic if and only if mass is non Zero, otherwise static
-	const bool isDynamic = p_mass != 0.f;
-	if (isDynamic) {
-		if (PhysicsServer3D::BODY_MODE_DYNAMIC != mode && PhysicsServer3D::MODE_DYNAMIC_LINEAR != mode) {
-			return;
-		}
-
-		m_isStatic = false;
-		if (mainShape) {
-			mainShape->calculateLocalInertia(p_mass, localInertia);
-		}
-
-		if (PhysicsServer3D::BODY_MODE_DYNAMIC == mode) {
-			btBody->setCollisionFlags(clearedCurrentFlags); // Just set the flags without Kin and Static
-		} else {
-			btBody->setCollisionFlags(clearedCurrentFlags | btCollisionObject::CF_CHARACTER_OBJECT);
-		}
-
-		if (can_sleep) {
-			btBody->forceActivationState(ACTIVE_TAG); // ACTIVE_TAG 1
-		} else {
-			btBody->forceActivationState(DISABLE_DEACTIVATION); // DISABLE_DEACTIVATION 4
-		}
-	} else {
-		if (PhysicsServer3D::BODY_MODE_STATIC != mode && PhysicsServer3D::BODY_MODE_KINEMATIC != mode) {
-			return;
-		}
-
-		m_isStatic = true;
-		if (PhysicsServer3D::BODY_MODE_STATIC == mode) {
-			btBody->setCollisionFlags(clearedCurrentFlags | btCollisionObject::CF_STATIC_OBJECT);
-		} else {
-			btBody->setCollisionFlags(clearedCurrentFlags | btCollisionObject::CF_KINEMATIC_OBJECT);
-			set_transform__bullet(btBody->getWorldTransform()); // Set current Transform using kinematic method
-		}
-		btBody->forceActivationState(DISABLE_SIMULATION); // DISABLE_SIMULATION 5
-	}
-
-	btBody->setMassProps(p_mass, localInertia);
-	btBody->updateInertiaTensor();
-
-	reload_body();
-}

modules/bullet/rigid_body_bullet.h

@@ -1,328 +0,0 @@
-/*************************************************************************/
-/*  rigid_body_bullet.h                                                  */
-/*************************************************************************/
-/*                       This file is part of:                           */
-/*                           GODOT ENGINE                                */
-/*                      https://godotengine.org                          */
-/*************************************************************************/
-/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur.                 */
-/* Copyright (c) 2014-2022 Godot Engine contributors (cf. AUTHORS.md).   */
-/*                                                                       */
-/* Permission is hereby granted, free of charge, to any person obtaining */
-/* a copy of this software and associated documentation files (the       */
-/* "Software"), to deal in the Software without restriction, including   */
-/* without limitation the rights to use, copy, modify, merge, publish,   */
-/* distribute, sublicense, and/or sell copies of the Software, and to    */
-/* permit persons to whom the Software is furnished to do so, subject to */
-/* the following conditions:                                             */
-/*                                                                       */
-/* The above copyright notice and this permission notice shall be        */
-/* included in all copies or substantial portions of the Software.       */
-/*                                                                       */
-/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
-/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
-/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
-/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
-/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION 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 RIGID_BODY_BULLET_H
-#define RIGID_BODY_BULLET_H
-
-#include "collision_object_bullet.h"
-#include "space_bullet.h"
-
-#include <BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h>
-#include <LinearMath/btTransform.h>
-
-class AreaBullet;
-class SpaceBullet;
-class btRigidBody;
-class GodotMotionState;
-class BulletPhysicsDirectBodyState3D;
-
-/// This class could be used in multi thread with few changes but currently
-/// is set to be only in one single thread.
-///
-/// In the system there is only one object at a time that manage all bodies and is
-/// created by BulletPhysicsServer3D and is held by the "singleton" variable of this class
-/// Each time something require it, the body must be set again.
-class BulletPhysicsDirectBodyState3D : public PhysicsDirectBodyState3D {
-	GDCLASS(BulletPhysicsDirectBodyState3D, PhysicsDirectBodyState3D);
-
-	static BulletPhysicsDirectBodyState3D *singleton;
-
-public:
-	/// This class avoid the creation of more object of this class
-	static void initSingleton() {
-		if (!singleton) {
-			singleton = memnew(BulletPhysicsDirectBodyState3D);
-		}
-	}
-
-	static void destroySingleton() {
-		memdelete(singleton);
-		singleton = nullptr;
-	}
-
-	static void singleton_setDeltaTime(real_t p_deltaTime) {
-		singleton->deltaTime = p_deltaTime;
-	}
-
-	static BulletPhysicsDirectBodyState3D *get_singleton(RigidBodyBullet *p_body) {
-		singleton->body = p_body;
-		return singleton;
-	}
-
-public:
-	RigidBodyBullet *body = nullptr;
-	real_t deltaTime = 0.0;
-
-private:
-	BulletPhysicsDirectBodyState3D() {}
-
-public:
-	virtual Vector3 get_total_gravity() const override;
-	virtual real_t get_total_angular_damp() const override;
-	virtual real_t get_total_linear_damp() const override;
-
-	virtual Vector3 get_center_of_mass() const override;
-	virtual Basis get_principal_inertia_axes() const override;
-	// get the mass
-	virtual real_t get_inverse_mass() const override;
-	// get density of this body space
-	virtual Vector3 get_inverse_inertia() const override;
-	// get density of this body space
-	virtual Basis get_inverse_inertia_tensor() const override;
-
-	virtual void set_linear_velocity(const Vector3 &p_velocity) override;
-	virtual Vector3 get_linear_velocity() const override;
-
-	virtual void set_angular_velocity(const Vector3 &p_velocity) override;
-	virtual Vector3 get_angular_velocity() const override;
-
-	virtual void set_transform(const Transform3D &p_transform) override;
-	virtual Transform3D get_transform() const override;
-
-	virtual Vector3 get_velocity_at_local_position(const Vector3 &p_position) const override;
-
-	virtual void add_central_force(const Vector3 &p_force) override;
-	virtual void add_force(const Vector3 &p_force, const Vector3 &p_position = Vector3()) override;
-	virtual void add_torque(const Vector3 &p_torque) override;
-	virtual void apply_central_impulse(const Vector3 &p_impulse) override;
-	virtual void apply_impulse(const Vector3 &p_impulse, const Vector3 &p_position = Vector3()) override;
-	virtual void apply_torque_impulse(const Vector3 &p_impulse) override;
-
-	virtual void set_sleep_state(bool p_sleep) override;
-	virtual bool is_sleeping() const override;
-
-	virtual int get_contact_count() const override;
-
-	virtual Vector3 get_contact_local_position(int p_contact_idx) const override;
-	virtual Vector3 get_contact_local_normal(int p_contact_idx) const override;
-	virtual real_t get_contact_impulse(int p_contact_idx) const override;
-	virtual int get_contact_local_shape(int p_contact_idx) const override;
-
-	virtual RID get_contact_collider(int p_contact_idx) const override;
-	virtual Vector3 get_contact_collider_position(int p_contact_idx) const override;
-	virtual ObjectID get_contact_collider_id(int p_contact_idx) const override;
-	virtual int get_contact_collider_shape(int p_contact_idx) const override;
-	virtual Vector3 get_contact_collider_velocity_at_position(int p_contact_idx) const override;
-
-	virtual real_t get_step() const override { return deltaTime; }
-	virtual void integrate_forces() override {
-		// Skip the execution of this function
-	}
-
-	virtual PhysicsDirectSpaceState3D *get_space_state() override;
-};
-
-class RigidBodyBullet : public RigidCollisionObjectBullet {
-public:
-	struct CollisionData {
-		RigidBodyBullet *otherObject = nullptr;
-		int other_object_shape = 0;
-		int local_shape = 0;
-		Vector3 hitLocalLocation;
-		Vector3 hitWorldLocation;
-		Vector3 hitNormal;
-		real_t appliedImpulse = 0.0;
-	};
-
-	struct ForceIntegrationCallback {
-		Callable callable;
-		Variant udata;
-	};
-
-	/// Used to hold shapes
-	struct KinematicShape {
-		class btConvexShape *shape = nullptr;
-		btTransform transform;
-
-		KinematicShape() {}
-		bool is_active() const { return shape; }
-	};
-
-	struct KinematicUtilities {
-		RigidBodyBullet *owner = nullptr;
-		btScalar safe_margin;
-		Vector<KinematicShape> shapes;
-
-		KinematicUtilities(RigidBodyBullet *p_owner);
-		~KinematicUtilities();
-
-		void setSafeMargin(btScalar p_margin);
-		/// Used to set the default shape to ghost
-		void copyAllOwnerShapes();
-
-	private:
-		void just_delete_shapes(int new_size);
-	};
-
-private:
-	friend class BulletPhysicsDirectBodyState3D;
-
-	// This is required only for Kinematic movement
-	KinematicUtilities *kinematic_utilities = nullptr;
-
-	PhysicsServer3D::BodyMode mode;
-	GodotMotionState *godotMotionState;
-	btRigidBody *btBody;
-	uint16_t locked_axis = 0;
-	real_t mass = 1.0;
-	real_t gravity_scale = 1.0;
-	real_t linearDamp = 0.0;
-	real_t angularDamp = 0.0;
-	bool can_sleep = true;
-	bool omit_forces_integration = false;
-	bool can_integrate_forces = false;
-
-	Vector<CollisionData> collisions;
-	Vector<RigidBodyBullet *> collision_traces_1;
-	Vector<RigidBodyBullet *> collision_traces_2;
-	Vector<RigidBodyBullet *> *prev_collision_traces;
-	Vector<RigidBodyBullet *> *curr_collision_traces;
-
-	// these parameters are used to avoid vector resize
-	int maxCollisionsDetection = 0;
-	int collisionsCount = 0;
-	int prev_collision_count = 0;
-
-	Vector<AreaBullet *> areasWhereIam;
-	// these parameters are used to avoid vector resize
-	int maxAreasWhereIam = 10;
-	int areaWhereIamCount = 0;
-	// Used to know if the area is used as gravity point
-	int countGravityPointSpaces = 0;
-	bool isScratchedSpaceOverrideModificator = false;
-
-	bool previousActiveState = true; // Last check state
-
-	ForceIntegrationCallback *force_integration_callback = nullptr;
-
-public:
-	RigidBodyBullet();
-	~RigidBodyBullet();
-
-	void init_kinematic_utilities();
-	void destroy_kinematic_utilities();
-	_FORCE_INLINE_ KinematicUtilities *get_kinematic_utilities() const { return kinematic_utilities; }
-
-	_FORCE_INLINE_ btRigidBody *get_bt_rigid_body() { return btBody; }
-
-	virtual void main_shape_changed();
-	virtual void reload_body();
-	virtual void set_space(SpaceBullet *p_space);
-
-	virtual void dispatch_callbacks();
-	void set_force_integration_callback(const Callable &p_callable, const Variant &p_udata = Variant());
-	void scratch_space_override_modificator();
-
-	virtual void on_collision_filters_change();
-	virtual void on_collision_checker_start();
-	virtual void on_collision_checker_end();
-
-	void set_max_collisions_detection(int p_maxCollisionsDetection) {
-		ERR_FAIL_COND(0 > p_maxCollisionsDetection);
-
-		maxCollisionsDetection = p_maxCollisionsDetection;
-
-		collisions.resize(p_maxCollisionsDetection);
-		collision_traces_1.resize(p_maxCollisionsDetection);
-		collision_traces_2.resize(p_maxCollisionsDetection);
-
-		collisionsCount = 0;
-		prev_collision_count = MIN(prev_collision_count, p_maxCollisionsDetection);
-	}
-	int get_max_collisions_detection() {
-		return maxCollisionsDetection;
-	}
-
-	bool can_add_collision() { return collisionsCount < maxCollisionsDetection; }
-	bool add_collision_object(RigidBodyBullet *p_otherObject, const Vector3 &p_hitWorldLocation, const Vector3 &p_hitLocalLocation, const Vector3 &p_hitNormal, const real_t &p_appliedImpulse, int p_other_shape_index, int p_local_shape_index);
-	bool was_colliding(RigidBodyBullet *p_other_object);
-
-	void set_activation_state(bool p_active);
-	bool is_active() const;
-
-	void set_omit_forces_integration(bool p_omit);
-	_FORCE_INLINE_ bool get_omit_forces_integration() const { return omit_forces_integration; }
-
-	void set_param(PhysicsServer3D::BodyParameter p_param, real_t);
-	real_t get_param(PhysicsServer3D::BodyParameter p_param) const;
-
-	void set_mode(PhysicsServer3D::BodyMode p_mode);
-	PhysicsServer3D::BodyMode get_mode() const;
-
-	void set_state(PhysicsServer3D::BodyState p_state, const Variant &p_variant);
-	Variant get_state(PhysicsServer3D::BodyState p_state) const;
-
-	void apply_central_impulse(const Vector3 &p_impulse);
-	void apply_impulse(const Vector3 &p_impulse, const Vector3 &p_position = Vector3());
-	void apply_torque_impulse(const Vector3 &p_impulse);
-
-	void apply_central_force(const Vector3 &p_force);
-	void apply_force(const Vector3 &p_force, const Vector3 &p_position = Vector3());
-	void apply_torque(const Vector3 &p_torque);
-
-	void set_applied_force(const Vector3 &p_force);
-	Vector3 get_applied_force() const;
-	void set_applied_torque(const Vector3 &p_torque);
-	Vector3 get_applied_torque() const;
-
-	void set_axis_lock(PhysicsServer3D::BodyAxis p_axis, bool lock);
-	bool is_axis_locked(PhysicsServer3D::BodyAxis p_axis) const;
-	void reload_axis_lock();
-
-	/// Doc:
-	/// https://web.archive.org/web/20180404091446/https://www.bulletphysics.org/mediawiki-1.5.8/index.php/Anti_tunneling_by_Motion_Clamping
-	void set_continuous_collision_detection(bool p_enable);
-	bool is_continuous_collision_detection_enabled() const;
-
-	void set_linear_velocity(const Vector3 &p_velocity);
-	Vector3 get_linear_velocity() const;
-
-	void set_angular_velocity(const Vector3 &p_velocity);
-	Vector3 get_angular_velocity() const;
-
-	virtual void set_transform__bullet(const btTransform &p_global_transform);
-	virtual const btTransform &get_transform__bullet() const;
-
-	virtual void reload_shapes();
-
-	virtual void on_enter_area(AreaBullet *p_area);
-	virtual void on_exit_area(AreaBullet *p_area);
-	void reload_space_override_modificator();
-
-	/// Kinematic
-	void reload_kinematic_shapes();
-
-	virtual void notify_transform_changed();
-
-private:
-	void _internal_set_mass(real_t p_mass);
-};
-
-#endif // RIGID_BODY_BULLET_H

modules/bullet/shape_bullet.cpp

@@ -1,595 +0,0 @@
-/*************************************************************************/
-/*  shape_bullet.cpp                                                     */
-/*************************************************************************/
-/*                       This file is part of:                           */
-/*                           GODOT ENGINE                                */
-/*                      https://godotengine.org                          */
-/*************************************************************************/
-/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur.                 */
-/* Copyright (c) 2014-2022 Godot Engine contributors (cf. AUTHORS.md).   */
-/*                                                                       */
-/* Permission is hereby granted, free of charge, to any person obtaining */
-/* a copy of this software and associated documentation files (the       */
-/* "Software"), to deal in the Software without restriction, including   */
-/* without limitation the rights to use, copy, modify, merge, publish,   */
-/* distribute, sublicense, and/or sell copies of the Software, and to    */
-/* permit persons to whom the Software is furnished to do so, subject to */
-/* the following conditions:                                             */
-/*                                                                       */
-/* The above copyright notice and this permission notice shall be        */
-/* included in all copies or substantial portions of the Software.       */
-/*                                                                       */
-/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
-/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
-/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
-/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
-/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION 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 "shape_bullet.h"
-
-#include "btRayShape.h"
-#include "bullet_physics_server.h"
-#include "bullet_types_converter.h"
-#include "bullet_utilities.h"
-#include "core/config/project_settings.h"
-#include "shape_owner_bullet.h"
-
-#include <BulletCollision/CollisionDispatch/btInternalEdgeUtility.h>
-#include <BulletCollision/CollisionShapes/btConvexPointCloudShape.h>
-#include <BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h>
-#include <btBulletCollisionCommon.h>
-
-ShapeBullet::ShapeBullet() {}
-
-ShapeBullet::~ShapeBullet() {}
-
-btCollisionShape *ShapeBullet::create_bt_shape(const Vector3 &p_implicit_scale, real_t p_extra_edge) {
-	btVector3 s;
-	G_TO_B(p_implicit_scale, s);
-	return create_bt_shape(s, p_extra_edge);
-}
-
-btCollisionShape *ShapeBullet::prepare(btCollisionShape *p_btShape) const {
-	p_btShape->setUserPointer(const_cast<ShapeBullet *>(this));
-	p_btShape->setMargin(margin);
-	return p_btShape;
-}
-
-void ShapeBullet::notifyShapeChanged() {
-	for (const KeyValue<ShapeOwnerBullet *, int> &E : owners) {
-		ShapeOwnerBullet *owner = static_cast<ShapeOwnerBullet *>(E.key);
-		owner->shape_changed(owner->find_shape(this));
-	}
-}
-
-void ShapeBullet::add_owner(ShapeOwnerBullet *p_owner) {
-	Map<ShapeOwnerBullet *, int>::Element *E = owners.find(p_owner);
-	if (E) {
-		E->get()++;
-	} else {
-		owners[p_owner] = 1; // add new owner
-	}
-}
-
-void ShapeBullet::remove_owner(ShapeOwnerBullet *p_owner, bool p_permanentlyFromThisBody) {
-	Map<ShapeOwnerBullet *, int>::Element *E = owners.find(p_owner);
-	if (!E) {
-		return;
-	}
-	E->get()--;
-	if (p_permanentlyFromThisBody || 0 >= E->get()) {
-		owners.erase(E);
-	}
-}
-
-bool ShapeBullet::is_owner(ShapeOwnerBullet *p_owner) const {
-	return owners.has(p_owner);
-}
-
-const Map<ShapeOwnerBullet *, int> &ShapeBullet::get_owners() const {
-	return owners;
-}
-
-void ShapeBullet::set_margin(real_t p_margin) {
-	margin = p_margin;
-	notifyShapeChanged();
-}
-
-real_t ShapeBullet::get_margin() const {
-	return margin;
-}
-
-btEmptyShape *ShapeBullet::create_shape_empty() {
-	return bulletnew(btEmptyShape);
-}
-
-btStaticPlaneShape *ShapeBullet::create_shape_world_boundary(const btVector3 &planeNormal, btScalar planeConstant) {
-	return bulletnew(btStaticPlaneShape(planeNormal, planeConstant));
-}
-
-btSphereShape *ShapeBullet::create_shape_sphere(btScalar radius) {
-	return bulletnew(btSphereShape(radius));
-}
-
-btBoxShape *ShapeBullet::create_shape_box(const btVector3 &boxHalfExtents) {
-	return bulletnew(btBoxShape(boxHalfExtents));
-}
-
-btCapsuleShape *ShapeBullet::create_shape_capsule(btScalar radius, btScalar height) {
-	return bulletnew(btCapsuleShape(radius, height));
-}
-
-btCylinderShape *ShapeBullet::create_shape_cylinder(btScalar radius, btScalar height) {
-	return bulletnew(btCylinderShape(btVector3(radius, height / 2.0, radius)));
-}
-
-btConvexPointCloudShape *ShapeBullet::create_shape_convex(btAlignedObjectArray<btVector3> &p_vertices, const btVector3 &p_local_scaling) {
-	return bulletnew(btConvexPointCloudShape(&p_vertices[0], p_vertices.size(), p_local_scaling));
-}
-
-btScaledBvhTriangleMeshShape *ShapeBullet::create_shape_concave(btBvhTriangleMeshShape *p_mesh_shape, const btVector3 &p_local_scaling) {
-	if (p_mesh_shape) {
-		return bulletnew(btScaledBvhTriangleMeshShape(p_mesh_shape, p_local_scaling));
-	} else {
-		return nullptr;
-	}
-}
-
-btHeightfieldTerrainShape *ShapeBullet::create_shape_height_field(Vector<float> &p_heights, int p_width, int p_depth, real_t p_min_height, real_t p_max_height) {
-	const btScalar ignoredHeightScale(1);
-	const int YAxis = 1; // 0=X, 1=Y, 2=Z
-	const bool flipQuadEdges = false;
-	const void *heightsPtr = p_heights.ptr();
-
-	btHeightfieldTerrainShape *heightfield = bulletnew(btHeightfieldTerrainShape(p_width, p_depth, heightsPtr, ignoredHeightScale, p_min_height, p_max_height, YAxis, PHY_FLOAT, flipQuadEdges));
-
-	// The shape can be created without params when you do PhysicsServer3D.shape_create(PhysicsServer3D.SHAPE_HEIGHTMAP)
-	if (heightsPtr) {
-		heightfield->buildAccelerator(16);
-	}
-
-	return heightfield;
-}
-
-btRayShape *ShapeBullet::create_shape_ray(real_t p_length, bool p_slips_on_slope) {
-	btRayShape *r(bulletnew(btRayShape(p_length)));
-	r->setSlipsOnSlope(p_slips_on_slope);
-	return r;
-}
-
-/* World boundary */
-
-WorldBoundaryShapeBullet::WorldBoundaryShapeBullet() :
-		ShapeBullet() {}
-
-void WorldBoundaryShapeBullet::set_data(const Variant &p_data) {
-	setup(p_data);
-}
-
-Variant WorldBoundaryShapeBullet::get_data() const {
-	return plane;
-}
-
-PhysicsServer3D::ShapeType WorldBoundaryShapeBullet::get_type() const {
-	return PhysicsServer3D::SHAPE_WORLD_BOUNDARY;
-}
-
-void WorldBoundaryShapeBullet::setup(const Plane &p_plane) {
-	plane = p_plane;
-	notifyShapeChanged();
-}
-
-btCollisionShape *WorldBoundaryShapeBullet::create_bt_shape(const btVector3 &p_implicit_scale, real_t p_extra_edge) {
-	btVector3 btPlaneNormal;
-	G_TO_B(plane.normal, btPlaneNormal);
-	return prepare(WorldBoundaryShapeBullet::create_shape_world_boundary(btPlaneNormal, plane.d));
-}
-
-/* Sphere */
-
-SphereShapeBullet::SphereShapeBullet() :
-		ShapeBullet() {}
-
-void SphereShapeBullet::set_data(const Variant &p_data) {
-	setup(p_data);
-}
-
-Variant SphereShapeBullet::get_data() const {
-	return radius;
-}
-
-PhysicsServer3D::ShapeType SphereShapeBullet::get_type() const {
-	return PhysicsServer3D::SHAPE_SPHERE;
-}
-
-void SphereShapeBullet::setup(real_t p_radius) {
-	radius = p_radius;
-	notifyShapeChanged();
-}
-
-btCollisionShape *SphereShapeBullet::create_bt_shape(const btVector3 &p_implicit_scale, real_t p_extra_edge) {
-	return prepare(ShapeBullet::create_shape_sphere(radius * p_implicit_scale[0] + p_extra_edge));
-}
-
-/* Box */
-BoxShapeBullet::BoxShapeBullet() :
-		ShapeBullet() {}
-
-void BoxShapeBullet::set_data(const Variant &p_data) {
-	setup(p_data);
-}
-
-Variant BoxShapeBullet::get_data() const {
-	Vector3 g_half_extents;
-	B_TO_G(half_extents, g_half_extents);
-	return g_half_extents;
-}
-
-PhysicsServer3D::ShapeType BoxShapeBullet::get_type() const {
-	return PhysicsServer3D::SHAPE_BOX;
-}
-
-void BoxShapeBullet::setup(const Vector3 &p_half_extents) {
-	G_TO_B(p_half_extents, half_extents);
-	notifyShapeChanged();
-}
-
-btCollisionShape *BoxShapeBullet::create_bt_shape(const btVector3 &p_implicit_scale, real_t p_extra_edge) {
-	return prepare(ShapeBullet::create_shape_box((half_extents * p_implicit_scale) + btVector3(p_extra_edge, p_extra_edge, p_extra_edge)));
-}
-
-/* Capsule */
-
-CapsuleShapeBullet::CapsuleShapeBullet() :
-		ShapeBullet() {}
-
-void CapsuleShapeBullet::set_data(const Variant &p_data) {
-	Dictionary d = p_data;
-	ERR_FAIL_COND(!d.has("radius"));
-	ERR_FAIL_COND(!d.has("height"));
-	setup(d["height"], d["radius"]);
-}
-
-Variant CapsuleShapeBullet::get_data() const {
-	Dictionary d;
-	d["radius"] = radius;
-	d["height"] = height;
-	return d;
-}
-
-PhysicsServer3D::ShapeType CapsuleShapeBullet::get_type() const {
-	return PhysicsServer3D::SHAPE_CAPSULE;
-}
-
-void CapsuleShapeBullet::setup(real_t p_height, real_t p_radius) {
-	radius = p_radius;
-	height = p_height;
-	notifyShapeChanged();
-}
-
-btCollisionShape *CapsuleShapeBullet::create_bt_shape(const btVector3 &p_implicit_scale, real_t p_extra_edge) {
-	return prepare(ShapeBullet::create_shape_capsule(radius * p_implicit_scale[0] + p_extra_edge, height * p_implicit_scale[1]));
-}
-
-/* Cylinder */
-
-CylinderShapeBullet::CylinderShapeBullet() :
-		ShapeBullet() {}
-
-void CylinderShapeBullet::set_data(const Variant &p_data) {
-	Dictionary d = p_data;
-	ERR_FAIL_COND(!d.has("radius"));
-	ERR_FAIL_COND(!d.has("height"));
-	setup(d["height"], d["radius"]);
-}
-
-Variant CylinderShapeBullet::get_data() const {
-	Dictionary d;
-	d["radius"] = radius;
-	d["height"] = height;
-	return d;
-}
-
-PhysicsServer3D::ShapeType CylinderShapeBullet::get_type() const {
-	return PhysicsServer3D::SHAPE_CYLINDER;
-}
-
-void CylinderShapeBullet::setup(real_t p_height, real_t p_radius) {
-	radius = p_radius;
-	height = p_height;
-	notifyShapeChanged();
-}
-
-btCollisionShape *CylinderShapeBullet::create_bt_shape(const btVector3 &p_implicit_scale, real_t p_margin) {
-	return prepare(ShapeBullet::create_shape_cylinder(radius * p_implicit_scale[0] + p_margin, height * p_implicit_scale[1] + p_margin));
-}
-
-/* Convex polygon */
-
-ConvexPolygonShapeBullet::ConvexPolygonShapeBullet() :
-		ShapeBullet() {}
-
-void ConvexPolygonShapeBullet::set_data(const Variant &p_data) {
-	setup(p_data);
-}
-
-void ConvexPolygonShapeBullet::get_vertices(Vector<Vector3> &out_vertices) {
-	const int n_of_vertices = vertices.size();
-	out_vertices.resize(n_of_vertices);
-	for (int i = n_of_vertices - 1; 0 <= i; --i) {
-		B_TO_G(vertices[i], out_vertices.write[i]);
-	}
-}
-
-Variant ConvexPolygonShapeBullet::get_data() const {
-	ConvexPolygonShapeBullet *variable_self = const_cast<ConvexPolygonShapeBullet *>(this);
-	Vector<Vector3> out_vertices;
-	variable_self->get_vertices(out_vertices);
-	return out_vertices;
-}
-
-PhysicsServer3D::ShapeType ConvexPolygonShapeBullet::get_type() const {
-	return PhysicsServer3D::SHAPE_CONVEX_POLYGON;
-}
-
-void ConvexPolygonShapeBullet::setup(const Vector<Vector3> &p_vertices) {
-	// Make a copy of vertices
-	const int n_of_vertices = p_vertices.size();
-	vertices.resize(n_of_vertices);
-	for (int i = n_of_vertices - 1; 0 <= i; --i) {
-		G_TO_B(p_vertices[i], vertices[i]);
-	}
-	notifyShapeChanged();
-}
-
-btCollisionShape *ConvexPolygonShapeBullet::create_bt_shape(const btVector3 &p_implicit_scale, real_t p_extra_edge) {
-	if (!vertices.size()) {
-		// This is necessary since 0 vertices
-		return prepare(ShapeBullet::create_shape_empty());
-	}
-	btCollisionShape *cs(ShapeBullet::create_shape_convex(vertices));
-	cs->setLocalScaling(p_implicit_scale);
-	prepare(cs);
-	return cs;
-}
-
-/* Concave polygon */
-
-ConcavePolygonShapeBullet::ConcavePolygonShapeBullet() :
-		ShapeBullet() {}
-
-ConcavePolygonShapeBullet::~ConcavePolygonShapeBullet() {
-	if (meshShape) {
-		delete meshShape->getMeshInterface();
-		delete meshShape->getTriangleInfoMap();
-		bulletdelete(meshShape);
-	}
-	faces = Vector<Vector3>();
-}
-
-void ConcavePolygonShapeBullet::set_data(const Variant &p_data) {
-	Dictionary d = p_data;
-	ERR_FAIL_COND(!d.has("faces"));
-
-	setup(d["faces"]);
-}
-
-Variant ConcavePolygonShapeBullet::get_data() const {
-	Dictionary d;
-	d["faces"] = faces;
-
-	return d;
-}
-
-PhysicsServer3D::ShapeType ConcavePolygonShapeBullet::get_type() const {
-	return PhysicsServer3D::SHAPE_CONCAVE_POLYGON;
-}
-
-void ConcavePolygonShapeBullet::setup(Vector<Vector3> p_faces) {
-	faces = p_faces;
-	if (meshShape) {
-		/// Clear previous created shape
-		delete meshShape->getMeshInterface();
-		delete meshShape->getTriangleInfoMap();
-		bulletdelete(meshShape);
-	}
-	int src_face_count = faces.size();
-	if (0 < src_face_count) {
-		// It counts the faces and assert the array contains the correct number of vertices.
-		ERR_FAIL_COND(src_face_count % 3);
-
-		btTriangleMesh *shapeInterface = bulletnew(btTriangleMesh);
-		src_face_count /= 3;
-		const Vector3 *r = p_faces.ptr();
-		const Vector3 *facesr = r;
-
-		btVector3 supVec_0;
-		btVector3 supVec_1;
-		btVector3 supVec_2;
-		for (int i = 0; i < src_face_count; ++i) {
-			G_TO_B(facesr[i * 3 + 0], supVec_0);
-			G_TO_B(facesr[i * 3 + 1], supVec_1);
-			G_TO_B(facesr[i * 3 + 2], supVec_2);
-
-			// Inverted from standard godot otherwise btGenerateInternalEdgeInfo generates wrong edge info
-			shapeInterface->addTriangle(supVec_2, supVec_1, supVec_0);
-		}
-
-		const bool useQuantizedAabbCompression = true;
-
-		meshShape = bulletnew(btBvhTriangleMeshShape(shapeInterface, useQuantizedAabbCompression));
-
-		if (GLOBAL_GET("physics/3d/smooth_trimesh_collision")) {
-			btTriangleInfoMap *triangleInfoMap = new btTriangleInfoMap();
-			btGenerateInternalEdgeInfo(meshShape, triangleInfoMap);
-		}
-	} else {
-		meshShape = nullptr;
-		ERR_PRINT("The faces count are 0, the mesh shape cannot be created");
-	}
-	notifyShapeChanged();
-}
-
-btCollisionShape *ConcavePolygonShapeBullet::create_bt_shape(const btVector3 &p_implicit_scale, real_t p_extra_edge) {
-	btCollisionShape *cs = ShapeBullet::create_shape_concave(meshShape);
-	if (!cs) {
-		// This is necessary since if 0 faces the creation of concave return null
-		cs = ShapeBullet::create_shape_empty();
-	}
-	cs->setLocalScaling(p_implicit_scale);
-	prepare(cs);
-	cs->setMargin(0);
-	return cs;
-}
-
-/* Height map shape */
-
-HeightMapShapeBullet::HeightMapShapeBullet() :
-		ShapeBullet() {}
-
-void HeightMapShapeBullet::set_data(const Variant &p_data) {
-	ERR_FAIL_COND(p_data.get_type() != Variant::DICTIONARY);
-	Dictionary d = p_data;
-	ERR_FAIL_COND(!d.has("width"));
-	ERR_FAIL_COND(!d.has("depth"));
-	ERR_FAIL_COND(!d.has("heights"));
-
-	real_t l_min_height = 0.0;
-	real_t l_max_height = 0.0;
-
-	// If specified, min and max height will be used as precomputed values
-	if (d.has("min_height")) {
-		l_min_height = d["min_height"];
-	}
-	if (d.has("max_height")) {
-		l_max_height = d["max_height"];
-	}
-
-	ERR_FAIL_COND(l_min_height > l_max_height);
-
-	int l_width = d["width"];
-	int l_depth = d["depth"];
-
-	ERR_FAIL_COND_MSG(l_width < 2, "Map width must be at least 2.");
-	ERR_FAIL_COND_MSG(l_depth < 2, "Map depth must be at least 2.");
-
-	Vector<float> l_heights;
-	Variant l_heights_v = d["heights"];
-
-	if (l_heights_v.get_type() == Variant::PACKED_FLOAT32_ARRAY) {
-		// Ready-to-use heights can be passed
-
-		l_heights = l_heights_v;
-
-	} else if (l_heights_v.get_type() == Variant::OBJECT) {
-		// If an image is passed, we have to convert it to a format Bullet supports.
-		// this would be expensive to do with a script, so it's nice to have it here.
-
-		Ref<Image> l_image = l_heights_v;
-		ERR_FAIL_COND(l_image.is_null());
-
-		// Float is the only common format between Godot and Bullet that can be used for decent collision.
-		// (Int16 would be nice too but we still don't have it)
-		// We could convert here automatically but it's better to not be intrusive and let the caller do it if necessary.
-		ERR_FAIL_COND(l_image->get_format() != Image::FORMAT_RF);
-
-		PackedByteArray im_data = l_image->get_data();
-
-		l_heights.resize(l_image->get_width() * l_image->get_height());
-
-		float *w = l_heights.ptrw();
-		const uint8_t *r = im_data.ptr();
-		float *rp = (float *)r;
-		// At this point, `rp` could be used directly for Bullet, but I don't know how safe it would be.
-
-		for (int i = 0; i < l_heights.size(); ++i) {
-			w[i] = rp[i];
-		}
-
-	} else {
-		ERR_FAIL_MSG("Expected PackedFloat32Array or float Image.");
-	}
-
-	ERR_FAIL_COND(l_width <= 0);
-	ERR_FAIL_COND(l_depth <= 0);
-	ERR_FAIL_COND(l_heights.size() != (l_width * l_depth));
-
-	// Compute min and max heights if not specified.
-	if (!d.has("min_height") && !d.has("max_height")) {
-		const float *r = l_heights.ptr();
-		int heights_size = l_heights.size();
-
-		for (int i = 0; i < heights_size; ++i) {
-			float h = r[i];
-
-			if (h < l_min_height) {
-				l_min_height = h;
-			} else if (h > l_max_height) {
-				l_max_height = h;
-			}
-		}
-	}
-
-	setup(l_heights, l_width, l_depth, l_min_height, l_max_height);
-}
-
-Variant HeightMapShapeBullet::get_data() const {
-	ERR_FAIL_V(Variant());
-}
-
-PhysicsServer3D::ShapeType HeightMapShapeBullet::get_type() const {
-	return PhysicsServer3D::SHAPE_HEIGHTMAP;
-}
-
-void HeightMapShapeBullet::setup(Vector<float> &p_heights, int p_width, int p_depth, real_t p_min_height, real_t p_max_height) {
-	// TODO cell size must be tweaked using localScaling, which is a shared property for all Bullet shapes
-
-	// If this array is resized outside of here, it should be preserved due to CoW
-	heights = p_heights;
-
-	width = p_width;
-	depth = p_depth;
-	min_height = p_min_height;
-	max_height = p_max_height;
-	notifyShapeChanged();
-}
-
-btCollisionShape *HeightMapShapeBullet::create_bt_shape(const btVector3 &p_implicit_scale, real_t p_extra_edge) {
-	btCollisionShape *cs(ShapeBullet::create_shape_height_field(heights, width, depth, min_height, max_height));
-	cs->setLocalScaling(p_implicit_scale);
-	prepare(cs);
-	return cs;
-}
-
-/* Ray shape */
-RayShapeBullet::RayShapeBullet() :
-		ShapeBullet() {}
-
-void RayShapeBullet::set_data(const Variant &p_data) {
-	Dictionary d = p_data;
-	setup(d["length"], d["slips_on_slope"]);
-}
-
-Variant RayShapeBullet::get_data() const {
-	Dictionary d;
-	d["length"] = length;
-	d["slips_on_slope"] = slips_on_slope;
-	return d;
-}
-
-PhysicsServer3D::ShapeType RayShapeBullet::get_type() const {
-	return PhysicsServer3D::SHAPE_RAY;
-}
-
-void RayShapeBullet::setup(real_t p_length, bool p_slips_on_slope) {
-	length = p_length;
-	slips_on_slope = p_slips_on_slope;
-	notifyShapeChanged();
-}
-
-btCollisionShape *RayShapeBullet::create_bt_shape(const btVector3 &p_implicit_scale, real_t p_extra_edge) {
-	return prepare(ShapeBullet::create_shape_ray(length * p_implicit_scale[1] + p_extra_edge, slips_on_slope));
-}

modules/bullet/shape_bullet.h

@@ -1,244 +0,0 @@
-/*************************************************************************/
-/*  shape_bullet.h                                                       */
-/*************************************************************************/
-/*                       This file is part of:                           */
-/*                           GODOT ENGINE                                */
-/*                      https://godotengine.org                          */
-/*************************************************************************/
-/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur.                 */
-/* Copyright (c) 2014-2022 Godot Engine contributors (cf. AUTHORS.md).   */
-/*                                                                       */
-/* Permission is hereby granted, free of charge, to any person obtaining */
-/* a copy of this software and associated documentation files (the       */
-/* "Software"), to deal in the Software without restriction, including   */
-/* without limitation the rights to use, copy, modify, merge, publish,   */
-/* distribute, sublicense, and/or sell copies of the Software, and to    */
-/* permit persons to whom the Software is furnished to do so, subject to */
-/* the following conditions:                                             */
-/*                                                                       */
-/* The above copyright notice and this permission notice shall be        */
-/* included in all copies or substantial portions of the Software.       */
-/*                                                                       */
-/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
-/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
-/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
-/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
-/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION 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 SHAPE_BULLET_H
-#define SHAPE_BULLET_H
-
-#include "core/math/geometry_3d.h"
-#include "core/variant/variant.h"
-#include "rid_bullet.h"
-#include "servers/physics_server_3d.h"
-
-#include <LinearMath/btAlignedObjectArray.h>
-#include <LinearMath/btScalar.h>
-#include <LinearMath/btVector3.h>
-
-class ShapeBullet;
-class btCollisionShape;
-class ShapeOwnerBullet;
-class btBvhTriangleMeshShape;
-
-class ShapeBullet : public RIDBullet {
-	Map<ShapeOwnerBullet *, int> owners;
-	real_t margin = 0.04;
-
-protected:
-	/// return self
-	btCollisionShape *prepare(btCollisionShape *p_btShape) const;
-	void notifyShapeChanged();
-
-public:
-	ShapeBullet();
-	virtual ~ShapeBullet();
-
-	btCollisionShape *create_bt_shape(const Vector3 &p_implicit_scale, real_t p_extra_edge = 0);
-	virtual btCollisionShape *create_bt_shape(const btVector3 &p_implicit_scale, real_t p_extra_edge = 0) = 0;
-
-	void add_owner(ShapeOwnerBullet *p_owner);
-	void remove_owner(ShapeOwnerBullet *p_owner, bool p_permanentlyFromThisBody = false);
-	bool is_owner(ShapeOwnerBullet *p_owner) const;
-	const Map<ShapeOwnerBullet *, int> &get_owners() const;
-
-	void set_margin(real_t p_margin);
-	real_t get_margin() const;
-
-	/// Setup the shape
-	virtual void set_data(const Variant &p_data) = 0;
-	virtual Variant get_data() const = 0;
-
-	virtual PhysicsServer3D::ShapeType get_type() const = 0;
-
-public:
-	static class btEmptyShape *create_shape_empty();
-	static class btStaticPlaneShape *create_shape_world_boundary(const btVector3 &planeNormal, btScalar planeConstant);
-	static class btSphereShape *create_shape_sphere(btScalar radius);
-	static class btBoxShape *create_shape_box(const btVector3 &boxHalfExtents);
-	static class btCapsuleShape *create_shape_capsule(btScalar radius, btScalar height);
-	static class btCylinderShape *create_shape_cylinder(btScalar radius, btScalar height);
-	/// IMPORTANT: Remember to delete the shape interface by calling: delete my_shape->getMeshInterface();
-	static class btConvexPointCloudShape *create_shape_convex(btAlignedObjectArray<btVector3> &p_vertices, const btVector3 &p_local_scaling = btVector3(1, 1, 1));
-	static class btScaledBvhTriangleMeshShape *create_shape_concave(btBvhTriangleMeshShape *p_mesh_shape, const btVector3 &p_local_scaling = btVector3(1, 1, 1));
-	static class btHeightfieldTerrainShape *create_shape_height_field(Vector<float> &p_heights, int p_width, int p_depth, real_t p_min_height, real_t p_max_height);
-	static class btRayShape *create_shape_ray(real_t p_length, bool p_slips_on_slope);
-};
-
-class WorldBoundaryShapeBullet : public ShapeBullet {
-	Plane plane;
-
-public:
-	WorldBoundaryShapeBullet();
-
-	virtual void set_data(const Variant &p_data);
-	virtual Variant get_data() const;
-	virtual PhysicsServer3D::ShapeType get_type() const;
-	virtual btCollisionShape *create_bt_shape(const btVector3 &p_implicit_scale, real_t p_extra_edge = 0);
-
-private:
-	void setup(const Plane &p_plane);
-};
-
-class SphereShapeBullet : public ShapeBullet {
-	real_t radius = 0.0;
-
-public:
-	SphereShapeBullet();
-
-	_FORCE_INLINE_ real_t get_radius() { return radius; }
-	virtual void set_data(const Variant &p_data);
-	virtual Variant get_data() const;
-	virtual PhysicsServer3D::ShapeType get_type() const;
-	virtual btCollisionShape *create_bt_shape(const btVector3 &p_implicit_scale, real_t p_extra_edge = 0);
-
-private:
-	void setup(real_t p_radius);
-};
-
-class BoxShapeBullet : public ShapeBullet {
-	btVector3 half_extents;
-
-public:
-	BoxShapeBullet();
-
-	_FORCE_INLINE_ const btVector3 &get_half_extents() { return half_extents; }
-	virtual void set_data(const Variant &p_data);
-	virtual Variant get_data() const;
-	virtual PhysicsServer3D::ShapeType get_type() const;
-	virtual btCollisionShape *create_bt_shape(const btVector3 &p_implicit_scale, real_t p_extra_edge = 0);
-
-private:
-	void setup(const Vector3 &p_half_extents);
-};
-
-class CapsuleShapeBullet : public ShapeBullet {
-	real_t height = 0.0;
-	real_t radius = 0.0;
-
-public:
-	CapsuleShapeBullet();
-
-	_FORCE_INLINE_ real_t get_height() { return height; }
-	_FORCE_INLINE_ real_t get_radius() { return radius; }
-	virtual void set_data(const Variant &p_data);
-	virtual Variant get_data() const;
-	virtual PhysicsServer3D::ShapeType get_type() const;
-	virtual btCollisionShape *create_bt_shape(const btVector3 &p_implicit_scale, real_t p_extra_edge = 0);
-
-private:
-	void setup(real_t p_height, real_t p_radius);
-};
-
-class CylinderShapeBullet : public ShapeBullet {
-	real_t height = 0.0;
-	real_t radius = 0.0;
-
-public:
-	CylinderShapeBullet();
-
-	_FORCE_INLINE_ real_t get_height() { return height; }
-	_FORCE_INLINE_ real_t get_radius() { return radius; }
-	virtual void set_data(const Variant &p_data);
-	virtual Variant get_data() const;
-	virtual PhysicsServer3D::ShapeType get_type() const;
-	virtual btCollisionShape *create_bt_shape(const btVector3 &p_implicit_scale, real_t p_margin = 0);
-
-private:
-	void setup(real_t p_height, real_t p_radius);
-};
-
-class ConvexPolygonShapeBullet : public ShapeBullet {
-public:
-	btAlignedObjectArray<btVector3> vertices;
-
-	ConvexPolygonShapeBullet();
-
-	virtual void set_data(const Variant &p_data);
-	void get_vertices(Vector<Vector3> &out_vertices);
-	virtual Variant get_data() const;
-	virtual PhysicsServer3D::ShapeType get_type() const;
-	virtual btCollisionShape *create_bt_shape(const btVector3 &p_implicit_scale, real_t p_extra_edge = 0);
-
-private:
-	void setup(const Vector<Vector3> &p_vertices);
-};
-
-class ConcavePolygonShapeBullet : public ShapeBullet {
-	class btBvhTriangleMeshShape *meshShape = nullptr;
-
-public:
-	Vector<Vector3> faces;
-
-	ConcavePolygonShapeBullet();
-	virtual ~ConcavePolygonShapeBullet();
-
-	virtual void set_data(const Variant &p_data);
-	virtual Variant get_data() const;
-	virtual PhysicsServer3D::ShapeType get_type() const;
-	virtual btCollisionShape *create_bt_shape(const btVector3 &p_implicit_scale, real_t p_extra_edge = 0);
-
-private:
-	void setup(Vector<Vector3> p_faces);
-};
-
-class HeightMapShapeBullet : public ShapeBullet {
-public:
-	Vector<float> heights;
-	int width = 0;
-	int depth = 0;
-	real_t min_height = 0.0;
-	real_t max_height = 0.0;
-
-	HeightMapShapeBullet();
-
-	virtual void set_data(const Variant &p_data);
-	virtual Variant get_data() const;
-	virtual PhysicsServer3D::ShapeType get_type() const;
-	virtual btCollisionShape *create_bt_shape(const btVector3 &p_implicit_scale, real_t p_extra_edge = 0);
-
-private:
-	void setup(Vector<float> &p_heights, int p_width, int p_depth, real_t p_min_height, real_t p_max_height);
-};
-
-class RayShapeBullet : public ShapeBullet {
-public:
-	real_t length = 1.0;
-	bool slips_on_slope = false;
-
-	RayShapeBullet();
-
-	virtual void set_data(const Variant &p_data);
-	virtual Variant get_data() const;
-	virtual PhysicsServer3D::ShapeType get_type() const;
-	virtual btCollisionShape *create_bt_shape(const btVector3 &p_implicit_scale, real_t p_extra_edge = 0);
-
-private:
-	void setup(real_t p_length, bool p_slips_on_slope);
-};
-
-#endif // SHAPE_BULLET_H

modules/bullet/shape_owner_bullet.h

@@ -1,51 +0,0 @@
-/*************************************************************************/
-/*  shape_owner_bullet.h                                                 */
-/*************************************************************************/
-/*                       This file is part of:                           */
-/*                           GODOT ENGINE                                */
-/*                      https://godotengine.org                          */
-/*************************************************************************/
-/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur.                 */
-/* Copyright (c) 2014-2022 Godot Engine contributors (cf. AUTHORS.md).   */
-/*                                                                       */
-/* Permission is hereby granted, free of charge, to any person obtaining */
-/* a copy of this software and associated documentation files (the       */
-/* "Software"), to deal in the Software without restriction, including   */
-/* without limitation the rights to use, copy, modify, merge, publish,   */
-/* distribute, sublicense, and/or sell copies of the Software, and to    */
-/* permit persons to whom the Software is furnished to do so, subject to */
-/* the following conditions:                                             */
-/*                                                                       */
-/* The above copyright notice and this permission notice shall be        */
-/* included in all copies or substantial portions of the Software.       */
-/*                                                                       */
-/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
-/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
-/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
-/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
-/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION 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 SHAPE_OWNER_BULLET_H
-#define SHAPE_OWNER_BULLET_H
-
-#include "rid_bullet.h"
-
-class ShapeBullet;
-class btCollisionShape;
-class CollisionObjectBullet;
-
-/// Each class that want to use Shapes must inherit this class
-/// E.G. BodyShape is a child of this
-class ShapeOwnerBullet {
-public:
-	virtual int find_shape(ShapeBullet *p_shape) const = 0;
-	virtual void shape_changed(int p_shape_index) = 0;
-	virtual void reload_shapes() = 0;
-	virtual void remove_shape_full(class ShapeBullet *p_shape) = 0;
-	virtual ~ShapeOwnerBullet() {}
-};
-
-#endif // SHAPE_OWNER_BULLET_H

modules/bullet/slider_joint_bullet.cpp

@@ -1,461 +0,0 @@
-/*************************************************************************/
-/*  slider_joint_bullet.cpp                                              */
-/*************************************************************************/
-/*                       This file is part of:                           */
-/*                           GODOT ENGINE                                */
-/*                      https://godotengine.org                          */
-/*************************************************************************/
-/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur.                 */
-/* Copyright (c) 2014-2022 Godot Engine contributors (cf. AUTHORS.md).   */
-/*                                                                       */
-/* Permission is hereby granted, free of charge, to any person obtaining */
-/* a copy of this software and associated documentation files (the       */
-/* "Software"), to deal in the Software without restriction, including   */
-/* without limitation the rights to use, copy, modify, merge, publish,   */
-/* distribute, sublicense, and/or sell copies of the Software, and to    */
-/* permit persons to whom the Software is furnished to do so, subject to */
-/* the following conditions:                                             */
-/*                                                                       */
-/* The above copyright notice and this permission notice shall be        */
-/* included in all copies or substantial portions of the Software.       */
-/*                                                                       */
-/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
-/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
-/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
-/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
-/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION 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 "slider_joint_bullet.h"
-
-#include "bullet_types_converter.h"
-#include "bullet_utilities.h"
-#include "rigid_body_bullet.h"
-
-#include <BulletDynamics/ConstraintSolver/btSliderConstraint.h>
-
-SliderJointBullet::SliderJointBullet(RigidBodyBullet *rbA, RigidBodyBullet *rbB, const Transform3D &frameInA, const Transform3D &frameInB) :
-		JointBullet() {
-	Transform3D scaled_AFrame(frameInA.scaled(rbA->get_body_scale()));
-	scaled_AFrame.basis.rotref_posscale_decomposition(scaled_AFrame.basis);
-
-	btTransform btFrameA;
-	G_TO_B(scaled_AFrame, btFrameA);
-
-	if (rbB) {
-		Transform3D scaled_BFrame(frameInB.scaled(rbB->get_body_scale()));
-		scaled_BFrame.basis.rotref_posscale_decomposition(scaled_BFrame.basis);
-
-		btTransform btFrameB;
-		G_TO_B(scaled_BFrame, btFrameB);
-		sliderConstraint = bulletnew(btSliderConstraint(*rbA->get_bt_rigid_body(), *rbB->get_bt_rigid_body(), btFrameA, btFrameB, true));
-
-	} else {
-		sliderConstraint = bulletnew(btSliderConstraint(*rbA->get_bt_rigid_body(), btFrameA, true));
-	}
-	setup(sliderConstraint);
-}
-
-const RigidBodyBullet *SliderJointBullet::getRigidBodyA() const {
-	return static_cast<RigidBodyBullet *>(sliderConstraint->getRigidBodyA().getUserPointer());
-}
-
-const RigidBodyBullet *SliderJointBullet::getRigidBodyB() const {
-	return static_cast<RigidBodyBullet *>(sliderConstraint->getRigidBodyB().getUserPointer());
-}
-
-const Transform3D SliderJointBullet::getCalculatedTransformA() const {
-	btTransform btTransform = sliderConstraint->getCalculatedTransformA();
-	Transform3D gTrans;
-	B_TO_G(btTransform, gTrans);
-	return gTrans;
-}
-
-const Transform3D SliderJointBullet::getCalculatedTransformB() const {
-	btTransform btTransform = sliderConstraint->getCalculatedTransformB();
-	Transform3D gTrans;
-	B_TO_G(btTransform, gTrans);
-	return gTrans;
-}
-
-const Transform3D SliderJointBullet::getFrameOffsetA() const {
-	btTransform btTransform = sliderConstraint->getFrameOffsetA();
-	Transform3D gTrans;
-	B_TO_G(btTransform, gTrans);
-	return gTrans;
-}
-
-const Transform3D SliderJointBullet::getFrameOffsetB() const {
-	btTransform btTransform = sliderConstraint->getFrameOffsetB();
-	Transform3D gTrans;
-	B_TO_G(btTransform, gTrans);
-	return gTrans;
-}
-
-Transform3D SliderJointBullet::getFrameOffsetA() {
-	btTransform btTransform = sliderConstraint->getFrameOffsetA();
-	Transform3D gTrans;
-	B_TO_G(btTransform, gTrans);
-	return gTrans;
-}
-
-Transform3D SliderJointBullet::getFrameOffsetB() {
-	btTransform btTransform = sliderConstraint->getFrameOffsetB();
-	Transform3D gTrans;
-	B_TO_G(btTransform, gTrans);
-	return gTrans;
-}
-
-real_t SliderJointBullet::getLowerLinLimit() const {
-	return sliderConstraint->getLowerLinLimit();
-}
-
-void SliderJointBullet::setLowerLinLimit(real_t lowerLimit) {
-	sliderConstraint->setLowerLinLimit(lowerLimit);
-}
-
-real_t SliderJointBullet::getUpperLinLimit() const {
-	return sliderConstraint->getUpperLinLimit();
-}
-
-void SliderJointBullet::setUpperLinLimit(real_t upperLimit) {
-	sliderConstraint->setUpperLinLimit(upperLimit);
-}
-
-real_t SliderJointBullet::getLowerAngLimit() const {
-	return sliderConstraint->getLowerAngLimit();
-}
-
-void SliderJointBullet::setLowerAngLimit(real_t lowerLimit) {
-	sliderConstraint->setLowerAngLimit(lowerLimit);
-}
-
-real_t SliderJointBullet::getUpperAngLimit() const {
-	return sliderConstraint->getUpperAngLimit();
-}
-
-void SliderJointBullet::setUpperAngLimit(real_t upperLimit) {
-	sliderConstraint->setUpperAngLimit(upperLimit);
-}
-
-real_t SliderJointBullet::getSoftnessDirLin() const {
-	return sliderConstraint->getSoftnessDirLin();
-}
-
-real_t SliderJointBullet::getRestitutionDirLin() const {
-	return sliderConstraint->getRestitutionDirLin();
-}
-
-real_t SliderJointBullet::getDampingDirLin() const {
-	return sliderConstraint->getDampingDirLin();
-}
-
-real_t SliderJointBullet::getSoftnessDirAng() const {
-	return sliderConstraint->getSoftnessDirAng();
-}
-
-real_t SliderJointBullet::getRestitutionDirAng() const {
-	return sliderConstraint->getRestitutionDirAng();
-}
-
-real_t SliderJointBullet::getDampingDirAng() const {
-	return sliderConstraint->getDampingDirAng();
-}
-
-real_t SliderJointBullet::getSoftnessLimLin() const {
-	return sliderConstraint->getSoftnessLimLin();
-}
-
-real_t SliderJointBullet::getRestitutionLimLin() const {
-	return sliderConstraint->getRestitutionLimLin();
-}
-
-real_t SliderJointBullet::getDampingLimLin() const {
-	return sliderConstraint->getDampingLimLin();
-}
-
-real_t SliderJointBullet::getSoftnessLimAng() const {
-	return sliderConstraint->getSoftnessLimAng();
-}
-
-real_t SliderJointBullet::getRestitutionLimAng() const {
-	return sliderConstraint->getRestitutionLimAng();
-}
-
-real_t SliderJointBullet::getDampingLimAng() const {
-	return sliderConstraint->getDampingLimAng();
-}
-
-real_t SliderJointBullet::getSoftnessOrthoLin() const {
-	return sliderConstraint->getSoftnessOrthoLin();
-}
-
-real_t SliderJointBullet::getRestitutionOrthoLin() const {
-	return sliderConstraint->getRestitutionOrthoLin();
-}
-
-real_t SliderJointBullet::getDampingOrthoLin() const {
-	return sliderConstraint->getDampingOrthoLin();
-}
-
-real_t SliderJointBullet::getSoftnessOrthoAng() const {
-	return sliderConstraint->getSoftnessOrthoAng();
-}
-
-real_t SliderJointBullet::getRestitutionOrthoAng() const {
-	return sliderConstraint->getRestitutionOrthoAng();
-}
-
-real_t SliderJointBullet::getDampingOrthoAng() const {
-	return sliderConstraint->getDampingOrthoAng();
-}
-
-void SliderJointBullet::setSoftnessDirLin(real_t softnessDirLin) {
-	sliderConstraint->setSoftnessDirLin(softnessDirLin);
-}
-
-void SliderJointBullet::setRestitutionDirLin(real_t restitutionDirLin) {
-	sliderConstraint->setRestitutionDirLin(restitutionDirLin);
-}
-
-void SliderJointBullet::setDampingDirLin(real_t dampingDirLin) {
-	sliderConstraint->setDampingDirLin(dampingDirLin);
-}
-
-void SliderJointBullet::setSoftnessDirAng(real_t softnessDirAng) {
-	sliderConstraint->setSoftnessDirAng(softnessDirAng);
-}
-
-void SliderJointBullet::setRestitutionDirAng(real_t restitutionDirAng) {
-	sliderConstraint->setRestitutionDirAng(restitutionDirAng);
-}
-
-void SliderJointBullet::setDampingDirAng(real_t dampingDirAng) {
-	sliderConstraint->setDampingDirAng(dampingDirAng);
-}
-
-void SliderJointBullet::setSoftnessLimLin(real_t softnessLimLin) {
-	sliderConstraint->setSoftnessLimLin(softnessLimLin);
-}
-
-void SliderJointBullet::setRestitutionLimLin(real_t restitutionLimLin) {
-	sliderConstraint->setRestitutionLimLin(restitutionLimLin);
-}
-
-void SliderJointBullet::setDampingLimLin(real_t dampingLimLin) {
-	sliderConstraint->setDampingLimLin(dampingLimLin);
-}
-
-void SliderJointBullet::setSoftnessLimAng(real_t softnessLimAng) {
-	sliderConstraint->setSoftnessLimAng(softnessLimAng);
-}
-
-void SliderJointBullet::setRestitutionLimAng(real_t restitutionLimAng) {
-	sliderConstraint->setRestitutionLimAng(restitutionLimAng);
-}
-
-void SliderJointBullet::setDampingLimAng(real_t dampingLimAng) {
-	sliderConstraint->setDampingLimAng(dampingLimAng);
-}
-
-void SliderJointBullet::setSoftnessOrthoLin(real_t softnessOrthoLin) {
-	sliderConstraint->setSoftnessOrthoLin(softnessOrthoLin);
-}
-
-void SliderJointBullet::setRestitutionOrthoLin(real_t restitutionOrthoLin) {
-	sliderConstraint->setRestitutionOrthoLin(restitutionOrthoLin);
-}
-
-void SliderJointBullet::setDampingOrthoLin(real_t dampingOrthoLin) {
-	sliderConstraint->setDampingOrthoLin(dampingOrthoLin);
-}
-
-void SliderJointBullet::setSoftnessOrthoAng(real_t softnessOrthoAng) {
-	sliderConstraint->setSoftnessOrthoAng(softnessOrthoAng);
-}
-
-void SliderJointBullet::setRestitutionOrthoAng(real_t restitutionOrthoAng) {
-	sliderConstraint->setRestitutionOrthoAng(restitutionOrthoAng);
-}
-
-void SliderJointBullet::setDampingOrthoAng(real_t dampingOrthoAng) {
-	sliderConstraint->setDampingOrthoAng(dampingOrthoAng);
-}
-
-void SliderJointBullet::setPoweredLinMotor(bool onOff) {
-	sliderConstraint->setPoweredLinMotor(onOff);
-}
-
-bool SliderJointBullet::getPoweredLinMotor() {
-	return sliderConstraint->getPoweredLinMotor();
-}
-
-void SliderJointBullet::setTargetLinMotorVelocity(real_t targetLinMotorVelocity) {
-	sliderConstraint->setTargetLinMotorVelocity(targetLinMotorVelocity);
-}
-
-real_t SliderJointBullet::getTargetLinMotorVelocity() {
-	return sliderConstraint->getTargetLinMotorVelocity();
-}
-
-void SliderJointBullet::setMaxLinMotorForce(real_t maxLinMotorForce) {
-	sliderConstraint->setMaxLinMotorForce(maxLinMotorForce);
-}
-
-real_t SliderJointBullet::getMaxLinMotorForce() {
-	return sliderConstraint->getMaxLinMotorForce();
-}
-
-void SliderJointBullet::setPoweredAngMotor(bool onOff) {
-	sliderConstraint->setPoweredAngMotor(onOff);
-}
-
-bool SliderJointBullet::getPoweredAngMotor() {
-	return sliderConstraint->getPoweredAngMotor();
-}
-
-void SliderJointBullet::setTargetAngMotorVelocity(real_t targetAngMotorVelocity) {
-	sliderConstraint->setTargetAngMotorVelocity(targetAngMotorVelocity);
-}
-
-real_t SliderJointBullet::getTargetAngMotorVelocity() {
-	return sliderConstraint->getTargetAngMotorVelocity();
-}
-
-void SliderJointBullet::setMaxAngMotorForce(real_t maxAngMotorForce) {
-	sliderConstraint->setMaxAngMotorForce(maxAngMotorForce);
-}
-
-real_t SliderJointBullet::getMaxAngMotorForce() {
-	return sliderConstraint->getMaxAngMotorForce();
-}
-
-real_t SliderJointBullet::getLinearPos() {
-	return sliderConstraint->getLinearPos();
-}
-
-void SliderJointBullet::set_param(PhysicsServer3D::SliderJointParam p_param, real_t p_value) {
-	switch (p_param) {
-		case PhysicsServer3D::SLIDER_JOINT_LINEAR_LIMIT_UPPER:
-			setUpperLinLimit(p_value);
-			break;
-		case PhysicsServer3D::SLIDER_JOINT_LINEAR_LIMIT_LOWER:
-			setLowerLinLimit(p_value);
-			break;
-		case PhysicsServer3D::SLIDER_JOINT_LINEAR_LIMIT_SOFTNESS:
-			setSoftnessLimLin(p_value);
-			break;
-		case PhysicsServer3D::SLIDER_JOINT_LINEAR_LIMIT_RESTITUTION:
-			setRestitutionLimLin(p_value);
-			break;
-		case PhysicsServer3D::SLIDER_JOINT_LINEAR_LIMIT_DAMPING:
-			setDampingLimLin(p_value);
-			break;
-		case PhysicsServer3D::SLIDER_JOINT_LINEAR_MOTION_SOFTNESS:
-			setSoftnessDirLin(p_value);
-			break;
-		case PhysicsServer3D::SLIDER_JOINT_LINEAR_MOTION_RESTITUTION:
-			setRestitutionDirLin(p_value);
-			break;
-		case PhysicsServer3D::SLIDER_JOINT_LINEAR_MOTION_DAMPING:
-			setDampingDirLin(p_value);
-			break;
-		case PhysicsServer3D::SLIDER_JOINT_LINEAR_ORTHOGONAL_SOFTNESS:
-			setSoftnessOrthoLin(p_value);
-			break;
-		case PhysicsServer3D::SLIDER_JOINT_LINEAR_ORTHOGONAL_RESTITUTION:
-			setRestitutionOrthoLin(p_value);
-			break;
-		case PhysicsServer3D::SLIDER_JOINT_LINEAR_ORTHOGONAL_DAMPING:
-			setDampingOrthoLin(p_value);
-			break;
-		case PhysicsServer3D::SLIDER_JOINT_ANGULAR_LIMIT_UPPER:
-			setUpperAngLimit(p_value);
-			break;
-		case PhysicsServer3D::SLIDER_JOINT_ANGULAR_LIMIT_LOWER:
-			setLowerAngLimit(p_value);
-			break;
-		case PhysicsServer3D::SLIDER_JOINT_ANGULAR_LIMIT_SOFTNESS:
-			setSoftnessLimAng(p_value);
-			break;
-		case PhysicsServer3D::SLIDER_JOINT_ANGULAR_LIMIT_RESTITUTION:
-			setRestitutionLimAng(p_value);
-			break;
-		case PhysicsServer3D::SLIDER_JOINT_ANGULAR_LIMIT_DAMPING:
-			setDampingLimAng(p_value);
-			break;
-		case PhysicsServer3D::SLIDER_JOINT_ANGULAR_MOTION_SOFTNESS:
-			setSoftnessDirAng(p_value);
-			break;
-		case PhysicsServer3D::SLIDER_JOINT_ANGULAR_MOTION_RESTITUTION:
-			setRestitutionDirAng(p_value);
-			break;
-		case PhysicsServer3D::SLIDER_JOINT_ANGULAR_MOTION_DAMPING:
-			setDampingDirAng(p_value);
-			break;
-		case PhysicsServer3D::SLIDER_JOINT_ANGULAR_ORTHOGONAL_SOFTNESS:
-			setSoftnessOrthoAng(p_value);
-			break;
-		case PhysicsServer3D::SLIDER_JOINT_ANGULAR_ORTHOGONAL_RESTITUTION:
-			setRestitutionOrthoAng(p_value);
-			break;
-		case PhysicsServer3D::SLIDER_JOINT_ANGULAR_ORTHOGONAL_DAMPING:
-			setDampingOrthoAng(p_value);
-			break;
-		case PhysicsServer3D::SLIDER_JOINT_MAX:
-			break; // Can't happen, but silences warning
-	}
-}
-
-real_t SliderJointBullet::get_param(PhysicsServer3D::SliderJointParam p_param) const {
-	switch (p_param) {
-		case PhysicsServer3D::SLIDER_JOINT_LINEAR_LIMIT_UPPER:
-			return getUpperLinLimit();
-		case PhysicsServer3D::SLIDER_JOINT_LINEAR_LIMIT_LOWER:
-			return getLowerLinLimit();
-		case PhysicsServer3D::SLIDER_JOINT_LINEAR_LIMIT_SOFTNESS:
-			return getSoftnessLimLin();
-		case PhysicsServer3D::SLIDER_JOINT_LINEAR_LIMIT_RESTITUTION:
-			return getRestitutionLimLin();
-		case PhysicsServer3D::SLIDER_JOINT_LINEAR_LIMIT_DAMPING:
-			return getDampingLimLin();
-		case PhysicsServer3D::SLIDER_JOINT_LINEAR_MOTION_SOFTNESS:
-			return getSoftnessDirLin();
-		case PhysicsServer3D::SLIDER_JOINT_LINEAR_MOTION_RESTITUTION:
-			return getRestitutionDirLin();
-		case PhysicsServer3D::SLIDER_JOINT_LINEAR_MOTION_DAMPING:
-			return getDampingDirLin();
-		case PhysicsServer3D::SLIDER_JOINT_LINEAR_ORTHOGONAL_SOFTNESS:
-			return getSoftnessOrthoLin();
-		case PhysicsServer3D::SLIDER_JOINT_LINEAR_ORTHOGONAL_RESTITUTION:
-			return getRestitutionOrthoLin();
-		case PhysicsServer3D::SLIDER_JOINT_LINEAR_ORTHOGONAL_DAMPING:
-			return getDampingOrthoLin();
-		case PhysicsServer3D::SLIDER_JOINT_ANGULAR_LIMIT_UPPER:
-			return getUpperAngLimit();
-		case PhysicsServer3D::SLIDER_JOINT_ANGULAR_LIMIT_LOWER:
-			return getLowerAngLimit();
-		case PhysicsServer3D::SLIDER_JOINT_ANGULAR_LIMIT_SOFTNESS:
-			return getSoftnessLimAng();
-		case PhysicsServer3D::SLIDER_JOINT_ANGULAR_LIMIT_RESTITUTION:
-			return getRestitutionLimAng();
-		case PhysicsServer3D::SLIDER_JOINT_ANGULAR_LIMIT_DAMPING:
-			return getDampingLimAng();
-		case PhysicsServer3D::SLIDER_JOINT_ANGULAR_MOTION_SOFTNESS:
-			return getSoftnessDirAng();
-		case PhysicsServer3D::SLIDER_JOINT_ANGULAR_MOTION_RESTITUTION:
-			return getRestitutionDirAng();
-		case PhysicsServer3D::SLIDER_JOINT_ANGULAR_MOTION_DAMPING:
-			return getDampingDirAng();
-		case PhysicsServer3D::SLIDER_JOINT_ANGULAR_ORTHOGONAL_SOFTNESS:
-			return getSoftnessOrthoAng();
-		case PhysicsServer3D::SLIDER_JOINT_ANGULAR_ORTHOGONAL_RESTITUTION:
-			return getRestitutionOrthoAng();
-		case PhysicsServer3D::SLIDER_JOINT_ANGULAR_ORTHOGONAL_DAMPING:
-			return getDampingOrthoAng();
-		default:
-			return 0;
-	}
-}

modules/bullet/slider_joint_bullet.h

@@ -1,118 +0,0 @@
-/*************************************************************************/
-/*  slider_joint_bullet.h                                                */
-/*************************************************************************/
-/*                       This file is part of:                           */
-/*                           GODOT ENGINE                                */
-/*                      https://godotengine.org                          */
-/*************************************************************************/
-/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur.                 */
-/* Copyright (c) 2014-2022 Godot Engine contributors (cf. AUTHORS.md).   */
-/*                                                                       */
-/* Permission is hereby granted, free of charge, to any person obtaining */
-/* a copy of this software and associated documentation files (the       */
-/* "Software"), to deal in the Software without restriction, including   */
-/* without limitation the rights to use, copy, modify, merge, publish,   */
-/* distribute, sublicense, and/or sell copies of the Software, and to    */
-/* permit persons to whom the Software is furnished to do so, subject to */
-/* the following conditions:                                             */
-/*                                                                       */
-/* The above copyright notice and this permission notice shall be        */
-/* included in all copies or substantial portions of the Software.       */
-/*                                                                       */
-/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
-/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
-/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
-/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
-/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION 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 SLIDER_JOINT_BULLET_H
-#define SLIDER_JOINT_BULLET_H
-
-#include "joint_bullet.h"
-
-class RigidBodyBullet;
-
-class SliderJointBullet : public JointBullet {
-	class btSliderConstraint *sliderConstraint;
-
-public:
-	/// Reference frame is A
-	SliderJointBullet(RigidBodyBullet *rbA, RigidBodyBullet *rbB, const Transform3D &frameInA, const Transform3D &frameInB);
-
-	virtual PhysicsServer3D::JointType get_type() const { return PhysicsServer3D::JOINT_SLIDER; }
-
-	const RigidBodyBullet *getRigidBodyA() const;
-	const RigidBodyBullet *getRigidBodyB() const;
-	const Transform3D getCalculatedTransformA() const;
-	const Transform3D getCalculatedTransformB() const;
-	const Transform3D getFrameOffsetA() const;
-	const Transform3D getFrameOffsetB() const;
-	Transform3D getFrameOffsetA();
-	Transform3D getFrameOffsetB();
-	real_t getLowerLinLimit() const;
-	void setLowerLinLimit(real_t lowerLimit);
-	real_t getUpperLinLimit() const;
-	void setUpperLinLimit(real_t upperLimit);
-	real_t getLowerAngLimit() const;
-	void setLowerAngLimit(real_t lowerLimit);
-	real_t getUpperAngLimit() const;
-	void setUpperAngLimit(real_t upperLimit);
-
-	real_t getSoftnessDirLin() const;
-	real_t getRestitutionDirLin() const;
-	real_t getDampingDirLin() const;
-	real_t getSoftnessDirAng() const;
-	real_t getRestitutionDirAng() const;
-	real_t getDampingDirAng() const;
-	real_t getSoftnessLimLin() const;
-	real_t getRestitutionLimLin() const;
-	real_t getDampingLimLin() const;
-	real_t getSoftnessLimAng() const;
-	real_t getRestitutionLimAng() const;
-	real_t getDampingLimAng() const;
-	real_t getSoftnessOrthoLin() const;
-	real_t getRestitutionOrthoLin() const;
-	real_t getDampingOrthoLin() const;
-	real_t getSoftnessOrthoAng() const;
-	real_t getRestitutionOrthoAng() const;
-	real_t getDampingOrthoAng() const;
-	void setSoftnessDirLin(real_t softnessDirLin);
-	void setRestitutionDirLin(real_t restitutionDirLin);
-	void setDampingDirLin(real_t dampingDirLin);
-	void setSoftnessDirAng(real_t softnessDirAng);
-	void setRestitutionDirAng(real_t restitutionDirAng);
-	void setDampingDirAng(real_t dampingDirAng);
-	void setSoftnessLimLin(real_t softnessLimLin);
-	void setRestitutionLimLin(real_t restitutionLimLin);
-	void setDampingLimLin(real_t dampingLimLin);
-	void setSoftnessLimAng(real_t softnessLimAng);
-	void setRestitutionLimAng(real_t restitutionLimAng);
-	void setDampingLimAng(real_t dampingLimAng);
-	void setSoftnessOrthoLin(real_t softnessOrthoLin);
-	void setRestitutionOrthoLin(real_t restitutionOrthoLin);
-	void setDampingOrthoLin(real_t dampingOrthoLin);
-	void setSoftnessOrthoAng(real_t softnessOrthoAng);
-	void setRestitutionOrthoAng(real_t restitutionOrthoAng);
-	void setDampingOrthoAng(real_t dampingOrthoAng);
-	void setPoweredLinMotor(bool onOff);
-	bool getPoweredLinMotor();
-	void setTargetLinMotorVelocity(real_t targetLinMotorVelocity);
-	real_t getTargetLinMotorVelocity();
-	void setMaxLinMotorForce(real_t maxLinMotorForce);
-	real_t getMaxLinMotorForce();
-	void setPoweredAngMotor(bool onOff);
-	bool getPoweredAngMotor();
-	void setTargetAngMotorVelocity(real_t targetAngMotorVelocity);
-	real_t getTargetAngMotorVelocity();
-	void setMaxAngMotorForce(real_t maxAngMotorForce);
-	real_t getMaxAngMotorForce();
-	real_t getLinearPos();
-
-	void set_param(PhysicsServer3D::SliderJointParam p_param, real_t p_value);
-	real_t get_param(PhysicsServer3D::SliderJointParam p_param) const;
-};
-
-#endif // SLIDER_JOINT_BULLET_H

modules/bullet/soft_body_bullet.cpp

@@ -1,456 +0,0 @@
-/*************************************************************************/
-/*  soft_body_bullet.cpp                                                 */
-/*************************************************************************/
-/*                       This file is part of:                           */
-/*                           GODOT ENGINE                                */
-/*                      https://godotengine.org                          */
-/*************************************************************************/
-/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur.                 */
-/* Copyright (c) 2014-2022 Godot Engine contributors (cf. AUTHORS.md).   */
-/*                                                                       */
-/* Permission is hereby granted, free of charge, to any person obtaining */
-/* a copy of this software and associated documentation files (the       */
-/* "Software"), to deal in the Software without restriction, including   */
-/* without limitation the rights to use, copy, modify, merge, publish,   */
-/* distribute, sublicense, and/or sell copies of the Software, and to    */
-/* permit persons to whom the Software is furnished to do so, subject to */
-/* the following conditions:                                             */
-/*                                                                       */
-/* The above copyright notice and this permission notice shall be        */
-/* included in all copies or substantial portions of the Software.       */
-/*                                                                       */
-/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
-/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
-/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
-/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
-/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION 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 "soft_body_bullet.h"
-
-#include "bullet_types_converter.h"
-#include "bullet_utilities.h"
-#include "space_bullet.h"
-
-#include "servers/rendering_server.h"
-
-SoftBodyBullet::SoftBodyBullet() :
-		CollisionObjectBullet(CollisionObjectBullet::TYPE_SOFT_BODY) {}
-
-SoftBodyBullet::~SoftBodyBullet() {
-}
-
-void SoftBodyBullet::reload_body() {
-	if (space) {
-		space->remove_soft_body(this);
-		space->add_soft_body(this);
-	}
-}
-
-void SoftBodyBullet::set_space(SpaceBullet *p_space) {
-	if (space) {
-		isScratched = false;
-		space->remove_soft_body(this);
-	}
-
-	space = p_space;
-
-	if (space) {
-		space->add_soft_body(this);
-	}
-}
-
-void SoftBodyBullet::on_enter_area(AreaBullet *p_area) {}
-
-void SoftBodyBullet::on_exit_area(AreaBullet *p_area) {}
-
-void SoftBodyBullet::update_rendering_server(RenderingServerHandler *p_rendering_server_handler) {
-	if (!bt_soft_body) {
-		return;
-	}
-
-	/// Update rendering server vertices
-	const btSoftBody::tNodeArray &nodes(bt_soft_body->m_nodes);
-	const int nodes_count = nodes.size();
-
-	const Vector<int> *vs_indices;
-	const void *vertex_position;
-	const void *vertex_normal;
-
-	for (int vertex_index = 0; vertex_index < nodes_count; ++vertex_index) {
-		vertex_position = reinterpret_cast<const void *>(&nodes[vertex_index].m_x);
-		vertex_normal = reinterpret_cast<const void *>(&nodes[vertex_index].m_n);
-
-		vs_indices = &indices_table[vertex_index];
-
-		const int vs_indices_size(vs_indices->size());
-		for (int x = 0; x < vs_indices_size; ++x) {
-			p_rendering_server_handler->set_vertex((*vs_indices)[x], vertex_position);
-			p_rendering_server_handler->set_normal((*vs_indices)[x], vertex_normal);
-		}
-	}
-
-	/// Generate AABB
-	btVector3 aabb_min;
-	btVector3 aabb_max;
-	bt_soft_body->getAabb(aabb_min, aabb_max);
-
-	btVector3 size(aabb_max - aabb_min);
-
-	AABB aabb;
-	B_TO_G(aabb_min, aabb.position);
-	B_TO_G(size, aabb.size);
-
-	p_rendering_server_handler->set_aabb(aabb);
-}
-
-void SoftBodyBullet::set_soft_mesh(RID p_mesh) {
-	destroy_soft_body();
-
-	soft_mesh = p_mesh;
-
-	if (soft_mesh.is_null()) {
-		return;
-	}
-
-	Array arrays = RenderingServer::get_singleton()->mesh_surface_get_arrays(soft_mesh, 0);
-	ERR_FAIL_COND(arrays.is_empty());
-
-	bool success = set_trimesh_body_shape(arrays[RS::ARRAY_INDEX], arrays[RS::ARRAY_VERTEX]);
-	if (!success) {
-		destroy_soft_body();
-	}
-}
-
-void SoftBodyBullet::destroy_soft_body() {
-	soft_mesh = RID();
-
-	if (!bt_soft_body) {
-		return;
-	}
-
-	if (space) {
-		/// Remove from world before deletion
-		space->remove_soft_body(this);
-	}
-
-	destroyBulletCollisionObject();
-	bt_soft_body = nullptr;
-}
-
-void SoftBodyBullet::set_soft_transform(const Transform3D &p_transform) {
-	reset_all_node_positions();
-	move_all_nodes(p_transform);
-}
-
-AABB SoftBodyBullet::get_bounds() const {
-	if (!bt_soft_body) {
-		return AABB();
-	}
-
-	btVector3 aabb_min;
-	btVector3 aabb_max;
-	bt_soft_body->getAabb(aabb_min, aabb_max);
-
-	btVector3 size(aabb_max - aabb_min);
-
-	AABB aabb;
-	B_TO_G(aabb_min, aabb.position);
-	B_TO_G(size, aabb.size);
-
-	return aabb;
-}
-
-void SoftBodyBullet::move_all_nodes(const Transform3D &p_transform) {
-	if (!bt_soft_body) {
-		return;
-	}
-	btTransform bt_transf;
-	G_TO_B(p_transform, bt_transf);
-	bt_soft_body->transform(bt_transf);
-}
-
-void SoftBodyBullet::set_node_position(int p_node_index, const Vector3 &p_global_position) {
-	btVector3 bt_pos;
-	G_TO_B(p_global_position, bt_pos);
-	set_node_position(p_node_index, bt_pos);
-}
-
-void SoftBodyBullet::set_node_position(int p_node_index, const btVector3 &p_global_position) {
-	if (bt_soft_body) {
-		bt_soft_body->m_nodes[p_node_index].m_q = bt_soft_body->m_nodes[p_node_index].m_x;
-		bt_soft_body->m_nodes[p_node_index].m_x = p_global_position;
-	}
-}
-
-void SoftBodyBullet::get_node_position(int p_node_index, Vector3 &r_position) const {
-	if (bt_soft_body) {
-		B_TO_G(bt_soft_body->m_nodes[p_node_index].m_x, r_position);
-	}
-}
-
-void SoftBodyBullet::set_node_mass(int p_node_index, btScalar p_mass) {
-	if (0 >= p_mass) {
-		pin_node(p_node_index);
-	} else {
-		unpin_node(p_node_index);
-	}
-	if (bt_soft_body) {
-		ERR_FAIL_INDEX(p_node_index, bt_soft_body->m_nodes.size());
-		bt_soft_body->setMass(p_node_index, p_mass);
-	}
-}
-
-btScalar SoftBodyBullet::get_node_mass(int p_node_index) const {
-	if (bt_soft_body) {
-		ERR_FAIL_INDEX_V(p_node_index, bt_soft_body->m_nodes.size(), 1);
-		return bt_soft_body->getMass(p_node_index);
-	} else {
-		return -1 == search_node_pinned(p_node_index) ? 1 : 0;
-	}
-}
-
-void SoftBodyBullet::reset_all_node_mass() {
-	if (bt_soft_body) {
-		for (int i = pinned_nodes.size() - 1; 0 <= i; --i) {
-			bt_soft_body->setMass(pinned_nodes[i], 1);
-		}
-	}
-	pinned_nodes.resize(0);
-}
-
-void SoftBodyBullet::reset_all_node_positions() {
-	if (soft_mesh.is_null()) {
-		return;
-	}
-
-	Array arrays = soft_mesh->surface_get_arrays(0);
-	Vector<Vector3> vs_vertices(arrays[RS::ARRAY_VERTEX]);
-	const Vector3 *vs_vertices_read = vs_vertices.ptr();
-
-	for (int vertex_index = bt_soft_body->m_nodes.size() - 1; 0 <= vertex_index; --vertex_index) {
-		G_TO_B(vs_vertices_read[indices_table[vertex_index][0]], bt_soft_body->m_nodes[vertex_index].m_x);
-
-		bt_soft_body->m_nodes[vertex_index].m_q = bt_soft_body->m_nodes[vertex_index].m_x;
-		bt_soft_body->m_nodes[vertex_index].m_v = btVector3(0, 0, 0);
-		bt_soft_body->m_nodes[vertex_index].m_f = btVector3(0, 0, 0);
-	}
-}
-
-void SoftBodyBullet::set_activation_state(bool p_active) {
-	if (p_active) {
-		bt_soft_body->setActivationState(ACTIVE_TAG);
-	} else {
-		bt_soft_body->setActivationState(WANTS_DEACTIVATION);
-	}
-}
-
-void SoftBodyBullet::set_total_mass(real_t p_val) {
-	if (0 >= p_val) {
-		p_val = 1;
-	}
-	total_mass = p_val;
-	if (bt_soft_body) {
-		bt_soft_body->setTotalMass(total_mass);
-	}
-}
-
-void SoftBodyBullet::set_linear_stiffness(real_t p_val) {
-	linear_stiffness = p_val;
-	if (bt_soft_body) {
-		mat0->m_kLST = linear_stiffness;
-	}
-}
-
-void SoftBodyBullet::set_simulation_precision(int p_val) {
-	simulation_precision = p_val;
-	if (bt_soft_body) {
-		bt_soft_body->m_cfg.piterations = simulation_precision;
-		bt_soft_body->m_cfg.viterations = simulation_precision;
-		bt_soft_body->m_cfg.diterations = simulation_precision;
-		bt_soft_body->m_cfg.citerations = simulation_precision;
-	}
-}
-
-void SoftBodyBullet::set_pressure_coefficient(real_t p_val) {
-	pressure_coefficient = p_val;
-	if (bt_soft_body) {
-		bt_soft_body->m_cfg.kPR = pressure_coefficient;
-	}
-}
-
-void SoftBodyBullet::set_damping_coefficient(real_t p_val) {
-	damping_coefficient = p_val;
-	if (bt_soft_body) {
-		bt_soft_body->m_cfg.kDP = damping_coefficient;
-	}
-}
-
-void SoftBodyBullet::set_drag_coefficient(real_t p_val) {
-	drag_coefficient = p_val;
-	if (bt_soft_body) {
-		bt_soft_body->m_cfg.kDG = drag_coefficient;
-	}
-}
-
-bool SoftBodyBullet::set_trimesh_body_shape(Vector<int> p_indices, Vector<Vector3> p_vertices) {
-	ERR_FAIL_COND_V(p_indices.is_empty(), false);
-	ERR_FAIL_COND_V(p_vertices.is_empty(), false);
-
-	/// Parse rendering server indices to physical indices.
-	/// Merge all overlapping vertices and create a map of physical vertices to rendering server
-
-	{
-		/// This is the map of rendering server indices to physics indices (So it's the inverse of idices_map), Thanks to it I don't need make a heavy search in the indices_map
-		Vector<int> vs_indices_to_physics_table;
-
-		{ // Map vertices
-			indices_table.resize(0);
-
-			int index = 0;
-			Map<Vector3, int> unique_vertices;
-
-			const int vs_vertices_size(p_vertices.size());
-
-			const Vector3 *p_vertices_read = p_vertices.ptr();
-
-			for (int vs_vertex_index = 0; vs_vertex_index < vs_vertices_size; ++vs_vertex_index) {
-				Map<Vector3, int>::Element *e = unique_vertices.find(p_vertices_read[vs_vertex_index]);
-				int vertex_id;
-				if (e) {
-					// Already existing
-					vertex_id = e->value();
-				} else {
-					// Create new one
-					unique_vertices[p_vertices_read[vs_vertex_index]] = vertex_id = index++;
-					indices_table.push_back(Vector<int>());
-				}
-
-				indices_table.write[vertex_id].push_back(vs_vertex_index);
-				vs_indices_to_physics_table.push_back(vertex_id);
-			}
-		}
-
-		const int indices_map_size(indices_table.size());
-
-		Vector<btScalar> bt_vertices;
-
-		{ // Parse vertices to bullet
-
-			bt_vertices.resize(indices_map_size * 3);
-			const Vector3 *p_vertices_read = p_vertices.ptr();
-
-			for (int i = 0; i < indices_map_size; ++i) {
-				bt_vertices.write[3 * i + 0] = p_vertices_read[indices_table[i][0]].x;
-				bt_vertices.write[3 * i + 1] = p_vertices_read[indices_table[i][0]].y;
-				bt_vertices.write[3 * i + 2] = p_vertices_read[indices_table[i][0]].z;
-			}
-		}
-
-		Vector<int> bt_triangles;
-		const int triangles_size(p_indices.size() / 3);
-
-		{ // Parse indices
-
-			bt_triangles.resize(triangles_size * 3);
-
-			const int *p_indices_read = p_indices.ptr();
-
-			for (int i = 0; i < triangles_size; ++i) {
-				bt_triangles.write[3 * i + 0] = vs_indices_to_physics_table[p_indices_read[3 * i + 2]];
-				bt_triangles.write[3 * i + 1] = vs_indices_to_physics_table[p_indices_read[3 * i + 1]];
-				bt_triangles.write[3 * i + 2] = vs_indices_to_physics_table[p_indices_read[3 * i + 0]];
-			}
-		}
-
-		btSoftBodyWorldInfo fake_world_info;
-		bt_soft_body = btSoftBodyHelpers::CreateFromTriMesh(fake_world_info, &bt_vertices[0], &bt_triangles[0], triangles_size, false);
-		setup_soft_body();
-	}
-
-	return true;
-}
-
-void SoftBodyBullet::setup_soft_body() {
-	if (!bt_soft_body) {
-		return;
-	}
-
-	// Soft body setup
-	setupBulletCollisionObject(bt_soft_body);
-	bt_soft_body->m_worldInfo = nullptr; // Remove fake world info
-	bt_soft_body->getCollisionShape()->setMargin(0.01);
-	bt_soft_body->setCollisionFlags(bt_soft_body->getCollisionFlags() & (~(btCollisionObject::CF_KINEMATIC_OBJECT | btCollisionObject::CF_STATIC_OBJECT)));
-
-	// Space setup
-	if (space) {
-		space->add_soft_body(this);
-	}
-
-	mat0 = bt_soft_body->appendMaterial();
-
-	// Assign soft body data
-	bt_soft_body->generateBendingConstraints(2, mat0);
-
-	mat0->m_kLST = linear_stiffness;
-
-	// Clusters allow to have Soft vs Soft collision but doesn't work well right now
-
-	//bt_soft_body->m_cfg.kSRHR_CL = 1;// Soft vs rigid hardness [0,1] (cluster only)
-	//bt_soft_body->m_cfg.kSKHR_CL = 1;// Soft vs kinematic hardness [0,1] (cluster only)
-	//bt_soft_body->m_cfg.kSSHR_CL = 1;// Soft vs soft hardness [0,1] (cluster only)
-	//bt_soft_body->m_cfg.kSR_SPLT_CL = 1; // Soft vs rigid impulse split [0,1] (cluster only)
-	//bt_soft_body->m_cfg.kSK_SPLT_CL = 1; // Soft vs kinematic impulse split [0,1] (cluster only)
-	//bt_soft_body->m_cfg.kSS_SPLT_CL = 1; // Soft vs Soft impulse split [0,1] (cluster only)
-	//bt_soft_body->m_cfg.collisions = btSoftBody::fCollision::CL_SS + btSoftBody::fCollision::CL_RS + btSoftBody::fCollision::VF_SS;
-	//bt_soft_body->generateClusters(64);
-
-	bt_soft_body->m_cfg.piterations = simulation_precision;
-	bt_soft_body->m_cfg.viterations = simulation_precision;
-	bt_soft_body->m_cfg.diterations = simulation_precision;
-	bt_soft_body->m_cfg.citerations = simulation_precision;
-	bt_soft_body->m_cfg.kDP = damping_coefficient;
-	bt_soft_body->m_cfg.kDG = drag_coefficient;
-	bt_soft_body->m_cfg.kPR = pressure_coefficient;
-	bt_soft_body->setTotalMass(total_mass);
-
-	btSoftBodyHelpers::ReoptimizeLinkOrder(bt_soft_body);
-	bt_soft_body->updateBounds();
-
-	// Set pinned nodes
-	for (int i = pinned_nodes.size() - 1; 0 <= i; --i) {
-		const int node_index = pinned_nodes[i];
-		ERR_CONTINUE(0 > node_index || bt_soft_body->m_nodes.size() <= node_index);
-		bt_soft_body->setMass(node_index, 0);
-	}
-}
-
-void SoftBodyBullet::pin_node(int p_node_index) {
-	if (bt_soft_body) {
-		ERR_FAIL_INDEX(p_node_index, bt_soft_body->m_nodes.size());
-	}
-	if (-1 == search_node_pinned(p_node_index)) {
-		pinned_nodes.push_back(p_node_index);
-	}
-}
-
-void SoftBodyBullet::unpin_node(int p_node_index) {
-	if (bt_soft_body) {
-		ERR_FAIL_INDEX(p_node_index, bt_soft_body->m_nodes.size());
-	}
-	const int id = search_node_pinned(p_node_index);
-	if (-1 != id) {
-		pinned_nodes.remove_at(id);
-	}
-}
-
-int SoftBodyBullet::search_node_pinned(int p_node_index) const {
-	for (int i = pinned_nodes.size() - 1; 0 <= i; --i) {
-		if (p_node_index == pinned_nodes[i]) {
-			return i;
-		}
-	}
-	return -1;
-}

modules/bullet/soft_body_bullet.h

@@ -1,144 +0,0 @@
-/*************************************************************************/
-/*  soft_body_bullet.h                                                   */
-/*************************************************************************/
-/*                       This file is part of:                           */
-/*                           GODOT ENGINE                                */
-/*                      https://godotengine.org                          */
-/*************************************************************************/
-/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur.                 */
-/* Copyright (c) 2014-2022 Godot Engine contributors (cf. AUTHORS.md).   */
-/*                                                                       */
-/* Permission is hereby granted, free of charge, to any person obtaining */
-/* a copy of this software and associated documentation files (the       */
-/* "Software"), to deal in the Software without restriction, including   */
-/* without limitation the rights to use, copy, modify, merge, publish,   */
-/* distribute, sublicense, and/or sell copies of the Software, and to    */
-/* permit persons to whom the Software is furnished to do so, subject to */
-/* the following conditions:                                             */
-/*                                                                       */
-/* The above copyright notice and this permission notice shall be        */
-/* included in all copies or substantial portions of the Software.       */
-/*                                                                       */
-/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
-/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
-/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
-/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
-/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION 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 SOFT_BODY_BULLET_H
-#define SOFT_BODY_BULLET_H
-
-#include "collision_object_bullet.h"
-
-#ifdef None
-/// This is required to remove the macro None defined by x11 compiler because this word "None" is used internally by Bullet
-#undef None
-#define x11_None 0L
-#endif
-
-#include "BulletSoftBody/btSoftBodyHelpers.h"
-#include "collision_object_bullet.h"
-#include "servers/physics_server_3d.h"
-
-#ifdef x11_None
-/// This is required to re add the macro None defined by x11 compiler
-#undef x11_None
-#define None 0L
-#endif
-
-class RenderingServerHandler;
-
-class SoftBodyBullet : public CollisionObjectBullet {
-private:
-	btSoftBody *bt_soft_body = nullptr;
-	Vector<Vector<int>> indices_table;
-	btSoftBody::Material *mat0 = nullptr; // This is just a copy of pointer managed by btSoftBody
-	bool isScratched = false;
-
-	RID soft_mesh;
-
-	int simulation_precision = 5;
-	real_t total_mass = 1.;
-	real_t linear_stiffness = 0.5; // [0,1]
-	real_t pressure_coefficient = 0.; // [-inf,+inf]
-	real_t damping_coefficient = 0.01; // [0,1]
-	real_t drag_coefficient = 0.; // [0,1]
-	Vector<int> pinned_nodes;
-
-	// Other property to add
-	//btScalar				kVC;			// Volume conversation coefficient [0,+inf]
-	//btScalar				kDF;			// Dynamic friction coefficient [0,1]
-	//btScalar				kMT;			// Pose matching coefficient [0,1]
-	//btScalar				kCHR;			// Rigid contacts hardness [0,1]
-	//btScalar				kKHR;			// Kinetic contacts hardness [0,1]
-	//btScalar				kSHR;			// Soft contacts hardness [0,1]
-
-public:
-	SoftBodyBullet();
-	~SoftBodyBullet();
-
-	virtual void reload_body();
-	virtual void set_space(SpaceBullet *p_space);
-
-	virtual void dispatch_callbacks() {}
-	virtual void on_collision_filters_change() {}
-	virtual void on_collision_checker_start() {}
-	virtual void on_collision_checker_end() {}
-	virtual void on_enter_area(AreaBullet *p_area);
-	virtual void on_exit_area(AreaBullet *p_area);
-
-	_FORCE_INLINE_ btSoftBody *get_bt_soft_body() const { return bt_soft_body; }
-
-	void update_rendering_server(RenderingServerHandler *p_rendering_server_handler);
-
-	void set_soft_mesh(RID p_mesh);
-	void destroy_soft_body();
-
-	// Special function. This function has bad performance
-	void set_soft_transform(const Transform3D &p_transform);
-
-	AABB get_bounds() const;
-
-	void move_all_nodes(const Transform3D &p_transform);
-	void set_node_position(int node_index, const Vector3 &p_global_position);
-	void set_node_position(int node_index, const btVector3 &p_global_position);
-	void get_node_position(int node_index, Vector3 &r_position) const;
-
-	void set_node_mass(int node_index, btScalar p_mass);
-	btScalar get_node_mass(int node_index) const;
-	void reset_all_node_mass();
-	void reset_all_node_positions();
-
-	void set_activation_state(bool p_active);
-
-	void set_total_mass(real_t p_val);
-	_FORCE_INLINE_ real_t get_total_mass() const { return total_mass; }
-
-	void set_linear_stiffness(real_t p_val);
-	_FORCE_INLINE_ real_t get_linear_stiffness() const { return linear_stiffness; }
-
-	void set_simulation_precision(int p_val);
-	_FORCE_INLINE_ int get_simulation_precision() const { return simulation_precision; }
-
-	void set_pressure_coefficient(real_t p_val);
-	_FORCE_INLINE_ real_t get_pressure_coefficient() const { return pressure_coefficient; }
-
-	void set_damping_coefficient(real_t p_val);
-	_FORCE_INLINE_ real_t get_damping_coefficient() const { return damping_coefficient; }
-
-	void set_drag_coefficient(real_t p_val);
-	_FORCE_INLINE_ real_t get_drag_coefficient() const { return drag_coefficient; }
-
-private:
-	bool set_trimesh_body_shape(Vector<int> p_indices, Vector<Vector3> p_vertices);
-	void setup_soft_body();
-
-	void pin_node(int p_node_index);
-	void unpin_node(int p_node_index);
-	int search_node_pinned(int p_node_index) const;
-};
-
-#endif // SOFT_BODY_BULLET_H

modules/bullet/space_bullet.cpp

@@ -1,1436 +0,0 @@
-/*************************************************************************/
-/*  space_bullet.cpp                                                     */
-/*************************************************************************/
-/*                       This file is part of:                           */
-/*                           GODOT ENGINE                                */
-/*                      https://godotengine.org                          */
-/*************************************************************************/
-/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur.                 */
-/* Copyright (c) 2014-2022 Godot Engine contributors (cf. AUTHORS.md).   */
-/*                                                                       */
-/* Permission is hereby granted, free of charge, to any person obtaining */
-/* a copy of this software and associated documentation files (the       */
-/* "Software"), to deal in the Software without restriction, including   */
-/* without limitation the rights to use, copy, modify, merge, publish,   */
-/* distribute, sublicense, and/or sell copies of the Software, and to    */
-/* permit persons to whom the Software is furnished to do so, subject to */
-/* the following conditions:                                             */
-/*                                                                       */
-/* The above copyright notice and this permission notice shall be        */
-/* included in all copies or substantial portions of the Software.       */
-/*                                                                       */
-/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
-/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
-/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
-/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
-/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION 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 "space_bullet.h"
-
-#include "bullet_physics_server.h"
-#include "bullet_types_converter.h"
-#include "bullet_utilities.h"
-#include "constraint_bullet.h"
-#include "core/config/project_settings.h"
-#include "core/string/ustring.h"
-#include "godot_collision_configuration.h"
-#include "godot_collision_dispatcher.h"
-#include "rigid_body_bullet.h"
-#include "servers/physics_server_3d.h"
-#include "soft_body_bullet.h"
-
-#include <BulletCollision/BroadphaseCollision/btBroadphaseProxy.h>
-#include <BulletCollision/CollisionDispatch/btCollisionObject.h>
-#include <BulletCollision/CollisionDispatch/btGhostObject.h>
-#include <BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h>
-#include <BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h>
-#include <BulletCollision/NarrowPhaseCollision/btPointCollector.h>
-#include <BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h>
-#include <BulletSoftBody/btSoftRigidDynamicsWorld.h>
-#include <btBulletDynamicsCommon.h>
-
-#include <assert.h>
-
-BulletPhysicsDirectSpaceState::BulletPhysicsDirectSpaceState(SpaceBullet *p_space) :
-		PhysicsDirectSpaceState3D(),
-		space(p_space) {}
-
-int BulletPhysicsDirectSpaceState::intersect_point(const Vector3 &p_point, ShapeResult *r_results, int p_result_max, const Set<RID> &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas) {
-	if (p_result_max <= 0) {
-		return 0;
-	}
-
-	btVector3 bt_point;
-	G_TO_B(p_point, bt_point);
-
-	btSphereShape sphere_point(0.001f);
-	btCollisionObject collision_object_point;
-	collision_object_point.setCollisionShape(&sphere_point);
-	collision_object_point.setWorldTransform(btTransform(btQuaternion::getIdentity(), bt_point));
-
-	// Setup query
-	GodotAllContactResultCallback btResult(&collision_object_point, r_results, p_result_max, &p_exclude, p_collide_with_bodies, p_collide_with_areas);
-	btResult.m_collisionFilterGroup = 0;
-	btResult.m_collisionFilterMask = p_collision_mask;
-	space->dynamicsWorld->contactTest(&collision_object_point, btResult);
-
-	// The results are already populated by GodotAllConvexResultCallback
-	return btResult.m_count;
-}
-
-bool BulletPhysicsDirectSpaceState::intersect_ray(const Vector3 &p_from, const Vector3 &p_to, RayResult &r_result, const Set<RID> &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas, bool p_pick_ray) {
-	btVector3 btVec_from;
-	btVector3 btVec_to;
-
-	G_TO_B(p_from, btVec_from);
-	G_TO_B(p_to, btVec_to);
-
-	// setup query
-	GodotClosestRayResultCallback btResult(btVec_from, btVec_to, &p_exclude, p_collide_with_bodies, p_collide_with_areas);
-	btResult.m_collisionFilterGroup = 0;
-	btResult.m_collisionFilterMask = p_collision_mask;
-	btResult.m_pickRay = p_pick_ray;
-
-	space->dynamicsWorld->rayTest(btVec_from, btVec_to, btResult);
-	if (btResult.hasHit()) {
-		B_TO_G(btResult.m_hitPointWorld, r_result.position);
-		B_TO_G(btResult.m_hitNormalWorld.normalize(), r_result.normal);
-		CollisionObjectBullet *gObj = static_cast<CollisionObjectBullet *>(btResult.m_collisionObject->getUserPointer());
-		if (gObj) {
-			r_result.shape = btResult.m_shapeId;
-			r_result.rid = gObj->get_self();
-			r_result.collider_id = gObj->get_instance_id();
-			r_result.collider = r_result.collider_id.is_null() ? nullptr : ObjectDB::get_instance(r_result.collider_id);
-		} else {
-			WARN_PRINT("The raycast performed has hit a collision object that is not part of Godot scene, please check it.");
-		}
-		return true;
-	} else {
-		return false;
-	}
-}
-
-int BulletPhysicsDirectSpaceState::intersect_shape(const RID &p_shape, const Transform3D &p_xform, real_t p_margin, ShapeResult *r_results, int p_result_max, const Set<RID> &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas) {
-	if (p_result_max <= 0) {
-		return 0;
-	}
-
-	ShapeBullet *shape = space->get_physics_server()->get_shape_owner()->get_or_null(p_shape);
-	ERR_FAIL_COND_V(!shape, 0);
-
-	btCollisionShape *btShape = shape->create_bt_shape(p_xform.basis.get_scale_abs(), p_margin);
-	if (!btShape->isConvex()) {
-		bulletdelete(btShape);
-		ERR_PRINT("The shape is not a convex shape, then is not supported: shape type: " + itos(shape->get_type()));
-		return 0;
-	}
-	btConvexShape *btConvex = static_cast<btConvexShape *>(btShape);
-
-	btTransform bt_xform;
-	G_TO_B(p_xform, bt_xform);
-	UNSCALE_BT_BASIS(bt_xform);
-
-	btCollisionObject collision_object;
-	collision_object.setCollisionShape(btConvex);
-	collision_object.setWorldTransform(bt_xform);
-
-	GodotAllContactResultCallback btQuery(&collision_object, r_results, p_result_max, &p_exclude, p_collide_with_bodies, p_collide_with_areas);
-	btQuery.m_collisionFilterGroup = 0;
-	btQuery.m_collisionFilterMask = p_collision_mask;
-	btQuery.m_closestDistanceThreshold = 0;
-	space->dynamicsWorld->contactTest(&collision_object, btQuery);
-
-	bulletdelete(btConvex);
-
-	return btQuery.m_count;
-}
-
-bool BulletPhysicsDirectSpaceState::cast_motion(const RID &p_shape, const Transform3D &p_xform, const Vector3 &p_motion, real_t p_margin, real_t &r_closest_safe, real_t &r_closest_unsafe, const Set<RID> &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas, ShapeRestInfo *r_info) {
-	r_closest_safe = 0.0f;
-	r_closest_unsafe = 0.0f;
-	btVector3 bt_motion;
-	G_TO_B(p_motion, bt_motion);
-
-	ShapeBullet *shape = space->get_physics_server()->get_shape_owner()->get_or_null(p_shape);
-	ERR_FAIL_COND_V(!shape, false);
-
-	btCollisionShape *btShape = shape->create_bt_shape(p_xform.basis.get_scale(), p_margin);
-	if (!btShape->isConvex()) {
-		bulletdelete(btShape);
-		ERR_PRINT("The shape is not a convex shape, then is not supported: shape type: " + itos(shape->get_type()));
-		return false;
-	}
-	btConvexShape *bt_convex_shape = static_cast<btConvexShape *>(btShape);
-
-	btTransform bt_xform_from;
-	G_TO_B(p_xform, bt_xform_from);
-	UNSCALE_BT_BASIS(bt_xform_from);
-
-	btTransform bt_xform_to(bt_xform_from);
-	bt_xform_to.getOrigin() += bt_motion;
-
-	if ((bt_xform_to.getOrigin() - bt_xform_from.getOrigin()).fuzzyZero()) {
-		r_closest_safe = 1.0f;
-		r_closest_unsafe = 1.0f;
-		bulletdelete(btShape);
-		return true;
-	}
-
-	GodotClosestConvexResultCallback btResult(bt_xform_from.getOrigin(), bt_xform_to.getOrigin(), &p_exclude, p_collide_with_bodies, p_collide_with_areas);
-	btResult.m_collisionFilterGroup = 0;
-	btResult.m_collisionFilterMask = p_collision_mask;
-
-	space->dynamicsWorld->convexSweepTest(bt_convex_shape, bt_xform_from, bt_xform_to, btResult, space->dynamicsWorld->getDispatchInfo().m_allowedCcdPenetration);
-
-	if (btResult.hasHit()) {
-		const btScalar l = bt_motion.length();
-		r_closest_unsafe = btResult.m_closestHitFraction;
-		r_closest_safe = MAX(r_closest_unsafe - (1 - ((l - 0.01) / l)), 0);
-		if (r_info) {
-			if (btCollisionObject::CO_RIGID_BODY == btResult.m_hitCollisionObject->getInternalType()) {
-				B_TO_G(static_cast<const btRigidBody *>(btResult.m_hitCollisionObject)->getVelocityInLocalPoint(btResult.m_hitPointWorld), r_info->linear_velocity);
-			}
-			CollisionObjectBullet *collision_object = static_cast<CollisionObjectBullet *>(btResult.m_hitCollisionObject->getUserPointer());
-			B_TO_G(btResult.m_hitPointWorld, r_info->point);
-			B_TO_G(btResult.m_hitNormalWorld, r_info->normal);
-			r_info->rid = collision_object->get_self();
-			r_info->collider_id = collision_object->get_instance_id();
-			r_info->shape = btResult.m_shapeId;
-		}
-	} else {
-		r_closest_safe = 1.0f;
-		r_closest_unsafe = 1.0f;
-	}
-
-	bulletdelete(bt_convex_shape);
-	return true; // Mean success
-}
-
-/// Returns the list of contacts pairs in this order: Local contact, other body contact
-bool BulletPhysicsDirectSpaceState::collide_shape(RID p_shape, const Transform3D &p_shape_xform, real_t p_margin, Vector3 *r_results, int p_result_max, int &r_result_count, const Set<RID> &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas) {
-	if (p_result_max <= 0) {
-		return false;
-	}
-
-	ShapeBullet *shape = space->get_physics_server()->get_shape_owner()->get_or_null(p_shape);
-	ERR_FAIL_COND_V(!shape, false);
-
-	btCollisionShape *btShape = shape->create_bt_shape(p_shape_xform.basis.get_scale_abs(), p_margin);
-	if (!btShape->isConvex()) {
-		bulletdelete(btShape);
-		ERR_PRINT("The shape is not a convex shape, then is not supported: shape type: " + itos(shape->get_type()));
-		return false;
-	}
-	btConvexShape *btConvex = static_cast<btConvexShape *>(btShape);
-
-	btTransform bt_xform;
-	G_TO_B(p_shape_xform, bt_xform);
-	UNSCALE_BT_BASIS(bt_xform);
-
-	btCollisionObject collision_object;
-	collision_object.setCollisionShape(btConvex);
-	collision_object.setWorldTransform(bt_xform);
-
-	GodotContactPairContactResultCallback btQuery(&collision_object, r_results, p_result_max, &p_exclude, p_collide_with_bodies, p_collide_with_areas);
-	btQuery.m_collisionFilterGroup = 0;
-	btQuery.m_collisionFilterMask = p_collision_mask;
-	btQuery.m_closestDistanceThreshold = 0;
-	space->dynamicsWorld->contactTest(&collision_object, btQuery);
-
-	r_result_count = btQuery.m_count;
-	bulletdelete(btConvex);
-
-	return btQuery.m_count;
-}
-
-bool BulletPhysicsDirectSpaceState::rest_info(RID p_shape, const Transform3D &p_shape_xform, real_t p_margin, ShapeRestInfo *r_info, const Set<RID> &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas) {
-	ShapeBullet *shape = space->get_physics_server()->get_shape_owner()->get_or_null(p_shape);
-	ERR_FAIL_COND_V(!shape, false);
-
-	btCollisionShape *btShape = shape->create_bt_shape(p_shape_xform.basis.get_scale_abs(), p_margin);
-	if (!btShape->isConvex()) {
-		bulletdelete(btShape);
-		ERR_PRINT("The shape is not a convex shape, then is not supported: shape type: " + itos(shape->get_type()));
-		return false;
-	}
-	btConvexShape *btConvex = static_cast<btConvexShape *>(btShape);
-
-	btTransform bt_xform;
-	G_TO_B(p_shape_xform, bt_xform);
-	UNSCALE_BT_BASIS(bt_xform);
-
-	btCollisionObject collision_object;
-	collision_object.setCollisionShape(btConvex);
-	collision_object.setWorldTransform(bt_xform);
-
-	GodotRestInfoContactResultCallback btQuery(&collision_object, r_info, &p_exclude, p_collide_with_bodies, p_collide_with_areas);
-	btQuery.m_collisionFilterGroup = 0;
-	btQuery.m_collisionFilterMask = p_collision_mask;
-	btQuery.m_closestDistanceThreshold = 0;
-	space->dynamicsWorld->contactTest(&collision_object, btQuery);
-
-	bulletdelete(btConvex);
-
-	if (btQuery.m_collided) {
-		if (btCollisionObject::CO_RIGID_BODY == btQuery.m_rest_info_collision_object->getInternalType()) {
-			B_TO_G(static_cast<const btRigidBody *>(btQuery.m_rest_info_collision_object)->getVelocityInLocalPoint(btQuery.m_rest_info_bt_point), r_info->linear_velocity);
-		}
-		B_TO_G(btQuery.m_rest_info_bt_point, r_info->point);
-	}
-
-	return btQuery.m_collided;
-}
-
-Vector3 BulletPhysicsDirectSpaceState::get_closest_point_to_object_volume(RID p_object, const Vector3 p_point) const {
-	RigidCollisionObjectBullet *rigid_object = space->get_physics_server()->get_rigid_collision_object(p_object);
-	ERR_FAIL_COND_V(!rigid_object, Vector3());
-
-	btVector3 out_closest_point(0, 0, 0);
-	btScalar out_distance = 1e20;
-
-	btVector3 bt_point;
-	G_TO_B(p_point, bt_point);
-
-	btSphereShape point_shape(0.);
-
-	btCollisionShape *shape;
-	btConvexShape *convex_shape;
-	btTransform child_transform;
-	btTransform body_transform(rigid_object->get_bt_collision_object()->getWorldTransform());
-
-	btGjkPairDetector::ClosestPointInput input;
-	input.m_transformA.getBasis().setIdentity();
-	input.m_transformA.setOrigin(bt_point);
-
-	bool shapes_found = false;
-
-	for (int i = rigid_object->get_shape_count() - 1; 0 <= i; --i) {
-		shape = rigid_object->get_bt_shape(i);
-		if (shape->isConvex()) {
-			child_transform = rigid_object->get_bt_shape_transform(i);
-			convex_shape = static_cast<btConvexShape *>(shape);
-
-			input.m_transformB = body_transform * child_transform;
-
-			btPointCollector result;
-			btGjkPairDetector gjk_pair_detector(&point_shape, convex_shape, space->gjk_simplex_solver, space->gjk_epa_pen_solver);
-			gjk_pair_detector.getClosestPoints(input, result, nullptr);
-
-			if (out_distance > result.m_distance) {
-				out_distance = result.m_distance;
-				out_closest_point = result.m_pointInWorld;
-			}
-		}
-		shapes_found = true;
-	}
-
-	if (shapes_found) {
-		Vector3 out;
-		B_TO_G(out_closest_point, out);
-		return out;
-	} else {
-		// no shapes found, use distance to origin.
-		return rigid_object->get_transform().get_origin();
-	}
-}
-
-SpaceBullet::SpaceBullet() {
-	create_empty_world(GLOBAL_DEF("physics/3d/active_soft_world", true));
-	direct_access = memnew(BulletPhysicsDirectSpaceState(this));
-}
-
-SpaceBullet::~SpaceBullet() {
-	memdelete(direct_access);
-	destroy_world();
-}
-
-void SpaceBullet::flush_queries() {
-	const btCollisionObjectArray &colObjArray = dynamicsWorld->getCollisionObjectArray();
-	for (int i = colObjArray.size() - 1; 0 <= i; --i) {
-		static_cast<CollisionObjectBullet *>(colObjArray[i]->getUserPointer())->dispatch_callbacks();
-	}
-}
-
-void SpaceBullet::step(real_t p_delta_time) {
-	delta_time = p_delta_time;
-	dynamicsWorld->stepSimulation(p_delta_time, 0, 0);
-}
-
-void SpaceBullet::set_param(PhysicsServer3D::AreaParameter p_param, const Variant &p_value) {
-	assert(dynamicsWorld);
-
-	switch (p_param) {
-		case PhysicsServer3D::AREA_PARAM_GRAVITY:
-			gravityMagnitude = p_value;
-			update_gravity();
-			break;
-		case PhysicsServer3D::AREA_PARAM_GRAVITY_VECTOR:
-			gravityDirection = p_value;
-			update_gravity();
-			break;
-		case PhysicsServer3D::AREA_PARAM_LINEAR_DAMP:
-			linear_damp = p_value;
-			break;
-		case PhysicsServer3D::AREA_PARAM_ANGULAR_DAMP:
-			angular_damp = p_value;
-			break;
-		case PhysicsServer3D::AREA_PARAM_PRIORITY:
-			// Priority is always 0, the lower
-			break;
-		case PhysicsServer3D::AREA_PARAM_GRAVITY_IS_POINT:
-		case PhysicsServer3D::AREA_PARAM_GRAVITY_DISTANCE_SCALE:
-		case PhysicsServer3D::AREA_PARAM_GRAVITY_POINT_ATTENUATION:
-			break;
-		default:
-			WARN_PRINT("This set parameter (" + itos(p_param) + ") is ignored, the SpaceBullet doesn't support it.");
-			break;
-	}
-}
-
-Variant SpaceBullet::get_param(PhysicsServer3D::AreaParameter p_param) {
-	switch (p_param) {
-		case PhysicsServer3D::AREA_PARAM_GRAVITY:
-			return gravityMagnitude;
-		case PhysicsServer3D::AREA_PARAM_GRAVITY_VECTOR:
-			return gravityDirection;
-		case PhysicsServer3D::AREA_PARAM_LINEAR_DAMP:
-			return linear_damp;
-		case PhysicsServer3D::AREA_PARAM_ANGULAR_DAMP:
-			return angular_damp;
-		case PhysicsServer3D::AREA_PARAM_PRIORITY:
-			return 0; // Priority is always 0, the lower
-		case PhysicsServer3D::AREA_PARAM_GRAVITY_IS_POINT:
-			return false;
-		case PhysicsServer3D::AREA_PARAM_GRAVITY_DISTANCE_SCALE:
-			return 0;
-		case PhysicsServer3D::AREA_PARAM_GRAVITY_POINT_ATTENUATION:
-			return 0;
-		default:
-			WARN_PRINT("This get parameter (" + itos(p_param) + ") is ignored, the SpaceBullet doesn't support it.");
-			return Variant();
-	}
-}
-
-void SpaceBullet::set_param(PhysicsServer3D::SpaceParameter p_param, real_t p_value) {
-	switch (p_param) {
-		case PhysicsServer3D::SPACE_PARAM_CONTACT_RECYCLE_RADIUS:
-		case PhysicsServer3D::SPACE_PARAM_CONTACT_MAX_SEPARATION:
-		case PhysicsServer3D::SPACE_PARAM_BODY_MAX_ALLOWED_PENETRATION:
-		case PhysicsServer3D::SPACE_PARAM_BODY_LINEAR_VELOCITY_SLEEP_THRESHOLD:
-		case PhysicsServer3D::SPACE_PARAM_BODY_ANGULAR_VELOCITY_SLEEP_THRESHOLD:
-		case PhysicsServer3D::SPACE_PARAM_BODY_TIME_TO_SLEEP:
-		default:
-			WARN_PRINT("This set parameter (" + itos(p_param) + ") is ignored, the SpaceBullet doesn't support it.");
-			break;
-	}
-}
-
-real_t SpaceBullet::get_param(PhysicsServer3D::SpaceParameter p_param) {
-	switch (p_param) {
-		case PhysicsServer3D::SPACE_PARAM_CONTACT_RECYCLE_RADIUS:
-		case PhysicsServer3D::SPACE_PARAM_CONTACT_MAX_SEPARATION:
-		case PhysicsServer3D::SPACE_PARAM_BODY_MAX_ALLOWED_PENETRATION:
-		case PhysicsServer3D::SPACE_PARAM_BODY_LINEAR_VELOCITY_SLEEP_THRESHOLD:
-		case PhysicsServer3D::SPACE_PARAM_BODY_ANGULAR_VELOCITY_SLEEP_THRESHOLD:
-		case PhysicsServer3D::SPACE_PARAM_BODY_TIME_TO_SLEEP:
-		default:
-			WARN_PRINT("The SpaceBullet doesn't support this get parameter (" + itos(p_param) + "), 0 is returned.");
-			return 0.f;
-	}
-}
-
-void SpaceBullet::add_area(AreaBullet *p_area) {
-	areas.push_back(p_area);
-	dynamicsWorld->addCollisionObject(p_area->get_bt_ghost(), p_area->get_collision_layer(), p_area->get_collision_mask());
-}
-
-void SpaceBullet::remove_area(AreaBullet *p_area) {
-	areas.erase(p_area);
-	dynamicsWorld->removeCollisionObject(p_area->get_bt_ghost());
-}
-
-void SpaceBullet::reload_collision_filters(AreaBullet *p_area) {
-	btGhostObject *ghost_object = p_area->get_bt_ghost();
-
-	btBroadphaseProxy *ghost_proxy = ghost_object->getBroadphaseHandle();
-	ghost_proxy->m_collisionFilterGroup = p_area->get_collision_layer();
-	ghost_proxy->m_collisionFilterMask = p_area->get_collision_mask();
-
-	dynamicsWorld->refreshBroadphaseProxy(ghost_object);
-}
-
-void SpaceBullet::add_rigid_body(RigidBodyBullet *p_body) {
-	if (p_body->is_static()) {
-		dynamicsWorld->addCollisionObject(p_body->get_bt_rigid_body(), p_body->get_collision_layer(), p_body->get_collision_mask());
-	} else {
-		dynamicsWorld->addRigidBody(p_body->get_bt_rigid_body(), p_body->get_collision_layer(), p_body->get_collision_mask());
-		p_body->scratch_space_override_modificator();
-	}
-}
-
-void SpaceBullet::remove_rigid_body_constraints(RigidBodyBullet *p_body) {
-	btRigidBody *btBody = p_body->get_bt_rigid_body();
-
-	int constraints = btBody->getNumConstraintRefs();
-	if (constraints > 0) {
-		ERR_PRINT("A body connected to joints was removed.");
-		for (int i = 0; i < constraints; i++) {
-			dynamicsWorld->removeConstraint(btBody->getConstraintRef(i));
-		}
-	}
-}
-
-void SpaceBullet::remove_rigid_body(RigidBodyBullet *p_body) {
-	btRigidBody *btBody = p_body->get_bt_rigid_body();
-
-	if (p_body->is_static()) {
-		dynamicsWorld->removeCollisionObject(btBody);
-	} else {
-		dynamicsWorld->removeRigidBody(btBody);
-	}
-}
-
-void SpaceBullet::reload_collision_filters(RigidBodyBullet *p_body) {
-	btRigidBody *rigid_body = p_body->get_bt_rigid_body();
-
-	btBroadphaseProxy *body_proxy = rigid_body->getBroadphaseProxy();
-	body_proxy->m_collisionFilterGroup = p_body->get_collision_layer();
-	body_proxy->m_collisionFilterMask = p_body->get_collision_mask();
-
-	dynamicsWorld->refreshBroadphaseProxy(rigid_body);
-}
-
-void SpaceBullet::add_soft_body(SoftBodyBullet *p_body) {
-	if (is_using_soft_world()) {
-		if (p_body->get_bt_soft_body()) {
-			p_body->get_bt_soft_body()->m_worldInfo = get_soft_body_world_info();
-			static_cast<btSoftRigidDynamicsWorld *>(dynamicsWorld)->addSoftBody(p_body->get_bt_soft_body(), p_body->get_collision_layer(), p_body->get_collision_mask());
-		}
-	} else {
-		ERR_PRINT("This soft body can't be added to non soft world");
-	}
-}
-
-void SpaceBullet::remove_soft_body(SoftBodyBullet *p_body) {
-	if (is_using_soft_world()) {
-		if (p_body->get_bt_soft_body()) {
-			static_cast<btSoftRigidDynamicsWorld *>(dynamicsWorld)->removeSoftBody(p_body->get_bt_soft_body());
-			p_body->get_bt_soft_body()->m_worldInfo = nullptr;
-		}
-	}
-}
-
-void SpaceBullet::reload_collision_filters(SoftBodyBullet *p_body) {
-	// This is necessary to change collision filter
-	remove_soft_body(p_body);
-	add_soft_body(p_body);
-}
-
-void SpaceBullet::add_constraint(ConstraintBullet *p_constraint, bool disableCollisionsBetweenLinkedBodies) {
-	p_constraint->set_space(this);
-	dynamicsWorld->addConstraint(p_constraint->get_bt_constraint(), disableCollisionsBetweenLinkedBodies);
-}
-
-void SpaceBullet::remove_constraint(ConstraintBullet *p_constraint) {
-	dynamicsWorld->removeConstraint(p_constraint->get_bt_constraint());
-}
-
-int SpaceBullet::get_num_collision_objects() const {
-	return dynamicsWorld->getNumCollisionObjects();
-}
-
-void SpaceBullet::remove_all_collision_objects() {
-	for (int i = dynamicsWorld->getNumCollisionObjects() - 1; 0 <= i; --i) {
-		btCollisionObject *btObj = dynamicsWorld->getCollisionObjectArray()[i];
-		CollisionObjectBullet *colObj = static_cast<CollisionObjectBullet *>(btObj->getUserPointer());
-		colObj->set_space(nullptr);
-	}
-}
-
-void onBulletTickCallback(btDynamicsWorld *p_dynamicsWorld, btScalar timeStep) {
-	const btCollisionObjectArray &colObjArray = p_dynamicsWorld->getCollisionObjectArray();
-
-	// Notify all Collision objects the collision checker is started
-	for (int i = colObjArray.size() - 1; 0 <= i; --i) {
-		static_cast<CollisionObjectBullet *>(colObjArray[i]->getUserPointer())->on_collision_checker_start();
-	}
-
-	SpaceBullet *sb = static_cast<SpaceBullet *>(p_dynamicsWorld->getWorldUserInfo());
-	sb->check_ghost_overlaps();
-	sb->check_body_collision();
-
-	for (int i = colObjArray.size() - 1; 0 <= i; --i) {
-		static_cast<CollisionObjectBullet *>(colObjArray[i]->getUserPointer())->on_collision_checker_end();
-	}
-}
-
-BulletPhysicsDirectSpaceState *SpaceBullet::get_direct_state() {
-	return direct_access;
-}
-
-btScalar calculateGodotCombinedRestitution(const btCollisionObject *body0, const btCollisionObject *body1) {
-	return CLAMP(body0->getRestitution() + body1->getRestitution(), 0, 1);
-}
-
-btScalar calculateGodotCombinedFriction(const btCollisionObject *body0, const btCollisionObject *body1) {
-	return ABS(MIN(body0->getFriction(), body1->getFriction()));
-}
-
-void SpaceBullet::create_empty_world(bool p_create_soft_world) {
-	gjk_epa_pen_solver = bulletnew(btGjkEpaPenetrationDepthSolver);
-	gjk_simplex_solver = bulletnew(btVoronoiSimplexSolver);
-
-	void *world_mem;
-	if (p_create_soft_world) {
-		world_mem = malloc(sizeof(btSoftRigidDynamicsWorld));
-	} else {
-		world_mem = malloc(sizeof(btDiscreteDynamicsWorld));
-	}
-
-	ERR_FAIL_COND_MSG(!world_mem, "Out of memory.");
-
-	if (p_create_soft_world) {
-		collisionConfiguration = bulletnew(GodotSoftCollisionConfiguration(static_cast<btDiscreteDynamicsWorld *>(world_mem)));
-	} else {
-		collisionConfiguration = bulletnew(GodotCollisionConfiguration(static_cast<btDiscreteDynamicsWorld *>(world_mem)));
-	}
-
-	dispatcher = bulletnew(GodotCollisionDispatcher(collisionConfiguration));
-	broadphase = bulletnew(btDbvtBroadphase);
-	solver = bulletnew(btSequentialImpulseConstraintSolver);
-
-	if (p_create_soft_world) {
-		dynamicsWorld = new (world_mem) btSoftRigidDynamicsWorld(dispatcher, broadphase, solver, collisionConfiguration);
-		soft_body_world_info = bulletnew(btSoftBodyWorldInfo);
-	} else {
-		dynamicsWorld = new (world_mem) btDiscreteDynamicsWorld(dispatcher, broadphase, solver, collisionConfiguration);
-	}
-
-	ghostPairCallback = bulletnew(btGhostPairCallback);
-	godotFilterCallback = bulletnew(GodotFilterCallback);
-	gCalculateCombinedRestitutionCallback = &calculateGodotCombinedRestitution;
-	gCalculateCombinedFrictionCallback = &calculateGodotCombinedFriction;
-	gContactAddedCallback = &godotContactAddedCallback;
-
-	dynamicsWorld->setWorldUserInfo(this);
-
-	dynamicsWorld->setInternalTickCallback(onBulletTickCallback, this, false);
-	dynamicsWorld->getBroadphase()->getOverlappingPairCache()->setInternalGhostPairCallback(ghostPairCallback); // Setup ghost check
-	dynamicsWorld->getPairCache()->setOverlapFilterCallback(godotFilterCallback);
-
-	if (soft_body_world_info) {
-		soft_body_world_info->m_broadphase = broadphase;
-		soft_body_world_info->m_dispatcher = dispatcher;
-		soft_body_world_info->m_sparsesdf.Initialize();
-	}
-
-	update_gravity();
-}
-
-void SpaceBullet::destroy_world() {
-	/// The world elements (like: Collision Objects, Constraints, Shapes) are managed by godot
-
-	dynamicsWorld->getBroadphase()->getOverlappingPairCache()->setInternalGhostPairCallback(nullptr);
-	dynamicsWorld->getPairCache()->setOverlapFilterCallback(nullptr);
-
-	bulletdelete(ghostPairCallback);
-	bulletdelete(godotFilterCallback);
-
-	// Deallocate world
-	dynamicsWorld->~btDiscreteDynamicsWorld();
-	free(dynamicsWorld);
-	dynamicsWorld = nullptr;
-
-	bulletdelete(solver);
-	bulletdelete(broadphase);
-	bulletdelete(dispatcher);
-	bulletdelete(collisionConfiguration);
-	bulletdelete(soft_body_world_info);
-	bulletdelete(gjk_simplex_solver);
-	bulletdelete(gjk_epa_pen_solver);
-}
-
-void SpaceBullet::check_ghost_overlaps() {
-	// For each area
-	for (int area_idx = 0; area_idx < areas.size(); area_idx++) {
-		AreaBullet *area = areas[area_idx];
-		if (!area->is_monitoring()) {
-			continue;
-		}
-
-		btGhostObject *bt_ghost = area->get_bt_ghost();
-		const btTransform &area_transform = area->get_transform__bullet();
-		const btVector3 &area_scale(area->get_bt_body_scale());
-
-		// Mark all current overlapping shapes dirty.
-		area->mark_all_overlaps_dirty();
-
-		// Broadphase
-		const btAlignedObjectArray<btCollisionObject *> overlapping_pairs = bt_ghost->getOverlappingPairs();
-		// Narrowphase
-		for (int pair_idx = 0; pair_idx < overlapping_pairs.size(); pair_idx++) {
-			btCollisionObject *other_bt_collision_object = overlapping_pairs[pair_idx];
-			RigidCollisionObjectBullet *other_object = static_cast<RigidCollisionObjectBullet *>(other_bt_collision_object->getUserPointer());
-			const btTransform &other_transform = other_object->get_transform__bullet();
-			const btVector3 &other_scale(other_object->get_bt_body_scale());
-
-			if (!area->is_updated() && !other_object->is_updated()) {
-				area->mark_object_overlaps_inside(other_object);
-				continue;
-			}
-
-			if (other_bt_collision_object->getUserIndex() == CollisionObjectBullet::TYPE_AREA) {
-				if (!static_cast<AreaBullet *>(other_bt_collision_object->getUserPointer())->is_monitorable()) {
-					continue;
-				}
-			} else if (other_bt_collision_object->getUserIndex() != CollisionObjectBullet::TYPE_RIGID_BODY) {
-				continue;
-			}
-
-			// For each area shape
-			for (int our_shape_id = 0; our_shape_id < area->get_shape_count(); our_shape_id++) {
-				btCollisionShape *area_shape = area->get_bt_shape(our_shape_id);
-				if (!area_shape->isConvex()) {
-					continue;
-				}
-				btConvexShape *area_convex_shape = static_cast<btConvexShape *>(area_shape);
-
-				btTransform area_shape_transform(area->get_bt_shape_transform(our_shape_id));
-				area_shape_transform.getOrigin() *= area_scale;
-				btGjkPairDetector::ClosestPointInput gjk_input;
-				gjk_input.m_transformA = area_transform * area_shape_transform;
-
-				// For each other object shape
-				for (int other_shape_id = 0; other_shape_id < other_object->get_shape_count(); other_shape_id++) {
-					btCollisionShape *other_shape = other_object->get_bt_shape(other_shape_id);
-					btTransform other_shape_transform(other_object->get_bt_shape_transform(other_shape_id));
-					other_shape_transform.getOrigin() *= other_scale;
-					gjk_input.m_transformB = other_transform * other_shape_transform;
-
-					if (other_shape->isConvex()) {
-						btPointCollector result;
-						btGjkPairDetector gjk_pair_detector(
-								area_convex_shape,
-								static_cast<btConvexShape *>(other_shape),
-								gjk_simplex_solver,
-								gjk_epa_pen_solver);
-
-						gjk_pair_detector.getClosestPoints(gjk_input, result, nullptr);
-						if (result.m_distance <= 0) {
-							area->set_overlap(other_object, other_shape_id, our_shape_id);
-						}
-					} else { // Other shape is not convex.
-						btCollisionObjectWrapper obA(nullptr, area_convex_shape, bt_ghost, gjk_input.m_transformA, -1, our_shape_id);
-						btCollisionObjectWrapper obB(nullptr, other_shape, other_bt_collision_object, gjk_input.m_transformB, -1, other_shape_id);
-						btCollisionAlgorithm *algorithm = dispatcher->findAlgorithm(&obA, &obB, nullptr, BT_CONTACT_POINT_ALGORITHMS);
-
-						if (!algorithm) {
-							continue;
-						}
-
-						GodotDeepPenetrationContactResultCallback contactPointResult(&obA, &obB);
-						algorithm->processCollision(&obA, &obB, dynamicsWorld->getDispatchInfo(), &contactPointResult);
-						algorithm->~btCollisionAlgorithm();
-						dispatcher->freeCollisionAlgorithm(algorithm);
-
-						if (contactPointResult.hasHit()) {
-							area->set_overlap(other_object, our_shape_id, other_shape_id);
-						}
-					}
-				} // End for each other object shape
-			} // End for each area shape
-		} // End for each overlapping pair
-
-		// All overlapping shapes still marked dirty must have exited.
-		area->mark_all_dirty_overlaps_as_exit();
-	} // End for each area
-}
-
-void SpaceBullet::check_body_collision() {
-#ifdef DEBUG_ENABLED
-	reset_debug_contact_count();
-#endif
-
-	const int numManifolds = dynamicsWorld->getDispatcher()->getNumManifolds();
-	for (int i = 0; i < numManifolds; ++i) {
-		btPersistentManifold *contactManifold = dynamicsWorld->getDispatcher()->getManifoldByIndexInternal(i);
-
-		// I know this static cast is a bit risky. But I'm checking its type just after it.
-		// This allow me to avoid a lot of other cast and checks
-		RigidBodyBullet *bodyA = static_cast<RigidBodyBullet *>(contactManifold->getBody0()->getUserPointer());
-		RigidBodyBullet *bodyB = static_cast<RigidBodyBullet *>(contactManifold->getBody1()->getUserPointer());
-
-		if (CollisionObjectBullet::TYPE_RIGID_BODY == bodyA->getType() && CollisionObjectBullet::TYPE_RIGID_BODY == bodyB->getType()) {
-			if (!bodyA->can_add_collision() && !bodyB->can_add_collision()) {
-				continue;
-			}
-
-			const int numContacts = contactManifold->getNumContacts();
-
-			/// Since I don't need report all contacts for these objects,
-			/// So report only the first
-#define REPORT_ALL_CONTACTS 0
-#if REPORT_ALL_CONTACTS
-			for (int j = 0; j < numContacts; j++) {
-				btManifoldPoint &pt = contactManifold->getContactPoint(j);
-#else
-			if (numContacts) {
-				btManifoldPoint &pt = contactManifold->getContactPoint(0);
-#endif
-				if (
-						pt.getDistance() < 0.0 ||
-						bodyA->was_colliding(bodyB) ||
-						bodyB->was_colliding(bodyA)) {
-					Vector3 collisionWorldPosition;
-					Vector3 collisionLocalPosition;
-					Vector3 normalOnB;
-					real_t appliedImpulse = pt.m_appliedImpulse;
-					B_TO_G(pt.m_normalWorldOnB, normalOnB);
-
-					// The pt.m_index only contains the shape index when more than one collision shape is used
-					// and only if the collision shape is not a concave collision shape.
-					// A value of -1 in pt.m_partId indicates the pt.m_index is a shape index.
-					int shape_index_a = 0;
-					if (bodyA->get_shape_count() > 1 && pt.m_partId0 == -1) {
-						shape_index_a = pt.m_index0;
-					}
-					int shape_index_b = 0;
-					if (bodyB->get_shape_count() > 1 && pt.m_partId1 == -1) {
-						shape_index_b = pt.m_index1;
-					}
-
-					if (bodyA->can_add_collision()) {
-						B_TO_G(pt.getPositionWorldOnB(), collisionWorldPosition);
-						/// pt.m_localPointB Doesn't report the exact point in local space
-						B_TO_G(pt.getPositionWorldOnB() - contactManifold->getBody1()->getWorldTransform().getOrigin(), collisionLocalPosition);
-						bodyA->add_collision_object(bodyB, collisionWorldPosition, collisionLocalPosition, normalOnB, appliedImpulse, shape_index_b, shape_index_a);
-					}
-					if (bodyB->can_add_collision()) {
-						B_TO_G(pt.getPositionWorldOnA(), collisionWorldPosition);
-						/// pt.m_localPointA Doesn't report the exact point in local space
-						B_TO_G(pt.getPositionWorldOnA() - contactManifold->getBody0()->getWorldTransform().getOrigin(), collisionLocalPosition);
-						bodyB->add_collision_object(bodyA, collisionWorldPosition, collisionLocalPosition, normalOnB * -1, appliedImpulse * -1, shape_index_a, shape_index_b);
-					}
-
-#ifdef DEBUG_ENABLED
-					if (is_debugging_contacts()) {
-						add_debug_contact(collisionWorldPosition);
-					}
-#endif
-				}
-			}
-		}
-	}
-}
-
-void SpaceBullet::update_gravity() {
-	btVector3 btGravity;
-	G_TO_B(gravityDirection * gravityMagnitude, btGravity);
-	//dynamicsWorld->setGravity(btGravity);
-	dynamicsWorld->setGravity(btVector3(0, 0, 0));
-	if (soft_body_world_info) {
-		soft_body_world_info->m_gravity = btGravity;
-	}
-}
-
-/// IMPORTANT: Please don't turn it ON this is not managed correctly!!
-/// I'm leaving this here just for future tests.
-/// Debug motion and normal vector drawing
-#define debug_test_motion 0
-
-#define RECOVERING_MOVEMENT_SCALE 0.4
-#define RECOVERING_MOVEMENT_CYCLES 4
-
-#if debug_test_motion
-
-#include "scene/3d/immediate_geometry.h"
-
-static ImmediateGeometry3D *motionVec(nullptr);
-static ImmediateGeometry3D *normalLine(nullptr);
-static Ref<StandardMaterial3D> red_mat;
-static Ref<StandardMaterial3D> blue_mat;
-#endif
-
-bool SpaceBullet::test_body_motion(RigidBodyBullet *p_body, const Transform3D &p_from, const Vector3 &p_motion, bool p_infinite_inertia, PhysicsServer3D::MotionResult *r_result, bool p_exclude_raycast_shapes, const Set<RID> &p_exclude) {
-#if debug_test_motion
-	/// Yes I know this is not good, but I've used it as fast debugging hack.
-	/// I'm leaving it here just for speedup the other eventual debugs
-	if (!normalLine) {
-		motionVec = memnew(ImmediateGeometry3D);
-		normalLine = memnew(ImmediateGeometry3D);
-		SceneTree::get_singleton()->get_current_scene()->add_child(motionVec);
-		SceneTree::get_singleton()->get_current_scene()->add_child(normalLine);
-
-		motionVec->set_as_top_level(true);
-		normalLine->set_as_top_level(true);
-
-		red_mat = Ref<StandardMaterial3D>(memnew(StandardMaterial3D));
-		red_mat->set_shading_mode(StandardMaterial3D::SHADING_MODE_UNSHADED);
-		red_mat->set_line_width(20.0);
-		red_mat->set_transparency(StandardMaterial3D::TRANSPARENCY_ALPHA);
-		red_mat->set_flag(StandardMaterial3D::FLAG_ALBEDO_FROM_VERTEX_COLOR, true);
-		red_mat->set_flag(StandardMaterial3D::FLAG_SRGB_VERTEX_COLOR, true);
-		red_mat->set_albedo(Color(1, 0, 0, 1));
-		motionVec->set_material_override(red_mat);
-
-		blue_mat = Ref<StandardMaterial3D>(memnew(StandardMaterial3D));
-		blue_mat->set_shading_mode(StandardMaterial3D::SHADING_MODE_UNSHADED);
-		blue_mat->set_line_width(20.0);
-		blue_mat->set_transparency(StandardMaterial3D::TRANSPARENCY_ALPHA);
-		blue_mat->set_flag(StandardMaterial3D::FLAG_ALBEDO_FROM_VERTEX_COLOR, true);
-		blue_mat->set_flag(StandardMaterial3D::FLAG_SRGB_VERTEX_COLOR, true);
-		blue_mat->set_albedo(Color(0, 0, 1, 1));
-		normalLine->set_material_override(blue_mat);
-	}
-#endif
-
-	btTransform body_transform;
-	G_TO_B(p_from, body_transform);
-	UNSCALE_BT_BASIS(body_transform);
-
-	if (!p_body->get_kinematic_utilities()) {
-		p_body->init_kinematic_utilities();
-	}
-
-	btVector3 initial_recover_motion(0, 0, 0);
-	{ /// Phase one - multi shapes depenetration using margin
-		for (int t(RECOVERING_MOVEMENT_CYCLES); 0 < t; --t) {
-			if (!recover_from_penetration(p_body, body_transform, RECOVERING_MOVEMENT_SCALE, p_infinite_inertia, initial_recover_motion, nullptr, p_exclude)) {
-				break;
-			}
-		}
-		// Add recover movement in order to make it safe
-		body_transform.getOrigin() += initial_recover_motion;
-	}
-
-	btVector3 motion;
-	G_TO_B(p_motion, motion);
-	real_t total_length = motion.length();
-	real_t unsafe_fraction = 1.0;
-	real_t safe_fraction = 1.0;
-	{
-		// Phase two - sweep test, from a secure position without margin
-
-		const int shape_count(p_body->get_shape_count());
-
-#if debug_test_motion
-		Vector3 sup_line;
-		B_TO_G(body_safe_position.getOrigin(), sup_line);
-		motionVec->clear();
-		motionVec->begin(Mesh::PRIMITIVE_LINES, nullptr);
-		motionVec->add_vertex(sup_line);
-		motionVec->add_vertex(sup_line + p_motion * 10);
-		motionVec->end();
-#endif
-
-		for (int shIndex = 0; shIndex < shape_count; ++shIndex) {
-			if (p_body->is_shape_disabled(shIndex)) {
-				continue;
-			}
-
-			if (!p_body->get_bt_shape(shIndex)->isConvex()) {
-				// Skip no convex shape
-				continue;
-			}
-
-			if (p_exclude_raycast_shapes && p_body->get_bt_shape(shIndex)->getShapeType() == CUSTOM_CONVEX_SHAPE_TYPE) {
-				// Skip rayshape in order to implement custom separation process
-				continue;
-			}
-
-			btConvexShape *convex_shape_test(static_cast<btConvexShape *>(p_body->get_bt_shape(shIndex)));
-
-			btTransform shape_world_from = body_transform * p_body->get_kinematic_utilities()->shapes[shIndex].transform;
-
-			btTransform shape_world_to(shape_world_from);
-			shape_world_to.getOrigin() += motion;
-
-			if ((shape_world_to.getOrigin() - shape_world_from.getOrigin()).fuzzyZero()) {
-				motion = btVector3(0, 0, 0);
-				break;
-			}
-
-			GodotKinClosestConvexResultCallback btResult(shape_world_from.getOrigin(), shape_world_to.getOrigin(), p_body, p_infinite_inertia, &p_exclude);
-			btResult.m_collisionFilterGroup = p_body->get_collision_layer();
-			btResult.m_collisionFilterMask = p_body->get_collision_mask();
-
-			dynamicsWorld->convexSweepTest(convex_shape_test, shape_world_from, shape_world_to, btResult, dynamicsWorld->getDispatchInfo().m_allowedCcdPenetration);
-
-			if (btResult.hasHit()) {
-				if (total_length > CMP_EPSILON) {
-					real_t hit_fraction = btResult.m_closestHitFraction * motion.length() / total_length;
-					if (hit_fraction < unsafe_fraction) {
-						unsafe_fraction = hit_fraction;
-						real_t margin = p_body->get_kinematic_utilities()->safe_margin;
-						safe_fraction = MAX(hit_fraction - (1 - ((total_length - margin) / total_length)), 0);
-					}
-				}
-
-				/// Since for each sweep test I fix the motion of new shapes in base the recover result,
-				/// if another shape will hit something it means that has a deepest penetration respect the previous shape
-				motion *= btResult.m_closestHitFraction;
-			}
-		}
-
-		body_transform.getOrigin() += motion;
-	}
-
-	bool has_penetration = false;
-
-	{ /// Phase three - contact test with margin
-
-		btVector3 __rec(0, 0, 0);
-		RecoverResult r_recover_result;
-
-		has_penetration = recover_from_penetration(p_body, body_transform, 1, p_infinite_inertia, __rec, &r_recover_result, p_exclude);
-
-		// Parse results
-		if (r_result) {
-			B_TO_G(motion + initial_recover_motion + __rec, r_result->motion);
-
-			if (has_penetration) {
-				const btRigidBody *btRigid = static_cast<const btRigidBody *>(r_recover_result.other_collision_object);
-				CollisionObjectBullet *collisionObject = static_cast<CollisionObjectBullet *>(btRigid->getUserPointer());
-
-				B_TO_G(motion, r_result->remainder); // is the remaining movements
-				r_result->remainder = p_motion - r_result->remainder;
-
-				B_TO_G(r_recover_result.pointWorld, r_result->collision_point);
-				B_TO_G(r_recover_result.normal, r_result->collision_normal);
-				B_TO_G(btRigid->getVelocityInLocalPoint(r_recover_result.pointWorld - btRigid->getWorldTransform().getOrigin()), r_result->collider_velocity); // It calculates velocity at point and assign it using special function Bullet_to_Godot
-				r_result->collider = collisionObject->get_self();
-				r_result->collider_id = collisionObject->get_instance_id();
-				r_result->collider_shape = r_recover_result.other_compound_shape_index;
-				r_result->collision_local_shape = r_recover_result.local_shape_most_recovered;
-				r_result->collision_depth = Math::abs(r_recover_result.penetration_distance);
-				r_result->collision_safe_fraction = safe_fraction;
-				r_result->collision_unsafe_fraction = unsafe_fraction;
-
-#if debug_test_motion
-				Vector3 sup_line2;
-				B_TO_G(motion, sup_line2);
-				normalLine->clear();
-				normalLine->begin(Mesh::PRIMITIVE_LINES, nullptr);
-				normalLine->add_vertex(r_result->collision_point);
-				normalLine->add_vertex(r_result->collision_point + r_result->collision_normal * 10);
-				normalLine->end();
-#endif
-			} else {
-				r_result->remainder = Vector3();
-			}
-		}
-	}
-
-	return has_penetration;
-}
-
-int SpaceBullet::test_ray_separation(RigidBodyBullet *p_body, const Transform3D &p_transform, bool p_infinite_inertia, Vector3 &r_recover_motion, PhysicsServer3D::SeparationResult *r_results, int p_result_max, real_t p_margin) {
-	btTransform body_transform;
-	G_TO_B(p_transform, body_transform);
-	UNSCALE_BT_BASIS(body_transform);
-
-	if (!p_body->get_kinematic_utilities()) {
-		p_body->init_kinematic_utilities();
-	}
-
-	btVector3 recover_motion(0, 0, 0);
-
-	int rays_found = 0;
-	int rays_found_this_round = 0;
-
-	for (int t(RECOVERING_MOVEMENT_CYCLES); 0 < t; --t) {
-		PhysicsServer3D::SeparationResult *next_results = &r_results[rays_found];
-		rays_found_this_round = recover_from_penetration_ray(p_body, body_transform, RECOVERING_MOVEMENT_SCALE, p_infinite_inertia, p_result_max - rays_found, recover_motion, next_results);
-
-		rays_found += rays_found_this_round;
-		if (rays_found_this_round == 0) {
-			body_transform.getOrigin() += recover_motion;
-			break;
-		}
-	}
-
-	B_TO_G(recover_motion, r_recover_motion);
-	return rays_found;
-}
-
-struct RecoverPenetrationBroadPhaseCallback : public btBroadphaseAabbCallback {
-private:
-	btDbvtVolume bounds;
-
-	const btCollisionObject *self_collision_object;
-	uint32_t collision_layer = 0;
-
-	struct CompoundLeafCallback : btDbvt::ICollide {
-	private:
-		RecoverPenetrationBroadPhaseCallback *parent_callback = nullptr;
-		btCollisionObject *collision_object = nullptr;
-
-	public:
-		CompoundLeafCallback(RecoverPenetrationBroadPhaseCallback *p_parent_callback, btCollisionObject *p_collision_object) :
-				parent_callback(p_parent_callback),
-				collision_object(p_collision_object) {
-		}
-
-		void Process(const btDbvtNode *leaf) {
-			BroadphaseResult result;
-			result.collision_object = collision_object;
-			result.compound_child_index = leaf->dataAsInt;
-			parent_callback->results.push_back(result);
-		}
-	};
-
-public:
-	struct BroadphaseResult {
-		btCollisionObject *collision_object = nullptr;
-		int compound_child_index = 0;
-	};
-
-	Vector<BroadphaseResult> results;
-
-public:
-	RecoverPenetrationBroadPhaseCallback(const btCollisionObject *p_self_collision_object, uint32_t p_collision_layer, btVector3 p_aabb_min, btVector3 p_aabb_max) :
-			self_collision_object(p_self_collision_object),
-			collision_layer(p_collision_layer) {
-		bounds = btDbvtVolume::FromMM(p_aabb_min, p_aabb_max);
-	}
-
-	virtual ~RecoverPenetrationBroadPhaseCallback() {}
-
-	virtual bool process(const btBroadphaseProxy *proxy) {
-		btCollisionObject *co = static_cast<btCollisionObject *>(proxy->m_clientObject);
-		if (co->getInternalType() <= btCollisionObject::CO_RIGID_BODY) {
-			if (self_collision_object != proxy->m_clientObject && (proxy->collision_layer & m_collisionFilterMask)) {
-				if (co->getCollisionShape()->isCompound()) {
-					const btCompoundShape *cs = static_cast<btCompoundShape *>(co->getCollisionShape());
-
-					if (cs->getNumChildShapes() > 1) {
-						const btDbvt *tree = cs->getDynamicAabbTree();
-						ERR_FAIL_COND_V(tree == nullptr, true);
-
-						// Transform bounds into compound shape local space
-						const btTransform other_in_compound_space = co->getWorldTransform().inverse();
-						const btMatrix3x3 abs_b = other_in_compound_space.getBasis().absolute();
-						const btVector3 local_center = other_in_compound_space(bounds.Center());
-						const btVector3 local_extent = bounds.Extents().dot3(abs_b[0], abs_b[1], abs_b[2]);
-						const btVector3 local_aabb_min = local_center - local_extent;
-						const btVector3 local_aabb_max = local_center + local_extent;
-						const btDbvtVolume local_bounds = btDbvtVolume::FromMM(local_aabb_min, local_aabb_max);
-
-						// Test collision against compound child shapes using its AABB tree
-						CompoundLeafCallback compound_leaf_callback(this, co);
-						tree->collideTV(tree->m_root, local_bounds, compound_leaf_callback);
-					} else {
-						// If there's only a single child shape then there's no need to search any more, we know which child overlaps
-						BroadphaseResult result;
-						result.collision_object = co;
-						result.compound_child_index = 0;
-						results.push_back(result);
-					}
-				} else {
-					BroadphaseResult result;
-					result.collision_object = co;
-					result.compound_child_index = -1;
-					results.push_back(result);
-				}
-				return true;
-			}
-		}
-		return false;
-	}
-};
-
-bool SpaceBullet::recover_from_penetration(RigidBodyBullet *p_body, const btTransform &p_body_position, btScalar p_recover_movement_scale, bool p_infinite_inertia, btVector3 &r_delta_recover_movement, RecoverResult *r_recover_result, const Set<RID> &p_exclude) {
-	// Calculate the cumulative AABB of all shapes of the kinematic body
-	btVector3 aabb_min, aabb_max;
-	bool shapes_found = false;
-
-	for (int kinIndex = p_body->get_kinematic_utilities()->shapes.size() - 1; 0 <= kinIndex; --kinIndex) {
-		const RigidBodyBullet::KinematicShape &kin_shape(p_body->get_kinematic_utilities()->shapes[kinIndex]);
-		if (!kin_shape.is_active()) {
-			continue;
-		}
-
-		if (kin_shape.shape->getShapeType() == CUSTOM_CONVEX_SHAPE_TYPE) {
-			// Skip rayshape in order to implement custom separation process
-			continue;
-		}
-
-		btTransform shape_transform = p_body_position * kin_shape.transform;
-		shape_transform.getOrigin() += r_delta_recover_movement;
-
-		btVector3 shape_aabb_min, shape_aabb_max;
-		kin_shape.shape->getAabb(shape_transform, shape_aabb_min, shape_aabb_max);
-
-		if (!shapes_found) {
-			aabb_min = shape_aabb_min;
-			aabb_max = shape_aabb_max;
-			shapes_found = true;
-		} else {
-			aabb_min.setX((aabb_min.x() < shape_aabb_min.x()) ? aabb_min.x() : shape_aabb_min.x());
-			aabb_min.setY((aabb_min.y() < shape_aabb_min.y()) ? aabb_min.y() : shape_aabb_min.y());
-			aabb_min.setZ((aabb_min.z() < shape_aabb_min.z()) ? aabb_min.z() : shape_aabb_min.z());
-
-			aabb_max.setX((aabb_max.x() > shape_aabb_max.x()) ? aabb_max.x() : shape_aabb_max.x());
-			aabb_max.setY((aabb_max.y() > shape_aabb_max.y()) ? aabb_max.y() : shape_aabb_max.y());
-			aabb_max.setZ((aabb_max.z() > shape_aabb_max.z()) ? aabb_max.z() : shape_aabb_max.z());
-		}
-	}
-
-	// If there are no shapes then there is no penetration either
-	if (!shapes_found) {
-		return false;
-	}
-
-	// Perform broadphase test
-	RecoverPenetrationBroadPhaseCallback recover_broad_result(p_body->get_bt_collision_object(), p_body->get_collision_layer(), aabb_min, aabb_max);
-	dynamicsWorld->getBroadphase()->aabbTest(aabb_min, aabb_max, recover_broad_result);
-
-	bool penetration = false;
-
-	// Perform narrowphase per shape
-	for (int kinIndex = p_body->get_kinematic_utilities()->shapes.size() - 1; 0 <= kinIndex; --kinIndex) {
-		const RigidBodyBullet::KinematicShape &kin_shape(p_body->get_kinematic_utilities()->shapes[kinIndex]);
-		if (!kin_shape.is_active()) {
-			continue;
-		}
-
-		if (kin_shape.shape->getShapeType() == CUSTOM_CONVEX_SHAPE_TYPE) {
-			// Skip rayshape in order to implement custom separation process
-			continue;
-		}
-
-		if (kin_shape.shape->getShapeType() == EMPTY_SHAPE_PROXYTYPE) {
-			continue;
-		}
-
-		btTransform shape_transform = p_body_position * kin_shape.transform;
-		shape_transform.getOrigin() += r_delta_recover_movement;
-
-		for (int i = recover_broad_result.results.size() - 1; 0 <= i; --i) {
-			btCollisionObject *otherObject = recover_broad_result.results[i].collision_object;
-
-			CollisionObjectBullet *gObj = static_cast<CollisionObjectBullet *>(otherObject->getUserPointer());
-			if (p_exclude.has(gObj->get_self())) {
-				continue;
-			}
-
-			if (p_infinite_inertia && !otherObject->isStaticOrKinematicObject()) {
-				otherObject->activate(); // Force activation of hitten rigid, soft body
-				continue;
-			} else if (!p_body->get_bt_collision_object()->checkCollideWith(otherObject) || !otherObject->checkCollideWith(p_body->get_bt_collision_object())) {
-				continue;
-			}
-
-			if (otherObject->getCollisionShape()->isCompound()) {
-				const btCompoundShape *cs = static_cast<const btCompoundShape *>(otherObject->getCollisionShape());
-				int shape_idx = recover_broad_result.results[i].compound_child_index;
-				ERR_FAIL_COND_V(shape_idx < 0 || shape_idx >= cs->getNumChildShapes(), false);
-
-				if (cs->getChildShape(shape_idx)->isConvex()) {
-					if (RFP_convex_convex_test(kin_shape.shape, static_cast<const btConvexShape *>(cs->getChildShape(shape_idx)), otherObject, kinIndex, shape_idx, shape_transform, otherObject->getWorldTransform() * cs->getChildTransform(shape_idx), p_recover_movement_scale, r_delta_recover_movement, r_recover_result)) {
-						penetration = true;
-					}
-				} else {
-					if (RFP_convex_world_test(kin_shape.shape, cs->getChildShape(shape_idx), p_body->get_bt_collision_object(), otherObject, kinIndex, shape_idx, shape_transform, otherObject->getWorldTransform() * cs->getChildTransform(shape_idx), p_recover_movement_scale, r_delta_recover_movement, r_recover_result)) {
-						penetration = true;
-					}
-				}
-			} else if (otherObject->getCollisionShape()->isConvex()) { /// Execute GJK test against object shape
-				if (RFP_convex_convex_test(kin_shape.shape, static_cast<const btConvexShape *>(otherObject->getCollisionShape()), otherObject, kinIndex, 0, shape_transform, otherObject->getWorldTransform(), p_recover_movement_scale, r_delta_recover_movement, r_recover_result)) {
-					penetration = true;
-				}
-			} else {
-				if (RFP_convex_world_test(kin_shape.shape, otherObject->getCollisionShape(), p_body->get_bt_collision_object(), otherObject, kinIndex, 0, shape_transform, otherObject->getWorldTransform(), p_recover_movement_scale, r_delta_recover_movement, r_recover_result)) {
-					penetration = true;
-				}
-			}
-		}
-	}
-
-	return penetration;
-}
-
-bool SpaceBullet::RFP_convex_convex_test(const btConvexShape *p_shapeA, const btConvexShape *p_shapeB, btCollisionObject *p_objectB, int p_shapeId_A, int p_shapeId_B, const btTransform &p_transformA, const btTransform &p_transformB, btScalar p_recover_movement_scale, btVector3 &r_delta_recover_movement, RecoverResult *r_recover_result) {
-	// Initialize GJK input
-	btGjkPairDetector::ClosestPointInput gjk_input;
-	gjk_input.m_transformA = p_transformA;
-	gjk_input.m_transformB = p_transformB;
-
-	// Perform GJK test
-	btPointCollector result;
-	btGjkPairDetector gjk_pair_detector(p_shapeA, p_shapeB, gjk_simplex_solver, gjk_epa_pen_solver);
-	gjk_pair_detector.getClosestPoints(gjk_input, result, nullptr);
-	if (0 > result.m_distance) {
-		// Has penetration
-		r_delta_recover_movement += result.m_normalOnBInWorld * (result.m_distance * -1 * p_recover_movement_scale);
-
-		if (r_recover_result) {
-			if (result.m_distance < r_recover_result->penetration_distance) {
-				r_recover_result->hasPenetration = true;
-				r_recover_result->local_shape_most_recovered = p_shapeId_A;
-				r_recover_result->other_collision_object = p_objectB;
-				r_recover_result->other_compound_shape_index = p_shapeId_B;
-				r_recover_result->penetration_distance = result.m_distance;
-				r_recover_result->pointWorld = result.m_pointInWorld;
-				r_recover_result->normal = result.m_normalOnBInWorld;
-			}
-		}
-		return true;
-	}
-	return false;
-}
-
-bool SpaceBullet::RFP_convex_world_test(const btConvexShape *p_shapeA, const btCollisionShape *p_shapeB, btCollisionObject *p_objectA, btCollisionObject *p_objectB, int p_shapeId_A, int p_shapeId_B, const btTransform &p_transformA, const btTransform &p_transformB, btScalar p_recover_movement_scale, btVector3 &r_delta_recover_movement, RecoverResult *r_recover_result) {
-	/// Contact test
-
-	btTransform tA(p_transformA);
-
-	btCollisionObjectWrapper obA(nullptr, p_shapeA, p_objectA, tA, -1, p_shapeId_A);
-	btCollisionObjectWrapper obB(nullptr, p_shapeB, p_objectB, p_transformB, -1, p_shapeId_B);
-
-	btCollisionAlgorithm *algorithm = dispatcher->findAlgorithm(&obA, &obB, nullptr, BT_CONTACT_POINT_ALGORITHMS);
-	if (algorithm) {
-		GodotDeepPenetrationContactResultCallback contactPointResult(&obA, &obB);
-		//discrete collision detection query
-		algorithm->processCollision(&obA, &obB, dynamicsWorld->getDispatchInfo(), &contactPointResult);
-
-		algorithm->~btCollisionAlgorithm();
-		dispatcher->freeCollisionAlgorithm(algorithm);
-
-		if (contactPointResult.hasHit()) {
-			r_delta_recover_movement += contactPointResult.m_pointNormalWorld * (contactPointResult.m_penetration_distance * -1 * p_recover_movement_scale);
-			if (r_recover_result) {
-				if (contactPointResult.m_penetration_distance < r_recover_result->penetration_distance) {
-					r_recover_result->hasPenetration = true;
-					r_recover_result->local_shape_most_recovered = p_shapeId_A;
-					r_recover_result->other_collision_object = p_objectB;
-					r_recover_result->other_compound_shape_index = p_shapeId_B;
-					r_recover_result->penetration_distance = contactPointResult.m_penetration_distance;
-					r_recover_result->pointWorld = contactPointResult.m_pointWorld;
-					r_recover_result->normal = contactPointResult.m_pointNormalWorld;
-				}
-			}
-			return true;
-		}
-	}
-	return false;
-}
-
-int SpaceBullet::add_separation_result(PhysicsServer3D::SeparationResult *r_result, const SpaceBullet::RecoverResult &p_recover_result, int p_shape_id, const btCollisionObject *p_other_object) const {
-	// optimize results (ignore non-colliding)
-	if (p_recover_result.penetration_distance < 0.0) {
-		const btRigidBody *btRigid = static_cast<const btRigidBody *>(p_other_object);
-		CollisionObjectBullet *collisionObject = static_cast<CollisionObjectBullet *>(p_other_object->getUserPointer());
-
-		r_result->collision_depth = p_recover_result.penetration_distance;
-		B_TO_G(p_recover_result.pointWorld, r_result->collision_point);
-		B_TO_G(p_recover_result.normal, r_result->collision_normal);
-		B_TO_G(btRigid->getVelocityInLocalPoint(p_recover_result.pointWorld - btRigid->getWorldTransform().getOrigin()), r_result->collider_velocity);
-		r_result->collision_local_shape = p_shape_id;
-		r_result->collider_id = collisionObject->get_instance_id();
-		r_result->collider = collisionObject->get_self();
-		r_result->collider_shape = p_recover_result.other_compound_shape_index;
-
-		return 1;
-	} else {
-		return 0;
-	}
-}
-
-int SpaceBullet::recover_from_penetration_ray(RigidBodyBullet *p_body, const btTransform &p_body_position, btScalar p_recover_movement_scale, bool p_infinite_inertia, int p_result_max, btVector3 &r_delta_recover_movement, PhysicsServer3D::SeparationResult *r_results) {
-	// Calculate the cumulative AABB of all shapes of the kinematic body
-	btVector3 aabb_min, aabb_max;
-	bool shapes_found = false;
-
-	for (int kinIndex = p_body->get_kinematic_utilities()->shapes.size() - 1; 0 <= kinIndex; --kinIndex) {
-		const RigidBodyBullet::KinematicShape &kin_shape(p_body->get_kinematic_utilities()->shapes[kinIndex]);
-		if (!kin_shape.is_active()) {
-			continue;
-		}
-
-		if (kin_shape.shape->getShapeType() != CUSTOM_CONVEX_SHAPE_TYPE) {
-			continue;
-		}
-
-		btTransform shape_transform = p_body_position * kin_shape.transform;
-		shape_transform.getOrigin() += r_delta_recover_movement;
-
-		btVector3 shape_aabb_min, shape_aabb_max;
-		kin_shape.shape->getAabb(shape_transform, shape_aabb_min, shape_aabb_max);
-
-		if (!shapes_found) {
-			aabb_min = shape_aabb_min;
-			aabb_max = shape_aabb_max;
-			shapes_found = true;
-		} else {
-			aabb_min.setX((aabb_min.x() < shape_aabb_min.x()) ? aabb_min.x() : shape_aabb_min.x());
-			aabb_min.setY((aabb_min.y() < shape_aabb_min.y()) ? aabb_min.y() : shape_aabb_min.y());
-			aabb_min.setZ((aabb_min.z() < shape_aabb_min.z()) ? aabb_min.z() : shape_aabb_min.z());
-
-			aabb_max.setX((aabb_max.x() > shape_aabb_max.x()) ? aabb_max.x() : shape_aabb_max.x());
-			aabb_max.setY((aabb_max.y() > shape_aabb_max.y()) ? aabb_max.y() : shape_aabb_max.y());
-			aabb_max.setZ((aabb_max.z() > shape_aabb_max.z()) ? aabb_max.z() : shape_aabb_max.z());
-		}
-	}
-
-	// If there are no shapes then there is no penetration either
-	if (!shapes_found) {
-		return 0;
-	}
-
-	// Perform broadphase test
-	RecoverPenetrationBroadPhaseCallback recover_broad_result(p_body->get_bt_collision_object(), p_body->get_collision_layer(), aabb_min, aabb_max);
-	dynamicsWorld->getBroadphase()->aabbTest(aabb_min, aabb_max, recover_broad_result);
-
-	int ray_count = 0;
-
-	// Perform narrowphase per shape
-	for (int kinIndex = p_body->get_kinematic_utilities()->shapes.size() - 1; 0 <= kinIndex; --kinIndex) {
-		if (ray_count >= p_result_max) {
-			break;
-		}
-
-		const RigidBodyBullet::KinematicShape &kin_shape(p_body->get_kinematic_utilities()->shapes[kinIndex]);
-		if (!kin_shape.is_active()) {
-			continue;
-		}
-
-		if (kin_shape.shape->getShapeType() != CUSTOM_CONVEX_SHAPE_TYPE) {
-			continue;
-		}
-
-		btTransform shape_transform = p_body_position * kin_shape.transform;
-		shape_transform.getOrigin() += r_delta_recover_movement;
-
-		for (int i = recover_broad_result.results.size() - 1; 0 <= i; --i) {
-			btCollisionObject *otherObject = recover_broad_result.results[i].collision_object;
-			if (p_infinite_inertia && !otherObject->isStaticOrKinematicObject()) {
-				otherObject->activate(); // Force activation of hitten rigid, soft body
-				continue;
-			} else if (!p_body->get_bt_collision_object()->checkCollideWith(otherObject) || !otherObject->checkCollideWith(p_body->get_bt_collision_object())) {
-				continue;
-			}
-
-			if (otherObject->getCollisionShape()->isCompound()) {
-				const btCompoundShape *cs = static_cast<const btCompoundShape *>(otherObject->getCollisionShape());
-				int shape_idx = recover_broad_result.results[i].compound_child_index;
-				ERR_FAIL_COND_V(shape_idx < 0 || shape_idx >= cs->getNumChildShapes(), false);
-
-				RecoverResult recover_result;
-				if (RFP_convex_world_test(kin_shape.shape, cs->getChildShape(shape_idx), p_body->get_bt_collision_object(), otherObject, kinIndex, shape_idx, shape_transform, otherObject->getWorldTransform() * cs->getChildTransform(shape_idx), p_recover_movement_scale, r_delta_recover_movement, &recover_result)) {
-					ray_count = add_separation_result(&r_results[ray_count], recover_result, kinIndex, otherObject);
-				}
-			} else {
-				RecoverResult recover_result;
-				if (RFP_convex_world_test(kin_shape.shape, otherObject->getCollisionShape(), p_body->get_bt_collision_object(), otherObject, kinIndex, 0, shape_transform, otherObject->getWorldTransform(), p_recover_movement_scale, r_delta_recover_movement, &recover_result)) {
-					ray_count = add_separation_result(&r_results[ray_count], recover_result, kinIndex, otherObject);
-				}
-			}
-		}
-	}
-
-	return ray_count;
-}

modules/bullet/space_bullet.h

@@ -1,220 +0,0 @@
-/*************************************************************************/
-/*  space_bullet.h                                                       */
-/*************************************************************************/
-/*                       This file is part of:                           */
-/*                           GODOT ENGINE                                */
-/*                      https://godotengine.org                          */
-/*************************************************************************/
-/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur.                 */
-/* Copyright (c) 2014-2022 Godot Engine contributors (cf. AUTHORS.md).   */
-/*                                                                       */
-/* Permission is hereby granted, free of charge, to any person obtaining */
-/* a copy of this software and associated documentation files (the       */
-/* "Software"), to deal in the Software without restriction, including   */
-/* without limitation the rights to use, copy, modify, merge, publish,   */
-/* distribute, sublicense, and/or sell copies of the Software, and to    */
-/* permit persons to whom the Software is furnished to do so, subject to */
-/* the following conditions:                                             */
-/*                                                                       */
-/* The above copyright notice and this permission notice shall be        */
-/* included in all copies or substantial portions of the Software.       */
-/*                                                                       */
-/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
-/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
-/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
-/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
-/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION 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 SPACE_BULLET_H
-#define SPACE_BULLET_H
-
-#include "core/templates/vector.h"
-#include "core/variant/variant.h"
-#include "godot_result_callbacks.h"
-#include "rid_bullet.h"
-#include "servers/physics_server_3d.h"
-
-#include <BulletCollision/BroadphaseCollision/btBroadphaseProxy.h>
-#include <BulletCollision/BroadphaseCollision/btOverlappingPairCache.h>
-#include <LinearMath/btScalar.h>
-#include <LinearMath/btTransform.h>
-#include <LinearMath/btVector3.h>
-
-class AreaBullet;
-class btBroadphaseInterface;
-class btCollisionDispatcher;
-class btConstraintSolver;
-class btDefaultCollisionConfiguration;
-class btDynamicsWorld;
-class btDiscreteDynamicsWorld;
-class btEmptyShape;
-class btGhostPairCallback;
-class btSoftRigidDynamicsWorld;
-struct btSoftBodyWorldInfo;
-class ConstraintBullet;
-class CollisionObjectBullet;
-class RigidBodyBullet;
-class SpaceBullet;
-class SoftBodyBullet;
-class btGjkEpaPenetrationDepthSolver;
-
-extern ContactAddedCallback gContactAddedCallback;
-
-class BulletPhysicsDirectSpaceState : public PhysicsDirectSpaceState3D {
-	GDCLASS(BulletPhysicsDirectSpaceState, PhysicsDirectSpaceState3D);
-
-private:
-	SpaceBullet *space;
-
-public:
-	BulletPhysicsDirectSpaceState(SpaceBullet *p_space);
-
-	virtual int intersect_point(const Vector3 &p_point, ShapeResult *r_results, int p_result_max, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_collision_mask = UINT32_MAX, bool p_collide_with_bodies = true, bool p_collide_with_areas = false) override;
-	virtual bool intersect_ray(const Vector3 &p_from, const Vector3 &p_to, RayResult &r_result, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_collision_mask = UINT32_MAX, bool p_collide_with_bodies = true, bool p_collide_with_areas = false, bool p_pick_ray = false) override;
-	virtual int intersect_shape(const RID &p_shape, const Transform3D &p_xform, real_t p_margin, ShapeResult *r_results, int p_result_max, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_collision_mask = UINT32_MAX, bool p_collide_with_bodies = true, bool p_collide_with_areas = false) override;
-	virtual bool cast_motion(const RID &p_shape, const Transform3D &p_xform, const Vector3 &p_motion, real_t p_margin, real_t &r_closest_safe, real_t &r_closest_unsafe, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_collision_mask = UINT32_MAX, bool p_collide_with_bodies = true, bool p_collide_with_areas = false, ShapeRestInfo *r_info = nullptr) override;
-	/// Returns the list of contacts pairs in this order: Local contact, other body contact
-	virtual bool collide_shape(RID p_shape, const Transform3D &p_shape_xform, real_t p_margin, Vector3 *r_results, int p_result_max, int &r_result_count, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_collision_mask = UINT32_MAX, bool p_collide_with_bodies = true, bool p_collide_with_areas = false) override;
-	virtual bool rest_info(RID p_shape, const Transform3D &p_shape_xform, real_t p_margin, ShapeRestInfo *r_info, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_collision_mask = UINT32_MAX, bool p_collide_with_bodies = true, bool p_collide_with_areas = false) override;
-	virtual Vector3 get_closest_point_to_object_volume(RID p_object, const Vector3 p_point) const override;
-};
-
-class SpaceBullet : public RIDBullet {
-	friend class AreaBullet;
-	friend void onBulletTickCallback(btDynamicsWorld *world, btScalar timeStep);
-	friend class BulletPhysicsDirectSpaceState;
-
-	btBroadphaseInterface *broadphase = nullptr;
-	btDefaultCollisionConfiguration *collisionConfiguration = nullptr;
-	btCollisionDispatcher *dispatcher = nullptr;
-	btConstraintSolver *solver = nullptr;
-	btDiscreteDynamicsWorld *dynamicsWorld = nullptr;
-	btSoftBodyWorldInfo *soft_body_world_info = nullptr;
-	btGhostPairCallback *ghostPairCallback = nullptr;
-	GodotFilterCallback *godotFilterCallback = nullptr;
-
-	btGjkEpaPenetrationDepthSolver *gjk_epa_pen_solver = nullptr;
-	btVoronoiSimplexSolver *gjk_simplex_solver = nullptr;
-
-	BulletPhysicsDirectSpaceState *direct_access;
-	Vector3 gravityDirection = Vector3(0, -1, 0);
-	real_t gravityMagnitude = 10.0;
-
-	real_t linear_damp = 0.0;
-	real_t angular_damp = 0.0;
-
-	Vector<AreaBullet *> areas;
-
-	Vector<Vector3> contactDebug;
-	int contactDebugCount = 0;
-	real_t delta_time = 0.;
-
-public:
-	SpaceBullet();
-	virtual ~SpaceBullet();
-
-	void flush_queries();
-	real_t get_delta_time() { return delta_time; }
-	void step(real_t p_delta_time);
-
-	_FORCE_INLINE_ btBroadphaseInterface *get_broadphase() const { return broadphase; }
-	_FORCE_INLINE_ btDefaultCollisionConfiguration *get_collision_configuration() const { return collisionConfiguration; }
-	_FORCE_INLINE_ btCollisionDispatcher *get_dispatcher() const { return dispatcher; }
-	_FORCE_INLINE_ btConstraintSolver *get_solver() const { return solver; }
-	_FORCE_INLINE_ btDiscreteDynamicsWorld *get_dynamic_world() const { return dynamicsWorld; }
-	_FORCE_INLINE_ btSoftBodyWorldInfo *get_soft_body_world_info() const { return soft_body_world_info; }
-	_FORCE_INLINE_ bool is_using_soft_world() { return soft_body_world_info; }
-
-	/// Used to set some parameters to Bullet world
-	/// @param p_param:
-	///     AREA_PARAM_GRAVITY          to set the gravity magnitude of entire world
-	///     AREA_PARAM_GRAVITY_VECTOR   to set the gravity direction of entire world
-	void set_param(PhysicsServer3D::AreaParameter p_param, const Variant &p_value);
-	/// Used to get some parameters to Bullet world
-	/// @param p_param:
-	///     AREA_PARAM_GRAVITY          to get the gravity magnitude of entire world
-	///     AREA_PARAM_GRAVITY_VECTOR   to get the gravity direction of entire world
-	Variant get_param(PhysicsServer3D::AreaParameter p_param);
-
-	void set_param(PhysicsServer3D::SpaceParameter p_param, real_t p_value);
-	real_t get_param(PhysicsServer3D::SpaceParameter p_param);
-
-	void add_area(AreaBullet *p_area);
-	void remove_area(AreaBullet *p_area);
-	void reload_collision_filters(AreaBullet *p_area);
-
-	void add_rigid_body(RigidBodyBullet *p_body);
-	void remove_rigid_body_constraints(RigidBodyBullet *p_body);
-	void remove_rigid_body(RigidBodyBullet *p_body);
-	void reload_collision_filters(RigidBodyBullet *p_body);
-
-	void add_soft_body(SoftBodyBullet *p_body);
-	void remove_soft_body(SoftBodyBullet *p_body);
-	void reload_collision_filters(SoftBodyBullet *p_body);
-
-	void add_constraint(ConstraintBullet *p_constraint, bool disableCollisionsBetweenLinkedBodies = false);
-	void remove_constraint(ConstraintBullet *p_constraint);
-
-	int get_num_collision_objects() const;
-	void remove_all_collision_objects();
-
-	BulletPhysicsDirectSpaceState *get_direct_state();
-
-	void set_debug_contacts(int p_amount) { contactDebug.resize(p_amount); }
-	_FORCE_INLINE_ bool is_debugging_contacts() const { return !contactDebug.is_empty(); }
-	_FORCE_INLINE_ void reset_debug_contact_count() {
-		contactDebugCount = 0;
-	}
-	_FORCE_INLINE_ void add_debug_contact(const Vector3 &p_contact) {
-		if (contactDebugCount < contactDebug.size()) {
-			contactDebug.write[contactDebugCount++] = p_contact;
-		}
-	}
-	_FORCE_INLINE_ Vector<Vector3> get_debug_contacts() { return contactDebug; }
-	_FORCE_INLINE_ int get_debug_contact_count() { return contactDebugCount; }
-
-	const Vector3 &get_gravity_direction() const { return gravityDirection; }
-	real_t get_gravity_magnitude() const { return gravityMagnitude; }
-
-	void update_gravity();
-
-	real_t get_linear_damp() const { return linear_damp; }
-	real_t get_angular_damp() const { return angular_damp; }
-
-	bool test_body_motion(RigidBodyBullet *p_body, const Transform3D &p_from, const Vector3 &p_motion, bool p_infinite_inertia, PhysicsServer3D::MotionResult *r_result, bool p_exclude_raycast_shapes, const Set<RID> &p_exclude = Set<RID>());
-	int test_ray_separation(RigidBodyBullet *p_body, const Transform3D &p_transform, bool p_infinite_inertia, Vector3 &r_recover_motion, PhysicsServer3D::SeparationResult *r_results, int p_result_max, real_t p_margin);
-
-private:
-	void create_empty_world(bool p_create_soft_world);
-	void destroy_world();
-	void check_ghost_overlaps();
-	void check_body_collision();
-
-	struct RecoverResult {
-		bool hasPenetration = false;
-		btVector3 normal = btVector3(0, 0, 0);
-		btVector3 pointWorld = btVector3(0, 0, 0);
-		btScalar penetration_distance = 1e20; // Negative mean penetration
-		int other_compound_shape_index = 0;
-		const btCollisionObject *other_collision_object = nullptr;
-		int local_shape_most_recovered = 0;
-
-		RecoverResult() {}
-	};
-
-	bool recover_from_penetration(RigidBodyBullet *p_body, const btTransform &p_body_position, btScalar p_recover_movement_scale, bool p_infinite_inertia, btVector3 &r_delta_recover_movement, RecoverResult *r_recover_result = nullptr, const Set<RID> &p_exclude = Set<RID>());
-	/// This is an API that recover a kinematic object from penetration
-	/// This allow only Convex Convex test and it always use GJK algorithm, With this API we don't benefit of Bullet special accelerated functions
-	bool RFP_convex_convex_test(const btConvexShape *p_shapeA, const btConvexShape *p_shapeB, btCollisionObject *p_objectB, int p_shapeId_A, int p_shapeId_B, const btTransform &p_transformA, const btTransform &p_transformB, btScalar p_recover_movement_scale, btVector3 &r_delta_recover_movement, RecoverResult *r_recover_result = nullptr);
-	/// This is an API that recover a kinematic object from penetration
-	/// Using this we leave Bullet to select the best algorithm, For example GJK in case we have Convex Convex, or a Bullet accelerated algorithm
-	bool RFP_convex_world_test(const btConvexShape *p_shapeA, const btCollisionShape *p_shapeB, btCollisionObject *p_objectA, btCollisionObject *p_objectB, int p_shapeId_A, int p_shapeId_B, const btTransform &p_transformA, const btTransform &p_transformB, btScalar p_recover_movement_scale, btVector3 &r_delta_recover_movement, RecoverResult *r_recover_result = nullptr);
-
-	int add_separation_result(PhysicsServer3D::SeparationResult *r_results, const SpaceBullet::RecoverResult &p_recover_result, int p_shape_id, const btCollisionObject *p_other_object) const;
-	int recover_from_penetration_ray(RigidBodyBullet *p_body, const btTransform &p_body_position, btScalar p_recover_movement_scale, bool p_infinite_inertia, int p_result_max, btVector3 &r_delta_recover_movement, PhysicsServer3D::SeparationResult *r_results);
-};
-
-#endif // SPACE_BULLET_H

platform/linuxbsd/detect.py

@@ -261,27 +261,6 @@ def configure(env):
     if not env["builtin_libpng"]:
         env.ParseConfig("pkg-config libpng16 --cflags --libs")
 
-    if not env["builtin_bullet"]:
-        # We need at least version 2.90
-        min_bullet_version = "2.90"
-
-        import subprocess
-
-        bullet_version = subprocess.check_output(["pkg-config", "bullet", "--modversion"]).strip()
-        if str(bullet_version) < min_bullet_version:
-            # Abort as system bullet was requested but too old
-            print(
-                "Bullet: System version {0} does not match minimal requirements ({1}). Aborting.".format(
-                    bullet_version, min_bullet_version
-                )
-            )
-            sys.exit(255)
-        env.ParseConfig("pkg-config bullet --cflags --libs")
-
-    if False:  # not env['builtin_assimp']:
-        # FIXME: Add min version check
-        env.ParseConfig("pkg-config assimp --cflags --libs")
-
     if not env["builtin_enet"]:
         env.ParseConfig("pkg-config libenet --cflags --libs")
 

scene/register_scene_types.cpp

@@ -928,8 +928,6 @@ void register_scene_types() {
 	ClassDB::add_compatibility_class("ARVRServer", "XRServer");
 	ClassDB::add_compatibility_class("BoneAttachment", "BoneAttachment3D");
 	ClassDB::add_compatibility_class("BoxShape", "BoxShape3D");
-	ClassDB::add_compatibility_class("BulletPhysicsDirectBodyState", "BulletPhysicsDirectBodyState3D");
-	ClassDB::add_compatibility_class("BulletPhysicsServer", "BulletPhysicsServer3D");
 	ClassDB::add_compatibility_class("Camera", "Camera3D");
 	ClassDB::add_compatibility_class("CapsuleShape", "CapsuleShape3D");
 	ClassDB::add_compatibility_class("ClippedCamera", "ClippedCamera3D");

thirdparty/README.md

@@ -29,20 +29,6 @@ Files extracted from upstream source:
 - `LICENSE`
 
 
-## bullet
-
-- Upstream: https://github.com/bulletphysics/bullet3
-- Version: 3.17 (ebe1916b90acae8b13cd8c6b637d8327cdc64e94, 2021)
-- License: zlib
-
-Files extracted from upstream source:
-
-- `src/*` apart from CMakeLists.txt and premake4.lua files
-- `LICENSE.txt`, and `VERSION` as `VERSION.txt`
-
-Includes some patches in the `patches` folder which have been sent upstream.
-
-
 ## certs
 
 - Upstream: Mozilla, via https://github.com/bagder/ca-bundle

thirdparty/bullet/Bullet3Collision/BroadPhaseCollision/b3BroadphaseCallback.h

@@ -1,38 +0,0 @@
-/*
-Bullet Continuous Collision Detection and Physics Library
-Copyright (c) 2003-2013 Erwin Coumans  http://bulletphysics.org
-
-This software is provided 'as-is', without any express or implied warranty.
-In no event will the authors be held liable for any damages arising from the use of this software.
-Permission is granted to anyone to use this software for any purpose,
-including commercial applications, and to alter it and redistribute it freely,
-subject to the following restrictions:
-
-1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
-2. Altered source versions must be plainly marked as 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 B3_BROADPHASE_CALLBACK_H
-#define B3_BROADPHASE_CALLBACK_H
-
-#include "Bullet3Common/b3Vector3.h"
-struct b3BroadphaseProxy;
-
-struct b3BroadphaseAabbCallback
-{
-	virtual ~b3BroadphaseAabbCallback() {}
-	virtual bool process(const b3BroadphaseProxy* proxy) = 0;
-};
-
-struct b3BroadphaseRayCallback : public b3BroadphaseAabbCallback
-{
-	///added some cached data to accelerate ray-AABB tests
-	b3Vector3 m_rayDirectionInverse;
-	unsigned int m_signs[3];
-	b3Scalar m_lambda_max;
-
-	virtual ~b3BroadphaseRayCallback() {}
-};
-
-#endif  //B3_BROADPHASE_CALLBACK_H

thirdparty/bullet/Bullet3Collision/BroadPhaseCollision/b3DynamicBvh.cpp

@@ -1,1352 +0,0 @@
-/*
-Bullet Continuous Collision Detection and Physics Library
-Copyright (c) 2003-2013 Erwin Coumans  http://bulletphysics.org
-
-This software is provided 'as-is', without any express or implied warranty.
-In no event will the authors be held liable for any damages arising from the use of this software.
-Permission is granted to anyone to use this software for any purpose,
-including commercial applications, and to alter it and redistribute it freely,
-subject to the following restrictions:
-
-1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
-2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
-3. This notice may not be removed or altered from any source distribution.
-*/
-///b3DynamicBvh implementation by Nathanael Presson
-
-#include "b3DynamicBvh.h"
-
-//
-typedef b3AlignedObjectArray<b3DbvtNode*> b3NodeArray;
-typedef b3AlignedObjectArray<const b3DbvtNode*> b3ConstNodeArray;
-
-//
-struct b3DbvtNodeEnumerator : b3DynamicBvh::ICollide
-{
-	b3ConstNodeArray nodes;
-	void Process(const b3DbvtNode* n) { nodes.push_back(n); }
-};
-
-//
-static B3_DBVT_INLINE int b3IndexOf(const b3DbvtNode* node)
-{
-	return (node->parent->childs[1] == node);
-}
-
-//
-static B3_DBVT_INLINE b3DbvtVolume b3Merge(const b3DbvtVolume& a,
-										   const b3DbvtVolume& b)
-{
-#if (B3_DBVT_MERGE_IMPL == B3_DBVT_IMPL_SSE)
-	B3_ATTRIBUTE_ALIGNED16(char locals[sizeof(b3DbvtAabbMm)]);
-	b3DbvtVolume& res = *(b3DbvtVolume*)locals;
-#else
-	b3DbvtVolume res;
-#endif
-	b3Merge(a, b, res);
-	return (res);
-}
-
-// volume+edge lengths
-static B3_DBVT_INLINE b3Scalar b3Size(const b3DbvtVolume& a)
-{
-	const b3Vector3 edges = a.Lengths();
-	return (edges.x * edges.y * edges.z +
-			edges.x + edges.y + edges.z);
-}
-
-//
-static void b3GetMaxDepth(const b3DbvtNode* node, int depth, int& maxdepth)
-{
-	if (node->isinternal())
-	{
-		b3GetMaxDepth(node->childs[0], depth + 1, maxdepth);
-		b3GetMaxDepth(node->childs[1], depth + 1, maxdepth);
-	}
-	else
-		maxdepth = b3Max(maxdepth, depth);
-}
-
-//
-static B3_DBVT_INLINE void b3DeleteNode(b3DynamicBvh* pdbvt,
-										b3DbvtNode* node)
-{
-	b3AlignedFree(pdbvt->m_free);
-	pdbvt->m_free = node;
-}
-
-//
-static void b3RecurseDeleteNode(b3DynamicBvh* pdbvt,
-								b3DbvtNode* node)
-{
-	if (!node->isleaf())
-	{
-		b3RecurseDeleteNode(pdbvt, node->childs[0]);
-		b3RecurseDeleteNode(pdbvt, node->childs[1]);
-	}
-	if (node == pdbvt->m_root) pdbvt->m_root = 0;
-	b3DeleteNode(pdbvt, node);
-}
-
-//
-static B3_DBVT_INLINE b3DbvtNode* b3CreateNode(b3DynamicBvh* pdbvt,
-											   b3DbvtNode* parent,
-											   void* data)
-{
-	b3DbvtNode* node;
-	if (pdbvt->m_free)
-	{
-		node = pdbvt->m_free;
-		pdbvt->m_free = 0;
-	}
-	else
-	{
-		node = new (b3AlignedAlloc(sizeof(b3DbvtNode), 16)) b3DbvtNode();
-	}
-	node->parent = parent;
-	node->data = data;
-	node->childs[1] = 0;
-	return (node);
-}
-
-//
-static B3_DBVT_INLINE b3DbvtNode* b3CreateNode(b3DynamicBvh* pdbvt,
-											   b3DbvtNode* parent,
-											   const b3DbvtVolume& volume,
-											   void* data)
-{
-	b3DbvtNode* node = b3CreateNode(pdbvt, parent, data);
-	node->volume = volume;
-	return (node);
-}
-
-//
-static B3_DBVT_INLINE b3DbvtNode* b3CreateNode(b3DynamicBvh* pdbvt,
-											   b3DbvtNode* parent,
-											   const b3DbvtVolume& volume0,
-											   const b3DbvtVolume& volume1,
-											   void* data)
-{
-	b3DbvtNode* node = b3CreateNode(pdbvt, parent, data);
-	b3Merge(volume0, volume1, node->volume);
-	return (node);
-}
-
-//
-static void b3InsertLeaf(b3DynamicBvh* pdbvt,
-						 b3DbvtNode* root,
-						 b3DbvtNode* leaf)
-{
-	if (!pdbvt->m_root)
-	{
-		pdbvt->m_root = leaf;
-		leaf->parent = 0;
-	}
-	else
-	{
-		if (!root->isleaf())
-		{
-			do
-			{
-				root = root->childs[b3Select(leaf->volume,
-											 root->childs[0]->volume,
-											 root->childs[1]->volume)];
-			} while (!root->isleaf());
-		}
-		b3DbvtNode* prev = root->parent;
-		b3DbvtNode* node = b3CreateNode(pdbvt, prev, leaf->volume, root->volume, 0);
-		if (prev)
-		{
-			prev->childs[b3IndexOf(root)] = node;
-			node->childs[0] = root;
-			root->parent = node;
-			node->childs[1] = leaf;
-			leaf->parent = node;
-			do
-			{
-				if (!prev->volume.Contain(node->volume))
-					b3Merge(prev->childs[0]->volume, prev->childs[1]->volume, prev->volume);
-				else
-					break;
-				node = prev;
-			} while (0 != (prev = node->parent));
-		}
-		else
-		{
-			node->childs[0] = root;
-			root->parent = node;
-			node->childs[1] = leaf;
-			leaf->parent = node;
-			pdbvt->m_root = node;
-		}
-	}
-}
-
-//
-static b3DbvtNode* b3RemoveLeaf(b3DynamicBvh* pdbvt,
-								b3DbvtNode* leaf)
-{
-	if (leaf == pdbvt->m_root)
-	{
-		pdbvt->m_root = 0;
-		return (0);
-	}
-	else
-	{
-		b3DbvtNode* parent = leaf->parent;
-		b3DbvtNode* prev = parent->parent;
-		b3DbvtNode* sibling = parent->childs[1 - b3IndexOf(leaf)];
-		if (prev)
-		{
-			prev->childs[b3IndexOf(parent)] = sibling;
-			sibling->parent = prev;
-			b3DeleteNode(pdbvt, parent);
-			while (prev)
-			{
-				const b3DbvtVolume pb = prev->volume;
-				b3Merge(prev->childs[0]->volume, prev->childs[1]->volume, prev->volume);
-				if (b3NotEqual(pb, prev->volume))
-				{
-					prev = prev->parent;
-				}
-				else
-					break;
-			}
-			return (prev ? prev : pdbvt->m_root);
-		}
-		else
-		{
-			pdbvt->m_root = sibling;
-			sibling->parent = 0;
-			b3DeleteNode(pdbvt, parent);
-			return (pdbvt->m_root);
-		}
-	}
-}
-
-//
-static void b3FetchLeaves(b3DynamicBvh* pdbvt,
-						  b3DbvtNode* root,
-						  b3NodeArray& leaves,
-						  int depth = -1)
-{
-	if (root->isinternal() && depth)
-	{
-		b3FetchLeaves(pdbvt, root->childs[0], leaves, depth - 1);
-		b3FetchLeaves(pdbvt, root->childs[1], leaves, depth - 1);
-		b3DeleteNode(pdbvt, root);
-	}
-	else
-	{
-		leaves.push_back(root);
-	}
-}
-
-static bool b3LeftOfAxis(const b3DbvtNode* node,
-						 const b3Vector3& org,
-						 const b3Vector3& axis)
-{
-	return b3Dot(axis, node->volume.Center() - org) <= 0;
-}
-
-// Partitions leaves such that leaves[0, n) are on the
-// left of axis, and leaves[n, count) are on the right
-// of axis. returns N.
-static int b3Split(b3DbvtNode** leaves,
-				   int count,
-				   const b3Vector3& org,
-				   const b3Vector3& axis)
-{
-	int begin = 0;
-	int end = count;
-	for (;;)
-	{
-		while (begin != end && b3LeftOfAxis(leaves[begin], org, axis))
-		{
-			++begin;
-		}
-
-		if (begin == end)
-		{
-			break;
-		}
-
-		while (begin != end && !b3LeftOfAxis(leaves[end - 1], org, axis))
-		{
-			--end;
-		}
-
-		if (begin == end)
-		{
-			break;
-		}
-
-		// swap out of place nodes
-		--end;
-		b3DbvtNode* temp = leaves[begin];
-		leaves[begin] = leaves[end];
-		leaves[end] = temp;
-		++begin;
-	}
-
-	return begin;
-}
-
-//
-static b3DbvtVolume b3Bounds(b3DbvtNode** leaves,
-							 int count)
-{
-#if B3_DBVT_MERGE_IMPL == B3_DBVT_IMPL_SSE
-	B3_ATTRIBUTE_ALIGNED16(char locals[sizeof(b3DbvtVolume)]);
-	b3DbvtVolume& volume = *(b3DbvtVolume*)locals;
-	volume = leaves[0]->volume;
-#else
-	b3DbvtVolume volume = leaves[0]->volume;
-#endif
-	for (int i = 1, ni = count; i < ni; ++i)
-	{
-		b3Merge(volume, leaves[i]->volume, volume);
-	}
-	return (volume);
-}
-
-//
-static void b3BottomUp(b3DynamicBvh* pdbvt,
-					   b3DbvtNode** leaves,
-					   int count)
-{
-	while (count > 1)
-	{
-		b3Scalar minsize = B3_INFINITY;
-		int minidx[2] = {-1, -1};
-		for (int i = 0; i < count; ++i)
-		{
-			for (int j = i + 1; j < count; ++j)
-			{
-				const b3Scalar sz = b3Size(b3Merge(leaves[i]->volume, leaves[j]->volume));
-				if (sz < minsize)
-				{
-					minsize = sz;
-					minidx[0] = i;
-					minidx[1] = j;
-				}
-			}
-		}
-		b3DbvtNode* n[] = {leaves[minidx[0]], leaves[minidx[1]]};
-		b3DbvtNode* p = b3CreateNode(pdbvt, 0, n[0]->volume, n[1]->volume, 0);
-		p->childs[0] = n[0];
-		p->childs[1] = n[1];
-		n[0]->parent = p;
-		n[1]->parent = p;
-		leaves[minidx[0]] = p;
-		leaves[minidx[1]] = leaves[count - 1];
-		--count;
-	}
-}
-
-//
-static b3DbvtNode* b3TopDown(b3DynamicBvh* pdbvt,
-							 b3DbvtNode** leaves,
-							 int count,
-							 int bu_treshold)
-{
-	static const b3Vector3 axis[] = {b3MakeVector3(1, 0, 0),
-									 b3MakeVector3(0, 1, 0),
-									 b3MakeVector3(0, 0, 1)};
-	b3Assert(bu_treshold > 1);
-	if (count > 1)
-	{
-		if (count > bu_treshold)
-		{
-			const b3DbvtVolume vol = b3Bounds(leaves, count);
-			const b3Vector3 org = vol.Center();
-			int partition;
-			int bestaxis = -1;
-			int bestmidp = count;
-			int splitcount[3][2] = {{0, 0}, {0, 0}, {0, 0}};
-			int i;
-			for (i = 0; i < count; ++i)
-			{
-				const b3Vector3 x = leaves[i]->volume.Center() - org;
-				for (int j = 0; j < 3; ++j)
-				{
-					++splitcount[j][b3Dot(x, axis[j]) > 0 ? 1 : 0];
-				}
-			}
-			for (i = 0; i < 3; ++i)
-			{
-				if ((splitcount[i][0] > 0) && (splitcount[i][1] > 0))
-				{
-					const int midp = (int)b3Fabs(b3Scalar(splitcount[i][0] - splitcount[i][1]));
-					if (midp < bestmidp)
-					{
-						bestaxis = i;
-						bestmidp = midp;
-					}
-				}
-			}
-			if (bestaxis >= 0)
-			{
-				partition = b3Split(leaves, count, org, axis[bestaxis]);
-				b3Assert(partition != 0 && partition != count);
-			}
-			else
-			{
-				partition = count / 2 + 1;
-			}
-			b3DbvtNode* node = b3CreateNode(pdbvt, 0, vol, 0);
-			node->childs[0] = b3TopDown(pdbvt, &leaves[0], partition, bu_treshold);
-			node->childs[1] = b3TopDown(pdbvt, &leaves[partition], count - partition, bu_treshold);
-			node->childs[0]->parent = node;
-			node->childs[1]->parent = node;
-			return (node);
-		}
-		else
-		{
-			b3BottomUp(pdbvt, leaves, count);
-			return (leaves[0]);
-		}
-	}
-	return (leaves[0]);
-}
-
-//
-static B3_DBVT_INLINE b3DbvtNode* b3Sort(b3DbvtNode* n, b3DbvtNode*& r)
-{
-	b3DbvtNode* p = n->parent;
-	b3Assert(n->isinternal());
-	if (p > n)
-	{
-		const int i = b3IndexOf(n);
-		const int j = 1 - i;
-		b3DbvtNode* s = p->childs[j];
-		b3DbvtNode* q = p->parent;
-		b3Assert(n == p->childs[i]);
-		if (q)
-			q->childs[b3IndexOf(p)] = n;
-		else
-			r = n;
-		s->parent = n;
-		p->parent = n;
-		n->parent = q;
-		p->childs[0] = n->childs[0];
-		p->childs[1] = n->childs[1];
-		n->childs[0]->parent = p;
-		n->childs[1]->parent = p;
-		n->childs[i] = p;
-		n->childs[j] = s;
-		b3Swap(p->volume, n->volume);
-		return (p);
-	}
-	return (n);
-}
-
-#if 0
-static B3_DBVT_INLINE b3DbvtNode*	walkup(b3DbvtNode* n,int count)
-{
-	while(n&&(count--)) n=n->parent;
-	return(n);
-}
-#endif
-
-//
-// Api
-//
-
-//
-b3DynamicBvh::b3DynamicBvh()
-{
-	m_root = 0;
-	m_free = 0;
-	m_lkhd = -1;
-	m_leaves = 0;
-	m_opath = 0;
-}
-
-//
-b3DynamicBvh::~b3DynamicBvh()
-{
-	clear();
-}
-
-//
-void b3DynamicBvh::clear()
-{
-	if (m_root)
-		b3RecurseDeleteNode(this, m_root);
-	b3AlignedFree(m_free);
-	m_free = 0;
-	m_lkhd = -1;
-	m_stkStack.clear();
-	m_opath = 0;
-}
-
-//
-void b3DynamicBvh::optimizeBottomUp()
-{
-	if (m_root)
-	{
-		b3NodeArray leaves;
-		leaves.reserve(m_leaves);
-		b3FetchLeaves(this, m_root, leaves);
-		b3BottomUp(this, &leaves[0], leaves.size());
-		m_root = leaves[0];
-	}
-}
-
-//
-void b3DynamicBvh::optimizeTopDown(int bu_treshold)
-{
-	if (m_root)
-	{
-		b3NodeArray leaves;
-		leaves.reserve(m_leaves);
-		b3FetchLeaves(this, m_root, leaves);
-		m_root = b3TopDown(this, &leaves[0], leaves.size(), bu_treshold);
-	}
-}
-
-//
-void b3DynamicBvh::optimizeIncremental(int passes)
-{
-	if (passes < 0) passes = m_leaves;
-	if (m_root && (passes > 0))
-	{
-		do
-		{
-			b3DbvtNode* node = m_root;
-			unsigned bit = 0;
-			while (node->isinternal())
-			{
-				node = b3Sort(node, m_root)->childs[(m_opath >> bit) & 1];
-				bit = (bit + 1) & (sizeof(unsigned) * 8 - 1);
-			}
-			update(node);
-			++m_opath;
-		} while (--passes);
-	}
-}
-
-//
-b3DbvtNode* b3DynamicBvh::insert(const b3DbvtVolume& volume, void* data)
-{
-	b3DbvtNode* leaf = b3CreateNode(this, 0, volume, data);
-	b3InsertLeaf(this, m_root, leaf);
-	++m_leaves;
-	return (leaf);
-}
-
-//
-void b3DynamicBvh::update(b3DbvtNode* leaf, int lookahead)
-{
-	b3DbvtNode* root = b3RemoveLeaf(this, leaf);
-	if (root)
-	{
-		if (lookahead >= 0)
-		{
-			for (int i = 0; (i < lookahead) && root->parent; ++i)
-			{
-				root = root->parent;
-			}
-		}
-		else
-			root = m_root;
-	}
-	b3InsertLeaf(this, root, leaf);
-}
-
-//
-void b3DynamicBvh::update(b3DbvtNode* leaf, b3DbvtVolume& volume)
-{
-	b3DbvtNode* root = b3RemoveLeaf(this, leaf);
-	if (root)
-	{
-		if (m_lkhd >= 0)
-		{
-			for (int i = 0; (i < m_lkhd) && root->parent; ++i)
-			{
-				root = root->parent;
-			}
-		}
-		else
-			root = m_root;
-	}
-	leaf->volume = volume;
-	b3InsertLeaf(this, root, leaf);
-}
-
-//
-bool b3DynamicBvh::update(b3DbvtNode* leaf, b3DbvtVolume& volume, const b3Vector3& velocity, b3Scalar margin)
-{
-	if (leaf->volume.Contain(volume)) return (false);
-	volume.Expand(b3MakeVector3(margin, margin, margin));
-	volume.SignedExpand(velocity);
-	update(leaf, volume);
-	return (true);
-}
-
-//
-bool b3DynamicBvh::update(b3DbvtNode* leaf, b3DbvtVolume& volume, const b3Vector3& velocity)
-{
-	if (leaf->volume.Contain(volume)) return (false);
-	volume.SignedExpand(velocity);
-	update(leaf, volume);
-	return (true);
-}
-
-//
-bool b3DynamicBvh::update(b3DbvtNode* leaf, b3DbvtVolume& volume, b3Scalar margin)
-{
-	if (leaf->volume.Contain(volume)) return (false);
-	volume.Expand(b3MakeVector3(margin, margin, margin));
-	update(leaf, volume);
-	return (true);
-}
-
-//
-void b3DynamicBvh::remove(b3DbvtNode* leaf)
-{
-	b3RemoveLeaf(this, leaf);
-	b3DeleteNode(this, leaf);
-	--m_leaves;
-}
-
-//
-void b3DynamicBvh::write(IWriter* iwriter) const
-{
-	b3DbvtNodeEnumerator nodes;
-	nodes.nodes.reserve(m_leaves * 2);
-	enumNodes(m_root, nodes);
-	iwriter->Prepare(m_root, nodes.nodes.size());
-	for (int i = 0; i < nodes.nodes.size(); ++i)
-	{
-		const b3DbvtNode* n = nodes.nodes[i];
-		int p = -1;
-		if (n->parent) p = nodes.nodes.findLinearSearch(n->parent);
-		if (n->isinternal())
-		{
-			const int c0 = nodes.nodes.findLinearSearch(n->childs[0]);
-			const int c1 = nodes.nodes.findLinearSearch(n->childs[1]);
-			iwriter->WriteNode(n, i, p, c0, c1);
-		}
-		else
-		{
-			iwriter->WriteLeaf(n, i, p);
-		}
-	}
-}
-
-//
-void b3DynamicBvh::clone(b3DynamicBvh& dest, IClone* iclone) const
-{
-	dest.clear();
-	if (m_root != 0)
-	{
-		b3AlignedObjectArray<sStkCLN> stack;
-		stack.reserve(m_leaves);
-		stack.push_back(sStkCLN(m_root, 0));
-		do
-		{
-			const int i = stack.size() - 1;
-			const sStkCLN e = stack[i];
-			b3DbvtNode* n = b3CreateNode(&dest, e.parent, e.node->volume, e.node->data);
-			stack.pop_back();
-			if (e.parent != 0)
-				e.parent->childs[i & 1] = n;
-			else
-				dest.m_root = n;
-			if (e.node->isinternal())
-			{
-				stack.push_back(sStkCLN(e.node->childs[0], n));
-				stack.push_back(sStkCLN(e.node->childs[1], n));
-			}
-			else
-			{
-				iclone->CloneLeaf(n);
-			}
-		} while (stack.size() > 0);
-	}
-}
-
-//
-int b3DynamicBvh::maxdepth(const b3DbvtNode* node)
-{
-	int depth = 0;
-	if (node) b3GetMaxDepth(node, 1, depth);
-	return (depth);
-}
-
-//
-int b3DynamicBvh::countLeaves(const b3DbvtNode* node)
-{
-	if (node->isinternal())
-		return (countLeaves(node->childs[0]) + countLeaves(node->childs[1]));
-	else
-		return (1);
-}
-
-//
-void b3DynamicBvh::extractLeaves(const b3DbvtNode* node, b3AlignedObjectArray<const b3DbvtNode*>& leaves)
-{
-	if (node->isinternal())
-	{
-		extractLeaves(node->childs[0], leaves);
-		extractLeaves(node->childs[1], leaves);
-	}
-	else
-	{
-		leaves.push_back(node);
-	}
-}
-
-//
-#if B3_DBVT_ENABLE_BENCHMARK
-
-#include <stdio.h>
-#include <stdlib.h>
-
-/*
-q6600,2.4ghz
-
-/Ox /Ob2 /Oi /Ot /I "." /I "..\.." /I "..\..\src" /D "NDEBUG" /D "_LIB" /D "_WINDOWS" /D "_CRT_SECURE_NO_DEPRECATE" /D "_CRT_NONSTDC_NO_DEPRECATE" /D "WIN32"
-/GF /FD /MT /GS- /Gy /arch:SSE2 /Zc:wchar_t- /Fp"..\..\out\release8\build\libbulletcollision\libbulletcollision.pch"
-/Fo"..\..\out\release8\build\libbulletcollision\\"
-/Fd"..\..\out\release8\build\libbulletcollision\bulletcollision.pdb"
-/W3 /nologo /c /Wp64 /Zi /errorReport:prompt
-
-Benchmarking dbvt...
-World scale: 100.000000
-Extents base: 1.000000
-Extents range: 4.000000
-Leaves: 8192
-sizeof(b3DbvtVolume): 32 bytes
-sizeof(b3DbvtNode):   44 bytes
-[1] b3DbvtVolume intersections: 3499 ms (-1%)
-[2] b3DbvtVolume merges: 1934 ms (0%)
-[3] b3DynamicBvh::collideTT: 5485 ms (-21%)
-[4] b3DynamicBvh::collideTT self: 2814 ms (-20%)
-[5] b3DynamicBvh::collideTT xform: 7379 ms (-1%)
-[6] b3DynamicBvh::collideTT xform,self: 7270 ms (-2%)
-[7] b3DynamicBvh::rayTest: 6314 ms (0%),(332143 r/s)
-[8] insert/remove: 2093 ms (0%),(1001983 ir/s)
-[9] updates (teleport): 1879 ms (-3%),(1116100 u/s)
-[10] updates (jitter): 1244 ms (-4%),(1685813 u/s)
-[11] optimize (incremental): 2514 ms (0%),(1668000 o/s)
-[12] b3DbvtVolume notequal: 3659 ms (0%)
-[13] culling(OCL+fullsort): 2218 ms (0%),(461 t/s)
-[14] culling(OCL+qsort): 3688 ms (5%),(2221 t/s)
-[15] culling(KDOP+qsort): 1139 ms (-1%),(7192 t/s)
-[16] insert/remove batch(256): 5092 ms (0%),(823704 bir/s)
-[17] b3DbvtVolume select: 3419 ms (0%)
-*/
-
-struct b3DbvtBenchmark
-{
-	struct NilPolicy : b3DynamicBvh::ICollide
-	{
-		NilPolicy() : m_pcount(0), m_depth(-B3_INFINITY), m_checksort(true) {}
-		void Process(const b3DbvtNode*, const b3DbvtNode*) { ++m_pcount; }
-		void Process(const b3DbvtNode*) { ++m_pcount; }
-		void Process(const b3DbvtNode*, b3Scalar depth)
-		{
-			++m_pcount;
-			if (m_checksort)
-			{
-				if (depth >= m_depth)
-					m_depth = depth;
-				else
-					printf("wrong depth: %f (should be >= %f)\r\n", depth, m_depth);
-			}
-		}
-		int m_pcount;
-		b3Scalar m_depth;
-		bool m_checksort;
-	};
-	struct P14 : b3DynamicBvh::ICollide
-	{
-		struct Node
-		{
-			const b3DbvtNode* leaf;
-			b3Scalar depth;
-		};
-		void Process(const b3DbvtNode* leaf, b3Scalar depth)
-		{
-			Node n;
-			n.leaf = leaf;
-			n.depth = depth;
-		}
-		static int sortfnc(const Node& a, const Node& b)
-		{
-			if (a.depth < b.depth) return (+1);
-			if (a.depth > b.depth) return (-1);
-			return (0);
-		}
-		b3AlignedObjectArray<Node> m_nodes;
-	};
-	struct P15 : b3DynamicBvh::ICollide
-	{
-		struct Node
-		{
-			const b3DbvtNode* leaf;
-			b3Scalar depth;
-		};
-		void Process(const b3DbvtNode* leaf)
-		{
-			Node n;
-			n.leaf = leaf;
-			n.depth = dot(leaf->volume.Center(), m_axis);
-		}
-		static int sortfnc(const Node& a, const Node& b)
-		{
-			if (a.depth < b.depth) return (+1);
-			if (a.depth > b.depth) return (-1);
-			return (0);
-		}
-		b3AlignedObjectArray<Node> m_nodes;
-		b3Vector3 m_axis;
-	};
-	static b3Scalar RandUnit()
-	{
-		return (rand() / (b3Scalar)RAND_MAX);
-	}
-	static b3Vector3 RandVector3()
-	{
-		return (b3Vector3(RandUnit(), RandUnit(), RandUnit()));
-	}
-	static b3Vector3 RandVector3(b3Scalar cs)
-	{
-		return (RandVector3() * cs - b3Vector3(cs, cs, cs) / 2);
-	}
-	static b3DbvtVolume RandVolume(b3Scalar cs, b3Scalar eb, b3Scalar es)
-	{
-		return (b3DbvtVolume::FromCE(RandVector3(cs), b3Vector3(eb, eb, eb) + RandVector3() * es));
-	}
-	static b3Transform RandTransform(b3Scalar cs)
-	{
-		b3Transform t;
-		t.setOrigin(RandVector3(cs));
-		t.setRotation(b3Quaternion(RandUnit() * B3_PI * 2, RandUnit() * B3_PI * 2, RandUnit() * B3_PI * 2).normalized());
-		return (t);
-	}
-	static void RandTree(b3Scalar cs, b3Scalar eb, b3Scalar es, int leaves, b3DynamicBvh& dbvt)
-	{
-		dbvt.clear();
-		for (int i = 0; i < leaves; ++i)
-		{
-			dbvt.insert(RandVolume(cs, eb, es), 0);
-		}
-	}
-};
-
-void b3DynamicBvh::benchmark()
-{
-	static const b3Scalar cfgVolumeCenterScale = 100;
-	static const b3Scalar cfgVolumeExentsBase = 1;
-	static const b3Scalar cfgVolumeExentsScale = 4;
-	static const int cfgLeaves = 8192;
-	static const bool cfgEnable = true;
-
-	//[1] b3DbvtVolume intersections
-	bool cfgBenchmark1_Enable = cfgEnable;
-	static const int cfgBenchmark1_Iterations = 8;
-	static const int cfgBenchmark1_Reference = 3499;
-	//[2] b3DbvtVolume merges
-	bool cfgBenchmark2_Enable = cfgEnable;
-	static const int cfgBenchmark2_Iterations = 4;
-	static const int cfgBenchmark2_Reference = 1945;
-	//[3] b3DynamicBvh::collideTT
-	bool cfgBenchmark3_Enable = cfgEnable;
-	static const int cfgBenchmark3_Iterations = 512;
-	static const int cfgBenchmark3_Reference = 5485;
-	//[4] b3DynamicBvh::collideTT self
-	bool cfgBenchmark4_Enable = cfgEnable;
-	static const int cfgBenchmark4_Iterations = 512;
-	static const int cfgBenchmark4_Reference = 2814;
-	//[5] b3DynamicBvh::collideTT xform
-	bool cfgBenchmark5_Enable = cfgEnable;
-	static const int cfgBenchmark5_Iterations = 512;
-	static const b3Scalar cfgBenchmark5_OffsetScale = 2;
-	static const int cfgBenchmark5_Reference = 7379;
-	//[6] b3DynamicBvh::collideTT xform,self
-	bool cfgBenchmark6_Enable = cfgEnable;
-	static const int cfgBenchmark6_Iterations = 512;
-	static const b3Scalar cfgBenchmark6_OffsetScale = 2;
-	static const int cfgBenchmark6_Reference = 7270;
-	//[7] b3DynamicBvh::rayTest
-	bool cfgBenchmark7_Enable = cfgEnable;
-	static const int cfgBenchmark7_Passes = 32;
-	static const int cfgBenchmark7_Iterations = 65536;
-	static const int cfgBenchmark7_Reference = 6307;
-	//[8] insert/remove
-	bool cfgBenchmark8_Enable = cfgEnable;
-	static const int cfgBenchmark8_Passes = 32;
-	static const int cfgBenchmark8_Iterations = 65536;
-	static const int cfgBenchmark8_Reference = 2105;
-	//[9] updates (teleport)
-	bool cfgBenchmark9_Enable = cfgEnable;
-	static const int cfgBenchmark9_Passes = 32;
-	static const int cfgBenchmark9_Iterations = 65536;
-	static const int cfgBenchmark9_Reference = 1879;
-	//[10] updates (jitter)
-	bool cfgBenchmark10_Enable = cfgEnable;
-	static const b3Scalar cfgBenchmark10_Scale = cfgVolumeCenterScale / 10000;
-	static const int cfgBenchmark10_Passes = 32;
-	static const int cfgBenchmark10_Iterations = 65536;
-	static const int cfgBenchmark10_Reference = 1244;
-	//[11] optimize (incremental)
-	bool cfgBenchmark11_Enable = cfgEnable;
-	static const int cfgBenchmark11_Passes = 64;
-	static const int cfgBenchmark11_Iterations = 65536;
-	static const int cfgBenchmark11_Reference = 2510;
-	//[12] b3DbvtVolume notequal
-	bool cfgBenchmark12_Enable = cfgEnable;
-	static const int cfgBenchmark12_Iterations = 32;
-	static const int cfgBenchmark12_Reference = 3677;
-	//[13] culling(OCL+fullsort)
-	bool cfgBenchmark13_Enable = cfgEnable;
-	static const int cfgBenchmark13_Iterations = 1024;
-	static const int cfgBenchmark13_Reference = 2231;
-	//[14] culling(OCL+qsort)
-	bool cfgBenchmark14_Enable = cfgEnable;
-	static const int cfgBenchmark14_Iterations = 8192;
-	static const int cfgBenchmark14_Reference = 3500;
-	//[15] culling(KDOP+qsort)
-	bool cfgBenchmark15_Enable = cfgEnable;
-	static const int cfgBenchmark15_Iterations = 8192;
-	static const int cfgBenchmark15_Reference = 1151;
-	//[16] insert/remove batch
-	bool cfgBenchmark16_Enable = cfgEnable;
-	static const int cfgBenchmark16_BatchCount = 256;
-	static const int cfgBenchmark16_Passes = 16384;
-	static const int cfgBenchmark16_Reference = 5138;
-	//[17] select
-	bool cfgBenchmark17_Enable = cfgEnable;
-	static const int cfgBenchmark17_Iterations = 4;
-	static const int cfgBenchmark17_Reference = 3390;
-
-	b3Clock wallclock;
-	printf("Benchmarking dbvt...\r\n");
-	printf("\tWorld scale: %f\r\n", cfgVolumeCenterScale);
-	printf("\tExtents base: %f\r\n", cfgVolumeExentsBase);
-	printf("\tExtents range: %f\r\n", cfgVolumeExentsScale);
-	printf("\tLeaves: %u\r\n", cfgLeaves);
-	printf("\tsizeof(b3DbvtVolume): %u bytes\r\n", sizeof(b3DbvtVolume));
-	printf("\tsizeof(b3DbvtNode):   %u bytes\r\n", sizeof(b3DbvtNode));
-	if (cfgBenchmark1_Enable)
-	{  // Benchmark 1
-		srand(380843);
-		b3AlignedObjectArray<b3DbvtVolume> volumes;
-		b3AlignedObjectArray<bool> results;
-		volumes.resize(cfgLeaves);
-		results.resize(cfgLeaves);
-		for (int i = 0; i < cfgLeaves; ++i)
-		{
-			volumes[i] = b3DbvtBenchmark::RandVolume(cfgVolumeCenterScale, cfgVolumeExentsBase, cfgVolumeExentsScale);
-		}
-		printf("[1] b3DbvtVolume intersections: ");
-		wallclock.reset();
-		for (int i = 0; i < cfgBenchmark1_Iterations; ++i)
-		{
-			for (int j = 0; j < cfgLeaves; ++j)
-			{
-				for (int k = 0; k < cfgLeaves; ++k)
-				{
-					results[k] = Intersect(volumes[j], volumes[k]);
-				}
-			}
-		}
-		const int time = (int)wallclock.getTimeMilliseconds();
-		printf("%u ms (%i%%)\r\n", time, (time - cfgBenchmark1_Reference) * 100 / time);
-	}
-	if (cfgBenchmark2_Enable)
-	{  // Benchmark 2
-		srand(380843);
-		b3AlignedObjectArray<b3DbvtVolume> volumes;
-		b3AlignedObjectArray<b3DbvtVolume> results;
-		volumes.resize(cfgLeaves);
-		results.resize(cfgLeaves);
-		for (int i = 0; i < cfgLeaves; ++i)
-		{
-			volumes[i] = b3DbvtBenchmark::RandVolume(cfgVolumeCenterScale, cfgVolumeExentsBase, cfgVolumeExentsScale);
-		}
-		printf("[2] b3DbvtVolume merges: ");
-		wallclock.reset();
-		for (int i = 0; i < cfgBenchmark2_Iterations; ++i)
-		{
-			for (int j = 0; j < cfgLeaves; ++j)
-			{
-				for (int k = 0; k < cfgLeaves; ++k)
-				{
-					Merge(volumes[j], volumes[k], results[k]);
-				}
-			}
-		}
-		const int time = (int)wallclock.getTimeMilliseconds();
-		printf("%u ms (%i%%)\r\n", time, (time - cfgBenchmark2_Reference) * 100 / time);
-	}
-	if (cfgBenchmark3_Enable)
-	{  // Benchmark 3
-		srand(380843);
-		b3DynamicBvh dbvt[2];
-		b3DbvtBenchmark::NilPolicy policy;
-		b3DbvtBenchmark::RandTree(cfgVolumeCenterScale, cfgVolumeExentsBase, cfgVolumeExentsScale, cfgLeaves, dbvt[0]);
-		b3DbvtBenchmark::RandTree(cfgVolumeCenterScale, cfgVolumeExentsBase, cfgVolumeExentsScale, cfgLeaves, dbvt[1]);
-		dbvt[0].optimizeTopDown();
-		dbvt[1].optimizeTopDown();
-		printf("[3] b3DynamicBvh::collideTT: ");
-		wallclock.reset();
-		for (int i = 0; i < cfgBenchmark3_Iterations; ++i)
-		{
-			b3DynamicBvh::collideTT(dbvt[0].m_root, dbvt[1].m_root, policy);
-		}
-		const int time = (int)wallclock.getTimeMilliseconds();
-		printf("%u ms (%i%%)\r\n", time, (time - cfgBenchmark3_Reference) * 100 / time);
-	}
-	if (cfgBenchmark4_Enable)
-	{  // Benchmark 4
-		srand(380843);
-		b3DynamicBvh dbvt;
-		b3DbvtBenchmark::NilPolicy policy;
-		b3DbvtBenchmark::RandTree(cfgVolumeCenterScale, cfgVolumeExentsBase, cfgVolumeExentsScale, cfgLeaves, dbvt);
-		dbvt.optimizeTopDown();
-		printf("[4] b3DynamicBvh::collideTT self: ");
-		wallclock.reset();
-		for (int i = 0; i < cfgBenchmark4_Iterations; ++i)
-		{
-			b3DynamicBvh::collideTT(dbvt.m_root, dbvt.m_root, policy);
-		}
-		const int time = (int)wallclock.getTimeMilliseconds();
-		printf("%u ms (%i%%)\r\n", time, (time - cfgBenchmark4_Reference) * 100 / time);
-	}
-	if (cfgBenchmark5_Enable)
-	{  // Benchmark 5
-		srand(380843);
-		b3DynamicBvh dbvt[2];
-		b3AlignedObjectArray<b3Transform> transforms;
-		b3DbvtBenchmark::NilPolicy policy;
-		transforms.resize(cfgBenchmark5_Iterations);
-		for (int i = 0; i < transforms.size(); ++i)
-		{
-			transforms[i] = b3DbvtBenchmark::RandTransform(cfgVolumeCenterScale * cfgBenchmark5_OffsetScale);
-		}
-		b3DbvtBenchmark::RandTree(cfgVolumeCenterScale, cfgVolumeExentsBase, cfgVolumeExentsScale, cfgLeaves, dbvt[0]);
-		b3DbvtBenchmark::RandTree(cfgVolumeCenterScale, cfgVolumeExentsBase, cfgVolumeExentsScale, cfgLeaves, dbvt[1]);
-		dbvt[0].optimizeTopDown();
-		dbvt[1].optimizeTopDown();
-		printf("[5] b3DynamicBvh::collideTT xform: ");
-		wallclock.reset();
-		for (int i = 0; i < cfgBenchmark5_Iterations; ++i)
-		{
-			b3DynamicBvh::collideTT(dbvt[0].m_root, dbvt[1].m_root, transforms[i], policy);
-		}
-		const int time = (int)wallclock.getTimeMilliseconds();
-		printf("%u ms (%i%%)\r\n", time, (time - cfgBenchmark5_Reference) * 100 / time);
-	}
-	if (cfgBenchmark6_Enable)
-	{  // Benchmark 6
-		srand(380843);
-		b3DynamicBvh dbvt;
-		b3AlignedObjectArray<b3Transform> transforms;
-		b3DbvtBenchmark::NilPolicy policy;
-		transforms.resize(cfgBenchmark6_Iterations);
-		for (int i = 0; i < transforms.size(); ++i)
-		{
-			transforms[i] = b3DbvtBenchmark::RandTransform(cfgVolumeCenterScale * cfgBenchmark6_OffsetScale);
-		}
-		b3DbvtBenchmark::RandTree(cfgVolumeCenterScale, cfgVolumeExentsBase, cfgVolumeExentsScale, cfgLeaves, dbvt);
-		dbvt.optimizeTopDown();
-		printf("[6] b3DynamicBvh::collideTT xform,self: ");
-		wallclock.reset();
-		for (int i = 0; i < cfgBenchmark6_Iterations; ++i)
-		{
-			b3DynamicBvh::collideTT(dbvt.m_root, dbvt.m_root, transforms[i], policy);
-		}
-		const int time = (int)wallclock.getTimeMilliseconds();
-		printf("%u ms (%i%%)\r\n", time, (time - cfgBenchmark6_Reference) * 100 / time);
-	}
-	if (cfgBenchmark7_Enable)
-	{  // Benchmark 7
-		srand(380843);
-		b3DynamicBvh dbvt;
-		b3AlignedObjectArray<b3Vector3> rayorg;
-		b3AlignedObjectArray<b3Vector3> raydir;
-		b3DbvtBenchmark::NilPolicy policy;
-		rayorg.resize(cfgBenchmark7_Iterations);
-		raydir.resize(cfgBenchmark7_Iterations);
-		for (int i = 0; i < rayorg.size(); ++i)
-		{
-			rayorg[i] = b3DbvtBenchmark::RandVector3(cfgVolumeCenterScale * 2);
-			raydir[i] = b3DbvtBenchmark::RandVector3(cfgVolumeCenterScale * 2);
-		}
-		b3DbvtBenchmark::RandTree(cfgVolumeCenterScale, cfgVolumeExentsBase, cfgVolumeExentsScale, cfgLeaves, dbvt);
-		dbvt.optimizeTopDown();
-		printf("[7] b3DynamicBvh::rayTest: ");
-		wallclock.reset();
-		for (int i = 0; i < cfgBenchmark7_Passes; ++i)
-		{
-			for (int j = 0; j < cfgBenchmark7_Iterations; ++j)
-			{
-				b3DynamicBvh::rayTest(dbvt.m_root, rayorg[j], rayorg[j] + raydir[j], policy);
-			}
-		}
-		const int time = (int)wallclock.getTimeMilliseconds();
-		unsigned rays = cfgBenchmark7_Passes * cfgBenchmark7_Iterations;
-		printf("%u ms (%i%%),(%u r/s)\r\n", time, (time - cfgBenchmark7_Reference) * 100 / time, (rays * 1000) / time);
-	}
-	if (cfgBenchmark8_Enable)
-	{  // Benchmark 8
-		srand(380843);
-		b3DynamicBvh dbvt;
-		b3DbvtBenchmark::RandTree(cfgVolumeCenterScale, cfgVolumeExentsBase, cfgVolumeExentsScale, cfgLeaves, dbvt);
-		dbvt.optimizeTopDown();
-		printf("[8] insert/remove: ");
-		wallclock.reset();
-		for (int i = 0; i < cfgBenchmark8_Passes; ++i)
-		{
-			for (int j = 0; j < cfgBenchmark8_Iterations; ++j)
-			{
-				dbvt.remove(dbvt.insert(b3DbvtBenchmark::RandVolume(cfgVolumeCenterScale, cfgVolumeExentsBase, cfgVolumeExentsScale), 0));
-			}
-		}
-		const int time = (int)wallclock.getTimeMilliseconds();
-		const int ir = cfgBenchmark8_Passes * cfgBenchmark8_Iterations;
-		printf("%u ms (%i%%),(%u ir/s)\r\n", time, (time - cfgBenchmark8_Reference) * 100 / time, ir * 1000 / time);
-	}
-	if (cfgBenchmark9_Enable)
-	{  // Benchmark 9
-		srand(380843);
-		b3DynamicBvh dbvt;
-		b3AlignedObjectArray<const b3DbvtNode*> leaves;
-		b3DbvtBenchmark::RandTree(cfgVolumeCenterScale, cfgVolumeExentsBase, cfgVolumeExentsScale, cfgLeaves, dbvt);
-		dbvt.optimizeTopDown();
-		dbvt.extractLeaves(dbvt.m_root, leaves);
-		printf("[9] updates (teleport): ");
-		wallclock.reset();
-		for (int i = 0; i < cfgBenchmark9_Passes; ++i)
-		{
-			for (int j = 0; j < cfgBenchmark9_Iterations; ++j)
-			{
-				dbvt.update(const_cast<b3DbvtNode*>(leaves[rand() % cfgLeaves]),
-							b3DbvtBenchmark::RandVolume(cfgVolumeCenterScale, cfgVolumeExentsBase, cfgVolumeExentsScale));
-			}
-		}
-		const int time = (int)wallclock.getTimeMilliseconds();
-		const int up = cfgBenchmark9_Passes * cfgBenchmark9_Iterations;
-		printf("%u ms (%i%%),(%u u/s)\r\n", time, (time - cfgBenchmark9_Reference) * 100 / time, up * 1000 / time);
-	}
-	if (cfgBenchmark10_Enable)
-	{  // Benchmark 10
-		srand(380843);
-		b3DynamicBvh dbvt;
-		b3AlignedObjectArray<const b3DbvtNode*> leaves;
-		b3AlignedObjectArray<b3Vector3> vectors;
-		vectors.resize(cfgBenchmark10_Iterations);
-		for (int i = 0; i < vectors.size(); ++i)
-		{
-			vectors[i] = (b3DbvtBenchmark::RandVector3() * 2 - b3Vector3(1, 1, 1)) * cfgBenchmark10_Scale;
-		}
-		b3DbvtBenchmark::RandTree(cfgVolumeCenterScale, cfgVolumeExentsBase, cfgVolumeExentsScale, cfgLeaves, dbvt);
-		dbvt.optimizeTopDown();
-		dbvt.extractLeaves(dbvt.m_root, leaves);
-		printf("[10] updates (jitter): ");
-		wallclock.reset();
-
-		for (int i = 0; i < cfgBenchmark10_Passes; ++i)
-		{
-			for (int j = 0; j < cfgBenchmark10_Iterations; ++j)
-			{
-				const b3Vector3& d = vectors[j];
-				b3DbvtNode* l = const_cast<b3DbvtNode*>(leaves[rand() % cfgLeaves]);
-				b3DbvtVolume v = b3DbvtVolume::FromMM(l->volume.Mins() + d, l->volume.Maxs() + d);
-				dbvt.update(l, v);
-			}
-		}
-		const int time = (int)wallclock.getTimeMilliseconds();
-		const int up = cfgBenchmark10_Passes * cfgBenchmark10_Iterations;
-		printf("%u ms (%i%%),(%u u/s)\r\n", time, (time - cfgBenchmark10_Reference) * 100 / time, up * 1000 / time);
-	}
-	if (cfgBenchmark11_Enable)
-	{  // Benchmark 11
-		srand(380843);
-		b3DynamicBvh dbvt;
-		b3DbvtBenchmark::RandTree(cfgVolumeCenterScale, cfgVolumeExentsBase, cfgVolumeExentsScale, cfgLeaves, dbvt);
-		dbvt.optimizeTopDown();
-		printf("[11] optimize (incremental): ");
-		wallclock.reset();
-		for (int i = 0; i < cfgBenchmark11_Passes; ++i)
-		{
-			dbvt.optimizeIncremental(cfgBenchmark11_Iterations);
-		}
-		const int time = (int)wallclock.getTimeMilliseconds();
-		const int op = cfgBenchmark11_Passes * cfgBenchmark11_Iterations;
-		printf("%u ms (%i%%),(%u o/s)\r\n", time, (time - cfgBenchmark11_Reference) * 100 / time, op / time * 1000);
-	}
-	if (cfgBenchmark12_Enable)
-	{  // Benchmark 12
-		srand(380843);
-		b3AlignedObjectArray<b3DbvtVolume> volumes;
-		b3AlignedObjectArray<bool> results;
-		volumes.resize(cfgLeaves);
-		results.resize(cfgLeaves);
-		for (int i = 0; i < cfgLeaves; ++i)
-		{
-			volumes[i] = b3DbvtBenchmark::RandVolume(cfgVolumeCenterScale, cfgVolumeExentsBase, cfgVolumeExentsScale);
-		}
-		printf("[12] b3DbvtVolume notequal: ");
-		wallclock.reset();
-		for (int i = 0; i < cfgBenchmark12_Iterations; ++i)
-		{
-			for (int j = 0; j < cfgLeaves; ++j)
-			{
-				for (int k = 0; k < cfgLeaves; ++k)
-				{
-					results[k] = NotEqual(volumes[j], volumes[k]);
-				}
-			}
-		}
-		const int time = (int)wallclock.getTimeMilliseconds();
-		printf("%u ms (%i%%)\r\n", time, (time - cfgBenchmark12_Reference) * 100 / time);
-	}
-	if (cfgBenchmark13_Enable)
-	{  // Benchmark 13
-		srand(380843);
-		b3DynamicBvh dbvt;
-		b3AlignedObjectArray<b3Vector3> vectors;
-		b3DbvtBenchmark::NilPolicy policy;
-		vectors.resize(cfgBenchmark13_Iterations);
-		for (int i = 0; i < vectors.size(); ++i)
-		{
-			vectors[i] = (b3DbvtBenchmark::RandVector3() * 2 - b3Vector3(1, 1, 1)).normalized();
-		}
-		b3DbvtBenchmark::RandTree(cfgVolumeCenterScale, cfgVolumeExentsBase, cfgVolumeExentsScale, cfgLeaves, dbvt);
-		dbvt.optimizeTopDown();
-		printf("[13] culling(OCL+fullsort): ");
-		wallclock.reset();
-		for (int i = 0; i < cfgBenchmark13_Iterations; ++i)
-		{
-			static const b3Scalar offset = 0;
-			policy.m_depth = -B3_INFINITY;
-			dbvt.collideOCL(dbvt.m_root, &vectors[i], &offset, vectors[i], 1, policy);
-		}
-		const int time = (int)wallclock.getTimeMilliseconds();
-		const int t = cfgBenchmark13_Iterations;
-		printf("%u ms (%i%%),(%u t/s)\r\n", time, (time - cfgBenchmark13_Reference) * 100 / time, (t * 1000) / time);
-	}
-	if (cfgBenchmark14_Enable)
-	{  // Benchmark 14
-		srand(380843);
-		b3DynamicBvh dbvt;
-		b3AlignedObjectArray<b3Vector3> vectors;
-		b3DbvtBenchmark::P14 policy;
-		vectors.resize(cfgBenchmark14_Iterations);
-		for (int i = 0; i < vectors.size(); ++i)
-		{
-			vectors[i] = (b3DbvtBenchmark::RandVector3() * 2 - b3Vector3(1, 1, 1)).normalized();
-		}
-		b3DbvtBenchmark::RandTree(cfgVolumeCenterScale, cfgVolumeExentsBase, cfgVolumeExentsScale, cfgLeaves, dbvt);
-		dbvt.optimizeTopDown();
-		policy.m_nodes.reserve(cfgLeaves);
-		printf("[14] culling(OCL+qsort): ");
-		wallclock.reset();
-		for (int i = 0; i < cfgBenchmark14_Iterations; ++i)
-		{
-			static const b3Scalar offset = 0;
-			policy.m_nodes.resize(0);
-			dbvt.collideOCL(dbvt.m_root, &vectors[i], &offset, vectors[i], 1, policy, false);
-			policy.m_nodes.quickSort(b3DbvtBenchmark::P14::sortfnc);
-		}
-		const int time = (int)wallclock.getTimeMilliseconds();
-		const int t = cfgBenchmark14_Iterations;
-		printf("%u ms (%i%%),(%u t/s)\r\n", time, (time - cfgBenchmark14_Reference) * 100 / time, (t * 1000) / time);
-	}
-	if (cfgBenchmark15_Enable)
-	{  // Benchmark 15
-		srand(380843);
-		b3DynamicBvh dbvt;
-		b3AlignedObjectArray<b3Vector3> vectors;
-		b3DbvtBenchmark::P15 policy;
-		vectors.resize(cfgBenchmark15_Iterations);
-		for (int i = 0; i < vectors.size(); ++i)
-		{
-			vectors[i] = (b3DbvtBenchmark::RandVector3() * 2 - b3Vector3(1, 1, 1)).normalized();
-		}
-		b3DbvtBenchmark::RandTree(cfgVolumeCenterScale, cfgVolumeExentsBase, cfgVolumeExentsScale, cfgLeaves, dbvt);
-		dbvt.optimizeTopDown();
-		policy.m_nodes.reserve(cfgLeaves);
-		printf("[15] culling(KDOP+qsort): ");
-		wallclock.reset();
-		for (int i = 0; i < cfgBenchmark15_Iterations; ++i)
-		{
-			static const b3Scalar offset = 0;
-			policy.m_nodes.resize(0);
-			policy.m_axis = vectors[i];
-			dbvt.collideKDOP(dbvt.m_root, &vectors[i], &offset, 1, policy);
-			policy.m_nodes.quickSort(b3DbvtBenchmark::P15::sortfnc);
-		}
-		const int time = (int)wallclock.getTimeMilliseconds();
-		const int t = cfgBenchmark15_Iterations;
-		printf("%u ms (%i%%),(%u t/s)\r\n", time, (time - cfgBenchmark15_Reference) * 100 / time, (t * 1000) / time);
-	}
-	if (cfgBenchmark16_Enable)
-	{  // Benchmark 16
-		srand(380843);
-		b3DynamicBvh dbvt;
-		b3AlignedObjectArray<b3DbvtNode*> batch;
-		b3DbvtBenchmark::RandTree(cfgVolumeCenterScale, cfgVolumeExentsBase, cfgVolumeExentsScale, cfgLeaves, dbvt);
-		dbvt.optimizeTopDown();
-		batch.reserve(cfgBenchmark16_BatchCount);
-		printf("[16] insert/remove batch(%u): ", cfgBenchmark16_BatchCount);
-		wallclock.reset();
-		for (int i = 0; i < cfgBenchmark16_Passes; ++i)
-		{
-			for (int j = 0; j < cfgBenchmark16_BatchCount; ++j)
-			{
-				batch.push_back(dbvt.insert(b3DbvtBenchmark::RandVolume(cfgVolumeCenterScale, cfgVolumeExentsBase, cfgVolumeExentsScale), 0));
-			}
-			for (int j = 0; j < cfgBenchmark16_BatchCount; ++j)
-			{
-				dbvt.remove(batch[j]);
-			}
-			batch.resize(0);
-		}
-		const int time = (int)wallclock.getTimeMilliseconds();
-		const int ir = cfgBenchmark16_Passes * cfgBenchmark16_BatchCount;
-		printf("%u ms (%i%%),(%u bir/s)\r\n", time, (time - cfgBenchmark16_Reference) * 100 / time, int(ir * 1000.0 / time));
-	}
-	if (cfgBenchmark17_Enable)
-	{  // Benchmark 17
-		srand(380843);
-		b3AlignedObjectArray<b3DbvtVolume> volumes;
-		b3AlignedObjectArray<int> results;
-		b3AlignedObjectArray<int> indices;
-		volumes.resize(cfgLeaves);
-		results.resize(cfgLeaves);
-		indices.resize(cfgLeaves);
-		for (int i = 0; i < cfgLeaves; ++i)
-		{
-			indices[i] = i;
-			volumes[i] = b3DbvtBenchmark::RandVolume(cfgVolumeCenterScale, cfgVolumeExentsBase, cfgVolumeExentsScale);
-		}
-		for (int i = 0; i < cfgLeaves; ++i)
-		{
-			b3Swap(indices[i], indices[rand() % cfgLeaves]);
-		}
-		printf("[17] b3DbvtVolume select: ");
-		wallclock.reset();
-		for (int i = 0; i < cfgBenchmark17_Iterations; ++i)
-		{
-			for (int j = 0; j < cfgLeaves; ++j)
-			{
-				for (int k = 0; k < cfgLeaves; ++k)
-				{
-					const int idx = indices[k];
-					results[idx] = Select(volumes[idx], volumes[j], volumes[k]);
-				}
-			}
-		}
-		const int time = (int)wallclock.getTimeMilliseconds();
-		printf("%u ms (%i%%)\r\n", time, (time - cfgBenchmark17_Reference) * 100 / time);
-	}
-	printf("\r\n\r\n");
-}
-#endif

thirdparty/bullet/Bullet3Collision/BroadPhaseCollision/b3DynamicBvh.h

@@ -1,1332 +0,0 @@
-/*
-Bullet Continuous Collision Detection and Physics Library
-Copyright (c) 2003-2013 Erwin Coumans  http://bulletphysics.org
-
-This software is provided 'as-is', without any express or implied warranty.
-In no event will the authors be held liable for any damages arising from the use of this software.
-Permission is granted to anyone to use this software for any purpose,
-including commercial applications, and to alter it and redistribute it freely,
-subject to the following restrictions:
-
-1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
-2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
-3. This notice may not be removed or altered from any source distribution.
-*/
-///b3DynamicBvh implementation by Nathanael Presson
-
-#ifndef B3_DYNAMIC_BOUNDING_VOLUME_TREE_H
-#define B3_DYNAMIC_BOUNDING_VOLUME_TREE_H
-
-#include "Bullet3Common/b3AlignedObjectArray.h"
-#include "Bullet3Common/b3Vector3.h"
-#include "Bullet3Common/b3Transform.h"
-#include "Bullet3Geometry/b3AabbUtil.h"
-
-//
-// Compile time configuration
-//
-
-// Implementation profiles
-#define B3_DBVT_IMPL_GENERIC 0  // Generic implementation
-#define B3_DBVT_IMPL_SSE 1      // SSE
-
-// Template implementation of ICollide
-#ifdef _WIN32
-#if (defined(_MSC_VER) && _MSC_VER >= 1400)
-#define B3_DBVT_USE_TEMPLATE 1
-#else
-#define B3_DBVT_USE_TEMPLATE 0
-#endif
-#else
-#define B3_DBVT_USE_TEMPLATE 0
-#endif
-
-// Use only intrinsics instead of inline asm
-#define B3_DBVT_USE_INTRINSIC_SSE 1
-
-// Using memmov for collideOCL
-#define B3_DBVT_USE_MEMMOVE 1
-
-// Enable benchmarking code
-#define B3_DBVT_ENABLE_BENCHMARK 0
-
-// Inlining
-#define B3_DBVT_INLINE B3_FORCE_INLINE
-
-// Specific methods implementation
-
-//SSE gives errors on a MSVC 7.1
-#if defined(B3_USE_SSE)  //&& defined (_WIN32)
-#define B3_DBVT_SELECT_IMPL B3_DBVT_IMPL_SSE
-#define B3_DBVT_MERGE_IMPL B3_DBVT_IMPL_SSE
-#define B3_DBVT_INT0_IMPL B3_DBVT_IMPL_SSE
-#else
-#define B3_DBVT_SELECT_IMPL B3_DBVT_IMPL_GENERIC
-#define B3_DBVT_MERGE_IMPL B3_DBVT_IMPL_GENERIC
-#define B3_DBVT_INT0_IMPL B3_DBVT_IMPL_GENERIC
-#endif
-
-#if (B3_DBVT_SELECT_IMPL == B3_DBVT_IMPL_SSE) || \
-	(B3_DBVT_MERGE_IMPL == B3_DBVT_IMPL_SSE) ||  \
-	(B3_DBVT_INT0_IMPL == B3_DBVT_IMPL_SSE)
-#include <emmintrin.h>
-#endif
-
-//
-// Auto config and checks
-//
-
-#if B3_DBVT_USE_TEMPLATE
-#define B3_DBVT_VIRTUAL
-#define B3_DBVT_VIRTUAL_DTOR(a)
-#define B3_DBVT_PREFIX template <typename T>
-#define B3_DBVT_IPOLICY T& policy
-#define B3_DBVT_CHECKTYPE                        \
-	static const ICollide& typechecker = *(T*)1; \
-	(void)typechecker;
-#else
-#define B3_DBVT_VIRTUAL_DTOR(a) \
-	virtual ~a() {}
-#define B3_DBVT_VIRTUAL virtual
-#define B3_DBVT_PREFIX
-#define B3_DBVT_IPOLICY ICollide& policy
-#define B3_DBVT_CHECKTYPE
-#endif
-
-#if B3_DBVT_USE_MEMMOVE
-#if !defined(__CELLOS_LV2__) && !defined(__MWERKS__)
-#include <memory.h>
-#endif
-#include <string.h>
-#endif
-
-#ifndef B3_DBVT_USE_TEMPLATE
-#error "B3_DBVT_USE_TEMPLATE undefined"
-#endif
-
-#ifndef B3_DBVT_USE_MEMMOVE
-#error "B3_DBVT_USE_MEMMOVE undefined"
-#endif
-
-#ifndef B3_DBVT_ENABLE_BENCHMARK
-#error "B3_DBVT_ENABLE_BENCHMARK undefined"
-#endif
-
-#ifndef B3_DBVT_SELECT_IMPL
-#error "B3_DBVT_SELECT_IMPL undefined"
-#endif
-
-#ifndef B3_DBVT_MERGE_IMPL
-#error "B3_DBVT_MERGE_IMPL undefined"
-#endif
-
-#ifndef B3_DBVT_INT0_IMPL
-#error "B3_DBVT_INT0_IMPL undefined"
-#endif
-
-//
-// Defaults volumes
-//
-
-/* b3DbvtAabbMm			*/
-struct b3DbvtAabbMm
-{
-	B3_DBVT_INLINE b3Vector3 Center() const { return ((mi + mx) / 2); }
-	B3_DBVT_INLINE b3Vector3 Lengths() const { return (mx - mi); }
-	B3_DBVT_INLINE b3Vector3 Extents() const { return ((mx - mi) / 2); }
-	B3_DBVT_INLINE const b3Vector3& Mins() const { return (mi); }
-	B3_DBVT_INLINE const b3Vector3& Maxs() const { return (mx); }
-	static inline b3DbvtAabbMm FromCE(const b3Vector3& c, const b3Vector3& e);
-	static inline b3DbvtAabbMm FromCR(const b3Vector3& c, b3Scalar r);
-	static inline b3DbvtAabbMm FromMM(const b3Vector3& mi, const b3Vector3& mx);
-	static inline b3DbvtAabbMm FromPoints(const b3Vector3* pts, int n);
-	static inline b3DbvtAabbMm FromPoints(const b3Vector3** ppts, int n);
-	B3_DBVT_INLINE void Expand(const b3Vector3& e);
-	B3_DBVT_INLINE void SignedExpand(const b3Vector3& e);
-	B3_DBVT_INLINE bool Contain(const b3DbvtAabbMm& a) const;
-	B3_DBVT_INLINE int Classify(const b3Vector3& n, b3Scalar o, int s) const;
-	B3_DBVT_INLINE b3Scalar ProjectMinimum(const b3Vector3& v, unsigned signs) const;
-	B3_DBVT_INLINE friend bool b3Intersect(const b3DbvtAabbMm& a,
-										   const b3DbvtAabbMm& b);
-
-	B3_DBVT_INLINE friend bool b3Intersect(const b3DbvtAabbMm& a,
-										   const b3Vector3& b);
-
-	B3_DBVT_INLINE friend b3Scalar b3Proximity(const b3DbvtAabbMm& a,
-											   const b3DbvtAabbMm& b);
-	B3_DBVT_INLINE friend int b3Select(const b3DbvtAabbMm& o,
-									   const b3DbvtAabbMm& a,
-									   const b3DbvtAabbMm& b);
-	B3_DBVT_INLINE friend void b3Merge(const b3DbvtAabbMm& a,
-									   const b3DbvtAabbMm& b,
-									   b3DbvtAabbMm& r);
-	B3_DBVT_INLINE friend bool b3NotEqual(const b3DbvtAabbMm& a,
-										  const b3DbvtAabbMm& b);
-
-	B3_DBVT_INLINE b3Vector3& tMins() { return (mi); }
-	B3_DBVT_INLINE b3Vector3& tMaxs() { return (mx); }
-
-private:
-	B3_DBVT_INLINE void AddSpan(const b3Vector3& d, b3Scalar& smi, b3Scalar& smx) const;
-
-private:
-	b3Vector3 mi, mx;
-};
-
-// Types
-typedef b3DbvtAabbMm b3DbvtVolume;
-
-/* b3DbvtNode				*/
-struct b3DbvtNode
-{
-	b3DbvtVolume volume;
-	b3DbvtNode* parent;
-	B3_DBVT_INLINE bool isleaf() const { return (childs[1] == 0); }
-	B3_DBVT_INLINE bool isinternal() const { return (!isleaf()); }
-	union {
-		b3DbvtNode* childs[2];
-		void* data;
-		int dataAsInt;
-	};
-};
-
-///The b3DynamicBvh class implements a fast dynamic bounding volume tree based on axis aligned bounding boxes (aabb tree).
-///This b3DynamicBvh is used for soft body collision detection and for the b3DynamicBvhBroadphase. It has a fast insert, remove and update of nodes.
-///Unlike the b3QuantizedBvh, nodes can be dynamically moved around, which allows for change in topology of the underlying data structure.
-struct b3DynamicBvh
-{
-	/* Stack element	*/
-	struct sStkNN
-	{
-		const b3DbvtNode* a;
-		const b3DbvtNode* b;
-		sStkNN() {}
-		sStkNN(const b3DbvtNode* na, const b3DbvtNode* nb) : a(na), b(nb) {}
-	};
-	struct sStkNP
-	{
-		const b3DbvtNode* node;
-		int mask;
-		sStkNP(const b3DbvtNode* n, unsigned m) : node(n), mask(m) {}
-	};
-	struct sStkNPS
-	{
-		const b3DbvtNode* node;
-		int mask;
-		b3Scalar value;
-		sStkNPS() {}
-		sStkNPS(const b3DbvtNode* n, unsigned m, b3Scalar v) : node(n), mask(m), value(v) {}
-	};
-	struct sStkCLN
-	{
-		const b3DbvtNode* node;
-		b3DbvtNode* parent;
-		sStkCLN(const b3DbvtNode* n, b3DbvtNode* p) : node(n), parent(p) {}
-	};
-	// Policies/Interfaces
-
-	/* ICollide	*/
-	struct ICollide
-	{
-		B3_DBVT_VIRTUAL_DTOR(ICollide)
-		B3_DBVT_VIRTUAL void Process(const b3DbvtNode*, const b3DbvtNode*) {}
-		B3_DBVT_VIRTUAL void Process(const b3DbvtNode*) {}
-		B3_DBVT_VIRTUAL void Process(const b3DbvtNode* n, b3Scalar) { Process(n); }
-		B3_DBVT_VIRTUAL bool Descent(const b3DbvtNode*) { return (true); }
-		B3_DBVT_VIRTUAL bool AllLeaves(const b3DbvtNode*) { return (true); }
-	};
-	/* IWriter	*/
-	struct IWriter
-	{
-		virtual ~IWriter() {}
-		virtual void Prepare(const b3DbvtNode* root, int numnodes) = 0;
-		virtual void WriteNode(const b3DbvtNode*, int index, int parent, int child0, int child1) = 0;
-		virtual void WriteLeaf(const b3DbvtNode*, int index, int parent) = 0;
-	};
-	/* IClone	*/
-	struct IClone
-	{
-		virtual ~IClone() {}
-		virtual void CloneLeaf(b3DbvtNode*) {}
-	};
-
-	// Constants
-	enum
-	{
-		B3_SIMPLE_STACKSIZE = 64,
-		B3_DOUBLE_STACKSIZE = B3_SIMPLE_STACKSIZE * 2
-	};
-
-	// Fields
-	b3DbvtNode* m_root;
-	b3DbvtNode* m_free;
-	int m_lkhd;
-	int m_leaves;
-	unsigned m_opath;
-
-	b3AlignedObjectArray<sStkNN> m_stkStack;
-	mutable b3AlignedObjectArray<const b3DbvtNode*> m_rayTestStack;
-
-	// Methods
-	b3DynamicBvh();
-	~b3DynamicBvh();
-	void clear();
-	bool empty() const { return (0 == m_root); }
-	void optimizeBottomUp();
-	void optimizeTopDown(int bu_treshold = 128);
-	void optimizeIncremental(int passes);
-	b3DbvtNode* insert(const b3DbvtVolume& box, void* data);
-	void update(b3DbvtNode* leaf, int lookahead = -1);
-	void update(b3DbvtNode* leaf, b3DbvtVolume& volume);
-	bool update(b3DbvtNode* leaf, b3DbvtVolume& volume, const b3Vector3& velocity, b3Scalar margin);
-	bool update(b3DbvtNode* leaf, b3DbvtVolume& volume, const b3Vector3& velocity);
-	bool update(b3DbvtNode* leaf, b3DbvtVolume& volume, b3Scalar margin);
-	void remove(b3DbvtNode* leaf);
-	void write(IWriter* iwriter) const;
-	void clone(b3DynamicBvh& dest, IClone* iclone = 0) const;
-	static int maxdepth(const b3DbvtNode* node);
-	static int countLeaves(const b3DbvtNode* node);
-	static void extractLeaves(const b3DbvtNode* node, b3AlignedObjectArray<const b3DbvtNode*>& leaves);
-#if B3_DBVT_ENABLE_BENCHMARK
-	static void benchmark();
-#else
-	static void benchmark()
-	{
-	}
-#endif
-	// B3_DBVT_IPOLICY must support ICollide policy/interface
-	B3_DBVT_PREFIX
-	static void enumNodes(const b3DbvtNode* root,
-						  B3_DBVT_IPOLICY);
-	B3_DBVT_PREFIX
-	static void enumLeaves(const b3DbvtNode* root,
-						   B3_DBVT_IPOLICY);
-	B3_DBVT_PREFIX
-	void collideTT(const b3DbvtNode* root0,
-				   const b3DbvtNode* root1,
-				   B3_DBVT_IPOLICY);
-
-	B3_DBVT_PREFIX
-	void collideTTpersistentStack(const b3DbvtNode* root0,
-								  const b3DbvtNode* root1,
-								  B3_DBVT_IPOLICY);
-#if 0
-	B3_DBVT_PREFIX
-		void		collideTT(	const b3DbvtNode* root0,
-		const b3DbvtNode* root1,
-		const b3Transform& xform,
-		B3_DBVT_IPOLICY);
-	B3_DBVT_PREFIX
-		void		collideTT(	const b3DbvtNode* root0,
-		const b3Transform& xform0,
-		const b3DbvtNode* root1,
-		const b3Transform& xform1,
-		B3_DBVT_IPOLICY);
-#endif
-
-	B3_DBVT_PREFIX
-	void collideTV(const b3DbvtNode* root,
-				   const b3DbvtVolume& volume,
-				   B3_DBVT_IPOLICY) const;
-	///rayTest is a re-entrant ray test, and can be called in parallel as long as the b3AlignedAlloc is thread-safe (uses locking etc)
-	///rayTest is slower than rayTestInternal, because it builds a local stack, using memory allocations, and it recomputes signs/rayDirectionInverses each time
-	B3_DBVT_PREFIX
-	static void rayTest(const b3DbvtNode* root,
-						const b3Vector3& rayFrom,
-						const b3Vector3& rayTo,
-						B3_DBVT_IPOLICY);
-	///rayTestInternal is faster than rayTest, because it uses a persistent stack (to reduce dynamic memory allocations to a minimum) and it uses precomputed signs/rayInverseDirections
-	///rayTestInternal is used by b3DynamicBvhBroadphase to accelerate world ray casts
-	B3_DBVT_PREFIX
-	void rayTestInternal(const b3DbvtNode* root,
-						 const b3Vector3& rayFrom,
-						 const b3Vector3& rayTo,
-						 const b3Vector3& rayDirectionInverse,
-						 unsigned int signs[3],
-						 b3Scalar lambda_max,
-						 const b3Vector3& aabbMin,
-						 const b3Vector3& aabbMax,
-						 B3_DBVT_IPOLICY) const;
-
-	B3_DBVT_PREFIX
-	static void collideKDOP(const b3DbvtNode* root,
-							const b3Vector3* normals,
-							const b3Scalar* offsets,
-							int count,
-							B3_DBVT_IPOLICY);
-	B3_DBVT_PREFIX
-	static void collideOCL(const b3DbvtNode* root,
-						   const b3Vector3* normals,
-						   const b3Scalar* offsets,
-						   const b3Vector3& sortaxis,
-						   int count,
-						   B3_DBVT_IPOLICY,
-						   bool fullsort = true);
-	B3_DBVT_PREFIX
-	static void collideTU(const b3DbvtNode* root,
-						  B3_DBVT_IPOLICY);
-	// Helpers
-	static B3_DBVT_INLINE int nearest(const int* i, const b3DynamicBvh::sStkNPS* a, b3Scalar v, int l, int h)
-	{
-		int m = 0;
-		while (l < h)
-		{
-			m = (l + h) >> 1;
-			if (a[i[m]].value >= v)
-				l = m + 1;
-			else
-				h = m;
-		}
-		return (h);
-	}
-	static B3_DBVT_INLINE int allocate(b3AlignedObjectArray<int>& ifree,
-									   b3AlignedObjectArray<sStkNPS>& stock,
-									   const sStkNPS& value)
-	{
-		int i;
-		if (ifree.size() > 0)
-		{
-			i = ifree[ifree.size() - 1];
-			ifree.pop_back();
-			stock[i] = value;
-		}
-		else
-		{
-			i = stock.size();
-			stock.push_back(value);
-		}
-		return (i);
-	}
-	//
-private:
-	b3DynamicBvh(const b3DynamicBvh&) {}
-};
-
-//
-// Inline's
-//
-
-//
-inline b3DbvtAabbMm b3DbvtAabbMm::FromCE(const b3Vector3& c, const b3Vector3& e)
-{
-	b3DbvtAabbMm box;
-	box.mi = c - e;
-	box.mx = c + e;
-	return (box);
-}
-
-//
-inline b3DbvtAabbMm b3DbvtAabbMm::FromCR(const b3Vector3& c, b3Scalar r)
-{
-	return (FromCE(c, b3MakeVector3(r, r, r)));
-}
-
-//
-inline b3DbvtAabbMm b3DbvtAabbMm::FromMM(const b3Vector3& mi, const b3Vector3& mx)
-{
-	b3DbvtAabbMm box;
-	box.mi = mi;
-	box.mx = mx;
-	return (box);
-}
-
-//
-inline b3DbvtAabbMm b3DbvtAabbMm::FromPoints(const b3Vector3* pts, int n)
-{
-	b3DbvtAabbMm box;
-	box.mi = box.mx = pts[0];
-	for (int i = 1; i < n; ++i)
-	{
-		box.mi.setMin(pts[i]);
-		box.mx.setMax(pts[i]);
-	}
-	return (box);
-}
-
-//
-inline b3DbvtAabbMm b3DbvtAabbMm::FromPoints(const b3Vector3** ppts, int n)
-{
-	b3DbvtAabbMm box;
-	box.mi = box.mx = *ppts[0];
-	for (int i = 1; i < n; ++i)
-	{
-		box.mi.setMin(*ppts[i]);
-		box.mx.setMax(*ppts[i]);
-	}
-	return (box);
-}
-
-//
-B3_DBVT_INLINE void b3DbvtAabbMm::Expand(const b3Vector3& e)
-{
-	mi -= e;
-	mx += e;
-}
-
-//
-B3_DBVT_INLINE void b3DbvtAabbMm::SignedExpand(const b3Vector3& e)
-{
-	if (e.x > 0)
-		mx.setX(mx.x + e[0]);
-	else
-		mi.setX(mi.x + e[0]);
-	if (e.y > 0)
-		mx.setY(mx.y + e[1]);
-	else
-		mi.setY(mi.y + e[1]);
-	if (e.z > 0)
-		mx.setZ(mx.z + e[2]);
-	else
-		mi.setZ(mi.z + e[2]);
-}
-
-//
-B3_DBVT_INLINE bool b3DbvtAabbMm::Contain(const b3DbvtAabbMm& a) const
-{
-	return ((mi.x <= a.mi.x) &&
-			(mi.y <= a.mi.y) &&
-			(mi.z <= a.mi.z) &&
-			(mx.x >= a.mx.x) &&
-			(mx.y >= a.mx.y) &&
-			(mx.z >= a.mx.z));
-}
-
-//
-B3_DBVT_INLINE int b3DbvtAabbMm::Classify(const b3Vector3& n, b3Scalar o, int s) const
-{
-	b3Vector3 pi, px;
-	switch (s)
-	{
-		case (0 + 0 + 0):
-			px = b3MakeVector3(mi.x, mi.y, mi.z);
-			pi = b3MakeVector3(mx.x, mx.y, mx.z);
-			break;
-		case (1 + 0 + 0):
-			px = b3MakeVector3(mx.x, mi.y, mi.z);
-			pi = b3MakeVector3(mi.x, mx.y, mx.z);
-			break;
-		case (0 + 2 + 0):
-			px = b3MakeVector3(mi.x, mx.y, mi.z);
-			pi = b3MakeVector3(mx.x, mi.y, mx.z);
-			break;
-		case (1 + 2 + 0):
-			px = b3MakeVector3(mx.x, mx.y, mi.z);
-			pi = b3MakeVector3(mi.x, mi.y, mx.z);
-			break;
-		case (0 + 0 + 4):
-			px = b3MakeVector3(mi.x, mi.y, mx.z);
-			pi = b3MakeVector3(mx.x, mx.y, mi.z);
-			break;
-		case (1 + 0 + 4):
-			px = b3MakeVector3(mx.x, mi.y, mx.z);
-			pi = b3MakeVector3(mi.x, mx.y, mi.z);
-			break;
-		case (0 + 2 + 4):
-			px = b3MakeVector3(mi.x, mx.y, mx.z);
-			pi = b3MakeVector3(mx.x, mi.y, mi.z);
-			break;
-		case (1 + 2 + 4):
-			px = b3MakeVector3(mx.x, mx.y, mx.z);
-			pi = b3MakeVector3(mi.x, mi.y, mi.z);
-			break;
-	}
-	if ((b3Dot(n, px) + o) < 0) return (-1);
-	if ((b3Dot(n, pi) + o) >= 0) return (+1);
-	return (0);
-}
-
-//
-B3_DBVT_INLINE b3Scalar b3DbvtAabbMm::ProjectMinimum(const b3Vector3& v, unsigned signs) const
-{
-	const b3Vector3* b[] = {&mx, &mi};
-	const b3Vector3 p = b3MakeVector3(b[(signs >> 0) & 1]->x,
-									  b[(signs >> 1) & 1]->y,
-									  b[(signs >> 2) & 1]->z);
-	return (b3Dot(p, v));
-}
-
-//
-B3_DBVT_INLINE void b3DbvtAabbMm::AddSpan(const b3Vector3& d, b3Scalar& smi, b3Scalar& smx) const
-{
-	for (int i = 0; i < 3; ++i)
-	{
-		if (d[i] < 0)
-		{
-			smi += mx[i] * d[i];
-			smx += mi[i] * d[i];
-		}
-		else
-		{
-			smi += mi[i] * d[i];
-			smx += mx[i] * d[i];
-		}
-	}
-}
-
-//
-B3_DBVT_INLINE bool b3Intersect(const b3DbvtAabbMm& a,
-								const b3DbvtAabbMm& b)
-{
-#if B3_DBVT_INT0_IMPL == B3_DBVT_IMPL_SSE
-	const __m128 rt(_mm_or_ps(_mm_cmplt_ps(_mm_load_ps(b.mx), _mm_load_ps(a.mi)),
-							  _mm_cmplt_ps(_mm_load_ps(a.mx), _mm_load_ps(b.mi))));
-#if defined(_WIN32)
-	const __int32* pu((const __int32*)&rt);
-#else
-	const int* pu((const int*)&rt);
-#endif
-	return ((pu[0] | pu[1] | pu[2]) == 0);
-#else
-	return ((a.mi.x <= b.mx.x) &&
-			(a.mx.x >= b.mi.x) &&
-			(a.mi.y <= b.mx.y) &&
-			(a.mx.y >= b.mi.y) &&
-			(a.mi.z <= b.mx.z) &&
-			(a.mx.z >= b.mi.z));
-#endif
-}
-
-//
-B3_DBVT_INLINE bool b3Intersect(const b3DbvtAabbMm& a,
-								const b3Vector3& b)
-{
-	return ((b.x >= a.mi.x) &&
-			(b.y >= a.mi.y) &&
-			(b.z >= a.mi.z) &&
-			(b.x <= a.mx.x) &&
-			(b.y <= a.mx.y) &&
-			(b.z <= a.mx.z));
-}
-
-//////////////////////////////////////
-
-//
-B3_DBVT_INLINE b3Scalar b3Proximity(const b3DbvtAabbMm& a,
-									const b3DbvtAabbMm& b)
-{
-	const b3Vector3 d = (a.mi + a.mx) - (b.mi + b.mx);
-	return (b3Fabs(d.x) + b3Fabs(d.y) + b3Fabs(d.z));
-}
-
-//
-B3_DBVT_INLINE int b3Select(const b3DbvtAabbMm& o,
-							const b3DbvtAabbMm& a,
-							const b3DbvtAabbMm& b)
-{
-#if B3_DBVT_SELECT_IMPL == B3_DBVT_IMPL_SSE
-
-#if defined(_WIN32)
-	static B3_ATTRIBUTE_ALIGNED16(const unsigned __int32) mask[] = {0x7fffffff, 0x7fffffff, 0x7fffffff, 0x7fffffff};
-#else
-	static B3_ATTRIBUTE_ALIGNED16(const unsigned int) mask[] = {0x7fffffff, 0x7fffffff, 0x7fffffff, 0x00000000 /*0x7fffffff*/};
-#endif
-	///@todo: the intrinsic version is 11% slower
-#if B3_DBVT_USE_INTRINSIC_SSE
-
-	union b3SSEUnion  ///NOTE: if we use more intrinsics, move b3SSEUnion into the LinearMath directory
-	{
-		__m128 ssereg;
-		float floats[4];
-		int ints[4];
-	};
-
-	__m128 omi(_mm_load_ps(o.mi));
-	omi = _mm_add_ps(omi, _mm_load_ps(o.mx));
-	__m128 ami(_mm_load_ps(a.mi));
-	ami = _mm_add_ps(ami, _mm_load_ps(a.mx));
-	ami = _mm_sub_ps(ami, omi);
-	ami = _mm_and_ps(ami, _mm_load_ps((const float*)mask));
-	__m128 bmi(_mm_load_ps(b.mi));
-	bmi = _mm_add_ps(bmi, _mm_load_ps(b.mx));
-	bmi = _mm_sub_ps(bmi, omi);
-	bmi = _mm_and_ps(bmi, _mm_load_ps((const float*)mask));
-	__m128 t0(_mm_movehl_ps(ami, ami));
-	ami = _mm_add_ps(ami, t0);
-	ami = _mm_add_ss(ami, _mm_shuffle_ps(ami, ami, 1));
-	__m128 t1(_mm_movehl_ps(bmi, bmi));
-	bmi = _mm_add_ps(bmi, t1);
-	bmi = _mm_add_ss(bmi, _mm_shuffle_ps(bmi, bmi, 1));
-
-	b3SSEUnion tmp;
-	tmp.ssereg = _mm_cmple_ss(bmi, ami);
-	return tmp.ints[0] & 1;
-
-#else
-	B3_ATTRIBUTE_ALIGNED16(__int32 r[1]);
-	__asm
-	{
-		mov		eax,o
-			mov		ecx,a
-			mov		edx,b
-			movaps	xmm0,[eax]
-		movaps	xmm5,mask
-			addps	xmm0,[eax+16]	
-		movaps	xmm1,[ecx]
-		movaps	xmm2,[edx]
-		addps	xmm1,[ecx+16]
-		addps	xmm2,[edx+16]
-		subps	xmm1,xmm0
-			subps	xmm2,xmm0
-			andps	xmm1,xmm5
-			andps	xmm2,xmm5
-			movhlps	xmm3,xmm1
-			movhlps	xmm4,xmm2
-			addps	xmm1,xmm3
-			addps	xmm2,xmm4
-			pshufd	xmm3,xmm1,1
-			pshufd	xmm4,xmm2,1
-			addss	xmm1,xmm3
-			addss	xmm2,xmm4
-			cmpless	xmm2,xmm1
-			movss	r,xmm2
-	}
-	return (r[0] & 1);
-#endif
-#else
-	return (b3Proximity(o, a) < b3Proximity(o, b) ? 0 : 1);
-#endif
-}
-
-//
-B3_DBVT_INLINE void b3Merge(const b3DbvtAabbMm& a,
-							const b3DbvtAabbMm& b,
-							b3DbvtAabbMm& r)
-{
-#if B3_DBVT_MERGE_IMPL == B3_DBVT_IMPL_SSE
-	__m128 ami(_mm_load_ps(a.mi));
-	__m128 amx(_mm_load_ps(a.mx));
-	__m128 bmi(_mm_load_ps(b.mi));
-	__m128 bmx(_mm_load_ps(b.mx));
-	ami = _mm_min_ps(ami, bmi);
-	amx = _mm_max_ps(amx, bmx);
-	_mm_store_ps(r.mi, ami);
-	_mm_store_ps(r.mx, amx);
-#else
-	for (int i = 0; i < 3; ++i)
-	{
-		if (a.mi[i] < b.mi[i])
-			r.mi[i] = a.mi[i];
-		else
-			r.mi[i] = b.mi[i];
-		if (a.mx[i] > b.mx[i])
-			r.mx[i] = a.mx[i];
-		else
-			r.mx[i] = b.mx[i];
-	}
-#endif
-}
-
-//
-B3_DBVT_INLINE bool b3NotEqual(const b3DbvtAabbMm& a,
-							   const b3DbvtAabbMm& b)
-{
-	return ((a.mi.x != b.mi.x) ||
-			(a.mi.y != b.mi.y) ||
-			(a.mi.z != b.mi.z) ||
-			(a.mx.x != b.mx.x) ||
-			(a.mx.y != b.mx.y) ||
-			(a.mx.z != b.mx.z));
-}
-
-//
-// Inline's
-//
-
-//
-B3_DBVT_PREFIX
-inline void b3DynamicBvh::enumNodes(const b3DbvtNode* root,
-									B3_DBVT_IPOLICY)
-{
-	B3_DBVT_CHECKTYPE
-	policy.Process(root);
-	if (root->isinternal())
-	{
-		enumNodes(root->childs[0], policy);
-		enumNodes(root->childs[1], policy);
-	}
-}
-
-//
-B3_DBVT_PREFIX
-inline void b3DynamicBvh::enumLeaves(const b3DbvtNode* root,
-									 B3_DBVT_IPOLICY)
-{
-	B3_DBVT_CHECKTYPE
-	if (root->isinternal())
-	{
-		enumLeaves(root->childs[0], policy);
-		enumLeaves(root->childs[1], policy);
-	}
-	else
-	{
-		policy.Process(root);
-	}
-}
-
-//
-B3_DBVT_PREFIX
-inline void b3DynamicBvh::collideTT(const b3DbvtNode* root0,
-									const b3DbvtNode* root1,
-									B3_DBVT_IPOLICY)
-{
-	B3_DBVT_CHECKTYPE
-	if (root0 && root1)
-	{
-		int depth = 1;
-		int treshold = B3_DOUBLE_STACKSIZE - 4;
-		b3AlignedObjectArray<sStkNN> stkStack;
-		stkStack.resize(B3_DOUBLE_STACKSIZE);
-		stkStack[0] = sStkNN(root0, root1);
-		do
-		{
-			sStkNN p = stkStack[--depth];
-			if (depth > treshold)
-			{
-				stkStack.resize(stkStack.size() * 2);
-				treshold = stkStack.size() - 4;
-			}
-			if (p.a == p.b)
-			{
-				if (p.a->isinternal())
-				{
-					stkStack[depth++] = sStkNN(p.a->childs[0], p.a->childs[0]);
-					stkStack[depth++] = sStkNN(p.a->childs[1], p.a->childs[1]);
-					stkStack[depth++] = sStkNN(p.a->childs[0], p.a->childs[1]);
-				}
-			}
-			else if (b3Intersect(p.a->volume, p.b->volume))
-			{
-				if (p.a->isinternal())
-				{
-					if (p.b->isinternal())
-					{
-						stkStack[depth++] = sStkNN(p.a->childs[0], p.b->childs[0]);
-						stkStack[depth++] = sStkNN(p.a->childs[1], p.b->childs[0]);
-						stkStack[depth++] = sStkNN(p.a->childs[0], p.b->childs[1]);
-						stkStack[depth++] = sStkNN(p.a->childs[1], p.b->childs[1]);
-					}
-					else
-					{
-						stkStack[depth++] = sStkNN(p.a->childs[0], p.b);
-						stkStack[depth++] = sStkNN(p.a->childs[1], p.b);
-					}
-				}
-				else
-				{
-					if (p.b->isinternal())
-					{
-						stkStack[depth++] = sStkNN(p.a, p.b->childs[0]);
-						stkStack[depth++] = sStkNN(p.a, p.b->childs[1]);
-					}
-					else
-					{
-						policy.Process(p.a, p.b);
-					}
-				}
-			}
-		} while (depth);
-	}
-}
-
-B3_DBVT_PREFIX
-inline void b3DynamicBvh::collideTTpersistentStack(const b3DbvtNode* root0,
-												   const b3DbvtNode* root1,
-												   B3_DBVT_IPOLICY)
-{
-	B3_DBVT_CHECKTYPE
-	if (root0 && root1)
-	{
-		int depth = 1;
-		int treshold = B3_DOUBLE_STACKSIZE - 4;
-
-		m_stkStack.resize(B3_DOUBLE_STACKSIZE);
-		m_stkStack[0] = sStkNN(root0, root1);
-		do
-		{
-			sStkNN p = m_stkStack[--depth];
-			if (depth > treshold)
-			{
-				m_stkStack.resize(m_stkStack.size() * 2);
-				treshold = m_stkStack.size() - 4;
-			}
-			if (p.a == p.b)
-			{
-				if (p.a->isinternal())
-				{
-					m_stkStack[depth++] = sStkNN(p.a->childs[0], p.a->childs[0]);
-					m_stkStack[depth++] = sStkNN(p.a->childs[1], p.a->childs[1]);
-					m_stkStack[depth++] = sStkNN(p.a->childs[0], p.a->childs[1]);
-				}
-			}
-			else if (b3Intersect(p.a->volume, p.b->volume))
-			{
-				if (p.a->isinternal())
-				{
-					if (p.b->isinternal())
-					{
-						m_stkStack[depth++] = sStkNN(p.a->childs[0], p.b->childs[0]);
-						m_stkStack[depth++] = sStkNN(p.a->childs[1], p.b->childs[0]);
-						m_stkStack[depth++] = sStkNN(p.a->childs[0], p.b->childs[1]);
-						m_stkStack[depth++] = sStkNN(p.a->childs[1], p.b->childs[1]);
-					}
-					else
-					{
-						m_stkStack[depth++] = sStkNN(p.a->childs[0], p.b);
-						m_stkStack[depth++] = sStkNN(p.a->childs[1], p.b);
-					}
-				}
-				else
-				{
-					if (p.b->isinternal())
-					{
-						m_stkStack[depth++] = sStkNN(p.a, p.b->childs[0]);
-						m_stkStack[depth++] = sStkNN(p.a, p.b->childs[1]);
-					}
-					else
-					{
-						policy.Process(p.a, p.b);
-					}
-				}
-			}
-		} while (depth);
-	}
-}
-
-#if 0
-//
-B3_DBVT_PREFIX
-inline void		b3DynamicBvh::collideTT(	const b3DbvtNode* root0,
-								  const b3DbvtNode* root1,
-								  const b3Transform& xform,
-								  B3_DBVT_IPOLICY)
-{
-	B3_DBVT_CHECKTYPE
-		if(root0&&root1)
-		{
-			int								depth=1;
-			int								treshold=B3_DOUBLE_STACKSIZE-4;
-			b3AlignedObjectArray<sStkNN>	stkStack;
-			stkStack.resize(B3_DOUBLE_STACKSIZE);
-			stkStack[0]=sStkNN(root0,root1);
-			do	{
-				sStkNN	p=stkStack[--depth];
-				if(b3Intersect(p.a->volume,p.b->volume,xform))
-				{
-					if(depth>treshold)
-					{
-						stkStack.resize(stkStack.size()*2);
-						treshold=stkStack.size()-4;
-					}
-					if(p.a->isinternal())
-					{
-						if(p.b->isinternal())
-						{					
-							stkStack[depth++]=sStkNN(p.a->childs[0],p.b->childs[0]);
-							stkStack[depth++]=sStkNN(p.a->childs[1],p.b->childs[0]);
-							stkStack[depth++]=sStkNN(p.a->childs[0],p.b->childs[1]);
-							stkStack[depth++]=sStkNN(p.a->childs[1],p.b->childs[1]);
-						}
-						else
-						{
-							stkStack[depth++]=sStkNN(p.a->childs[0],p.b);
-							stkStack[depth++]=sStkNN(p.a->childs[1],p.b);
-						}
-					}
-					else
-					{
-						if(p.b->isinternal())
-						{
-							stkStack[depth++]=sStkNN(p.a,p.b->childs[0]);
-							stkStack[depth++]=sStkNN(p.a,p.b->childs[1]);
-						}
-						else
-						{
-							policy.Process(p.a,p.b);
-						}
-					}
-				}
-			} while(depth);
-		}
-}
-//
-B3_DBVT_PREFIX
-inline void		b3DynamicBvh::collideTT(	const b3DbvtNode* root0,
-								  const b3Transform& xform0,
-								  const b3DbvtNode* root1,
-								  const b3Transform& xform1,
-								  B3_DBVT_IPOLICY)
-{
-	const b3Transform	xform=xform0.inverse()*xform1;
-	collideTT(root0,root1,xform,policy);
-}
-#endif
-
-//
-B3_DBVT_PREFIX
-inline void b3DynamicBvh::collideTV(const b3DbvtNode* root,
-									const b3DbvtVolume& vol,
-									B3_DBVT_IPOLICY) const
-{
-	B3_DBVT_CHECKTYPE
-	if (root)
-	{
-		B3_ATTRIBUTE_ALIGNED16(b3DbvtVolume)
-		volume(vol);
-		b3AlignedObjectArray<const b3DbvtNode*> stack;
-		stack.resize(0);
-		stack.reserve(B3_SIMPLE_STACKSIZE);
-		stack.push_back(root);
-		do
-		{
-			const b3DbvtNode* n = stack[stack.size() - 1];
-			stack.pop_back();
-			if (b3Intersect(n->volume, volume))
-			{
-				if (n->isinternal())
-				{
-					stack.push_back(n->childs[0]);
-					stack.push_back(n->childs[1]);
-				}
-				else
-				{
-					policy.Process(n);
-				}
-			}
-		} while (stack.size() > 0);
-	}
-}
-
-B3_DBVT_PREFIX
-inline void b3DynamicBvh::rayTestInternal(const b3DbvtNode* root,
-										  const b3Vector3& rayFrom,
-										  const b3Vector3& rayTo,
-										  const b3Vector3& rayDirectionInverse,
-										  unsigned int signs[3],
-										  b3Scalar lambda_max,
-										  const b3Vector3& aabbMin,
-										  const b3Vector3& aabbMax,
-										  B3_DBVT_IPOLICY) const
-{
-	(void)rayTo;
-	B3_DBVT_CHECKTYPE
-	if (root)
-	{
-		int depth = 1;
-		int treshold = B3_DOUBLE_STACKSIZE - 2;
-		b3AlignedObjectArray<const b3DbvtNode*>& stack = m_rayTestStack;
-		stack.resize(B3_DOUBLE_STACKSIZE);
-		stack[0] = root;
-		b3Vector3 bounds[2];
-		do
-		{
-			const b3DbvtNode* node = stack[--depth];
-			bounds[0] = node->volume.Mins() - aabbMax;
-			bounds[1] = node->volume.Maxs() - aabbMin;
-			b3Scalar tmin = 1.f, lambda_min = 0.f;
-			unsigned int result1 = false;
-			result1 = b3RayAabb2(rayFrom, rayDirectionInverse, signs, bounds, tmin, lambda_min, lambda_max);
-			if (result1)
-			{
-				if (node->isinternal())
-				{
-					if (depth > treshold)
-					{
-						stack.resize(stack.size() * 2);
-						treshold = stack.size() - 2;
-					}
-					stack[depth++] = node->childs[0];
-					stack[depth++] = node->childs[1];
-				}
-				else
-				{
-					policy.Process(node);
-				}
-			}
-		} while (depth);
-	}
-}
-
-//
-B3_DBVT_PREFIX
-inline void b3DynamicBvh::rayTest(const b3DbvtNode* root,
-								  const b3Vector3& rayFrom,
-								  const b3Vector3& rayTo,
-								  B3_DBVT_IPOLICY)
-{
-	B3_DBVT_CHECKTYPE
-	if (root)
-	{
-		b3Vector3 rayDir = (rayTo - rayFrom);
-		rayDir.normalize();
-
-		///what about division by zero? --> just set rayDirection[i] to INF/B3_LARGE_FLOAT
-		b3Vector3 rayDirectionInverse;
-		rayDirectionInverse[0] = rayDir[0] == b3Scalar(0.0) ? b3Scalar(B3_LARGE_FLOAT) : b3Scalar(1.0) / rayDir[0];
-		rayDirectionInverse[1] = rayDir[1] == b3Scalar(0.0) ? b3Scalar(B3_LARGE_FLOAT) : b3Scalar(1.0) / rayDir[1];
-		rayDirectionInverse[2] = rayDir[2] == b3Scalar(0.0) ? b3Scalar(B3_LARGE_FLOAT) : b3Scalar(1.0) / rayDir[2];
-		unsigned int signs[3] = {rayDirectionInverse[0] < 0.0, rayDirectionInverse[1] < 0.0, rayDirectionInverse[2] < 0.0};
-
-		b3Scalar lambda_max = rayDir.dot(rayTo - rayFrom);
-#ifdef COMPARE_BTRAY_AABB2
-		b3Vector3 resultNormal;
-#endif  //COMPARE_BTRAY_AABB2
-
-		b3AlignedObjectArray<const b3DbvtNode*> stack;
-
-		int depth = 1;
-		int treshold = B3_DOUBLE_STACKSIZE - 2;
-
-		stack.resize(B3_DOUBLE_STACKSIZE);
-		stack[0] = root;
-		b3Vector3 bounds[2];
-		do
-		{
-			const b3DbvtNode* node = stack[--depth];
-
-			bounds[0] = node->volume.Mins();
-			bounds[1] = node->volume.Maxs();
-
-			b3Scalar tmin = 1.f, lambda_min = 0.f;
-			unsigned int result1 = b3RayAabb2(rayFrom, rayDirectionInverse, signs, bounds, tmin, lambda_min, lambda_max);
-
-#ifdef COMPARE_BTRAY_AABB2
-			b3Scalar param = 1.f;
-			bool result2 = b3RayAabb(rayFrom, rayTo, node->volume.Mins(), node->volume.Maxs(), param, resultNormal);
-			b3Assert(result1 == result2);
-#endif  //TEST_BTRAY_AABB2
-
-			if (result1)
-			{
-				if (node->isinternal())
-				{
-					if (depth > treshold)
-					{
-						stack.resize(stack.size() * 2);
-						treshold = stack.size() - 2;
-					}
-					stack[depth++] = node->childs[0];
-					stack[depth++] = node->childs[1];
-				}
-				else
-				{
-					policy.Process(node);
-				}
-			}
-		} while (depth);
-	}
-}
-
-//
-B3_DBVT_PREFIX
-inline void b3DynamicBvh::collideKDOP(const b3DbvtNode* root,
-									  const b3Vector3* normals,
-									  const b3Scalar* offsets,
-									  int count,
-									  B3_DBVT_IPOLICY)
-{
-	B3_DBVT_CHECKTYPE
-	if (root)
-	{
-		const int inside = (1 << count) - 1;
-		b3AlignedObjectArray<sStkNP> stack;
-		int signs[sizeof(unsigned) * 8];
-		b3Assert(count < int(sizeof(signs) / sizeof(signs[0])));
-		for (int i = 0; i < count; ++i)
-		{
-			signs[i] = ((normals[i].x >= 0) ? 1 : 0) +
-					   ((normals[i].y >= 0) ? 2 : 0) +
-					   ((normals[i].z >= 0) ? 4 : 0);
-		}
-		stack.reserve(B3_SIMPLE_STACKSIZE);
-		stack.push_back(sStkNP(root, 0));
-		do
-		{
-			sStkNP se = stack[stack.size() - 1];
-			bool out = false;
-			stack.pop_back();
-			for (int i = 0, j = 1; (!out) && (i < count); ++i, j <<= 1)
-			{
-				if (0 == (se.mask & j))
-				{
-					const int side = se.node->volume.Classify(normals[i], offsets[i], signs[i]);
-					switch (side)
-					{
-						case -1:
-							out = true;
-							break;
-						case +1:
-							se.mask |= j;
-							break;
-					}
-				}
-			}
-			if (!out)
-			{
-				if ((se.mask != inside) && (se.node->isinternal()))
-				{
-					stack.push_back(sStkNP(se.node->childs[0], se.mask));
-					stack.push_back(sStkNP(se.node->childs[1], se.mask));
-				}
-				else
-				{
-					if (policy.AllLeaves(se.node)) enumLeaves(se.node, policy);
-				}
-			}
-		} while (stack.size());
-	}
-}
-
-//
-B3_DBVT_PREFIX
-inline void b3DynamicBvh::collideOCL(const b3DbvtNode* root,
-									 const b3Vector3* normals,
-									 const b3Scalar* offsets,
-									 const b3Vector3& sortaxis,
-									 int count,
-									 B3_DBVT_IPOLICY,
-									 bool fsort)
-{
-	B3_DBVT_CHECKTYPE
-	if (root)
-	{
-		const unsigned srtsgns = (sortaxis[0] >= 0 ? 1 : 0) +
-								 (sortaxis[1] >= 0 ? 2 : 0) +
-								 (sortaxis[2] >= 0 ? 4 : 0);
-		const int inside = (1 << count) - 1;
-		b3AlignedObjectArray<sStkNPS> stock;
-		b3AlignedObjectArray<int> ifree;
-		b3AlignedObjectArray<int> stack;
-		int signs[sizeof(unsigned) * 8];
-		b3Assert(count < int(sizeof(signs) / sizeof(signs[0])));
-		for (int i = 0; i < count; ++i)
-		{
-			signs[i] = ((normals[i].x >= 0) ? 1 : 0) +
-					   ((normals[i].y >= 0) ? 2 : 0) +
-					   ((normals[i].z >= 0) ? 4 : 0);
-		}
-		stock.reserve(B3_SIMPLE_STACKSIZE);
-		stack.reserve(B3_SIMPLE_STACKSIZE);
-		ifree.reserve(B3_SIMPLE_STACKSIZE);
-		stack.push_back(allocate(ifree, stock, sStkNPS(root, 0, root->volume.ProjectMinimum(sortaxis, srtsgns))));
-		do
-		{
-			const int id = stack[stack.size() - 1];
-			sStkNPS se = stock[id];
-			stack.pop_back();
-			ifree.push_back(id);
-			if (se.mask != inside)
-			{
-				bool out = false;
-				for (int i = 0, j = 1; (!out) && (i < count); ++i, j <<= 1)
-				{
-					if (0 == (se.mask & j))
-					{
-						const int side = se.node->volume.Classify(normals[i], offsets[i], signs[i]);
-						switch (side)
-						{
-							case -1:
-								out = true;
-								break;
-							case +1:
-								se.mask |= j;
-								break;
-						}
-					}
-				}
-				if (out) continue;
-			}
-			if (policy.Descent(se.node))
-			{
-				if (se.node->isinternal())
-				{
-					const b3DbvtNode* pns[] = {se.node->childs[0], se.node->childs[1]};
-					sStkNPS nes[] = {sStkNPS(pns[0], se.mask, pns[0]->volume.ProjectMinimum(sortaxis, srtsgns)),
-									 sStkNPS(pns[1], se.mask, pns[1]->volume.ProjectMinimum(sortaxis, srtsgns))};
-					const int q = nes[0].value < nes[1].value ? 1 : 0;
-					int j = stack.size();
-					if (fsort && (j > 0))
-					{
-						/* Insert 0	*/
-						j = nearest(&stack[0], &stock[0], nes[q].value, 0, stack.size());
-						stack.push_back(0);
-#if B3_DBVT_USE_MEMMOVE
-						memmove(&stack[j + 1], &stack[j], sizeof(int) * (stack.size() - j - 1));
-#else
-						for (int k = stack.size() - 1; k > j; --k) stack[k] = stack[k - 1];
-#endif
-						stack[j] = allocate(ifree, stock, nes[q]);
-						/* Insert 1	*/
-						j = nearest(&stack[0], &stock[0], nes[1 - q].value, j, stack.size());
-						stack.push_back(0);
-#if B3_DBVT_USE_MEMMOVE
-						memmove(&stack[j + 1], &stack[j], sizeof(int) * (stack.size() - j - 1));
-#else
-						for (int k = stack.size() - 1; k > j; --k) stack[k] = stack[k - 1];
-#endif
-						stack[j] = allocate(ifree, stock, nes[1 - q]);
-					}
-					else
-					{
-						stack.push_back(allocate(ifree, stock, nes[q]));
-						stack.push_back(allocate(ifree, stock, nes[1 - q]));
-					}
-				}
-				else
-				{
-					policy.Process(se.node, se.value);
-				}
-			}
-		} while (stack.size());
-	}
-}
-
-//
-B3_DBVT_PREFIX
-inline void b3DynamicBvh::collideTU(const b3DbvtNode* root,
-									B3_DBVT_IPOLICY)
-{
-	B3_DBVT_CHECKTYPE
-	if (root)
-	{
-		b3AlignedObjectArray<const b3DbvtNode*> stack;
-		stack.reserve(B3_SIMPLE_STACKSIZE);
-		stack.push_back(root);
-		do
-		{
-			const b3DbvtNode* n = stack[stack.size() - 1];
-			stack.pop_back();
-			if (policy.Descent(n))
-			{
-				if (n->isinternal())
-				{
-					stack.push_back(n->childs[0]);
-					stack.push_back(n->childs[1]);
-				}
-				else
-				{
-					policy.Process(n);
-				}
-			}
-		} while (stack.size() > 0);
-	}
-}
-
-//
-// PP Cleanup
-//
-
-#undef B3_DBVT_USE_MEMMOVE
-#undef B3_DBVT_USE_TEMPLATE
-#undef B3_DBVT_VIRTUAL_DTOR
-#undef B3_DBVT_VIRTUAL
-#undef B3_DBVT_PREFIX
-#undef B3_DBVT_IPOLICY
-#undef B3_DBVT_CHECKTYPE
-#undef B3_DBVT_IMPL_GENERIC
-#undef B3_DBVT_IMPL_SSE
-#undef B3_DBVT_USE_INTRINSIC_SSE
-#undef B3_DBVT_SELECT_IMPL
-#undef B3_DBVT_MERGE_IMPL
-#undef B3_DBVT_INT0_IMPL
-
-#endif

thirdparty/bullet/Bullet3Collision/BroadPhaseCollision/b3DynamicBvhBroadphase.cpp

@@ -1,808 +0,0 @@
-/*
-Bullet Continuous Collision Detection and Physics Library
-Copyright (c) 2003-2013 Erwin Coumans  http://bulletphysics.org
-
-This software is provided 'as-is', without any express or implied warranty.
-In no event will the authors be held liable for any damages arising from the use of this software.
-Permission is granted to anyone to use this software for any purpose,
-including commercial applications, and to alter it and redistribute it freely,
-subject to the following restrictions:
-
-1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
-2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
-3. This notice may not be removed or altered from any source distribution.
-*/
-
-///b3DynamicBvhBroadphase implementation by Nathanael Presson
-
-#include "b3DynamicBvhBroadphase.h"
-#include "b3OverlappingPair.h"
-
-//
-// Profiling
-//
-
-#if B3_DBVT_BP_PROFILE || B3_DBVT_BP_ENABLE_BENCHMARK
-#include <stdio.h>
-#endif
-
-#if B3_DBVT_BP_PROFILE
-struct b3ProfileScope
-{
-	__forceinline b3ProfileScope(b3Clock& clock, unsigned long& value) : m_clock(&clock), m_value(&value), m_base(clock.getTimeMicroseconds())
-	{
-	}
-	__forceinline ~b3ProfileScope()
-	{
-		(*m_value) += m_clock->getTimeMicroseconds() - m_base;
-	}
-	b3Clock* m_clock;
-	unsigned long* m_value;
-	unsigned long m_base;
-};
-#define b3SPC(_value_) b3ProfileScope spc_scope(m_clock, _value_)
-#else
-#define b3SPC(_value_)
-#endif
-
-//
-// Helpers
-//
-
-//
-template <typename T>
-static inline void b3ListAppend(T* item, T*& list)
-{
-	item->links[0] = 0;
-	item->links[1] = list;
-	if (list) list->links[0] = item;
-	list = item;
-}
-
-//
-template <typename T>
-static inline void b3ListRemove(T* item, T*& list)
-{
-	if (item->links[0])
-		item->links[0]->links[1] = item->links[1];
-	else
-		list = item->links[1];
-	if (item->links[1]) item->links[1]->links[0] = item->links[0];
-}
-
-//
-template <typename T>
-static inline int b3ListCount(T* root)
-{
-	int n = 0;
-	while (root)
-	{
-		++n;
-		root = root->links[1];
-	}
-	return (n);
-}
-
-//
-template <typename T>
-static inline void b3Clear(T& value)
-{
-	static const struct ZeroDummy : T
-	{
-	} zerodummy;
-	value = zerodummy;
-}
-
-//
-// Colliders
-//
-
-/* Tree collider	*/
-struct b3DbvtTreeCollider : b3DynamicBvh::ICollide
-{
-	b3DynamicBvhBroadphase* pbp;
-	b3DbvtProxy* proxy;
-	b3DbvtTreeCollider(b3DynamicBvhBroadphase* p) : pbp(p) {}
-	void Process(const b3DbvtNode* na, const b3DbvtNode* nb)
-	{
-		if (na != nb)
-		{
-			b3DbvtProxy* pa = (b3DbvtProxy*)na->data;
-			b3DbvtProxy* pb = (b3DbvtProxy*)nb->data;
-#if B3_DBVT_BP_SORTPAIRS
-			if (pa->m_uniqueId > pb->m_uniqueId)
-				b3Swap(pa, pb);
-#endif
-			pbp->m_paircache->addOverlappingPair(pa->getUid(), pb->getUid());
-			++pbp->m_newpairs;
-		}
-	}
-	void Process(const b3DbvtNode* n)
-	{
-		Process(n, proxy->leaf);
-	}
-};
-
-//
-// b3DynamicBvhBroadphase
-//
-
-//
-b3DynamicBvhBroadphase::b3DynamicBvhBroadphase(int proxyCapacity, b3OverlappingPairCache* paircache)
-{
-	m_deferedcollide = false;
-	m_needcleanup = true;
-	m_releasepaircache = (paircache != 0) ? false : true;
-	m_prediction = 0;
-	m_stageCurrent = 0;
-	m_fixedleft = 0;
-	m_fupdates = 1;
-	m_dupdates = 0;
-	m_cupdates = 10;
-	m_newpairs = 1;
-	m_updates_call = 0;
-	m_updates_done = 0;
-	m_updates_ratio = 0;
-	m_paircache = paircache ? paircache : new (b3AlignedAlloc(sizeof(b3HashedOverlappingPairCache), 16)) b3HashedOverlappingPairCache();
-
-	m_pid = 0;
-	m_cid = 0;
-	for (int i = 0; i <= STAGECOUNT; ++i)
-	{
-		m_stageRoots[i] = 0;
-	}
-#if B3_DBVT_BP_PROFILE
-	b3Clear(m_profiling);
-#endif
-	m_proxies.resize(proxyCapacity);
-}
-
-//
-b3DynamicBvhBroadphase::~b3DynamicBvhBroadphase()
-{
-	if (m_releasepaircache)
-	{
-		m_paircache->~b3OverlappingPairCache();
-		b3AlignedFree(m_paircache);
-	}
-}
-
-//
-b3BroadphaseProxy* b3DynamicBvhBroadphase::createProxy(const b3Vector3& aabbMin,
-													   const b3Vector3& aabbMax,
-													   int objectId,
-													   void* userPtr,
-													   int collisionFilterGroup,
-													   int collisionFilterMask)
-{
-	b3DbvtProxy* mem = &m_proxies[objectId];
-	b3DbvtProxy* proxy = new (mem) b3DbvtProxy(aabbMin, aabbMax, userPtr,
-											   collisionFilterGroup,
-											   collisionFilterMask);
-
-	b3DbvtAabbMm aabb = b3DbvtVolume::FromMM(aabbMin, aabbMax);
-
-	//bproxy->aabb			=	b3DbvtVolume::FromMM(aabbMin,aabbMax);
-	proxy->stage = m_stageCurrent;
-	proxy->m_uniqueId = objectId;
-	proxy->leaf = m_sets[0].insert(aabb, proxy);
-	b3ListAppend(proxy, m_stageRoots[m_stageCurrent]);
-	if (!m_deferedcollide)
-	{
-		b3DbvtTreeCollider collider(this);
-		collider.proxy = proxy;
-		m_sets[0].collideTV(m_sets[0].m_root, aabb, collider);
-		m_sets[1].collideTV(m_sets[1].m_root, aabb, collider);
-	}
-	return (proxy);
-}
-
-//
-void b3DynamicBvhBroadphase::destroyProxy(b3BroadphaseProxy* absproxy,
-										  b3Dispatcher* dispatcher)
-{
-	b3DbvtProxy* proxy = (b3DbvtProxy*)absproxy;
-	if (proxy->stage == STAGECOUNT)
-		m_sets[1].remove(proxy->leaf);
-	else
-		m_sets[0].remove(proxy->leaf);
-	b3ListRemove(proxy, m_stageRoots[proxy->stage]);
-	m_paircache->removeOverlappingPairsContainingProxy(proxy->getUid(), dispatcher);
-
-	m_needcleanup = true;
-}
-
-void b3DynamicBvhBroadphase::getAabb(int objectId, b3Vector3& aabbMin, b3Vector3& aabbMax) const
-{
-	const b3DbvtProxy* proxy = &m_proxies[objectId];
-	aabbMin = proxy->m_aabbMin;
-	aabbMax = proxy->m_aabbMax;
-}
-/*
-void	b3DynamicBvhBroadphase::getAabb(b3BroadphaseProxy* absproxy,b3Vector3& aabbMin, b3Vector3& aabbMax ) const
-{
-	b3DbvtProxy*						proxy=(b3DbvtProxy*)absproxy;
-	aabbMin = proxy->m_aabbMin;
-	aabbMax = proxy->m_aabbMax;
-}
-*/
-
-struct BroadphaseRayTester : b3DynamicBvh::ICollide
-{
-	b3BroadphaseRayCallback& m_rayCallback;
-	BroadphaseRayTester(b3BroadphaseRayCallback& orgCallback)
-		: m_rayCallback(orgCallback)
-	{
-	}
-	void Process(const b3DbvtNode* leaf)
-	{
-		b3DbvtProxy* proxy = (b3DbvtProxy*)leaf->data;
-		m_rayCallback.process(proxy);
-	}
-};
-
-void b3DynamicBvhBroadphase::rayTest(const b3Vector3& rayFrom, const b3Vector3& rayTo, b3BroadphaseRayCallback& rayCallback, const b3Vector3& aabbMin, const b3Vector3& aabbMax)
-{
-	BroadphaseRayTester callback(rayCallback);
-
-	m_sets[0].rayTestInternal(m_sets[0].m_root,
-							  rayFrom,
-							  rayTo,
-							  rayCallback.m_rayDirectionInverse,
-							  rayCallback.m_signs,
-							  rayCallback.m_lambda_max,
-							  aabbMin,
-							  aabbMax,
-							  callback);
-
-	m_sets[1].rayTestInternal(m_sets[1].m_root,
-							  rayFrom,
-							  rayTo,
-							  rayCallback.m_rayDirectionInverse,
-							  rayCallback.m_signs,
-							  rayCallback.m_lambda_max,
-							  aabbMin,
-							  aabbMax,
-							  callback);
-}
-
-struct BroadphaseAabbTester : b3DynamicBvh::ICollide
-{
-	b3BroadphaseAabbCallback& m_aabbCallback;
-	BroadphaseAabbTester(b3BroadphaseAabbCallback& orgCallback)
-		: m_aabbCallback(orgCallback)
-	{
-	}
-	void Process(const b3DbvtNode* leaf)
-	{
-		b3DbvtProxy* proxy = (b3DbvtProxy*)leaf->data;
-		m_aabbCallback.process(proxy);
-	}
-};
-
-void b3DynamicBvhBroadphase::aabbTest(const b3Vector3& aabbMin, const b3Vector3& aabbMax, b3BroadphaseAabbCallback& aabbCallback)
-{
-	BroadphaseAabbTester callback(aabbCallback);
-
-	const B3_ATTRIBUTE_ALIGNED16(b3DbvtVolume) bounds = b3DbvtVolume::FromMM(aabbMin, aabbMax);
-	//process all children, that overlap with  the given AABB bounds
-	m_sets[0].collideTV(m_sets[0].m_root, bounds, callback);
-	m_sets[1].collideTV(m_sets[1].m_root, bounds, callback);
-}
-
-//
-void b3DynamicBvhBroadphase::setAabb(int objectId,
-									 const b3Vector3& aabbMin,
-									 const b3Vector3& aabbMax,
-									 b3Dispatcher* /*dispatcher*/)
-{
-	b3DbvtProxy* proxy = &m_proxies[objectId];
-	//	b3DbvtProxy*						proxy=(b3DbvtProxy*)absproxy;
-	B3_ATTRIBUTE_ALIGNED16(b3DbvtVolume)
-	aabb = b3DbvtVolume::FromMM(aabbMin, aabbMax);
-#if B3_DBVT_BP_PREVENTFALSEUPDATE
-	if (b3NotEqual(aabb, proxy->leaf->volume))
-#endif
-	{
-		bool docollide = false;
-		if (proxy->stage == STAGECOUNT)
-		{ /* fixed -> dynamic set	*/
-			m_sets[1].remove(proxy->leaf);
-			proxy->leaf = m_sets[0].insert(aabb, proxy);
-			docollide = true;
-		}
-		else
-		{ /* dynamic set				*/
-			++m_updates_call;
-			if (b3Intersect(proxy->leaf->volume, aabb))
-			{ /* Moving				*/
-
-				const b3Vector3 delta = aabbMin - proxy->m_aabbMin;
-				b3Vector3 velocity(((proxy->m_aabbMax - proxy->m_aabbMin) / 2) * m_prediction);
-				if (delta[0] < 0) velocity[0] = -velocity[0];
-				if (delta[1] < 0) velocity[1] = -velocity[1];
-				if (delta[2] < 0) velocity[2] = -velocity[2];
-				if (
-#ifdef B3_DBVT_BP_MARGIN
-					m_sets[0].update(proxy->leaf, aabb, velocity, B3_DBVT_BP_MARGIN)
-#else
-					m_sets[0].update(proxy->leaf, aabb, velocity)
-#endif
-				)
-				{
-					++m_updates_done;
-					docollide = true;
-				}
-			}
-			else
-			{ /* Teleporting			*/
-				m_sets[0].update(proxy->leaf, aabb);
-				++m_updates_done;
-				docollide = true;
-			}
-		}
-		b3ListRemove(proxy, m_stageRoots[proxy->stage]);
-		proxy->m_aabbMin = aabbMin;
-		proxy->m_aabbMax = aabbMax;
-		proxy->stage = m_stageCurrent;
-		b3ListAppend(proxy, m_stageRoots[m_stageCurrent]);
-		if (docollide)
-		{
-			m_needcleanup = true;
-			if (!m_deferedcollide)
-			{
-				b3DbvtTreeCollider collider(this);
-				m_sets[1].collideTTpersistentStack(m_sets[1].m_root, proxy->leaf, collider);
-				m_sets[0].collideTTpersistentStack(m_sets[0].m_root, proxy->leaf, collider);
-			}
-		}
-	}
-}
-
-//
-void b3DynamicBvhBroadphase::setAabbForceUpdate(b3BroadphaseProxy* absproxy,
-												const b3Vector3& aabbMin,
-												const b3Vector3& aabbMax,
-												b3Dispatcher* /*dispatcher*/)
-{
-	b3DbvtProxy* proxy = (b3DbvtProxy*)absproxy;
-	B3_ATTRIBUTE_ALIGNED16(b3DbvtVolume)
-	aabb = b3DbvtVolume::FromMM(aabbMin, aabbMax);
-	bool docollide = false;
-	if (proxy->stage == STAGECOUNT)
-	{ /* fixed -> dynamic set	*/
-		m_sets[1].remove(proxy->leaf);
-		proxy->leaf = m_sets[0].insert(aabb, proxy);
-		docollide = true;
-	}
-	else
-	{ /* dynamic set				*/
-		++m_updates_call;
-		/* Teleporting			*/
-		m_sets[0].update(proxy->leaf, aabb);
-		++m_updates_done;
-		docollide = true;
-	}
-	b3ListRemove(proxy, m_stageRoots[proxy->stage]);
-	proxy->m_aabbMin = aabbMin;
-	proxy->m_aabbMax = aabbMax;
-	proxy->stage = m_stageCurrent;
-	b3ListAppend(proxy, m_stageRoots[m_stageCurrent]);
-	if (docollide)
-	{
-		m_needcleanup = true;
-		if (!m_deferedcollide)
-		{
-			b3DbvtTreeCollider collider(this);
-			m_sets[1].collideTTpersistentStack(m_sets[1].m_root, proxy->leaf, collider);
-			m_sets[0].collideTTpersistentStack(m_sets[0].m_root, proxy->leaf, collider);
-		}
-	}
-}
-
-//
-void b3DynamicBvhBroadphase::calculateOverlappingPairs(b3Dispatcher* dispatcher)
-{
-	collide(dispatcher);
-#if B3_DBVT_BP_PROFILE
-	if (0 == (m_pid % B3_DBVT_BP_PROFILING_RATE))
-	{
-		printf("fixed(%u) dynamics(%u) pairs(%u)\r\n", m_sets[1].m_leaves, m_sets[0].m_leaves, m_paircache->getNumOverlappingPairs());
-		unsigned int total = m_profiling.m_total;
-		if (total <= 0) total = 1;
-		printf("ddcollide: %u%% (%uus)\r\n", (50 + m_profiling.m_ddcollide * 100) / total, m_profiling.m_ddcollide / B3_DBVT_BP_PROFILING_RATE);
-		printf("fdcollide: %u%% (%uus)\r\n", (50 + m_profiling.m_fdcollide * 100) / total, m_profiling.m_fdcollide / B3_DBVT_BP_PROFILING_RATE);
-		printf("cleanup:   %u%% (%uus)\r\n", (50 + m_profiling.m_cleanup * 100) / total, m_profiling.m_cleanup / B3_DBVT_BP_PROFILING_RATE);
-		printf("total:     %uus\r\n", total / B3_DBVT_BP_PROFILING_RATE);
-		const unsigned long sum = m_profiling.m_ddcollide +
-								  m_profiling.m_fdcollide +
-								  m_profiling.m_cleanup;
-		printf("leaked: %u%% (%uus)\r\n", 100 - ((50 + sum * 100) / total), (total - sum) / B3_DBVT_BP_PROFILING_RATE);
-		printf("job counts: %u%%\r\n", (m_profiling.m_jobcount * 100) / ((m_sets[0].m_leaves + m_sets[1].m_leaves) * B3_DBVT_BP_PROFILING_RATE));
-		b3Clear(m_profiling);
-		m_clock.reset();
-	}
-#endif
-
-	performDeferredRemoval(dispatcher);
-}
-
-void b3DynamicBvhBroadphase::performDeferredRemoval(b3Dispatcher* dispatcher)
-{
-	if (m_paircache->hasDeferredRemoval())
-	{
-		b3BroadphasePairArray& overlappingPairArray = m_paircache->getOverlappingPairArray();
-
-		//perform a sort, to find duplicates and to sort 'invalid' pairs to the end
-		overlappingPairArray.quickSort(b3BroadphasePairSortPredicate());
-
-		int invalidPair = 0;
-
-		int i;
-
-		b3BroadphasePair previousPair = b3MakeBroadphasePair(-1, -1);
-
-		for (i = 0; i < overlappingPairArray.size(); i++)
-		{
-			b3BroadphasePair& pair = overlappingPairArray[i];
-
-			bool isDuplicate = (pair == previousPair);
-
-			previousPair = pair;
-
-			bool needsRemoval = false;
-
-			if (!isDuplicate)
-			{
-				//important to perform AABB check that is consistent with the broadphase
-				b3DbvtProxy* pa = &m_proxies[pair.x];
-				b3DbvtProxy* pb = &m_proxies[pair.y];
-				bool hasOverlap = b3Intersect(pa->leaf->volume, pb->leaf->volume);
-
-				if (hasOverlap)
-				{
-					needsRemoval = false;
-				}
-				else
-				{
-					needsRemoval = true;
-				}
-			}
-			else
-			{
-				//remove duplicate
-				needsRemoval = true;
-				//should have no algorithm
-			}
-
-			if (needsRemoval)
-			{
-				m_paircache->cleanOverlappingPair(pair, dispatcher);
-
-				pair.x = -1;
-				pair.y = -1;
-				invalidPair++;
-			}
-		}
-
-		//perform a sort, to sort 'invalid' pairs to the end
-		overlappingPairArray.quickSort(b3BroadphasePairSortPredicate());
-		overlappingPairArray.resize(overlappingPairArray.size() - invalidPair);
-	}
-}
-
-//
-void b3DynamicBvhBroadphase::collide(b3Dispatcher* dispatcher)
-{
-	/*printf("---------------------------------------------------------\n");
-	printf("m_sets[0].m_leaves=%d\n",m_sets[0].m_leaves);
-	printf("m_sets[1].m_leaves=%d\n",m_sets[1].m_leaves);
-	printf("numPairs = %d\n",getOverlappingPairCache()->getNumOverlappingPairs());
-	{
-		int i;
-		for (i=0;i<getOverlappingPairCache()->getNumOverlappingPairs();i++)
-		{
-			printf("pair[%d]=(%d,%d),",i,getOverlappingPairCache()->getOverlappingPairArray()[i].m_pProxy0->getUid(),
-				getOverlappingPairCache()->getOverlappingPairArray()[i].m_pProxy1->getUid());
-		}
-		printf("\n");
-	}
-*/
-
-	b3SPC(m_profiling.m_total);
-	/* optimize				*/
-	m_sets[0].optimizeIncremental(1 + (m_sets[0].m_leaves * m_dupdates) / 100);
-	if (m_fixedleft)
-	{
-		const int count = 1 + (m_sets[1].m_leaves * m_fupdates) / 100;
-		m_sets[1].optimizeIncremental(1 + (m_sets[1].m_leaves * m_fupdates) / 100);
-		m_fixedleft = b3Max<int>(0, m_fixedleft - count);
-	}
-	/* dynamic -> fixed set	*/
-	m_stageCurrent = (m_stageCurrent + 1) % STAGECOUNT;
-	b3DbvtProxy* current = m_stageRoots[m_stageCurrent];
-	if (current)
-	{
-		b3DbvtTreeCollider collider(this);
-		do
-		{
-			b3DbvtProxy* next = current->links[1];
-			b3ListRemove(current, m_stageRoots[current->stage]);
-			b3ListAppend(current, m_stageRoots[STAGECOUNT]);
-#if B3_DBVT_BP_ACCURATESLEEPING
-			m_paircache->removeOverlappingPairsContainingProxy(current, dispatcher);
-			collider.proxy = current;
-			b3DynamicBvh::collideTV(m_sets[0].m_root, current->aabb, collider);
-			b3DynamicBvh::collideTV(m_sets[1].m_root, current->aabb, collider);
-#endif
-			m_sets[0].remove(current->leaf);
-			B3_ATTRIBUTE_ALIGNED16(b3DbvtVolume)
-			curAabb = b3DbvtVolume::FromMM(current->m_aabbMin, current->m_aabbMax);
-			current->leaf = m_sets[1].insert(curAabb, current);
-			current->stage = STAGECOUNT;
-			current = next;
-		} while (current);
-		m_fixedleft = m_sets[1].m_leaves;
-		m_needcleanup = true;
-	}
-	/* collide dynamics		*/
-	{
-		b3DbvtTreeCollider collider(this);
-		if (m_deferedcollide)
-		{
-			b3SPC(m_profiling.m_fdcollide);
-			m_sets[0].collideTTpersistentStack(m_sets[0].m_root, m_sets[1].m_root, collider);
-		}
-		if (m_deferedcollide)
-		{
-			b3SPC(m_profiling.m_ddcollide);
-			m_sets[0].collideTTpersistentStack(m_sets[0].m_root, m_sets[0].m_root, collider);
-		}
-	}
-	/* clean up				*/
-	if (m_needcleanup)
-	{
-		b3SPC(m_profiling.m_cleanup);
-		b3BroadphasePairArray& pairs = m_paircache->getOverlappingPairArray();
-		if (pairs.size() > 0)
-		{
-			int ni = b3Min(pairs.size(), b3Max<int>(m_newpairs, (pairs.size() * m_cupdates) / 100));
-			for (int i = 0; i < ni; ++i)
-			{
-				b3BroadphasePair& p = pairs[(m_cid + i) % pairs.size()];
-				b3DbvtProxy* pa = &m_proxies[p.x];
-				b3DbvtProxy* pb = &m_proxies[p.y];
-				if (!b3Intersect(pa->leaf->volume, pb->leaf->volume))
-				{
-#if B3_DBVT_BP_SORTPAIRS
-					if (pa->m_uniqueId > pb->m_uniqueId)
-						b3Swap(pa, pb);
-#endif
-					m_paircache->removeOverlappingPair(pa->getUid(), pb->getUid(), dispatcher);
-					--ni;
-					--i;
-				}
-			}
-			if (pairs.size() > 0)
-				m_cid = (m_cid + ni) % pairs.size();
-			else
-				m_cid = 0;
-		}
-	}
-	++m_pid;
-	m_newpairs = 1;
-	m_needcleanup = false;
-	if (m_updates_call > 0)
-	{
-		m_updates_ratio = m_updates_done / (b3Scalar)m_updates_call;
-	}
-	else
-	{
-		m_updates_ratio = 0;
-	}
-	m_updates_done /= 2;
-	m_updates_call /= 2;
-}
-
-//
-void b3DynamicBvhBroadphase::optimize()
-{
-	m_sets[0].optimizeTopDown();
-	m_sets[1].optimizeTopDown();
-}
-
-//
-b3OverlappingPairCache* b3DynamicBvhBroadphase::getOverlappingPairCache()
-{
-	return (m_paircache);
-}
-
-//
-const b3OverlappingPairCache* b3DynamicBvhBroadphase::getOverlappingPairCache() const
-{
-	return (m_paircache);
-}
-
-//
-void b3DynamicBvhBroadphase::getBroadphaseAabb(b3Vector3& aabbMin, b3Vector3& aabbMax) const
-{
-	B3_ATTRIBUTE_ALIGNED16(b3DbvtVolume)
-	bounds;
-
-	if (!m_sets[0].empty())
-		if (!m_sets[1].empty())
-			b3Merge(m_sets[0].m_root->volume,
-					m_sets[1].m_root->volume, bounds);
-		else
-			bounds = m_sets[0].m_root->volume;
-	else if (!m_sets[1].empty())
-		bounds = m_sets[1].m_root->volume;
-	else
-		bounds = b3DbvtVolume::FromCR(b3MakeVector3(0, 0, 0), 0);
-	aabbMin = bounds.Mins();
-	aabbMax = bounds.Maxs();
-}
-
-void b3DynamicBvhBroadphase::resetPool(b3Dispatcher* dispatcher)
-{
-	int totalObjects = m_sets[0].m_leaves + m_sets[1].m_leaves;
-	if (!totalObjects)
-	{
-		//reset internal dynamic tree data structures
-		m_sets[0].clear();
-		m_sets[1].clear();
-
-		m_deferedcollide = false;
-		m_needcleanup = true;
-		m_stageCurrent = 0;
-		m_fixedleft = 0;
-		m_fupdates = 1;
-		m_dupdates = 0;
-		m_cupdates = 10;
-		m_newpairs = 1;
-		m_updates_call = 0;
-		m_updates_done = 0;
-		m_updates_ratio = 0;
-
-		m_pid = 0;
-		m_cid = 0;
-		for (int i = 0; i <= STAGECOUNT; ++i)
-		{
-			m_stageRoots[i] = 0;
-		}
-	}
-}
-
-//
-void b3DynamicBvhBroadphase::printStats()
-{
-}
-
-//
-#if B3_DBVT_BP_ENABLE_BENCHMARK
-
-struct b3BroadphaseBenchmark
-{
-	struct Experiment
-	{
-		const char* name;
-		int object_count;
-		int update_count;
-		int spawn_count;
-		int iterations;
-		b3Scalar speed;
-		b3Scalar amplitude;
-	};
-	struct Object
-	{
-		b3Vector3 center;
-		b3Vector3 extents;
-		b3BroadphaseProxy* proxy;
-		b3Scalar time;
-		void update(b3Scalar speed, b3Scalar amplitude, b3BroadphaseInterface* pbi)
-		{
-			time += speed;
-			center[0] = b3Cos(time * (b3Scalar)2.17) * amplitude +
-						b3Sin(time) * amplitude / 2;
-			center[1] = b3Cos(time * (b3Scalar)1.38) * amplitude +
-						b3Sin(time) * amplitude;
-			center[2] = b3Sin(time * (b3Scalar)0.777) * amplitude;
-			pbi->setAabb(proxy, center - extents, center + extents, 0);
-		}
-	};
-	static int UnsignedRand(int range = RAND_MAX - 1) { return (rand() % (range + 1)); }
-	static b3Scalar UnitRand() { return (UnsignedRand(16384) / (b3Scalar)16384); }
-	static void OutputTime(const char* name, b3Clock& c, unsigned count = 0)
-	{
-		const unsigned long us = c.getTimeMicroseconds();
-		const unsigned long ms = (us + 500) / 1000;
-		const b3Scalar sec = us / (b3Scalar)(1000 * 1000);
-		if (count > 0)
-			printf("%s : %u us (%u ms), %.2f/s\r\n", name, us, ms, count / sec);
-		else
-			printf("%s : %u us (%u ms)\r\n", name, us, ms);
-	}
-};
-
-void b3DynamicBvhBroadphase::benchmark(b3BroadphaseInterface* pbi)
-{
-	static const b3BroadphaseBenchmark::Experiment experiments[] =
-		{
-			{"1024o.10%", 1024, 10, 0, 8192, (b3Scalar)0.005, (b3Scalar)100},
-			/*{"4096o.10%",4096,10,0,8192,(b3Scalar)0.005,(b3Scalar)100},
-		{"8192o.10%",8192,10,0,8192,(b3Scalar)0.005,(b3Scalar)100},*/
-		};
-	static const int nexperiments = sizeof(experiments) / sizeof(experiments[0]);
-	b3AlignedObjectArray<b3BroadphaseBenchmark::Object*> objects;
-	b3Clock wallclock;
-	/* Begin			*/
-	for (int iexp = 0; iexp < nexperiments; ++iexp)
-	{
-		const b3BroadphaseBenchmark::Experiment& experiment = experiments[iexp];
-		const int object_count = experiment.object_count;
-		const int update_count = (object_count * experiment.update_count) / 100;
-		const int spawn_count = (object_count * experiment.spawn_count) / 100;
-		const b3Scalar speed = experiment.speed;
-		const b3Scalar amplitude = experiment.amplitude;
-		printf("Experiment #%u '%s':\r\n", iexp, experiment.name);
-		printf("\tObjects: %u\r\n", object_count);
-		printf("\tUpdate: %u\r\n", update_count);
-		printf("\tSpawn: %u\r\n", spawn_count);
-		printf("\tSpeed: %f\r\n", speed);
-		printf("\tAmplitude: %f\r\n", amplitude);
-		srand(180673);
-		/* Create objects	*/
-		wallclock.reset();
-		objects.reserve(object_count);
-		for (int i = 0; i < object_count; ++i)
-		{
-			b3BroadphaseBenchmark::Object* po = new b3BroadphaseBenchmark::Object();
-			po->center[0] = b3BroadphaseBenchmark::UnitRand() * 50;
-			po->center[1] = b3BroadphaseBenchmark::UnitRand() * 50;
-			po->center[2] = b3BroadphaseBenchmark::UnitRand() * 50;
-			po->extents[0] = b3BroadphaseBenchmark::UnitRand() * 2 + 2;
-			po->extents[1] = b3BroadphaseBenchmark::UnitRand() * 2 + 2;
-			po->extents[2] = b3BroadphaseBenchmark::UnitRand() * 2 + 2;
-			po->time = b3BroadphaseBenchmark::UnitRand() * 2000;
-			po->proxy = pbi->createProxy(po->center - po->extents, po->center + po->extents, 0, po, 1, 1, 0, 0);
-			objects.push_back(po);
-		}
-		b3BroadphaseBenchmark::OutputTime("\tInitialization", wallclock);
-		/* First update		*/
-		wallclock.reset();
-		for (int i = 0; i < objects.size(); ++i)
-		{
-			objects[i]->update(speed, amplitude, pbi);
-		}
-		b3BroadphaseBenchmark::OutputTime("\tFirst update", wallclock);
-		/* Updates			*/
-		wallclock.reset();
-		for (int i = 0; i < experiment.iterations; ++i)
-		{
-			for (int j = 0; j < update_count; ++j)
-			{
-				objects[j]->update(speed, amplitude, pbi);
-			}
-			pbi->calculateOverlappingPairs(0);
-		}
-		b3BroadphaseBenchmark::OutputTime("\tUpdate", wallclock, experiment.iterations);
-		/* Clean up			*/
-		wallclock.reset();
-		for (int i = 0; i < objects.size(); ++i)
-		{
-			pbi->destroyProxy(objects[i]->proxy, 0);
-			delete objects[i];
-		}
-		objects.resize(0);
-		b3BroadphaseBenchmark::OutputTime("\tRelease", wallclock);
-	}
-}
-#else
-/*void							b3DynamicBvhBroadphase::benchmark(b3BroadphaseInterface*)
-{}
-*/
-#endif
-
-#if B3_DBVT_BP_PROFILE
-#undef b3SPC
-#endif

thirdparty/bullet/Bullet3Collision/BroadPhaseCollision/b3DynamicBvhBroadphase.h

@@ -1,197 +0,0 @@
-/*
-Bullet Continuous Collision Detection and Physics Library
-Copyright (c) 2003-2013 Erwin Coumans  http://bulletphysics.org
-
-This software is provided 'as-is', without any express or implied warranty.
-In no event will the authors be held liable for any damages arising from the use of this software.
-Permission is granted to anyone to use this software for any purpose,
-including commercial applications, and to alter it and redistribute it freely,
-subject to the following restrictions:
-
-1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
-2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
-3. This notice may not be removed or altered from any source distribution.
-*/
-
-///b3DynamicBvhBroadphase implementation by Nathanael Presson
-#ifndef B3_DBVT_BROADPHASE_H
-#define B3_DBVT_BROADPHASE_H
-
-#include "Bullet3Collision/BroadPhaseCollision/b3DynamicBvh.h"
-#include "Bullet3Collision/BroadPhaseCollision/b3OverlappingPairCache.h"
-#include "Bullet3Common/b3AlignedObjectArray.h"
-
-#include "b3BroadphaseCallback.h"
-
-//
-// Compile time config
-//
-
-#define B3_DBVT_BP_PROFILE 0
-//#define B3_DBVT_BP_SORTPAIRS				1
-#define B3_DBVT_BP_PREVENTFALSEUPDATE 0
-#define B3_DBVT_BP_ACCURATESLEEPING 0
-#define B3_DBVT_BP_ENABLE_BENCHMARK 0
-#define B3_DBVT_BP_MARGIN (b3Scalar)0.05
-
-#if B3_DBVT_BP_PROFILE
-#define B3_DBVT_BP_PROFILING_RATE 256
-
-#endif
-
-B3_ATTRIBUTE_ALIGNED16(struct)
-b3BroadphaseProxy
-{
-	B3_DECLARE_ALIGNED_ALLOCATOR();
-
-	///optional filtering to cull potential collisions
-	enum CollisionFilterGroups
-	{
-		DefaultFilter = 1,
-		StaticFilter = 2,
-		KinematicFilter = 4,
-		DebrisFilter = 8,
-		SensorTrigger = 16,
-		CharacterFilter = 32,
-		AllFilter = -1  //all bits sets: DefaultFilter | StaticFilter | KinematicFilter | DebrisFilter | SensorTrigger
-	};
-
-	//Usually the client b3CollisionObject or Rigidbody class
-	void* m_clientObject;
-	int m_collisionFilterGroup;
-	int m_collisionFilterMask;
-	int m_uniqueId;  //m_uniqueId is introduced for paircache. could get rid of this, by calculating the address offset etc.
-
-	b3Vector3 m_aabbMin;
-	b3Vector3 m_aabbMax;
-
-	B3_FORCE_INLINE int getUid() const
-	{
-		return m_uniqueId;
-	}
-
-	//used for memory pools
-	b3BroadphaseProxy() : m_clientObject(0)
-	{
-	}
-
-	b3BroadphaseProxy(const b3Vector3& aabbMin, const b3Vector3& aabbMax, void* userPtr, int collisionFilterGroup, int collisionFilterMask)
-		: m_clientObject(userPtr),
-		  m_collisionFilterGroup(collisionFilterGroup),
-		  m_collisionFilterMask(collisionFilterMask),
-		  m_aabbMin(aabbMin),
-		  m_aabbMax(aabbMax)
-	{
-	}
-};
-
-//
-// b3DbvtProxy
-//
-struct b3DbvtProxy : b3BroadphaseProxy
-{
-	/* Fields		*/
-	//b3DbvtAabbMm	aabb;
-	b3DbvtNode* leaf;
-	b3DbvtProxy* links[2];
-	int stage;
-	/* ctor			*/
-
-	explicit b3DbvtProxy() {}
-	b3DbvtProxy(const b3Vector3& aabbMin, const b3Vector3& aabbMax, void* userPtr, int collisionFilterGroup, int collisionFilterMask) : b3BroadphaseProxy(aabbMin, aabbMax, userPtr, collisionFilterGroup, collisionFilterMask)
-	{
-		links[0] = links[1] = 0;
-	}
-};
-
-typedef b3AlignedObjectArray<b3DbvtProxy*> b3DbvtProxyArray;
-
-///The b3DynamicBvhBroadphase implements a broadphase using two dynamic AABB bounding volume hierarchies/trees (see b3DynamicBvh).
-///One tree is used for static/non-moving objects, and another tree is used for dynamic objects. Objects can move from one tree to the other.
-///This is a very fast broadphase, especially for very dynamic worlds where many objects are moving. Its insert/add and remove of objects is generally faster than the sweep and prune broadphases b3AxisSweep3 and b332BitAxisSweep3.
-struct b3DynamicBvhBroadphase
-{
-	/* Config		*/
-	enum
-	{
-		DYNAMIC_SET = 0, /* Dynamic set index	*/
-		FIXED_SET = 1,   /* Fixed set index		*/
-		STAGECOUNT = 2   /* Number of stages		*/
-	};
-	/* Fields		*/
-	b3DynamicBvh m_sets[2];                     // Dbvt sets
-	b3DbvtProxy* m_stageRoots[STAGECOUNT + 1];  // Stages list
-
-	b3AlignedObjectArray<b3DbvtProxy> m_proxies;
-	b3OverlappingPairCache* m_paircache;  // Pair cache
-	b3Scalar m_prediction;                // Velocity prediction
-	int m_stageCurrent;                   // Current stage
-	int m_fupdates;                       // % of fixed updates per frame
-	int m_dupdates;                       // % of dynamic updates per frame
-	int m_cupdates;                       // % of cleanup updates per frame
-	int m_newpairs;                       // Number of pairs created
-	int m_fixedleft;                      // Fixed optimization left
-	unsigned m_updates_call;              // Number of updates call
-	unsigned m_updates_done;              // Number of updates done
-	b3Scalar m_updates_ratio;             // m_updates_done/m_updates_call
-	int m_pid;                            // Parse id
-	int m_cid;                            // Cleanup index
-	bool m_releasepaircache;              // Release pair cache on delete
-	bool m_deferedcollide;                // Defere dynamic/static collision to collide call
-	bool m_needcleanup;                   // Need to run cleanup?
-#if B3_DBVT_BP_PROFILE
-	b3Clock m_clock;
-	struct
-	{
-		unsigned long m_total;
-		unsigned long m_ddcollide;
-		unsigned long m_fdcollide;
-		unsigned long m_cleanup;
-		unsigned long m_jobcount;
-	} m_profiling;
-#endif
-	/* Methods		*/
-	b3DynamicBvhBroadphase(int proxyCapacity, b3OverlappingPairCache* paircache = 0);
-	virtual ~b3DynamicBvhBroadphase();
-	void collide(b3Dispatcher* dispatcher);
-	void optimize();
-
-	/* b3BroadphaseInterface Implementation	*/
-	b3BroadphaseProxy* createProxy(const b3Vector3& aabbMin, const b3Vector3& aabbMax, int objectIndex, void* userPtr, int collisionFilterGroup, int collisionFilterMask);
-	virtual void destroyProxy(b3BroadphaseProxy* proxy, b3Dispatcher* dispatcher);
-	virtual void setAabb(int objectId, const b3Vector3& aabbMin, const b3Vector3& aabbMax, b3Dispatcher* dispatcher);
-	virtual void rayTest(const b3Vector3& rayFrom, const b3Vector3& rayTo, b3BroadphaseRayCallback& rayCallback, const b3Vector3& aabbMin = b3MakeVector3(0, 0, 0), const b3Vector3& aabbMax = b3MakeVector3(0, 0, 0));
-	virtual void aabbTest(const b3Vector3& aabbMin, const b3Vector3& aabbMax, b3BroadphaseAabbCallback& callback);
-
-	//virtual void					getAabb(b3BroadphaseProxy* proxy,b3Vector3& aabbMin, b3Vector3& aabbMax ) const;
-	virtual void getAabb(int objectId, b3Vector3& aabbMin, b3Vector3& aabbMax) const;
-	virtual void calculateOverlappingPairs(b3Dispatcher* dispatcher = 0);
-	virtual b3OverlappingPairCache* getOverlappingPairCache();
-	virtual const b3OverlappingPairCache* getOverlappingPairCache() const;
-	virtual void getBroadphaseAabb(b3Vector3& aabbMin, b3Vector3& aabbMax) const;
-	virtual void printStats();
-
-	///reset broadphase internal structures, to ensure determinism/reproducability
-	virtual void resetPool(b3Dispatcher* dispatcher);
-
-	void performDeferredRemoval(b3Dispatcher* dispatcher);
-
-	void setVelocityPrediction(b3Scalar prediction)
-	{
-		m_prediction = prediction;
-	}
-	b3Scalar getVelocityPrediction() const
-	{
-		return m_prediction;
-	}
-
-	///this setAabbForceUpdate is similar to setAabb but always forces the aabb update.
-	///it is not part of the b3BroadphaseInterface but specific to b3DynamicBvhBroadphase.
-	///it bypasses certain optimizations that prevent aabb updates (when the aabb shrinks), see
-	///http://code.google.com/p/bullet/issues/detail?id=223
-	void setAabbForceUpdate(b3BroadphaseProxy* absproxy, const b3Vector3& aabbMin, const b3Vector3& aabbMax, b3Dispatcher* /*dispatcher*/);
-
-	//static void						benchmark(b3BroadphaseInterface*);
-};
-
-#endif

thirdparty/bullet/Bullet3Collision/BroadPhaseCollision/b3OverlappingPair.h

@@ -1,70 +0,0 @@
-/*
-Bullet Continuous Collision Detection and Physics Library
-Copyright (c) 2003-2013 Erwin Coumans  http://bulletphysics.org
-
-This software is provided 'as-is', without any express or implied warranty.
-In no event will the authors be held liable for any damages arising from the use of this software.
-Permission is granted to anyone to use this software for any purpose,
-including commercial applications, and to alter it and redistribute it freely,
-subject to the following restrictions:
-
-1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
-2. Altered source versions must be plainly marked as 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 B3_OVERLAPPING_PAIR_H
-#define B3_OVERLAPPING_PAIR_H
-
-#include "Bullet3Common/shared/b3Int4.h"
-
-#define B3_NEW_PAIR_MARKER -1
-#define B3_REMOVED_PAIR_MARKER -2
-
-typedef b3Int4 b3BroadphasePair;
-
-inline b3Int4 b3MakeBroadphasePair(int xx, int yy)
-{
-	b3Int4 pair;
-
-	if (xx < yy)
-	{
-		pair.x = xx;
-		pair.y = yy;
-	}
-	else
-	{
-		pair.x = yy;
-		pair.y = xx;
-	}
-	pair.z = B3_NEW_PAIR_MARKER;
-	pair.w = B3_NEW_PAIR_MARKER;
-	return pair;
-}
-
-/*struct b3BroadphasePair : public b3Int4
-{
-	explicit b3BroadphasePair(){}
-	
-};
-*/
-
-class b3BroadphasePairSortPredicate
-{
-public:
-	bool operator()(const b3BroadphasePair& a, const b3BroadphasePair& b) const
-	{
-		const int uidA0 = a.x;
-		const int uidB0 = b.x;
-		const int uidA1 = a.y;
-		const int uidB1 = b.y;
-		return uidA0 > uidB0 || (uidA0 == uidB0 && uidA1 > uidB1);
-	}
-};
-
-B3_FORCE_INLINE bool operator==(const b3BroadphasePair& a, const b3BroadphasePair& b)
-{
-	return (a.x == b.x) && (a.y == b.y);
-}
-
-#endif  //B3_OVERLAPPING_PAIR_H

thirdparty/bullet/Bullet3Collision/BroadPhaseCollision/b3OverlappingPairCache.cpp

@@ -1,559 +0,0 @@
-/*
-Bullet Continuous Collision Detection and Physics Library
-Copyright (c) 2003-2013 Erwin Coumans  http://bulletphysics.org
-
-This software is provided 'as-is', without any express or implied warranty.
-In no event will the authors be held liable for any damages arising from the use of this software.
-Permission is granted to anyone to use this software for any purpose,
-including commercial applications, and to alter it and redistribute it freely,
-subject to the following restrictions:
-
-1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
-2. Altered source versions must be plainly marked as 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 "b3OverlappingPairCache.h"
-
-//#include "b3Dispatcher.h"
-//#include "b3CollisionAlgorithm.h"
-#include "Bullet3Geometry/b3AabbUtil.h"
-
-#include <stdio.h>
-
-int b3g_overlappingPairs = 0;
-int b3g_removePairs = 0;
-int b3g_addedPairs = 0;
-int b3g_findPairs = 0;
-
-b3HashedOverlappingPairCache::b3HashedOverlappingPairCache() : m_overlapFilterCallback(0)
-//,	m_blockedForChanges(false)
-{
-	int initialAllocatedSize = 2;
-	m_overlappingPairArray.reserve(initialAllocatedSize);
-	growTables();
-}
-
-b3HashedOverlappingPairCache::~b3HashedOverlappingPairCache()
-{
-}
-
-void b3HashedOverlappingPairCache::cleanOverlappingPair(b3BroadphasePair& pair, b3Dispatcher* dispatcher)
-{
-	/*	if (pair.m_algorithm)
-	{
-		{
-			pair.m_algorithm->~b3CollisionAlgorithm();
-			dispatcher->freeCollisionAlgorithm(pair.m_algorithm);
-			pair.m_algorithm=0;
-		}
-	}
-	*/
-}
-
-void b3HashedOverlappingPairCache::cleanProxyFromPairs(int proxy, b3Dispatcher* dispatcher)
-{
-	class CleanPairCallback : public b3OverlapCallback
-	{
-		int m_cleanProxy;
-		b3OverlappingPairCache* m_pairCache;
-		b3Dispatcher* m_dispatcher;
-
-	public:
-		CleanPairCallback(int cleanProxy, b3OverlappingPairCache* pairCache, b3Dispatcher* dispatcher)
-			: m_cleanProxy(cleanProxy),
-			  m_pairCache(pairCache),
-			  m_dispatcher(dispatcher)
-		{
-		}
-		virtual bool processOverlap(b3BroadphasePair& pair)
-		{
-			if ((pair.x == m_cleanProxy) ||
-				(pair.y == m_cleanProxy))
-			{
-				m_pairCache->cleanOverlappingPair(pair, m_dispatcher);
-			}
-			return false;
-		}
-	};
-
-	CleanPairCallback cleanPairs(proxy, this, dispatcher);
-
-	processAllOverlappingPairs(&cleanPairs, dispatcher);
-}
-
-void b3HashedOverlappingPairCache::removeOverlappingPairsContainingProxy(int proxy, b3Dispatcher* dispatcher)
-{
-	class RemovePairCallback : public b3OverlapCallback
-	{
-		int m_obsoleteProxy;
-
-	public:
-		RemovePairCallback(int obsoleteProxy)
-			: m_obsoleteProxy(obsoleteProxy)
-		{
-		}
-		virtual bool processOverlap(b3BroadphasePair& pair)
-		{
-			return ((pair.x == m_obsoleteProxy) ||
-					(pair.y == m_obsoleteProxy));
-		}
-	};
-
-	RemovePairCallback removeCallback(proxy);
-
-	processAllOverlappingPairs(&removeCallback, dispatcher);
-}
-
-b3BroadphasePair* b3HashedOverlappingPairCache::findPair(int proxy0, int proxy1)
-{
-	b3g_findPairs++;
-	if (proxy0 > proxy1)
-		b3Swap(proxy0, proxy1);
-	int proxyId1 = proxy0;
-	int proxyId2 = proxy1;
-
-	/*if (proxyId1 > proxyId2) 
-		b3Swap(proxyId1, proxyId2);*/
-
-	int hash = static_cast<int>(getHash(static_cast<unsigned int>(proxyId1), static_cast<unsigned int>(proxyId2)) & (m_overlappingPairArray.capacity() - 1));
-
-	if (hash >= m_hashTable.size())
-	{
-		return NULL;
-	}
-
-	int index = m_hashTable[hash];
-	while (index != B3_NULL_PAIR && equalsPair(m_overlappingPairArray[index], proxyId1, proxyId2) == false)
-	{
-		index = m_next[index];
-	}
-
-	if (index == B3_NULL_PAIR)
-	{
-		return NULL;
-	}
-
-	b3Assert(index < m_overlappingPairArray.size());
-
-	return &m_overlappingPairArray[index];
-}
-
-//#include <stdio.h>
-
-void b3HashedOverlappingPairCache::growTables()
-{
-	int newCapacity = m_overlappingPairArray.capacity();
-
-	if (m_hashTable.size() < newCapacity)
-	{
-		//grow hashtable and next table
-		int curHashtableSize = m_hashTable.size();
-
-		m_hashTable.resize(newCapacity);
-		m_next.resize(newCapacity);
-
-		int i;
-
-		for (i = 0; i < newCapacity; ++i)
-		{
-			m_hashTable[i] = B3_NULL_PAIR;
-		}
-		for (i = 0; i < newCapacity; ++i)
-		{
-			m_next[i] = B3_NULL_PAIR;
-		}
-
-		for (i = 0; i < curHashtableSize; i++)
-		{
-			const b3BroadphasePair& pair = m_overlappingPairArray[i];
-			int proxyId1 = pair.x;
-			int proxyId2 = pair.y;
-			/*if (proxyId1 > proxyId2) 
-				b3Swap(proxyId1, proxyId2);*/
-			int hashValue = static_cast<int>(getHash(static_cast<unsigned int>(proxyId1), static_cast<unsigned int>(proxyId2)) & (m_overlappingPairArray.capacity() - 1));  // New hash value with new mask
-			m_next[i] = m_hashTable[hashValue];
-			m_hashTable[hashValue] = i;
-		}
-	}
-}
-
-b3BroadphasePair* b3HashedOverlappingPairCache::internalAddPair(int proxy0, int proxy1)
-{
-	if (proxy0 > proxy1)
-		b3Swap(proxy0, proxy1);
-	int proxyId1 = proxy0;
-	int proxyId2 = proxy1;
-
-	/*if (proxyId1 > proxyId2) 
-		b3Swap(proxyId1, proxyId2);*/
-
-	int hash = static_cast<int>(getHash(static_cast<unsigned int>(proxyId1), static_cast<unsigned int>(proxyId2)) & (m_overlappingPairArray.capacity() - 1));  // New hash value with new mask
-
-	b3BroadphasePair* pair = internalFindPair(proxy0, proxy1, hash);
-	if (pair != NULL)
-	{
-		return pair;
-	}
-	/*for(int i=0;i<m_overlappingPairArray.size();++i)
-		{
-		if(	(m_overlappingPairArray[i].m_pProxy0==proxy0)&&
-			(m_overlappingPairArray[i].m_pProxy1==proxy1))
-			{
-			printf("Adding duplicated %u<>%u\r\n",proxyId1,proxyId2);
-			internalFindPair(proxy0, proxy1, hash);
-			}
-		}*/
-	int count = m_overlappingPairArray.size();
-	int oldCapacity = m_overlappingPairArray.capacity();
-	pair = &m_overlappingPairArray.expandNonInitializing();
-
-	//this is where we add an actual pair, so also call the 'ghost'
-	//	if (m_ghostPairCallback)
-	//		m_ghostPairCallback->addOverlappingPair(proxy0,proxy1);
-
-	int newCapacity = m_overlappingPairArray.capacity();
-
-	if (oldCapacity < newCapacity)
-	{
-		growTables();
-		//hash with new capacity
-		hash = static_cast<int>(getHash(static_cast<unsigned int>(proxyId1), static_cast<unsigned int>(proxyId2)) & (m_overlappingPairArray.capacity() - 1));
-	}
-
-	*pair = b3MakeBroadphasePair(proxy0, proxy1);
-
-	//	pair->m_pProxy0 = proxy0;
-	//	pair->m_pProxy1 = proxy1;
-	//pair->m_algorithm = 0;
-	//pair->m_internalTmpValue = 0;
-
-	m_next[count] = m_hashTable[hash];
-	m_hashTable[hash] = count;
-
-	return pair;
-}
-
-void* b3HashedOverlappingPairCache::removeOverlappingPair(int proxy0, int proxy1, b3Dispatcher* dispatcher)
-{
-	b3g_removePairs++;
-	if (proxy0 > proxy1)
-		b3Swap(proxy0, proxy1);
-	int proxyId1 = proxy0;
-	int proxyId2 = proxy1;
-
-	/*if (proxyId1 > proxyId2) 
-		b3Swap(proxyId1, proxyId2);*/
-
-	int hash = static_cast<int>(getHash(static_cast<unsigned int>(proxyId1), static_cast<unsigned int>(proxyId2)) & (m_overlappingPairArray.capacity() - 1));
-
-	b3BroadphasePair* pair = internalFindPair(proxy0, proxy1, hash);
-	if (pair == NULL)
-	{
-		return 0;
-	}
-
-	cleanOverlappingPair(*pair, dispatcher);
-
-	int pairIndex = int(pair - &m_overlappingPairArray[0]);
-	b3Assert(pairIndex < m_overlappingPairArray.size());
-
-	// Remove the pair from the hash table.
-	int index = m_hashTable[hash];
-	b3Assert(index != B3_NULL_PAIR);
-
-	int previous = B3_NULL_PAIR;
-	while (index != pairIndex)
-	{
-		previous = index;
-		index = m_next[index];
-	}
-
-	if (previous != B3_NULL_PAIR)
-	{
-		b3Assert(m_next[previous] == pairIndex);
-		m_next[previous] = m_next[pairIndex];
-	}
-	else
-	{
-		m_hashTable[hash] = m_next[pairIndex];
-	}
-
-	// We now move the last pair into spot of the
-	// pair being removed. We need to fix the hash
-	// table indices to support the move.
-
-	int lastPairIndex = m_overlappingPairArray.size() - 1;
-
-	//if (m_ghostPairCallback)
-	//	m_ghostPairCallback->removeOverlappingPair(proxy0, proxy1,dispatcher);
-
-	// If the removed pair is the last pair, we are done.
-	if (lastPairIndex == pairIndex)
-	{
-		m_overlappingPairArray.pop_back();
-		return 0;
-	}
-
-	// Remove the last pair from the hash table.
-	const b3BroadphasePair* last = &m_overlappingPairArray[lastPairIndex];
-	/* missing swap here too, Nat. */
-	int lastHash = static_cast<int>(getHash(static_cast<unsigned int>(last->x), static_cast<unsigned int>(last->y)) & (m_overlappingPairArray.capacity() - 1));
-
-	index = m_hashTable[lastHash];
-	b3Assert(index != B3_NULL_PAIR);
-
-	previous = B3_NULL_PAIR;
-	while (index != lastPairIndex)
-	{
-		previous = index;
-		index = m_next[index];
-	}
-
-	if (previous != B3_NULL_PAIR)
-	{
-		b3Assert(m_next[previous] == lastPairIndex);
-		m_next[previous] = m_next[lastPairIndex];
-	}
-	else
-	{
-		m_hashTable[lastHash] = m_next[lastPairIndex];
-	}
-
-	// Copy the last pair into the remove pair's spot.
-	m_overlappingPairArray[pairIndex] = m_overlappingPairArray[lastPairIndex];
-
-	// Insert the last pair into the hash table
-	m_next[pairIndex] = m_hashTable[lastHash];
-	m_hashTable[lastHash] = pairIndex;
-
-	m_overlappingPairArray.pop_back();
-
-	return 0;
-}
-//#include <stdio.h>
-
-void b3HashedOverlappingPairCache::processAllOverlappingPairs(b3OverlapCallback* callback, b3Dispatcher* dispatcher)
-{
-	int i;
-
-	//	printf("m_overlappingPairArray.size()=%d\n",m_overlappingPairArray.size());
-	for (i = 0; i < m_overlappingPairArray.size();)
-	{
-		b3BroadphasePair* pair = &m_overlappingPairArray[i];
-		if (callback->processOverlap(*pair))
-		{
-			removeOverlappingPair(pair->x, pair->y, dispatcher);
-
-			b3g_overlappingPairs--;
-		}
-		else
-		{
-			i++;
-		}
-	}
-}
-
-void b3HashedOverlappingPairCache::sortOverlappingPairs(b3Dispatcher* dispatcher)
-{
-	///need to keep hashmap in sync with pair address, so rebuild all
-	b3BroadphasePairArray tmpPairs;
-	int i;
-	for (i = 0; i < m_overlappingPairArray.size(); i++)
-	{
-		tmpPairs.push_back(m_overlappingPairArray[i]);
-	}
-
-	for (i = 0; i < tmpPairs.size(); i++)
-	{
-		removeOverlappingPair(tmpPairs[i].x, tmpPairs[i].y, dispatcher);
-	}
-
-	for (i = 0; i < m_next.size(); i++)
-	{
-		m_next[i] = B3_NULL_PAIR;
-	}
-
-	tmpPairs.quickSort(b3BroadphasePairSortPredicate());
-
-	for (i = 0; i < tmpPairs.size(); i++)
-	{
-		addOverlappingPair(tmpPairs[i].x, tmpPairs[i].y);
-	}
-}
-
-void* b3SortedOverlappingPairCache::removeOverlappingPair(int proxy0, int proxy1, b3Dispatcher* dispatcher)
-{
-	if (!hasDeferredRemoval())
-	{
-		b3BroadphasePair findPair = b3MakeBroadphasePair(proxy0, proxy1);
-
-		int findIndex = m_overlappingPairArray.findLinearSearch(findPair);
-		if (findIndex < m_overlappingPairArray.size())
-		{
-			b3g_overlappingPairs--;
-			b3BroadphasePair& pair = m_overlappingPairArray[findIndex];
-
-			cleanOverlappingPair(pair, dispatcher);
-			//if (m_ghostPairCallback)
-			//	m_ghostPairCallback->removeOverlappingPair(proxy0, proxy1,dispatcher);
-
-			m_overlappingPairArray.swap(findIndex, m_overlappingPairArray.capacity() - 1);
-			m_overlappingPairArray.pop_back();
-			return 0;
-		}
-	}
-
-	return 0;
-}
-
-b3BroadphasePair* b3SortedOverlappingPairCache::addOverlappingPair(int proxy0, int proxy1)
-{
-	//don't add overlap with own
-	b3Assert(proxy0 != proxy1);
-
-	if (!needsBroadphaseCollision(proxy0, proxy1))
-		return 0;
-
-	b3BroadphasePair* pair = &m_overlappingPairArray.expandNonInitializing();
-	*pair = b3MakeBroadphasePair(proxy0, proxy1);
-
-	b3g_overlappingPairs++;
-	b3g_addedPairs++;
-
-	//	if (m_ghostPairCallback)
-	//		m_ghostPairCallback->addOverlappingPair(proxy0, proxy1);
-	return pair;
-}
-
-///this findPair becomes really slow. Either sort the list to speedup the query, or
-///use a different solution. It is mainly used for Removing overlapping pairs. Removal could be delayed.
-///we could keep a linked list in each proxy, and store pair in one of the proxies (with lowest memory address)
-///Also we can use a 2D bitmap, which can be useful for a future GPU implementation
-b3BroadphasePair* b3SortedOverlappingPairCache::findPair(int proxy0, int proxy1)
-{
-	if (!needsBroadphaseCollision(proxy0, proxy1))
-		return 0;
-
-	b3BroadphasePair tmpPair = b3MakeBroadphasePair(proxy0, proxy1);
-	int findIndex = m_overlappingPairArray.findLinearSearch(tmpPair);
-
-	if (findIndex < m_overlappingPairArray.size())
-	{
-		//b3Assert(it != m_overlappingPairSet.end());
-		b3BroadphasePair* pair = &m_overlappingPairArray[findIndex];
-		return pair;
-	}
-	return 0;
-}
-
-//#include <stdio.h>
-
-void b3SortedOverlappingPairCache::processAllOverlappingPairs(b3OverlapCallback* callback, b3Dispatcher* dispatcher)
-{
-	int i;
-
-	for (i = 0; i < m_overlappingPairArray.size();)
-	{
-		b3BroadphasePair* pair = &m_overlappingPairArray[i];
-		if (callback->processOverlap(*pair))
-		{
-			cleanOverlappingPair(*pair, dispatcher);
-			pair->x = -1;
-			pair->y = -1;
-			m_overlappingPairArray.swap(i, m_overlappingPairArray.size() - 1);
-			m_overlappingPairArray.pop_back();
-			b3g_overlappingPairs--;
-		}
-		else
-		{
-			i++;
-		}
-	}
-}
-
-b3SortedOverlappingPairCache::b3SortedOverlappingPairCache() : m_blockedForChanges(false),
-															   m_hasDeferredRemoval(true),
-															   m_overlapFilterCallback(0)
-
-{
-	int initialAllocatedSize = 2;
-	m_overlappingPairArray.reserve(initialAllocatedSize);
-}
-
-b3SortedOverlappingPairCache::~b3SortedOverlappingPairCache()
-{
-}
-
-void b3SortedOverlappingPairCache::cleanOverlappingPair(b3BroadphasePair& pair, b3Dispatcher* dispatcher)
-{
-	/*	if (pair.m_algorithm)
-	{
-		{
-			pair.m_algorithm->~b3CollisionAlgorithm();
-			dispatcher->freeCollisionAlgorithm(pair.m_algorithm);
-			pair.m_algorithm=0;
-			b3g_removePairs--;
-		}
-	}
-	*/
-}
-
-void b3SortedOverlappingPairCache::cleanProxyFromPairs(int proxy, b3Dispatcher* dispatcher)
-{
-	class CleanPairCallback : public b3OverlapCallback
-	{
-		int m_cleanProxy;
-		b3OverlappingPairCache* m_pairCache;
-		b3Dispatcher* m_dispatcher;
-
-	public:
-		CleanPairCallback(int cleanProxy, b3OverlappingPairCache* pairCache, b3Dispatcher* dispatcher)
-			: m_cleanProxy(cleanProxy),
-			  m_pairCache(pairCache),
-			  m_dispatcher(dispatcher)
-		{
-		}
-		virtual bool processOverlap(b3BroadphasePair& pair)
-		{
-			if ((pair.x == m_cleanProxy) ||
-				(pair.y == m_cleanProxy))
-			{
-				m_pairCache->cleanOverlappingPair(pair, m_dispatcher);
-			}
-			return false;
-		}
-	};
-
-	CleanPairCallback cleanPairs(proxy, this, dispatcher);
-
-	processAllOverlappingPairs(&cleanPairs, dispatcher);
-}
-
-void b3SortedOverlappingPairCache::removeOverlappingPairsContainingProxy(int proxy, b3Dispatcher* dispatcher)
-{
-	class RemovePairCallback : public b3OverlapCallback
-	{
-		int m_obsoleteProxy;
-
-	public:
-		RemovePairCallback(int obsoleteProxy)
-			: m_obsoleteProxy(obsoleteProxy)
-		{
-		}
-		virtual bool processOverlap(b3BroadphasePair& pair)
-		{
-			return ((pair.x == m_obsoleteProxy) ||
-					(pair.y == m_obsoleteProxy));
-		}
-	};
-
-	RemovePairCallback removeCallback(proxy);
-
-	processAllOverlappingPairs(&removeCallback, dispatcher);
-}
-
-void b3SortedOverlappingPairCache::sortOverlappingPairs(b3Dispatcher* dispatcher)
-{
-	//should already be sorted
-}

thirdparty/bullet/Bullet3Collision/BroadPhaseCollision/b3OverlappingPairCache.h

@@ -1,427 +0,0 @@
-/*
-Bullet Continuous Collision Detection and Physics Library
-Copyright (c) 2003-2013 Erwin Coumans  http://bulletphysics.org
-
-This software is provided 'as-is', without any express or implied warranty.
-In no event will the authors be held liable for any damages arising from the use of this software.
-Permission is granted to anyone to use this software for any purpose,
-including commercial applications, and to alter it and redistribute it freely,
-subject to the following restrictions:
-
-1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
-2. Altered source versions must be plainly marked as 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 B3_OVERLAPPING_PAIR_CACHE_H
-#define B3_OVERLAPPING_PAIR_CACHE_H
-
-#include "Bullet3Common/shared/b3Int2.h"
-#include "Bullet3Common/b3AlignedObjectArray.h"
-
-class b3Dispatcher;
-#include "b3OverlappingPair.h"
-
-typedef b3AlignedObjectArray<b3BroadphasePair> b3BroadphasePairArray;
-
-struct b3OverlapCallback
-{
-	virtual ~b3OverlapCallback()
-	{
-	}
-	//return true for deletion of the pair
-	virtual bool processOverlap(b3BroadphasePair& pair) = 0;
-};
-
-struct b3OverlapFilterCallback
-{
-	virtual ~b3OverlapFilterCallback()
-	{
-	}
-	// return true when pairs need collision
-	virtual bool needBroadphaseCollision(int proxy0, int proxy1) const = 0;
-};
-
-extern int b3g_removePairs;
-extern int b3g_addedPairs;
-extern int b3g_findPairs;
-
-const int B3_NULL_PAIR = 0xffffffff;
-
-///The b3OverlappingPairCache provides an interface for overlapping pair management (add, remove, storage), used by the b3BroadphaseInterface broadphases.
-///The b3HashedOverlappingPairCache and b3SortedOverlappingPairCache classes are two implementations.
-class b3OverlappingPairCache
-{
-public:
-	virtual ~b3OverlappingPairCache() {}  // this is needed so we can get to the derived class destructor
-
-	virtual b3BroadphasePair* getOverlappingPairArrayPtr() = 0;
-
-	virtual const b3BroadphasePair* getOverlappingPairArrayPtr() const = 0;
-
-	virtual b3BroadphasePairArray& getOverlappingPairArray() = 0;
-
-	virtual void cleanOverlappingPair(b3BroadphasePair& pair, b3Dispatcher* dispatcher) = 0;
-
-	virtual int getNumOverlappingPairs() const = 0;
-
-	virtual void cleanProxyFromPairs(int proxy, b3Dispatcher* dispatcher) = 0;
-
-	virtual void setOverlapFilterCallback(b3OverlapFilterCallback* callback) = 0;
-
-	virtual void processAllOverlappingPairs(b3OverlapCallback*, b3Dispatcher* dispatcher) = 0;
-
-	virtual b3BroadphasePair* findPair(int proxy0, int proxy1) = 0;
-
-	virtual bool hasDeferredRemoval() = 0;
-
-	//virtual	void	setInternalGhostPairCallback(b3OverlappingPairCallback* ghostPairCallback)=0;
-
-	virtual b3BroadphasePair* addOverlappingPair(int proxy0, int proxy1) = 0;
-	virtual void* removeOverlappingPair(int proxy0, int proxy1, b3Dispatcher* dispatcher) = 0;
-	virtual void removeOverlappingPairsContainingProxy(int /*proxy0*/, b3Dispatcher* /*dispatcher*/) = 0;
-
-	virtual void sortOverlappingPairs(b3Dispatcher* dispatcher) = 0;
-};
-
-/// Hash-space based Pair Cache, thanks to Erin Catto, Box2D, http://www.box2d.org, and Pierre Terdiman, Codercorner, http://codercorner.com
-class b3HashedOverlappingPairCache : public b3OverlappingPairCache
-{
-	b3BroadphasePairArray m_overlappingPairArray;
-	b3OverlapFilterCallback* m_overlapFilterCallback;
-	//	bool		m_blockedForChanges;
-
-public:
-	b3HashedOverlappingPairCache();
-	virtual ~b3HashedOverlappingPairCache();
-
-	virtual void removeOverlappingPairsContainingProxy(int proxy, b3Dispatcher* dispatcher);
-
-	virtual void* removeOverlappingPair(int proxy0, int proxy1, b3Dispatcher* dispatcher);
-
-	B3_FORCE_INLINE bool needsBroadphaseCollision(int proxy0, int proxy1) const
-	{
-		if (m_overlapFilterCallback)
-			return m_overlapFilterCallback->needBroadphaseCollision(proxy0, proxy1);
-
-		bool collides = true;  //(proxy0->m_collisionFilterGroup & proxy1->m_collisionFilterMask) != 0;
-		//collides = collides && (proxy1->m_collisionFilterGroup & proxy0->m_collisionFilterMask);
-
-		return collides;
-	}
-
-	// Add a pair and return the new pair. If the pair already exists,
-	// no new pair is created and the old one is returned.
-	virtual b3BroadphasePair* addOverlappingPair(int proxy0, int proxy1)
-	{
-		b3g_addedPairs++;
-
-		if (!needsBroadphaseCollision(proxy0, proxy1))
-			return 0;
-
-		return internalAddPair(proxy0, proxy1);
-	}
-
-	void cleanProxyFromPairs(int proxy, b3Dispatcher* dispatcher);
-
-	virtual void processAllOverlappingPairs(b3OverlapCallback*, b3Dispatcher* dispatcher);
-
-	virtual b3BroadphasePair* getOverlappingPairArrayPtr()
-	{
-		return &m_overlappingPairArray[0];
-	}
-
-	const b3BroadphasePair* getOverlappingPairArrayPtr() const
-	{
-		return &m_overlappingPairArray[0];
-	}
-
-	b3BroadphasePairArray& getOverlappingPairArray()
-	{
-		return m_overlappingPairArray;
-	}
-
-	const b3BroadphasePairArray& getOverlappingPairArray() const
-	{
-		return m_overlappingPairArray;
-	}
-
-	void cleanOverlappingPair(b3BroadphasePair& pair, b3Dispatcher* dispatcher);
-
-	b3BroadphasePair* findPair(int proxy0, int proxy1);
-
-	int GetCount() const { return m_overlappingPairArray.size(); }
-	//	b3BroadphasePair* GetPairs() { return m_pairs; }
-
-	b3OverlapFilterCallback* getOverlapFilterCallback()
-	{
-		return m_overlapFilterCallback;
-	}
-
-	void setOverlapFilterCallback(b3OverlapFilterCallback* callback)
-	{
-		m_overlapFilterCallback = callback;
-	}
-
-	int getNumOverlappingPairs() const
-	{
-		return m_overlappingPairArray.size();
-	}
-
-private:
-	b3BroadphasePair* internalAddPair(int proxy0, int proxy1);
-
-	void growTables();
-
-	B3_FORCE_INLINE bool equalsPair(const b3BroadphasePair& pair, int proxyId1, int proxyId2)
-	{
-		return pair.x == proxyId1 && pair.y == proxyId2;
-	}
-
-	/*
-	// Thomas Wang's hash, see: http://www.concentric.net/~Ttwang/tech/inthash.htm
-	// This assumes proxyId1 and proxyId2 are 16-bit.
-	B3_FORCE_INLINE int getHash(int proxyId1, int proxyId2)
-	{
-		int key = (proxyId2 << 16) | proxyId1;
-		key = ~key + (key << 15);
-		key = key ^ (key >> 12);
-		key = key + (key << 2);
-		key = key ^ (key >> 4);
-		key = key * 2057;
-		key = key ^ (key >> 16);
-		return key;
-	}
-	*/
-
-	B3_FORCE_INLINE unsigned int getHash(unsigned int proxyId1, unsigned int proxyId2)
-	{
-		int key = static_cast<int>(((unsigned int)proxyId1) | (((unsigned int)proxyId2) << 16));
-		// Thomas Wang's hash
-
-		key += ~(key << 15);
-		key ^= (key >> 10);
-		key += (key << 3);
-		key ^= (key >> 6);
-		key += ~(key << 11);
-		key ^= (key >> 16);
-		return static_cast<unsigned int>(key);
-	}
-
-	B3_FORCE_INLINE b3BroadphasePair* internalFindPair(int proxy0, int proxy1, int hash)
-	{
-		int proxyId1 = proxy0;
-		int proxyId2 = proxy1;
-#if 0  // wrong, 'equalsPair' use unsorted uids, copy-past devil striked again. Nat.
-		if (proxyId1 > proxyId2) 
-			b3Swap(proxyId1, proxyId2);
-#endif
-
-		int index = m_hashTable[hash];
-
-		while (index != B3_NULL_PAIR && equalsPair(m_overlappingPairArray[index], proxyId1, proxyId2) == false)
-		{
-			index = m_next[index];
-		}
-
-		if (index == B3_NULL_PAIR)
-		{
-			return NULL;
-		}
-
-		b3Assert(index < m_overlappingPairArray.size());
-
-		return &m_overlappingPairArray[index];
-	}
-
-	virtual bool hasDeferredRemoval()
-	{
-		return false;
-	}
-
-	/*	virtual	void	setInternalGhostPairCallback(b3OverlappingPairCallback* ghostPairCallback)
-	{
-		m_ghostPairCallback = ghostPairCallback;
-	}
-	*/
-
-	virtual void sortOverlappingPairs(b3Dispatcher* dispatcher);
-
-protected:
-	b3AlignedObjectArray<int> m_hashTable;
-	b3AlignedObjectArray<int> m_next;
-	//	b3OverlappingPairCallback*	m_ghostPairCallback;
-};
-
-///b3SortedOverlappingPairCache maintains the objects with overlapping AABB
-///Typically managed by the Broadphase, Axis3Sweep or b3SimpleBroadphase
-class b3SortedOverlappingPairCache : public b3OverlappingPairCache
-{
-protected:
-	//avoid brute-force finding all the time
-	b3BroadphasePairArray m_overlappingPairArray;
-
-	//during the dispatch, check that user doesn't destroy/create proxy
-	bool m_blockedForChanges;
-
-	///by default, do the removal during the pair traversal
-	bool m_hasDeferredRemoval;
-
-	//if set, use the callback instead of the built in filter in needBroadphaseCollision
-	b3OverlapFilterCallback* m_overlapFilterCallback;
-
-	//		b3OverlappingPairCallback*	m_ghostPairCallback;
-
-public:
-	b3SortedOverlappingPairCache();
-	virtual ~b3SortedOverlappingPairCache();
-
-	virtual void processAllOverlappingPairs(b3OverlapCallback*, b3Dispatcher* dispatcher);
-
-	void* removeOverlappingPair(int proxy0, int proxy1, b3Dispatcher* dispatcher);
-
-	void cleanOverlappingPair(b3BroadphasePair& pair, b3Dispatcher* dispatcher);
-
-	b3BroadphasePair* addOverlappingPair(int proxy0, int proxy1);
-
-	b3BroadphasePair* findPair(int proxy0, int proxy1);
-
-	void cleanProxyFromPairs(int proxy, b3Dispatcher* dispatcher);
-
-	virtual void removeOverlappingPairsContainingProxy(int proxy, b3Dispatcher* dispatcher);
-
-	inline bool needsBroadphaseCollision(int proxy0, int proxy1) const
-	{
-		if (m_overlapFilterCallback)
-			return m_overlapFilterCallback->needBroadphaseCollision(proxy0, proxy1);
-
-		bool collides = true;  //(proxy0->m_collisionFilterGroup & proxy1->m_collisionFilterMask) != 0;
-		//collides = collides && (proxy1->m_collisionFilterGroup & proxy0->m_collisionFilterMask);
-
-		return collides;
-	}
-
-	b3BroadphasePairArray& getOverlappingPairArray()
-	{
-		return m_overlappingPairArray;
-	}
-
-	const b3BroadphasePairArray& getOverlappingPairArray() const
-	{
-		return m_overlappingPairArray;
-	}
-
-	b3BroadphasePair* getOverlappingPairArrayPtr()
-	{
-		return &m_overlappingPairArray[0];
-	}
-
-	const b3BroadphasePair* getOverlappingPairArrayPtr() const
-	{
-		return &m_overlappingPairArray[0];
-	}
-
-	int getNumOverlappingPairs() const
-	{
-		return m_overlappingPairArray.size();
-	}
-
-	b3OverlapFilterCallback* getOverlapFilterCallback()
-	{
-		return m_overlapFilterCallback;
-	}
-
-	void setOverlapFilterCallback(b3OverlapFilterCallback* callback)
-	{
-		m_overlapFilterCallback = callback;
-	}
-
-	virtual bool hasDeferredRemoval()
-	{
-		return m_hasDeferredRemoval;
-	}
-
-	/*		virtual	void	setInternalGhostPairCallback(b3OverlappingPairCallback* ghostPairCallback)
-		{
-			m_ghostPairCallback = ghostPairCallback;
-		}
-		*/
-	virtual void sortOverlappingPairs(b3Dispatcher* dispatcher);
-};
-
-///b3NullPairCache skips add/removal of overlapping pairs. Userful for benchmarking and unit testing.
-class b3NullPairCache : public b3OverlappingPairCache
-{
-	b3BroadphasePairArray m_overlappingPairArray;
-
-public:
-	virtual b3BroadphasePair* getOverlappingPairArrayPtr()
-	{
-		return &m_overlappingPairArray[0];
-	}
-	const b3BroadphasePair* getOverlappingPairArrayPtr() const
-	{
-		return &m_overlappingPairArray[0];
-	}
-	b3BroadphasePairArray& getOverlappingPairArray()
-	{
-		return m_overlappingPairArray;
-	}
-
-	virtual void cleanOverlappingPair(b3BroadphasePair& /*pair*/, b3Dispatcher* /*dispatcher*/)
-	{
-	}
-
-	virtual int getNumOverlappingPairs() const
-	{
-		return 0;
-	}
-
-	virtual void cleanProxyFromPairs(int /*proxy*/, b3Dispatcher* /*dispatcher*/)
-	{
-	}
-
-	virtual void setOverlapFilterCallback(b3OverlapFilterCallback* /*callback*/)
-	{
-	}
-
-	virtual void processAllOverlappingPairs(b3OverlapCallback*, b3Dispatcher* /*dispatcher*/)
-	{
-	}
-
-	virtual b3BroadphasePair* findPair(int /*proxy0*/, int /*proxy1*/)
-	{
-		return 0;
-	}
-
-	virtual bool hasDeferredRemoval()
-	{
-		return true;
-	}
-
-	//	virtual	void	setInternalGhostPairCallback(b3OverlappingPairCallback* /* ghostPairCallback */)
-	//	{
-	//
-	//	}
-
-	virtual b3BroadphasePair* addOverlappingPair(int /*proxy0*/, int /*proxy1*/)
-	{
-		return 0;
-	}
-
-	virtual void* removeOverlappingPair(int /*proxy0*/, int /*proxy1*/, b3Dispatcher* /*dispatcher*/)
-	{
-		return 0;
-	}
-
-	virtual void removeOverlappingPairsContainingProxy(int /*proxy0*/, b3Dispatcher* /*dispatcher*/)
-	{
-	}
-
-	virtual void sortOverlappingPairs(b3Dispatcher* dispatcher)
-	{
-		(void)dispatcher;
-	}
-};
-
-#endif  //B3_OVERLAPPING_PAIR_CACHE_H

thirdparty/bullet/Bullet3Collision/BroadPhaseCollision/shared/b3Aabb.h

@@ -1,56 +0,0 @@
-
-#ifndef B3_AABB_H
-#define B3_AABB_H
-
-#include "Bullet3Common/shared/b3Float4.h"
-#include "Bullet3Common/shared/b3Mat3x3.h"
-
-typedef struct b3Aabb b3Aabb_t;
-
-struct b3Aabb
-{
-	union {
-		float m_min[4];
-		b3Float4 m_minVec;
-		int m_minIndices[4];
-	};
-	union {
-		float m_max[4];
-		b3Float4 m_maxVec;
-		int m_signedMaxIndices[4];
-	};
-};
-
-inline void b3TransformAabb2(b3Float4ConstArg localAabbMin, b3Float4ConstArg localAabbMax, float margin,
-							 b3Float4ConstArg pos,
-							 b3QuatConstArg orn,
-							 b3Float4* aabbMinOut, b3Float4* aabbMaxOut)
-{
-	b3Float4 localHalfExtents = 0.5f * (localAabbMax - localAabbMin);
-	localHalfExtents += b3MakeFloat4(margin, margin, margin, 0.f);
-	b3Float4 localCenter = 0.5f * (localAabbMax + localAabbMin);
-	b3Mat3x3 m;
-	m = b3QuatGetRotationMatrix(orn);
-	b3Mat3x3 abs_b = b3AbsoluteMat3x3(m);
-	b3Float4 center = b3TransformPoint(localCenter, pos, orn);
-
-	b3Float4 extent = b3MakeFloat4(b3Dot3F4(localHalfExtents, b3GetRow(abs_b, 0)),
-								   b3Dot3F4(localHalfExtents, b3GetRow(abs_b, 1)),
-								   b3Dot3F4(localHalfExtents, b3GetRow(abs_b, 2)),
-								   0.f);
-	*aabbMinOut = center - extent;
-	*aabbMaxOut = center + extent;
-}
-
-/// conservative test for overlap between two aabbs
-inline bool b3TestAabbAgainstAabb(b3Float4ConstArg aabbMin1, b3Float4ConstArg aabbMax1,
-								  b3Float4ConstArg aabbMin2, b3Float4ConstArg aabbMax2)
-{
-	bool overlap = true;
-	overlap = (aabbMin1.x > aabbMax2.x || aabbMax1.x < aabbMin2.x) ? false : overlap;
-	overlap = (aabbMin1.z > aabbMax2.z || aabbMax1.z < aabbMin2.z) ? false : overlap;
-	overlap = (aabbMin1.y > aabbMax2.y || aabbMax1.y < aabbMin2.y) ? false : overlap;
-	return overlap;
-}
-
-#endif  //B3_AABB_H

thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/b3Config.h

@@ -1,39 +0,0 @@
-#ifndef B3_CONFIG_H
-#define B3_CONFIG_H
-
-struct b3Config
-{
-	int m_maxConvexBodies;
-	int m_maxConvexShapes;
-	int m_maxBroadphasePairs;
-	int m_maxContactCapacity;
-	int m_compoundPairCapacity;
-
-	int m_maxVerticesPerFace;
-	int m_maxFacesPerShape;
-	int m_maxConvexVertices;
-	int m_maxConvexIndices;
-	int m_maxConvexUniqueEdges;
-
-	int m_maxCompoundChildShapes;
-
-	int m_maxTriConvexPairCapacity;
-
-	b3Config()
-		: m_maxConvexBodies(128 * 1024),
-		  m_maxVerticesPerFace(64),
-		  m_maxFacesPerShape(12),
-		  m_maxConvexVertices(8192),
-		  m_maxConvexIndices(81920),
-		  m_maxConvexUniqueEdges(8192),
-		  m_maxCompoundChildShapes(8192),
-		  m_maxTriConvexPairCapacity(256 * 1024)
-	{
-		m_maxConvexShapes = m_maxConvexBodies;
-		m_maxBroadphasePairs = 16 * m_maxConvexBodies;
-		m_maxContactCapacity = m_maxBroadphasePairs;
-		m_compoundPairCapacity = 1024 * 1024;
-	}
-};
-
-#endif  //B3_CONFIG_H

thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/b3Contact4.h

@@ -1,55 +0,0 @@
-/*
-Bullet Continuous Collision Detection and Physics Library
-Copyright (c) 2003-2013 Erwin Coumans  http://bulletphysics.org
-
-This software is provided 'as-is', without any express or implied warranty.
-In no event will the authors be held liable for any damages arising from the use of this software.
-Permission is granted to anyone to use this software for any purpose,
-including commercial applications, and to alter it and redistribute it freely,
-subject to the following restrictions:
-
-1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
-2. Altered source versions must be plainly marked as 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 B3_CONTACT4_H
-#define B3_CONTACT4_H
-
-#include "Bullet3Common/b3Vector3.h"
-#include "Bullet3Collision/NarrowPhaseCollision/shared/b3Contact4Data.h"
-
-B3_ATTRIBUTE_ALIGNED16(struct)
-b3Contact4 : public b3Contact4Data
-{
-	B3_DECLARE_ALIGNED_ALLOCATOR();
-
-	int getBodyA() const { return abs(m_bodyAPtrAndSignBit); }
-	int getBodyB() const { return abs(m_bodyBPtrAndSignBit); }
-	bool isBodyAFixed() const { return m_bodyAPtrAndSignBit < 0; }
-	bool isBodyBFixed() const { return m_bodyBPtrAndSignBit < 0; }
-	//	todo. make it safer
-	int& getBatchIdx() { return m_batchIdx; }
-	const int& getBatchIdx() const { return m_batchIdx; }
-	float getRestituitionCoeff() const { return ((float)m_restituitionCoeffCmp / (float)0xffff); }
-	void setRestituitionCoeff(float c)
-	{
-		b3Assert(c >= 0.f && c <= 1.f);
-		m_restituitionCoeffCmp = (unsigned short)(c * 0xffff);
-	}
-	float getFrictionCoeff() const { return ((float)m_frictionCoeffCmp / (float)0xffff); }
-	void setFrictionCoeff(float c)
-	{
-		b3Assert(c >= 0.f && c <= 1.f);
-		m_frictionCoeffCmp = (unsigned short)(c * 0xffff);
-	}
-
-	//float& getNPoints() { return m_worldNormal[3]; }
-	int getNPoints() const { return (int)m_worldNormalOnB.w; }
-
-	float getPenetration(int idx) const { return m_worldPosB[idx].w; }
-
-	bool isInvalid() const { return (getBodyA() == 0 || getBodyB() == 0); }
-};
-
-#endif  //B3_CONTACT4_H

thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/b3ConvexUtility.cpp

@@ -1,500 +0,0 @@
-/*
-Copyright (c) 2012 Advanced Micro Devices, Inc.  
-
-This software is provided 'as-is', without any express or implied warranty.
-In no event will the authors be held liable for any damages arising from the use of this software.
-Permission is granted to anyone to use this software for any purpose, 
-including commercial applications, and to alter it and redistribute it freely, 
-subject to the following restrictions:
-
-1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
-2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
-3. This notice may not be removed or altered from any source distribution.
-*/
-//Originally written by Erwin Coumans
-
-#include "b3ConvexUtility.h"
-#include "Bullet3Geometry/b3ConvexHullComputer.h"
-#include "Bullet3Geometry/b3GrahamScan2dConvexHull.h"
-#include "Bullet3Common/b3Quaternion.h"
-#include "Bullet3Common/b3HashMap.h"
-
-b3ConvexUtility::~b3ConvexUtility()
-{
-}
-
-bool b3ConvexUtility::initializePolyhedralFeatures(const b3Vector3* orgVertices, int numPoints, bool mergeCoplanarTriangles)
-{
-	b3ConvexHullComputer conv;
-	conv.compute(&orgVertices[0].getX(), sizeof(b3Vector3), numPoints, 0.f, 0.f);
-
-	b3AlignedObjectArray<b3Vector3> faceNormals;
-	int numFaces = conv.faces.size();
-	faceNormals.resize(numFaces);
-	b3ConvexHullComputer* convexUtil = &conv;
-
-	b3AlignedObjectArray<b3MyFace> tmpFaces;
-	tmpFaces.resize(numFaces);
-
-	int numVertices = convexUtil->vertices.size();
-	m_vertices.resize(numVertices);
-	for (int p = 0; p < numVertices; p++)
-	{
-		m_vertices[p] = convexUtil->vertices[p];
-	}
-
-	for (int i = 0; i < numFaces; i++)
-	{
-		int face = convexUtil->faces[i];
-		//printf("face=%d\n",face);
-		const b3ConvexHullComputer::Edge* firstEdge = &convexUtil->edges[face];
-		const b3ConvexHullComputer::Edge* edge = firstEdge;
-
-		b3Vector3 edges[3];
-		int numEdges = 0;
-		//compute face normals
-
-		do
-		{
-			int src = edge->getSourceVertex();
-			tmpFaces[i].m_indices.push_back(src);
-			int targ = edge->getTargetVertex();
-			b3Vector3 wa = convexUtil->vertices[src];
-
-			b3Vector3 wb = convexUtil->vertices[targ];
-			b3Vector3 newEdge = wb - wa;
-			newEdge.normalize();
-			if (numEdges < 2)
-				edges[numEdges++] = newEdge;
-
-			edge = edge->getNextEdgeOfFace();
-		} while (edge != firstEdge);
-
-		b3Scalar planeEq = 1e30f;
-
-		if (numEdges == 2)
-		{
-			faceNormals[i] = edges[0].cross(edges[1]);
-			faceNormals[i].normalize();
-			tmpFaces[i].m_plane[0] = faceNormals[i].getX();
-			tmpFaces[i].m_plane[1] = faceNormals[i].getY();
-			tmpFaces[i].m_plane[2] = faceNormals[i].getZ();
-			tmpFaces[i].m_plane[3] = planeEq;
-		}
-		else
-		{
-			b3Assert(0);  //degenerate?
-			faceNormals[i].setZero();
-		}
-
-		for (int v = 0; v < tmpFaces[i].m_indices.size(); v++)
-		{
-			b3Scalar eq = m_vertices[tmpFaces[i].m_indices[v]].dot(faceNormals[i]);
-			if (planeEq > eq)
-			{
-				planeEq = eq;
-			}
-		}
-		tmpFaces[i].m_plane[3] = -planeEq;
-	}
-
-	//merge coplanar faces and copy them to m_polyhedron
-
-	b3Scalar faceWeldThreshold = 0.999f;
-	b3AlignedObjectArray<int> todoFaces;
-	for (int i = 0; i < tmpFaces.size(); i++)
-		todoFaces.push_back(i);
-
-	while (todoFaces.size())
-	{
-		b3AlignedObjectArray<int> coplanarFaceGroup;
-		int refFace = todoFaces[todoFaces.size() - 1];
-
-		coplanarFaceGroup.push_back(refFace);
-		b3MyFace& faceA = tmpFaces[refFace];
-		todoFaces.pop_back();
-
-		b3Vector3 faceNormalA = b3MakeVector3(faceA.m_plane[0], faceA.m_plane[1], faceA.m_plane[2]);
-		for (int j = todoFaces.size() - 1; j >= 0; j--)
-		{
-			int i = todoFaces[j];
-			b3MyFace& faceB = tmpFaces[i];
-			b3Vector3 faceNormalB = b3MakeVector3(faceB.m_plane[0], faceB.m_plane[1], faceB.m_plane[2]);
-			if (faceNormalA.dot(faceNormalB) > faceWeldThreshold)
-			{
-				coplanarFaceGroup.push_back(i);
-				todoFaces.remove(i);
-			}
-		}
-
-		bool did_merge = false;
-		if (coplanarFaceGroup.size() > 1)
-		{
-			//do the merge: use Graham Scan 2d convex hull
-
-			b3AlignedObjectArray<b3GrahamVector3> orgpoints;
-			b3Vector3 averageFaceNormal = b3MakeVector3(0, 0, 0);
-
-			for (int i = 0; i < coplanarFaceGroup.size(); i++)
-			{
-				//				m_polyhedron->m_faces.push_back(tmpFaces[coplanarFaceGroup[i]]);
-
-				b3MyFace& face = tmpFaces[coplanarFaceGroup[i]];
-				b3Vector3 faceNormal = b3MakeVector3(face.m_plane[0], face.m_plane[1], face.m_plane[2]);
-				averageFaceNormal += faceNormal;
-				for (int f = 0; f < face.m_indices.size(); f++)
-				{
-					int orgIndex = face.m_indices[f];
-					b3Vector3 pt = m_vertices[orgIndex];
-
-					bool found = false;
-
-					for (int i = 0; i < orgpoints.size(); i++)
-					{
-						//if ((orgpoints[i].m_orgIndex == orgIndex) || ((rotatedPt-orgpoints[i]).length2()<0.0001))
-						if (orgpoints[i].m_orgIndex == orgIndex)
-						{
-							found = true;
-							break;
-						}
-					}
-					if (!found)
-						orgpoints.push_back(b3GrahamVector3(pt, orgIndex));
-				}
-			}
-
-			b3MyFace combinedFace;
-			for (int i = 0; i < 4; i++)
-				combinedFace.m_plane[i] = tmpFaces[coplanarFaceGroup[0]].m_plane[i];
-
-			b3AlignedObjectArray<b3GrahamVector3> hull;
-
-			averageFaceNormal.normalize();
-			b3GrahamScanConvexHull2D(orgpoints, hull, averageFaceNormal);
-
-			for (int i = 0; i < hull.size(); i++)
-			{
-				combinedFace.m_indices.push_back(hull[i].m_orgIndex);
-				for (int k = 0; k < orgpoints.size(); k++)
-				{
-					if (orgpoints[k].m_orgIndex == hull[i].m_orgIndex)
-					{
-						orgpoints[k].m_orgIndex = -1;  // invalidate...
-						break;
-					}
-				}
-			}
-
-			// are there rejected vertices?
-			bool reject_merge = false;
-
-			for (int i = 0; i < orgpoints.size(); i++)
-			{
-				if (orgpoints[i].m_orgIndex == -1)
-					continue;  // this is in the hull...
-				// this vertex is rejected -- is anybody else using this vertex?
-				for (int j = 0; j < tmpFaces.size(); j++)
-				{
-					b3MyFace& face = tmpFaces[j];
-					// is this a face of the current coplanar group?
-					bool is_in_current_group = false;
-					for (int k = 0; k < coplanarFaceGroup.size(); k++)
-					{
-						if (coplanarFaceGroup[k] == j)
-						{
-							is_in_current_group = true;
-							break;
-						}
-					}
-					if (is_in_current_group)  // ignore this face...
-						continue;
-					// does this face use this rejected vertex?
-					for (int v = 0; v < face.m_indices.size(); v++)
-					{
-						if (face.m_indices[v] == orgpoints[i].m_orgIndex)
-						{
-							// this rejected vertex is used in another face -- reject merge
-							reject_merge = true;
-							break;
-						}
-					}
-					if (reject_merge)
-						break;
-				}
-				if (reject_merge)
-					break;
-			}
-
-			if (!reject_merge)
-			{
-				// do this merge!
-				did_merge = true;
-				m_faces.push_back(combinedFace);
-			}
-		}
-		if (!did_merge)
-		{
-			for (int i = 0; i < coplanarFaceGroup.size(); i++)
-			{
-				b3MyFace face = tmpFaces[coplanarFaceGroup[i]];
-				m_faces.push_back(face);
-			}
-		}
-	}
-
-	initialize();
-
-	return true;
-}
-
-inline bool IsAlmostZero(const b3Vector3& v)
-{
-	if (fabsf(v.getX()) > 1e-6 || fabsf(v.getY()) > 1e-6 || fabsf(v.getZ()) > 1e-6) return false;
-	return true;
-}
-
-struct b3InternalVertexPair
-{
-	b3InternalVertexPair(short int v0, short int v1)
-		: m_v0(v0),
-		  m_v1(v1)
-	{
-		if (m_v1 > m_v0)
-			b3Swap(m_v0, m_v1);
-	}
-	short int m_v0;
-	short int m_v1;
-	int getHash() const
-	{
-		return m_v0 + (m_v1 << 16);
-	}
-	bool equals(const b3InternalVertexPair& other) const
-	{
-		return m_v0 == other.m_v0 && m_v1 == other.m_v1;
-	}
-};
-
-struct b3InternalEdge
-{
-	b3InternalEdge()
-		: m_face0(-1),
-		  m_face1(-1)
-	{
-	}
-	short int m_face0;
-	short int m_face1;
-};
-
-//
-
-#ifdef TEST_INTERNAL_OBJECTS
-bool b3ConvexUtility::testContainment() const
-{
-	for (int p = 0; p < 8; p++)
-	{
-		b3Vector3 LocalPt;
-		if (p == 0)
-			LocalPt = m_localCenter + b3Vector3(m_extents[0], m_extents[1], m_extents[2]);
-		else if (p == 1)
-			LocalPt = m_localCenter + b3Vector3(m_extents[0], m_extents[1], -m_extents[2]);
-		else if (p == 2)
-			LocalPt = m_localCenter + b3Vector3(m_extents[0], -m_extents[1], m_extents[2]);
-		else if (p == 3)
-			LocalPt = m_localCenter + b3Vector3(m_extents[0], -m_extents[1], -m_extents[2]);
-		else if (p == 4)
-			LocalPt = m_localCenter + b3Vector3(-m_extents[0], m_extents[1], m_extents[2]);
-		else if (p == 5)
-			LocalPt = m_localCenter + b3Vector3(-m_extents[0], m_extents[1], -m_extents[2]);
-		else if (p == 6)
-			LocalPt = m_localCenter + b3Vector3(-m_extents[0], -m_extents[1], m_extents[2]);
-		else if (p == 7)
-			LocalPt = m_localCenter + b3Vector3(-m_extents[0], -m_extents[1], -m_extents[2]);
-
-		for (int i = 0; i < m_faces.size(); i++)
-		{
-			const b3Vector3 Normal(m_faces[i].m_plane[0], m_faces[i].m_plane[1], m_faces[i].m_plane[2]);
-			const b3Scalar d = LocalPt.dot(Normal) + m_faces[i].m_plane[3];
-			if (d > 0.0f)
-				return false;
-		}
-	}
-	return true;
-}
-#endif
-
-void b3ConvexUtility::initialize()
-{
-	b3HashMap<b3InternalVertexPair, b3InternalEdge> edges;
-
-	b3Scalar TotalArea = 0.0f;
-
-	m_localCenter.setValue(0, 0, 0);
-	for (int i = 0; i < m_faces.size(); i++)
-	{
-		int numVertices = m_faces[i].m_indices.size();
-		int NbTris = numVertices;
-		for (int j = 0; j < NbTris; j++)
-		{
-			int k = (j + 1) % numVertices;
-			b3InternalVertexPair vp(m_faces[i].m_indices[j], m_faces[i].m_indices[k]);
-			b3InternalEdge* edptr = edges.find(vp);
-			b3Vector3 edge = m_vertices[vp.m_v1] - m_vertices[vp.m_v0];
-			edge.normalize();
-
-			bool found = false;
-			b3Vector3 diff, diff2;
-
-			for (int p = 0; p < m_uniqueEdges.size(); p++)
-			{
-				diff = m_uniqueEdges[p] - edge;
-				diff2 = m_uniqueEdges[p] + edge;
-
-				//	if ((diff.length2()==0.f) ||
-				//	(diff2.length2()==0.f))
-
-				if (IsAlmostZero(diff) ||
-					IsAlmostZero(diff2))
-				{
-					found = true;
-					break;
-				}
-			}
-
-			if (!found)
-			{
-				m_uniqueEdges.push_back(edge);
-			}
-
-			if (edptr)
-			{
-				//TBD: figure out why I added this assert
-				//				b3Assert(edptr->m_face0>=0);
-				//			b3Assert(edptr->m_face1<0);
-				edptr->m_face1 = i;
-			}
-			else
-			{
-				b3InternalEdge ed;
-				ed.m_face0 = i;
-				edges.insert(vp, ed);
-			}
-		}
-	}
-
-#ifdef USE_CONNECTED_FACES
-	for (int i = 0; i < m_faces.size(); i++)
-	{
-		int numVertices = m_faces[i].m_indices.size();
-		m_faces[i].m_connectedFaces.resize(numVertices);
-
-		for (int j = 0; j < numVertices; j++)
-		{
-			int k = (j + 1) % numVertices;
-			b3InternalVertexPair vp(m_faces[i].m_indices[j], m_faces[i].m_indices[k]);
-			b3InternalEdge* edptr = edges.find(vp);
-			b3Assert(edptr);
-			b3Assert(edptr->m_face0 >= 0);
-			b3Assert(edptr->m_face1 >= 0);
-
-			int connectedFace = (edptr->m_face0 == i) ? edptr->m_face1 : edptr->m_face0;
-			m_faces[i].m_connectedFaces[j] = connectedFace;
-		}
-	}
-#endif  //USE_CONNECTED_FACES
-
-	for (int i = 0; i < m_faces.size(); i++)
-	{
-		int numVertices = m_faces[i].m_indices.size();
-		int NbTris = numVertices - 2;
-
-		const b3Vector3& p0 = m_vertices[m_faces[i].m_indices[0]];
-		for (int j = 1; j <= NbTris; j++)
-		{
-			int k = (j + 1) % numVertices;
-			const b3Vector3& p1 = m_vertices[m_faces[i].m_indices[j]];
-			const b3Vector3& p2 = m_vertices[m_faces[i].m_indices[k]];
-			b3Scalar Area = ((p0 - p1).cross(p0 - p2)).length() * 0.5f;
-			b3Vector3 Center = (p0 + p1 + p2) / 3.0f;
-			m_localCenter += Area * Center;
-			TotalArea += Area;
-		}
-	}
-	m_localCenter /= TotalArea;
-
-#ifdef TEST_INTERNAL_OBJECTS
-	if (1)
-	{
-		m_radius = FLT_MAX;
-		for (int i = 0; i < m_faces.size(); i++)
-		{
-			const b3Vector3 Normal(m_faces[i].m_plane[0], m_faces[i].m_plane[1], m_faces[i].m_plane[2]);
-			const b3Scalar dist = b3Fabs(m_localCenter.dot(Normal) + m_faces[i].m_plane[3]);
-			if (dist < m_radius)
-				m_radius = dist;
-		}
-
-		b3Scalar MinX = FLT_MAX;
-		b3Scalar MinY = FLT_MAX;
-		b3Scalar MinZ = FLT_MAX;
-		b3Scalar MaxX = -FLT_MAX;
-		b3Scalar MaxY = -FLT_MAX;
-		b3Scalar MaxZ = -FLT_MAX;
-		for (int i = 0; i < m_vertices.size(); i++)
-		{
-			const b3Vector3& pt = m_vertices[i];
-			if (pt.getX() < MinX) MinX = pt.getX();
-			if (pt.getX() > MaxX) MaxX = pt.getX();
-			if (pt.getY() < MinY) MinY = pt.getY();
-			if (pt.getY() > MaxY) MaxY = pt.getY();
-			if (pt.getZ() < MinZ) MinZ = pt.getZ();
-			if (pt.getZ() > MaxZ) MaxZ = pt.getZ();
-		}
-		mC.setValue(MaxX + MinX, MaxY + MinY, MaxZ + MinZ);
-		mE.setValue(MaxX - MinX, MaxY - MinY, MaxZ - MinZ);
-
-		//		const b3Scalar r = m_radius / sqrtf(2.0f);
-		const b3Scalar r = m_radius / sqrtf(3.0f);
-		const int LargestExtent = mE.maxAxis();
-		const b3Scalar Step = (mE[LargestExtent] * 0.5f - r) / 1024.0f;
-		m_extents[0] = m_extents[1] = m_extents[2] = r;
-		m_extents[LargestExtent] = mE[LargestExtent] * 0.5f;
-		bool FoundBox = false;
-		for (int j = 0; j < 1024; j++)
-		{
-			if (testContainment())
-			{
-				FoundBox = true;
-				break;
-			}
-
-			m_extents[LargestExtent] -= Step;
-		}
-		if (!FoundBox)
-		{
-			m_extents[0] = m_extents[1] = m_extents[2] = r;
-		}
-		else
-		{
-			// Refine the box
-			const b3Scalar Step = (m_radius - r) / 1024.0f;
-			const int e0 = (1 << LargestExtent) & 3;
-			const int e1 = (1 << e0) & 3;
-
-			for (int j = 0; j < 1024; j++)
-			{
-				const b3Scalar Saved0 = m_extents[e0];
-				const b3Scalar Saved1 = m_extents[e1];
-				m_extents[e0] += Step;
-				m_extents[e1] += Step;
-
-				if (!testContainment())
-				{
-					m_extents[e0] = Saved0;
-					m_extents[e1] = Saved1;
-					break;
-				}
-			}
-		}
-	}
-#endif
-}

thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/b3ConvexUtility.h

@@ -1,55 +0,0 @@
-
-/*
-Copyright (c) 2012 Advanced Micro Devices, Inc.  
-
-This software is provided 'as-is', without any express or implied warranty.
-In no event will the authors be held liable for any damages arising from the use of this software.
-Permission is granted to anyone to use this software for any purpose, 
-including commercial applications, and to alter it and redistribute it freely, 
-subject to the following restrictions:
-
-1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
-2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
-3. This notice may not be removed or altered from any source distribution.
-*/
-//Originally written by Erwin Coumans
-
-#ifndef _BT_CONVEX_UTILITY_H
-#define _BT_CONVEX_UTILITY_H
-
-#include "Bullet3Common/b3AlignedObjectArray.h"
-#include "Bullet3Common/b3Transform.h"
-
-struct b3MyFace
-{
-	b3AlignedObjectArray<int> m_indices;
-	b3Scalar m_plane[4];
-};
-
-B3_ATTRIBUTE_ALIGNED16(class)
-b3ConvexUtility
-{
-public:
-	B3_DECLARE_ALIGNED_ALLOCATOR();
-
-	b3Vector3 m_localCenter;
-	b3Vector3 m_extents;
-	b3Vector3 mC;
-	b3Vector3 mE;
-	b3Scalar m_radius;
-
-	b3AlignedObjectArray<b3Vector3> m_vertices;
-	b3AlignedObjectArray<b3MyFace> m_faces;
-	b3AlignedObjectArray<b3Vector3> m_uniqueEdges;
-
-	b3ConvexUtility()
-	{
-	}
-	virtual ~b3ConvexUtility();
-
-	bool initializePolyhedralFeatures(const b3Vector3* orgVertices, int numVertices, bool mergeCoplanarTriangles = true);
-
-	void initialize();
-	bool testContainment() const;
-};
-#endif

thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/b3CpuNarrowPhase.cpp

@@ -1,297 +0,0 @@
-#include "b3CpuNarrowPhase.h"
-#include "Bullet3Collision/NarrowPhaseCollision/b3ConvexUtility.h"
-#include "Bullet3Collision/NarrowPhaseCollision/b3Config.h"
-
-#include "Bullet3Collision/NarrowPhaseCollision/shared/b3ConvexPolyhedronData.h"
-#include "Bullet3Collision/NarrowPhaseCollision/shared/b3ContactConvexConvexSAT.h"
-
-struct b3CpuNarrowPhaseInternalData
-{
-	b3AlignedObjectArray<b3Aabb> m_localShapeAABBCPU;
-	b3AlignedObjectArray<b3Collidable> m_collidablesCPU;
-	b3AlignedObjectArray<b3ConvexUtility*> m_convexData;
-	b3Config m_config;
-
-	b3AlignedObjectArray<b3ConvexPolyhedronData> m_convexPolyhedra;
-	b3AlignedObjectArray<b3Vector3> m_uniqueEdges;
-	b3AlignedObjectArray<b3Vector3> m_convexVertices;
-	b3AlignedObjectArray<int> m_convexIndices;
-	b3AlignedObjectArray<b3GpuFace> m_convexFaces;
-
-	b3AlignedObjectArray<b3Contact4Data> m_contacts;
-
-	int m_numAcceleratedShapes;
-};
-
-const b3AlignedObjectArray<b3Contact4Data>& b3CpuNarrowPhase::getContacts() const
-{
-	return m_data->m_contacts;
-}
-
-b3Collidable& b3CpuNarrowPhase::getCollidableCpu(int collidableIndex)
-{
-	return m_data->m_collidablesCPU[collidableIndex];
-}
-
-const b3Collidable& b3CpuNarrowPhase::getCollidableCpu(int collidableIndex) const
-{
-	return m_data->m_collidablesCPU[collidableIndex];
-}
-
-b3CpuNarrowPhase::b3CpuNarrowPhase(const struct b3Config& config)
-{
-	m_data = new b3CpuNarrowPhaseInternalData;
-	m_data->m_config = config;
-	m_data->m_numAcceleratedShapes = 0;
-}
-
-b3CpuNarrowPhase::~b3CpuNarrowPhase()
-{
-	delete m_data;
-}
-
-void b3CpuNarrowPhase::computeContacts(b3AlignedObjectArray<b3Int4>& pairs, b3AlignedObjectArray<b3Aabb>& aabbsWorldSpace, b3AlignedObjectArray<b3RigidBodyData>& bodies)
-{
-	int nPairs = pairs.size();
-	int numContacts = 0;
-	int maxContactCapacity = m_data->m_config.m_maxContactCapacity;
-	m_data->m_contacts.resize(maxContactCapacity);
-
-	for (int i = 0; i < nPairs; i++)
-	{
-		int bodyIndexA = pairs[i].x;
-		int bodyIndexB = pairs[i].y;
-		int collidableIndexA = bodies[bodyIndexA].m_collidableIdx;
-		int collidableIndexB = bodies[bodyIndexB].m_collidableIdx;
-
-		if (m_data->m_collidablesCPU[collidableIndexA].m_shapeType == SHAPE_SPHERE &&
-			m_data->m_collidablesCPU[collidableIndexB].m_shapeType == SHAPE_CONVEX_HULL)
-		{
-			//			computeContactSphereConvex(i,bodyIndexA,bodyIndexB,collidableIndexA,collidableIndexB,&bodies[0],
-			//				&m_data->m_collidablesCPU[0],&hostConvexData[0],&hostVertices[0],&hostIndices[0],&hostFaces[0],&hostContacts[0],nContacts,maxContactCapacity);
-		}
-
-		if (m_data->m_collidablesCPU[collidableIndexA].m_shapeType == SHAPE_CONVEX_HULL &&
-			m_data->m_collidablesCPU[collidableIndexB].m_shapeType == SHAPE_SPHERE)
-		{
-			//			computeContactSphereConvex(i,bodyIndexB,bodyIndexA,collidableIndexB,collidableIndexA,&bodies[0],
-			//				&m_data->m_collidablesCPU[0],&hostConvexData[0],&hostVertices[0],&hostIndices[0],&hostFaces[0],&hostContacts[0],nContacts,maxContactCapacity);
-			//printf("convex-sphere\n");
-		}
-
-		if (m_data->m_collidablesCPU[collidableIndexA].m_shapeType == SHAPE_CONVEX_HULL &&
-			m_data->m_collidablesCPU[collidableIndexB].m_shapeType == SHAPE_PLANE)
-		{
-			//			computeContactPlaneConvex(i,bodyIndexB,bodyIndexA,collidableIndexB,collidableIndexA,&bodies[0],
-			//			&m_data->m_collidablesCPU[0],&hostConvexData[0],&hostVertices[0],&hostIndices[0],&hostFaces[0],&hostContacts[0],nContacts,maxContactCapacity);
-			//			printf("convex-plane\n");
-		}
-
-		if (m_data->m_collidablesCPU[collidableIndexA].m_shapeType == SHAPE_PLANE &&
-			m_data->m_collidablesCPU[collidableIndexB].m_shapeType == SHAPE_CONVEX_HULL)
-		{
-			//			computeContactPlaneConvex(i,bodyIndexA,bodyIndexB,collidableIndexA,collidableIndexB,&bodies[0],
-			//			&m_data->m_collidablesCPU[0],&hostConvexData[0],&hostVertices[0],&hostIndices[0],&hostFaces[0],&hostContacts[0],nContacts,maxContactCapacity);
-			//			printf("plane-convex\n");
-		}
-
-		if (m_data->m_collidablesCPU[collidableIndexA].m_shapeType == SHAPE_COMPOUND_OF_CONVEX_HULLS &&
-			m_data->m_collidablesCPU[collidableIndexB].m_shapeType == SHAPE_COMPOUND_OF_CONVEX_HULLS)
-		{
-			//			computeContactCompoundCompound(i,bodyIndexB,bodyIndexA,collidableIndexB,collidableIndexA,&bodies[0],
-			//			&m_data->m_collidablesCPU[0],&hostConvexData[0],&cpuChildShapes[0], hostAabbsWorldSpace,hostAabbsLocalSpace,hostVertices,hostUniqueEdges,hostIndices,hostFaces,&hostContacts[0],
-			//			nContacts,maxContactCapacity,treeNodesCPU,subTreesCPU,bvhInfoCPU);
-			//			printf("convex-plane\n");
-		}
-
-		if (m_data->m_collidablesCPU[collidableIndexA].m_shapeType == SHAPE_COMPOUND_OF_CONVEX_HULLS &&
-			m_data->m_collidablesCPU[collidableIndexB].m_shapeType == SHAPE_PLANE)
-		{
-			//			computeContactPlaneCompound(i,bodyIndexB,bodyIndexA,collidableIndexB,collidableIndexA,&bodies[0],
-			//			&m_data->m_collidablesCPU[0],&hostConvexData[0],&cpuChildShapes[0], &hostVertices[0],&hostIndices[0],&hostFaces[0],&hostContacts[0],nContacts,maxContactCapacity);
-			//			printf("convex-plane\n");
-		}
-
-		if (m_data->m_collidablesCPU[collidableIndexA].m_shapeType == SHAPE_PLANE &&
-			m_data->m_collidablesCPU[collidableIndexB].m_shapeType == SHAPE_COMPOUND_OF_CONVEX_HULLS)
-		{
-			//			computeContactPlaneCompound(i,bodyIndexA,bodyIndexB,collidableIndexA,collidableIndexB,&bodies[0],
-			//			&m_data->m_collidablesCPU[0],&hostConvexData[0],&cpuChildShapes[0],&hostVertices[0],&hostIndices[0],&hostFaces[0],&hostContacts[0],nContacts,maxContactCapacity);
-			//			printf("plane-convex\n");
-		}
-
-		if (m_data->m_collidablesCPU[collidableIndexA].m_shapeType == SHAPE_CONVEX_HULL &&
-			m_data->m_collidablesCPU[collidableIndexB].m_shapeType == SHAPE_CONVEX_HULL)
-		{
-			//printf("pairs[i].z=%d\n",pairs[i].z);
-			//int contactIndex = computeContactConvexConvex2(i,bodyIndexA,bodyIndexB,collidableIndexA,collidableIndexB,bodies,
-			//		m_data->m_collidablesCPU,hostConvexData,hostVertices,hostUniqueEdges,hostIndices,hostFaces,hostContacts,nContacts,maxContactCapacity,oldHostContacts);
-			int contactIndex = b3ContactConvexConvexSAT(i, bodyIndexA, bodyIndexB, collidableIndexA, collidableIndexB, bodies,
-														m_data->m_collidablesCPU, m_data->m_convexPolyhedra, m_data->m_convexVertices, m_data->m_uniqueEdges, m_data->m_convexIndices, m_data->m_convexFaces, m_data->m_contacts, numContacts, maxContactCapacity);
-
-			if (contactIndex >= 0)
-			{
-				pairs[i].z = contactIndex;
-			}
-			//			printf("plane-convex\n");
-		}
-	}
-
-	m_data->m_contacts.resize(numContacts);
-}
-
-int b3CpuNarrowPhase::registerConvexHullShape(b3ConvexUtility* utilPtr)
-{
-	int collidableIndex = allocateCollidable();
-	if (collidableIndex < 0)
-		return collidableIndex;
-
-	b3Collidable& col = m_data->m_collidablesCPU[collidableIndex];
-	col.m_shapeType = SHAPE_CONVEX_HULL;
-	col.m_shapeIndex = -1;
-
-	{
-		b3Vector3 localCenter = b3MakeVector3(0, 0, 0);
-		for (int i = 0; i < utilPtr->m_vertices.size(); i++)
-			localCenter += utilPtr->m_vertices[i];
-		localCenter *= (1.f / utilPtr->m_vertices.size());
-		utilPtr->m_localCenter = localCenter;
-
-		col.m_shapeIndex = registerConvexHullShapeInternal(utilPtr, col);
-	}
-
-	if (col.m_shapeIndex >= 0)
-	{
-		b3Aabb aabb;
-
-		b3Vector3 myAabbMin = b3MakeVector3(1e30f, 1e30f, 1e30f);
-		b3Vector3 myAabbMax = b3MakeVector3(-1e30f, -1e30f, -1e30f);
-
-		for (int i = 0; i < utilPtr->m_vertices.size(); i++)
-		{
-			myAabbMin.setMin(utilPtr->m_vertices[i]);
-			myAabbMax.setMax(utilPtr->m_vertices[i]);
-		}
-		aabb.m_min[0] = myAabbMin[0];
-		aabb.m_min[1] = myAabbMin[1];
-		aabb.m_min[2] = myAabbMin[2];
-		aabb.m_minIndices[3] = 0;
-
-		aabb.m_max[0] = myAabbMax[0];
-		aabb.m_max[1] = myAabbMax[1];
-		aabb.m_max[2] = myAabbMax[2];
-		aabb.m_signedMaxIndices[3] = 0;
-
-		m_data->m_localShapeAABBCPU.push_back(aabb);
-	}
-
-	return collidableIndex;
-}
-
-int b3CpuNarrowPhase::allocateCollidable()
-{
-	int curSize = m_data->m_collidablesCPU.size();
-	if (curSize < m_data->m_config.m_maxConvexShapes)
-	{
-		m_data->m_collidablesCPU.expand();
-		return curSize;
-	}
-	else
-	{
-		b3Error("allocateCollidable out-of-range %d\n", m_data->m_config.m_maxConvexShapes);
-	}
-	return -1;
-}
-
-int b3CpuNarrowPhase::registerConvexHullShape(const float* vertices, int strideInBytes, int numVertices, const float* scaling)
-{
-	b3AlignedObjectArray<b3Vector3> verts;
-
-	unsigned char* vts = (unsigned char*)vertices;
-	for (int i = 0; i < numVertices; i++)
-	{
-		float* vertex = (float*)&vts[i * strideInBytes];
-		verts.push_back(b3MakeVector3(vertex[0] * scaling[0], vertex[1] * scaling[1], vertex[2] * scaling[2]));
-	}
-
-	b3ConvexUtility* utilPtr = new b3ConvexUtility();
-	bool merge = true;
-	if (numVertices)
-	{
-		utilPtr->initializePolyhedralFeatures(&verts[0], verts.size(), merge);
-	}
-
-	int collidableIndex = registerConvexHullShape(utilPtr);
-
-	delete utilPtr;
-	return collidableIndex;
-}
-
-int b3CpuNarrowPhase::registerConvexHullShapeInternal(b3ConvexUtility* convexPtr, b3Collidable& col)
-{
-	m_data->m_convexData.resize(m_data->m_numAcceleratedShapes + 1);
-	m_data->m_convexPolyhedra.resize(m_data->m_numAcceleratedShapes + 1);
-
-	b3ConvexPolyhedronData& convex = m_data->m_convexPolyhedra.at(m_data->m_convexPolyhedra.size() - 1);
-	convex.mC = convexPtr->mC;
-	convex.mE = convexPtr->mE;
-	convex.m_extents = convexPtr->m_extents;
-	convex.m_localCenter = convexPtr->m_localCenter;
-	convex.m_radius = convexPtr->m_radius;
-
-	convex.m_numUniqueEdges = convexPtr->m_uniqueEdges.size();
-	int edgeOffset = m_data->m_uniqueEdges.size();
-	convex.m_uniqueEdgesOffset = edgeOffset;
-
-	m_data->m_uniqueEdges.resize(edgeOffset + convex.m_numUniqueEdges);
-
-	//convex data here
-	int i;
-	for (i = 0; i < convexPtr->m_uniqueEdges.size(); i++)
-	{
-		m_data->m_uniqueEdges[edgeOffset + i] = convexPtr->m_uniqueEdges[i];
-	}
-
-	int faceOffset = m_data->m_convexFaces.size();
-	convex.m_faceOffset = faceOffset;
-	convex.m_numFaces = convexPtr->m_faces.size();
-
-	m_data->m_convexFaces.resize(faceOffset + convex.m_numFaces);
-
-	for (i = 0; i < convexPtr->m_faces.size(); i++)
-	{
-		m_data->m_convexFaces[convex.m_faceOffset + i].m_plane = b3MakeVector3(convexPtr->m_faces[i].m_plane[0],
-																			   convexPtr->m_faces[i].m_plane[1],
-																			   convexPtr->m_faces[i].m_plane[2],
-																			   convexPtr->m_faces[i].m_plane[3]);
-
-		int indexOffset = m_data->m_convexIndices.size();
-		int numIndices = convexPtr->m_faces[i].m_indices.size();
-		m_data->m_convexFaces[convex.m_faceOffset + i].m_numIndices = numIndices;
-		m_data->m_convexFaces[convex.m_faceOffset + i].m_indexOffset = indexOffset;
-		m_data->m_convexIndices.resize(indexOffset + numIndices);
-		for (int p = 0; p < numIndices; p++)
-		{
-			m_data->m_convexIndices[indexOffset + p] = convexPtr->m_faces[i].m_indices[p];
-		}
-	}
-
-	convex.m_numVertices = convexPtr->m_vertices.size();
-	int vertexOffset = m_data->m_convexVertices.size();
-	convex.m_vertexOffset = vertexOffset;
-
-	m_data->m_convexVertices.resize(vertexOffset + convex.m_numVertices);
-	for (int i = 0; i < convexPtr->m_vertices.size(); i++)
-	{
-		m_data->m_convexVertices[vertexOffset + i] = convexPtr->m_vertices[i];
-	}
-
-	(m_data->m_convexData)[m_data->m_numAcceleratedShapes] = convexPtr;
-
-	return m_data->m_numAcceleratedShapes++;
-}
-
-const b3Aabb& b3CpuNarrowPhase::getLocalSpaceAabb(int collidableIndex) const
-{
-	return m_data->m_localShapeAABBCPU[collidableIndex];
-}

thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/b3CpuNarrowPhase.h

@@ -1,92 +0,0 @@
-#ifndef B3_CPU_NARROWPHASE_H
-#define B3_CPU_NARROWPHASE_H
-
-#include "Bullet3Collision/NarrowPhaseCollision/shared/b3Collidable.h"
-#include "Bullet3Common/b3AlignedObjectArray.h"
-#include "Bullet3Common/b3Vector3.h"
-#include "Bullet3Collision/BroadPhaseCollision/shared/b3Aabb.h"
-#include "Bullet3Common/shared/b3Int4.h"
-#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h"
-#include "Bullet3Collision/NarrowPhaseCollision/shared/b3Contact4Data.h"
-
-class b3CpuNarrowPhase
-{
-protected:
-	struct b3CpuNarrowPhaseInternalData* m_data;
-	int m_acceleratedCompanionShapeIndex;
-	int m_planeBodyIndex;
-	int m_static0Index;
-
-	int registerConvexHullShapeInternal(class b3ConvexUtility* convexPtr, b3Collidable& col);
-	int registerConcaveMeshShape(b3AlignedObjectArray<b3Vector3>* vertices, b3AlignedObjectArray<int>* indices, b3Collidable& col, const float* scaling);
-
-public:
-	b3CpuNarrowPhase(const struct b3Config& config);
-
-	virtual ~b3CpuNarrowPhase(void);
-
-	int registerSphereShape(float radius);
-	int registerPlaneShape(const b3Vector3& planeNormal, float planeConstant);
-
-	int registerCompoundShape(b3AlignedObjectArray<b3GpuChildShape>* childShapes);
-	int registerFace(const b3Vector3& faceNormal, float faceConstant);
-
-	int registerConcaveMesh(b3AlignedObjectArray<b3Vector3>* vertices, b3AlignedObjectArray<int>* indices, const float* scaling);
-
-	//do they need to be merged?
-
-	int registerConvexHullShape(b3ConvexUtility* utilPtr);
-	int registerConvexHullShape(const float* vertices, int strideInBytes, int numVertices, const float* scaling);
-
-	//int registerRigidBody(int collidableIndex, float mass, const float* position, const float* orientation, const float* aabbMin, const float* aabbMax,bool writeToGpu);
-	void setObjectTransform(const float* position, const float* orientation, int bodyIndex);
-
-	void writeAllBodiesToGpu();
-	void reset();
-	void readbackAllBodiesToCpu();
-	bool getObjectTransformFromCpu(float* position, float* orientation, int bodyIndex) const;
-
-	void setObjectTransformCpu(float* position, float* orientation, int bodyIndex);
-	void setObjectVelocityCpu(float* linVel, float* angVel, int bodyIndex);
-
-	//virtual void computeContacts(cl_mem broadphasePairs, int numBroadphasePairs, cl_mem aabbsWorldSpace, int numObjects);
-	virtual void computeContacts(b3AlignedObjectArray<b3Int4>& pairs, b3AlignedObjectArray<b3Aabb>& aabbsWorldSpace, b3AlignedObjectArray<b3RigidBodyData>& bodies);
-
-	const struct b3RigidBodyData* getBodiesCpu() const;
-	//struct b3RigidBodyData* getBodiesCpu();
-
-	int getNumBodiesGpu() const;
-
-	int getNumBodyInertiasGpu() const;
-
-	const struct b3Collidable* getCollidablesCpu() const;
-	int getNumCollidablesGpu() const;
-
-	/*const struct b3Contact4* getContactsCPU() const;
-
-	
-	int	getNumContactsGpu() const;
-	*/
-
-	const b3AlignedObjectArray<b3Contact4Data>& getContacts() const;
-
-	int getNumRigidBodies() const;
-
-	int allocateCollidable();
-
-	int getStatic0Index() const
-	{
-		return m_static0Index;
-	}
-	b3Collidable& getCollidableCpu(int collidableIndex);
-	const b3Collidable& getCollidableCpu(int collidableIndex) const;
-
-	const b3CpuNarrowPhaseInternalData* getInternalData() const
-	{
-		return m_data;
-	}
-
-	const struct b3Aabb& getLocalSpaceAabb(int collidableIndex) const;
-};
-
-#endif  //B3_CPU_NARROWPHASE_H

thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/b3RaycastInfo.h

@@ -1,25 +0,0 @@
-
-#ifndef B3_RAYCAST_INFO_H
-#define B3_RAYCAST_INFO_H
-
-#include "Bullet3Common/b3Vector3.h"
-
-B3_ATTRIBUTE_ALIGNED16(struct)
-b3RayInfo
-{
-	b3Vector3 m_from;
-	b3Vector3 m_to;
-};
-
-B3_ATTRIBUTE_ALIGNED16(struct)
-b3RayHit
-{
-	b3Scalar m_hitFraction;
-	int m_hitBody;
-	int m_hitResult1;
-	int m_hitResult2;
-	b3Vector3 m_hitPoint;
-	b3Vector3 m_hitNormal;
-};
-
-#endif  //B3_RAYCAST_INFO_H

thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/b3RigidBodyCL.h

@@ -1,28 +0,0 @@
-/*
-Bullet Continuous Collision Detection and Physics Library
-Copyright (c) 2003-2013 Erwin Coumans  http://bulletphysics.org
-
-This software is provided 'as-is', without any express or implied warranty.
-In no event will the authors be held liable for any damages arising from the use of this software.
-Permission is granted to anyone to use this software for any purpose,
-including commercial applications, and to alter it and redistribute it freely,
-subject to the following restrictions:
-
-1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
-2. Altered source versions must be plainly marked as 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 B3_RIGID_BODY_CL
-#define B3_RIGID_BODY_CL
-
-#include "Bullet3Common/b3Scalar.h"
-#include "Bullet3Common/b3Matrix3x3.h"
-#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h"
-
-inline float b3GetInvMass(const b3RigidBodyData& body)
-{
-	return body.m_invMass;
-}
-
-#endif  //B3_RIGID_BODY_CL

thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3BvhSubtreeInfoData.h

@@ -1,19 +0,0 @@
-
-#ifndef B3_BVH_SUBTREE_INFO_DATA_H
-#define B3_BVH_SUBTREE_INFO_DATA_H
-
-typedef struct b3BvhSubtreeInfoData b3BvhSubtreeInfoData_t;
-
-struct b3BvhSubtreeInfoData
-{
-	//12 bytes
-	unsigned short int m_quantizedAabbMin[3];
-	unsigned short int m_quantizedAabbMax[3];
-	//4 bytes, points to the root of the subtree
-	int m_rootNodeIndex;
-	//4 bytes
-	int m_subtreeSize;
-	int m_padding[3];
-};
-
-#endif  //B3_BVH_SUBTREE_INFO_DATA_H

thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3BvhTraversal.h

@@ -1,123 +0,0 @@
-
-
-#include "Bullet3Common/shared/b3Int4.h"
-#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h"
-#include "Bullet3Collision/NarrowPhaseCollision/shared/b3Collidable.h"
-#include "Bullet3Collision/BroadPhaseCollision/shared/b3Aabb.h"
-#include "Bullet3Collision/NarrowPhaseCollision/shared/b3BvhSubtreeInfoData.h"
-#include "Bullet3Collision/NarrowPhaseCollision/shared/b3QuantizedBvhNodeData.h"
-
-// work-in-progress
-void b3BvhTraversal(__global const b3Int4* pairs,
-					__global const b3RigidBodyData* rigidBodies,
-					__global const b3Collidable* collidables,
-					__global b3Aabb* aabbs,
-					__global b3Int4* concavePairsOut,
-					__global volatile int* numConcavePairsOut,
-					__global const b3BvhSubtreeInfo* subtreeHeadersRoot,
-					__global const b3QuantizedBvhNode* quantizedNodesRoot,
-					__global const b3BvhInfo* bvhInfos,
-					int numPairs,
-					int maxNumConcavePairsCapacity,
-					int id)
-{
-	int bodyIndexA = pairs[id].x;
-	int bodyIndexB = pairs[id].y;
-	int collidableIndexA = rigidBodies[bodyIndexA].m_collidableIdx;
-	int collidableIndexB = rigidBodies[bodyIndexB].m_collidableIdx;
-
-	//once the broadphase avoids static-static pairs, we can remove this test
-	if ((rigidBodies[bodyIndexA].m_invMass == 0) && (rigidBodies[bodyIndexB].m_invMass == 0))
-	{
-		return;
-	}
-
-	if (collidables[collidableIndexA].m_shapeType != SHAPE_CONCAVE_TRIMESH)
-		return;
-
-	int shapeTypeB = collidables[collidableIndexB].m_shapeType;
-
-	if (shapeTypeB != SHAPE_CONVEX_HULL &&
-		shapeTypeB != SHAPE_SPHERE &&
-		shapeTypeB != SHAPE_COMPOUND_OF_CONVEX_HULLS)
-		return;
-
-	b3BvhInfo bvhInfo = bvhInfos[collidables[collidableIndexA].m_numChildShapes];
-
-	b3Float4 bvhAabbMin = bvhInfo.m_aabbMin;
-	b3Float4 bvhAabbMax = bvhInfo.m_aabbMax;
-	b3Float4 bvhQuantization = bvhInfo.m_quantization;
-	int numSubtreeHeaders = bvhInfo.m_numSubTrees;
-	__global const b3BvhSubtreeInfoData* subtreeHeaders = &subtreeHeadersRoot[bvhInfo.m_subTreeOffset];
-	__global const b3QuantizedBvhNodeData* quantizedNodes = &quantizedNodesRoot[bvhInfo.m_nodeOffset];
-
-	unsigned short int quantizedQueryAabbMin[3];
-	unsigned short int quantizedQueryAabbMax[3];
-	b3QuantizeWithClamp(quantizedQueryAabbMin, aabbs[bodyIndexB].m_minVec, false, bvhAabbMin, bvhAabbMax, bvhQuantization);
-	b3QuantizeWithClamp(quantizedQueryAabbMax, aabbs[bodyIndexB].m_maxVec, true, bvhAabbMin, bvhAabbMax, bvhQuantization);
-
-	for (int i = 0; i < numSubtreeHeaders; i++)
-	{
-		b3BvhSubtreeInfoData subtree = subtreeHeaders[i];
-
-		int overlap = b3TestQuantizedAabbAgainstQuantizedAabbSlow(quantizedQueryAabbMin, quantizedQueryAabbMax, subtree.m_quantizedAabbMin, subtree.m_quantizedAabbMax);
-		if (overlap != 0)
-		{
-			int startNodeIndex = subtree.m_rootNodeIndex;
-			int endNodeIndex = subtree.m_rootNodeIndex + subtree.m_subtreeSize;
-			int curIndex = startNodeIndex;
-			int escapeIndex;
-			int isLeafNode;
-			int aabbOverlap;
-			while (curIndex < endNodeIndex)
-			{
-				b3QuantizedBvhNodeData rootNode = quantizedNodes[curIndex];
-				aabbOverlap = b3TestQuantizedAabbAgainstQuantizedAabbSlow(quantizedQueryAabbMin, quantizedQueryAabbMax, rootNode.m_quantizedAabbMin, rootNode.m_quantizedAabbMax);
-				isLeafNode = b3IsLeaf(&rootNode);
-				if (aabbOverlap)
-				{
-					if (isLeafNode)
-					{
-						int triangleIndex = b3GetTriangleIndex(&rootNode);
-						if (shapeTypeB == SHAPE_COMPOUND_OF_CONVEX_HULLS)
-						{
-							int numChildrenB = collidables[collidableIndexB].m_numChildShapes;
-							int pairIdx = b3AtomicAdd(numConcavePairsOut, numChildrenB);
-							for (int b = 0; b < numChildrenB; b++)
-							{
-								if ((pairIdx + b) < maxNumConcavePairsCapacity)
-								{
-									int childShapeIndexB = collidables[collidableIndexB].m_shapeIndex + b;
-									b3Int4 newPair = b3MakeInt4(bodyIndexA, bodyIndexB, triangleIndex, childShapeIndexB);
-									concavePairsOut[pairIdx + b] = newPair;
-								}
-							}
-						}
-						else
-						{
-							int pairIdx = b3AtomicInc(numConcavePairsOut);
-							if (pairIdx < maxNumConcavePairsCapacity)
-							{
-								b3Int4 newPair = b3MakeInt4(bodyIndexA, bodyIndexB, triangleIndex, 0);
-								concavePairsOut[pairIdx] = newPair;
-							}
-						}
-					}
-					curIndex++;
-				}
-				else
-				{
-					if (isLeafNode)
-					{
-						curIndex++;
-					}
-					else
-					{
-						escapeIndex = b3GetEscapeIndex(&rootNode);
-						curIndex += escapeIndex;
-					}
-				}
-			}
-		}
-	}
-}

thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3ClipFaces.h

@@ -1,171 +0,0 @@
-#ifndef B3_CLIP_FACES_H
-#define B3_CLIP_FACES_H
-
-#include "Bullet3Common/shared/b3Int4.h"
-#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h"
-#include "Bullet3Collision/NarrowPhaseCollision/shared/b3Collidable.h"
-#include "Bullet3Collision/BroadPhaseCollision/shared/b3Aabb.h"
-#include "Bullet3Collision/NarrowPhaseCollision/shared/b3BvhSubtreeInfoData.h"
-#include "Bullet3Collision/NarrowPhaseCollision/shared/b3QuantizedBvhNodeData.h"
-#include "Bullet3Collision/NarrowPhaseCollision/shared/b3ConvexPolyhedronData.h"
-
-inline b3Float4 b3Lerp3(b3Float4ConstArg a, b3Float4ConstArg b, float t)
-{
-	return b3MakeFloat4(a.x + (b.x - a.x) * t,
-						a.y + (b.y - a.y) * t,
-						a.z + (b.z - a.z) * t,
-						0.f);
-}
-
-// Clips a face to the back of a plane, return the number of vertices out, stored in ppVtxOut
-int clipFaceGlobal(__global const b3Float4* pVtxIn, int numVertsIn, b3Float4ConstArg planeNormalWS, float planeEqWS, __global b3Float4* ppVtxOut)
-{
-	int ve;
-	float ds, de;
-	int numVertsOut = 0;
-	//double-check next test
-	//	if (numVertsIn < 2)
-	//		return 0;
-
-	b3Float4 firstVertex = pVtxIn[numVertsIn - 1];
-	b3Float4 endVertex = pVtxIn[0];
-
-	ds = b3Dot(planeNormalWS, firstVertex) + planeEqWS;
-
-	for (ve = 0; ve < numVertsIn; ve++)
-	{
-		endVertex = pVtxIn[ve];
-		de = b3Dot(planeNormalWS, endVertex) + planeEqWS;
-		if (ds < 0)
-		{
-			if (de < 0)
-			{
-				// Start < 0, end < 0, so output endVertex
-				ppVtxOut[numVertsOut++] = endVertex;
-			}
-			else
-			{
-				// Start < 0, end >= 0, so output intersection
-				ppVtxOut[numVertsOut++] = b3Lerp3(firstVertex, endVertex, (ds * 1.f / (ds - de)));
-			}
-		}
-		else
-		{
-			if (de < 0)
-			{
-				// Start >= 0, end < 0 so output intersection and end
-				ppVtxOut[numVertsOut++] = b3Lerp3(firstVertex, endVertex, (ds * 1.f / (ds - de)));
-				ppVtxOut[numVertsOut++] = endVertex;
-			}
-		}
-		firstVertex = endVertex;
-		ds = de;
-	}
-	return numVertsOut;
-}
-
-__kernel void clipFacesAndFindContactsKernel(__global const b3Float4* separatingNormals,
-											 __global const int* hasSeparatingAxis,
-											 __global b3Int4* clippingFacesOut,
-											 __global b3Float4* worldVertsA1,
-											 __global b3Float4* worldNormalsA1,
-											 __global b3Float4* worldVertsB1,
-											 __global b3Float4* worldVertsB2,
-											 int vertexFaceCapacity,
-											 int pairIndex)
-{
-	//    int i = get_global_id(0);
-	//int pairIndex = i;
-	int i = pairIndex;
-
-	float minDist = -1e30f;
-	float maxDist = 0.02f;
-
-	//	if (i<numPairs)
-	{
-		if (hasSeparatingAxis[i])
-		{
-			//			int bodyIndexA = pairs[i].x;
-			//		int bodyIndexB = pairs[i].y;
-
-			int numLocalContactsOut = 0;
-
-			int capacityWorldVertsB2 = vertexFaceCapacity;
-
-			__global b3Float4* pVtxIn = &worldVertsB1[pairIndex * capacityWorldVertsB2];
-			__global b3Float4* pVtxOut = &worldVertsB2[pairIndex * capacityWorldVertsB2];
-
-			{
-				__global b3Int4* clippingFaces = clippingFacesOut;
-
-				int closestFaceA = clippingFaces[pairIndex].x;
-				// int closestFaceB = clippingFaces[pairIndex].y;
-				int numVertsInA = clippingFaces[pairIndex].z;
-				int numVertsInB = clippingFaces[pairIndex].w;
-
-				int numVertsOut = 0;
-
-				if (closestFaceA >= 0)
-				{
-					// clip polygon to back of planes of all faces of hull A that are adjacent to witness face
-
-					for (int e0 = 0; e0 < numVertsInA; e0++)
-					{
-						const b3Float4 aw = worldVertsA1[pairIndex * capacityWorldVertsB2 + e0];
-						const b3Float4 bw = worldVertsA1[pairIndex * capacityWorldVertsB2 + ((e0 + 1) % numVertsInA)];
-						const b3Float4 WorldEdge0 = aw - bw;
-						b3Float4 worldPlaneAnormal1 = worldNormalsA1[pairIndex];
-						b3Float4 planeNormalWS1 = -b3Cross(WorldEdge0, worldPlaneAnormal1);
-						b3Float4 worldA1 = aw;
-						float planeEqWS1 = -b3Dot(worldA1, planeNormalWS1);
-						b3Float4 planeNormalWS = planeNormalWS1;
-						float planeEqWS = planeEqWS1;
-						numVertsOut = clipFaceGlobal(pVtxIn, numVertsInB, planeNormalWS, planeEqWS, pVtxOut);
-						__global b3Float4* tmp = pVtxOut;
-						pVtxOut = pVtxIn;
-						pVtxIn = tmp;
-						numVertsInB = numVertsOut;
-						numVertsOut = 0;
-					}
-
-					b3Float4 planeNormalWS = worldNormalsA1[pairIndex];
-					float planeEqWS = -b3Dot(planeNormalWS, worldVertsA1[pairIndex * capacityWorldVertsB2]);
-
-					for (int i = 0; i < numVertsInB; i++)
-					{
-						float depth = b3Dot(planeNormalWS, pVtxIn[i]) + planeEqWS;
-						if (depth <= minDist)
-						{
-							depth = minDist;
-						}
-						/*
-						static float maxDepth = 0.f;
-						if (depth < maxDepth)
-						{
-							maxDepth = depth;
-							if (maxDepth < -10)
-							{
-								printf("error at framecount %d?\n",myframecount);
-							}
-							printf("maxDepth = %f\n", maxDepth);
-
-						}
-*/
-						if (depth <= maxDist)
-						{
-							b3Float4 pointInWorld = pVtxIn[i];
-							pVtxOut[numLocalContactsOut++] = b3MakeFloat4(pointInWorld.x, pointInWorld.y, pointInWorld.z, depth);
-						}
-					}
-				}
-				clippingFaces[pairIndex].w = numLocalContactsOut;
-			}
-
-			for (int i = 0; i < numLocalContactsOut; i++)
-				pVtxIn[i] = pVtxOut[i];
-
-		}  //		if (hasSeparatingAxis[i])
-	}      //	if (i<numPairs)
-}
-
-#endif  //B3_CLIP_FACES_H

thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3Collidable.h

@@ -1,69 +0,0 @@
-
-#ifndef B3_COLLIDABLE_H
-#define B3_COLLIDABLE_H
-
-#include "Bullet3Common/shared/b3Float4.h"
-#include "Bullet3Common/shared/b3Quat.h"
-
-enum b3ShapeTypes
-{
-	SHAPE_HEIGHT_FIELD = 1,
-
-	SHAPE_CONVEX_HULL = 3,
-	SHAPE_PLANE = 4,
-	SHAPE_CONCAVE_TRIMESH = 5,
-	SHAPE_COMPOUND_OF_CONVEX_HULLS = 6,
-	SHAPE_SPHERE = 7,
-	MAX_NUM_SHAPE_TYPES,
-};
-
-typedef struct b3Collidable b3Collidable_t;
-
-struct b3Collidable
-{
-	union {
-		int m_numChildShapes;
-		int m_bvhIndex;
-	};
-	union {
-		float m_radius;
-		int m_compoundBvhIndex;
-	};
-
-	int m_shapeType;
-	union {
-		int m_shapeIndex;
-		float m_height;
-	};
-};
-
-typedef struct b3GpuChildShape b3GpuChildShape_t;
-struct b3GpuChildShape
-{
-	b3Float4 m_childPosition;
-	b3Quat m_childOrientation;
-	union {
-		int m_shapeIndex;  //used for SHAPE_COMPOUND_OF_CONVEX_HULLS
-		int m_capsuleAxis;
-	};
-	union {
-		float m_radius;        //used for childshape of SHAPE_COMPOUND_OF_SPHERES or SHAPE_COMPOUND_OF_CAPSULES
-		int m_numChildShapes;  //used for compound shape
-	};
-	union {
-		float m_height;  //used for childshape of SHAPE_COMPOUND_OF_CAPSULES
-		int m_collidableShapeIndex;
-	};
-	int m_shapeType;
-};
-
-struct b3CompoundOverlappingPair
-{
-	int m_bodyIndexA;
-	int m_bodyIndexB;
-	//	int	m_pairType;
-	int m_childShapeIndexA;
-	int m_childShapeIndexB;
-};
-
-#endif  //B3_COLLIDABLE_H

thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3Contact4Data.h

@@ -1,36 +0,0 @@
-#ifndef B3_CONTACT4DATA_H
-#define B3_CONTACT4DATA_H
-
-#include "Bullet3Common/shared/b3Float4.h"
-
-typedef struct b3Contact4Data b3Contact4Data_t;
-
-struct b3Contact4Data
-{
-	b3Float4 m_worldPosB[4];
-	//	b3Float4	m_localPosA[4];
-	//	b3Float4	m_localPosB[4];
-	b3Float4 m_worldNormalOnB;  //	w: m_nPoints
-	unsigned short m_restituitionCoeffCmp;
-	unsigned short m_frictionCoeffCmp;
-	int m_batchIdx;
-	int m_bodyAPtrAndSignBit;  //x:m_bodyAPtr, y:m_bodyBPtr
-	int m_bodyBPtrAndSignBit;
-
-	int m_childIndexA;
-	int m_childIndexB;
-	int m_unused1;
-	int m_unused2;
-};
-
-inline int b3Contact4Data_getNumPoints(const struct b3Contact4Data* contact)
-{
-	return (int)contact->m_worldNormalOnB.w;
-};
-
-inline void b3Contact4Data_setNumPoints(struct b3Contact4Data* contact, int numPoints)
-{
-	contact->m_worldNormalOnB.w = (float)numPoints;
-};
-
-#endif  //B3_CONTACT4DATA_H

thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3ContactConvexConvexSAT.h

@@ -1,486 +0,0 @@
-
-#ifndef B3_CONTACT_CONVEX_CONVEX_SAT_H
-#define B3_CONTACT_CONVEX_CONVEX_SAT_H
-
-#include "Bullet3Collision/NarrowPhaseCollision/shared/b3Contact4Data.h"
-#include "Bullet3Collision/NarrowPhaseCollision/shared/b3FindSeparatingAxis.h"
-#include "Bullet3Collision/NarrowPhaseCollision/shared/b3ReduceContacts.h"
-
-#define B3_MAX_VERTS 1024
-
-inline b3Float4 b3Lerp3(const b3Float4& a, const b3Float4& b, float t)
-{
-	return b3MakeVector3(a.x + (b.x - a.x) * t,
-						 a.y + (b.y - a.y) * t,
-						 a.z + (b.z - a.z) * t,
-						 0.f);
-}
-
-// Clips a face to the back of a plane, return the number of vertices out, stored in ppVtxOut
-inline int b3ClipFace(const b3Float4* pVtxIn, int numVertsIn, b3Float4& planeNormalWS, float planeEqWS, b3Float4* ppVtxOut)
-{
-	int ve;
-	float ds, de;
-	int numVertsOut = 0;
-	if (numVertsIn < 2)
-		return 0;
-
-	b3Float4 firstVertex = pVtxIn[numVertsIn - 1];
-	b3Float4 endVertex = pVtxIn[0];
-
-	ds = b3Dot3F4(planeNormalWS, firstVertex) + planeEqWS;
-
-	for (ve = 0; ve < numVertsIn; ve++)
-	{
-		endVertex = pVtxIn[ve];
-
-		de = b3Dot3F4(planeNormalWS, endVertex) + planeEqWS;
-
-		if (ds < 0)
-		{
-			if (de < 0)
-			{
-				// Start < 0, end < 0, so output endVertex
-				ppVtxOut[numVertsOut++] = endVertex;
-			}
-			else
-			{
-				// Start < 0, end >= 0, so output intersection
-				ppVtxOut[numVertsOut++] = b3Lerp3(firstVertex, endVertex, (ds * 1.f / (ds - de)));
-			}
-		}
-		else
-		{
-			if (de < 0)
-			{
-				// Start >= 0, end < 0 so output intersection and end
-				ppVtxOut[numVertsOut++] = b3Lerp3(firstVertex, endVertex, (ds * 1.f / (ds - de)));
-				ppVtxOut[numVertsOut++] = endVertex;
-			}
-		}
-		firstVertex = endVertex;
-		ds = de;
-	}
-	return numVertsOut;
-}
-
-inline int b3ClipFaceAgainstHull(const b3Float4& separatingNormal, const b3ConvexPolyhedronData* hullA,
-								 const b3Float4& posA, const b3Quaternion& ornA, b3Float4* worldVertsB1, int numWorldVertsB1,
-								 b3Float4* worldVertsB2, int capacityWorldVertsB2,
-								 const float minDist, float maxDist,
-								 const b3AlignedObjectArray<b3Float4>& verticesA, const b3AlignedObjectArray<b3GpuFace>& facesA, const b3AlignedObjectArray<int>& indicesA,
-								 //const b3Float4* verticesB,	const b3GpuFace* facesB,	const int* indicesB,
-								 b3Float4* contactsOut,
-								 int contactCapacity)
-{
-	int numContactsOut = 0;
-
-	b3Float4* pVtxIn = worldVertsB1;
-	b3Float4* pVtxOut = worldVertsB2;
-
-	int numVertsIn = numWorldVertsB1;
-	int numVertsOut = 0;
-
-	int closestFaceA = -1;
-	{
-		float dmin = FLT_MAX;
-		for (int face = 0; face < hullA->m_numFaces; face++)
-		{
-			const b3Float4 Normal = b3MakeVector3(
-				facesA[hullA->m_faceOffset + face].m_plane.x,
-				facesA[hullA->m_faceOffset + face].m_plane.y,
-				facesA[hullA->m_faceOffset + face].m_plane.z, 0.f);
-			const b3Float4 faceANormalWS = b3QuatRotate(ornA, Normal);
-
-			float d = b3Dot3F4(faceANormalWS, separatingNormal);
-			if (d < dmin)
-			{
-				dmin = d;
-				closestFaceA = face;
-			}
-		}
-	}
-	if (closestFaceA < 0)
-		return numContactsOut;
-
-	b3GpuFace polyA = facesA[hullA->m_faceOffset + closestFaceA];
-
-	// clip polygon to back of planes of all faces of hull A that are adjacent to witness face
-	//int numContacts = numWorldVertsB1;
-	int numVerticesA = polyA.m_numIndices;
-	for (int e0 = 0; e0 < numVerticesA; e0++)
-	{
-		const b3Float4 a = verticesA[hullA->m_vertexOffset + indicesA[polyA.m_indexOffset + e0]];
-		const b3Float4 b = verticesA[hullA->m_vertexOffset + indicesA[polyA.m_indexOffset + ((e0 + 1) % numVerticesA)]];
-		const b3Float4 edge0 = a - b;
-		const b3Float4 WorldEdge0 = b3QuatRotate(ornA, edge0);
-		b3Float4 planeNormalA = b3MakeFloat4(polyA.m_plane.x, polyA.m_plane.y, polyA.m_plane.z, 0.f);
-		b3Float4 worldPlaneAnormal1 = b3QuatRotate(ornA, planeNormalA);
-
-		b3Float4 planeNormalWS1 = -b3Cross3(WorldEdge0, worldPlaneAnormal1);
-		b3Float4 worldA1 = b3TransformPoint(a, posA, ornA);
-		float planeEqWS1 = -b3Dot3F4(worldA1, planeNormalWS1);
-
-		b3Float4 planeNormalWS = planeNormalWS1;
-		float planeEqWS = planeEqWS1;
-
-		//clip face
-		//clipFace(*pVtxIn, *pVtxOut,planeNormalWS,planeEqWS);
-		numVertsOut = b3ClipFace(pVtxIn, numVertsIn, planeNormalWS, planeEqWS, pVtxOut);
-
-		//btSwap(pVtxIn,pVtxOut);
-		b3Float4* tmp = pVtxOut;
-		pVtxOut = pVtxIn;
-		pVtxIn = tmp;
-		numVertsIn = numVertsOut;
-		numVertsOut = 0;
-	}
-
-	// only keep points that are behind the witness face
-	{
-		b3Float4 localPlaneNormal = b3MakeFloat4(polyA.m_plane.x, polyA.m_plane.y, polyA.m_plane.z, 0.f);
-		float localPlaneEq = polyA.m_plane.w;
-		b3Float4 planeNormalWS = b3QuatRotate(ornA, localPlaneNormal);
-		float planeEqWS = localPlaneEq - b3Dot3F4(planeNormalWS, posA);
-		for (int i = 0; i < numVertsIn; i++)
-		{
-			float depth = b3Dot3F4(planeNormalWS, pVtxIn[i]) + planeEqWS;
-			if (depth <= minDist)
-			{
-				depth = minDist;
-			}
-			if (numContactsOut < contactCapacity)
-			{
-				if (depth <= maxDist)
-				{
-					b3Float4 pointInWorld = pVtxIn[i];
-					//resultOut.addContactPoint(separatingNormal,point,depth);
-					contactsOut[numContactsOut++] = b3MakeVector3(pointInWorld.x, pointInWorld.y, pointInWorld.z, depth);
-					//printf("depth=%f\n",depth);
-				}
-			}
-			else
-			{
-				b3Error("exceeding contact capacity (%d,%df)\n", numContactsOut, contactCapacity);
-			}
-		}
-	}
-
-	return numContactsOut;
-}
-
-inline int b3ClipHullAgainstHull(const b3Float4& separatingNormal,
-								 const b3ConvexPolyhedronData& hullA, const b3ConvexPolyhedronData& hullB,
-								 const b3Float4& posA, const b3Quaternion& ornA, const b3Float4& posB, const b3Quaternion& ornB,
-								 b3Float4* worldVertsB1, b3Float4* worldVertsB2, int capacityWorldVerts,
-								 const float minDist, float maxDist,
-								 const b3AlignedObjectArray<b3Float4>& verticesA, const b3AlignedObjectArray<b3GpuFace>& facesA, const b3AlignedObjectArray<int>& indicesA,
-								 const b3AlignedObjectArray<b3Float4>& verticesB, const b3AlignedObjectArray<b3GpuFace>& facesB, const b3AlignedObjectArray<int>& indicesB,
-
-								 b3Float4* contactsOut,
-								 int contactCapacity)
-{
-	int numContactsOut = 0;
-	int numWorldVertsB1 = 0;
-
-	B3_PROFILE("clipHullAgainstHull");
-
-	//float curMaxDist=maxDist;
-	int closestFaceB = -1;
-	float dmax = -FLT_MAX;
-
-	{
-		//B3_PROFILE("closestFaceB");
-		if (hullB.m_numFaces != 1)
-		{
-			//printf("wtf\n");
-		}
-		static bool once = true;
-		//printf("separatingNormal=%f,%f,%f\n",separatingNormal.x,separatingNormal.y,separatingNormal.z);
-
-		for (int face = 0; face < hullB.m_numFaces; face++)
-		{
-#ifdef BT_DEBUG_SAT_FACE
-			if (once)
-				printf("face %d\n", face);
-			const b3GpuFace* faceB = &facesB[hullB.m_faceOffset + face];
-			if (once)
-			{
-				for (int i = 0; i < faceB->m_numIndices; i++)
-				{
-					b3Float4 vert = verticesB[hullB.m_vertexOffset + indicesB[faceB->m_indexOffset + i]];
-					printf("vert[%d] = %f,%f,%f\n", i, vert.x, vert.y, vert.z);
-				}
-			}
-#endif  //BT_DEBUG_SAT_FACE \
-	//if (facesB[hullB.m_faceOffset+face].m_numIndices>2)
-			{
-				const b3Float4 Normal = b3MakeVector3(facesB[hullB.m_faceOffset + face].m_plane.x,
-													  facesB[hullB.m_faceOffset + face].m_plane.y, facesB[hullB.m_faceOffset + face].m_plane.z, 0.f);
-				const b3Float4 WorldNormal = b3QuatRotate(ornB, Normal);
-#ifdef BT_DEBUG_SAT_FACE
-				if (once)
-					printf("faceNormal = %f,%f,%f\n", Normal.x, Normal.y, Normal.z);
-#endif
-				float d = b3Dot3F4(WorldNormal, separatingNormal);
-				if (d > dmax)
-				{
-					dmax = d;
-					closestFaceB = face;
-				}
-			}
-		}
-		once = false;
-	}
-
-	b3Assert(closestFaceB >= 0);
-	{
-		//B3_PROFILE("worldVertsB1");
-		const b3GpuFace& polyB = facesB[hullB.m_faceOffset + closestFaceB];
-		const int numVertices = polyB.m_numIndices;
-		for (int e0 = 0; e0 < numVertices; e0++)
-		{
-			const b3Float4& b = verticesB[hullB.m_vertexOffset + indicesB[polyB.m_indexOffset + e0]];
-			worldVertsB1[numWorldVertsB1++] = b3TransformPoint(b, posB, ornB);
-		}
-	}
-
-	if (closestFaceB >= 0)
-	{
-		//B3_PROFILE("clipFaceAgainstHull");
-		numContactsOut = b3ClipFaceAgainstHull((b3Float4&)separatingNormal, &hullA,
-											   posA, ornA,
-											   worldVertsB1, numWorldVertsB1, worldVertsB2, capacityWorldVerts, minDist, maxDist,
-											   verticesA, facesA, indicesA,
-											   contactsOut, contactCapacity);
-	}
-
-	return numContactsOut;
-}
-
-inline int b3ClipHullHullSingle(
-	int bodyIndexA, int bodyIndexB,
-	const b3Float4& posA,
-	const b3Quaternion& ornA,
-	const b3Float4& posB,
-	const b3Quaternion& ornB,
-
-	int collidableIndexA, int collidableIndexB,
-
-	const b3AlignedObjectArray<b3RigidBodyData>* bodyBuf,
-	b3AlignedObjectArray<b3Contact4Data>* globalContactOut,
-	int& nContacts,
-
-	const b3AlignedObjectArray<b3ConvexPolyhedronData>& hostConvexDataA,
-	const b3AlignedObjectArray<b3ConvexPolyhedronData>& hostConvexDataB,
-
-	const b3AlignedObjectArray<b3Vector3>& verticesA,
-	const b3AlignedObjectArray<b3Vector3>& uniqueEdgesA,
-	const b3AlignedObjectArray<b3GpuFace>& facesA,
-	const b3AlignedObjectArray<int>& indicesA,
-
-	const b3AlignedObjectArray<b3Vector3>& verticesB,
-	const b3AlignedObjectArray<b3Vector3>& uniqueEdgesB,
-	const b3AlignedObjectArray<b3GpuFace>& facesB,
-	const b3AlignedObjectArray<int>& indicesB,
-
-	const b3AlignedObjectArray<b3Collidable>& hostCollidablesA,
-	const b3AlignedObjectArray<b3Collidable>& hostCollidablesB,
-	const b3Vector3& sepNormalWorldSpace,
-	int maxContactCapacity)
-{
-	int contactIndex = -1;
-	b3ConvexPolyhedronData hullA, hullB;
-
-	b3Collidable colA = hostCollidablesA[collidableIndexA];
-	hullA = hostConvexDataA[colA.m_shapeIndex];
-	//printf("numvertsA = %d\n",hullA.m_numVertices);
-
-	b3Collidable colB = hostCollidablesB[collidableIndexB];
-	hullB = hostConvexDataB[colB.m_shapeIndex];
-	//printf("numvertsB = %d\n",hullB.m_numVertices);
-
-	b3Float4 contactsOut[B3_MAX_VERTS];
-	int localContactCapacity = B3_MAX_VERTS;
-
-#ifdef _WIN32
-	b3Assert(_finite(bodyBuf->at(bodyIndexA).m_pos.x));
-	b3Assert(_finite(bodyBuf->at(bodyIndexB).m_pos.x));
-#endif
-
-	{
-		b3Float4 worldVertsB1[B3_MAX_VERTS];
-		b3Float4 worldVertsB2[B3_MAX_VERTS];
-		int capacityWorldVerts = B3_MAX_VERTS;
-
-		b3Float4 hostNormal = b3MakeFloat4(sepNormalWorldSpace.x, sepNormalWorldSpace.y, sepNormalWorldSpace.z, 0.f);
-		int shapeA = hostCollidablesA[collidableIndexA].m_shapeIndex;
-		int shapeB = hostCollidablesB[collidableIndexB].m_shapeIndex;
-
-		b3Scalar minDist = -1;
-		b3Scalar maxDist = 0.;
-
-		b3Transform trA, trB;
-		{
-			//B3_PROFILE("b3TransformPoint computation");
-			//trA.setIdentity();
-			trA.setOrigin(b3MakeVector3(posA.x, posA.y, posA.z));
-			trA.setRotation(b3Quaternion(ornA.x, ornA.y, ornA.z, ornA.w));
-
-			//trB.setIdentity();
-			trB.setOrigin(b3MakeVector3(posB.x, posB.y, posB.z));
-			trB.setRotation(b3Quaternion(ornB.x, ornB.y, ornB.z, ornB.w));
-		}
-
-		b3Quaternion trAorn = trA.getRotation();
-		b3Quaternion trBorn = trB.getRotation();
-
-		int numContactsOut = b3ClipHullAgainstHull(hostNormal,
-												   hostConvexDataA.at(shapeA),
-												   hostConvexDataB.at(shapeB),
-												   (b3Float4&)trA.getOrigin(), (b3Quaternion&)trAorn,
-												   (b3Float4&)trB.getOrigin(), (b3Quaternion&)trBorn,
-												   worldVertsB1, worldVertsB2, capacityWorldVerts,
-												   minDist, maxDist,
-												   verticesA, facesA, indicesA,
-												   verticesB, facesB, indicesB,
-
-												   contactsOut, localContactCapacity);
-
-		if (numContactsOut > 0)
-		{
-			B3_PROFILE("overlap");
-
-			b3Float4 normalOnSurfaceB = (b3Float4&)hostNormal;
-			//			b3Float4 centerOut;
-
-			b3Int4 contactIdx;
-			contactIdx.x = 0;
-			contactIdx.y = 1;
-			contactIdx.z = 2;
-			contactIdx.w = 3;
-
-			int numPoints = 0;
-
-			{
-				B3_PROFILE("extractManifold");
-				numPoints = b3ReduceContacts(contactsOut, numContactsOut, normalOnSurfaceB, &contactIdx);
-			}
-
-			b3Assert(numPoints);
-
-			if (nContacts < maxContactCapacity)
-			{
-				contactIndex = nContacts;
-				globalContactOut->expand();
-				b3Contact4Data& contact = globalContactOut->at(nContacts);
-				contact.m_batchIdx = 0;  //i;
-				contact.m_bodyAPtrAndSignBit = (bodyBuf->at(bodyIndexA).m_invMass == 0) ? -bodyIndexA : bodyIndexA;
-				contact.m_bodyBPtrAndSignBit = (bodyBuf->at(bodyIndexB).m_invMass == 0) ? -bodyIndexB : bodyIndexB;
-
-				contact.m_frictionCoeffCmp = 45874;
-				contact.m_restituitionCoeffCmp = 0;
-
-				//	float distance = 0.f;
-				for (int p = 0; p < numPoints; p++)
-				{
-					contact.m_worldPosB[p] = contactsOut[contactIdx.s[p]];  //check if it is actually on B
-					contact.m_worldNormalOnB = normalOnSurfaceB;
-				}
-				//printf("bodyIndexA %d,bodyIndexB %d,normal=%f,%f,%f numPoints %d\n",bodyIndexA,bodyIndexB,normalOnSurfaceB.x,normalOnSurfaceB.y,normalOnSurfaceB.z,numPoints);
-				contact.m_worldNormalOnB.w = (b3Scalar)numPoints;
-				nContacts++;
-			}
-			else
-			{
-				b3Error("Error: exceeding contact capacity (%d/%d)\n", nContacts, maxContactCapacity);
-			}
-		}
-	}
-	return contactIndex;
-}
-
-inline int b3ContactConvexConvexSAT(
-	int pairIndex,
-	int bodyIndexA, int bodyIndexB,
-	int collidableIndexA, int collidableIndexB,
-	const b3AlignedObjectArray<b3RigidBodyData>& rigidBodies,
-	const b3AlignedObjectArray<b3Collidable>& collidables,
-	const b3AlignedObjectArray<b3ConvexPolyhedronData>& convexShapes,
-	const b3AlignedObjectArray<b3Float4>& convexVertices,
-	const b3AlignedObjectArray<b3Float4>& uniqueEdges,
-	const b3AlignedObjectArray<int>& convexIndices,
-	const b3AlignedObjectArray<b3GpuFace>& faces,
-	b3AlignedObjectArray<b3Contact4Data>& globalContactsOut,
-	int& nGlobalContactsOut,
-	int maxContactCapacity)
-{
-	int contactIndex = -1;
-
-	b3Float4 posA = rigidBodies[bodyIndexA].m_pos;
-	b3Quaternion ornA = rigidBodies[bodyIndexA].m_quat;
-	b3Float4 posB = rigidBodies[bodyIndexB].m_pos;
-	b3Quaternion ornB = rigidBodies[bodyIndexB].m_quat;
-
-	b3ConvexPolyhedronData hullA, hullB;
-
-	b3Float4 sepNormalWorldSpace;
-
-	b3Collidable colA = collidables[collidableIndexA];
-	hullA = convexShapes[colA.m_shapeIndex];
-	//printf("numvertsA = %d\n",hullA.m_numVertices);
-
-	b3Collidable colB = collidables[collidableIndexB];
-	hullB = convexShapes[colB.m_shapeIndex];
-	//printf("numvertsB = %d\n",hullB.m_numVertices);
-
-#ifdef _WIN32
-	b3Assert(_finite(rigidBodies[bodyIndexA].m_pos.x));
-	b3Assert(_finite(rigidBodies[bodyIndexB].m_pos.x));
-#endif
-
-	bool foundSepAxis = b3FindSeparatingAxis(hullA, hullB,
-											 posA,
-											 ornA,
-											 posB,
-											 ornB,
-
-											 convexVertices, uniqueEdges, faces, convexIndices,
-											 convexVertices, uniqueEdges, faces, convexIndices,
-
-											 sepNormalWorldSpace);
-
-	if (foundSepAxis)
-	{
-		contactIndex = b3ClipHullHullSingle(
-			bodyIndexA, bodyIndexB,
-			posA, ornA,
-			posB, ornB,
-			collidableIndexA, collidableIndexB,
-			&rigidBodies,
-			&globalContactsOut,
-			nGlobalContactsOut,
-
-			convexShapes,
-			convexShapes,
-
-			convexVertices,
-			uniqueEdges,
-			faces,
-			convexIndices,
-
-			convexVertices,
-			uniqueEdges,
-			faces,
-			convexIndices,
-
-			collidables,
-			collidables,
-			sepNormalWorldSpace,
-			maxContactCapacity);
-	}
-
-	return contactIndex;
-}
-
-#endif  //B3_CONTACT_CONVEX_CONVEX_SAT_H

thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3ContactSphereSphere.h

@@ -1,153 +0,0 @@
-
-#ifndef B3_CONTACT_SPHERE_SPHERE_H
-#define B3_CONTACT_SPHERE_SPHERE_H
-
-void computeContactSphereConvex(int pairIndex,
-								int bodyIndexA, int bodyIndexB,
-								int collidableIndexA, int collidableIndexB,
-								const b3RigidBodyData* rigidBodies,
-								const b3Collidable* collidables,
-								const b3ConvexPolyhedronData* convexShapes,
-								const b3Vector3* convexVertices,
-								const int* convexIndices,
-								const b3GpuFace* faces,
-								b3Contact4* globalContactsOut,
-								int& nGlobalContactsOut,
-								int maxContactCapacity)
-{
-	float radius = collidables[collidableIndexA].m_radius;
-	float4 spherePos1 = rigidBodies[bodyIndexA].m_pos;
-	b3Quaternion sphereOrn = rigidBodies[bodyIndexA].m_quat;
-
-	float4 pos = rigidBodies[bodyIndexB].m_pos;
-
-	b3Quaternion quat = rigidBodies[bodyIndexB].m_quat;
-
-	b3Transform tr;
-	tr.setIdentity();
-	tr.setOrigin(pos);
-	tr.setRotation(quat);
-	b3Transform trInv = tr.inverse();
-
-	float4 spherePos = trInv(spherePos1);
-
-	int collidableIndex = rigidBodies[bodyIndexB].m_collidableIdx;
-	int shapeIndex = collidables[collidableIndex].m_shapeIndex;
-	int numFaces = convexShapes[shapeIndex].m_numFaces;
-	float4 closestPnt = b3MakeVector3(0, 0, 0, 0);
-	float4 hitNormalWorld = b3MakeVector3(0, 0, 0, 0);
-	float minDist = -1000000.f;  // TODO: What is the largest/smallest float?
-	bool bCollide = true;
-	int region = -1;
-	float4 localHitNormal;
-	for (int f = 0; f < numFaces; f++)
-	{
-		b3GpuFace face = faces[convexShapes[shapeIndex].m_faceOffset + f];
-		float4 planeEqn;
-		float4 localPlaneNormal = b3MakeVector3(face.m_plane.x, face.m_plane.y, face.m_plane.z, 0.f);
-		float4 n1 = localPlaneNormal;  //quatRotate(quat,localPlaneNormal);
-		planeEqn = n1;
-		planeEqn[3] = face.m_plane.w;
-
-		float4 pntReturn;
-		float dist = signedDistanceFromPointToPlane(spherePos, planeEqn, &pntReturn);
-
-		if (dist > radius)
-		{
-			bCollide = false;
-			break;
-		}
-
-		if (dist > 0)
-		{
-			//might hit an edge or vertex
-			b3Vector3 out;
-
-			bool isInPoly = IsPointInPolygon(spherePos,
-											 &face,
-											 &convexVertices[convexShapes[shapeIndex].m_vertexOffset],
-											 convexIndices,
-											 &out);
-			if (isInPoly)
-			{
-				if (dist > minDist)
-				{
-					minDist = dist;
-					closestPnt = pntReturn;
-					localHitNormal = planeEqn;
-					region = 1;
-				}
-			}
-			else
-			{
-				b3Vector3 tmp = spherePos - out;
-				b3Scalar l2 = tmp.length2();
-				if (l2 < radius * radius)
-				{
-					dist = b3Sqrt(l2);
-					if (dist > minDist)
-					{
-						minDist = dist;
-						closestPnt = out;
-						localHitNormal = tmp / dist;
-						region = 2;
-					}
-				}
-				else
-				{
-					bCollide = false;
-					break;
-				}
-			}
-		}
-		else
-		{
-			if (dist > minDist)
-			{
-				minDist = dist;
-				closestPnt = pntReturn;
-				localHitNormal = planeEqn;
-				region = 3;
-			}
-		}
-	}
-	static int numChecks = 0;
-	numChecks++;
-
-	if (bCollide && minDist > -10000)
-	{
-		float4 normalOnSurfaceB1 = tr.getBasis() * localHitNormal;  //-hitNormalWorld;
-		float4 pOnB1 = tr(closestPnt);
-		//printf("dist ,%f,",minDist);
-		float actualDepth = minDist - radius;
-		if (actualDepth < 0)
-		{
-			//printf("actualDepth = ,%f,", actualDepth);
-			//printf("normalOnSurfaceB1 = ,%f,%f,%f,", normalOnSurfaceB1.x,normalOnSurfaceB1.y,normalOnSurfaceB1.z);
-			//printf("region=,%d,\n", region);
-			pOnB1[3] = actualDepth;
-
-			int dstIdx;
-			//    dstIdx = nGlobalContactsOut++;//AppendInc( nGlobalContactsOut, dstIdx );
-
-			if (nGlobalContactsOut < maxContactCapacity)
-			{
-				dstIdx = nGlobalContactsOut;
-				nGlobalContactsOut++;
-
-				b3Contact4* c = &globalContactsOut[dstIdx];
-				c->m_worldNormalOnB = normalOnSurfaceB1;
-				c->setFrictionCoeff(0.7);
-				c->setRestituitionCoeff(0.f);
-
-				c->m_batchIdx = pairIndex;
-				c->m_bodyAPtrAndSignBit = rigidBodies[bodyIndexA].m_invMass == 0 ? -bodyIndexA : bodyIndexA;
-				c->m_bodyBPtrAndSignBit = rigidBodies[bodyIndexB].m_invMass == 0 ? -bodyIndexB : bodyIndexB;
-				c->m_worldPosB[0] = pOnB1;
-				int numPoints = 1;
-				c->m_worldNormalOnB.w = (b3Scalar)numPoints;
-			}  //if (dstIdx < numPairs)
-		}
-	}  //if (hasCollision)
-}
-#endif  //B3_CONTACT_SPHERE_SPHERE_H

thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3ConvexPolyhedronData.h

@@ -1,38 +0,0 @@
-
-#ifndef B3_CONVEX_POLYHEDRON_DATA_H
-#define B3_CONVEX_POLYHEDRON_DATA_H
-
-#include "Bullet3Common/shared/b3Float4.h"
-#include "Bullet3Common/shared/b3Quat.h"
-
-typedef struct b3GpuFace b3GpuFace_t;
-struct b3GpuFace
-{
-	b3Float4 m_plane;
-	int m_indexOffset;
-	int m_numIndices;
-	int m_unusedPadding1;
-	int m_unusedPadding2;
-};
-
-typedef struct b3ConvexPolyhedronData b3ConvexPolyhedronData_t;
-
-struct b3ConvexPolyhedronData
-{
-	b3Float4 m_localCenter;
-	b3Float4 m_extents;
-	b3Float4 mC;
-	b3Float4 mE;
-
-	float m_radius;
-	int m_faceOffset;
-	int m_numFaces;
-	int m_numVertices;
-
-	int m_vertexOffset;
-	int m_uniqueEdgesOffset;
-	int m_numUniqueEdges;
-	int m_unused;
-};
-
-#endif  //B3_CONVEX_POLYHEDRON_DATA_H

thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3FindConcaveSatAxis.h

@@ -1,797 +0,0 @@
-#ifndef B3_FIND_CONCAVE_SEPARATING_AXIS_H
-#define B3_FIND_CONCAVE_SEPARATING_AXIS_H
-
-#define B3_TRIANGLE_NUM_CONVEX_FACES 5
-
-#include "Bullet3Common/shared/b3Int4.h"
-#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h"
-#include "Bullet3Collision/NarrowPhaseCollision/shared/b3Collidable.h"
-#include "Bullet3Collision/BroadPhaseCollision/shared/b3Aabb.h"
-#include "Bullet3Collision/NarrowPhaseCollision/shared/b3BvhSubtreeInfoData.h"
-#include "Bullet3Collision/NarrowPhaseCollision/shared/b3QuantizedBvhNodeData.h"
-#include "Bullet3Collision/NarrowPhaseCollision/shared/b3ConvexPolyhedronData.h"
-
-inline void b3Project(__global const b3ConvexPolyhedronData* hull, b3Float4ConstArg pos, b3QuatConstArg orn,
-					  const b3Float4* dir, __global const b3Float4* vertices, float* min, float* max)
-{
-	min[0] = FLT_MAX;
-	max[0] = -FLT_MAX;
-	int numVerts = hull->m_numVertices;
-
-	const b3Float4 localDir = b3QuatRotate(b3QuatInverse(orn), *dir);
-	float offset = b3Dot(pos, *dir);
-	for (int i = 0; i < numVerts; i++)
-	{
-		float dp = b3Dot(vertices[hull->m_vertexOffset + i], localDir);
-		if (dp < min[0])
-			min[0] = dp;
-		if (dp > max[0])
-			max[0] = dp;
-	}
-	if (min[0] > max[0])
-	{
-		float tmp = min[0];
-		min[0] = max[0];
-		max[0] = tmp;
-	}
-	min[0] += offset;
-	max[0] += offset;
-}
-
-inline bool b3TestSepAxis(const b3ConvexPolyhedronData* hullA, __global const b3ConvexPolyhedronData* hullB,
-						  b3Float4ConstArg posA, b3QuatConstArg ornA,
-						  b3Float4ConstArg posB, b3QuatConstArg ornB,
-						  b3Float4* sep_axis, const b3Float4* verticesA, __global const b3Float4* verticesB, float* depth)
-{
-	float Min0, Max0;
-	float Min1, Max1;
-	b3Project(hullA, posA, ornA, sep_axis, verticesA, &Min0, &Max0);
-	b3Project(hullB, posB, ornB, sep_axis, verticesB, &Min1, &Max1);
-
-	if (Max0 < Min1 || Max1 < Min0)
-		return false;
-
-	float d0 = Max0 - Min1;
-	float d1 = Max1 - Min0;
-	*depth = d0 < d1 ? d0 : d1;
-	return true;
-}
-
-bool b3FindSeparatingAxis(const b3ConvexPolyhedronData* hullA, __global const b3ConvexPolyhedronData* hullB,
-						  b3Float4ConstArg posA1,
-						  b3QuatConstArg ornA,
-						  b3Float4ConstArg posB1,
-						  b3QuatConstArg ornB,
-						  b3Float4ConstArg DeltaC2,
-
-						  const b3Float4* verticesA,
-						  const b3Float4* uniqueEdgesA,
-						  const b3GpuFace* facesA,
-						  const int* indicesA,
-
-						  __global const b3Float4* verticesB,
-						  __global const b3Float4* uniqueEdgesB,
-						  __global const b3GpuFace* facesB,
-						  __global const int* indicesB,
-						  b3Float4* sep,
-						  float* dmin)
-{
-	b3Float4 posA = posA1;
-	posA.w = 0.f;
-	b3Float4 posB = posB1;
-	posB.w = 0.f;
-	/*
-	static int maxFaceVertex = 0;
-
-	int curFaceVertexAB = hullA->m_numFaces*hullB->m_numVertices;
-	curFaceVertexAB+= hullB->m_numFaces*hullA->m_numVertices;
-
-	if (curFaceVertexAB>maxFaceVertex)
-	{
-		maxFaceVertex = curFaceVertexAB;
-		printf("curFaceVertexAB = %d\n",curFaceVertexAB);
-		printf("hullA->m_numFaces = %d\n",hullA->m_numFaces);
-		printf("hullA->m_numVertices = %d\n",hullA->m_numVertices);
-		printf("hullB->m_numVertices = %d\n",hullB->m_numVertices);
-	}
-*/
-
-	int curPlaneTests = 0;
-	{
-		int numFacesA = hullA->m_numFaces;
-		// Test normals from hullA
-		for (int i = 0; i < numFacesA; i++)
-		{
-			const b3Float4 normal = facesA[hullA->m_faceOffset + i].m_plane;
-			b3Float4 faceANormalWS = b3QuatRotate(ornA, normal);
-			if (b3Dot(DeltaC2, faceANormalWS) < 0)
-				faceANormalWS *= -1.f;
-			curPlaneTests++;
-			float d;
-			if (!b3TestSepAxis(hullA, hullB, posA, ornA, posB, ornB, &faceANormalWS, verticesA, verticesB, &d))
-				return false;
-			if (d < *dmin)
-			{
-				*dmin = d;
-				*sep = faceANormalWS;
-			}
-		}
-	}
-	if ((b3Dot(-DeltaC2, *sep)) > 0.0f)
-	{
-		*sep = -(*sep);
-	}
-	return true;
-}
-
-b3Vector3 unitSphere162[] =
-	{
-		b3MakeVector3(0.000000, -1.000000, 0.000000),
-		b3MakeVector3(0.203181, -0.967950, 0.147618),
-		b3MakeVector3(-0.077607, -0.967950, 0.238853),
-		b3MakeVector3(0.723607, -0.447220, 0.525725),
-		b3MakeVector3(0.609547, -0.657519, 0.442856),
-		b3MakeVector3(0.812729, -0.502301, 0.295238),
-		b3MakeVector3(-0.251147, -0.967949, 0.000000),
-		b3MakeVector3(-0.077607, -0.967950, -0.238853),
-		b3MakeVector3(0.203181, -0.967950, -0.147618),
-		b3MakeVector3(0.860698, -0.251151, 0.442858),
-		b3MakeVector3(-0.276388, -0.447220, 0.850649),
-		b3MakeVector3(-0.029639, -0.502302, 0.864184),
-		b3MakeVector3(-0.155215, -0.251152, 0.955422),
-		b3MakeVector3(-0.894426, -0.447216, 0.000000),
-		b3MakeVector3(-0.831051, -0.502299, 0.238853),
-		b3MakeVector3(-0.956626, -0.251149, 0.147618),
-		b3MakeVector3(-0.276388, -0.447220, -0.850649),
-		b3MakeVector3(-0.483971, -0.502302, -0.716565),
-		b3MakeVector3(-0.436007, -0.251152, -0.864188),
-		b3MakeVector3(0.723607, -0.447220, -0.525725),
-		b3MakeVector3(0.531941, -0.502302, -0.681712),
-		b3MakeVector3(0.687159, -0.251152, -0.681715),
-		b3MakeVector3(0.687159, -0.251152, 0.681715),
-		b3MakeVector3(-0.436007, -0.251152, 0.864188),
-		b3MakeVector3(-0.956626, -0.251149, -0.147618),
-		b3MakeVector3(-0.155215, -0.251152, -0.955422),
-		b3MakeVector3(0.860698, -0.251151, -0.442858),
-		b3MakeVector3(0.276388, 0.447220, 0.850649),
-		b3MakeVector3(0.483971, 0.502302, 0.716565),
-		b3MakeVector3(0.232822, 0.657519, 0.716563),
-		b3MakeVector3(-0.723607, 0.447220, 0.525725),
-		b3MakeVector3(-0.531941, 0.502302, 0.681712),
-		b3MakeVector3(-0.609547, 0.657519, 0.442856),
-		b3MakeVector3(-0.723607, 0.447220, -0.525725),
-		b3MakeVector3(-0.812729, 0.502301, -0.295238),
-		b3MakeVector3(-0.609547, 0.657519, -0.442856),
-		b3MakeVector3(0.276388, 0.447220, -0.850649),
-		b3MakeVector3(0.029639, 0.502302, -0.864184),
-		b3MakeVector3(0.232822, 0.657519, -0.716563),
-		b3MakeVector3(0.894426, 0.447216, 0.000000),
-		b3MakeVector3(0.831051, 0.502299, -0.238853),
-		b3MakeVector3(0.753442, 0.657515, 0.000000),
-		b3MakeVector3(-0.232822, -0.657519, 0.716563),
-		b3MakeVector3(-0.162456, -0.850654, 0.499995),
-		b3MakeVector3(0.052790, -0.723612, 0.688185),
-		b3MakeVector3(0.138199, -0.894429, 0.425321),
-		b3MakeVector3(0.262869, -0.525738, 0.809012),
-		b3MakeVector3(0.361805, -0.723611, 0.587779),
-		b3MakeVector3(0.531941, -0.502302, 0.681712),
-		b3MakeVector3(0.425323, -0.850654, 0.309011),
-		b3MakeVector3(0.812729, -0.502301, -0.295238),
-		b3MakeVector3(0.609547, -0.657519, -0.442856),
-		b3MakeVector3(0.850648, -0.525736, 0.000000),
-		b3MakeVector3(0.670817, -0.723611, -0.162457),
-		b3MakeVector3(0.670817, -0.723610, 0.162458),
-		b3MakeVector3(0.425323, -0.850654, -0.309011),
-		b3MakeVector3(0.447211, -0.894428, 0.000001),
-		b3MakeVector3(-0.753442, -0.657515, 0.000000),
-		b3MakeVector3(-0.525730, -0.850652, 0.000000),
-		b3MakeVector3(-0.638195, -0.723609, 0.262864),
-		b3MakeVector3(-0.361801, -0.894428, 0.262864),
-		b3MakeVector3(-0.688189, -0.525736, 0.499997),
-		b3MakeVector3(-0.447211, -0.723610, 0.525729),
-		b3MakeVector3(-0.483971, -0.502302, 0.716565),
-		b3MakeVector3(-0.232822, -0.657519, -0.716563),
-		b3MakeVector3(-0.162456, -0.850654, -0.499995),
-		b3MakeVector3(-0.447211, -0.723611, -0.525727),
-		b3MakeVector3(-0.361801, -0.894429, -0.262863),
-		b3MakeVector3(-0.688189, -0.525736, -0.499997),
-		b3MakeVector3(-0.638195, -0.723609, -0.262863),
-		b3MakeVector3(-0.831051, -0.502299, -0.238853),
-		b3MakeVector3(0.361804, -0.723612, -0.587779),
-		b3MakeVector3(0.138197, -0.894429, -0.425321),
-		b3MakeVector3(0.262869, -0.525738, -0.809012),
-		b3MakeVector3(0.052789, -0.723611, -0.688186),
-		b3MakeVector3(-0.029639, -0.502302, -0.864184),
-		b3MakeVector3(0.956626, 0.251149, 0.147618),
-		b3MakeVector3(0.956626, 0.251149, -0.147618),
-		b3MakeVector3(0.951058, -0.000000, 0.309013),
-		b3MakeVector3(1.000000, 0.000000, 0.000000),
-		b3MakeVector3(0.947213, -0.276396, 0.162458),
-		b3MakeVector3(0.951058, 0.000000, -0.309013),
-		b3MakeVector3(0.947213, -0.276396, -0.162458),
-		b3MakeVector3(0.155215, 0.251152, 0.955422),
-		b3MakeVector3(0.436007, 0.251152, 0.864188),
-		b3MakeVector3(-0.000000, -0.000000, 1.000000),
-		b3MakeVector3(0.309017, 0.000000, 0.951056),
-		b3MakeVector3(0.138199, -0.276398, 0.951055),
-		b3MakeVector3(0.587786, 0.000000, 0.809017),
-		b3MakeVector3(0.447216, -0.276398, 0.850648),
-		b3MakeVector3(-0.860698, 0.251151, 0.442858),
-		b3MakeVector3(-0.687159, 0.251152, 0.681715),
-		b3MakeVector3(-0.951058, -0.000000, 0.309013),
-		b3MakeVector3(-0.809018, 0.000000, 0.587783),
-		b3MakeVector3(-0.861803, -0.276396, 0.425324),
-		b3MakeVector3(-0.587786, 0.000000, 0.809017),
-		b3MakeVector3(-0.670819, -0.276397, 0.688191),
-		b3MakeVector3(-0.687159, 0.251152, -0.681715),
-		b3MakeVector3(-0.860698, 0.251151, -0.442858),
-		b3MakeVector3(-0.587786, -0.000000, -0.809017),
-		b3MakeVector3(-0.809018, -0.000000, -0.587783),
-		b3MakeVector3(-0.670819, -0.276397, -0.688191),
-		b3MakeVector3(-0.951058, 0.000000, -0.309013),
-		b3MakeVector3(-0.861803, -0.276396, -0.425324),
-		b3MakeVector3(0.436007, 0.251152, -0.864188),
-		b3MakeVector3(0.155215, 0.251152, -0.955422),
-		b3MakeVector3(0.587786, -0.000000, -0.809017),
-		b3MakeVector3(0.309017, -0.000000, -0.951056),
-		b3MakeVector3(0.447216, -0.276398, -0.850648),
-		b3MakeVector3(0.000000, 0.000000, -1.000000),
-		b3MakeVector3(0.138199, -0.276398, -0.951055),
-		b3MakeVector3(0.670820, 0.276396, 0.688190),
-		b3MakeVector3(0.809019, -0.000002, 0.587783),
-		b3MakeVector3(0.688189, 0.525736, 0.499997),
-		b3MakeVector3(0.861804, 0.276394, 0.425323),
-		b3MakeVector3(0.831051, 0.502299, 0.238853),
-		b3MakeVector3(-0.447216, 0.276397, 0.850649),
-		b3MakeVector3(-0.309017, -0.000001, 0.951056),
-		b3MakeVector3(-0.262869, 0.525738, 0.809012),
-		b3MakeVector3(-0.138199, 0.276397, 0.951055),
-		b3MakeVector3(0.029639, 0.502302, 0.864184),
-		b3MakeVector3(-0.947213, 0.276396, -0.162458),
-		b3MakeVector3(-1.000000, 0.000001, 0.000000),
-		b3MakeVector3(-0.850648, 0.525736, -0.000000),
-		b3MakeVector3(-0.947213, 0.276397, 0.162458),
-		b3MakeVector3(-0.812729, 0.502301, 0.295238),
-		b3MakeVector3(-0.138199, 0.276397, -0.951055),
-		b3MakeVector3(-0.309016, -0.000000, -0.951057),
-		b3MakeVector3(-0.262869, 0.525738, -0.809012),
-		b3MakeVector3(-0.447215, 0.276397, -0.850649),
-		b3MakeVector3(-0.531941, 0.502302, -0.681712),
-		b3MakeVector3(0.861804, 0.276396, -0.425322),
-		b3MakeVector3(0.809019, 0.000000, -0.587782),
-		b3MakeVector3(0.688189, 0.525736, -0.499997),
-		b3MakeVector3(0.670821, 0.276397, -0.688189),
-		b3MakeVector3(0.483971, 0.502302, -0.716565),
-		b3MakeVector3(0.077607, 0.967950, 0.238853),
-		b3MakeVector3(0.251147, 0.967949, 0.000000),
-		b3MakeVector3(0.000000, 1.000000, 0.000000),
-		b3MakeVector3(0.162456, 0.850654, 0.499995),
-		b3MakeVector3(0.361800, 0.894429, 0.262863),
-		b3MakeVector3(0.447209, 0.723612, 0.525728),
-		b3MakeVector3(0.525730, 0.850652, 0.000000),
-		b3MakeVector3(0.638194, 0.723610, 0.262864),
-		b3MakeVector3(-0.203181, 0.967950, 0.147618),
-		b3MakeVector3(-0.425323, 0.850654, 0.309011),
-		b3MakeVector3(-0.138197, 0.894430, 0.425320),
-		b3MakeVector3(-0.361804, 0.723612, 0.587778),
-		b3MakeVector3(-0.052790, 0.723612, 0.688185),
-		b3MakeVector3(-0.203181, 0.967950, -0.147618),
-		b3MakeVector3(-0.425323, 0.850654, -0.309011),
-		b3MakeVector3(-0.447210, 0.894429, 0.000000),
-		b3MakeVector3(-0.670817, 0.723611, -0.162457),
-		b3MakeVector3(-0.670817, 0.723611, 0.162457),
-		b3MakeVector3(0.077607, 0.967950, -0.238853),
-		b3MakeVector3(0.162456, 0.850654, -0.499995),
-		b3MakeVector3(-0.138197, 0.894430, -0.425320),
-		b3MakeVector3(-0.052790, 0.723612, -0.688185),
-		b3MakeVector3(-0.361804, 0.723612, -0.587778),
-		b3MakeVector3(0.361800, 0.894429, -0.262863),
-		b3MakeVector3(0.638194, 0.723610, -0.262864),
-		b3MakeVector3(0.447209, 0.723612, -0.525728)};
-
-bool b3FindSeparatingAxisEdgeEdge(const b3ConvexPolyhedronData* hullA, __global const b3ConvexPolyhedronData* hullB,
-								  b3Float4ConstArg posA1,
-								  b3QuatConstArg ornA,
-								  b3Float4ConstArg posB1,
-								  b3QuatConstArg ornB,
-								  b3Float4ConstArg DeltaC2,
-								  const b3Float4* verticesA,
-								  const b3Float4* uniqueEdgesA,
-								  const b3GpuFace* facesA,
-								  const int* indicesA,
-								  __global const b3Float4* verticesB,
-								  __global const b3Float4* uniqueEdgesB,
-								  __global const b3GpuFace* facesB,
-								  __global const int* indicesB,
-								  b3Float4* sep,
-								  float* dmin,
-								  bool searchAllEdgeEdge)
-{
-	b3Float4 posA = posA1;
-	posA.w = 0.f;
-	b3Float4 posB = posB1;
-	posB.w = 0.f;
-
-	//	int curPlaneTests=0;
-
-	int curEdgeEdge = 0;
-	// Test edges
-	static int maxEdgeTests = 0;
-	int curEdgeTests = hullA->m_numUniqueEdges * hullB->m_numUniqueEdges;
-	if (curEdgeTests > maxEdgeTests)
-	{
-		maxEdgeTests = curEdgeTests;
-		printf("maxEdgeTests = %d\n", maxEdgeTests);
-		printf("hullA->m_numUniqueEdges = %d\n", hullA->m_numUniqueEdges);
-		printf("hullB->m_numUniqueEdges = %d\n", hullB->m_numUniqueEdges);
-	}
-
-	if (searchAllEdgeEdge)
-	{
-		for (int e0 = 0; e0 < hullA->m_numUniqueEdges; e0++)
-		{
-			const b3Float4 edge0 = uniqueEdgesA[hullA->m_uniqueEdgesOffset + e0];
-			b3Float4 edge0World = b3QuatRotate(ornA, edge0);
-
-			for (int e1 = 0; e1 < hullB->m_numUniqueEdges; e1++)
-			{
-				const b3Float4 edge1 = uniqueEdgesB[hullB->m_uniqueEdgesOffset + e1];
-				b3Float4 edge1World = b3QuatRotate(ornB, edge1);
-
-				b3Float4 crossje = b3Cross(edge0World, edge1World);
-
-				curEdgeEdge++;
-				if (!b3IsAlmostZero(crossje))
-				{
-					crossje = b3Normalized(crossje);
-					if (b3Dot(DeltaC2, crossje) < 0)
-						crossje *= -1.f;
-
-					float dist;
-					bool result = true;
-					{
-						float Min0, Max0;
-						float Min1, Max1;
-						b3Project(hullA, posA, ornA, &crossje, verticesA, &Min0, &Max0);
-						b3Project(hullB, posB, ornB, &crossje, verticesB, &Min1, &Max1);
-
-						if (Max0 < Min1 || Max1 < Min0)
-							return false;
-
-						float d0 = Max0 - Min1;
-						float d1 = Max1 - Min0;
-						dist = d0 < d1 ? d0 : d1;
-						result = true;
-					}
-
-					if (dist < *dmin)
-					{
-						*dmin = dist;
-						*sep = crossje;
-					}
-				}
-			}
-		}
-	}
-	else
-	{
-		int numDirections = sizeof(unitSphere162) / sizeof(b3Vector3);
-		//printf("numDirections =%d\n",numDirections );
-
-		for (int i = 0; i < numDirections; i++)
-		{
-			b3Float4 crossje = unitSphere162[i];
-			{
-				//if (b3Dot(DeltaC2,crossje)>0)
-				{
-					float dist;
-					bool result = true;
-					{
-						float Min0, Max0;
-						float Min1, Max1;
-						b3Project(hullA, posA, ornA, &crossje, verticesA, &Min0, &Max0);
-						b3Project(hullB, posB, ornB, &crossje, verticesB, &Min1, &Max1);
-
-						if (Max0 < Min1 || Max1 < Min0)
-							return false;
-
-						float d0 = Max0 - Min1;
-						float d1 = Max1 - Min0;
-						dist = d0 < d1 ? d0 : d1;
-						result = true;
-					}
-
-					if (dist < *dmin)
-					{
-						*dmin = dist;
-						*sep = crossje;
-					}
-				}
-			}
-		}
-	}
-
-	if ((b3Dot(-DeltaC2, *sep)) > 0.0f)
-	{
-		*sep = -(*sep);
-	}
-	return true;
-}
-
-inline int b3FindClippingFaces(b3Float4ConstArg separatingNormal,
-							   __global const b3ConvexPolyhedronData_t* hullA, __global const b3ConvexPolyhedronData_t* hullB,
-							   b3Float4ConstArg posA, b3QuatConstArg ornA, b3Float4ConstArg posB, b3QuatConstArg ornB,
-							   __global b3Float4* worldVertsA1,
-							   __global b3Float4* worldNormalsA1,
-							   __global b3Float4* worldVertsB1,
-							   int capacityWorldVerts,
-							   const float minDist, float maxDist,
-							   __global const b3Float4* verticesA,
-							   __global const b3GpuFace_t* facesA,
-							   __global const int* indicesA,
-							   __global const b3Float4* verticesB,
-							   __global const b3GpuFace_t* facesB,
-							   __global const int* indicesB,
-
-							   __global b3Int4* clippingFaces, int pairIndex)
-{
-	int numContactsOut = 0;
-	int numWorldVertsB1 = 0;
-
-	int closestFaceB = -1;
-	float dmax = -FLT_MAX;
-
-	{
-		for (int face = 0; face < hullB->m_numFaces; face++)
-		{
-			const b3Float4 Normal = b3MakeFloat4(facesB[hullB->m_faceOffset + face].m_plane.x,
-												 facesB[hullB->m_faceOffset + face].m_plane.y, facesB[hullB->m_faceOffset + face].m_plane.z, 0.f);
-			const b3Float4 WorldNormal = b3QuatRotate(ornB, Normal);
-			float d = b3Dot(WorldNormal, separatingNormal);
-			if (d > dmax)
-			{
-				dmax = d;
-				closestFaceB = face;
-			}
-		}
-	}
-
-	{
-		const b3GpuFace_t polyB = facesB[hullB->m_faceOffset + closestFaceB];
-		const int numVertices = polyB.m_numIndices;
-		for (int e0 = 0; e0 < numVertices; e0++)
-		{
-			const b3Float4 b = verticesB[hullB->m_vertexOffset + indicesB[polyB.m_indexOffset + e0]];
-			worldVertsB1[pairIndex * capacityWorldVerts + numWorldVertsB1++] = b3TransformPoint(b, posB, ornB);
-		}
-	}
-
-	int closestFaceA = -1;
-	{
-		float dmin = FLT_MAX;
-		for (int face = 0; face < hullA->m_numFaces; face++)
-		{
-			const b3Float4 Normal = b3MakeFloat4(
-				facesA[hullA->m_faceOffset + face].m_plane.x,
-				facesA[hullA->m_faceOffset + face].m_plane.y,
-				facesA[hullA->m_faceOffset + face].m_plane.z,
-				0.f);
-			const b3Float4 faceANormalWS = b3QuatRotate(ornA, Normal);
-
-			float d = b3Dot(faceANormalWS, separatingNormal);
-			if (d < dmin)
-			{
-				dmin = d;
-				closestFaceA = face;
-				worldNormalsA1[pairIndex] = faceANormalWS;
-			}
-		}
-	}
-
-	int numVerticesA = facesA[hullA->m_faceOffset + closestFaceA].m_numIndices;
-	for (int e0 = 0; e0 < numVerticesA; e0++)
-	{
-		const b3Float4 a = verticesA[hullA->m_vertexOffset + indicesA[facesA[hullA->m_faceOffset + closestFaceA].m_indexOffset + e0]];
-		worldVertsA1[pairIndex * capacityWorldVerts + e0] = b3TransformPoint(a, posA, ornA);
-	}
-
-	clippingFaces[pairIndex].x = closestFaceA;
-	clippingFaces[pairIndex].y = closestFaceB;
-	clippingFaces[pairIndex].z = numVerticesA;
-	clippingFaces[pairIndex].w = numWorldVertsB1;
-
-	return numContactsOut;
-}
-
-__kernel void b3FindConcaveSeparatingAxisKernel(__global b3Int4* concavePairs,
-												__global const b3RigidBodyData* rigidBodies,
-												__global const b3Collidable* collidables,
-												__global const b3ConvexPolyhedronData* convexShapes,
-												__global const b3Float4* vertices,
-												__global const b3Float4* uniqueEdges,
-												__global const b3GpuFace* faces,
-												__global const int* indices,
-												__global const b3GpuChildShape* gpuChildShapes,
-												__global b3Aabb* aabbs,
-												__global b3Float4* concaveSeparatingNormalsOut,
-												__global b3Int4* clippingFacesOut,
-												__global b3Vector3* worldVertsA1Out,
-												__global b3Vector3* worldNormalsA1Out,
-												__global b3Vector3* worldVertsB1Out,
-												__global int* hasSeparatingNormals,
-												int vertexFaceCapacity,
-												int numConcavePairs,
-												int pairIdx)
-{
-	int i = pairIdx;
-	/*	int i = get_global_id(0);
-	if (i>=numConcavePairs)
-		return;
-	int pairIdx = i;
-	*/
-
-	int bodyIndexA = concavePairs[i].x;
-	int bodyIndexB = concavePairs[i].y;
-
-	int collidableIndexA = rigidBodies[bodyIndexA].m_collidableIdx;
-	int collidableIndexB = rigidBodies[bodyIndexB].m_collidableIdx;
-
-	int shapeIndexA = collidables[collidableIndexA].m_shapeIndex;
-	int shapeIndexB = collidables[collidableIndexB].m_shapeIndex;
-
-	if (collidables[collidableIndexB].m_shapeType != SHAPE_CONVEX_HULL &&
-		collidables[collidableIndexB].m_shapeType != SHAPE_COMPOUND_OF_CONVEX_HULLS)
-	{
-		concavePairs[pairIdx].w = -1;
-		return;
-	}
-
-	hasSeparatingNormals[i] = 0;
-
-	//	int numFacesA = convexShapes[shapeIndexA].m_numFaces;
-	int numActualConcaveConvexTests = 0;
-
-	int f = concavePairs[i].z;
-
-	bool overlap = false;
-
-	b3ConvexPolyhedronData convexPolyhedronA;
-
-	//add 3 vertices of the triangle
-	convexPolyhedronA.m_numVertices = 3;
-	convexPolyhedronA.m_vertexOffset = 0;
-	b3Float4 localCenter = b3MakeFloat4(0.f, 0.f, 0.f, 0.f);
-
-	b3GpuFace face = faces[convexShapes[shapeIndexA].m_faceOffset + f];
-	b3Aabb triAabb;
-	triAabb.m_minVec = b3MakeFloat4(1e30f, 1e30f, 1e30f, 0.f);
-	triAabb.m_maxVec = b3MakeFloat4(-1e30f, -1e30f, -1e30f, 0.f);
-
-	b3Float4 verticesA[3];
-	for (int i = 0; i < 3; i++)
-	{
-		int index = indices[face.m_indexOffset + i];
-		b3Float4 vert = vertices[convexShapes[shapeIndexA].m_vertexOffset + index];
-		verticesA[i] = vert;
-		localCenter += vert;
-
-		triAabb.m_minVec = b3MinFloat4(triAabb.m_minVec, vert);
-		triAabb.m_maxVec = b3MaxFloat4(triAabb.m_maxVec, vert);
-	}
-
-	overlap = true;
-	overlap = (triAabb.m_minVec.x > aabbs[bodyIndexB].m_maxVec.x || triAabb.m_maxVec.x < aabbs[bodyIndexB].m_minVec.x) ? false : overlap;
-	overlap = (triAabb.m_minVec.z > aabbs[bodyIndexB].m_maxVec.z || triAabb.m_maxVec.z < aabbs[bodyIndexB].m_minVec.z) ? false : overlap;
-	overlap = (triAabb.m_minVec.y > aabbs[bodyIndexB].m_maxVec.y || triAabb.m_maxVec.y < aabbs[bodyIndexB].m_minVec.y) ? false : overlap;
-
-	if (overlap)
-	{
-		float dmin = FLT_MAX;
-		int hasSeparatingAxis = 5;
-		b3Float4 sepAxis = b3MakeFloat4(1, 2, 3, 4);
-
-		//	int localCC=0;
-		numActualConcaveConvexTests++;
-
-		//a triangle has 3 unique edges
-		convexPolyhedronA.m_numUniqueEdges = 3;
-		convexPolyhedronA.m_uniqueEdgesOffset = 0;
-		b3Float4 uniqueEdgesA[3];
-
-		uniqueEdgesA[0] = (verticesA[1] - verticesA[0]);
-		uniqueEdgesA[1] = (verticesA[2] - verticesA[1]);
-		uniqueEdgesA[2] = (verticesA[0] - verticesA[2]);
-
-		convexPolyhedronA.m_faceOffset = 0;
-
-		b3Float4 normal = b3MakeFloat4(face.m_plane.x, face.m_plane.y, face.m_plane.z, 0.f);
-
-		b3GpuFace facesA[B3_TRIANGLE_NUM_CONVEX_FACES];
-		int indicesA[3 + 3 + 2 + 2 + 2];
-		int curUsedIndices = 0;
-		int fidx = 0;
-
-		//front size of triangle
-		{
-			facesA[fidx].m_indexOffset = curUsedIndices;
-			indicesA[0] = 0;
-			indicesA[1] = 1;
-			indicesA[2] = 2;
-			curUsedIndices += 3;
-			float c = face.m_plane.w;
-			facesA[fidx].m_plane.x = normal.x;
-			facesA[fidx].m_plane.y = normal.y;
-			facesA[fidx].m_plane.z = normal.z;
-			facesA[fidx].m_plane.w = c;
-			facesA[fidx].m_numIndices = 3;
-		}
-		fidx++;
-		//back size of triangle
-		{
-			facesA[fidx].m_indexOffset = curUsedIndices;
-			indicesA[3] = 2;
-			indicesA[4] = 1;
-			indicesA[5] = 0;
-			curUsedIndices += 3;
-			float c = b3Dot(normal, verticesA[0]);
-			//	float c1 = -face.m_plane.w;
-			facesA[fidx].m_plane.x = -normal.x;
-			facesA[fidx].m_plane.y = -normal.y;
-			facesA[fidx].m_plane.z = -normal.z;
-			facesA[fidx].m_plane.w = c;
-			facesA[fidx].m_numIndices = 3;
-		}
-		fidx++;
-
-		bool addEdgePlanes = true;
-		if (addEdgePlanes)
-		{
-			int numVertices = 3;
-			int prevVertex = numVertices - 1;
-			for (int i = 0; i < numVertices; i++)
-			{
-				b3Float4 v0 = verticesA[i];
-				b3Float4 v1 = verticesA[prevVertex];
-
-				b3Float4 edgeNormal = b3Normalized(b3Cross(normal, v1 - v0));
-				float c = -b3Dot(edgeNormal, v0);
-
-				facesA[fidx].m_numIndices = 2;
-				facesA[fidx].m_indexOffset = curUsedIndices;
-				indicesA[curUsedIndices++] = i;
-				indicesA[curUsedIndices++] = prevVertex;
-
-				facesA[fidx].m_plane.x = edgeNormal.x;
-				facesA[fidx].m_plane.y = edgeNormal.y;
-				facesA[fidx].m_plane.z = edgeNormal.z;
-				facesA[fidx].m_plane.w = c;
-				fidx++;
-				prevVertex = i;
-			}
-		}
-		convexPolyhedronA.m_numFaces = B3_TRIANGLE_NUM_CONVEX_FACES;
-		convexPolyhedronA.m_localCenter = localCenter * (1.f / 3.f);
-
-		b3Float4 posA = rigidBodies[bodyIndexA].m_pos;
-		posA.w = 0.f;
-		b3Float4 posB = rigidBodies[bodyIndexB].m_pos;
-		posB.w = 0.f;
-
-		b3Quaternion ornA = rigidBodies[bodyIndexA].m_quat;
-		b3Quaternion ornB = rigidBodies[bodyIndexB].m_quat;
-
-		///////////////////
-		///compound shape support
-
-		if (collidables[collidableIndexB].m_shapeType == SHAPE_COMPOUND_OF_CONVEX_HULLS)
-		{
-			int compoundChild = concavePairs[pairIdx].w;
-			int childShapeIndexB = compoundChild;  //collidables[collidableIndexB].m_shapeIndex+compoundChild;
-			int childColIndexB = gpuChildShapes[childShapeIndexB].m_shapeIndex;
-			b3Float4 childPosB = gpuChildShapes[childShapeIndexB].m_childPosition;
-			b3Quaternion childOrnB = gpuChildShapes[childShapeIndexB].m_childOrientation;
-			b3Float4 newPosB = b3TransformPoint(childPosB, posB, ornB);
-			b3Quaternion newOrnB = b3QuatMul(ornB, childOrnB);
-			posB = newPosB;
-			ornB = newOrnB;
-			shapeIndexB = collidables[childColIndexB].m_shapeIndex;
-		}
-		//////////////////
-
-		b3Float4 c0local = convexPolyhedronA.m_localCenter;
-		b3Float4 c0 = b3TransformPoint(c0local, posA, ornA);
-		b3Float4 c1local = convexShapes[shapeIndexB].m_localCenter;
-		b3Float4 c1 = b3TransformPoint(c1local, posB, ornB);
-		const b3Float4 DeltaC2 = c0 - c1;
-
-		bool sepA = b3FindSeparatingAxis(&convexPolyhedronA, &convexShapes[shapeIndexB],
-										 posA, ornA,
-										 posB, ornB,
-										 DeltaC2,
-										 verticesA, uniqueEdgesA, facesA, indicesA,
-										 vertices, uniqueEdges, faces, indices,
-										 &sepAxis, &dmin);
-		hasSeparatingAxis = 4;
-		if (!sepA)
-		{
-			hasSeparatingAxis = 0;
-		}
-		else
-		{
-			bool sepB = b3FindSeparatingAxis(&convexShapes[shapeIndexB], &convexPolyhedronA,
-											 posB, ornB,
-											 posA, ornA,
-											 DeltaC2,
-											 vertices, uniqueEdges, faces, indices,
-											 verticesA, uniqueEdgesA, facesA, indicesA,
-											 &sepAxis, &dmin);
-
-			if (!sepB)
-			{
-				hasSeparatingAxis = 0;
-			}
-			else
-			{
-				bool sepEE = b3FindSeparatingAxisEdgeEdge(&convexPolyhedronA, &convexShapes[shapeIndexB],
-														  posA, ornA,
-														  posB, ornB,
-														  DeltaC2,
-														  verticesA, uniqueEdgesA, facesA, indicesA,
-														  vertices, uniqueEdges, faces, indices,
-														  &sepAxis, &dmin, true);
-
-				if (!sepEE)
-				{
-					hasSeparatingAxis = 0;
-				}
-				else
-				{
-					hasSeparatingAxis = 1;
-				}
-			}
-		}
-
-		if (hasSeparatingAxis)
-		{
-			hasSeparatingNormals[i] = 1;
-			sepAxis.w = dmin;
-			concaveSeparatingNormalsOut[pairIdx] = sepAxis;
-
-			//now compute clipping faces A and B, and world-space clipping vertices A and B...
-
-			float minDist = -1e30f;
-			float maxDist = 0.02f;
-
-			b3FindClippingFaces(sepAxis,
-								&convexPolyhedronA,
-								&convexShapes[shapeIndexB],
-								posA, ornA,
-								posB, ornB,
-								worldVertsA1Out,
-								worldNormalsA1Out,
-								worldVertsB1Out,
-								vertexFaceCapacity,
-								minDist, maxDist,
-								verticesA,
-								facesA,
-								indicesA,
-
-								vertices,
-								faces,
-								indices,
-								clippingFacesOut, pairIdx);
-		}
-		else
-		{
-			//mark this pair as in-active
-			concavePairs[pairIdx].w = -1;
-		}
-	}
-	else
-	{
-		//mark this pair as in-active
-		concavePairs[pairIdx].w = -1;
-	}
-}
-
-#endif  //B3_FIND_CONCAVE_SEPARATING_AXIS_H

thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3FindSeparatingAxis.h

@@ -1,197 +0,0 @@
-#ifndef B3_FIND_SEPARATING_AXIS_H
-#define B3_FIND_SEPARATING_AXIS_H
-
-inline void b3ProjectAxis(const b3ConvexPolyhedronData& hull, const b3Float4& pos, const b3Quaternion& orn, const b3Float4& dir, const b3AlignedObjectArray<b3Vector3>& vertices, b3Scalar& min, b3Scalar& max)
-{
-	min = FLT_MAX;
-	max = -FLT_MAX;
-	int numVerts = hull.m_numVertices;
-
-	const b3Float4 localDir = b3QuatRotate(orn.inverse(), dir);
-
-	b3Scalar offset = b3Dot3F4(pos, dir);
-
-	for (int i = 0; i < numVerts; i++)
-	{
-		//b3Vector3 pt = trans * vertices[m_vertexOffset+i];
-		//b3Scalar dp = pt.dot(dir);
-		//b3Vector3 vertex = vertices[hull.m_vertexOffset+i];
-		b3Scalar dp = b3Dot3F4((b3Float4&)vertices[hull.m_vertexOffset + i], localDir);
-		//b3Assert(dp==dpL);
-		if (dp < min) min = dp;
-		if (dp > max) max = dp;
-	}
-	if (min > max)
-	{
-		b3Scalar tmp = min;
-		min = max;
-		max = tmp;
-	}
-	min += offset;
-	max += offset;
-}
-
-inline bool b3TestSepAxis(const b3ConvexPolyhedronData& hullA, const b3ConvexPolyhedronData& hullB,
-						  const b3Float4& posA, const b3Quaternion& ornA,
-						  const b3Float4& posB, const b3Quaternion& ornB,
-						  const b3Float4& sep_axis, const b3AlignedObjectArray<b3Vector3>& verticesA, const b3AlignedObjectArray<b3Vector3>& verticesB, b3Scalar& depth)
-{
-	b3Scalar Min0, Max0;
-	b3Scalar Min1, Max1;
-	b3ProjectAxis(hullA, posA, ornA, sep_axis, verticesA, Min0, Max0);
-	b3ProjectAxis(hullB, posB, ornB, sep_axis, verticesB, Min1, Max1);
-
-	if (Max0 < Min1 || Max1 < Min0)
-		return false;
-
-	b3Scalar d0 = Max0 - Min1;
-	b3Assert(d0 >= 0.0f);
-	b3Scalar d1 = Max1 - Min0;
-	b3Assert(d1 >= 0.0f);
-	depth = d0 < d1 ? d0 : d1;
-	return true;
-}
-
-inline bool b3FindSeparatingAxis(const b3ConvexPolyhedronData& hullA, const b3ConvexPolyhedronData& hullB,
-								 const b3Float4& posA1,
-								 const b3Quaternion& ornA,
-								 const b3Float4& posB1,
-								 const b3Quaternion& ornB,
-								 const b3AlignedObjectArray<b3Vector3>& verticesA,
-								 const b3AlignedObjectArray<b3Vector3>& uniqueEdgesA,
-								 const b3AlignedObjectArray<b3GpuFace>& facesA,
-								 const b3AlignedObjectArray<int>& indicesA,
-								 const b3AlignedObjectArray<b3Vector3>& verticesB,
-								 const b3AlignedObjectArray<b3Vector3>& uniqueEdgesB,
-								 const b3AlignedObjectArray<b3GpuFace>& facesB,
-								 const b3AlignedObjectArray<int>& indicesB,
-
-								 b3Vector3& sep)
-{
-	B3_PROFILE("findSeparatingAxis");
-
-	b3Float4 posA = posA1;
-	posA.w = 0.f;
-	b3Float4 posB = posB1;
-	posB.w = 0.f;
-	//#ifdef TEST_INTERNAL_OBJECTS
-	b3Float4 c0local = (b3Float4&)hullA.m_localCenter;
-
-	b3Float4 c0 = b3TransformPoint(c0local, posA, ornA);
-	b3Float4 c1local = (b3Float4&)hullB.m_localCenter;
-	b3Float4 c1 = b3TransformPoint(c1local, posB, ornB);
-	const b3Float4 deltaC2 = c0 - c1;
-	//#endif
-
-	b3Scalar dmin = FLT_MAX;
-	int curPlaneTests = 0;
-
-	int numFacesA = hullA.m_numFaces;
-	// Test normals from hullA
-	for (int i = 0; i < numFacesA; i++)
-	{
-		const b3Float4& normal = (b3Float4&)facesA[hullA.m_faceOffset + i].m_plane;
-		b3Float4 faceANormalWS = b3QuatRotate(ornA, normal);
-
-		if (b3Dot3F4(deltaC2, faceANormalWS) < 0)
-			faceANormalWS *= -1.f;
-
-		curPlaneTests++;
-#ifdef TEST_INTERNAL_OBJECTS
-		gExpectedNbTests++;
-		if (gUseInternalObject && !TestInternalObjects(transA, transB, DeltaC2, faceANormalWS, hullA, hullB, dmin))
-			continue;
-		gActualNbTests++;
-#endif
-
-		b3Scalar d;
-		if (!b3TestSepAxis(hullA, hullB, posA, ornA, posB, ornB, faceANormalWS, verticesA, verticesB, d))
-			return false;
-
-		if (d < dmin)
-		{
-			dmin = d;
-			sep = (b3Vector3&)faceANormalWS;
-		}
-	}
-
-	int numFacesB = hullB.m_numFaces;
-	// Test normals from hullB
-	for (int i = 0; i < numFacesB; i++)
-	{
-		b3Float4 normal = (b3Float4&)facesB[hullB.m_faceOffset + i].m_plane;
-		b3Float4 WorldNormal = b3QuatRotate(ornB, normal);
-
-		if (b3Dot3F4(deltaC2, WorldNormal) < 0)
-		{
-			WorldNormal *= -1.f;
-		}
-		curPlaneTests++;
-#ifdef TEST_INTERNAL_OBJECTS
-		gExpectedNbTests++;
-		if (gUseInternalObject && !TestInternalObjects(transA, transB, DeltaC2, WorldNormal, hullA, hullB, dmin))
-			continue;
-		gActualNbTests++;
-#endif
-
-		b3Scalar d;
-		if (!b3TestSepAxis(hullA, hullB, posA, ornA, posB, ornB, WorldNormal, verticesA, verticesB, d))
-			return false;
-
-		if (d < dmin)
-		{
-			dmin = d;
-			sep = (b3Vector3&)WorldNormal;
-		}
-	}
-
-	//	b3Vector3 edgeAstart,edgeAend,edgeBstart,edgeBend;
-
-	int curEdgeEdge = 0;
-	// Test edges
-	for (int e0 = 0; e0 < hullA.m_numUniqueEdges; e0++)
-	{
-		const b3Float4& edge0 = (b3Float4&)uniqueEdgesA[hullA.m_uniqueEdgesOffset + e0];
-		b3Float4 edge0World = b3QuatRotate(ornA, (b3Float4&)edge0);
-
-		for (int e1 = 0; e1 < hullB.m_numUniqueEdges; e1++)
-		{
-			const b3Vector3 edge1 = uniqueEdgesB[hullB.m_uniqueEdgesOffset + e1];
-			b3Float4 edge1World = b3QuatRotate(ornB, (b3Float4&)edge1);
-
-			b3Float4 crossje = b3Cross3(edge0World, edge1World);
-
-			curEdgeEdge++;
-			if (!b3IsAlmostZero((b3Vector3&)crossje))
-			{
-				crossje = b3FastNormalized3(crossje);
-				if (b3Dot3F4(deltaC2, crossje) < 0)
-					crossje *= -1.f;
-
-#ifdef TEST_INTERNAL_OBJECTS
-				gExpectedNbTests++;
-				if (gUseInternalObject && !TestInternalObjects(transA, transB, DeltaC2, Cross, hullA, hullB, dmin))
-					continue;
-				gActualNbTests++;
-#endif
-
-				b3Scalar dist;
-				if (!b3TestSepAxis(hullA, hullB, posA, ornA, posB, ornB, crossje, verticesA, verticesB, dist))
-					return false;
-
-				if (dist < dmin)
-				{
-					dmin = dist;
-					sep = (b3Vector3&)crossje;
-				}
-			}
-		}
-	}
-
-	if ((b3Dot3F4(-deltaC2, (b3Float4&)sep)) > 0.0f)
-		sep = -sep;
-
-	return true;
-}
-
-#endif  //B3_FIND_SEPARATING_AXIS_H

thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3MprPenetration.h

@@ -1,888 +0,0 @@
-
-/***
- * ---------------------------------
- * Copyright (c)2012 Daniel Fiser <[email protected]>
- *
- *  This file was ported from mpr.c file, part of libccd.
- *  The Minkoski Portal Refinement implementation was ported 
- *  to OpenCL by Erwin Coumans for the Bullet 3 Physics library.
- *  at http://github.com/erwincoumans/bullet3
- *
- *  Distributed under the OSI-approved BSD License (the "License");
- *  see <http://www.opensource.org/licenses/bsd-license.php>.
- *  This software is distributed WITHOUT ANY WARRANTY; without even the
- *  implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
- *  See the License for more information.
- */
-
-#ifndef B3_MPR_PENETRATION_H
-#define B3_MPR_PENETRATION_H
-
-#include "Bullet3Common/shared/b3PlatformDefinitions.h"
-#include "Bullet3Common/shared/b3Float4.h"
-#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h"
-#include "Bullet3Collision/NarrowPhaseCollision/shared/b3ConvexPolyhedronData.h"
-#include "Bullet3Collision/NarrowPhaseCollision/shared/b3Collidable.h"
-
-#ifdef __cplusplus
-#define B3_MPR_SQRT sqrtf
-#else
-#define B3_MPR_SQRT sqrt
-#endif
-#define B3_MPR_FMIN(x, y) ((x) < (y) ? (x) : (y))
-#define B3_MPR_FABS fabs
-
-#define B3_MPR_TOLERANCE 1E-6f
-#define B3_MPR_MAX_ITERATIONS 1000
-
-struct _b3MprSupport_t
-{
-	b3Float4 v;   //!< Support point in minkowski sum
-	b3Float4 v1;  //!< Support point in obj1
-	b3Float4 v2;  //!< Support point in obj2
-};
-typedef struct _b3MprSupport_t b3MprSupport_t;
-
-struct _b3MprSimplex_t
-{
-	b3MprSupport_t ps[4];
-	int last;  //!< index of last added point
-};
-typedef struct _b3MprSimplex_t b3MprSimplex_t;
-
-inline b3MprSupport_t *b3MprSimplexPointW(b3MprSimplex_t *s, int idx)
-{
-	return &s->ps[idx];
-}
-
-inline void b3MprSimplexSetSize(b3MprSimplex_t *s, int size)
-{
-	s->last = size - 1;
-}
-
-inline int b3MprSimplexSize(const b3MprSimplex_t *s)
-{
-	return s->last + 1;
-}
-
-inline const b3MprSupport_t *b3MprSimplexPoint(const b3MprSimplex_t *s, int idx)
-{
-	// here is no check on boundaries
-	return &s->ps[idx];
-}
-
-inline void b3MprSupportCopy(b3MprSupport_t *d, const b3MprSupport_t *s)
-{
-	*d = *s;
-}
-
-inline void b3MprSimplexSet(b3MprSimplex_t *s, size_t pos, const b3MprSupport_t *a)
-{
-	b3MprSupportCopy(s->ps + pos, a);
-}
-
-inline void b3MprSimplexSwap(b3MprSimplex_t *s, size_t pos1, size_t pos2)
-{
-	b3MprSupport_t supp;
-
-	b3MprSupportCopy(&supp, &s->ps[pos1]);
-	b3MprSupportCopy(&s->ps[pos1], &s->ps[pos2]);
-	b3MprSupportCopy(&s->ps[pos2], &supp);
-}
-
-inline int b3MprIsZero(float val)
-{
-	return B3_MPR_FABS(val) < FLT_EPSILON;
-}
-
-inline int b3MprEq(float _a, float _b)
-{
-	float ab;
-	float a, b;
-
-	ab = B3_MPR_FABS(_a - _b);
-	if (B3_MPR_FABS(ab) < FLT_EPSILON)
-		return 1;
-
-	a = B3_MPR_FABS(_a);
-	b = B3_MPR_FABS(_b);
-	if (b > a)
-	{
-		return ab < FLT_EPSILON * b;
-	}
-	else
-	{
-		return ab < FLT_EPSILON * a;
-	}
-}
-
-inline int b3MprVec3Eq(const b3Float4 *a, const b3Float4 *b)
-{
-	return b3MprEq((*a).x, (*b).x) && b3MprEq((*a).y, (*b).y) && b3MprEq((*a).z, (*b).z);
-}
-
-inline b3Float4 b3LocalGetSupportVertex(b3Float4ConstArg supportVec, __global const b3ConvexPolyhedronData_t *hull, b3ConstArray(b3Float4) verticesA)
-{
-	b3Float4 supVec = b3MakeFloat4(0, 0, 0, 0);
-	float maxDot = -B3_LARGE_FLOAT;
-
-	if (0 < hull->m_numVertices)
-	{
-		const b3Float4 scaled = supportVec;
-		int index = b3MaxDot(scaled, &verticesA[hull->m_vertexOffset], hull->m_numVertices, &maxDot);
-		return verticesA[hull->m_vertexOffset + index];
-	}
-
-	return supVec;
-}
-
-B3_STATIC void b3MprConvexSupport(int pairIndex, int bodyIndex, b3ConstArray(b3RigidBodyData_t) cpuBodyBuf,
-								  b3ConstArray(b3ConvexPolyhedronData_t) cpuConvexData,
-								  b3ConstArray(b3Collidable_t) cpuCollidables,
-								  b3ConstArray(b3Float4) cpuVertices,
-								  __global b3Float4 *sepAxis,
-								  const b3Float4 *_dir, b3Float4 *outp, int logme)
-{
-	//dir is in worldspace, move to local space
-
-	b3Float4 pos = cpuBodyBuf[bodyIndex].m_pos;
-	b3Quat orn = cpuBodyBuf[bodyIndex].m_quat;
-
-	b3Float4 dir = b3MakeFloat4((*_dir).x, (*_dir).y, (*_dir).z, 0.f);
-
-	const b3Float4 localDir = b3QuatRotate(b3QuatInverse(orn), dir);
-
-	//find local support vertex
-	int colIndex = cpuBodyBuf[bodyIndex].m_collidableIdx;
-
-	b3Assert(cpuCollidables[colIndex].m_shapeType == SHAPE_CONVEX_HULL);
-	__global const b3ConvexPolyhedronData_t *hull = &cpuConvexData[cpuCollidables[colIndex].m_shapeIndex];
-
-	b3Float4 pInA;
-	if (logme)
-	{
-		//	b3Float4 supVec = b3MakeFloat4(0,0,0,0);
-		float maxDot = -B3_LARGE_FLOAT;
-
-		if (0 < hull->m_numVertices)
-		{
-			const b3Float4 scaled = localDir;
-			int index = b3MaxDot(scaled, &cpuVertices[hull->m_vertexOffset], hull->m_numVertices, &maxDot);
-			pInA = cpuVertices[hull->m_vertexOffset + index];
-		}
-	}
-	else
-	{
-		pInA = b3LocalGetSupportVertex(localDir, hull, cpuVertices);
-	}
-
-	//move vertex to world space
-	*outp = b3TransformPoint(pInA, pos, orn);
-}
-
-inline void b3MprSupport(int pairIndex, int bodyIndexA, int bodyIndexB, b3ConstArray(b3RigidBodyData_t) cpuBodyBuf,
-						 b3ConstArray(b3ConvexPolyhedronData_t) cpuConvexData,
-						 b3ConstArray(b3Collidable_t) cpuCollidables,
-						 b3ConstArray(b3Float4) cpuVertices,
-						 __global b3Float4 *sepAxis,
-						 const b3Float4 *_dir, b3MprSupport_t *supp)
-{
-	b3Float4 dir;
-	dir = *_dir;
-	b3MprConvexSupport(pairIndex, bodyIndexA, cpuBodyBuf, cpuConvexData, cpuCollidables, cpuVertices, sepAxis, &dir, &supp->v1, 0);
-	dir = *_dir * -1.f;
-	b3MprConvexSupport(pairIndex, bodyIndexB, cpuBodyBuf, cpuConvexData, cpuCollidables, cpuVertices, sepAxis, &dir, &supp->v2, 0);
-	supp->v = supp->v1 - supp->v2;
-}
-
-inline void b3FindOrigin(int bodyIndexA, int bodyIndexB, b3ConstArray(b3RigidBodyData_t) cpuBodyBuf, b3MprSupport_t *center)
-{
-	center->v1 = cpuBodyBuf[bodyIndexA].m_pos;
-	center->v2 = cpuBodyBuf[bodyIndexB].m_pos;
-	center->v = center->v1 - center->v2;
-}
-
-inline void b3MprVec3Set(b3Float4 *v, float x, float y, float z)
-{
-	(*v).x = x;
-	(*v).y = y;
-	(*v).z = z;
-	(*v).w = 0.f;
-}
-
-inline void b3MprVec3Add(b3Float4 *v, const b3Float4 *w)
-{
-	(*v).x += (*w).x;
-	(*v).y += (*w).y;
-	(*v).z += (*w).z;
-}
-
-inline void b3MprVec3Copy(b3Float4 *v, const b3Float4 *w)
-{
-	*v = *w;
-}
-
-inline void b3MprVec3Scale(b3Float4 *d, float k)
-{
-	*d *= k;
-}
-
-inline float b3MprVec3Dot(const b3Float4 *a, const b3Float4 *b)
-{
-	float dot;
-
-	dot = b3Dot3F4(*a, *b);
-	return dot;
-}
-
-inline float b3MprVec3Len2(const b3Float4 *v)
-{
-	return b3MprVec3Dot(v, v);
-}
-
-inline void b3MprVec3Normalize(b3Float4 *d)
-{
-	float k = 1.f / B3_MPR_SQRT(b3MprVec3Len2(d));
-	b3MprVec3Scale(d, k);
-}
-
-inline void b3MprVec3Cross(b3Float4 *d, const b3Float4 *a, const b3Float4 *b)
-{
-	*d = b3Cross3(*a, *b);
-}
-
-inline void b3MprVec3Sub2(b3Float4 *d, const b3Float4 *v, const b3Float4 *w)
-{
-	*d = *v - *w;
-}
-
-inline void b3PortalDir(const b3MprSimplex_t *portal, b3Float4 *dir)
-{
-	b3Float4 v2v1, v3v1;
-
-	b3MprVec3Sub2(&v2v1, &b3MprSimplexPoint(portal, 2)->v,
-				  &b3MprSimplexPoint(portal, 1)->v);
-	b3MprVec3Sub2(&v3v1, &b3MprSimplexPoint(portal, 3)->v,
-				  &b3MprSimplexPoint(portal, 1)->v);
-	b3MprVec3Cross(dir, &v2v1, &v3v1);
-	b3MprVec3Normalize(dir);
-}
-
-inline int portalEncapsulesOrigin(const b3MprSimplex_t *portal,
-								  const b3Float4 *dir)
-{
-	float dot;
-	dot = b3MprVec3Dot(dir, &b3MprSimplexPoint(portal, 1)->v);
-	return b3MprIsZero(dot) || dot > 0.f;
-}
-
-inline int portalReachTolerance(const b3MprSimplex_t *portal,
-								const b3MprSupport_t *v4,
-								const b3Float4 *dir)
-{
-	float dv1, dv2, dv3, dv4;
-	float dot1, dot2, dot3;
-
-	// find the smallest dot product of dir and {v1-v4, v2-v4, v3-v4}
-
-	dv1 = b3MprVec3Dot(&b3MprSimplexPoint(portal, 1)->v, dir);
-	dv2 = b3MprVec3Dot(&b3MprSimplexPoint(portal, 2)->v, dir);
-	dv3 = b3MprVec3Dot(&b3MprSimplexPoint(portal, 3)->v, dir);
-	dv4 = b3MprVec3Dot(&v4->v, dir);
-
-	dot1 = dv4 - dv1;
-	dot2 = dv4 - dv2;
-	dot3 = dv4 - dv3;
-
-	dot1 = B3_MPR_FMIN(dot1, dot2);
-	dot1 = B3_MPR_FMIN(dot1, dot3);
-
-	return b3MprEq(dot1, B3_MPR_TOLERANCE) || dot1 < B3_MPR_TOLERANCE;
-}
-
-inline int portalCanEncapsuleOrigin(const b3MprSimplex_t *portal,
-									const b3MprSupport_t *v4,
-									const b3Float4 *dir)
-{
-	float dot;
-	dot = b3MprVec3Dot(&v4->v, dir);
-	return b3MprIsZero(dot) || dot > 0.f;
-}
-
-inline void b3ExpandPortal(b3MprSimplex_t *portal,
-						   const b3MprSupport_t *v4)
-{
-	float dot;
-	b3Float4 v4v0;
-
-	b3MprVec3Cross(&v4v0, &v4->v, &b3MprSimplexPoint(portal, 0)->v);
-	dot = b3MprVec3Dot(&b3MprSimplexPoint(portal, 1)->v, &v4v0);
-	if (dot > 0.f)
-	{
-		dot = b3MprVec3Dot(&b3MprSimplexPoint(portal, 2)->v, &v4v0);
-		if (dot > 0.f)
-		{
-			b3MprSimplexSet(portal, 1, v4);
-		}
-		else
-		{
-			b3MprSimplexSet(portal, 3, v4);
-		}
-	}
-	else
-	{
-		dot = b3MprVec3Dot(&b3MprSimplexPoint(portal, 3)->v, &v4v0);
-		if (dot > 0.f)
-		{
-			b3MprSimplexSet(portal, 2, v4);
-		}
-		else
-		{
-			b3MprSimplexSet(portal, 1, v4);
-		}
-	}
-}
-
-B3_STATIC int b3DiscoverPortal(int pairIndex, int bodyIndexA, int bodyIndexB, b3ConstArray(b3RigidBodyData_t) cpuBodyBuf,
-							   b3ConstArray(b3ConvexPolyhedronData_t) cpuConvexData,
-							   b3ConstArray(b3Collidable_t) cpuCollidables,
-							   b3ConstArray(b3Float4) cpuVertices,
-							   __global b3Float4 *sepAxis,
-							   __global int *hasSepAxis,
-							   b3MprSimplex_t *portal)
-{
-	b3Float4 dir, va, vb;
-	float dot;
-	int cont;
-
-	// vertex 0 is center of portal
-	b3FindOrigin(bodyIndexA, bodyIndexB, cpuBodyBuf, b3MprSimplexPointW(portal, 0));
-	// vertex 0 is center of portal
-	b3MprSimplexSetSize(portal, 1);
-
-	b3Float4 zero = b3MakeFloat4(0, 0, 0, 0);
-	b3Float4 *b3mpr_vec3_origin = &zero;
-
-	if (b3MprVec3Eq(&b3MprSimplexPoint(portal, 0)->v, b3mpr_vec3_origin))
-	{
-		// Portal's center lies on origin (0,0,0) => we know that objects
-		// intersect but we would need to know penetration info.
-		// So move center little bit...
-		b3MprVec3Set(&va, FLT_EPSILON * 10.f, 0.f, 0.f);
-		b3MprVec3Add(&b3MprSimplexPointW(portal, 0)->v, &va);
-	}
-
-	// vertex 1 = support in direction of origin
-	b3MprVec3Copy(&dir, &b3MprSimplexPoint(portal, 0)->v);
-	b3MprVec3Scale(&dir, -1.f);
-	b3MprVec3Normalize(&dir);
-
-	b3MprSupport(pairIndex, bodyIndexA, bodyIndexB, cpuBodyBuf, cpuConvexData, cpuCollidables, cpuVertices, sepAxis, &dir, b3MprSimplexPointW(portal, 1));
-
-	b3MprSimplexSetSize(portal, 2);
-
-	// test if origin isn't outside of v1
-	dot = b3MprVec3Dot(&b3MprSimplexPoint(portal, 1)->v, &dir);
-
-	if (b3MprIsZero(dot) || dot < 0.f)
-		return -1;
-
-	// vertex 2
-	b3MprVec3Cross(&dir, &b3MprSimplexPoint(portal, 0)->v,
-				   &b3MprSimplexPoint(portal, 1)->v);
-	if (b3MprIsZero(b3MprVec3Len2(&dir)))
-	{
-		if (b3MprVec3Eq(&b3MprSimplexPoint(portal, 1)->v, b3mpr_vec3_origin))
-		{
-			// origin lies on v1
-			return 1;
-		}
-		else
-		{
-			// origin lies on v0-v1 segment
-			return 2;
-		}
-	}
-
-	b3MprVec3Normalize(&dir);
-	b3MprSupport(pairIndex, bodyIndexA, bodyIndexB, cpuBodyBuf, cpuConvexData, cpuCollidables, cpuVertices, sepAxis, &dir, b3MprSimplexPointW(portal, 2));
-
-	dot = b3MprVec3Dot(&b3MprSimplexPoint(portal, 2)->v, &dir);
-	if (b3MprIsZero(dot) || dot < 0.f)
-		return -1;
-
-	b3MprSimplexSetSize(portal, 3);
-
-	// vertex 3 direction
-	b3MprVec3Sub2(&va, &b3MprSimplexPoint(portal, 1)->v,
-				  &b3MprSimplexPoint(portal, 0)->v);
-	b3MprVec3Sub2(&vb, &b3MprSimplexPoint(portal, 2)->v,
-				  &b3MprSimplexPoint(portal, 0)->v);
-	b3MprVec3Cross(&dir, &va, &vb);
-	b3MprVec3Normalize(&dir);
-
-	// it is better to form portal faces to be oriented "outside" origin
-	dot = b3MprVec3Dot(&dir, &b3MprSimplexPoint(portal, 0)->v);
-	if (dot > 0.f)
-	{
-		b3MprSimplexSwap(portal, 1, 2);
-		b3MprVec3Scale(&dir, -1.f);
-	}
-
-	while (b3MprSimplexSize(portal) < 4)
-	{
-		b3MprSupport(pairIndex, bodyIndexA, bodyIndexB, cpuBodyBuf, cpuConvexData, cpuCollidables, cpuVertices, sepAxis, &dir, b3MprSimplexPointW(portal, 3));
-
-		dot = b3MprVec3Dot(&b3MprSimplexPoint(portal, 3)->v, &dir);
-		if (b3MprIsZero(dot) || dot < 0.f)
-			return -1;
-
-		cont = 0;
-
-		// test if origin is outside (v1, v0, v3) - set v2 as v3 and
-		// continue
-		b3MprVec3Cross(&va, &b3MprSimplexPoint(portal, 1)->v,
-					   &b3MprSimplexPoint(portal, 3)->v);
-		dot = b3MprVec3Dot(&va, &b3MprSimplexPoint(portal, 0)->v);
-		if (dot < 0.f && !b3MprIsZero(dot))
-		{
-			b3MprSimplexSet(portal, 2, b3MprSimplexPoint(portal, 3));
-			cont = 1;
-		}
-
-		if (!cont)
-		{
-			// test if origin is outside (v3, v0, v2) - set v1 as v3 and
-			// continue
-			b3MprVec3Cross(&va, &b3MprSimplexPoint(portal, 3)->v,
-						   &b3MprSimplexPoint(portal, 2)->v);
-			dot = b3MprVec3Dot(&va, &b3MprSimplexPoint(portal, 0)->v);
-			if (dot < 0.f && !b3MprIsZero(dot))
-			{
-				b3MprSimplexSet(portal, 1, b3MprSimplexPoint(portal, 3));
-				cont = 1;
-			}
-		}
-
-		if (cont)
-		{
-			b3MprVec3Sub2(&va, &b3MprSimplexPoint(portal, 1)->v,
-						  &b3MprSimplexPoint(portal, 0)->v);
-			b3MprVec3Sub2(&vb, &b3MprSimplexPoint(portal, 2)->v,
-						  &b3MprSimplexPoint(portal, 0)->v);
-			b3MprVec3Cross(&dir, &va, &vb);
-			b3MprVec3Normalize(&dir);
-		}
-		else
-		{
-			b3MprSimplexSetSize(portal, 4);
-		}
-	}
-
-	return 0;
-}
-
-B3_STATIC int b3RefinePortal(int pairIndex, int bodyIndexA, int bodyIndexB, b3ConstArray(b3RigidBodyData_t) cpuBodyBuf,
-							 b3ConstArray(b3ConvexPolyhedronData_t) cpuConvexData,
-							 b3ConstArray(b3Collidable_t) cpuCollidables,
-							 b3ConstArray(b3Float4) cpuVertices,
-							 __global b3Float4 *sepAxis,
-							 b3MprSimplex_t *portal)
-{
-	b3Float4 dir;
-	b3MprSupport_t v4;
-
-	for (int i = 0; i < B3_MPR_MAX_ITERATIONS; i++)
-	//while (1)
-	{
-		// compute direction outside the portal (from v0 throught v1,v2,v3
-		// face)
-		b3PortalDir(portal, &dir);
-
-		// test if origin is inside the portal
-		if (portalEncapsulesOrigin(portal, &dir))
-			return 0;
-
-		// get next support point
-
-		b3MprSupport(pairIndex, bodyIndexA, bodyIndexB, cpuBodyBuf, cpuConvexData, cpuCollidables, cpuVertices, sepAxis, &dir, &v4);
-
-		// test if v4 can expand portal to contain origin and if portal
-		// expanding doesn't reach given tolerance
-		if (!portalCanEncapsuleOrigin(portal, &v4, &dir) || portalReachTolerance(portal, &v4, &dir))
-		{
-			return -1;
-		}
-
-		// v1-v2-v3 triangle must be rearranged to face outside Minkowski
-		// difference (direction from v0).
-		b3ExpandPortal(portal, &v4);
-	}
-
-	return -1;
-}
-
-B3_STATIC void b3FindPos(const b3MprSimplex_t *portal, b3Float4 *pos)
-{
-	b3Float4 zero = b3MakeFloat4(0, 0, 0, 0);
-	b3Float4 *b3mpr_vec3_origin = &zero;
-
-	b3Float4 dir;
-	size_t i;
-	float b[4], sum, inv;
-	b3Float4 vec, p1, p2;
-
-	b3PortalDir(portal, &dir);
-
-	// use barycentric coordinates of tetrahedron to find origin
-	b3MprVec3Cross(&vec, &b3MprSimplexPoint(portal, 1)->v,
-				   &b3MprSimplexPoint(portal, 2)->v);
-	b[0] = b3MprVec3Dot(&vec, &b3MprSimplexPoint(portal, 3)->v);
-
-	b3MprVec3Cross(&vec, &b3MprSimplexPoint(portal, 3)->v,
-				   &b3MprSimplexPoint(portal, 2)->v);
-	b[1] = b3MprVec3Dot(&vec, &b3MprSimplexPoint(portal, 0)->v);
-
-	b3MprVec3Cross(&vec, &b3MprSimplexPoint(portal, 0)->v,
-				   &b3MprSimplexPoint(portal, 1)->v);
-	b[2] = b3MprVec3Dot(&vec, &b3MprSimplexPoint(portal, 3)->v);
-
-	b3MprVec3Cross(&vec, &b3MprSimplexPoint(portal, 2)->v,
-				   &b3MprSimplexPoint(portal, 1)->v);
-	b[3] = b3MprVec3Dot(&vec, &b3MprSimplexPoint(portal, 0)->v);
-
-	sum = b[0] + b[1] + b[2] + b[3];
-
-	if (b3MprIsZero(sum) || sum < 0.f)
-	{
-		b[0] = 0.f;
-
-		b3MprVec3Cross(&vec, &b3MprSimplexPoint(portal, 2)->v,
-					   &b3MprSimplexPoint(portal, 3)->v);
-		b[1] = b3MprVec3Dot(&vec, &dir);
-		b3MprVec3Cross(&vec, &b3MprSimplexPoint(portal, 3)->v,
-					   &b3MprSimplexPoint(portal, 1)->v);
-		b[2] = b3MprVec3Dot(&vec, &dir);
-		b3MprVec3Cross(&vec, &b3MprSimplexPoint(portal, 1)->v,
-					   &b3MprSimplexPoint(portal, 2)->v);
-		b[3] = b3MprVec3Dot(&vec, &dir);
-
-		sum = b[1] + b[2] + b[3];
-	}
-
-	inv = 1.f / sum;
-
-	b3MprVec3Copy(&p1, b3mpr_vec3_origin);
-	b3MprVec3Copy(&p2, b3mpr_vec3_origin);
-	for (i = 0; i < 4; i++)
-	{
-		b3MprVec3Copy(&vec, &b3MprSimplexPoint(portal, i)->v1);
-		b3MprVec3Scale(&vec, b[i]);
-		b3MprVec3Add(&p1, &vec);
-
-		b3MprVec3Copy(&vec, &b3MprSimplexPoint(portal, i)->v2);
-		b3MprVec3Scale(&vec, b[i]);
-		b3MprVec3Add(&p2, &vec);
-	}
-	b3MprVec3Scale(&p1, inv);
-	b3MprVec3Scale(&p2, inv);
-
-	b3MprVec3Copy(pos, &p1);
-	b3MprVec3Add(pos, &p2);
-	b3MprVec3Scale(pos, 0.5);
-}
-
-inline float b3MprVec3Dist2(const b3Float4 *a, const b3Float4 *b)
-{
-	b3Float4 ab;
-	b3MprVec3Sub2(&ab, a, b);
-	return b3MprVec3Len2(&ab);
-}
-
-inline float _b3MprVec3PointSegmentDist2(const b3Float4 *P,
-										 const b3Float4 *x0,
-										 const b3Float4 *b,
-										 b3Float4 *witness)
-{
-	// The computation comes from solving equation of segment:
-	//      S(t) = x0 + t.d
-	//          where - x0 is initial point of segment
-	//                - d is direction of segment from x0 (|d| > 0)
-	//                - t belongs to <0, 1> interval
-	//
-	// Than, distance from a segment to some point P can be expressed:
-	//      D(t) = |x0 + t.d - P|^2
-	//          which is distance from any point on segment. Minimization
-	//          of this function brings distance from P to segment.
-	// Minimization of D(t) leads to simple quadratic equation that's
-	// solving is straightforward.
-	//
-	// Bonus of this method is witness point for free.
-
-	float dist, t;
-	b3Float4 d, a;
-
-	// direction of segment
-	b3MprVec3Sub2(&d, b, x0);
-
-	// precompute vector from P to x0
-	b3MprVec3Sub2(&a, x0, P);
-
-	t = -1.f * b3MprVec3Dot(&a, &d);
-	t /= b3MprVec3Len2(&d);
-
-	if (t < 0.f || b3MprIsZero(t))
-	{
-		dist = b3MprVec3Dist2(x0, P);
-		if (witness)
-			b3MprVec3Copy(witness, x0);
-	}
-	else if (t > 1.f || b3MprEq(t, 1.f))
-	{
-		dist = b3MprVec3Dist2(b, P);
-		if (witness)
-			b3MprVec3Copy(witness, b);
-	}
-	else
-	{
-		if (witness)
-		{
-			b3MprVec3Copy(witness, &d);
-			b3MprVec3Scale(witness, t);
-			b3MprVec3Add(witness, x0);
-			dist = b3MprVec3Dist2(witness, P);
-		}
-		else
-		{
-			// recycling variables
-			b3MprVec3Scale(&d, t);
-			b3MprVec3Add(&d, &a);
-			dist = b3MprVec3Len2(&d);
-		}
-	}
-
-	return dist;
-}
-
-inline float b3MprVec3PointTriDist2(const b3Float4 *P,
-									const b3Float4 *x0, const b3Float4 *B,
-									const b3Float4 *C,
-									b3Float4 *witness)
-{
-	// Computation comes from analytic expression for triangle (x0, B, C)
-	//      T(s, t) = x0 + s.d1 + t.d2, where d1 = B - x0 and d2 = C - x0 and
-	// Then equation for distance is:
-	//      D(s, t) = | T(s, t) - P |^2
-	// This leads to minimization of quadratic function of two variables.
-	// The solution from is taken only if s is between 0 and 1, t is
-	// between 0 and 1 and t + s < 1, otherwise distance from segment is
-	// computed.
-
-	b3Float4 d1, d2, a;
-	float u, v, w, p, q, r;
-	float s, t, dist, dist2;
-	b3Float4 witness2;
-
-	b3MprVec3Sub2(&d1, B, x0);
-	b3MprVec3Sub2(&d2, C, x0);
-	b3MprVec3Sub2(&a, x0, P);
-
-	u = b3MprVec3Dot(&a, &a);
-	v = b3MprVec3Dot(&d1, &d1);
-	w = b3MprVec3Dot(&d2, &d2);
-	p = b3MprVec3Dot(&a, &d1);
-	q = b3MprVec3Dot(&a, &d2);
-	r = b3MprVec3Dot(&d1, &d2);
-
-	s = (q * r - w * p) / (w * v - r * r);
-	t = (-s * r - q) / w;
-
-	if ((b3MprIsZero(s) || s > 0.f) && (b3MprEq(s, 1.f) || s < 1.f) && (b3MprIsZero(t) || t > 0.f) && (b3MprEq(t, 1.f) || t < 1.f) && (b3MprEq(t + s, 1.f) || t + s < 1.f))
-	{
-		if (witness)
-		{
-			b3MprVec3Scale(&d1, s);
-			b3MprVec3Scale(&d2, t);
-			b3MprVec3Copy(witness, x0);
-			b3MprVec3Add(witness, &d1);
-			b3MprVec3Add(witness, &d2);
-
-			dist = b3MprVec3Dist2(witness, P);
-		}
-		else
-		{
-			dist = s * s * v;
-			dist += t * t * w;
-			dist += 2.f * s * t * r;
-			dist += 2.f * s * p;
-			dist += 2.f * t * q;
-			dist += u;
-		}
-	}
-	else
-	{
-		dist = _b3MprVec3PointSegmentDist2(P, x0, B, witness);
-
-		dist2 = _b3MprVec3PointSegmentDist2(P, x0, C, &witness2);
-		if (dist2 < dist)
-		{
-			dist = dist2;
-			if (witness)
-				b3MprVec3Copy(witness, &witness2);
-		}
-
-		dist2 = _b3MprVec3PointSegmentDist2(P, B, C, &witness2);
-		if (dist2 < dist)
-		{
-			dist = dist2;
-			if (witness)
-				b3MprVec3Copy(witness, &witness2);
-		}
-	}
-
-	return dist;
-}
-
-B3_STATIC void b3FindPenetr(int pairIndex, int bodyIndexA, int bodyIndexB, b3ConstArray(b3RigidBodyData_t) cpuBodyBuf,
-							b3ConstArray(b3ConvexPolyhedronData_t) cpuConvexData,
-							b3ConstArray(b3Collidable_t) cpuCollidables,
-							b3ConstArray(b3Float4) cpuVertices,
-							__global b3Float4 *sepAxis,
-							b3MprSimplex_t *portal,
-							float *depth, b3Float4 *pdir, b3Float4 *pos)
-{
-	b3Float4 dir;
-	b3MprSupport_t v4;
-	unsigned long iterations;
-
-	b3Float4 zero = b3MakeFloat4(0, 0, 0, 0);
-	b3Float4 *b3mpr_vec3_origin = &zero;
-
-	iterations = 1UL;
-	for (int i = 0; i < B3_MPR_MAX_ITERATIONS; i++)
-	//while (1)
-	{
-		// compute portal direction and obtain next support point
-		b3PortalDir(portal, &dir);
-
-		b3MprSupport(pairIndex, bodyIndexA, bodyIndexB, cpuBodyBuf, cpuConvexData, cpuCollidables, cpuVertices, sepAxis, &dir, &v4);
-
-		// reached tolerance -> find penetration info
-		if (portalReachTolerance(portal, &v4, &dir) || iterations == B3_MPR_MAX_ITERATIONS)
-		{
-			*depth = b3MprVec3PointTriDist2(b3mpr_vec3_origin, &b3MprSimplexPoint(portal, 1)->v, &b3MprSimplexPoint(portal, 2)->v, &b3MprSimplexPoint(portal, 3)->v, pdir);
-			*depth = B3_MPR_SQRT(*depth);
-
-			if (b3MprIsZero((*pdir).x) && b3MprIsZero((*pdir).y) && b3MprIsZero((*pdir).z))
-			{
-				*pdir = dir;
-			}
-			b3MprVec3Normalize(pdir);
-
-			// barycentric coordinates:
-			b3FindPos(portal, pos);
-
-			return;
-		}
-
-		b3ExpandPortal(portal, &v4);
-
-		iterations++;
-	}
-}
-
-B3_STATIC void b3FindPenetrTouch(b3MprSimplex_t *portal, float *depth, b3Float4 *dir, b3Float4 *pos)
-{
-	// Touching contact on portal's v1 - so depth is zero and direction
-	// is unimportant and pos can be guessed
-	*depth = 0.f;
-	b3Float4 zero = b3MakeFloat4(0, 0, 0, 0);
-	b3Float4 *b3mpr_vec3_origin = &zero;
-
-	b3MprVec3Copy(dir, b3mpr_vec3_origin);
-
-	b3MprVec3Copy(pos, &b3MprSimplexPoint(portal, 1)->v1);
-	b3MprVec3Add(pos, &b3MprSimplexPoint(portal, 1)->v2);
-	b3MprVec3Scale(pos, 0.5);
-}
-
-B3_STATIC void b3FindPenetrSegment(b3MprSimplex_t *portal,
-								   float *depth, b3Float4 *dir, b3Float4 *pos)
-{
-	// Origin lies on v0-v1 segment.
-	// Depth is distance to v1, direction also and position must be
-	// computed
-
-	b3MprVec3Copy(pos, &b3MprSimplexPoint(portal, 1)->v1);
-	b3MprVec3Add(pos, &b3MprSimplexPoint(portal, 1)->v2);
-	b3MprVec3Scale(pos, 0.5f);
-
-	b3MprVec3Copy(dir, &b3MprSimplexPoint(portal, 1)->v);
-	*depth = B3_MPR_SQRT(b3MprVec3Len2(dir));
-	b3MprVec3Normalize(dir);
-}
-
-inline int b3MprPenetration(int pairIndex, int bodyIndexA, int bodyIndexB,
-							b3ConstArray(b3RigidBodyData_t) cpuBodyBuf,
-							b3ConstArray(b3ConvexPolyhedronData_t) cpuConvexData,
-							b3ConstArray(b3Collidable_t) cpuCollidables,
-							b3ConstArray(b3Float4) cpuVertices,
-							__global b3Float4 *sepAxis,
-							__global int *hasSepAxis,
-							float *depthOut, b3Float4 *dirOut, b3Float4 *posOut)
-{
-	b3MprSimplex_t portal;
-
-	//	if (!hasSepAxis[pairIndex])
-	//	return -1;
-
-	hasSepAxis[pairIndex] = 0;
-	int res;
-
-	// Phase 1: Portal discovery
-	res = b3DiscoverPortal(pairIndex, bodyIndexA, bodyIndexB, cpuBodyBuf, cpuConvexData, cpuCollidables, cpuVertices, sepAxis, hasSepAxis, &portal);
-
-	//sepAxis[pairIndex] = *pdir;//or -dir?
-
-	switch (res)
-	{
-		case 0:
-		{
-			// Phase 2: Portal refinement
-
-			res = b3RefinePortal(pairIndex, bodyIndexA, bodyIndexB, cpuBodyBuf, cpuConvexData, cpuCollidables, cpuVertices, sepAxis, &portal);
-			if (res < 0)
-				return -1;
-
-			// Phase 3. Penetration info
-			b3FindPenetr(pairIndex, bodyIndexA, bodyIndexB, cpuBodyBuf, cpuConvexData, cpuCollidables, cpuVertices, sepAxis, &portal, depthOut, dirOut, posOut);
-			hasSepAxis[pairIndex] = 1;
-			sepAxis[pairIndex] = -*dirOut;
-			break;
-		}
-		case 1:
-		{
-			// Touching contact on portal's v1.
-			b3FindPenetrTouch(&portal, depthOut, dirOut, posOut);
-			break;
-		}
-		case 2:
-		{
-			b3FindPenetrSegment(&portal, depthOut, dirOut, posOut);
-			break;
-		}
-		default:
-		{
-			hasSepAxis[pairIndex] = 0;
-			//if (res < 0)
-			//{
-			// Origin isn't inside portal - no collision.
-			return -1;
-			//}
-		}
-	};
-
-	return 0;
-};
-
-#endif  //B3_MPR_PENETRATION_H

thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3NewContactReduction.h

@@ -1,175 +0,0 @@
-
-#ifndef B3_NEW_CONTACT_REDUCTION_H
-#define B3_NEW_CONTACT_REDUCTION_H
-
-#include "Bullet3Common/shared/b3Float4.h"
-#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h"
-#include "Bullet3Collision/NarrowPhaseCollision/shared/b3Contact4Data.h"
-
-#define GET_NPOINTS(x) (x).m_worldNormalOnB.w
-
-int b3ExtractManifoldSequentialGlobal(__global const b3Float4* p, int nPoints, b3Float4ConstArg nearNormal, b3Int4* contactIdx)
-{
-	if (nPoints == 0)
-		return 0;
-
-	if (nPoints <= 4)
-		return nPoints;
-
-	if (nPoints > 64)
-		nPoints = 64;
-
-	b3Float4 center = b3MakeFloat4(0, 0, 0, 0);
-	{
-		for (int i = 0; i < nPoints; i++)
-			center += p[i];
-		center /= (float)nPoints;
-	}
-
-	//	sample 4 directions
-
-	b3Float4 aVector = p[0] - center;
-	b3Float4 u = b3Cross(nearNormal, aVector);
-	b3Float4 v = b3Cross(nearNormal, u);
-	u = b3Normalized(u);
-	v = b3Normalized(v);
-
-	//keep point with deepest penetration
-	float minW = FLT_MAX;
-
-	int minIndex = -1;
-
-	b3Float4 maxDots;
-	maxDots.x = FLT_MIN;
-	maxDots.y = FLT_MIN;
-	maxDots.z = FLT_MIN;
-	maxDots.w = FLT_MIN;
-
-	//	idx, distance
-	for (int ie = 0; ie < nPoints; ie++)
-	{
-		if (p[ie].w < minW)
-		{
-			minW = p[ie].w;
-			minIndex = ie;
-		}
-		float f;
-		b3Float4 r = p[ie] - center;
-		f = b3Dot(u, r);
-		if (f < maxDots.x)
-		{
-			maxDots.x = f;
-			contactIdx[0].x = ie;
-		}
-
-		f = b3Dot(-u, r);
-		if (f < maxDots.y)
-		{
-			maxDots.y = f;
-			contactIdx[0].y = ie;
-		}
-
-		f = b3Dot(v, r);
-		if (f < maxDots.z)
-		{
-			maxDots.z = f;
-			contactIdx[0].z = ie;
-		}
-
-		f = b3Dot(-v, r);
-		if (f < maxDots.w)
-		{
-			maxDots.w = f;
-			contactIdx[0].w = ie;
-		}
-	}
-
-	if (contactIdx[0].x != minIndex && contactIdx[0].y != minIndex && contactIdx[0].z != minIndex && contactIdx[0].w != minIndex)
-	{
-		//replace the first contact with minimum (todo: replace contact with least penetration)
-		contactIdx[0].x = minIndex;
-	}
-
-	return 4;
-}
-
-__kernel void b3NewContactReductionKernel(__global b3Int4* pairs,
-										  __global const b3RigidBodyData_t* rigidBodies,
-										  __global const b3Float4* separatingNormals,
-										  __global const int* hasSeparatingAxis,
-										  __global struct b3Contact4Data* globalContactsOut,
-										  __global b3Int4* clippingFaces,
-										  __global b3Float4* worldVertsB2,
-										  volatile __global int* nGlobalContactsOut,
-										  int vertexFaceCapacity,
-										  int contactCapacity,
-										  int numPairs,
-										  int pairIndex)
-{
-	//    int i = get_global_id(0);
-	//int pairIndex = i;
-	int i = pairIndex;
-
-	b3Int4 contactIdx;
-	contactIdx = b3MakeInt4(0, 1, 2, 3);
-
-	if (i < numPairs)
-	{
-		if (hasSeparatingAxis[i])
-		{
-			int nPoints = clippingFaces[pairIndex].w;
-
-			if (nPoints > 0)
-			{
-				__global b3Float4* pointsIn = &worldVertsB2[pairIndex * vertexFaceCapacity];
-				b3Float4 normal = -separatingNormals[i];
-
-				int nReducedContacts = b3ExtractManifoldSequentialGlobal(pointsIn, nPoints, normal, &contactIdx);
-
-				int dstIdx;
-				dstIdx = b3AtomicInc(nGlobalContactsOut);
-
-				//#if 0
-				b3Assert(dstIdx < contactCapacity);
-				if (dstIdx < contactCapacity)
-				{
-					__global struct b3Contact4Data* c = &globalContactsOut[dstIdx];
-					c->m_worldNormalOnB = -normal;
-					c->m_restituitionCoeffCmp = (0.f * 0xffff);
-					c->m_frictionCoeffCmp = (0.7f * 0xffff);
-					c->m_batchIdx = pairIndex;
-					int bodyA = pairs[pairIndex].x;
-					int bodyB = pairs[pairIndex].y;
-
-					pairs[pairIndex].w = dstIdx;
-
-					c->m_bodyAPtrAndSignBit = rigidBodies[bodyA].m_invMass == 0 ? -bodyA : bodyA;
-					c->m_bodyBPtrAndSignBit = rigidBodies[bodyB].m_invMass == 0 ? -bodyB : bodyB;
-					c->m_childIndexA = -1;
-					c->m_childIndexB = -1;
-
-					switch (nReducedContacts)
-					{
-						case 4:
-							c->m_worldPosB[3] = pointsIn[contactIdx.w];
-						case 3:
-							c->m_worldPosB[2] = pointsIn[contactIdx.z];
-						case 2:
-							c->m_worldPosB[1] = pointsIn[contactIdx.y];
-						case 1:
-							c->m_worldPosB[0] = pointsIn[contactIdx.x];
-						default:
-						{
-						}
-					};
-
-					GET_NPOINTS(*c) = nReducedContacts;
-				}
-
-				//#endif
-
-			}  //		if (numContactsOut>0)
-		}      //		if (hasSeparatingAxis[i])
-	}          //	if (i<numPairs)
-}
-#endif

thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3QuantizedBvhNodeData.h

@@ -1,88 +0,0 @@
-
-
-#ifndef B3_QUANTIZED_BVH_NODE_H
-#define B3_QUANTIZED_BVH_NODE_H
-
-#include "Bullet3Common/shared/b3Float4.h"
-
-#define B3_MAX_NUM_PARTS_IN_BITS 10
-
-///b3QuantizedBvhNodeData is a compressed aabb node, 16 bytes.
-///Node can be used for leafnode or internal node. Leafnodes can point to 32-bit triangle index (non-negative range).
-typedef struct b3QuantizedBvhNodeData b3QuantizedBvhNodeData_t;
-
-struct b3QuantizedBvhNodeData
-{
-	//12 bytes
-	unsigned short int m_quantizedAabbMin[3];
-	unsigned short int m_quantizedAabbMax[3];
-	//4 bytes
-	int m_escapeIndexOrTriangleIndex;
-};
-
-inline int b3GetTriangleIndex(const b3QuantizedBvhNodeData* rootNode)
-{
-	unsigned int x = 0;
-	unsigned int y = (~(x & 0)) << (31 - B3_MAX_NUM_PARTS_IN_BITS);
-	// Get only the lower bits where the triangle index is stored
-	return (rootNode->m_escapeIndexOrTriangleIndex & ~(y));
-}
-
-inline int b3IsLeaf(const b3QuantizedBvhNodeData* rootNode)
-{
-	//skipindex is negative (internal node), triangleindex >=0 (leafnode)
-	return (rootNode->m_escapeIndexOrTriangleIndex >= 0) ? 1 : 0;
-}
-
-inline int b3GetEscapeIndex(const b3QuantizedBvhNodeData* rootNode)
-{
-	return -rootNode->m_escapeIndexOrTriangleIndex;
-}
-
-inline void b3QuantizeWithClamp(unsigned short* out, b3Float4ConstArg point2, int isMax, b3Float4ConstArg bvhAabbMin, b3Float4ConstArg bvhAabbMax, b3Float4ConstArg bvhQuantization)
-{
-	b3Float4 clampedPoint = b3MaxFloat4(point2, bvhAabbMin);
-	clampedPoint = b3MinFloat4(clampedPoint, bvhAabbMax);
-
-	b3Float4 v = (clampedPoint - bvhAabbMin) * bvhQuantization;
-	if (isMax)
-	{
-		out[0] = (unsigned short)(((unsigned short)(v.x + 1.f) | 1));
-		out[1] = (unsigned short)(((unsigned short)(v.y + 1.f) | 1));
-		out[2] = (unsigned short)(((unsigned short)(v.z + 1.f) | 1));
-	}
-	else
-	{
-		out[0] = (unsigned short)(((unsigned short)(v.x) & 0xfffe));
-		out[1] = (unsigned short)(((unsigned short)(v.y) & 0xfffe));
-		out[2] = (unsigned short)(((unsigned short)(v.z) & 0xfffe));
-	}
-}
-
-inline int b3TestQuantizedAabbAgainstQuantizedAabbSlow(
-	const unsigned short int* aabbMin1,
-	const unsigned short int* aabbMax1,
-	const unsigned short int* aabbMin2,
-	const unsigned short int* aabbMax2)
-{
-	//int overlap = 1;
-	if (aabbMin1[0] > aabbMax2[0])
-		return 0;
-	if (aabbMax1[0] < aabbMin2[0])
-		return 0;
-	if (aabbMin1[1] > aabbMax2[1])
-		return 0;
-	if (aabbMax1[1] < aabbMin2[1])
-		return 0;
-	if (aabbMin1[2] > aabbMax2[2])
-		return 0;
-	if (aabbMax1[2] < aabbMin2[2])
-		return 0;
-	return 1;
-	//overlap = ((aabbMin1[0] > aabbMax2[0]) || (aabbMax1[0] < aabbMin2[0])) ? 0 : overlap;
-	//overlap = ((aabbMin1[2] > aabbMax2[2]) || (aabbMax1[2] < aabbMin2[2])) ? 0 : overlap;
-	//overlap = ((aabbMin1[1] > aabbMax2[1]) || (aabbMax1[1] < aabbMin2[1])) ? 0 : overlap;
-	//return overlap;
-}
-
-#endif  //B3_QUANTIZED_BVH_NODE_H

thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3ReduceContacts.h

@@ -1,89 +0,0 @@
-#ifndef B3_REDUCE_CONTACTS_H
-#define B3_REDUCE_CONTACTS_H
-
-inline int b3ReduceContacts(const b3Float4* p, int nPoints, const b3Float4& nearNormal, b3Int4* contactIdx)
-{
-	if (nPoints == 0)
-		return 0;
-
-	if (nPoints <= 4)
-		return nPoints;
-
-	if (nPoints > 64)
-		nPoints = 64;
-
-	b3Float4 center = b3MakeFloat4(0, 0, 0, 0);
-	{
-		for (int i = 0; i < nPoints; i++)
-			center += p[i];
-		center /= (float)nPoints;
-	}
-
-	//	sample 4 directions
-
-	b3Float4 aVector = p[0] - center;
-	b3Float4 u = b3Cross3(nearNormal, aVector);
-	b3Float4 v = b3Cross3(nearNormal, u);
-	u = b3FastNormalized3(u);
-	v = b3FastNormalized3(v);
-
-	//keep point with deepest penetration
-	float minW = FLT_MAX;
-
-	int minIndex = -1;
-
-	b3Float4 maxDots;
-	maxDots.x = FLT_MIN;
-	maxDots.y = FLT_MIN;
-	maxDots.z = FLT_MIN;
-	maxDots.w = FLT_MIN;
-
-	//	idx, distance
-	for (int ie = 0; ie < nPoints; ie++)
-	{
-		if (p[ie].w < minW)
-		{
-			minW = p[ie].w;
-			minIndex = ie;
-		}
-		float f;
-		b3Float4 r = p[ie] - center;
-		f = b3Dot3F4(u, r);
-		if (f < maxDots.x)
-		{
-			maxDots.x = f;
-			contactIdx[0].x = ie;
-		}
-
-		f = b3Dot3F4(-u, r);
-		if (f < maxDots.y)
-		{
-			maxDots.y = f;
-			contactIdx[0].y = ie;
-		}
-
-		f = b3Dot3F4(v, r);
-		if (f < maxDots.z)
-		{
-			maxDots.z = f;
-			contactIdx[0].z = ie;
-		}
-
-		f = b3Dot3F4(-v, r);
-		if (f < maxDots.w)
-		{
-			maxDots.w = f;
-			contactIdx[0].w = ie;
-		}
-	}
-
-	if (contactIdx[0].x != minIndex && contactIdx[0].y != minIndex && contactIdx[0].z != minIndex && contactIdx[0].w != minIndex)
-	{
-		//replace the first contact with minimum (todo: replace contact with least penetration)
-		contactIdx[0].x = minIndex;
-	}
-
-	return 4;
-}
-
-#endif  //B3_REDUCE_CONTACTS_H

thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h

@@ -1,31 +0,0 @@
-#ifndef B3_RIGIDBODY_DATA_H
-#define B3_RIGIDBODY_DATA_H
-
-#include "Bullet3Common/shared/b3Float4.h"
-#include "Bullet3Common/shared/b3Quat.h"
-#include "Bullet3Common/shared/b3Mat3x3.h"
-
-typedef struct b3RigidBodyData b3RigidBodyData_t;
-
-struct b3RigidBodyData
-{
-	b3Float4 m_pos;
-	b3Quat m_quat;
-	b3Float4 m_linVel;
-	b3Float4 m_angVel;
-
-	int m_collidableIdx;
-	float m_invMass;
-	float m_restituitionCoeff;
-	float m_frictionCoeff;
-};
-
-typedef struct b3InertiaData b3InertiaData_t;
-
-struct b3InertiaData
-{
-	b3Mat3x3 m_invInertiaWorld;
-	b3Mat3x3 m_initInvInertia;
-};
-
-#endif  //B3_RIGIDBODY_DATA_H

thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3UpdateAabbs.h

@@ -1,35 +0,0 @@
-#ifndef B3_UPDATE_AABBS_H
-#define B3_UPDATE_AABBS_H
-
-#include "Bullet3Collision/BroadPhaseCollision/shared/b3Aabb.h"
-#include "Bullet3Collision/NarrowPhaseCollision/shared/b3Collidable.h"
-#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h"
-
-void b3ComputeWorldAabb(int bodyId, __global const b3RigidBodyData_t* bodies, __global const b3Collidable_t* collidables, __global const b3Aabb_t* localShapeAABB, __global b3Aabb_t* worldAabbs)
-{
-	__global const b3RigidBodyData_t* body = &bodies[bodyId];
-
-	b3Float4 position = body->m_pos;
-	b3Quat orientation = body->m_quat;
-
-	int collidableIndex = body->m_collidableIdx;
-	int shapeIndex = collidables[collidableIndex].m_shapeIndex;
-
-	if (shapeIndex >= 0)
-	{
-		b3Aabb_t localAabb = localShapeAABB[collidableIndex];
-		b3Aabb_t worldAabb;
-
-		b3Float4 aabbAMinOut, aabbAMaxOut;
-		float margin = 0.f;
-		b3TransformAabb2(localAabb.m_minVec, localAabb.m_maxVec, margin, position, orientation, &aabbAMinOut, &aabbAMaxOut);
-
-		worldAabb.m_minVec = aabbAMinOut;
-		worldAabb.m_minIndices[3] = bodyId;
-		worldAabb.m_maxVec = aabbAMaxOut;
-		worldAabb.m_signedMaxIndices[3] = body[bodyId].m_invMass == 0.f ? 0 : 1;
-		worldAabbs[bodyId] = worldAabb;
-	}
-}
-
-#endif  //B3_UPDATE_AABBS_H

thirdparty/bullet/Bullet3Common/b3AlignedAllocator.cpp

@@ -1,186 +0,0 @@
-/*
-Bullet Continuous Collision Detection and Physics Library
-Copyright (c) 2003-2013 Erwin Coumans  http://bulletphysics.org
-
-This software is provided 'as-is', without any express or implied warranty.
-In no event will the authors be held liable for any damages arising from the use of this software.
-Permission is granted to anyone to use this software for any purpose,
-including commercial applications, and to alter it and redistribute it freely,
-subject to the following restrictions:
-
-1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
-2. Altered source versions must be plainly marked as 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 "b3AlignedAllocator.h"
-
-#ifdef B3_ALLOCATOR_STATISTICS
-int b3g_numAlignedAllocs = 0;
-int b3g_numAlignedFree = 0;
-int b3g_totalBytesAlignedAllocs = 0;  //detect memory leaks
-#endif
-
-static void *b3AllocDefault(size_t size)
-{
-	return malloc(size);
-}
-
-static void b3FreeDefault(void *ptr)
-{
-	free(ptr);
-}
-
-static b3AllocFunc *b3s_allocFunc = b3AllocDefault;
-static b3FreeFunc *b3s_freeFunc = b3FreeDefault;
-
-#if defined(B3_HAS_ALIGNED_ALLOCATOR)
-#include <malloc.h>
-static void *b3AlignedAllocDefault(size_t size, int alignment)
-{
-	return _aligned_malloc(size, (size_t)alignment);
-}
-
-static void b3AlignedFreeDefault(void *ptr)
-{
-	_aligned_free(ptr);
-}
-#elif defined(__CELLOS_LV2__)
-#include <stdlib.h>
-
-static inline void *b3AlignedAllocDefault(size_t size, int alignment)
-{
-	return memalign(alignment, size);
-}
-
-static inline void b3AlignedFreeDefault(void *ptr)
-{
-	free(ptr);
-}
-#else
-
-static inline void *b3AlignedAllocDefault(size_t size, int alignment)
-{
-	void *ret;
-	char *real;
-	real = (char *)b3s_allocFunc(size + sizeof(void *) + (alignment - 1));
-	if (real)
-	{
-		ret = b3AlignPointer(real + sizeof(void *), alignment);
-		*((void **)(ret)-1) = (void *)(real);
-	}
-	else
-	{
-		ret = (void *)(real);
-	}
-	return (ret);
-}
-
-static inline void b3AlignedFreeDefault(void *ptr)
-{
-	void *real;
-
-	if (ptr)
-	{
-		real = *((void **)(ptr)-1);
-		b3s_freeFunc(real);
-	}
-}
-#endif
-
-static b3AlignedAllocFunc *b3s_alignedAllocFunc = b3AlignedAllocDefault;
-static b3AlignedFreeFunc *b3s_alignedFreeFunc = b3AlignedFreeDefault;
-
-void b3AlignedAllocSetCustomAligned(b3AlignedAllocFunc *allocFunc, b3AlignedFreeFunc *freeFunc)
-{
-	b3s_alignedAllocFunc = allocFunc ? allocFunc : b3AlignedAllocDefault;
-	b3s_alignedFreeFunc = freeFunc ? freeFunc : b3AlignedFreeDefault;
-}
-
-void b3AlignedAllocSetCustom(b3AllocFunc *allocFunc, b3FreeFunc *freeFunc)
-{
-	b3s_allocFunc = allocFunc ? allocFunc : b3AllocDefault;
-	b3s_freeFunc = freeFunc ? freeFunc : b3FreeDefault;
-}
-
-#ifdef B3_DEBUG_MEMORY_ALLOCATIONS
-//this generic allocator provides the total allocated number of bytes
-#include <stdio.h>
-
-void *b3AlignedAllocInternal(size_t size, int alignment, int line, char *filename)
-{
-	void *ret;
-	char *real;
-#ifdef B3_ALLOCATOR_STATISTICS
-	b3g_totalBytesAlignedAllocs += size;
-	b3g_numAlignedAllocs++;
-#endif
-	real = (char *)b3s_allocFunc(size + 2 * sizeof(void *) + (alignment - 1));
-	if (real)
-	{
-		ret = (void *)b3AlignPointer(real + 2 * sizeof(void *), alignment);
-		*((void **)(ret)-1) = (void *)(real);
-		*((int *)(ret)-2) = size;
-	}
-	else
-	{
-		ret = (void *)(real);  //??
-	}
-
-	b3Printf("allocation#%d at address %x, from %s,line %d, size %d\n", b3g_numAlignedAllocs, real, filename, line, size);
-
-	int *ptr = (int *)ret;
-	*ptr = 12;
-	return (ret);
-}
-
-void b3AlignedFreeInternal(void *ptr, int line, char *filename)
-{
-	void *real;
-#ifdef B3_ALLOCATOR_STATISTICS
-	b3g_numAlignedFree++;
-#endif
-	if (ptr)
-	{
-		real = *((void **)(ptr)-1);
-		int size = *((int *)(ptr)-2);
-#ifdef B3_ALLOCATOR_STATISTICS
-		b3g_totalBytesAlignedAllocs -= size;
-#endif
-		b3Printf("free #%d at address %x, from %s,line %d, size %d\n", b3g_numAlignedFree, real, filename, line, size);
-
-		b3s_freeFunc(real);
-	}
-	else
-	{
-		b3Printf("NULL ptr\n");
-	}
-}
-
-#else  //B3_DEBUG_MEMORY_ALLOCATIONS
-
-void *b3AlignedAllocInternal(size_t size, int alignment)
-{
-#ifdef B3_ALLOCATOR_STATISTICS
-	b3g_numAlignedAllocs++;
-#endif
-	void *ptr;
-	ptr = b3s_alignedAllocFunc(size, alignment);
-	//	b3Printf("b3AlignedAllocInternal %d, %x\n",size,ptr);
-	return ptr;
-}
-
-void b3AlignedFreeInternal(void *ptr)
-{
-	if (!ptr)
-	{
-		return;
-	}
-#ifdef B3_ALLOCATOR_STATISTICS
-	b3g_numAlignedFree++;
-#endif
-	//	b3Printf("b3AlignedFreeInternal %x\n",ptr);
-	b3s_alignedFreeFunc(ptr);
-}
-
-#endif  //B3_DEBUG_MEMORY_ALLOCATIONS

thirdparty/bullet/Bullet3Common/b3AlignedAllocator.h

@@ -1,110 +0,0 @@
-/*
-Bullet Continuous Collision Detection and Physics Library
-Copyright (c) 2003-2013 Erwin Coumans  http://bulletphysics.org
-
-This software is provided 'as-is', without any express or implied warranty.
-In no event will the authors be held liable for any damages arising from the use of this software.
-Permission is granted to anyone to use this software for any purpose,
-including commercial applications, and to alter it and redistribute it freely,
-subject to the following restrictions:
-
-1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
-2. Altered source versions must be plainly marked as 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 B3_ALIGNED_ALLOCATOR
-#define B3_ALIGNED_ALLOCATOR
-
-///we probably replace this with our own aligned memory allocator
-///so we replace _aligned_malloc and _aligned_free with our own
-///that is better portable and more predictable
-
-#include "b3Scalar.h"
-//#define B3_DEBUG_MEMORY_ALLOCATIONS 1
-#ifdef B3_DEBUG_MEMORY_ALLOCATIONS
-
-#define b3AlignedAlloc(a, b) \
-	b3AlignedAllocInternal(a, b, __LINE__, __FILE__)
-
-#define b3AlignedFree(ptr) \
-	b3AlignedFreeInternal(ptr, __LINE__, __FILE__)
-
-void* b3AlignedAllocInternal(size_t size, int alignment, int line, char* filename);
-
-void b3AlignedFreeInternal(void* ptr, int line, char* filename);
-
-#else
-void* b3AlignedAllocInternal(size_t size, int alignment);
-void b3AlignedFreeInternal(void* ptr);
-
-#define b3AlignedAlloc(size, alignment) b3AlignedAllocInternal(size, alignment)
-#define b3AlignedFree(ptr) b3AlignedFreeInternal(ptr)
-
-#endif
-typedef int btSizeType;
-
-typedef void*(b3AlignedAllocFunc)(size_t size, int alignment);
-typedef void(b3AlignedFreeFunc)(void* memblock);
-typedef void*(b3AllocFunc)(size_t size);
-typedef void(b3FreeFunc)(void* memblock);
-
-///The developer can let all Bullet memory allocations go through a custom memory allocator, using b3AlignedAllocSetCustom
-void b3AlignedAllocSetCustom(b3AllocFunc* allocFunc, b3FreeFunc* freeFunc);
-///If the developer has already an custom aligned allocator, then b3AlignedAllocSetCustomAligned can be used. The default aligned allocator pre-allocates extra memory using the non-aligned allocator, and instruments it.
-void b3AlignedAllocSetCustomAligned(b3AlignedAllocFunc* allocFunc, b3AlignedFreeFunc* freeFunc);
-
-///The b3AlignedAllocator is a portable class for aligned memory allocations.
-///Default implementations for unaligned and aligned allocations can be overridden by a custom allocator using b3AlignedAllocSetCustom and b3AlignedAllocSetCustomAligned.
-template <typename T, unsigned Alignment>
-class b3AlignedAllocator
-{
-	typedef b3AlignedAllocator<T, Alignment> self_type;
-
-public:
-	//just going down a list:
-	b3AlignedAllocator() {}
-	/*
-	b3AlignedAllocator( const self_type & ) {}
-	*/
-
-	template <typename Other>
-	b3AlignedAllocator(const b3AlignedAllocator<Other, Alignment>&)
-	{
-	}
-
-	typedef const T* const_pointer;
-	typedef const T& const_reference;
-	typedef T* pointer;
-	typedef T& reference;
-	typedef T value_type;
-
-	pointer address(reference ref) const { return &ref; }
-	const_pointer address(const_reference ref) const { return &ref; }
-	pointer allocate(btSizeType n, const_pointer* hint = 0)
-	{
-		(void)hint;
-		return reinterpret_cast<pointer>(b3AlignedAlloc(sizeof(value_type) * n, Alignment));
-	}
-	void construct(pointer ptr, const value_type& value) { new (ptr) value_type(value); }
-	void deallocate(pointer ptr)
-	{
-		b3AlignedFree(reinterpret_cast<void*>(ptr));
-	}
-	void destroy(pointer ptr) { ptr->~value_type(); }
-
-	template <typename O>
-	struct rebind
-	{
-		typedef b3AlignedAllocator<O, Alignment> other;
-	};
-	template <typename O>
-	self_type& operator=(const b3AlignedAllocator<O, Alignment>&)
-	{
-		return *this;
-	}
-
-	friend bool operator==(const self_type&, const self_type&) { return true; }
-};
-
-#endif  //B3_ALIGNED_ALLOCATOR

thirdparty/bullet/Bullet3Common/b3AlignedObjectArray.h

@@ -1,522 +0,0 @@
-/*
-Bullet Continuous Collision Detection and Physics Library
-Copyright (c) 2003-2013 Erwin Coumans  http://bulletphysics.org
-
-This software is provided 'as-is', without any express or implied warranty.
-In no event will the authors be held liable for any damages arising from the use of this software.
-Permission is granted to anyone to use this software for any purpose,
-including commercial applications, and to alter it and redistribute it freely,
-subject to the following restrictions:
-
-1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
-2. Altered source versions must be plainly marked as 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 B3_OBJECT_ARRAY__
-#define B3_OBJECT_ARRAY__
-
-#include "b3Scalar.h"  // has definitions like B3_FORCE_INLINE
-#include "b3AlignedAllocator.h"
-
-///If the platform doesn't support placement new, you can disable B3_USE_PLACEMENT_NEW
-///then the b3AlignedObjectArray doesn't support objects with virtual methods, and non-trivial constructors/destructors
-///You can enable B3_USE_MEMCPY, then swapping elements in the array will use memcpy instead of operator=
-///see discussion here: http://continuousphysics.com/Bullet/phpBB2/viewtopic.php?t=1231 and
-///http://www.continuousphysics.com/Bullet/phpBB2/viewtopic.php?t=1240
-
-#define B3_USE_PLACEMENT_NEW 1
-//#define B3_USE_MEMCPY 1 //disable, because it is cumbersome to find out for each platform where memcpy is defined. It can be in <memory.h> or <string.h> or otherwise...
-#define B3_ALLOW_ARRAY_COPY_OPERATOR  // enabling this can accidently perform deep copies of data if you are not careful
-
-#ifdef B3_USE_MEMCPY
-#include <memory.h>
-#include <string.h>
-#endif  //B3_USE_MEMCPY
-
-#ifdef B3_USE_PLACEMENT_NEW
-#include <new>  //for placement new
-#endif          //B3_USE_PLACEMENT_NEW
-
-///The b3AlignedObjectArray template class uses a subset of the stl::vector interface for its methods
-///It is developed to replace stl::vector to avoid portability issues, including STL alignment issues to add SIMD/SSE data
-template <typename T>
-//template <class T>
-class b3AlignedObjectArray
-{
-	b3AlignedAllocator<T, 16> m_allocator;
-
-	int m_size;
-	int m_capacity;
-	T* m_data;
-	//PCK: added this line
-	bool m_ownsMemory;
-
-#ifdef B3_ALLOW_ARRAY_COPY_OPERATOR
-public:
-	B3_FORCE_INLINE b3AlignedObjectArray<T>& operator=(const b3AlignedObjectArray<T>& other)
-	{
-		copyFromArray(other);
-		return *this;
-	}
-#else   //B3_ALLOW_ARRAY_COPY_OPERATOR
-private:
-	B3_FORCE_INLINE b3AlignedObjectArray<T>& operator=(const b3AlignedObjectArray<T>& other);
-#endif  //B3_ALLOW_ARRAY_COPY_OPERATOR
-
-protected:
-	B3_FORCE_INLINE int allocSize(int size)
-	{
-		return (size ? size * 2 : 1);
-	}
-	B3_FORCE_INLINE void copy(int start, int end, T* dest) const
-	{
-		int i;
-		for (i = start; i < end; ++i)
-#ifdef B3_USE_PLACEMENT_NEW
-			new (&dest[i]) T(m_data[i]);
-#else
-			dest[i] = m_data[i];
-#endif  //B3_USE_PLACEMENT_NEW
-	}
-
-	B3_FORCE_INLINE void init()
-	{
-		//PCK: added this line
-		m_ownsMemory = true;
-		m_data = 0;
-		m_size = 0;
-		m_capacity = 0;
-	}
-	B3_FORCE_INLINE void destroy(int first, int last)
-	{
-		int i;
-		for (i = first; i < last; i++)
-		{
-			m_data[i].~T();
-		}
-	}
-
-	B3_FORCE_INLINE void* allocate(int size)
-	{
-		if (size)
-			return m_allocator.allocate(size);
-		return 0;
-	}
-
-	B3_FORCE_INLINE void deallocate()
-	{
-		if (m_data)
-		{
-			//PCK: enclosed the deallocation in this block
-			if (m_ownsMemory)
-			{
-				m_allocator.deallocate(m_data);
-			}
-			m_data = 0;
-		}
-	}
-
-public:
-	b3AlignedObjectArray()
-	{
-		init();
-	}
-
-	~b3AlignedObjectArray()
-	{
-		clear();
-	}
-
-	///Generally it is best to avoid using the copy constructor of an b3AlignedObjectArray, and use a (const) reference to the array instead.
-	b3AlignedObjectArray(const b3AlignedObjectArray& otherArray)
-	{
-		init();
-
-		int otherSize = otherArray.size();
-		resize(otherSize);
-		otherArray.copy(0, otherSize, m_data);
-	}
-
-	/// return the number of elements in the array
-	B3_FORCE_INLINE int size() const
-	{
-		return m_size;
-	}
-
-	B3_FORCE_INLINE const T& at(int n) const
-	{
-		b3Assert(n >= 0);
-		b3Assert(n < size());
-		return m_data[n];
-	}
-
-	B3_FORCE_INLINE T& at(int n)
-	{
-		b3Assert(n >= 0);
-		b3Assert(n < size());
-		return m_data[n];
-	}
-
-	B3_FORCE_INLINE const T& operator[](int n) const
-	{
-		b3Assert(n >= 0);
-		b3Assert(n < size());
-		return m_data[n];
-	}
-
-	B3_FORCE_INLINE T& operator[](int n)
-	{
-		b3Assert(n >= 0);
-		b3Assert(n < size());
-		return m_data[n];
-	}
-
-	///clear the array, deallocated memory. Generally it is better to use array.resize(0), to reduce performance overhead of run-time memory (de)allocations.
-	B3_FORCE_INLINE void clear()
-	{
-		destroy(0, size());
-
-		deallocate();
-
-		init();
-	}
-
-	B3_FORCE_INLINE void pop_back()
-	{
-		b3Assert(m_size > 0);
-		m_size--;
-		m_data[m_size].~T();
-	}
-
-	///resize changes the number of elements in the array. If the new size is larger, the new elements will be constructed using the optional second argument.
-	///when the new number of elements is smaller, the destructor will be called, but memory will not be freed, to reduce performance overhead of run-time memory (de)allocations.
-	B3_FORCE_INLINE void resizeNoInitialize(int newsize)
-	{
-		int curSize = size();
-
-		if (newsize < curSize)
-		{
-		}
-		else
-		{
-			if (newsize > size())
-			{
-				reserve(newsize);
-			}
-			//leave this uninitialized
-		}
-		m_size = newsize;
-	}
-
-	B3_FORCE_INLINE void resize(int newsize, const T& fillData = T())
-	{
-		int curSize = size();
-
-		if (newsize < curSize)
-		{
-			for (int i = newsize; i < curSize; i++)
-			{
-				m_data[i].~T();
-			}
-		}
-		else
-		{
-			if (newsize > size())
-			{
-				reserve(newsize);
-			}
-#ifdef B3_USE_PLACEMENT_NEW
-			for (int i = curSize; i < newsize; i++)
-			{
-				new (&m_data[i]) T(fillData);
-			}
-#endif  //B3_USE_PLACEMENT_NEW
-		}
-
-		m_size = newsize;
-	}
-	B3_FORCE_INLINE T& expandNonInitializing()
-	{
-		int sz = size();
-		if (sz == capacity())
-		{
-			reserve(allocSize(size()));
-		}
-		m_size++;
-
-		return m_data[sz];
-	}
-
-	B3_FORCE_INLINE T& expand(const T& fillValue = T())
-	{
-		int sz = size();
-		if (sz == capacity())
-		{
-			reserve(allocSize(size()));
-		}
-		m_size++;
-#ifdef B3_USE_PLACEMENT_NEW
-		new (&m_data[sz]) T(fillValue);  //use the in-place new (not really allocating heap memory)
-#endif
-
-		return m_data[sz];
-	}
-
-	B3_FORCE_INLINE void push_back(const T& _Val)
-	{
-		int sz = size();
-		if (sz == capacity())
-		{
-			reserve(allocSize(size()));
-		}
-
-#ifdef B3_USE_PLACEMENT_NEW
-		new (&m_data[m_size]) T(_Val);
-#else
-		m_data[size()] = _Val;
-#endif  //B3_USE_PLACEMENT_NEW
-
-		m_size++;
-	}
-
-	/// return the pre-allocated (reserved) elements, this is at least as large as the total number of elements,see size() and reserve()
-	B3_FORCE_INLINE int capacity() const
-	{
-		return m_capacity;
-	}
-
-	B3_FORCE_INLINE void reserve(int _Count)
-	{  // determine new minimum length of allocated storage
-		if (capacity() < _Count)
-		{  // not enough room, reallocate
-			T* s = (T*)allocate(_Count);
-			b3Assert(s);
-			if (s == 0)
-			{
-				b3Error("b3AlignedObjectArray reserve out-of-memory\n");
-				_Count = 0;
-				m_size = 0;
-			}
-			copy(0, size(), s);
-
-			destroy(0, size());
-
-			deallocate();
-
-			//PCK: added this line
-			m_ownsMemory = true;
-
-			m_data = s;
-
-			m_capacity = _Count;
-		}
-	}
-
-	class less
-	{
-	public:
-		bool operator()(const T& a, const T& b)
-		{
-			return (a < b);
-		}
-	};
-
-	template <typename L>
-	void quickSortInternal(const L& CompareFunc, int lo, int hi)
-	{
-		//  lo is the lower index, hi is the upper index
-		//  of the region of array a that is to be sorted
-		int i = lo, j = hi;
-		T x = m_data[(lo + hi) / 2];
-
-		//  partition
-		do
-		{
-			while (CompareFunc(m_data[i], x))
-				i++;
-			while (CompareFunc(x, m_data[j]))
-				j--;
-			if (i <= j)
-			{
-				swap(i, j);
-				i++;
-				j--;
-			}
-		} while (i <= j);
-
-		//  recursion
-		if (lo < j)
-			quickSortInternal(CompareFunc, lo, j);
-		if (i < hi)
-			quickSortInternal(CompareFunc, i, hi);
-	}
-
-	template <typename L>
-	void quickSort(const L& CompareFunc)
-	{
-		//don't sort 0 or 1 elements
-		if (size() > 1)
-		{
-			quickSortInternal(CompareFunc, 0, size() - 1);
-		}
-	}
-
-	///heap sort from http://www.csse.monash.edu.au/~lloyd/tildeAlgDS/Sort/Heap/
-	template <typename L>
-	void downHeap(T* pArr, int k, int n, const L& CompareFunc)
-	{
-		/*  PRE: a[k+1..N] is a heap */
-		/* POST:  a[k..N]  is a heap */
-
-		T temp = pArr[k - 1];
-		/* k has child(s) */
-		while (k <= n / 2)
-		{
-			int child = 2 * k;
-
-			if ((child < n) && CompareFunc(pArr[child - 1], pArr[child]))
-			{
-				child++;
-			}
-			/* pick larger child */
-			if (CompareFunc(temp, pArr[child - 1]))
-			{
-				/* move child up */
-				pArr[k - 1] = pArr[child - 1];
-				k = child;
-			}
-			else
-			{
-				break;
-			}
-		}
-		pArr[k - 1] = temp;
-	} /*downHeap*/
-
-	void swap(int index0, int index1)
-	{
-#ifdef B3_USE_MEMCPY
-		char temp[sizeof(T)];
-		memcpy(temp, &m_data[index0], sizeof(T));
-		memcpy(&m_data[index0], &m_data[index1], sizeof(T));
-		memcpy(&m_data[index1], temp, sizeof(T));
-#else
-		T temp = m_data[index0];
-		m_data[index0] = m_data[index1];
-		m_data[index1] = temp;
-#endif  //B3_USE_PLACEMENT_NEW
-	}
-
-	template <typename L>
-	void heapSort(const L& CompareFunc)
-	{
-		/* sort a[0..N-1],  N.B. 0 to N-1 */
-		int k;
-		int n = m_size;
-		for (k = n / 2; k > 0; k--)
-		{
-			downHeap(m_data, k, n, CompareFunc);
-		}
-
-		/* a[1..N] is now a heap */
-		while (n >= 1)
-		{
-			swap(0, n - 1); /* largest of a[0..n-1] */
-
-			n = n - 1;
-			/* restore a[1..i-1] heap */
-			downHeap(m_data, 1, n, CompareFunc);
-		}
-	}
-
-	///non-recursive binary search, assumes sorted array
-	int findBinarySearch(const T& key) const
-	{
-		int first = 0;
-		int last = size() - 1;
-
-		//assume sorted array
-		while (first <= last)
-		{
-			int mid = (first + last) / 2;  // compute mid point.
-			if (key > m_data[mid])
-				first = mid + 1;  // repeat search in top half.
-			else if (key < m_data[mid])
-				last = mid - 1;  // repeat search in bottom half.
-			else
-				return mid;  // found it. return position /////
-		}
-		return size();  // failed to find key
-	}
-
-	int findLinearSearch(const T& key) const
-	{
-		int index = size();
-		int i;
-
-		for (i = 0; i < size(); i++)
-		{
-			if (m_data[i] == key)
-			{
-				index = i;
-				break;
-			}
-		}
-		return index;
-	}
-
-	int findLinearSearch2(const T& key) const
-	{
-		int index = -1;
-		int i;
-
-		for (i = 0; i < size(); i++)
-		{
-			if (m_data[i] == key)
-			{
-				index = i;
-				break;
-			}
-		}
-		return index;
-	}
-
-	void remove(const T& key)
-	{
-		int findIndex = findLinearSearch(key);
-		if (findIndex < size())
-		{
-			swap(findIndex, size() - 1);
-			pop_back();
-		}
-	}
-
-	//PCK: whole function
-	void initializeFromBuffer(void* buffer, int size, int capacity)
-	{
-		clear();
-		m_ownsMemory = false;
-		m_data = (T*)buffer;
-		m_size = size;
-		m_capacity = capacity;
-	}
-
-	void copyFromArray(const b3AlignedObjectArray& otherArray)
-	{
-		int otherSize = otherArray.size();
-		resize(otherSize);
-		otherArray.copy(0, otherSize, m_data);
-	}
-
-	void removeAtIndex(int index)
-	{
-		if (index < size())
-		{
-			swap(index, size() - 1);
-			pop_back();
-		}
-	}
-};
-
-#endif  //B3_OBJECT_ARRAY__

thirdparty/bullet/Bullet3Common/b3CommandLineArgs.h

@@ -1,106 +0,0 @@
-#ifndef COMMAND_LINE_ARGS_H
-#define COMMAND_LINE_ARGS_H
-
-/******************************************************************************
- * Command-line parsing
- ******************************************************************************/
-#include <map>
-#include <algorithm>
-#include <string>
-#include <cstring>
-#include <sstream>
-class b3CommandLineArgs
-{
-protected:
-	std::map<std::string, std::string> pairs;
-
-public:
-	// Constructor
-	b3CommandLineArgs(int argc, char **argv)
-	{
-		addArgs(argc, argv);
-	}
-
-	void addArgs(int argc, char **argv)
-	{
-		for (int i = 1; i < argc; i++)
-		{
-			std::string arg = argv[i];
-
-			if ((arg.length() < 2) || (arg[0] != '-') || (arg[1] != '-'))
-			{
-				continue;
-			}
-
-			std::string::size_type pos;
-			std::string key, val;
-			if ((pos = arg.find('=')) == std::string::npos)
-			{
-				key = std::string(arg, 2, arg.length() - 2);
-				val = "";
-			}
-			else
-			{
-				key = std::string(arg, 2, pos - 2);
-				val = std::string(arg, pos + 1, arg.length() - 1);
-			}
-
-			//only add new keys, don't replace existing
-			if (pairs.find(key) == pairs.end())
-			{
-				pairs[key] = val;
-			}
-		}
-	}
-
-	bool CheckCmdLineFlag(const char *arg_name)
-	{
-		std::map<std::string, std::string>::iterator itr;
-		if ((itr = pairs.find(arg_name)) != pairs.end())
-		{
-			return true;
-		}
-		return false;
-	}
-
-	template <typename T>
-	bool GetCmdLineArgument(const char *arg_name, T &val);
-
-	int ParsedArgc()
-	{
-		return pairs.size();
-	}
-};
-
-template <typename T>
-inline bool b3CommandLineArgs::GetCmdLineArgument(const char *arg_name, T &val)
-{
-	std::map<std::string, std::string>::iterator itr;
-	if ((itr = pairs.find(arg_name)) != pairs.end())
-	{
-		std::istringstream strstream(itr->second);
-		strstream >> val;
-		return true;
-	}
-	return false;
-}
-
-template <>
-inline bool b3CommandLineArgs::GetCmdLineArgument<char *>(const char *arg_name, char *&val)
-{
-	std::map<std::string, std::string>::iterator itr;
-	if ((itr = pairs.find(arg_name)) != pairs.end())
-	{
-		std::string s = itr->second;
-		val = (char *)malloc(sizeof(char) * (s.length() + 1));
-		std::strcpy(val, s.c_str());
-		return true;
-	}
-	else
-	{
-		val = NULL;
-	}
-	return false;
-}
-
-#endif  //COMMAND_LINE_ARGS_H

thirdparty/bullet/Bullet3Common/b3FileUtils.h

@@ -1,133 +0,0 @@
-#ifndef B3_FILE_UTILS_H
-#define B3_FILE_UTILS_H
-
-#include <stdio.h>
-#include "b3Scalar.h"
-#include <stddef.h>  //ptrdiff_h
-#include <string.h>
-
-struct b3FileUtils
-{
-	b3FileUtils()
-	{
-	}
-	virtual ~b3FileUtils()
-	{
-	}
-
-	static bool findFile(const char* orgFileName, char* relativeFileName, int maxRelativeFileNameMaxLen)
-	{
-		FILE* f = 0;
-		f = fopen(orgFileName, "rb");
-		if (f)
-		{
-			//printf("original file found: [%s]\n", orgFileName);
-			sprintf(relativeFileName, "%s", orgFileName);
-			fclose(f);
-			return true;
-		}
-
-		//printf("Trying various directories, relative to current working directory\n");
-		const char* prefix[] = {"./", "./data/", "../data/", "../../data/", "../../../data/", "../../../../data/"};
-		int numPrefixes = sizeof(prefix) / sizeof(const char*);
-
-		f = 0;
-		bool fileFound = false;
-
-		for (int i = 0; !f && i < numPrefixes; i++)
-		{
-#ifdef _MSC_VER
-			sprintf_s(relativeFileName, maxRelativeFileNameMaxLen, "%s%s", prefix[i], orgFileName);
-#else
-			sprintf(relativeFileName, "%s%s", prefix[i], orgFileName);
-#endif
-			f = fopen(relativeFileName, "rb");
-			if (f)
-			{
-				fileFound = true;
-				break;
-			}
-		}
-		if (f)
-		{
-			fclose(f);
-		}
-
-		return fileFound;
-	}
-
-	static const char* strip2(const char* name, const char* pattern)
-	{
-		size_t const patlen = strlen(pattern);
-		size_t patcnt = 0;
-		const char* oriptr;
-		const char* patloc;
-		// find how many times the pattern occurs in the original string
-		for (oriptr = name; (patloc = strstr(oriptr, pattern)); oriptr = patloc + patlen)
-		{
-			patcnt++;
-		}
-		return oriptr;
-	}
-
-	static int extractPath(const char* fileName, char* path, int maxPathLength)
-	{
-		const char* stripped = strip2(fileName, "/");
-		stripped = strip2(stripped, "\\");
-
-		ptrdiff_t len = stripped - fileName;
-		b3Assert((len + 1) < maxPathLength);
-
-		if (len && ((len + 1) < maxPathLength))
-		{
-			for (int i = 0; i < len; i++)
-			{
-				path[i] = fileName[i];
-			}
-			path[len] = 0;
-		}
-		else
-		{
-			len = 0;
-			b3Assert(maxPathLength > 0);
-			if (maxPathLength > 0)
-			{
-				path[len] = 0;
-			}
-		}
-		return len;
-	}
-
-	static char toLowerChar(const char t)
-	{
-		if (t >= (char)'A' && t <= (char)'Z')
-			return t + ((char)'a' - (char)'A');
-		else
-			return t;
-	}
-
-	static void toLower(char* str)
-	{
-		int len = strlen(str);
-		for (int i = 0; i < len; i++)
-		{
-			str[i] = toLowerChar(str[i]);
-		}
-	}
-
-	/*static const char* strip2(const char* name, const char* pattern)
-	{
-		size_t const patlen = strlen(pattern);
-		size_t patcnt = 0;
-		const char * oriptr;
-		const char * patloc;
-		// find how many times the pattern occurs in the original string
-		for (oriptr = name; patloc = strstr(oriptr, pattern); oriptr = patloc + patlen)
-		{
-			patcnt++;
-		}
-		return oriptr;
-	}
-	*/
-};
-#endif  //B3_FILE_UTILS_H

thirdparty/bullet/Bullet3Common/b3HashMap.h

@@ -1,462 +0,0 @@
-/*
-Bullet Continuous Collision Detection and Physics Library
-Copyright (c) 2003-2013 Erwin Coumans  http://bulletphysics.org
-
-This software is provided 'as-is', without any express or implied warranty.
-In no event will the authors be held liable for any damages arising from the use of this software.
-Permission is granted to anyone to use this software for any purpose,
-including commercial applications, and to alter it and redistribute it freely,
-subject to the following restrictions:
-
-1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
-2. Altered source versions must be plainly marked as 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 B3_HASH_MAP_H
-#define B3_HASH_MAP_H
-
-#include "b3AlignedObjectArray.h"
-
-#include <string>
-
-///very basic hashable string implementation, compatible with b3HashMap
-struct b3HashString
-{
-	std::string m_string;
-	unsigned int m_hash;
-
-	B3_FORCE_INLINE unsigned int getHash() const
-	{
-		return m_hash;
-	}
-
-	b3HashString(const char* name)
-		: m_string(name)
-	{
-		/* magic numbers from http://www.isthe.com/chongo/tech/comp/fnv/ */
-		static const unsigned int InitialFNV = 2166136261u;
-		static const unsigned int FNVMultiple = 16777619u;
-
-		/* Fowler / Noll / Vo (FNV) Hash */
-		unsigned int hash = InitialFNV;
-		int len = m_string.length();
-		for (int i = 0; i < len; i++)
-		{
-			hash = hash ^ (m_string[i]); /* xor  the low 8 bits */
-			hash = hash * FNVMultiple;   /* multiply by the magic number */
-		}
-		m_hash = hash;
-	}
-
-	int portableStringCompare(const char* src, const char* dst) const
-	{
-		int ret = 0;
-
-		while (!(ret = *(unsigned char*)src - *(unsigned char*)dst) && *dst)
-			++src, ++dst;
-
-		if (ret < 0)
-			ret = -1;
-		else if (ret > 0)
-			ret = 1;
-
-		return (ret);
-	}
-
-	bool equals(const b3HashString& other) const
-	{
-		return (m_string == other.m_string);
-	}
-};
-
-const int B3_HASH_NULL = 0xffffffff;
-
-class b3HashInt
-{
-	int m_uid;
-
-public:
-	b3HashInt(int uid) : m_uid(uid)
-	{
-	}
-
-	int getUid1() const
-	{
-		return m_uid;
-	}
-
-	void setUid1(int uid)
-	{
-		m_uid = uid;
-	}
-
-	bool equals(const b3HashInt& other) const
-	{
-		return getUid1() == other.getUid1();
-	}
-	//to our success
-	B3_FORCE_INLINE unsigned int getHash() const
-	{
-		int key = m_uid;
-		// Thomas Wang's hash
-		key += ~(key << 15);
-		key ^= (key >> 10);
-		key += (key << 3);
-		key ^= (key >> 6);
-		key += ~(key << 11);
-		key ^= (key >> 16);
-		return key;
-	}
-};
-
-class b3HashPtr
-{
-	union {
-		const void* m_pointer;
-		int m_hashValues[2];
-	};
-
-public:
-	b3HashPtr(const void* ptr)
-		: m_pointer(ptr)
-	{
-	}
-
-	const void* getPointer() const
-	{
-		return m_pointer;
-	}
-
-	bool equals(const b3HashPtr& other) const
-	{
-		return getPointer() == other.getPointer();
-	}
-
-	//to our success
-	B3_FORCE_INLINE unsigned int getHash() const
-	{
-		const bool VOID_IS_8 = ((sizeof(void*) == 8));
-
-		int key = VOID_IS_8 ? m_hashValues[0] + m_hashValues[1] : m_hashValues[0];
-
-		// Thomas Wang's hash
-		key += ~(key << 15);
-		key ^= (key >> 10);
-		key += (key << 3);
-		key ^= (key >> 6);
-		key += ~(key << 11);
-		key ^= (key >> 16);
-		return key;
-	}
-};
-
-template <class Value>
-class b3HashKeyPtr
-{
-	int m_uid;
-
-public:
-	b3HashKeyPtr(int uid) : m_uid(uid)
-	{
-	}
-
-	int getUid1() const
-	{
-		return m_uid;
-	}
-
-	bool equals(const b3HashKeyPtr<Value>& other) const
-	{
-		return getUid1() == other.getUid1();
-	}
-
-	//to our success
-	B3_FORCE_INLINE unsigned int getHash() const
-	{
-		int key = m_uid;
-		// Thomas Wang's hash
-		key += ~(key << 15);
-		key ^= (key >> 10);
-		key += (key << 3);
-		key ^= (key >> 6);
-		key += ~(key << 11);
-		key ^= (key >> 16);
-		return key;
-	}
-};
-
-template <class Value>
-class b3HashKey
-{
-	int m_uid;
-
-public:
-	b3HashKey(int uid) : m_uid(uid)
-	{
-	}
-
-	int getUid1() const
-	{
-		return m_uid;
-	}
-
-	bool equals(const b3HashKey<Value>& other) const
-	{
-		return getUid1() == other.getUid1();
-	}
-	//to our success
-	B3_FORCE_INLINE unsigned int getHash() const
-	{
-		int key = m_uid;
-		// Thomas Wang's hash
-		key += ~(key << 15);
-		key ^= (key >> 10);
-		key += (key << 3);
-		key ^= (key >> 6);
-		key += ~(key << 11);
-		key ^= (key >> 16);
-		return key;
-	}
-};
-
-///The b3HashMap template class implements a generic and lightweight hashmap.
-///A basic sample of how to use b3HashMap is located in Demos\BasicDemo\main.cpp
-template <class Key, class Value>
-class b3HashMap
-{
-protected:
-	b3AlignedObjectArray<int> m_hashTable;
-	b3AlignedObjectArray<int> m_next;
-
-	b3AlignedObjectArray<Value> m_valueArray;
-	b3AlignedObjectArray<Key> m_keyArray;
-
-	void growTables(const Key& /*key*/)
-	{
-		int newCapacity = m_valueArray.capacity();
-
-		if (m_hashTable.size() < newCapacity)
-		{
-			//grow hashtable and next table
-			int curHashtableSize = m_hashTable.size();
-
-			m_hashTable.resize(newCapacity);
-			m_next.resize(newCapacity);
-
-			int i;
-
-			for (i = 0; i < newCapacity; ++i)
-			{
-				m_hashTable[i] = B3_HASH_NULL;
-			}
-			for (i = 0; i < newCapacity; ++i)
-			{
-				m_next[i] = B3_HASH_NULL;
-			}
-
-			for (i = 0; i < curHashtableSize; i++)
-			{
-				//const Value& value = m_valueArray[i];
-				//const Key& key = m_keyArray[i];
-
-				int hashValue = m_keyArray[i].getHash() & (m_valueArray.capacity() - 1);  // New hash value with new mask
-				m_next[i] = m_hashTable[hashValue];
-				m_hashTable[hashValue] = i;
-			}
-		}
-	}
-
-public:
-	void insert(const Key& key, const Value& value)
-	{
-		int hash = key.getHash() & (m_valueArray.capacity() - 1);
-
-		//replace value if the key is already there
-		int index = findIndex(key);
-		if (index != B3_HASH_NULL)
-		{
-			m_valueArray[index] = value;
-			return;
-		}
-
-		int count = m_valueArray.size();
-		int oldCapacity = m_valueArray.capacity();
-		m_valueArray.push_back(value);
-		m_keyArray.push_back(key);
-
-		int newCapacity = m_valueArray.capacity();
-		if (oldCapacity < newCapacity)
-		{
-			growTables(key);
-			//hash with new capacity
-			hash = key.getHash() & (m_valueArray.capacity() - 1);
-		}
-		m_next[count] = m_hashTable[hash];
-		m_hashTable[hash] = count;
-	}
-
-	void remove(const Key& key)
-	{
-		int hash = key.getHash() & (m_valueArray.capacity() - 1);
-
-		int pairIndex = findIndex(key);
-
-		if (pairIndex == B3_HASH_NULL)
-		{
-			return;
-		}
-
-		// Remove the pair from the hash table.
-		int index = m_hashTable[hash];
-		b3Assert(index != B3_HASH_NULL);
-
-		int previous = B3_HASH_NULL;
-		while (index != pairIndex)
-		{
-			previous = index;
-			index = m_next[index];
-		}
-
-		if (previous != B3_HASH_NULL)
-		{
-			b3Assert(m_next[previous] == pairIndex);
-			m_next[previous] = m_next[pairIndex];
-		}
-		else
-		{
-			m_hashTable[hash] = m_next[pairIndex];
-		}
-
-		// We now move the last pair into spot of the
-		// pair being removed. We need to fix the hash
-		// table indices to support the move.
-
-		int lastPairIndex = m_valueArray.size() - 1;
-
-		// If the removed pair is the last pair, we are done.
-		if (lastPairIndex == pairIndex)
-		{
-			m_valueArray.pop_back();
-			m_keyArray.pop_back();
-			return;
-		}
-
-		// Remove the last pair from the hash table.
-		int lastHash = m_keyArray[lastPairIndex].getHash() & (m_valueArray.capacity() - 1);
-
-		index = m_hashTable[lastHash];
-		b3Assert(index != B3_HASH_NULL);
-
-		previous = B3_HASH_NULL;
-		while (index != lastPairIndex)
-		{
-			previous = index;
-			index = m_next[index];
-		}
-
-		if (previous != B3_HASH_NULL)
-		{
-			b3Assert(m_next[previous] == lastPairIndex);
-			m_next[previous] = m_next[lastPairIndex];
-		}
-		else
-		{
-			m_hashTable[lastHash] = m_next[lastPairIndex];
-		}
-
-		// Copy the last pair into the remove pair's spot.
-		m_valueArray[pairIndex] = m_valueArray[lastPairIndex];
-		m_keyArray[pairIndex] = m_keyArray[lastPairIndex];
-
-		// Insert the last pair into the hash table
-		m_next[pairIndex] = m_hashTable[lastHash];
-		m_hashTable[lastHash] = pairIndex;
-
-		m_valueArray.pop_back();
-		m_keyArray.pop_back();
-	}
-
-	int size() const
-	{
-		return m_valueArray.size();
-	}
-
-	const Value* getAtIndex(int index) const
-	{
-		b3Assert(index < m_valueArray.size());
-
-		return &m_valueArray[index];
-	}
-
-	Value* getAtIndex(int index)
-	{
-		b3Assert(index < m_valueArray.size());
-
-		return &m_valueArray[index];
-	}
-
-	Key getKeyAtIndex(int index)
-	{
-		b3Assert(index < m_keyArray.size());
-		return m_keyArray[index];
-	}
-
-	const Key getKeyAtIndex(int index) const
-	{
-		b3Assert(index < m_keyArray.size());
-		return m_keyArray[index];
-	}
-
-	Value* operator[](const Key& key)
-	{
-		return find(key);
-	}
-
-	const Value* find(const Key& key) const
-	{
-		int index = findIndex(key);
-		if (index == B3_HASH_NULL)
-		{
-			return NULL;
-		}
-		return &m_valueArray[index];
-	}
-
-	Value* find(const Key& key)
-	{
-		int index = findIndex(key);
-		if (index == B3_HASH_NULL)
-		{
-			return NULL;
-		}
-		return &m_valueArray[index];
-	}
-
-	int findIndex(const Key& key) const
-	{
-		unsigned int hash = key.getHash() & (m_valueArray.capacity() - 1);
-
-		if (hash >= (unsigned int)m_hashTable.size())
-		{
-			return B3_HASH_NULL;
-		}
-
-		int index = m_hashTable[hash];
-		while ((index != B3_HASH_NULL) && key.equals(m_keyArray[index]) == false)
-		{
-			index = m_next[index];
-		}
-		return index;
-	}
-
-	void clear()
-	{
-		m_hashTable.clear();
-		m_next.clear();
-		m_valueArray.clear();
-		m_keyArray.clear();
-	}
-};
-
-#endif  //B3_HASH_MAP_H

thirdparty/bullet/Bullet3Common/b3Logging.cpp

@@ -1,145 +0,0 @@
-/*
-Copyright (c) 2013 Advanced Micro Devices, Inc.  
-
-This software is provided 'as-is', without any express or implied warranty.
-In no event will the authors be held liable for any damages arising from the use of this software.
-Permission is granted to anyone to use this software for any purpose, 
-including commercial applications, and to alter it and redistribute it freely, 
-subject to the following restrictions:
-
-1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
-2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
-3. This notice may not be removed or altered from any source distribution.
-*/
-//Originally written by Erwin Coumans
-
-#include "b3Logging.h"
-
-#include <stdio.h>
-#include <stdarg.h>
-
-#ifdef _WIN32
-#include <windows.h>
-#endif  //_WIN32
-
-void b3PrintfFuncDefault(const char* msg)
-{
-#ifdef _WIN32
-	OutputDebugStringA(msg);
-#endif
-	printf("%s", msg);
-	//is this portable?
-	fflush(stdout);
-}
-
-void b3WarningMessageFuncDefault(const char* msg)
-{
-#ifdef _WIN32
-	OutputDebugStringA(msg);
-#endif
-	printf("%s", msg);
-	//is this portable?
-	fflush(stdout);
-}
-
-void b3ErrorMessageFuncDefault(const char* msg)
-{
-#ifdef _WIN32
-	OutputDebugStringA(msg);
-#endif
-	printf("%s", msg);
-
-	//is this portable?
-	fflush(stdout);
-}
-
-static b3PrintfFunc* b3s_printfFunc = b3PrintfFuncDefault;
-static b3WarningMessageFunc* b3s_warningMessageFunc = b3WarningMessageFuncDefault;
-static b3ErrorMessageFunc* b3s_errorMessageFunc = b3ErrorMessageFuncDefault;
-
-///The developer can route b3Printf output using their own implementation
-void b3SetCustomPrintfFunc(b3PrintfFunc* printfFunc)
-{
-	b3s_printfFunc = printfFunc;
-}
-void b3SetCustomWarningMessageFunc(b3PrintfFunc* warningMessageFunc)
-{
-	b3s_warningMessageFunc = warningMessageFunc;
-}
-void b3SetCustomErrorMessageFunc(b3PrintfFunc* errorMessageFunc)
-{
-	b3s_errorMessageFunc = errorMessageFunc;
-}
-
-//#define B3_MAX_DEBUG_STRING_LENGTH 2048
-#define B3_MAX_DEBUG_STRING_LENGTH 32768
-
-void b3OutputPrintfVarArgsInternal(const char* str, ...)
-{
-	char strDebug[B3_MAX_DEBUG_STRING_LENGTH] = {0};
-	va_list argList;
-	va_start(argList, str);
-#ifdef _MSC_VER
-	vsprintf_s(strDebug, B3_MAX_DEBUG_STRING_LENGTH, str, argList);
-#else
-	vsnprintf(strDebug, B3_MAX_DEBUG_STRING_LENGTH, str, argList);
-#endif
-	(b3s_printfFunc)(strDebug);
-	va_end(argList);
-}
-void b3OutputWarningMessageVarArgsInternal(const char* str, ...)
-{
-	char strDebug[B3_MAX_DEBUG_STRING_LENGTH] = {0};
-	va_list argList;
-	va_start(argList, str);
-#ifdef _MSC_VER
-	vsprintf_s(strDebug, B3_MAX_DEBUG_STRING_LENGTH, str, argList);
-#else
-	vsnprintf(strDebug, B3_MAX_DEBUG_STRING_LENGTH, str, argList);
-#endif
-	(b3s_warningMessageFunc)(strDebug);
-	va_end(argList);
-}
-void b3OutputErrorMessageVarArgsInternal(const char* str, ...)
-{
-	char strDebug[B3_MAX_DEBUG_STRING_LENGTH] = {0};
-	va_list argList;
-	va_start(argList, str);
-#ifdef _MSC_VER
-	vsprintf_s(strDebug, B3_MAX_DEBUG_STRING_LENGTH, str, argList);
-#else
-	vsnprintf(strDebug, B3_MAX_DEBUG_STRING_LENGTH, str, argList);
-#endif
-	(b3s_errorMessageFunc)(strDebug);
-	va_end(argList);
-}
-
-void b3EnterProfileZoneDefault(const char* name)
-{
-}
-void b3LeaveProfileZoneDefault()
-{
-}
-static b3EnterProfileZoneFunc* b3s_enterFunc = b3EnterProfileZoneDefault;
-static b3LeaveProfileZoneFunc* b3s_leaveFunc = b3LeaveProfileZoneDefault;
-void b3EnterProfileZone(const char* name)
-{
-	(b3s_enterFunc)(name);
-}
-void b3LeaveProfileZone()
-{
-	(b3s_leaveFunc)();
-}
-
-void b3SetCustomEnterProfileZoneFunc(b3EnterProfileZoneFunc* enterFunc)
-{
-	b3s_enterFunc = enterFunc;
-}
-void b3SetCustomLeaveProfileZoneFunc(b3LeaveProfileZoneFunc* leaveFunc)
-{
-	b3s_leaveFunc = leaveFunc;
-}
-
-#ifndef _MSC_VER
-#undef vsprintf_s
-#endif

thirdparty/bullet/Bullet3Common/b3Logging.h

@@ -1,74 +0,0 @@
-
-#ifndef B3_LOGGING_H
-#define B3_LOGGING_H
-
-#ifdef __cplusplus
-extern "C"
-{
-#endif
-
-///We add the do/while so that the statement "if (condition) b3Printf("test"); else {...}" would fail
-///You can also customize the message by uncommenting out a different line below
-#define b3Printf(...) b3OutputPrintfVarArgsInternal(__VA_ARGS__)
-	//#define b3Printf(...) do {b3OutputPrintfVarArgsInternal("b3Printf[%s,%d]:",__FILE__,__LINE__);b3OutputPrintfVarArgsInternal(__VA_ARGS__); } while(0)
-	//#define b3Printf b3OutputPrintfVarArgsInternal
-	//#define b3Printf(...) printf(__VA_ARGS__)
-	//#define b3Printf(...)
-#define b3Warning(...) do{	b3OutputWarningMessageVarArgsInternal("b3Warning[%s,%d]:\n", __FILE__, __LINE__);b3OutputWarningMessageVarArgsInternal(__VA_ARGS__);} while (0)
-#define b3Error(...)do	{b3OutputErrorMessageVarArgsInternal("b3Error[%s,%d]:\n", __FILE__, __LINE__);b3OutputErrorMessageVarArgsInternal(__VA_ARGS__);} while (0)
-#ifndef B3_NO_PROFILE
-
-	void b3EnterProfileZone(const char* name);
-	void b3LeaveProfileZone();
-#ifdef __cplusplus
-
-	class b3ProfileZone
-	{
-	public:
-		b3ProfileZone(const char* name)
-		{
-			b3EnterProfileZone(name);
-		}
-
-		~b3ProfileZone()
-		{
-			b3LeaveProfileZone();
-		}
-	};
-
-#define B3_PROFILE(name) b3ProfileZone __profile(name)
-#endif
-
-#else  //B3_NO_PROFILE
-
-#define B3_PROFILE(name)
-#define b3StartProfile(a)
-#define b3StopProfile
-
-#endif  //#ifndef B3_NO_PROFILE
-
-	typedef void(b3PrintfFunc)(const char* msg);
-	typedef void(b3WarningMessageFunc)(const char* msg);
-	typedef void(b3ErrorMessageFunc)(const char* msg);
-	typedef void(b3EnterProfileZoneFunc)(const char* msg);
-	typedef void(b3LeaveProfileZoneFunc)();
-
-	///The developer can route b3Printf output using their own implementation
-	void b3SetCustomPrintfFunc(b3PrintfFunc* printfFunc);
-	void b3SetCustomWarningMessageFunc(b3WarningMessageFunc* warningMsgFunc);
-	void b3SetCustomErrorMessageFunc(b3ErrorMessageFunc* errorMsgFunc);
-
-	///Set custom profile zone functions (zones can be nested)
-	void b3SetCustomEnterProfileZoneFunc(b3EnterProfileZoneFunc* enterFunc);
-	void b3SetCustomLeaveProfileZoneFunc(b3LeaveProfileZoneFunc* leaveFunc);
-
-	///Don't use those internal functions directly, use the b3Printf or b3SetCustomPrintfFunc instead (or warning/error version)
-	void b3OutputPrintfVarArgsInternal(const char* str, ...);
-	void b3OutputWarningMessageVarArgsInternal(const char* str, ...);
-	void b3OutputErrorMessageVarArgsInternal(const char* str, ...);
-
-#ifdef __cplusplus
-}
-#endif
-
-#endif  //B3_LOGGING_H

thirdparty/bullet/Bullet3Common/b3Matrix3x3.h

@@ -1,1354 +0,0 @@
-/*
-Copyright (c) 2003-2013 Gino van den Bergen / Erwin Coumans  http://bulletphysics.org
-
-This software is provided 'as-is', without any express or implied warranty.
-In no event will the authors be held liable for any damages arising from the use of this software.
-Permission is granted to anyone to use this software for any purpose, 
-including commercial applications, and to alter it and redistribute it freely, 
-subject to the following restrictions:
-
-1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
-2. Altered source versions must be plainly marked as 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 B3_MATRIX3x3_H
-#define B3_MATRIX3x3_H
-
-#include "b3Vector3.h"
-#include "b3Quaternion.h"
-#include <stdio.h>
-
-#ifdef B3_USE_SSE
-//const __m128 B3_ATTRIBUTE_ALIGNED16(b3v2220) = {2.0f, 2.0f, 2.0f, 0.0f};
-const __m128 B3_ATTRIBUTE_ALIGNED16(b3vMPPP) = {-0.0f, +0.0f, +0.0f, +0.0f};
-#endif
-
-#if defined(B3_USE_SSE) || defined(B3_USE_NEON)
-const b3SimdFloat4 B3_ATTRIBUTE_ALIGNED16(b3v1000) = {1.0f, 0.0f, 0.0f, 0.0f};
-const b3SimdFloat4 B3_ATTRIBUTE_ALIGNED16(b3v0100) = {0.0f, 1.0f, 0.0f, 0.0f};
-const b3SimdFloat4 B3_ATTRIBUTE_ALIGNED16(b3v0010) = {0.0f, 0.0f, 1.0f, 0.0f};
-#endif
-
-#ifdef B3_USE_DOUBLE_PRECISION
-#define b3Matrix3x3Data b3Matrix3x3DoubleData
-#else
-#define b3Matrix3x3Data b3Matrix3x3FloatData
-#endif  //B3_USE_DOUBLE_PRECISION
-
-/**@brief The b3Matrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with b3Quaternion, b3Transform and b3Vector3.
-* Make sure to only include a pure orthogonal matrix without scaling. */
-B3_ATTRIBUTE_ALIGNED16(class)
-b3Matrix3x3
-{
-	///Data storage for the matrix, each vector is a row of the matrix
-	b3Vector3 m_el[3];
-
-public:
-	/** @brief No initializaion constructor */
-	b3Matrix3x3() {}
-
-	//		explicit b3Matrix3x3(const b3Scalar *m) { setFromOpenGLSubMatrix(m); }
-
-	/**@brief Constructor from Quaternion */
-	explicit b3Matrix3x3(const b3Quaternion& q) { setRotation(q); }
-	/*
-	template <typename b3Scalar>
-	Matrix3x3(const b3Scalar& yaw, const b3Scalar& pitch, const b3Scalar& roll)
-	{ 
-	setEulerYPR(yaw, pitch, roll);
-	}
-	*/
-	/** @brief Constructor with row major formatting */
-	b3Matrix3x3(const b3Scalar& xx, const b3Scalar& xy, const b3Scalar& xz,
-				const b3Scalar& yx, const b3Scalar& yy, const b3Scalar& yz,
-				const b3Scalar& zx, const b3Scalar& zy, const b3Scalar& zz)
-	{
-		setValue(xx, xy, xz,
-				 yx, yy, yz,
-				 zx, zy, zz);
-	}
-
-#if (defined(B3_USE_SSE_IN_API) && defined(B3_USE_SSE)) || defined(B3_USE_NEON)
-	B3_FORCE_INLINE b3Matrix3x3(const b3SimdFloat4 v0, const b3SimdFloat4 v1, const b3SimdFloat4 v2)
-	{
-		m_el[0].mVec128 = v0;
-		m_el[1].mVec128 = v1;
-		m_el[2].mVec128 = v2;
-	}
-
-	B3_FORCE_INLINE b3Matrix3x3(const b3Vector3& v0, const b3Vector3& v1, const b3Vector3& v2)
-	{
-		m_el[0] = v0;
-		m_el[1] = v1;
-		m_el[2] = v2;
-	}
-
-	// Copy constructor
-	B3_FORCE_INLINE b3Matrix3x3(const b3Matrix3x3& rhs)
-	{
-		m_el[0].mVec128 = rhs.m_el[0].mVec128;
-		m_el[1].mVec128 = rhs.m_el[1].mVec128;
-		m_el[2].mVec128 = rhs.m_el[2].mVec128;
-	}
-
-	// Assignment Operator
-	B3_FORCE_INLINE b3Matrix3x3& operator=(const b3Matrix3x3& m)
-	{
-		m_el[0].mVec128 = m.m_el[0].mVec128;
-		m_el[1].mVec128 = m.m_el[1].mVec128;
-		m_el[2].mVec128 = m.m_el[2].mVec128;
-
-		return *this;
-	}
-
-#else
-
-	/** @brief Copy constructor */
-	B3_FORCE_INLINE b3Matrix3x3(const b3Matrix3x3& other)
-	{
-		m_el[0] = other.m_el[0];
-		m_el[1] = other.m_el[1];
-		m_el[2] = other.m_el[2];
-	}
-
-	/** @brief Assignment Operator */
-	B3_FORCE_INLINE b3Matrix3x3& operator=(const b3Matrix3x3& other)
-	{
-		m_el[0] = other.m_el[0];
-		m_el[1] = other.m_el[1];
-		m_el[2] = other.m_el[2];
-		return *this;
-	}
-
-#endif
-
-	/** @brief Get a column of the matrix as a vector 
-	*  @param i Column number 0 indexed */
-	B3_FORCE_INLINE b3Vector3 getColumn(int i) const
-	{
-		return b3MakeVector3(m_el[0][i], m_el[1][i], m_el[2][i]);
-	}
-
-	/** @brief Get a row of the matrix as a vector 
-	*  @param i Row number 0 indexed */
-	B3_FORCE_INLINE const b3Vector3& getRow(int i) const
-	{
-		b3FullAssert(0 <= i && i < 3);
-		return m_el[i];
-	}
-
-	/** @brief Get a mutable reference to a row of the matrix as a vector 
-	*  @param i Row number 0 indexed */
-	B3_FORCE_INLINE b3Vector3& operator[](int i)
-	{
-		b3FullAssert(0 <= i && i < 3);
-		return m_el[i];
-	}
-
-	/** @brief Get a const reference to a row of the matrix as a vector 
-	*  @param i Row number 0 indexed */
-	B3_FORCE_INLINE const b3Vector3& operator[](int i) const
-	{
-		b3FullAssert(0 <= i && i < 3);
-		return m_el[i];
-	}
-
-	/** @brief Multiply by the target matrix on the right
-	*  @param m Rotation matrix to be applied 
-	* Equivilant to this = this * m */
-	b3Matrix3x3& operator*=(const b3Matrix3x3& m);
-
-	/** @brief Adds by the target matrix on the right
-	*  @param m matrix to be applied 
-	* Equivilant to this = this + m */
-	b3Matrix3x3& operator+=(const b3Matrix3x3& m);
-
-	/** @brief Substractss by the target matrix on the right
-	*  @param m matrix to be applied 
-	* Equivilant to this = this - m */
-	b3Matrix3x3& operator-=(const b3Matrix3x3& m);
-
-	/** @brief Set from the rotational part of a 4x4 OpenGL matrix
-	*  @param m A pointer to the beginning of the array of scalars*/
-	void setFromOpenGLSubMatrix(const b3Scalar* m)
-	{
-		m_el[0].setValue(m[0], m[4], m[8]);
-		m_el[1].setValue(m[1], m[5], m[9]);
-		m_el[2].setValue(m[2], m[6], m[10]);
-	}
-	/** @brief Set the values of the matrix explicitly (row major)
-	*  @param xx Top left
-	*  @param xy Top Middle
-	*  @param xz Top Right
-	*  @param yx Middle Left
-	*  @param yy Middle Middle
-	*  @param yz Middle Right
-	*  @param zx Bottom Left
-	*  @param zy Bottom Middle
-	*  @param zz Bottom Right*/
-	void setValue(const b3Scalar& xx, const b3Scalar& xy, const b3Scalar& xz,
-				  const b3Scalar& yx, const b3Scalar& yy, const b3Scalar& yz,
-				  const b3Scalar& zx, const b3Scalar& zy, const b3Scalar& zz)
-	{
-		m_el[0].setValue(xx, xy, xz);
-		m_el[1].setValue(yx, yy, yz);
-		m_el[2].setValue(zx, zy, zz);
-	}
-
-	/** @brief Set the matrix from a quaternion
-	*  @param q The Quaternion to match */
-	void setRotation(const b3Quaternion& q)
-	{
-		b3Scalar d = q.length2();
-		b3FullAssert(d != b3Scalar(0.0));
-		b3Scalar s = b3Scalar(2.0) / d;
-
-#if defined(B3_USE_SSE_IN_API) && defined(B3_USE_SSE)
-		__m128 vs, Q = q.get128();
-		__m128i Qi = b3CastfTo128i(Q);
-		__m128 Y, Z;
-		__m128 V1, V2, V3;
-		__m128 V11, V21, V31;
-		__m128 NQ = _mm_xor_ps(Q, b3vMzeroMask);
-		__m128i NQi = b3CastfTo128i(NQ);
-
-		V1 = b3CastiTo128f(_mm_shuffle_epi32(Qi, B3_SHUFFLE(1, 0, 2, 3)));  // Y X Z W
-		V2 = _mm_shuffle_ps(NQ, Q, B3_SHUFFLE(0, 0, 1, 3));                 // -X -X  Y  W
-		V3 = b3CastiTo128f(_mm_shuffle_epi32(Qi, B3_SHUFFLE(2, 1, 0, 3)));  // Z Y X W
-		V1 = _mm_xor_ps(V1, b3vMPPP);                                       //	change the sign of the first element
-
-		V11 = b3CastiTo128f(_mm_shuffle_epi32(Qi, B3_SHUFFLE(1, 1, 0, 3)));  // Y Y X W
-		V21 = _mm_unpackhi_ps(Q, Q);                                         //  Z  Z  W  W
-		V31 = _mm_shuffle_ps(Q, NQ, B3_SHUFFLE(0, 2, 0, 3));                 //  X  Z -X -W
-
-		V2 = V2 * V1;   //
-		V1 = V1 * V11;  //
-		V3 = V3 * V31;  //
-
-		V11 = _mm_shuffle_ps(NQ, Q, B3_SHUFFLE(2, 3, 1, 3));                //	-Z -W  Y  W
-		V11 = V11 * V21;                                                    //
-		V21 = _mm_xor_ps(V21, b3vMPPP);                                     //	change the sign of the first element
-		V31 = _mm_shuffle_ps(Q, NQ, B3_SHUFFLE(3, 3, 1, 3));                //	 W  W -Y -W
-		V31 = _mm_xor_ps(V31, b3vMPPP);                                     //	change the sign of the first element
-		Y = b3CastiTo128f(_mm_shuffle_epi32(NQi, B3_SHUFFLE(3, 2, 0, 3)));  // -W -Z -X -W
-		Z = b3CastiTo128f(_mm_shuffle_epi32(Qi, B3_SHUFFLE(1, 0, 1, 3)));   //  Y  X  Y  W
-
-		vs = _mm_load_ss(&s);
-		V21 = V21 * Y;
-		V31 = V31 * Z;
-
-		V1 = V1 + V11;
-		V2 = V2 + V21;
-		V3 = V3 + V31;
-
-		vs = b3_splat3_ps(vs, 0);
-		//	s ready
-		V1 = V1 * vs;
-		V2 = V2 * vs;
-		V3 = V3 * vs;
-
-		V1 = V1 + b3v1000;
-		V2 = V2 + b3v0100;
-		V3 = V3 + b3v0010;
-
-		m_el[0] = b3MakeVector3(V1);
-		m_el[1] = b3MakeVector3(V2);
-		m_el[2] = b3MakeVector3(V3);
-#else
-		b3Scalar xs = q.getX() * s, ys = q.getY() * s, zs = q.getZ() * s;
-		b3Scalar wx = q.getW() * xs, wy = q.getW() * ys, wz = q.getW() * zs;
-		b3Scalar xx = q.getX() * xs, xy = q.getX() * ys, xz = q.getX() * zs;
-		b3Scalar yy = q.getY() * ys, yz = q.getY() * zs, zz = q.getZ() * zs;
-		setValue(
-			b3Scalar(1.0) - (yy + zz), xy - wz, xz + wy,
-			xy + wz, b3Scalar(1.0) - (xx + zz), yz - wx,
-			xz - wy, yz + wx, b3Scalar(1.0) - (xx + yy));
-#endif
-	}
-
-	/** @brief Set the matrix from euler angles using YPR around YXZ respectively
-	*  @param yaw Yaw about Y axis
-	*  @param pitch Pitch about X axis
-	*  @param roll Roll about Z axis 
-	*/
-	void setEulerYPR(const b3Scalar& yaw, const b3Scalar& pitch, const b3Scalar& roll)
-	{
-		setEulerZYX(roll, pitch, yaw);
-	}
-
-	/** @brief Set the matrix from euler angles YPR around ZYX axes
-	* @param eulerX Roll about X axis
-	* @param eulerY Pitch around Y axis
-	* @param eulerZ Yaw aboud Z axis
-	* 
-	* These angles are used to produce a rotation matrix. The euler
-	* angles are applied in ZYX order. I.e a vector is first rotated 
-	* about X then Y and then Z
-	**/
-	void setEulerZYX(b3Scalar eulerX, b3Scalar eulerY, b3Scalar eulerZ)
-	{
-		///@todo proposed to reverse this since it's labeled zyx but takes arguments xyz and it will match all other parts of the code
-		b3Scalar ci(b3Cos(eulerX));
-		b3Scalar cj(b3Cos(eulerY));
-		b3Scalar ch(b3Cos(eulerZ));
-		b3Scalar si(b3Sin(eulerX));
-		b3Scalar sj(b3Sin(eulerY));
-		b3Scalar sh(b3Sin(eulerZ));
-		b3Scalar cc = ci * ch;
-		b3Scalar cs = ci * sh;
-		b3Scalar sc = si * ch;
-		b3Scalar ss = si * sh;
-
-		setValue(cj * ch, sj * sc - cs, sj * cc + ss,
-				 cj * sh, sj * ss + cc, sj * cs - sc,
-				 -sj, cj * si, cj * ci);
-	}
-
-	/**@brief Set the matrix to the identity */
-	void setIdentity()
-	{
-#if (defined(B3_USE_SSE_IN_API) && defined(B3_USE_SSE)) || defined(B3_USE_NEON)
-		m_el[0] = b3MakeVector3(b3v1000);
-		m_el[1] = b3MakeVector3(b3v0100);
-		m_el[2] = b3MakeVector3(b3v0010);
-#else
-		setValue(b3Scalar(1.0), b3Scalar(0.0), b3Scalar(0.0),
-				 b3Scalar(0.0), b3Scalar(1.0), b3Scalar(0.0),
-				 b3Scalar(0.0), b3Scalar(0.0), b3Scalar(1.0));
-#endif
-	}
-
-	static const b3Matrix3x3& getIdentity()
-	{
-#if (defined(B3_USE_SSE_IN_API) && defined(B3_USE_SSE)) || defined(B3_USE_NEON)
-		static const b3Matrix3x3
-			identityMatrix(b3v1000, b3v0100, b3v0010);
-#else
-		static const b3Matrix3x3
-			identityMatrix(
-				b3Scalar(1.0), b3Scalar(0.0), b3Scalar(0.0),
-				b3Scalar(0.0), b3Scalar(1.0), b3Scalar(0.0),
-				b3Scalar(0.0), b3Scalar(0.0), b3Scalar(1.0));
-#endif
-		return identityMatrix;
-	}
-
-	/**@brief Fill the rotational part of an OpenGL matrix and clear the shear/perspective
-	* @param m The array to be filled */
-	void getOpenGLSubMatrix(b3Scalar * m) const
-	{
-#if defined(B3_USE_SSE_IN_API) && defined(B3_USE_SSE)
-		__m128 v0 = m_el[0].mVec128;
-		__m128 v1 = m_el[1].mVec128;
-		__m128 v2 = m_el[2].mVec128;  //  x2 y2 z2 w2
-		__m128* vm = (__m128*)m;
-		__m128 vT;
-
-		v2 = _mm_and_ps(v2, b3vFFF0fMask);  //  x2 y2 z2 0
-
-		vT = _mm_unpackhi_ps(v0, v1);  //	z0 z1 * *
-		v0 = _mm_unpacklo_ps(v0, v1);  //	x0 x1 y0 y1
-
-		v1 = _mm_shuffle_ps(v0, v2, B3_SHUFFLE(2, 3, 1, 3));                    // y0 y1 y2 0
-		v0 = _mm_shuffle_ps(v0, v2, B3_SHUFFLE(0, 1, 0, 3));                    // x0 x1 x2 0
-		v2 = b3CastdTo128f(_mm_move_sd(b3CastfTo128d(v2), b3CastfTo128d(vT)));  // z0 z1 z2 0
-
-		vm[0] = v0;
-		vm[1] = v1;
-		vm[2] = v2;
-#elif defined(B3_USE_NEON)
-		// note: zeros the w channel. We can preserve it at the cost of two more vtrn instructions.
-		static const uint32x2_t zMask = (const uint32x2_t){-1, 0};
-		float32x4_t* vm = (float32x4_t*)m;
-		float32x4x2_t top = vtrnq_f32(m_el[0].mVec128, m_el[1].mVec128);               // {x0 x1 z0 z1}, {y0 y1 w0 w1}
-		float32x2x2_t bl = vtrn_f32(vget_low_f32(m_el[2].mVec128), vdup_n_f32(0.0f));  // {x2  0 }, {y2 0}
-		float32x4_t v0 = vcombine_f32(vget_low_f32(top.val[0]), bl.val[0]);
-		float32x4_t v1 = vcombine_f32(vget_low_f32(top.val[1]), bl.val[1]);
-		float32x2_t q = (float32x2_t)vand_u32((uint32x2_t)vget_high_f32(m_el[2].mVec128), zMask);
-		float32x4_t v2 = vcombine_f32(vget_high_f32(top.val[0]), q);  // z0 z1 z2  0
-
-		vm[0] = v0;
-		vm[1] = v1;
-		vm[2] = v2;
-#else
-		m[0] = b3Scalar(m_el[0].getX());
-		m[1] = b3Scalar(m_el[1].getX());
-		m[2] = b3Scalar(m_el[2].getX());
-		m[3] = b3Scalar(0.0);
-		m[4] = b3Scalar(m_el[0].getY());
-		m[5] = b3Scalar(m_el[1].getY());
-		m[6] = b3Scalar(m_el[2].getY());
-		m[7] = b3Scalar(0.0);
-		m[8] = b3Scalar(m_el[0].getZ());
-		m[9] = b3Scalar(m_el[1].getZ());
-		m[10] = b3Scalar(m_el[2].getZ());
-		m[11] = b3Scalar(0.0);
-#endif
-	}
-
-	/**@brief Get the matrix represented as a quaternion 
-	* @param q The quaternion which will be set */
-	void getRotation(b3Quaternion & q) const
-	{
-#if (defined(B3_USE_SSE_IN_API) && defined(B3_USE_SSE)) || defined(B3_USE_NEON)
-		b3Scalar trace = m_el[0].getX() + m_el[1].getY() + m_el[2].getZ();
-		b3Scalar s, x;
-
-		union {
-			b3SimdFloat4 vec;
-			b3Scalar f[4];
-		} temp;
-
-		if (trace > b3Scalar(0.0))
-		{
-			x = trace + b3Scalar(1.0);
-
-			temp.f[0] = m_el[2].getY() - m_el[1].getZ();
-			temp.f[1] = m_el[0].getZ() - m_el[2].getX();
-			temp.f[2] = m_el[1].getX() - m_el[0].getY();
-			temp.f[3] = x;
-			//temp.f[3]= s * b3Scalar(0.5);
-		}
-		else
-		{
-			int i, j, k;
-			if (m_el[0].getX() < m_el[1].getY())
-			{
-				if (m_el[1].getY() < m_el[2].getZ())
-				{
-					i = 2;
-					j = 0;
-					k = 1;
-				}
-				else
-				{
-					i = 1;
-					j = 2;
-					k = 0;
-				}
-			}
-			else
-			{
-				if (m_el[0].getX() < m_el[2].getZ())
-				{
-					i = 2;
-					j = 0;
-					k = 1;
-				}
-				else
-				{
-					i = 0;
-					j = 1;
-					k = 2;
-				}
-			}
-
-			x = m_el[i][i] - m_el[j][j] - m_el[k][k] + b3Scalar(1.0);
-
-			temp.f[3] = (m_el[k][j] - m_el[j][k]);
-			temp.f[j] = (m_el[j][i] + m_el[i][j]);
-			temp.f[k] = (m_el[k][i] + m_el[i][k]);
-			temp.f[i] = x;
-			//temp.f[i] = s * b3Scalar(0.5);
-		}
-
-		s = b3Sqrt(x);
-		q.set128(temp.vec);
-		s = b3Scalar(0.5) / s;
-
-		q *= s;
-#else
-		b3Scalar trace = m_el[0].getX() + m_el[1].getY() + m_el[2].getZ();
-
-		b3Scalar temp[4];
-
-		if (trace > b3Scalar(0.0))
-		{
-			b3Scalar s = b3Sqrt(trace + b3Scalar(1.0));
-			temp[3] = (s * b3Scalar(0.5));
-			s = b3Scalar(0.5) / s;
-
-			temp[0] = ((m_el[2].getY() - m_el[1].getZ()) * s);
-			temp[1] = ((m_el[0].getZ() - m_el[2].getX()) * s);
-			temp[2] = ((m_el[1].getX() - m_el[0].getY()) * s);
-		}
-		else
-		{
-			int i = m_el[0].getX() < m_el[1].getY() ? (m_el[1].getY() < m_el[2].getZ() ? 2 : 1) : (m_el[0].getX() < m_el[2].getZ() ? 2 : 0);
-			int j = (i + 1) % 3;
-			int k = (i + 2) % 3;
-
-			b3Scalar s = b3Sqrt(m_el[i][i] - m_el[j][j] - m_el[k][k] + b3Scalar(1.0));
-			temp[i] = s * b3Scalar(0.5);
-			s = b3Scalar(0.5) / s;
-
-			temp[3] = (m_el[k][j] - m_el[j][k]) * s;
-			temp[j] = (m_el[j][i] + m_el[i][j]) * s;
-			temp[k] = (m_el[k][i] + m_el[i][k]) * s;
-		}
-		q.setValue(temp[0], temp[1], temp[2], temp[3]);
-#endif
-	}
-
-	/**@brief Get the matrix represented as euler angles around YXZ, roundtrip with setEulerYPR
-	* @param yaw Yaw around Y axis
-	* @param pitch Pitch around X axis
-	* @param roll around Z axis */
-	void getEulerYPR(b3Scalar & yaw, b3Scalar & pitch, b3Scalar & roll) const
-	{
-		// first use the normal calculus
-		yaw = b3Scalar(b3Atan2(m_el[1].getX(), m_el[0].getX()));
-		pitch = b3Scalar(b3Asin(-m_el[2].getX()));
-		roll = b3Scalar(b3Atan2(m_el[2].getY(), m_el[2].getZ()));
-
-		// on pitch = +/-HalfPI
-		if (b3Fabs(pitch) == B3_HALF_PI)
-		{
-			if (yaw > 0)
-				yaw -= B3_PI;
-			else
-				yaw += B3_PI;
-
-			if (roll > 0)
-				roll -= B3_PI;
-			else
-				roll += B3_PI;
-		}
-	};
-
-	/**@brief Get the matrix represented as euler angles around ZYX
-	* @param yaw Yaw around X axis
-	* @param pitch Pitch around Y axis
-	* @param roll around X axis 
-	* @param solution_number Which solution of two possible solutions ( 1 or 2) are possible values*/
-	void getEulerZYX(b3Scalar & yaw, b3Scalar & pitch, b3Scalar & roll, unsigned int solution_number = 1) const
-	{
-		struct Euler
-		{
-			b3Scalar yaw;
-			b3Scalar pitch;
-			b3Scalar roll;
-		};
-
-		Euler euler_out;
-		Euler euler_out2;  //second solution
-		//get the pointer to the raw data
-
-		// Check that pitch is not at a singularity
-		if (b3Fabs(m_el[2].getX()) >= 1)
-		{
-			euler_out.yaw = 0;
-			euler_out2.yaw = 0;
-
-			// From difference of angles formula
-			b3Scalar delta = b3Atan2(m_el[0].getX(), m_el[0].getZ());
-			if (m_el[2].getX() > 0)  //gimbal locked up
-			{
-				euler_out.pitch = B3_PI / b3Scalar(2.0);
-				euler_out2.pitch = B3_PI / b3Scalar(2.0);
-				euler_out.roll = euler_out.pitch + delta;
-				euler_out2.roll = euler_out.pitch + delta;
-			}
-			else  // gimbal locked down
-			{
-				euler_out.pitch = -B3_PI / b3Scalar(2.0);
-				euler_out2.pitch = -B3_PI / b3Scalar(2.0);
-				euler_out.roll = -euler_out.pitch + delta;
-				euler_out2.roll = -euler_out.pitch + delta;
-			}
-		}
-		else
-		{
-			euler_out.pitch = -b3Asin(m_el[2].getX());
-			euler_out2.pitch = B3_PI - euler_out.pitch;
-
-			euler_out.roll = b3Atan2(m_el[2].getY() / b3Cos(euler_out.pitch),
-									 m_el[2].getZ() / b3Cos(euler_out.pitch));
-			euler_out2.roll = b3Atan2(m_el[2].getY() / b3Cos(euler_out2.pitch),
-									  m_el[2].getZ() / b3Cos(euler_out2.pitch));
-
-			euler_out.yaw = b3Atan2(m_el[1].getX() / b3Cos(euler_out.pitch),
-									m_el[0].getX() / b3Cos(euler_out.pitch));
-			euler_out2.yaw = b3Atan2(m_el[1].getX() / b3Cos(euler_out2.pitch),
-									 m_el[0].getX() / b3Cos(euler_out2.pitch));
-		}
-
-		if (solution_number == 1)
-		{
-			yaw = euler_out.yaw;
-			pitch = euler_out.pitch;
-			roll = euler_out.roll;
-		}
-		else
-		{
-			yaw = euler_out2.yaw;
-			pitch = euler_out2.pitch;
-			roll = euler_out2.roll;
-		}
-	}
-
-	/**@brief Create a scaled copy of the matrix 
-	* @param s Scaling vector The elements of the vector will scale each column */
-
-	b3Matrix3x3 scaled(const b3Vector3& s) const
-	{
-#if (defined(B3_USE_SSE_IN_API) && defined(B3_USE_SSE)) || defined(B3_USE_NEON)
-		return b3Matrix3x3(m_el[0] * s, m_el[1] * s, m_el[2] * s);
-#else
-		return b3Matrix3x3(
-			m_el[0].getX() * s.getX(), m_el[0].getY() * s.getY(), m_el[0].getZ() * s.getZ(),
-			m_el[1].getX() * s.getX(), m_el[1].getY() * s.getY(), m_el[1].getZ() * s.getZ(),
-			m_el[2].getX() * s.getX(), m_el[2].getY() * s.getY(), m_el[2].getZ() * s.getZ());
-#endif
-	}
-
-	/**@brief Return the determinant of the matrix */
-	b3Scalar determinant() const;
-	/**@brief Return the adjoint of the matrix */
-	b3Matrix3x3 adjoint() const;
-	/**@brief Return the matrix with all values non negative */
-	b3Matrix3x3 absolute() const;
-	/**@brief Return the transpose of the matrix */
-	b3Matrix3x3 transpose() const;
-	/**@brief Return the inverse of the matrix */
-	b3Matrix3x3 inverse() const;
-
-	b3Matrix3x3 transposeTimes(const b3Matrix3x3& m) const;
-	b3Matrix3x3 timesTranspose(const b3Matrix3x3& m) const;
-
-	B3_FORCE_INLINE b3Scalar tdotx(const b3Vector3& v) const
-	{
-		return m_el[0].getX() * v.getX() + m_el[1].getX() * v.getY() + m_el[2].getX() * v.getZ();
-	}
-	B3_FORCE_INLINE b3Scalar tdoty(const b3Vector3& v) const
-	{
-		return m_el[0].getY() * v.getX() + m_el[1].getY() * v.getY() + m_el[2].getY() * v.getZ();
-	}
-	B3_FORCE_INLINE b3Scalar tdotz(const b3Vector3& v) const
-	{
-		return m_el[0].getZ() * v.getX() + m_el[1].getZ() * v.getY() + m_el[2].getZ() * v.getZ();
-	}
-
-	/**@brief diagonalizes this matrix by the Jacobi method.
-	* @param rot stores the rotation from the coordinate system in which the matrix is diagonal to the original
-	* coordinate system, i.e., old_this = rot * new_this * rot^T. 
-	* @param threshold See iteration
-	* @param iteration The iteration stops when all off-diagonal elements are less than the threshold multiplied 
-	* by the sum of the absolute values of the diagonal, or when maxSteps have been executed. 
-	* 
-	* Note that this matrix is assumed to be symmetric. 
-	*/
-	void diagonalize(b3Matrix3x3 & rot, b3Scalar threshold, int maxSteps)
-	{
-		rot.setIdentity();
-		for (int step = maxSteps; step > 0; step--)
-		{
-			// find off-diagonal element [p][q] with largest magnitude
-			int p = 0;
-			int q = 1;
-			int r = 2;
-			b3Scalar max = b3Fabs(m_el[0][1]);
-			b3Scalar v = b3Fabs(m_el[0][2]);
-			if (v > max)
-			{
-				q = 2;
-				r = 1;
-				max = v;
-			}
-			v = b3Fabs(m_el[1][2]);
-			if (v > max)
-			{
-				p = 1;
-				q = 2;
-				r = 0;
-				max = v;
-			}
-
-			b3Scalar t = threshold * (b3Fabs(m_el[0][0]) + b3Fabs(m_el[1][1]) + b3Fabs(m_el[2][2]));
-			if (max <= t)
-			{
-				if (max <= B3_EPSILON * t)
-				{
-					return;
-				}
-				step = 1;
-			}
-
-			// compute Jacobi rotation J which leads to a zero for element [p][q]
-			b3Scalar mpq = m_el[p][q];
-			b3Scalar theta = (m_el[q][q] - m_el[p][p]) / (2 * mpq);
-			b3Scalar theta2 = theta * theta;
-			b3Scalar cos;
-			b3Scalar sin;
-			if (theta2 * theta2 < b3Scalar(10 / B3_EPSILON))
-			{
-				t = (theta >= 0) ? 1 / (theta + b3Sqrt(1 + theta2))
-								 : 1 / (theta - b3Sqrt(1 + theta2));
-				cos = 1 / b3Sqrt(1 + t * t);
-				sin = cos * t;
-			}
-			else
-			{
-				// approximation for large theta-value, i.e., a nearly diagonal matrix
-				t = 1 / (theta * (2 + b3Scalar(0.5) / theta2));
-				cos = 1 - b3Scalar(0.5) * t * t;
-				sin = cos * t;
-			}
-
-			// apply rotation to matrix (this = J^T * this * J)
-			m_el[p][q] = m_el[q][p] = 0;
-			m_el[p][p] -= t * mpq;
-			m_el[q][q] += t * mpq;
-			b3Scalar mrp = m_el[r][p];
-			b3Scalar mrq = m_el[r][q];
-			m_el[r][p] = m_el[p][r] = cos * mrp - sin * mrq;
-			m_el[r][q] = m_el[q][r] = cos * mrq + sin * mrp;
-
-			// apply rotation to rot (rot = rot * J)
-			for (int i = 0; i < 3; i++)
-			{
-				b3Vector3& row = rot[i];
-				mrp = row[p];
-				mrq = row[q];
-				row[p] = cos * mrp - sin * mrq;
-				row[q] = cos * mrq + sin * mrp;
-			}
-		}
-	}
-
-	/**@brief Calculate the matrix cofactor 
-	* @param r1 The first row to use for calculating the cofactor
-	* @param c1 The first column to use for calculating the cofactor
-	* @param r1 The second row to use for calculating the cofactor
-	* @param c1 The second column to use for calculating the cofactor
-	* See http://en.wikipedia.org/wiki/Cofactor_(linear_algebra) for more details
-	*/
-	b3Scalar cofac(int r1, int c1, int r2, int c2) const
-	{
-		return m_el[r1][c1] * m_el[r2][c2] - m_el[r1][c2] * m_el[r2][c1];
-	}
-
-	void serialize(struct b3Matrix3x3Data & dataOut) const;
-
-	void serializeFloat(struct b3Matrix3x3FloatData & dataOut) const;
-
-	void deSerialize(const struct b3Matrix3x3Data& dataIn);
-
-	void deSerializeFloat(const struct b3Matrix3x3FloatData& dataIn);
-
-	void deSerializeDouble(const struct b3Matrix3x3DoubleData& dataIn);
-};
-
-B3_FORCE_INLINE b3Matrix3x3&
-b3Matrix3x3::operator*=(const b3Matrix3x3& m)
-{
-#if defined(B3_USE_SSE_IN_API) && defined(B3_USE_SSE)
-	__m128 rv00, rv01, rv02;
-	__m128 rv10, rv11, rv12;
-	__m128 rv20, rv21, rv22;
-	__m128 mv0, mv1, mv2;
-
-	rv02 = m_el[0].mVec128;
-	rv12 = m_el[1].mVec128;
-	rv22 = m_el[2].mVec128;
-
-	mv0 = _mm_and_ps(m[0].mVec128, b3vFFF0fMask);
-	mv1 = _mm_and_ps(m[1].mVec128, b3vFFF0fMask);
-	mv2 = _mm_and_ps(m[2].mVec128, b3vFFF0fMask);
-
-	// rv0
-	rv00 = b3_splat_ps(rv02, 0);
-	rv01 = b3_splat_ps(rv02, 1);
-	rv02 = b3_splat_ps(rv02, 2);
-
-	rv00 = _mm_mul_ps(rv00, mv0);
-	rv01 = _mm_mul_ps(rv01, mv1);
-	rv02 = _mm_mul_ps(rv02, mv2);
-
-	// rv1
-	rv10 = b3_splat_ps(rv12, 0);
-	rv11 = b3_splat_ps(rv12, 1);
-	rv12 = b3_splat_ps(rv12, 2);
-
-	rv10 = _mm_mul_ps(rv10, mv0);
-	rv11 = _mm_mul_ps(rv11, mv1);
-	rv12 = _mm_mul_ps(rv12, mv2);
-
-	// rv2
-	rv20 = b3_splat_ps(rv22, 0);
-	rv21 = b3_splat_ps(rv22, 1);
-	rv22 = b3_splat_ps(rv22, 2);
-
-	rv20 = _mm_mul_ps(rv20, mv0);
-	rv21 = _mm_mul_ps(rv21, mv1);
-	rv22 = _mm_mul_ps(rv22, mv2);
-
-	rv00 = _mm_add_ps(rv00, rv01);
-	rv10 = _mm_add_ps(rv10, rv11);
-	rv20 = _mm_add_ps(rv20, rv21);
-
-	m_el[0].mVec128 = _mm_add_ps(rv00, rv02);
-	m_el[1].mVec128 = _mm_add_ps(rv10, rv12);
-	m_el[2].mVec128 = _mm_add_ps(rv20, rv22);
-
-#elif defined(B3_USE_NEON)
-
-	float32x4_t rv0, rv1, rv2;
-	float32x4_t v0, v1, v2;
-	float32x4_t mv0, mv1, mv2;
-
-	v0 = m_el[0].mVec128;
-	v1 = m_el[1].mVec128;
-	v2 = m_el[2].mVec128;
-
-	mv0 = (float32x4_t)vandq_s32((int32x4_t)m[0].mVec128, b3vFFF0Mask);
-	mv1 = (float32x4_t)vandq_s32((int32x4_t)m[1].mVec128, b3vFFF0Mask);
-	mv2 = (float32x4_t)vandq_s32((int32x4_t)m[2].mVec128, b3vFFF0Mask);
-
-	rv0 = vmulq_lane_f32(mv0, vget_low_f32(v0), 0);
-	rv1 = vmulq_lane_f32(mv0, vget_low_f32(v1), 0);
-	rv2 = vmulq_lane_f32(mv0, vget_low_f32(v2), 0);
-
-	rv0 = vmlaq_lane_f32(rv0, mv1, vget_low_f32(v0), 1);
-	rv1 = vmlaq_lane_f32(rv1, mv1, vget_low_f32(v1), 1);
-	rv2 = vmlaq_lane_f32(rv2, mv1, vget_low_f32(v2), 1);
-
-	rv0 = vmlaq_lane_f32(rv0, mv2, vget_high_f32(v0), 0);
-	rv1 = vmlaq_lane_f32(rv1, mv2, vget_high_f32(v1), 0);
-	rv2 = vmlaq_lane_f32(rv2, mv2, vget_high_f32(v2), 0);
-
-	m_el[0].mVec128 = rv0;
-	m_el[1].mVec128 = rv1;
-	m_el[2].mVec128 = rv2;
-#else
-	setValue(
-		m.tdotx(m_el[0]), m.tdoty(m_el[0]), m.tdotz(m_el[0]),
-		m.tdotx(m_el[1]), m.tdoty(m_el[1]), m.tdotz(m_el[1]),
-		m.tdotx(m_el[2]), m.tdoty(m_el[2]), m.tdotz(m_el[2]));
-#endif
-	return *this;
-}
-
-B3_FORCE_INLINE b3Matrix3x3&
-b3Matrix3x3::operator+=(const b3Matrix3x3& m)
-{
-#if (defined(B3_USE_SSE_IN_API) && defined(B3_USE_SSE)) || defined(B3_USE_NEON)
-	m_el[0].mVec128 = m_el[0].mVec128 + m.m_el[0].mVec128;
-	m_el[1].mVec128 = m_el[1].mVec128 + m.m_el[1].mVec128;
-	m_el[2].mVec128 = m_el[2].mVec128 + m.m_el[2].mVec128;
-#else
-	setValue(
-		m_el[0][0] + m.m_el[0][0],
-		m_el[0][1] + m.m_el[0][1],
-		m_el[0][2] + m.m_el[0][2],
-		m_el[1][0] + m.m_el[1][0],
-		m_el[1][1] + m.m_el[1][1],
-		m_el[1][2] + m.m_el[1][2],
-		m_el[2][0] + m.m_el[2][0],
-		m_el[2][1] + m.m_el[2][1],
-		m_el[2][2] + m.m_el[2][2]);
-#endif
-	return *this;
-}
-
-B3_FORCE_INLINE b3Matrix3x3
-operator*(const b3Matrix3x3& m, const b3Scalar& k)
-{
-#if (defined(B3_USE_SSE_IN_API) && defined(B3_USE_SSE))
-	__m128 vk = b3_splat_ps(_mm_load_ss((float*)&k), 0x80);
-	return b3Matrix3x3(
-		_mm_mul_ps(m[0].mVec128, vk),
-		_mm_mul_ps(m[1].mVec128, vk),
-		_mm_mul_ps(m[2].mVec128, vk));
-#elif defined(B3_USE_NEON)
-	return b3Matrix3x3(
-		vmulq_n_f32(m[0].mVec128, k),
-		vmulq_n_f32(m[1].mVec128, k),
-		vmulq_n_f32(m[2].mVec128, k));
-#else
-	return b3Matrix3x3(
-		m[0].getX() * k, m[0].getY() * k, m[0].getZ() * k,
-		m[1].getX() * k, m[1].getY() * k, m[1].getZ() * k,
-		m[2].getX() * k, m[2].getY() * k, m[2].getZ() * k);
-#endif
-}
-
-B3_FORCE_INLINE b3Matrix3x3
-operator+(const b3Matrix3x3& m1, const b3Matrix3x3& m2)
-{
-#if (defined(B3_USE_SSE_IN_API) && defined(B3_USE_SSE)) || defined(B3_USE_NEON)
-	return b3Matrix3x3(
-		m1[0].mVec128 + m2[0].mVec128,
-		m1[1].mVec128 + m2[1].mVec128,
-		m1[2].mVec128 + m2[2].mVec128);
-#else
-	return b3Matrix3x3(
-		m1[0][0] + m2[0][0],
-		m1[0][1] + m2[0][1],
-		m1[0][2] + m2[0][2],
-
-		m1[1][0] + m2[1][0],
-		m1[1][1] + m2[1][1],
-		m1[1][2] + m2[1][2],
-
-		m1[2][0] + m2[2][0],
-		m1[2][1] + m2[2][1],
-		m1[2][2] + m2[2][2]);
-#endif
-}
-
-B3_FORCE_INLINE b3Matrix3x3
-operator-(const b3Matrix3x3& m1, const b3Matrix3x3& m2)
-{
-#if (defined(B3_USE_SSE_IN_API) && defined(B3_USE_SSE)) || defined(B3_USE_NEON)
-	return b3Matrix3x3(
-		m1[0].mVec128 - m2[0].mVec128,
-		m1[1].mVec128 - m2[1].mVec128,
-		m1[2].mVec128 - m2[2].mVec128);
-#else
-	return b3Matrix3x3(
-		m1[0][0] - m2[0][0],
-		m1[0][1] - m2[0][1],
-		m1[0][2] - m2[0][2],
-
-		m1[1][0] - m2[1][0],
-		m1[1][1] - m2[1][1],
-		m1[1][2] - m2[1][2],
-
-		m1[2][0] - m2[2][0],
-		m1[2][1] - m2[2][1],
-		m1[2][2] - m2[2][2]);
-#endif
-}
-
-B3_FORCE_INLINE b3Matrix3x3&
-b3Matrix3x3::operator-=(const b3Matrix3x3& m)
-{
-#if (defined(B3_USE_SSE_IN_API) && defined(B3_USE_SSE)) || defined(B3_USE_NEON)
-	m_el[0].mVec128 = m_el[0].mVec128 - m.m_el[0].mVec128;
-	m_el[1].mVec128 = m_el[1].mVec128 - m.m_el[1].mVec128;
-	m_el[2].mVec128 = m_el[2].mVec128 - m.m_el[2].mVec128;
-#else
-	setValue(
-		m_el[0][0] - m.m_el[0][0],
-		m_el[0][1] - m.m_el[0][1],
-		m_el[0][2] - m.m_el[0][2],
-		m_el[1][0] - m.m_el[1][0],
-		m_el[1][1] - m.m_el[1][1],
-		m_el[1][2] - m.m_el[1][2],
-		m_el[2][0] - m.m_el[2][0],
-		m_el[2][1] - m.m_el[2][1],
-		m_el[2][2] - m.m_el[2][2]);
-#endif
-	return *this;
-}
-
-B3_FORCE_INLINE b3Scalar
-b3Matrix3x3::determinant() const
-{
-	return b3Triple((*this)[0], (*this)[1], (*this)[2]);
-}
-
-B3_FORCE_INLINE b3Matrix3x3
-b3Matrix3x3::absolute() const
-{
-#if (defined(B3_USE_SSE_IN_API) && defined(B3_USE_SSE))
-	return b3Matrix3x3(
-		_mm_and_ps(m_el[0].mVec128, b3vAbsfMask),
-		_mm_and_ps(m_el[1].mVec128, b3vAbsfMask),
-		_mm_and_ps(m_el[2].mVec128, b3vAbsfMask));
-#elif defined(B3_USE_NEON)
-	return b3Matrix3x3(
-		(float32x4_t)vandq_s32((int32x4_t)m_el[0].mVec128, b3v3AbsMask),
-		(float32x4_t)vandq_s32((int32x4_t)m_el[1].mVec128, b3v3AbsMask),
-		(float32x4_t)vandq_s32((int32x4_t)m_el[2].mVec128, b3v3AbsMask));
-#else
-	return b3Matrix3x3(
-		b3Fabs(m_el[0].getX()), b3Fabs(m_el[0].getY()), b3Fabs(m_el[0].getZ()),
-		b3Fabs(m_el[1].getX()), b3Fabs(m_el[1].getY()), b3Fabs(m_el[1].getZ()),
-		b3Fabs(m_el[2].getX()), b3Fabs(m_el[2].getY()), b3Fabs(m_el[2].getZ()));
-#endif
-}
-
-B3_FORCE_INLINE b3Matrix3x3
-b3Matrix3x3::transpose() const
-{
-#if (defined(B3_USE_SSE_IN_API) && defined(B3_USE_SSE))
-	__m128 v0 = m_el[0].mVec128;
-	__m128 v1 = m_el[1].mVec128;
-	__m128 v2 = m_el[2].mVec128;  //  x2 y2 z2 w2
-	__m128 vT;
-
-	v2 = _mm_and_ps(v2, b3vFFF0fMask);  //  x2 y2 z2 0
-
-	vT = _mm_unpackhi_ps(v0, v1);  //	z0 z1 * *
-	v0 = _mm_unpacklo_ps(v0, v1);  //	x0 x1 y0 y1
-
-	v1 = _mm_shuffle_ps(v0, v2, B3_SHUFFLE(2, 3, 1, 3));                    // y0 y1 y2 0
-	v0 = _mm_shuffle_ps(v0, v2, B3_SHUFFLE(0, 1, 0, 3));                    // x0 x1 x2 0
-	v2 = b3CastdTo128f(_mm_move_sd(b3CastfTo128d(v2), b3CastfTo128d(vT)));  // z0 z1 z2 0
-
-	return b3Matrix3x3(v0, v1, v2);
-#elif defined(B3_USE_NEON)
-	// note: zeros the w channel. We can preserve it at the cost of two more vtrn instructions.
-	static const uint32x2_t zMask = (const uint32x2_t){-1, 0};
-	float32x4x2_t top = vtrnq_f32(m_el[0].mVec128, m_el[1].mVec128);               // {x0 x1 z0 z1}, {y0 y1 w0 w1}
-	float32x2x2_t bl = vtrn_f32(vget_low_f32(m_el[2].mVec128), vdup_n_f32(0.0f));  // {x2  0 }, {y2 0}
-	float32x4_t v0 = vcombine_f32(vget_low_f32(top.val[0]), bl.val[0]);
-	float32x4_t v1 = vcombine_f32(vget_low_f32(top.val[1]), bl.val[1]);
-	float32x2_t q = (float32x2_t)vand_u32((uint32x2_t)vget_high_f32(m_el[2].mVec128), zMask);
-	float32x4_t v2 = vcombine_f32(vget_high_f32(top.val[0]), q);  // z0 z1 z2  0
-	return b3Matrix3x3(v0, v1, v2);
-#else
-	return b3Matrix3x3(m_el[0].getX(), m_el[1].getX(), m_el[2].getX(),
-					   m_el[0].getY(), m_el[1].getY(), m_el[2].getY(),
-					   m_el[0].getZ(), m_el[1].getZ(), m_el[2].getZ());
-#endif
-}
-
-B3_FORCE_INLINE b3Matrix3x3
-b3Matrix3x3::adjoint() const
-{
-	return b3Matrix3x3(cofac(1, 1, 2, 2), cofac(0, 2, 2, 1), cofac(0, 1, 1, 2),
-					   cofac(1, 2, 2, 0), cofac(0, 0, 2, 2), cofac(0, 2, 1, 0),
-					   cofac(1, 0, 2, 1), cofac(0, 1, 2, 0), cofac(0, 0, 1, 1));
-}
-
-B3_FORCE_INLINE b3Matrix3x3
-b3Matrix3x3::inverse() const
-{
-	b3Vector3 co = b3MakeVector3(cofac(1, 1, 2, 2), cofac(1, 2, 2, 0), cofac(1, 0, 2, 1));
-	b3Scalar det = (*this)[0].dot(co);
-	b3FullAssert(det != b3Scalar(0.0));
-	b3Scalar s = b3Scalar(1.0) / det;
-	return b3Matrix3x3(co.getX() * s, cofac(0, 2, 2, 1) * s, cofac(0, 1, 1, 2) * s,
-					   co.getY() * s, cofac(0, 0, 2, 2) * s, cofac(0, 2, 1, 0) * s,
-					   co.getZ() * s, cofac(0, 1, 2, 0) * s, cofac(0, 0, 1, 1) * s);
-}
-
-B3_FORCE_INLINE b3Matrix3x3
-b3Matrix3x3::transposeTimes(const b3Matrix3x3& m) const
-{
-#if (defined(B3_USE_SSE_IN_API) && defined(B3_USE_SSE))
-	// zeros w
-	//    static const __m128i xyzMask = (const __m128i){ -1ULL, 0xffffffffULL };
-	__m128 row = m_el[0].mVec128;
-	__m128 m0 = _mm_and_ps(m.getRow(0).mVec128, b3vFFF0fMask);
-	__m128 m1 = _mm_and_ps(m.getRow(1).mVec128, b3vFFF0fMask);
-	__m128 m2 = _mm_and_ps(m.getRow(2).mVec128, b3vFFF0fMask);
-	__m128 r0 = _mm_mul_ps(m0, _mm_shuffle_ps(row, row, 0));
-	__m128 r1 = _mm_mul_ps(m0, _mm_shuffle_ps(row, row, 0x55));
-	__m128 r2 = _mm_mul_ps(m0, _mm_shuffle_ps(row, row, 0xaa));
-	row = m_el[1].mVec128;
-	r0 = _mm_add_ps(r0, _mm_mul_ps(m1, _mm_shuffle_ps(row, row, 0)));
-	r1 = _mm_add_ps(r1, _mm_mul_ps(m1, _mm_shuffle_ps(row, row, 0x55)));
-	r2 = _mm_add_ps(r2, _mm_mul_ps(m1, _mm_shuffle_ps(row, row, 0xaa)));
-	row = m_el[2].mVec128;
-	r0 = _mm_add_ps(r0, _mm_mul_ps(m2, _mm_shuffle_ps(row, row, 0)));
-	r1 = _mm_add_ps(r1, _mm_mul_ps(m2, _mm_shuffle_ps(row, row, 0x55)));
-	r2 = _mm_add_ps(r2, _mm_mul_ps(m2, _mm_shuffle_ps(row, row, 0xaa)));
-	return b3Matrix3x3(r0, r1, r2);
-
-#elif defined B3_USE_NEON
-	// zeros w
-	static const uint32x4_t xyzMask = (const uint32x4_t){-1, -1, -1, 0};
-	float32x4_t m0 = (float32x4_t)vandq_u32((uint32x4_t)m.getRow(0).mVec128, xyzMask);
-	float32x4_t m1 = (float32x4_t)vandq_u32((uint32x4_t)m.getRow(1).mVec128, xyzMask);
-	float32x4_t m2 = (float32x4_t)vandq_u32((uint32x4_t)m.getRow(2).mVec128, xyzMask);
-	float32x4_t row = m_el[0].mVec128;
-	float32x4_t r0 = vmulq_lane_f32(m0, vget_low_f32(row), 0);
-	float32x4_t r1 = vmulq_lane_f32(m0, vget_low_f32(row), 1);
-	float32x4_t r2 = vmulq_lane_f32(m0, vget_high_f32(row), 0);
-	row = m_el[1].mVec128;
-	r0 = vmlaq_lane_f32(r0, m1, vget_low_f32(row), 0);
-	r1 = vmlaq_lane_f32(r1, m1, vget_low_f32(row), 1);
-	r2 = vmlaq_lane_f32(r2, m1, vget_high_f32(row), 0);
-	row = m_el[2].mVec128;
-	r0 = vmlaq_lane_f32(r0, m2, vget_low_f32(row), 0);
-	r1 = vmlaq_lane_f32(r1, m2, vget_low_f32(row), 1);
-	r2 = vmlaq_lane_f32(r2, m2, vget_high_f32(row), 0);
-	return b3Matrix3x3(r0, r1, r2);
-#else
-	return b3Matrix3x3(
-		m_el[0].getX() * m[0].getX() + m_el[1].getX() * m[1].getX() + m_el[2].getX() * m[2].getX(),
-		m_el[0].getX() * m[0].getY() + m_el[1].getX() * m[1].getY() + m_el[2].getX() * m[2].getY(),
-		m_el[0].getX() * m[0].getZ() + m_el[1].getX() * m[1].getZ() + m_el[2].getX() * m[2].getZ(),
-		m_el[0].getY() * m[0].getX() + m_el[1].getY() * m[1].getX() + m_el[2].getY() * m[2].getX(),
-		m_el[0].getY() * m[0].getY() + m_el[1].getY() * m[1].getY() + m_el[2].getY() * m[2].getY(),
-		m_el[0].getY() * m[0].getZ() + m_el[1].getY() * m[1].getZ() + m_el[2].getY() * m[2].getZ(),
-		m_el[0].getZ() * m[0].getX() + m_el[1].getZ() * m[1].getX() + m_el[2].getZ() * m[2].getX(),
-		m_el[0].getZ() * m[0].getY() + m_el[1].getZ() * m[1].getY() + m_el[2].getZ() * m[2].getY(),
-		m_el[0].getZ() * m[0].getZ() + m_el[1].getZ() * m[1].getZ() + m_el[2].getZ() * m[2].getZ());
-#endif
-}
-
-B3_FORCE_INLINE b3Matrix3x3
-b3Matrix3x3::timesTranspose(const b3Matrix3x3& m) const
-{
-#if (defined(B3_USE_SSE_IN_API) && defined(B3_USE_SSE))
-	__m128 a0 = m_el[0].mVec128;
-	__m128 a1 = m_el[1].mVec128;
-	__m128 a2 = m_el[2].mVec128;
-
-	b3Matrix3x3 mT = m.transpose();  // we rely on transpose() zeroing w channel so that we don't have to do it here
-	__m128 mx = mT[0].mVec128;
-	__m128 my = mT[1].mVec128;
-	__m128 mz = mT[2].mVec128;
-
-	__m128 r0 = _mm_mul_ps(mx, _mm_shuffle_ps(a0, a0, 0x00));
-	__m128 r1 = _mm_mul_ps(mx, _mm_shuffle_ps(a1, a1, 0x00));
-	__m128 r2 = _mm_mul_ps(mx, _mm_shuffle_ps(a2, a2, 0x00));
-	r0 = _mm_add_ps(r0, _mm_mul_ps(my, _mm_shuffle_ps(a0, a0, 0x55)));
-	r1 = _mm_add_ps(r1, _mm_mul_ps(my, _mm_shuffle_ps(a1, a1, 0x55)));
-	r2 = _mm_add_ps(r2, _mm_mul_ps(my, _mm_shuffle_ps(a2, a2, 0x55)));
-	r0 = _mm_add_ps(r0, _mm_mul_ps(mz, _mm_shuffle_ps(a0, a0, 0xaa)));
-	r1 = _mm_add_ps(r1, _mm_mul_ps(mz, _mm_shuffle_ps(a1, a1, 0xaa)));
-	r2 = _mm_add_ps(r2, _mm_mul_ps(mz, _mm_shuffle_ps(a2, a2, 0xaa)));
-	return b3Matrix3x3(r0, r1, r2);
-
-#elif defined B3_USE_NEON
-	float32x4_t a0 = m_el[0].mVec128;
-	float32x4_t a1 = m_el[1].mVec128;
-	float32x4_t a2 = m_el[2].mVec128;
-
-	b3Matrix3x3 mT = m.transpose();  // we rely on transpose() zeroing w channel so that we don't have to do it here
-	float32x4_t mx = mT[0].mVec128;
-	float32x4_t my = mT[1].mVec128;
-	float32x4_t mz = mT[2].mVec128;
-
-	float32x4_t r0 = vmulq_lane_f32(mx, vget_low_f32(a0), 0);
-	float32x4_t r1 = vmulq_lane_f32(mx, vget_low_f32(a1), 0);
-	float32x4_t r2 = vmulq_lane_f32(mx, vget_low_f32(a2), 0);
-	r0 = vmlaq_lane_f32(r0, my, vget_low_f32(a0), 1);
-	r1 = vmlaq_lane_f32(r1, my, vget_low_f32(a1), 1);
-	r2 = vmlaq_lane_f32(r2, my, vget_low_f32(a2), 1);
-	r0 = vmlaq_lane_f32(r0, mz, vget_high_f32(a0), 0);
-	r1 = vmlaq_lane_f32(r1, mz, vget_high_f32(a1), 0);
-	r2 = vmlaq_lane_f32(r2, mz, vget_high_f32(a2), 0);
-	return b3Matrix3x3(r0, r1, r2);
-
-#else
-	return b3Matrix3x3(
-		m_el[0].dot(m[0]), m_el[0].dot(m[1]), m_el[0].dot(m[2]),
-		m_el[1].dot(m[0]), m_el[1].dot(m[1]), m_el[1].dot(m[2]),
-		m_el[2].dot(m[0]), m_el[2].dot(m[1]), m_el[2].dot(m[2]));
-#endif
-}
-
-B3_FORCE_INLINE b3Vector3
-operator*(const b3Matrix3x3& m, const b3Vector3& v)
-{
-#if (defined(B3_USE_SSE_IN_API) && defined(B3_USE_SSE)) || defined(B3_USE_NEON)
-	return v.dot3(m[0], m[1], m[2]);
-#else
-	return b3MakeVector3(m[0].dot(v), m[1].dot(v), m[2].dot(v));
-#endif
-}
-
-B3_FORCE_INLINE b3Vector3
-operator*(const b3Vector3& v, const b3Matrix3x3& m)
-{
-#if (defined(B3_USE_SSE_IN_API) && defined(B3_USE_SSE))
-
-	const __m128 vv = v.mVec128;
-
-	__m128 c0 = b3_splat_ps(vv, 0);
-	__m128 c1 = b3_splat_ps(vv, 1);
-	__m128 c2 = b3_splat_ps(vv, 2);
-
-	c0 = _mm_mul_ps(c0, _mm_and_ps(m[0].mVec128, b3vFFF0fMask));
-	c1 = _mm_mul_ps(c1, _mm_and_ps(m[1].mVec128, b3vFFF0fMask));
-	c0 = _mm_add_ps(c0, c1);
-	c2 = _mm_mul_ps(c2, _mm_and_ps(m[2].mVec128, b3vFFF0fMask));
-
-	return b3MakeVector3(_mm_add_ps(c0, c2));
-#elif defined(B3_USE_NEON)
-	const float32x4_t vv = v.mVec128;
-	const float32x2_t vlo = vget_low_f32(vv);
-	const float32x2_t vhi = vget_high_f32(vv);
-
-	float32x4_t c0, c1, c2;
-
-	c0 = (float32x4_t)vandq_s32((int32x4_t)m[0].mVec128, b3vFFF0Mask);
-	c1 = (float32x4_t)vandq_s32((int32x4_t)m[1].mVec128, b3vFFF0Mask);
-	c2 = (float32x4_t)vandq_s32((int32x4_t)m[2].mVec128, b3vFFF0Mask);
-
-	c0 = vmulq_lane_f32(c0, vlo, 0);
-	c1 = vmulq_lane_f32(c1, vlo, 1);
-	c2 = vmulq_lane_f32(c2, vhi, 0);
-	c0 = vaddq_f32(c0, c1);
-	c0 = vaddq_f32(c0, c2);
-
-	return b3MakeVector3(c0);
-#else
-	return b3MakeVector3(m.tdotx(v), m.tdoty(v), m.tdotz(v));
-#endif
-}
-
-B3_FORCE_INLINE b3Matrix3x3
-operator*(const b3Matrix3x3& m1, const b3Matrix3x3& m2)
-{
-#if (defined(B3_USE_SSE_IN_API) && defined(B3_USE_SSE))
-
-	__m128 m10 = m1[0].mVec128;
-	__m128 m11 = m1[1].mVec128;
-	__m128 m12 = m1[2].mVec128;
-
-	__m128 m2v = _mm_and_ps(m2[0].mVec128, b3vFFF0fMask);
-
-	__m128 c0 = b3_splat_ps(m10, 0);
-	__m128 c1 = b3_splat_ps(m11, 0);
-	__m128 c2 = b3_splat_ps(m12, 0);
-
-	c0 = _mm_mul_ps(c0, m2v);
-	c1 = _mm_mul_ps(c1, m2v);
-	c2 = _mm_mul_ps(c2, m2v);
-
-	m2v = _mm_and_ps(m2[1].mVec128, b3vFFF0fMask);
-
-	__m128 c0_1 = b3_splat_ps(m10, 1);
-	__m128 c1_1 = b3_splat_ps(m11, 1);
-	__m128 c2_1 = b3_splat_ps(m12, 1);
-
-	c0_1 = _mm_mul_ps(c0_1, m2v);
-	c1_1 = _mm_mul_ps(c1_1, m2v);
-	c2_1 = _mm_mul_ps(c2_1, m2v);
-
-	m2v = _mm_and_ps(m2[2].mVec128, b3vFFF0fMask);
-
-	c0 = _mm_add_ps(c0, c0_1);
-	c1 = _mm_add_ps(c1, c1_1);
-	c2 = _mm_add_ps(c2, c2_1);
-
-	m10 = b3_splat_ps(m10, 2);
-	m11 = b3_splat_ps(m11, 2);
-	m12 = b3_splat_ps(m12, 2);
-
-	m10 = _mm_mul_ps(m10, m2v);
-	m11 = _mm_mul_ps(m11, m2v);
-	m12 = _mm_mul_ps(m12, m2v);
-
-	c0 = _mm_add_ps(c0, m10);
-	c1 = _mm_add_ps(c1, m11);
-	c2 = _mm_add_ps(c2, m12);
-
-	return b3Matrix3x3(c0, c1, c2);
-
-#elif defined(B3_USE_NEON)
-
-	float32x4_t rv0, rv1, rv2;
-	float32x4_t v0, v1, v2;
-	float32x4_t mv0, mv1, mv2;
-
-	v0 = m1[0].mVec128;
-	v1 = m1[1].mVec128;
-	v2 = m1[2].mVec128;
-
-	mv0 = (float32x4_t)vandq_s32((int32x4_t)m2[0].mVec128, b3vFFF0Mask);
-	mv1 = (float32x4_t)vandq_s32((int32x4_t)m2[1].mVec128, b3vFFF0Mask);
-	mv2 = (float32x4_t)vandq_s32((int32x4_t)m2[2].mVec128, b3vFFF0Mask);
-
-	rv0 = vmulq_lane_f32(mv0, vget_low_f32(v0), 0);
-	rv1 = vmulq_lane_f32(mv0, vget_low_f32(v1), 0);
-	rv2 = vmulq_lane_f32(mv0, vget_low_f32(v2), 0);
-
-	rv0 = vmlaq_lane_f32(rv0, mv1, vget_low_f32(v0), 1);
-	rv1 = vmlaq_lane_f32(rv1, mv1, vget_low_f32(v1), 1);
-	rv2 = vmlaq_lane_f32(rv2, mv1, vget_low_f32(v2), 1);
-
-	rv0 = vmlaq_lane_f32(rv0, mv2, vget_high_f32(v0), 0);
-	rv1 = vmlaq_lane_f32(rv1, mv2, vget_high_f32(v1), 0);
-	rv2 = vmlaq_lane_f32(rv2, mv2, vget_high_f32(v2), 0);
-
-	return b3Matrix3x3(rv0, rv1, rv2);
-
-#else
-	return b3Matrix3x3(
-		m2.tdotx(m1[0]), m2.tdoty(m1[0]), m2.tdotz(m1[0]),
-		m2.tdotx(m1[1]), m2.tdoty(m1[1]), m2.tdotz(m1[1]),
-		m2.tdotx(m1[2]), m2.tdoty(m1[2]), m2.tdotz(m1[2]));
-#endif
-}
-
-/*
-B3_FORCE_INLINE b3Matrix3x3 b3MultTransposeLeft(const b3Matrix3x3& m1, const b3Matrix3x3& m2) {
-return b3Matrix3x3(
-m1[0][0] * m2[0][0] + m1[1][0] * m2[1][0] + m1[2][0] * m2[2][0],
-m1[0][0] * m2[0][1] + m1[1][0] * m2[1][1] + m1[2][0] * m2[2][1],
-m1[0][0] * m2[0][2] + m1[1][0] * m2[1][2] + m1[2][0] * m2[2][2],
-m1[0][1] * m2[0][0] + m1[1][1] * m2[1][0] + m1[2][1] * m2[2][0],
-m1[0][1] * m2[0][1] + m1[1][1] * m2[1][1] + m1[2][1] * m2[2][1],
-m1[0][1] * m2[0][2] + m1[1][1] * m2[1][2] + m1[2][1] * m2[2][2],
-m1[0][2] * m2[0][0] + m1[1][2] * m2[1][0] + m1[2][2] * m2[2][0],
-m1[0][2] * m2[0][1] + m1[1][2] * m2[1][1] + m1[2][2] * m2[2][1],
-m1[0][2] * m2[0][2] + m1[1][2] * m2[1][2] + m1[2][2] * m2[2][2]);
-}
-*/
-
-/**@brief Equality operator between two matrices
-* It will test all elements are equal.  */
-B3_FORCE_INLINE bool operator==(const b3Matrix3x3& m1, const b3Matrix3x3& m2)
-{
-#if (defined(B3_USE_SSE_IN_API) && defined(B3_USE_SSE))
-
-	__m128 c0, c1, c2;
-
-	c0 = _mm_cmpeq_ps(m1[0].mVec128, m2[0].mVec128);
-	c1 = _mm_cmpeq_ps(m1[1].mVec128, m2[1].mVec128);
-	c2 = _mm_cmpeq_ps(m1[2].mVec128, m2[2].mVec128);
-
-	c0 = _mm_and_ps(c0, c1);
-	c0 = _mm_and_ps(c0, c2);
-
-	return (0x7 == _mm_movemask_ps((__m128)c0));
-#else
-	return (m1[0][0] == m2[0][0] && m1[1][0] == m2[1][0] && m1[2][0] == m2[2][0] &&
-			m1[0][1] == m2[0][1] && m1[1][1] == m2[1][1] && m1[2][1] == m2[2][1] &&
-			m1[0][2] == m2[0][2] && m1[1][2] == m2[1][2] && m1[2][2] == m2[2][2]);
-#endif
-}
-
-///for serialization
-struct b3Matrix3x3FloatData
-{
-	b3Vector3FloatData m_el[3];
-};
-
-///for serialization
-struct b3Matrix3x3DoubleData
-{
-	b3Vector3DoubleData m_el[3];
-};
-
-B3_FORCE_INLINE void b3Matrix3x3::serialize(struct b3Matrix3x3Data& dataOut) const
-{
-	for (int i = 0; i < 3; i++)
-		m_el[i].serialize(dataOut.m_el[i]);
-}
-
-B3_FORCE_INLINE void b3Matrix3x3::serializeFloat(struct b3Matrix3x3FloatData& dataOut) const
-{
-	for (int i = 0; i < 3; i++)
-		m_el[i].serializeFloat(dataOut.m_el[i]);
-}
-
-B3_FORCE_INLINE void b3Matrix3x3::deSerialize(const struct b3Matrix3x3Data& dataIn)
-{
-	for (int i = 0; i < 3; i++)
-		m_el[i].deSerialize(dataIn.m_el[i]);
-}
-
-B3_FORCE_INLINE void b3Matrix3x3::deSerializeFloat(const struct b3Matrix3x3FloatData& dataIn)
-{
-	for (int i = 0; i < 3; i++)
-		m_el[i].deSerializeFloat(dataIn.m_el[i]);
-}
-
-B3_FORCE_INLINE void b3Matrix3x3::deSerializeDouble(const struct b3Matrix3x3DoubleData& dataIn)
-{
-	for (int i = 0; i < 3; i++)
-		m_el[i].deSerializeDouble(dataIn.m_el[i]);
-}
-
-#endif  //B3_MATRIX3x3_H

thirdparty/bullet/Bullet3Common/b3MinMax.h

@@ -1,69 +0,0 @@
-/*
-Copyright (c) 2003-2013 Gino van den Bergen / Erwin Coumans  http://bulletphysics.org
-
-This software is provided 'as-is', without any express or implied warranty.
-In no event will the authors be held liable for any damages arising from the use of this software.
-Permission is granted to anyone to use this software for any purpose, 
-including commercial applications, and to alter it and redistribute it freely, 
-subject to the following restrictions:
-
-1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
-2. Altered source versions must be plainly marked as 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 B3_GEN_MINMAX_H
-#define B3_GEN_MINMAX_H
-
-#include "b3Scalar.h"
-
-template <class T>
-B3_FORCE_INLINE const T& b3Min(const T& a, const T& b)
-{
-	return a < b ? a : b;
-}
-
-template <class T>
-B3_FORCE_INLINE const T& b3Max(const T& a, const T& b)
-{
-	return a > b ? a : b;
-}
-
-template <class T>
-B3_FORCE_INLINE const T& b3Clamped(const T& a, const T& lb, const T& ub)
-{
-	return a < lb ? lb : (ub < a ? ub : a);
-}
-
-template <class T>
-B3_FORCE_INLINE void b3SetMin(T& a, const T& b)
-{
-	if (b < a)
-	{
-		a = b;
-	}
-}
-
-template <class T>
-B3_FORCE_INLINE void b3SetMax(T& a, const T& b)
-{
-	if (a < b)
-	{
-		a = b;
-	}
-}
-
-template <class T>
-B3_FORCE_INLINE void b3Clamp(T& a, const T& lb, const T& ub)
-{
-	if (a < lb)
-	{
-		a = lb;
-	}
-	else if (ub < a)
-	{
-		a = ub;
-	}
-}
-
-#endif  //B3_GEN_MINMAX_H

thirdparty/bullet/Bullet3Common/b3PoolAllocator.h

@@ -1,121 +0,0 @@
-/*
-Copyright (c) 2003-2013 Gino van den Bergen / Erwin Coumans  http://bulletphysics.org
-
-This software is provided 'as-is', without any express or implied warranty.
-In no event will the authors be held liable for any damages arising from the use of this software.
-Permission is granted to anyone to use this software for any purpose, 
-including commercial applications, and to alter it and redistribute it freely, 
-subject to the following restrictions:
-
-1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
-2. Altered source versions must be plainly marked as 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 _BT_POOL_ALLOCATOR_H
-#define _BT_POOL_ALLOCATOR_H
-
-#include "b3Scalar.h"
-#include "b3AlignedAllocator.h"
-
-///The b3PoolAllocator class allows to efficiently allocate a large pool of objects, instead of dynamically allocating them separately.
-class b3PoolAllocator
-{
-	int m_elemSize;
-	int m_maxElements;
-	int m_freeCount;
-	void* m_firstFree;
-	unsigned char* m_pool;
-
-public:
-	b3PoolAllocator(int elemSize, int maxElements)
-		: m_elemSize(elemSize),
-		  m_maxElements(maxElements)
-	{
-		m_pool = (unsigned char*)b3AlignedAlloc(static_cast<unsigned int>(m_elemSize * m_maxElements), 16);
-
-		unsigned char* p = m_pool;
-		m_firstFree = p;
-		m_freeCount = m_maxElements;
-		int count = m_maxElements;
-		while (--count)
-		{
-			*(void**)p = (p + m_elemSize);
-			p += m_elemSize;
-		}
-		*(void**)p = 0;
-	}
-
-	~b3PoolAllocator()
-	{
-		b3AlignedFree(m_pool);
-	}
-
-	int getFreeCount() const
-	{
-		return m_freeCount;
-	}
-
-	int getUsedCount() const
-	{
-		return m_maxElements - m_freeCount;
-	}
-
-	int getMaxCount() const
-	{
-		return m_maxElements;
-	}
-
-	void* allocate(int size)
-	{
-		// release mode fix
-		(void)size;
-		b3Assert(!size || size <= m_elemSize);
-		b3Assert(m_freeCount > 0);
-		void* result = m_firstFree;
-		m_firstFree = *(void**)m_firstFree;
-		--m_freeCount;
-		return result;
-	}
-
-	bool validPtr(void* ptr)
-	{
-		if (ptr)
-		{
-			if (((unsigned char*)ptr >= m_pool && (unsigned char*)ptr < m_pool + m_maxElements * m_elemSize))
-			{
-				return true;
-			}
-		}
-		return false;
-	}
-
-	void freeMemory(void* ptr)
-	{
-		if (ptr)
-		{
-			b3Assert((unsigned char*)ptr >= m_pool && (unsigned char*)ptr < m_pool + m_maxElements * m_elemSize);
-
-			*(void**)ptr = m_firstFree;
-			m_firstFree = ptr;
-			++m_freeCount;
-		}
-	}
-
-	int getElementSize() const
-	{
-		return m_elemSize;
-	}
-
-	unsigned char* getPoolAddress()
-	{
-		return m_pool;
-	}
-
-	const unsigned char* getPoolAddress() const
-	{
-		return m_pool;
-	}
-};
-
-#endif  //_BT_POOL_ALLOCATOR_H

thirdparty/bullet/Bullet3Common/b3QuadWord.h

@@ -1,242 +0,0 @@
-/*
-Copyright (c) 2003-2013 Gino van den Bergen / Erwin Coumans  http://bulletphysics.org
-
-This software is provided 'as-is', without any express or implied warranty.
-In no event will the authors be held liable for any damages arising from the use of this software.
-Permission is granted to anyone to use this software for any purpose, 
-including commercial applications, and to alter it and redistribute it freely, 
-subject to the following restrictions:
-
-1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
-2. Altered source versions must be plainly marked as 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 B3_SIMD_QUADWORD_H
-#define B3_SIMD_QUADWORD_H
-
-#include "b3Scalar.h"
-#include "b3MinMax.h"
-
-#if defined(__CELLOS_LV2) && defined(__SPU__)
-#include <altivec.h>
-#endif
-
-/**@brief The b3QuadWord class is base class for b3Vector3 and b3Quaternion. 
- * Some issues under PS3 Linux with IBM 2.1 SDK, gcc compiler prevent from using aligned quadword.
- */
-#ifndef USE_LIBSPE2
-B3_ATTRIBUTE_ALIGNED16(class)
-b3QuadWord
-#else
-class b3QuadWord
-#endif
-{
-protected:
-#if defined(__SPU__) && defined(__CELLOS_LV2__)
-	union {
-		vec_float4 mVec128;
-		b3Scalar m_floats[4];
-	};
-
-public:
-	vec_float4 get128() const
-	{
-		return mVec128;
-	}
-
-#else  //__CELLOS_LV2__ __SPU__
-
-#if defined(B3_USE_SSE) || defined(B3_USE_NEON)
-public:
-	union {
-		b3SimdFloat4 mVec128;
-		b3Scalar m_floats[4];
-		struct
-		{
-			b3Scalar x, y, z, w;
-		};
-	};
-
-public:
-	B3_FORCE_INLINE b3SimdFloat4 get128() const
-	{
-		return mVec128;
-	}
-	B3_FORCE_INLINE void set128(b3SimdFloat4 v128)
-	{
-		mVec128 = v128;
-	}
-#else
-public:
-	union {
-		b3Scalar m_floats[4];
-		struct
-		{
-			b3Scalar x, y, z, w;
-		};
-	};
-#endif  // B3_USE_SSE
-
-#endif  //__CELLOS_LV2__ __SPU__
-
-public:
-#if defined(B3_USE_SSE) || defined(B3_USE_NEON)
-
-	// Set Vector
-	B3_FORCE_INLINE b3QuadWord(const b3SimdFloat4 vec)
-	{
-		mVec128 = vec;
-	}
-
-	// Copy constructor
-	B3_FORCE_INLINE b3QuadWord(const b3QuadWord& rhs)
-	{
-		mVec128 = rhs.mVec128;
-	}
-
-	// Assignment Operator
-	B3_FORCE_INLINE b3QuadWord&
-	operator=(const b3QuadWord& v)
-	{
-		mVec128 = v.mVec128;
-
-		return *this;
-	}
-
-#endif
-
-	/**@brief Return the x value */
-	B3_FORCE_INLINE const b3Scalar& getX() const { return m_floats[0]; }
-	/**@brief Return the y value */
-	B3_FORCE_INLINE const b3Scalar& getY() const { return m_floats[1]; }
-	/**@brief Return the z value */
-	B3_FORCE_INLINE const b3Scalar& getZ() const { return m_floats[2]; }
-	/**@brief Set the x value */
-	B3_FORCE_INLINE void setX(b3Scalar _x) { m_floats[0] = _x; };
-	/**@brief Set the y value */
-	B3_FORCE_INLINE void setY(b3Scalar _y) { m_floats[1] = _y; };
-	/**@brief Set the z value */
-	B3_FORCE_INLINE void setZ(b3Scalar _z) { m_floats[2] = _z; };
-	/**@brief Set the w value */
-	B3_FORCE_INLINE void setW(b3Scalar _w) { m_floats[3] = _w; };
-	/**@brief Return the x value */
-
-	//B3_FORCE_INLINE b3Scalar&       operator[](int i)       { return (&m_floats[0])[i];	}
-	//B3_FORCE_INLINE const b3Scalar& operator[](int i) const { return (&m_floats[0])[i]; }
-	///operator b3Scalar*() replaces operator[], using implicit conversion. We added operator != and operator == to avoid pointer comparisons.
-	B3_FORCE_INLINE operator b3Scalar*() { return &m_floats[0]; }
-	B3_FORCE_INLINE operator const b3Scalar*() const { return &m_floats[0]; }
-
-	B3_FORCE_INLINE bool operator==(const b3QuadWord& other) const
-	{
-#ifdef B3_USE_SSE
-		return (0xf == _mm_movemask_ps((__m128)_mm_cmpeq_ps(mVec128, other.mVec128)));
-#else
-		return ((m_floats[3] == other.m_floats[3]) &&
-				(m_floats[2] == other.m_floats[2]) &&
-				(m_floats[1] == other.m_floats[1]) &&
-				(m_floats[0] == other.m_floats[0]));
-#endif
-	}
-
-	B3_FORCE_INLINE bool operator!=(const b3QuadWord& other) const
-	{
-		return !(*this == other);
-	}
-
-	/**@brief Set x,y,z and zero w 
-   * @param x Value of x
-   * @param y Value of y
-   * @param z Value of z
-   */
-	B3_FORCE_INLINE void setValue(const b3Scalar& _x, const b3Scalar& _y, const b3Scalar& _z)
-	{
-		m_floats[0] = _x;
-		m_floats[1] = _y;
-		m_floats[2] = _z;
-		m_floats[3] = 0.f;
-	}
-
-	/*		void getValue(b3Scalar *m) const 
-		{
-			m[0] = m_floats[0];
-			m[1] = m_floats[1];
-			m[2] = m_floats[2];
-		}
-*/
-	/**@brief Set the values 
-   * @param x Value of x
-   * @param y Value of y
-   * @param z Value of z
-   * @param w Value of w
-   */
-	B3_FORCE_INLINE void setValue(const b3Scalar& _x, const b3Scalar& _y, const b3Scalar& _z, const b3Scalar& _w)
-	{
-		m_floats[0] = _x;
-		m_floats[1] = _y;
-		m_floats[2] = _z;
-		m_floats[3] = _w;
-	}
-	/**@brief No initialization constructor */
-	B3_FORCE_INLINE b3QuadWord()
-	//	:m_floats[0](b3Scalar(0.)),m_floats[1](b3Scalar(0.)),m_floats[2](b3Scalar(0.)),m_floats[3](b3Scalar(0.))
-	{
-	}
-
-	/**@brief Three argument constructor (zeros w)
-   * @param x Value of x
-   * @param y Value of y
-   * @param z Value of z
-   */
-	B3_FORCE_INLINE b3QuadWord(const b3Scalar& _x, const b3Scalar& _y, const b3Scalar& _z)
-	{
-		m_floats[0] = _x, m_floats[1] = _y, m_floats[2] = _z, m_floats[3] = 0.0f;
-	}
-
-	/**@brief Initializing constructor
-   * @param x Value of x
-   * @param y Value of y
-   * @param z Value of z
-   * @param w Value of w
-   */
-	B3_FORCE_INLINE b3QuadWord(const b3Scalar& _x, const b3Scalar& _y, const b3Scalar& _z, const b3Scalar& _w)
-	{
-		m_floats[0] = _x, m_floats[1] = _y, m_floats[2] = _z, m_floats[3] = _w;
-	}
-
-	/**@brief Set each element to the max of the current values and the values of another b3QuadWord
-   * @param other The other b3QuadWord to compare with 
-   */
-	B3_FORCE_INLINE void setMax(const b3QuadWord& other)
-	{
-#ifdef B3_USE_SSE
-		mVec128 = _mm_max_ps(mVec128, other.mVec128);
-#elif defined(B3_USE_NEON)
-		mVec128 = vmaxq_f32(mVec128, other.mVec128);
-#else
-		b3SetMax(m_floats[0], other.m_floats[0]);
-		b3SetMax(m_floats[1], other.m_floats[1]);
-		b3SetMax(m_floats[2], other.m_floats[2]);
-		b3SetMax(m_floats[3], other.m_floats[3]);
-#endif
-	}
-	/**@brief Set each element to the min of the current values and the values of another b3QuadWord
-   * @param other The other b3QuadWord to compare with 
-   */
-	B3_FORCE_INLINE void setMin(const b3QuadWord& other)
-	{
-#ifdef B3_USE_SSE
-		mVec128 = _mm_min_ps(mVec128, other.mVec128);
-#elif defined(B3_USE_NEON)
-		mVec128 = vminq_f32(mVec128, other.mVec128);
-#else
-		b3SetMin(m_floats[0], other.m_floats[0]);
-		b3SetMin(m_floats[1], other.m_floats[1]);
-		b3SetMin(m_floats[2], other.m_floats[2]);
-		b3SetMin(m_floats[3], other.m_floats[3]);
-#endif
-	}
-};
-
-#endif  //B3_SIMD_QUADWORD_H